CN111854733A

CN111854733A - Multi-source fusion positioning method and system

Info

Publication number: CN111854733A
Application number: CN202010742197.1A
Authority: CN
Inventors: 储志伟; 陈雪峰
Original assignee: Guangdong Bozhilin Robot Co Ltd
Current assignee: Guangdong Bozhilin Robot Co Ltd
Priority date: 2020-07-28
Filing date: 2020-07-28
Publication date: 2020-10-30
Anticipated expiration: 2040-07-28
Also published as: CN111854733B

Abstract

The application provides a multi-source fusion positioning method and system, which relate to the technical field of positioning, and the method is used in a multi-source fusion positioning system comprising an IMU, a GNSS RTK, a UWB device, a wheel type odometer and a VIO, and comprises the following steps: setting positioning priorities of the GNSS RTK, the UWB equipment, the wheel type odometer and the VIO from high to low, judging whether the positioning equipment in the GNSS RTK, the UWB equipment, the wheel type odometer and the VIO meets corresponding positioning requirements or not according to the priorities, and if the positioning equipment exists, triggering the corresponding positioning equipment and the IMU to perform fusion positioning to obtain a positioning result. Therefore, the implementation of the embodiment can select the optimal positioning technology in the specified environment, thereby improving the accuracy of positioning.

Description

Multi-source fusion positioning method and system

Technical Field

The application relates to the technical field of positioning, in particular to a multi-source fusion positioning method and system.

Background

At present, more and more positioning technologies appear in front of people, and tools such as navigation and electronic maps are provided for people, so that the convenience degree of work and life of people is improved. However, in practice, it is found that the current positioning technologies have more or less corresponding limitations, so that when different positioning technologies are used in different environments, the positioning result has a certain uncertainty, and thus the accuracy of positioning is affected.

Disclosure of Invention

An object of the embodiments of the present application is to provide a multi-source fusion positioning method and system, which can select an optimal positioning technology in a specified environment, so as to improve the accuracy of positioning.

In a first aspect, an embodiment of the present application provides a multi-source fusion positioning method, which is used in a multi-source fusion positioning system including an IMU, a GNSS RTK, a UWB device, a wheel odometer, and a VIO, and the method includes the following steps:

the method comprises the following steps: judging whether the GNSS RTK positioning state is an RTK locking state, and performing fusion positioning through the IMU and the GNSS RTK to obtain a positioning result when the GNSS RTK positioning state is the RTK locking state; entering a second step when the GNSS RTK positioning state is not the RTK locking state;

step two: judging whether the precision factor of the UWB equipment is greater than a preset first precision threshold, and performing fusion positioning through the IMU and the UWB equipment to obtain a positioning result when the precision factor of the UWB equipment is not greater than the first precision threshold; entering a third step when the UWB device precision factor is greater than the first precision threshold;

step three: judging whether the wheel-type odometer slips or not, and performing fusion positioning through the IMU and the wheel-type odometer to obtain a positioning result when the wheel-type odometer does not slip; when the wheel type odometer slips, entering a fourth step;

step four: and starting the VIO, and performing fusion positioning through the IMU and the VIO to obtain a positioning result.

In the implementation process, the method can preferentially judge whether the GNSS RTK positioning state is the RTK locking state in each positioning cycle; when the GNSS RTK positioning state is not the RTK locking state, judging whether the UWB accuracy factor is larger than a preset first accuracy threshold value or not, so that UWB positioning becomes a second priority; when the UWB precision factor is larger than a first precision threshold value, judging whether the wheel type odometer slips or not, and enabling the wheel type odometer to be positioned to be a third priority; when the wheel type odometer slips, the VIO at the fourth priority is triggered to be started, and fusion positioning is carried out through the IMU and the VIO to obtain a positioning result. Therefore, by implementing the implementation mode, the GNSS RTK, the UWB equipment, the wheel type odometer and the VIO can be divided into four priorities, and the optimal positioning mode can be determined in various positioning equipment in real time according to the actual situation, so that the accuracy of the positioning result is highest, and the reliability is strongest.

Further, in the method, when the GNSS RTK positioning state is the RTK locking state, a positioning result is obtained by performing fusion positioning by the IMU and the GNSS RTK.

In the implementation process, in the first priority, the IMU and the GNSS RTK may be used for performing fusion positioning, so as to obtain a high-precision positioning result.

Further, in the method, when the UWB accuracy factor is not greater than the first accuracy threshold, performing fusion positioning by the IMU and the UWB to obtain a positioning result.

In the implementation described above, in this second priority, the IMU and UWB may be used for fusion positioning, thereby obtaining a relatively high accuracy positioning result.

Further, in the method, when the wheel type odometer does not slip, fusion positioning is carried out through the IMU and the wheel type odometer, and a positioning result is obtained.

In the implementation process, in the third priority, the IMU and the wheel odometer may be used for fusion positioning, so as to obtain a relatively stable positioning result.

Further, the method further comprises:

judging whether the VIO detection precision is greater than a preset second precision threshold value or not;

when the VIO detection precision is larger than the second precision threshold, triggering and executing the step of judging whether the GNSS RTK positioning state is an RTK locking state;

and triggering and executing the step of performing fusion positioning through the IMU and the VIO to obtain a positioning result when the VIO detection precision is not greater than the second precision threshold.

In the implementation process, when the VIO detection precision is insufficient, the repeated judgment of the RTK locking state is triggered, so that the positioning judgment is carried out again from the first priority, and the positioning result can be obtained.

Further, after the positioning result is obtained in the first step, the second step, the third step or the fourth step, the step of determining whether the GNSS RTK positioning state is the RTK locking state is triggered to be executed.

In the implementation process, after the positioning result is obtained, whether the positioning priority can be improved is judged, so that switching to higher priority positioning in the low priority positioning process is ensured, and the positioning precision is further ensured.

Further, the method further comprises:

judging whether a starting signal is received or not;

and when the starting signal is received, triggering and executing the step of judging whether the UWB precision factor is larger than a preset first precision threshold value.

In the implementation process, when the starting signal is received, the RTK preferentially enters the second priority, so that the time loss that the RTK is not in a state necessarily is avoided, and the overall positioning efficiency and effect are ensured.

Further, after the positioning result is obtained in the first step, the second step, the third step or the fourth step, the method further includes:

and correcting and updating the wheel type odometer according to the positioning result.

In the implementation process, the wheel-type odometer is corrected and updated according to actual conditions, so that the positioning effect precision of the wheel-type odometer is high.

Further, after obtaining the positioning result, the method further includes:

judging whether the current positioning mode is the IMU and VIO fusion positioning mode;

and when the current positioning mode is not the IMU and VIO fusion positioning mode and the VIO is in an open state, controlling the VIO to shut down.

In the implementation process, the VIO restart mechanism improves the accuracy of multi-scene navigation positioning, realizes continuous seamless navigation in complex multi-scenes such as indoor and outdoor scenes and the like, and can reduce certain power consumption.

A second aspect of the embodiments of the present application provides a multi-source fusion positioning system, which includes:

the device comprises a first judging unit, a second judging unit and a control unit, wherein the first judging unit is used for judging whether the GNSS RTK positioning state is an RTK locking state;

a second judging unit, configured to judge whether the UWB accuracy factor is greater than a preset first accuracy threshold when the GNSS RTK positioning state is not the RTK locking state;

the third judging unit is used for judging whether the wheel type odometer slips or not when the UWB precision factor is larger than the first precision threshold value;

the fourth judgment unit is used for judging whether the VIO detection precision is greater than a preset second precision threshold value or not when the wheel type odometer slips;

and the positioning unit is used for performing fusion positioning through the IMU and the VIO when the VIO detection precision is not greater than the second precision threshold value to obtain a positioning result.

In the implementation process, the fusion work among the units can greatly improve the working efficiency and ensure the working stability; meanwhile, the multi-element fusion positioning device can determine the optimal positioning result according to the positioning priority, thereby ensuring the highest positioning precision and the best effect.

Further, the first determining unit is further configured to determine whether the GNSS RTK positioning state is an RTK locking state after the positioning result is obtained by the fusion positioning unit.

Further, the multi-source fusion positioning system further comprises:

and the correction updating unit is used for correcting and updating the wheel-type odometer according to the positioning result after the fusion positioning unit obtains the positioning result.

Further, the multi-source fusion positioning system further comprises:

a fifth judging unit, configured to judge whether a current positioning mode is the IMU and VIO fusion positioning mode;

and the shutdown unit is used for controlling the VIO to perform shutdown processing when the current positioning mode is not the IMU and the VIO fusion positioning mode and the VIO is in an open state.

A third aspect of embodiments of the present application provides an electronic device, including a memory and a processor, where the memory is used to store a computer program, and the processor runs the computer program to enable the electronic device to perform the multi-source fusion positioning method according to any one of the first aspect of embodiments of the present application.

A fourth aspect of the embodiments of the present application provides a computer-readable storage medium, which stores computer program instructions, where the computer program instructions, when read and executed by a processor, perform the multi-source fusion positioning method according to any one of the first aspect of the embodiments of the present application.

Drawings

In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings that are required to be used in the embodiments of the present application will be briefly described below, it should be understood that the following drawings only illustrate some embodiments of the present application and therefore should not be considered as limiting the scope, and that those skilled in the art can also obtain other related drawings based on the drawings without inventive efforts.

Fig. 1 is a schematic flow chart of a multi-source fusion positioning method according to an embodiment of the present disclosure;

fig. 2 is a schematic flowchart of another multi-source fusion positioning method according to an embodiment of the present disclosure;

fig. 3 is a schematic flowchart of another multi-source fusion positioning method according to an embodiment of the present disclosure;

fig. 4 is a schematic flowchart of another multi-source fusion positioning method according to an embodiment of the present disclosure;

fig. 5 is a schematic structural diagram of a multi-source fusion positioning system according to an embodiment of the present disclosure;

fig. 6 is a schematic structural diagram of another multi-source fusion positioning system provided in the embodiment of the present application;

fig. 7 is a schematic hardware composition diagram of a multi-source fusion positioning system according to an embodiment of the present disclosure;

fig. 8 is a flowchart of a restart mechanism for observing priorities and updating of a multi-source fusion positioning system according to an embodiment of the present disclosure;

fig. 9 is a diagram of a standard kalman filter iteration structure provided in an embodiment of the present application;

FIG. 10 is a table of multi-source fusion positioning mode classifications provided in accordance with an embodiment of the present application;

fig. 11 is a horizontal movement track under the indoor UWB partial occlusion condition according to an embodiment of the present application;

FIG. 12 is a partially enlarged view of a horizontal movement trace under partial indoor UWB shielding according to an embodiment of the present application;

fig. 13 is a horizontal movement track of an outdoor GNSS provided in the embodiment of the present application under a partial occlusion condition.

Detailed Description

The technical solutions in the embodiments of the present application will be described below with reference to the drawings in the embodiments of the present application.

It should be noted that: like reference numbers and letters refer to like items in the following figures, and thus, once an item is defined in one figure, it need not be further defined and explained in subsequent figures. Meanwhile, in the description of the present application, the terms "first", "second", and the like are used only for distinguishing the description, and are not to be construed as indicating or implying relative importance.

Example 1

Referring to fig. 1, fig. 1 is a schematic flow chart of a multi-source fusion positioning method according to an embodiment of the present application. The multi-source fusion positioning method comprises the following steps:

s101, judging whether the GNSS RTK positioning state is an RTK locking state, and if so, executing a step S107; if not, step S102 is executed.

In the embodiment of the application, the method is used for a multi-source fusion positioning system comprising GNSS RTK, UWB equipment, a wheel type odometer and VIO. The optimal observation quantity can be selected in different scenes such as indoor and outdoor scenes, and the positioning precision of multi-source fusion navigation is improved; and the mechanism of machine selection updating and restarting is applied to the wheel type/visual odometer, so that the accumulated error of the observed quantity is eliminated in time, the continuous navigation and positioning of different scenes can be realized, and the overall power consumption of the navigation module is reduced.

In the embodiment of the application, an RTK (Real-time kinematic) carrier phase differential technique is a differential method for processing carrier phase observations of two measurement stations in Real time, and sends information such as carrier phases acquired by a reference station to a user receiver for calculating a difference and a coordinate. The RTK positioning technology is a real-time dynamic positioning technology based on a carrier phase observation value, can provide a three-dimensional positioning result of a measuring station in a specified coordinate system in real time and achieves centimeter-level precision. In the RTK mode of operation, the base station transmits its observations to the rover station along with the coordinate information of the rover station via the data chain. The rover station not only receives data from the reference station through a data chain, but also collects observation data of a Global Navigation Satellite System (GNSS), and forms differential observation values in the System for real-time processing, and simultaneously provides a centimeter-level positioning result.

In the embodiment of the present application, the GNSS uses observations such as pseudoranges, ephemeris, and satellite transmission time of a set of satellites, and also needs to know a user clock error. The global navigation satellite system is a space-based radio navigation positioning system that can provide users with all-weather 3-dimensional coordinates and velocity and time information at any location on the earth's surface or in near-earth space.

In the embodiment of the application, the UWB (Ultra Wide Band) technology is a wireless carrier communication technology, and it does not adopt sinusoidal carrier, but utilizes nanosecond-level non-sinusoidal narrow pulse to transmit data, so that the occupied frequency spectrum range is very Wide, and the UWB (Ultra Wide Band) technology has the advantages of low system complexity, low power spectrum density of transmitted signals, insensitivity to channel fading, low interception capability, high positioning accuracy and the like, and is particularly suitable for high-speed wireless access in dense multipath places such as indoor places.

In the embodiment of the present application, a VIO (visual-inertial odometer), also called a visual inertial system (VINS), is an algorithm for fusing camera and IMU data to realize SLAM (instant positioning and mapping), and is divided into tight coupling and loose coupling according to the difference of a fusion framework, where the visual motion estimation and inertial motion estimation systems in loose coupling are two independent modules, and the output result of each module is fused, and the tight coupling is to jointly estimate a set of variables using the raw data of two sensors, and the sensor noise is also influenced mutually, and the tight coupling algorithm is relatively complex, but makes full use of the sensor data, so as to achieve a better effect, and is a key point in current research.

S102, judging whether the UWB precision factor is larger than a preset first precision threshold value or not, and if not, executing a step S106; if so, step S103 is performed.

In the embodiment of the present application, the UWB Precision factor is a Precision factor DOP, and the Precision factor DOP may be one of a Position Precision factor (PDOP), a time difference Precision factor (TDOP), a horizontal component Precision factor (HDOP), a vertical component Precision factor (VDOP), and the like, which is not limited in the embodiment of the present application.

S103, judging whether the wheel type odometer slips or not, and if not, executing a step S105; if so, go to step S104.

In the embodiment of the present application, an imu (inertial measurement unit) is a device for measuring a three-axis angular velocity and an acceleration of an object. Typical IMUs include three-axis gyroscopes and three-axis accelerometers. The IMU information is measurement data of the IMU device.

In the embodiment of the present application, the wheel-type odometer is a device that estimates a change in the position of an object with time by using data obtained from a motion sensor, and may specifically be a wheel-type odometer or the like, and the embodiment of the present application is not limited thereto. The odometer information is measurement data of the wheel type odometer.

And S104, starting the VIO, performing fusion positioning through the IMU and the VIO to obtain a positioning result, and ending the process.

In the embodiment of the application, when the VIO detection precision is judged to be not greater than the preset second precision threshold, an IMU/VIO fusion mode is entered, and fusion positioning is carried out through the IMU and the VIO to obtain a positioning result.

In the embodiment of the application, the IMU/VIO fusion mode is used as the fusion mode with the lowest priority, in order to further guarantee the standby positioning information in the complex environment and avoid the overlarge positioning error, the second precision threshold is directly determined according to the characteristics of the final positioning information, such as output noise, change statistics and the like.

And S105, performing fusion positioning through the IMU and the wheel type odometer to obtain a positioning result, and ending the process.

In the embodiment of the application, when the wheel type odometer is judged not to slip, an IMU/wheel type odometer fusion mode is entered, fusion positioning is carried out through the IMU and the wheel type odometer, and a positioning result is obtained.

In the embodiment of the application, the criterion of whether to switch to the IMU/wheel-type odometer fusion mode is mainly to evaluate whether the wheel-type odometer slips or not according to respective one-step position recursion comparison of the IMU and the wheel-type odometer, and a critical threshold value of a comparison difference value can be determined by a test to ensure that wheel-type odometer recursion abnormality such as slipping occurs, so that VIO can be resolved and started in time in a large probability.

And S106, performing fusion positioning through the IMU and the UWB to obtain a positioning result, and ending the process.

In the embodiment of the application, when the UWB accuracy factor is judged to be not greater than the preset first accuracy threshold, the IMU/UWB fusion mode is entered, and fusion positioning is carried out through the IMU and the UWB to obtain the positioning result.

In the embodiment of the application, the criterion for switching to the IMU/UWB fusion mode is mainly the UWB positioning accuracy factor DOP, that is, whether the UWB accuracy factor (DOP) is the preset first accuracy threshold is determined. The positioning accuracy factor DOP is an evaluation value comprehensively defined by the UWB module according to base station distribution, ranging variation, and the like, and represents a first accuracy threshold value of the level positioning accuracy determined by experimental analysis.

S107, performing fusion positioning through the IMU and the GNSS RTK to obtain a positioning result, and ending the process.

In the embodiment of the application, when the GNSS RTK positioning state is judged to be the RTK locking state, the RTK locking state is entered, the precision is reliable when the RTK locking state is locked, the IMU/GNSS fusion mode can be entered according to the preset highest priority, namely fusion positioning is carried out through the IMU and the GNSS RTK, and the positioning result is obtained. The positioning result includes coordinates of a world coordinate system (WGS84 system).

In the embodiment of the application, the criterion for switching to the IMU/GNSS fusion mode is mainly to determine the RTK state position posType, that is, to determine whether the GNSS RTK positioning state is the RTK locking state. The RTK state position posType is a flag bit of whether the RTK carried by the satellite receiver chip is in a narrow lane fixed unlocking (locking) state or not.

In the embodiment of the application, the real-time state of the wheel-type odometer can be kept updated in an IMU/GNSS fusion mode; once the sensor enters the shielding mode or enters the room again, if the RTK is unlocked or the precision factor is too large, the data of the effective (reliable precision) sensor with the highest priority is selected for fusion according to the preset observation priority.

In this embodiment of the application, before step S104, the method may further include:

judging whether the VIO detection precision is greater than a preset second precision threshold value or not, and if not, executing the step S104; if so, step S101 is executed to continuously determine whether the GNSS RTK positioning state is the RTK locking state.

In this embodiment of the present application, the preset first precision threshold and the preset second precision threshold are preset, specifically, the precision factor threshold may be 1.5, and the like, and this is not limited in this embodiment of the present application.

In the embodiment of the application, the values of the preset first precision threshold and the preset second precision threshold can be obtained through independent tests or experiments.

In this embodiment, an execution subject of the method may be a computing device such as a computer, a server, a multi-source fusion positioning system, and the like, which is not limited in this embodiment.

In this embodiment of the application, an execution subject of the method may also be a smart device such as a smart phone and a tablet, which is not limited in this embodiment.

Referring to fig. 7, fig. 7 is a schematic diagram of a hardware component of a multi-source fusion positioning system according to the present embodiment. As shown in fig. 7, the hardware components of the multi-source fusion positioning system include a sensing source, a VIO module, a satellite module, an IMU module, a GNSS RTK module, a data transmission module, a UWB module, a wheel-type odometer, and a data transmission antenna. The method can solve the problem that the state estimation cannot achieve the optimal precision due to the influence of the signal characteristics of all sensing sources in any scene in the multi-source fusion positioning method, and simultaneously can also solve the problem that the wheel type odometer sensor cannot eliminate the accumulated error in time due to the fact that each sensing source (comprising a VIO module, an IMU module, a GNSS RTK module, a UWB module and the wheel type odometer) works independently from the beginning, so that continuous seamless navigation in different scenes cannot be realized.

In this embodiment, a value less than or equal to both the first accuracy threshold and the second accuracy threshold is used to indicate that the positioning accuracy is higher, and conversely, is used to indicate that the positioning accuracy is insufficient.

Therefore, by implementing the multi-source fusion positioning method described in fig. 1, the optimal positioning technology can be selected in a specified environment, thereby improving the accuracy of positioning.

Example 2

Please refer to fig. 2, fig. 2 is a schematic flowchart of a multi-source fusion positioning method according to an embodiment of the present application. As shown in fig. 2, the multi-source fusion positioning method includes:

s201, judging whether a starting signal is received or not, and if so, executing a step S203; if not, the flow is ended.

S202, judging whether the GNSS RTK positioning state is an RTK locking state, and if not, executing the step S203; if so, step S209 is performed.

S203, judging whether the UWB precision factor is larger than a preset first precision threshold value, and if not, executing a step S208; if so, step S204 is performed.

S204, judging whether the wheel type odometer slips or not, and if not, executing a step S207; if so, step S205 is performed.

S205, judging whether the VIO detection precision is larger than a preset second precision threshold, and if not, executing the step S206; if so, step S202 is performed.

And S206, starting the VIO, carrying out fusion positioning through the IMU and the VIO to obtain a positioning result, and executing the step S210.

S207, fusion positioning is carried out through the IMU and the wheel type odometer to obtain a positioning result, and step S210 is executed.

S208, fusion positioning is carried out through the IMU and the UWB to obtain a positioning result, and the step S210 is executed.

S209, performing fusion positioning through the IMU and the GNSS RTK to obtain a positioning result, and executing the step S210.

And S210, after a preset time interval, triggering to execute the step S202.

In this embodiment, the preset time interval may be from 0 second to any time, and is not limited in this embodiment.

In this embodiment, when the preset time interval is 0 second, it can be regarded that the time interval does not exist, i.e. the step S202 can be executed without waiting.

Therefore, by implementing the multi-source fusion positioning method described in fig. 2, the optimal positioning technology can be selected in a specified environment, thereby improving the accuracy of positioning.

Example 3

Referring to fig. 3, fig. 3 is a specific implementation process of a multi-source fusion positioning method provided in an embodiment of the present application, and fig. 3 is obtained by further refining fig. 2, where as an optional implementation manner, the multi-source fusion positioning method further includes:

s207, fusion positioning is carried out through the IMU and the wheel type odometer to obtain a positioning result, and step S211 is executed.

S208, fusion positioning is carried out through the IMU and the UWB to obtain a positioning result, and the step S211 is executed.

S209, fusion positioning is carried out through the IMU and the GNSS RTK to obtain a positioning result, and step S211 is executed.

S211, judging whether the VIO is opened or not, if so, executing step S212, and if not, executing step S210.

And S212, when the VIO is started, performing shutdown processing on the VIO, and executing the step S210.

In the embodiment of the application, through steps S211 to 212, a restart mechanism of the VIO can be realized, and the restart mechanism not only can reduce the power consumption of multi-source fusion positioning to the maximum extent, but also can eliminate the accumulated error generated by the independent work of the modules in time, and can also realize seamless connection (continuous positioning) when multiple fusion states are switched, thereby improving the positioning accuracy and stability in a complex environment.

In the embodiment of the application, in the IMU/VIO fusion mode, an independent module of a nonlinear optimization algorithm is generally adopted, and updating and resetting the data in real time often affects the convergence effect of the data, even causes divergence, so that a restart mechanism is adopted to process the data: here, the VIO priority is set to be the lowest, and the default is the shutdown state; when other sensing sources can not provide a better observation value, a starting (restarting) command is sent to the sensing sources, after the starting is successful, the sensing sources output data under a VIO local coordinate system, and dead reckoning can be continuously carried out according to the positioning information of the system at the moment of starting; once other sensing sources work normally and can provide a better observation value, priority switching is carried out immediately, and the VIO is closed, so that the influence of accumulated errors of the VIO module is reduced to the maximum extent; meanwhile, the probability of divergence of the VIO module in a complex environment can be reduced, and the power consumption of the navigation fusion system is saved.

In the embodiment of the application, a mechanism based on priority selection is adopted in the processing of the observed quantity, and a machine selection updating and VIO command restarting mechanism of the wheel type odometer is combined, so that the accuracy of multi-scene navigation and positioning is improved, continuous seamless navigation under complex multi-scenes such as indoor and outdoor scenes is realized, and certain power consumption can be reduced.

Referring to fig. 8, fig. 8 is a flowchart of the observation priority and the update restart mechanism of the multi-source fusion positioning system according to the present embodiment. As shown in fig. 8, the multi-source fusion positioning system employs cascaded kalman filtering (attitude Unscented Kalman Filtering (UKF), velocity/position standard Kalman Filtering (KF)) to realize multi-scene dynamic sensing of three-dimensional attitude, velocity, and position, wherein observed quantities of states can be provided by sensing sources, which include GNSS RTK devices, UWB devices, wheeled odometers, and VIO devices, wherein the GNSS RTK devices can detect outdoor motion states, including dual-antenna heading, velocity, position, etc.; the UWB device may detect indoor motion states including heading, speed, location, etc.; the wheel-type odometer can detect indoor and outdoor motion states including course, speed, position and the like; the VIO device may detect indoor and outdoor motion states including heading, speed, location, etc.

As shown in fig. 8, the priorities selected according to the adaptive performance of the assigned measurements in different environments are: GNSS (RTK) > UWB > wheel-type odometer > VIO, and the corresponding fusion mode is IMU/GNSS fusion mode > IMU/UWB fusion mode > IMU/wheel-type odometer fusion mode > IMU/VIO fusion mode. And only when the sensing source with higher priority is 'invalid' (or the precision factor is greater than a preset precision threshold), selecting the next sensing source with the highest priority according to the priority sequence as the observed quantity in the Kalman filtering iteration. When judging whether the sensing source is invalid, the preset precision threshold value comprises the first preset precision threshold value and the second precision threshold value.

In the embodiment of the application, under an IMU/GNSS fusion mode (outdoor RTK locking state) or an IMU/UWB fusion mode (good UWB precision), the course and the position of the wheel-type odometer are continuously updated by the fused course and position, so that the optimal navigation information can be tracked in real time, and meanwhile, the accumulated error is continuously eliminated. Once the outdoor RTK is unlocked or the UWB is interfered and the like, the observation priority optimal observation source is transmitted to the wheel type odometer for timing, the IMU/wheel type odometer fusion mode is switched, seamless connection can be guaranteed at the switching moment, and then the wheel type odometer is continuously relied on for calculating to obtain the current optimal positioning result.

As shown in fig. 8, on the basis of building the multi-source fusion positioning of IMU/GNSS/UWB/wheeled odometer/VIO fusion, the important point is to establish an improved fusion method mechanism of multi-source observation priority selection and VIO command restart.

In the embodiment of the application, the constructed mathematical models of the three-level Kalman filtering algorithm for the attitude, the speed and the position are respectively as follows:

1. the attitude estimation algorithm model comprises a state equation and a measurement equation, wherein the state equation is as follows:

the measurement equation is as follows:

wherein,

yaw (ψ) heading (GNSS dual antenna/UWB/wheel odometer/VIO heading);

2. the speed estimation algorithm model comprises a state equation and a measurement equation, wherein the state equation is as follows:

the measurement equation is as follows:

3. the position estimation algorithm model comprises a state equation and a measurement equation, wherein the state equation is as follows:

the measurement equation is as follows:

the measurement equations are linear, the speed and position stage state equations are linear, and standard five-step Kalman filtering iteration can be directly applied. Referring to fig. 9, fig. 9 is a diagram of a standard kalman filter iteration structure according to the present embodiment. FIG. 9 shows a block diagram of a standard five-step Kalman filter iteration.

Wherein, the attitude and heading state equation is nonlinear, and EKF (extended Kalman Filter) or UKF (unscented Kalman Filter) and the like can be applied. The embodiments of the present application are not limited thereto.

It can be seen that, by implementing the multi-source fusion positioning method described in this embodiment, a restart mechanism of the VIO can be implemented, and an optimal positioning technology is selected in a specified environment, thereby improving the accuracy of positioning.

Example 4

Referring to fig. 4, fig. 4 is a specific implementation process of a multi-source fusion positioning method provided in an embodiment of the present application, and fig. 4 is obtained by further refining fig. 2, where as an optional implementation manner, the multi-source fusion positioning method further includes:

s206, fusion positioning is carried out through the IMU and the VIO to obtain a positioning result, and the step S213 is executed.

S208, fusion positioning is carried out through the IMU and the UWB to obtain a positioning result, and the step S213 is executed.

S209, fusion positioning is carried out through the IMU and the GNSS RTK to obtain a positioning result, and the step S213 is executed.

And S213, correcting and updating the wheel-type odometer which acquires the odometer information according to the positioning result, and executing the step S210.

In the embodiment of the application, the fused course position is required to be used for updating the value of the wheel-type odometer in real time in an IMU/UWB fusion mode, so that the wheel-type odometer keeps current and reliable information output, and accumulated errors are eliminated in time.

In the embodiment of the application, the accumulated error of the wheel-type odometer sensor is eliminated in time by a passive updating mechanism adopted by the wheel-type odometer sensor, continuous seamless positioning of multiple scenes is ensured, and the continuous uninterrupted high-precision navigation capability under different environments is enhanced.

In the embodiment of the application, once the IMU/UWB fusion mode enters a UWB interference or shielding state, the algorithm can be switched to the IMU/wheel type odometer mode in time according to the criteria such as precision factors, and the like, and the previous wheel type odometer is always kept in a passive updating state, so that the navigation and positioning can be continuously and seamlessly continued; and if the wheel-type odometer is invalid, the VIO module can still be started (restarted), and navigation and positioning are continued according to superposition calculation.

In the embodiment of the application, the step S213 can realize the passive update of the opportunity selection of the wheel-type odometer, the update mechanism can eliminate the accumulated error generated by the independent work of the modules in time, and can realize the seamless connection (continuous positioning) during the switching of various fusion states, thereby improving the positioning accuracy and stability in a complex environment.

In the embodiment of the application, a mechanism that the RTK locking or UWB reliable time-wheel type odometer is updated in real time and the VIO is restarted at the lowest priority is utilized to ensure that continuous seamless navigation positioning can still be carried out when different scenes are entered subsequently.

Therefore, the multi-source fusion positioning method described in the embodiment can realize the wheeled odometer computer-selective updating, and select the optimal positioning technology in the specified environment, so as to improve the accuracy of positioning.

Example 5

Referring to fig. 5, fig. 5 is a schematic structural diagram of a multi-source fusion positioning system according to an embodiment of the present disclosure. As shown in fig. 5, the multi-source fusion positioning system includes:

a first determining unit 310, configured to determine whether the GNSS RTK positioning state is an RTK locking state;

a second determining unit 320, configured to determine whether the UWB accuracy factor is greater than a preset first accuracy threshold when the GNSS RTK positioning state is not the RTK locking state;

a third judging unit 330, configured to judge whether the wheel-type odometer slips when the UWB accuracy factor is greater than the first accuracy threshold;

the fusion positioning unit 350 is used for starting the VIO when the wheel-type odometer slips, and performing fusion positioning through the IMU and the VIO to obtain a positioning result;

the fusion positioning unit 350 is further configured to perform fusion positioning through the IMU and the GNSSRTK to obtain a positioning result when the GNSS RTK positioning state is the RTK locking state;

the fusion positioning unit 350 is further configured to perform fusion positioning through the IMU and the UWB device to obtain a positioning result when the accuracy factor of the UWB device is not greater than the first accuracy threshold;

and the fusion positioning unit 350 is further configured to perform fusion positioning through the IMU and the wheel type odometer when the wheel type odometer does not slip, so as to obtain a positioning result.

As an optional implementation, the multi-source fusion positioning system may further include:

the fourth judging unit 340 is configured to start the VIO when the wheel-type odometer slips, and judge whether the VIO detection accuracy is greater than a preset second accuracy threshold;

and a fusion positioning unit 350, configured to perform fusion positioning through the IMU and the VIO to obtain a positioning result when the VIO detection accuracy is not greater than the second accuracy threshold.

In this embodiment, for the explanation of the multi-source fusion positioning system, reference may be made to the description in embodiment 1 or embodiment 2, and details are not repeated in this embodiment.

It can be seen that, by implementing the multi-source fusion positioning system described in this embodiment, an optimal positioning technology can be selected in a specified environment, so as to improve the accuracy of positioning.

Example 6

Referring to fig. 6, fig. 6 is a schematic structural diagram of a multi-source fusion positioning system according to an embodiment of the present application. As shown in fig. 6, the positioning unit 350 is further configured to perform fusion positioning through the IMU and the GNSS RTK to obtain a positioning result when the GNSS RTK positioning state is the RTK locking state.

As an alternative embodiment, the first determining unit 310 is further configured to determine whether the GNSS RTK positioning state is the RTK locking state after the positioning result is obtained by the fusion positioning unit.

As an optional implementation manner, the fused positioning unit 350 is further configured to perform fused positioning through the IMU and the UWB when the UWB precision factor is not greater than the first precision threshold, so as to obtain a positioning result.

As an alternative embodiment, the fusion positioning unit 350 is further configured to perform fusion positioning through the IMU and the wheel odometer when the wheel odometer does not slip, so as to obtain a positioning result.

As an optional implementation, the multi-source fusion positioning system further includes:

a triggering unit 360, configured to trigger the first determining unit 310 to determine whether the GNSS RTK positioning state is the RTK locking state when the VIO detection accuracy is greater than the second accuracy threshold.

a fifth determining unit 370, configured to determine whether a power-on signal is received;

the triggering unit 360 is further configured to trigger the second determining unit 320 to determine whether the UWB accuracy factor is greater than a preset first accuracy threshold when the power-on signal is received.

and a correction updating unit 380, configured to, after the fusion positioning unit 350 obtains the positioning result, perform correction updating on the wheel-type odometer, which obtains the odometer information, according to the positioning result.

As an optional implementation manner, the fifth determining unit 370 is further configured to determine whether the current positioning mode is a VIO fusion positioning mode of the IMU and VIO;

and a shutdown unit 390, configured to control the VIO to perform shutdown processing when the current positioning mode is a VIO fusion positioning mode of the IMU and the VIO is in an open state.

As an alternative embodiment, the triggering unit 360 is further configured to trigger the step of determining whether the GNSS RTK positioning state is the RTK locking state after a preset time interval after obtaining the positioning result.

Referring to fig. 10, fig. 10 is a table of multi-source fusion positioning mode classification provided in this embodiment. As shown in fig. 10, the multi-source fusion positioning mode (fusion mode) includes an IMU mode (pure IMU), an IMU/VIO fusion mode (IMU/VIO), an IMU/wheel odometer fusion mode (IMU/wheel odometer), an IMU/UWB fusion mode (IMU/UWB), and an IMU/GNSS fusion mode (IMU/GNSS). Specifically, reference may be made to the content described in fig. 10, which is not described herein again.

In the embodiment of the application, after the multi-source fusion positioning system is powered on and started, each hardware module of the multi-source fusion positioning system is initialized, at this time, the multi-source fusion positioning system may be located indoors or outdoors, but even outdoors, certain time is still needed for RTK first locking. Therefore, when the Kalman filtering starts to operate, based on the observation priority sequence: GNSS (RTK) > UWB > wheeled odometer > VIO, wherein the RTK is not locked at the beginning stage (the positioning state of the GNSS RTK is not the RTK locking state), and if the UWB is detected to be effective (namely the UWB accuracy factor is judged to be not more than a preset first accuracy threshold), the IMU/UWB fusion mode is entered; if the UWB is invalid, the UWB precision factor is judged to be larger than a preset first precision threshold value, and the wheel type odometer is valid (namely the wheel type odometer is judged not to slip), an IMU/wheel type odometer fusion mode is entered; and if the UWB and the wheel type odometer are invalid, sending a starting command to the VIO module, and entering an IMU/VIO fusion mode.

In the embodiment of the application, if the IMU/UWB fusion mode is entered, the fused course position is required to be used for updating the value of the wheel type odometer in real time in the mode, so that the wheel type odometer keeps current and reliable information output, and accumulated errors are eliminated in time; once the wheeled odometer enters a UWB interference or shielding state in an IMU/UWB fusion mode, whether the wheeled odometer slips or not can be judged, and when the wheeled odometer is judged not to slip, the wheeled odometer is switched to the IMU/wheeled odometer mode in time, and the previous wheeled odometer is always kept in a passive updating state, so that the navigation and positioning can be continuously and seamlessly continued; if the wheel-type odometer is invalid (when the wheel-type odometer is judged to be slipped), the VIO module can be started (restarted), and when the VIO detection precision is not greater than a preset second precision threshold value, navigation positioning can still be continuously carried out according to superposition calculation.

In the embodiment of the application, if the RTK locking state is entered and the accuracy is reliable (namely the GNSS RTK positioning state is the RTK locking state), the IMU/GNSS fusion mode is entered according to the setting of the highest priority, the coordinates of the world coordinate system (WGS84 system) can be acquired at this time, and the real-time state of the wheeled odometer is kept updated in this mode as well; once the RTK is unlocked or the precision factor is too large when the RTK enters the shielding or enters the room again, selecting an effective (precision reliable) sensor with the highest priority for fusion according to the observation priority; and a mechanism that the RTK locking or UWB reliable time-wheel type odometer is used for real-time updating and the VIO is restarted at the lowest priority is utilized to ensure that continuous and seamless navigation positioning can still be carried out when different scenes are entered subsequently.

In the embodiment of the application, the process of switching steps aiming at different fusion modes can be regarded as reversible, if the VIO module is started due to unreliable other sensing sources, and at least one type of sensors such as RTK (real time kinematic), UWB (ultra wide band) and wheel type odometers recover a reliable positioning state in a certain scene, the Kalman filter still switches the observed quantity in time according to an observation priority sequence to send a shutdown command to the VIO module; and switching to an RTK or UWB effective state again in any state, and keeping the real-time passive updating state if the wheel type odometer is effective.

Referring to fig. 11, 12 and 13 together, fig. 11 is a horizontal movement track under the indoor UWB partial occlusion condition provided by the embodiment, fig. 12 is a partially enlarged view of the horizontal movement track under the indoor UWB partial occlusion condition provided by the embodiment, and fig. 13 is a horizontal movement track under the outdoor GNSS partial occlusion condition provided by the embodiment. As shown in fig. 11, 12 and 13, the ordinate represents the forward displacement of the positioned object, and the abscissa represents the right displacement of the positioned object. As can be seen from fig. 11, 12, and 13, the multi-source fusion positioning module provided in this embodiment can select an optimal observed quantity in different scenes, so as to avoid adverse effects on the fusion result when some sensors are unreliable, and improve the accuracy and the anti-interference capability of navigation positioning.

An embodiment of the present application provides an electronic device, which includes a memory and a processor, where the memory is used to store a computer program, and the processor runs the computer program to enable the electronic device to execute any multi-source fusion positioning method in embodiment 1 or embodiment 2 of the present application.

An embodiment of the present application provides a computer-readable storage medium, which stores computer program instructions, and when the computer program instructions are read and executed by a processor, the multi-source fusion positioning method according to any one of embodiment 1 or embodiment 2 of the present application is executed.

In the embodiments provided in the present application, it should be understood that the disclosed apparatus and method can be implemented in other ways. The apparatus embodiments described above are merely illustrative, and for example, the flowchart and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods and computer program products according to various embodiments of the present application. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that, in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.

The functions, if implemented in the form of software functional modules and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present application or portions thereof that substantially contribute to the prior art may be embodied in the form of a software product stored in a storage medium and including instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.

The above description is only for the specific embodiments of the present application, but the scope of the present application is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present application, and shall be covered by the scope of the present application. Therefore, the protection scope of the present application shall be subject to the protection scope of the claims.

It is noted that, herein, relational terms such as first and second, and the like may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Also, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising an … …" does not exclude the presence of other identical elements in a process, method, article, or apparatus that comprises the element.

Claims

1. A multi-source fusion positioning method used in a multi-source fusion positioning system comprising an IMU, a GNSS RTK, a UWB device, a wheeled odometer and a VIO, the method comprising the steps of:

the method comprises the following steps: judging whether the GNSS RTK positioning state is an RTK locking state, and performing fusion positioning through the IMU and the GNSS RTK to obtain a positioning result when the GNSS RTK positioning state is the RTK locking state; entering a second step when the GNSSRTK positioning state is not the RTK locking state;

2. The multi-source fusion positioning method according to claim 1, wherein after the positioning result obtained in the first step, the second step, the third step or the fourth step, the step of determining whether the GNSS RTK positioning state is the RTK locking state is triggered to be executed.

3. The multi-source fusion positioning method according to claim 1, wherein after the positioning result obtained in the first step, the second step, the third step or the fourth step, the method further comprises:

4. The multi-source fusion localization method of claim 1, further comprising:

5. A multi-source fusion positioning system comprising an IMU, a GNSS RTK, a UWB device, a wheeled odometer, and a VIO, the multi-source fusion positioning system comprising:

the second judging unit is used for judging whether the precision factor of the UWB equipment is greater than a preset first precision threshold value or not when the GNSS RTK positioning state is not the RTK locking state;

the third judging unit is used for judging whether the wheeled odometer slips or not when the precision factor of the UWB equipment is larger than the first precision threshold;

the fusion positioning unit is used for starting the VIO when the wheel-type odometer slips, and performing fusion positioning through the IMU and the VIO to obtain a positioning result;

the fusion positioning unit is further configured to perform fusion positioning through the IMU and the GNSS RTK to obtain a positioning result when the GNSS RTK positioning state is the RTK locking state;

the fusion positioning unit is further configured to perform fusion positioning through the IMU and the UWB device to obtain a positioning result when the accuracy factor of the UWB device is not greater than the first accuracy threshold;

and the fusion positioning unit is also used for performing fusion positioning through the IMU and the wheel type odometer when the wheel type odometer does not slip, so as to obtain a positioning result.

6. The multi-source fusion localization system of claim 5,

the first judging unit is further configured to judge whether the GNSS RTK positioning state is an RTK locking state after the positioning result is obtained by the fusion positioning unit.

7. The multi-source fusion localization system of claim 5, further comprising:

8. The multi-source fusion localization system of claim 5, further comprising:

9. An electronic device, comprising a memory for storing a computer program and a processor for executing the computer program to cause the electronic device to perform the multi-source fusion localization method of any one of claims 1 to 4.

10. A readable storage medium having stored thereon computer program instructions, which when read and executed by a processor, perform the multi-source fusion localization method according to any one of claims 1 to 4.