A multi-sensor based integrated navigation method and device

Abstract

The specification discloses a multi-sensor-based combined navigation method and device, which can acquire sensor data collected by multiple sensors. Then, the sensor data corresponding to the sensors is input into a second-level filter according to a first combination mode to perform information fusion, so as to obtain a state estimation value of a local state quantity. Then, the output result of at least part of the second-level filter or the sensor data of several sensors is input into a third-level filter according to a second combination mode to perform information fusion, so as to obtain a state estimation value of a global state quantity. Finally, the state estimation values of each navigation state quantity from different data sources are input into a fourth-level filter, so as to obtain a final estimation value of each navigation state quantity. By combining different sensors, multiple combined navigation systems with independent navigation capabilities are constructed to redundantly estimate the navigation state quantity, so that the navigation state quantity remains independent of each other in the combined navigation process, failure coupling is avoided, and the reliability and fault tolerance of the navigation system are improved.

Description

Technical Field

[0001] This application relates to the field of autonomous driving technology, and in particular to a multi-sensor-based integrated navigation method and device. Background Technology

[0002] With the development of autonomous driving technology, the application fields of drones are becoming increasingly widespread. In order to improve the navigation and positioning capabilities and reliability of drones, multi-sensor-based integrated navigation algorithms are usually used for drone positioning. This is to avoid inaccurate positioning due to the failure of individual sensors during flight, and to compensate for the shortcomings of single positioning algorithms.

[0003] Currently, a common integrated navigation method is the multi-source fusion integrated navigation method based on federated filtering, such as... Figure 1 As shown, a strapdown inertial navigation system (SINS) is used as the reference navigation source. It is fused with other navigation sources such as the Global Navigation Satellite System (GNSS), odometry, and altimeter, and estimates of each state variable are obtained through each sub-filter. Then, the state estimates and covariance matrices output from each sub-filter are input into the main filter for information fusion to obtain the optimal estimate.

[0004] However, in the above-mentioned integrated navigation method based on federated filtering, the output of each sub-filter is heavily dependent on the reference navigation source. When the reference navigation source malfunctions, the integrated navigation system cannot achieve accurate positioning. Summary of the Invention

[0005] This specification provides a multi-sensor-based integrated navigation method and apparatus to partially solve problems in the prior art.

[0006] The embodiments in this specification adopt the following technical solutions:

[0007] This specification provides a multi-sensor-based integrated navigation method, including:

[0008] Acquire sensor data collected by various types of sensors;

[0009] Several first combination methods corresponding to local state variables are determined, and for each determined first combination method, the sensor data of several types of sensors corresponding to the first combination method are input into a second-level filter for information fusion to obtain the output result of the second-level filter. The output result of the second-level filter includes at least the state estimate of the local state variables.

[0010] Several second combination methods corresponding to the global state variables are determined, and for each determined second combination method, the sensor data corresponding to the second combination method is input into a third-level filter for information fusion to obtain the output result of the third-level filter. The output result of the third-level filter includes at least the state estimate of the global state variables, and the sensor data corresponding to the second combination method includes at least part of the output result of the second-level filter or the sensor data of several types of sensors.

[0011] For each navigation state quantity, at least a portion of the output of the third-level filter and at least a portion of the output of the second-level filter corresponding to that navigation state quantity are input into the fourth-level filter corresponding to that navigation state quantity to obtain the final estimated value of the navigation state quantity output by the fourth-level filter. Based on the final estimated values ​​of each navigation state quantity, the current state of the autonomous driving device is determined.

[0012] Optionally, before inputting the sensor data of the several sensors corresponding to the first combination method into the second-stage filter, the method further includes:

[0013] For each type of sensor, the sensor data acquired by that sensor is filtered using a first-stage filter based on the sensor characteristics of that type.

[0014] Optionally, before inputting the sensor data of the several sensors corresponding to the first combination method into the second-stage filter, the method further includes:

[0015] For each type of sensor data, outliers are detected and compensated for.

[0016] Optionally, the number of sensors of at least some types is multiple;

[0017] Acquire sensor data collected by various types of sensors, specifically including:

[0018] For each type of sensor, when there are multiple sensors of that type, determine the sensor data output by each candidate sensor of that type;

[0019] Based on the state changes of the sensor data output by each candidate sensor, determine the quality index of each candidate sensor;

[0020] Based on the quality indicators of each candidate sensor, the target sensor corresponding to this type is determined from each candidate sensor, and the sensor data collected by the target sensor is used as the sensor data collected by this type of sensor.

[0021] The quality indicators include at least one of measurement accuracy, data noise, and data change rate.

[0022] Optionally, obtaining the final estimate of the navigation state quantity output by the fourth-level filter specifically includes:

[0023] Determine the final estimated value of the navigation state quantity at a historical moment, and use it as prior information;

[0024] Based on the prior information and the covariance matrix of the navigation state quantity output by at least part of the third-level filter, the confidence level of the state estimate of the navigation state quantity output by the third-level filter is determined.

[0025] Based on the prior information and the covariance matrix of the navigation state quantity output by at least part of the second-level filter, determine the confidence level of the state estimate of the navigation state quantity output by the second-level filter.

[0026] Based on the state estimates and confidence levels of the navigation state quantity output by each third-level filter, and at least some of the state estimates and confidence levels of the navigation state quantity output by the second-level filters, the final estimate of the navigation state quantity output by the fourth-level filter is obtained.

[0027] Optionally, before obtaining the final estimate of the navigation state quantity output by the fourth-level filter, the method further includes:

[0028] Determine whether the final estimate of the navigation state variable is consistent with the final estimate of the previous moment;

[0029] If so, the final estimated value of the navigation state quantity is output through the fourth-level filter;

[0030] If not, the final estimated value of the navigation state quantity is compensated, and the final estimated value of the navigation state quantity after compensation is output by the fourth-level filter.

[0031] This specification provides a multi-sensor-based integrated navigation device, including:

[0032] The acquisition module is configured to acquire sensor data collected by various types of sensors;

[0033] The first determining module is configured to determine several first combination methods corresponding to local state variables, and for each determined first combination method, input sensor data of several types of sensors corresponding to the first combination method into a second-level filter to perform information fusion and obtain the output result of the second-level filter. The output result of the second-level filter includes at least the state estimate value of the local state variable.

[0034] The second determining module is configured to determine several second combination methods corresponding to the global state quantity, and for each determined second combination method, input the sensor data corresponding to the second combination method into the third-level filter to perform information fusion and obtain the output result of the third-level filter. The output result of the third-level filter includes at least the state estimate value of the global state quantity, and the sensor data corresponding to the second combination method includes at least part of the output result of the second-level filter or the sensor data of several types of sensors.

[0035] The third determining module is configured to, for each navigation state quantity, input at least a portion of the output of the third-level filter and at least a portion of the output of the second-level filter corresponding to that navigation state quantity into the fourth-level filter corresponding to that navigation state quantity, to obtain the final estimated value of the navigation state quantity output by the fourth-level filter, so as to determine the current state of the autonomous driving device based on the final estimated value of each navigation state quantity.

[0036] Optionally, the third determining module is specifically used to: determine the final estimated value of the navigation state quantity obtained at a historical time as prior information; determine the confidence level of the state estimate of the navigation state quantity output by each third-level filter based on the prior information and the covariance matrix of the navigation state quantity output by at least a portion of the third-level filters; determine the confidence level of the state estimate of the navigation state quantity output by the second-level filter based on the prior information and the covariance matrix of the navigation state quantity output by at least a portion of the second-level filters; and determine the final estimated value of the navigation state quantity output by the fourth-level filter based on the state estimate of the navigation state quantity output by each third-level filter and its confidence level, and the state estimate of the navigation state quantity output by each second-level filter and its confidence level.

[0037] This specification provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the above-described multi-sensor-based integrated navigation method.

[0038] This specification provides an electronic device including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the above-described multi-sensor-based integrated navigation method.

[0039] The above-described at least one technical solution adopted in the embodiments of this specification can achieve the following beneficial effects:

[0040] In this specification, sensor data collected by various types of sensors is first acquired. Then, several first combination methods corresponding to local state variables are determined, and sensor data from the corresponding sensors are input into a second-level filter for information fusion according to each first combination method to obtain the state estimate of the local state variable. Next, several second combination methods corresponding to global state variables are determined, and sensor data from the corresponding sensors are input into a third-level filter for information fusion according to each second combination method to obtain the state estimate of the global state variable. The sensor data corresponding to the second combination methods includes at least part of the output results of the second-level filter or sensor data from several types of sensors. Finally, the state estimates of each navigation state variable from different data sources are input into a fourth-level filter to obtain the final estimate of each navigation state variable. By combining different sensors, multiple integrated navigation systems with independent navigation capabilities are constructed to perform redundant estimation of navigation state variables, ensuring their independence during integrated navigation, avoiding fault coupling, and improving the reliability and fault tolerance of the navigation system. Attached Figure Description

[0041] The accompanying drawings, which are included to provide a further understanding of this application and form part of this application, illustrate exemplary embodiments and are used to explain this application, but do not constitute an undue limitation of this application. In the drawings:

[0042] Figure 1 This is a structural diagram of an existing multi-source fusion integrated navigation system based on federated filtering;

[0043] Figure 2 A flowchart illustrating a multi-sensor-based integrated navigation method provided in an embodiment of this specification;

[0044] Figure 3 This specification provides a schematic diagram of the architecture of a combined navigation system as illustrated in an embodiment.

[0045] Figure 4 This specification provides a schematic diagram of the architecture of a combined navigation system as illustrated in an embodiment.

[0046] Figure 5 This specification provides a schematic diagram of the structure of a multi-sensor-based integrated navigation device as an embodiment of the present invention.

[0047] Figure 6 This is a schematic diagram of an electronic device that implements a multi-sensor-based integrated navigation method, as provided in the embodiments of this specification. Detailed Implementation

[0048] To make the objectives, technical solutions, and advantages of this specification clearer, the technical solutions of this application will be clearly and completely described below in conjunction with specific embodiments and corresponding drawings. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments in this specification without creative effort are within the scope of protection of this application.

[0049] It should be noted that all actions involving the acquisition of signals, information, or data in this application are carried out in compliance with the relevant data protection laws and policies of the country where the application is located, and with the authorization granted by the owner of the relevant device.

[0050] This specification provides a multi-sensor-based integrated navigation method. The technical solutions provided by the various embodiments of this application are described in detail below with reference to the accompanying drawings.

[0051] Figure 2 This specification provides a flowchart illustrating a multi-sensor-based integrated navigation method, which may include the following steps:

[0052] S100: Acquires sensor data collected by various types of sensors.

[0053] Currently, in order to ensure the reliability of navigation and positioning capabilities of autonomous driving equipment, multiple types of sensors are usually used in combination for navigation to avoid individual sensor failures and to compensate for the shortcomings of a single positioning algorithm.

[0054] Therefore, when performing combined navigation based on multiple sensors in this manual, the sensor data currently collected by the various sensors configured on the unmanned vehicle can be acquired first. These sensor types include at least an inertial measurement unit (IMU), a Global Navigation Satellite System (GNSS), a magnetometer, a barometer, a Time of Flight (TOF) distance sensor, and a camera.

[0055] It should be noted that the multi-sensor-based integrated navigation method described in this specification can be executed by the autonomous driving device or by the server controlling the autonomous driving device. When the server executes the integrated navigation method, the autonomous driving device can send sensor data collected by various sensors to the backend server, allowing the server to determine the current state of the autonomous driving device based on the sensor data collected by multiple sensors, in order to make control decisions. For ease of description, the following explanation will use the execution of this integrated navigation method by an autonomous driving device as an example.

[0056] Furthermore, to filter out interference signals in the sensor data, a first-stage filter can be used to filter the sensor data acquired by each sensor, based on its specific characteristics. Specifically, when the sensor data is high-frequency, low-frequency drift caused by interference is filtered out; when the sensor data is low-frequency, high-frequency noise is eliminated.

[0057] In one embodiment of this specification, after obtaining the sensor data collected by each sensor, it is also necessary to perform fault detection on the collected sensor data to filter out abrupt changes in the sensor data. Specifically, a fixed-size window can be slid across the sensor signal, and for each sampling point within the sliding window, the standard deviation of the data for each sampling point is determined. Then, based on the 3σ criterion, outliers are detected from each sampling point, and outliers that are detected as outliers are removed. Alternatively, data compensation can be performed on the outliers according to the standard deviation of the data for each sampling point within the sliding window to reduce the data discrepancies of the outliers.

[0058] Furthermore, to avoid navigation system failure due to sudden sensor malfunctions and to improve sensor reliability, this specification allows for the setting of multiple candidate sensors for sensor types with high status detection importance, such as IMU and GNSS. During navigation, the status changes of the sensor data output by each candidate sensor can be tracked to determine the quality indicators of each candidate sensor. Then, based on the quality indicators of each candidate sensor, a target sensor corresponding to that type is selected from the candidate sensors, and the sensor data collected by that target sensor is used as the sensor data collected by that type of sensor. The more stable the sensor data output, the lower the data noise, and the fewer the data jumps, the better the sensor quality.

[0059] Of course, if the measurement accuracy of the candidate sensors is different, the candidate sensor with higher measurement accuracy will be selected as the target sensor according to the priority order of measurement accuracy.

[0060] In one or more embodiments of this specification, since the acquisition frequencies of different types of sensors are not entirely the same, it is necessary to perform time synchronization processing on the sensor data collected by each sensor before subsequent integrated navigation to determine the sensor data of each sensor at the same time. Furthermore, the sensor data collected by each sensor is relative to its own sensor coordinate system. For example, the position of the autonomous driving device determined by the IMU is relative to the starting point of the IMU coordinate system. Therefore, it is also necessary to spatially align the sensor data collected by each sensor to transform the sensor data to the same reference coordinate system.

[0061] S102: Determine several first combination methods corresponding to local state variables, and for each determined first combination method, input the sensor data of several types of sensors corresponding to the first combination method into the second-level filter for information fusion to obtain the output result of the second-level filter.

[0062] After acquiring sensor data from multiple sensors on an autonomous driving device, information fusion can be performed on the data from these multiple sensors to obtain a more accurate state estimation result.

[0063] In one embodiment of this specification, sensor data collected by two or more heterogeneous sensors can be fused to estimate some navigation state parameters of the autonomous driving device. These navigation state parameters include at least navigation data such as position, speed, and attitude.

[0064] Specifically, when estimating local navigation state variables, at least some combinations can be selected from the various sensor combinations corresponding to each local state variable as the first combination. For example, combining an IMU with a magnetometer allows for the estimation of the autonomous vehicle's attitude information based on the collected sensor data. Combining a barometer, distance sensor, and camera allows for the estimation of the autonomous vehicle's altitude information based on the collected sensor data. Combining GNSS, IMU, and magnetometer allows for the acquisition of the autonomous vehicle's heading angle relative to the geographic North Pole using sensor data collected by the GNSS dual antennas, followed by obtaining the magnetic heading information through the IMU and magnetometer, and then estimating the autonomous vehicle's flight heading based on the fusion of the dual-antenna heading and magnetic heading. Combining an IMU with a camera allows for the estimation of the autonomous vehicle's position, attitude, and velocity based on the collected sensor data, and so on.

[0065] Next, for each determined first combination method, the sensor data of several sensors corresponding to that first combination method are input into a second-level filter for local filtering to obtain the output result of the second-level filter. This output result includes the state estimate and covariance matrix of the local state variables corresponding to that first combination method.

[0066] S104: Determine several second combination methods corresponding to the global state variables, and for each determined second combination method, input the sensor data corresponding to the second combination method into the third-level filter to perform information fusion and obtain the output result of the third-level filter.

[0067] In this specification, after obtaining each local state variable through the second-stage filter, the estimated value of the global state variable can be predicted based on each local state variable.

[0068] Specifically, when estimating the global navigation state variables, at least some combinations of sensors corresponding to the global state variables can be determined as a second combination. This second combination involves combining several second-level filters that solve for the local state variables with several types of sensors. For example, a camera, IMU, and GNSS can be combined to predict the estimated value of the global state variables. The camera and IMU correspond to the first combination of the aforementioned second-level filters.

[0069] Subsequently, for each determined second combination method, the sensor data corresponding to that second combination method is used as the input to the third-level filter for global filtering, yielding the output of the third-level filter. The output of the third-level filter includes the state estimate and covariance matrix of the global state variables. The sensor data corresponding to the second combination method includes at least a portion of the output of the second-level filter or sensor data from several different sensors.

[0070] Of course, in one embodiment of this specification, since sensor data collected by multiple sensors can cover all state variables required for navigation, the estimated values ​​of global state variables can be predicted based on the sensor data directly collected by various sensors. For example, a combination of GNSS and IMU, or a combination of lidar equipment and IMU, can predict the estimated values ​​of global state variables such as the position, attitude, and velocity of the unmanned vehicle by fusing the collected sensor data. The GNSS and IMU can be integrated in a loosely coupled, tightly coupled, or deeply coupled manner as needed, and this specification does not impose any restrictions on this.

[0071] In this specification, the second-stage and third-stage filters can employ traditional Kalman filtering, its derivative extended Kalman filtering, or other filtering algorithms such as particle filtering, sequential fusion, factor graph optimization, and convolutional neural networks.

[0072] S106: For each navigation state quantity, at least part of the output of the third-level filter and at least part of the output of the second-level filter corresponding to the navigation state quantity are input into the fourth-level filter corresponding to the navigation state quantity to obtain the final estimated value of the navigation state quantity output by the fourth-level filter, so as to determine the current state of the unmanned driving device based on the final estimated value of each navigation state quantity.

[0073] Once the estimated values ​​of each navigation state variable predicted by different data sources are determined, a decision module can be set up for each navigation state variable to select the optimal estimated value for each navigation state variable.

[0074] Specifically, for each navigation state variable, the state estimate and covariance matrix corresponding to that navigation state variable are determined from at least a portion of the outputs of the third-level filters, and at least a portion of the state estimate and covariance matrix corresponding to that navigation state variable are determined from the outputs of each second-level filter. Then, the state estimates and covariance matrices corresponding to that navigation state variable from different data sources are input into the fourth-level filter corresponding to that navigation state variable to obtain the final estimate output by the fourth-level filter, which is the optimal estimate of that navigation state variable. Finally, based on the final estimates of each navigation state variable of the autonomous driving device, the current state of the autonomous driving device is determined.

[0075] Furthermore, in the fourth-level filter corresponding to the navigation state variable, for state estimates from different data sources, a data self-check can be performed first, followed by a mutual check based on the state estimates from multiple data sources. During the data self-check, the final estimate of the navigation state variable output at the previous time step can be obtained and subtracted from the state estimate predicted by the data source. If the difference is less than a preset difference, the state estimate predicted by the data source is retained; otherwise, it indicates a sudden change in the state estimate predicted by the data source, and the outlier can be removed. During the mutual check, a chi-square test can be used, using the state estimate predicted by one data source as a reference to determine the degree of deviation of the state estimates predicted by the other data sources, and removing values ​​with large deviations.

[0076] Furthermore, in the fourth-level filter corresponding to the navigation state quantity, the final estimated value of the navigation state quantity obtained at historical moments can be determined as prior information. Then, based on the known prior information and the covariance matrix of the navigation state quantity output by at least a portion of the third-level filters, the confidence level of the state estimate of the navigation state quantity output by the third-level filters is determined. Similarly, based on the known prior information and the covariance matrix of the navigation state quantity output by at least a portion of the second-level filters, the confidence level of the state estimate of the navigation state quantity output by the second-level filters is determined. Finally, based on the state estimates and their confidence levels of the navigation state quantity output by each third-level filter and each second-level filter, the state estimate with the highest confidence level is selected as the final estimated value of the navigation state quantity.

[0077] Since the position and speed of autonomous vehicles do not change abruptly, to maintain the continuity of state changes, before outputting the final estimate, it is necessary to determine whether the final estimate of the navigation state variable is consistent with the final estimate of the previous moment. If the current final estimate is the same as the previous moment, the final estimate is directly output. Otherwise, compensation is applied to the final estimate of the navigation state variable to maintain its smoothness from the previous moment to the current moment.

[0078] Figure 3 This is a schematic diagram of the architecture of the integrated navigation system provided in the embodiments of this specification. The integrated navigation system includes multiple sensors such as an inertial measurement unit (IMU), a global navigation satellite system (GNSS), a magnetometer, a baroscope, and a camera. Then, from the combinations of sensors corresponding to local state variables, at least some combinations are determined as the first combination. The sensor data of each sensor in the corresponding combination are input into the corresponding second-level filter for local filtering to obtain the state estimate of the local state variables. In the figure, sensor data collected by the IMU and Magnometer are input into the second-level filter B1 to obtain the state estimate of the current attitude. Sensor data collected by the GNSS, IMU, and Magnometer are input into the second-level filter B2 to obtain the state estimate of the current heading. Sensor data collected by the IMU and camera are input into the second-level filter B3 to obtain the state estimates of the current pose and velocity.

[0079] Then, from the combinations of sensors corresponding to the global state variables, at least some combinations are determined as the second combination. The corresponding sensor data is then input into the corresponding third-level filter for global filtering to obtain the state estimate of the global state variables. The sensor data corresponding to the second combination includes at least some of the output results of the second-level filter or sensor data from several sensors. In the figure, for example, sensor data acquired by the IMU and GNSS are input into the third-level filter C1 to obtain the state estimates of the current position, velocity, and attitude. Sensor data acquired by the IMU and lidar are input into the third-level filter C2 to obtain the state estimates of the current position, velocity, and attitude. Sensor data acquired by the GNSS and the output result of the second-level filter B3 are input into the third-level filter C3 to obtain the state estimates of the current position, velocity, and attitude.

[0080] Finally, for each navigation state variable, the state estimates corresponding to that navigation state variable from different data sources are input into the corresponding fourth-level filter to obtain the final estimate of each navigation state variable.

[0081] Furthermore, the final output of the navigation system needs to be fed back to the second-level and third-level filters to correct the state estimates output by each filter, so as to predict the navigation state variables at the next moment based on the modified parameters.

[0082] Furthermore, to improve sensor reliability, the sensor data can be filtered by a first-stage filter before the second-stage filter at the sensor data output, removing noise. For example... Figure 4As shown, the sensor data collected by each sensor is input into the first-stage filter to filter out interference noise and outlier points are removed through outlier detection. For sensors with high importance for state prediction, such as the IMU and GNSS, multiple candidate sensors can be set, and switching can be performed when a sensor malfunctions.

[0083] like Figure 2 The multi-sensor-based integrated navigation method illustrated first acquires sensor data from multiple sensors. Then, following a first combination method, the sensor data from the corresponding sensors are input into a second-level filter for information fusion to obtain state estimates of local state variables. Following a second combination method, the corresponding sensor data and the state estimates of local state variables are input into a third-level filter for information fusion to obtain state estimates of global state variables. Finally, the state estimates of each navigation state variable from different data sources are input into a fourth-level filter to obtain the final estimates of each navigation state variable. By combining different sensors, multiple integrated navigation systems with independent navigation capabilities are constructed to perform redundant estimation of navigation state variables, ensuring their independence during integrated navigation, avoiding fault coupling, and improving the reliability and fault tolerance of the navigation system.

[0084] The integrated navigation method described in this specification combines the redundancy estimation results of various integrated navigation systems to obtain the optimal estimation results of navigation state variables, thus making the navigation results more accurate. Furthermore, by freely adjusting the combined sensor data at the algorithm level, it supports a plug-and-play approach, possessing flexibility and high scalability.

[0085] Furthermore, in this specification, for second-stage and third-stage filters that do not have a sequential input relationship, the execution order of steps S102 and S104 is not restricted and can be performed according to the filter's operating frequency. However, for second-stage and third-stage filters that have a sequential input relationship, step S102 must be executed first, followed by step S104.

[0086] based on Figure 2 The embodiment of this specification also provides a structural schematic diagram of a multi-sensor-based integrated navigation device, as shown in the example. Figure 5 As shown.

[0087] Figure 5 A schematic diagram of a multi-sensor-based integrated navigation device provided in this specification embodiment includes:

[0088] The acquisition module 200 is configured to acquire sensor data collected by various types of sensors;

[0089] The first determining module 202 is configured to determine a number of first combination methods corresponding to local state variables, and for each determined first combination method, input sensor data of a number of sensors corresponding to the first combination method into a second-level filter to perform information fusion and obtain the output result of the second-level filter. The output result of the second-level filter includes at least the state estimate value of the local state variable.

[0090] The second determining module 204 is configured to determine several second combination methods corresponding to the global state quantity, and for each determined second combination method, input the sensor data corresponding to the second combination method into the third-level filter to perform information fusion and obtain the output result of the third-level filter. The output result of the third-level filter includes at least the state estimate value of the global state quantity, and the sensor data corresponding to the second combination method includes at least part of the output result of the second-level filter or the sensor data of several types of sensors.

[0091] The third determining module 206 is configured to, for each navigation state quantity, input at least a portion of the output of the third-level filter and at least a portion of the output of the second-level filter corresponding to the navigation state quantity into the fourth-level filter corresponding to the navigation state quantity, to obtain the final estimated value of the navigation state quantity output by the fourth-level filter, so as to determine the current state of the unmanned driving device based on the final estimated value of each navigation state quantity.

[0092] Optionally, the first determining module 202 is further configured to, for each type of sensor, filter the sensor data collected by that type of sensor using a first-level filter based on the sensor characteristics of that type of sensor.

[0093] Optionally, the first determining module 202 is further configured to detect abnormal values ​​in the sensor data collected by each type of sensor, and compensate for the detected abnormal values.

[0094] Optionally, at least some types of sensors are multiple. The acquisition module 200 is specifically used to, for each type of sensor, when there are multiple sensors of that type, determine the sensor data output by each candidate sensor of that type, determine the quality index of each candidate sensor based on the state change of the sensor data output by each candidate sensor, determine the target sensor corresponding to that type from the candidate sensors based on the quality index of each candidate sensor, and use the sensor data collected by the target sensor as the sensor data collected by that type of sensor. The quality index includes at least one of measurement accuracy, data noise, and data change rate.

[0095] Optionally, the third determining module 206 is specifically used to: determine the final estimated value of the navigation state quantity obtained at a historical time as prior information; determine the confidence level of the state estimate of the navigation state quantity output by the third-level filter based on the prior information and the covariance matrix of the navigation state quantity output by at least a portion of the third-level filter; determine the confidence level of the state estimate of the navigation state quantity output by the second-level filter based on the prior information and the covariance matrix of the navigation state quantity output by at least a portion of the second-level filter; and determine the final estimated value of the navigation state quantity output by the fourth-level filter based on the state estimates of the navigation state quantity output by each third-level filter and their confidence levels, and the state estimates of the navigation state quantity output by each second-level filter and their confidence levels.

[0096] Optionally, the third determining module 206 is further configured to determine whether the final estimated value of the navigation state quantity is consistent with the final estimated value at the previous moment. If so, the final estimated value of the navigation state quantity is output through the fourth-level filter. If not, the final estimated value of the navigation state quantity is compensated, and the final estimated value of the navigation state quantity after compensation is output through the fourth-level filter.

[0097] This specification also provides a computer-readable storage medium storing a computer program that can be used to execute the above-described embodiments. Figure 2 The provided method is a multi-sensor-based integrated navigation method.

[0098] according to Figure 2 The multi-sensor-based integrated navigation method shown in this specification also proposes embodiments... Figure 6 The diagram shows a schematic structural representation of the electronic device. Figure 6 At the hardware level, this electronic device includes a processor, internal bus, network interface, memory, and non-volatile memory, and may also include other hardware required for business operations. The processor reads the corresponding computer program from the non-volatile memory into memory and then executes it to achieve the above. Figure 2 The method shown is a multi-sensor-based integrated navigation method.

[0099] Of course, in addition to software implementation, this specification does not exclude other implementation methods, such as logic devices or a combination of hardware and software. In other words, the execution subject of the following processing flow is not limited to each logic unit, but can also be hardware or logic devices.

[0100] In the 1990s, improvements to a technology could be clearly distinguished as either hardware improvements (e.g., improvements to the circuit structure of diodes, transistors, switches, etc.) or software improvements (improvements to the methodology). However, with technological advancements, many methodological improvements today can be considered direct improvements to the hardware circuit structure. Designers almost always obtain the corresponding hardware circuit structure by programming the improved methodology into the hardware circuit. Therefore, it cannot be said that a methodological improvement cannot be implemented using hardware physical modules. For example, a Programmable Logic Device (PLD) (e.g., a Field Programmable Gate Array (FPGA)) is such an integrated circuit whose logic function is determined by the user programming the device. Designers can program and "integrate" a digital system onto a PLD themselves, without needing chip manufacturers to design and manufacture dedicated integrated circuit chips. Moreover, nowadays, instead of manually generating integrated circuit chips, this programming is mostly implemented using "logic compiler" software. Similar to the software compiler used in program development, the original code before compilation must be written in a specific programming language, called a Hardware Description Language (HDL). There are many HDLs, such as ABEL (Advanced Boolean Expression Language), AHDL (Altera Hardware Description Language), Confluence, CUPL (Cornell University Programming Language), HDCal, JHDL (Java Hardware Description Language), Lava, Lola, MyHDL, PALASM, and RHDL (Ruby Hardware Description Language). Currently, the most commonly used are VHDL (Very-High-Speed ​​Integrated Circuit Hardware Description Language) and Verilog. Those skilled in the art should also understand that by simply performing some logic programming on the method flow using one of these hardware description languages ​​and programming it into an integrated circuit, the hardware circuit implementing the logical method flow can be easily obtained.

[0101] The controller can be implemented in any suitable manner. For example, it can take the form of a microprocessor or processor and a computer-readable medium storing computer-readable program code (e.g., software or firmware) executable by the (micro)processor, logic gates, switches, application-specific integrated circuits (ASICs), programmable logic controllers, and embedded microcontrollers. Examples of controllers include, but are not limited to, the following microcontrollers: ARC 625D, Atmel AT91SAM, Microchip PIC18F26K20, and Silicon Labs C8051F320. A memory controller can also be implemented as part of the control logic of the memory. Those skilled in the art will also recognize that, in addition to implementing the controller in purely computer-readable program code form, the same functionality can be achieved by logically programming the method steps to make the controller take the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers. Therefore, such a controller can be considered a hardware component, and the means included therein for implementing various functions can also be considered as structures within the hardware component. Alternatively, the means for implementing various functions can be considered as both software modules implementing the method and structures within the hardware component.

[0102] The systems, devices, modules, or units described in the above embodiments can be implemented by computer chips or entities, or by products with certain functions. A typical implementation device is a computer. Specifically, a computer can be, for example, a personal computer, laptop computer, cellular phone, camera phone, smartphone, personal digital assistant, media player, navigation device, email device, game console, tablet computer, wearable device, or any combination of these devices.

[0103] For ease of description, the above devices are described in terms of function, divided into various units. Of course, in implementing this specification, the functions of each unit can be implemented in one or more software and / or hardware components.

[0104] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0105] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0106] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0107] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0108] In a typical configuration, a computing device includes one or more processors (CPU), input / output interfaces, network interfaces, and memory.

[0109] Memory may include non-persistent storage in computer-readable media, such as random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM. Memory is an example of computer-readable media.

[0110] Computer-readable media includes both permanent and non-permanent, removable and non-removable media that can store information using any method or technology. Information can be computer-readable instructions, data structures, modules of programs, or other data. Examples of computer storage media include, but are not limited to, phase-change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technologies, CD-ROM, digital versatile optical disc (DVD) or other optical storage, magnetic tape, magnetic magnetic disk storage or other magnetic storage devices, or any other non-transferable medium that can be used to store information accessible by a computing device. As defined herein, computer-readable media does not include transient computer-readable media, such as modulated data signals and carrier waves.

[0111] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0112] Those skilled in the art will understand that the embodiments of this specification can be provided as methods, systems, or computer program products. Therefore, this specification may take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this specification may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0113] This specification can be described in the general context of computer-executable instructions that are executed by a computer, such as program modules. Generally, program modules include routines, programs, objects, components, data structures, etc., that perform a specific task or implement a specific abstract data type. This specification can also be practiced in distributed computing environments, where tasks are performed by remote processing devices connected via a communication network. In distributed computing environments, program modules can reside in local and remote computer storage media, including storage devices.

[0114] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to interchangeably. Each embodiment focuses on describing the differences from other embodiments. In particular, the system embodiments are basically similar to the method embodiments, so the description is relatively simple; relevant parts can be referred to the descriptions in the method embodiments.

[0115] The above description is merely an embodiment of this specification and is not intended to limit this specification. Various modifications and variations can be made to this specification by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this specification should be included within the scope of the claims of this specification.

Claims

1. A multi-sensor-based integrated navigation method, characterized in that, include: Acquire sensor data collected by various types of sensors; Several first combination methods corresponding to local state variables are determined, and for each determined first combination method, the sensor data of several types of sensors corresponding to the first combination method are input into a second-level filter for information fusion to obtain the output result of the second-level filter. The output result of the second-level filter includes at least the state estimate of the local state variables. Several second combination methods corresponding to the global state variables are determined, and for each determined second combination method, the sensor data corresponding to the second combination method is input into a third-level filter for information fusion to obtain the output result of the third-level filter. The output result of the third-level filter includes at least the state estimate of the global state variables, and the sensor data corresponding to the second combination method includes at least part of the output result of the second-level filter or the sensor data of several types of sensors. For each navigation state quantity, at least a portion of the output of the third-level filter and at least a portion of the output of the second-level filter corresponding to that navigation state quantity are input into the fourth-level filter corresponding to that navigation state quantity to obtain the final estimated value of the navigation state quantity output by the fourth-level filter. Based on the final estimated values ​​of each navigation state quantity, the current state of the autonomous driving device is determined.

2. The method as described in claim 1, characterized in that, Before inputting the sensor data of several sensors corresponding to the first combination method into the second-stage filter, the method further includes: For each type of sensor, the sensor data acquired by that sensor is filtered using a first-stage filter based on the sensor characteristics of that type.

3. The method as described in claim 1, characterized in that, Before inputting the sensor data of several sensors corresponding to the first combination method into the second-stage filter, the method further includes: For each type of sensor data, outliers are detected and compensated for.

4. The method as described in claim 1, characterized in that, At least some types of sensors are in multiple quantities; Acquire sensor data collected by various types of sensors, specifically including: For each type of sensor, when there are multiple sensors of that type, determine the sensor data output by each candidate sensor of that type; Based on the state changes of the sensor data output by each candidate sensor, determine the quality index of each candidate sensor; Based on the quality indicators of each candidate sensor, the target sensor corresponding to this type is determined from each candidate sensor, and the sensor data collected by the target sensor is used as the sensor data collected by this type of sensor. The quality indicators include at least one of measurement accuracy, data noise, and data change rate.

5. The method as described in claim 1, characterized in that, The final estimate of the navigation state quantity output by the fourth-level filter is obtained, specifically including: Determine the final estimated value of the navigation state quantity at a historical moment, and use it as prior information; Based on the prior information and the covariance matrix of the navigation state quantity output by at least part of the third-level filter, the confidence level of the state estimate of the navigation state quantity output by the third-level filter is determined. Based on the prior information and the covariance matrix of the navigation state quantity output by at least part of the second-level filter, determine the confidence level of the state estimate of the navigation state quantity output by the second-level filter. Based on the state estimates and confidence levels of the navigation state quantity output by each third-level filter and the state estimates and confidence levels of the navigation state quantity output by each second-level filter, the final estimate of the navigation state quantity output by the fourth-level filter is determined.

6. The method as described in claim 1, characterized in that, Before obtaining the final estimate of the navigation state quantity output by the fourth-level filter, the method further includes: Determine whether the final estimate of the navigation state variable is consistent with the final estimate of the previous moment; If so, the final estimated value of the navigation state quantity is output through the fourth-level filter; If not, the final estimated value of the navigation state quantity is compensated, and the final estimated value of the navigation state quantity after compensation is output by the fourth-level filter.

7. A multi-sensor-based integrated navigation device, characterized in that, include: The acquisition module is configured to acquire sensor data collected by various types of sensors; The first determining module is configured to determine several first combination methods corresponding to local state variables, and for each determined first combination method, input sensor data of several types of sensors corresponding to the first combination method into a second-level filter to perform information fusion and obtain the output result of the second-level filter. The output result of the second-level filter includes at least the state estimate value of the local state variable. The second determining module is configured to determine several second combination methods corresponding to the global state quantity, and for each determined second combination method, input the sensor data corresponding to the second combination method into the third-level filter to perform information fusion and obtain the output result of the third-level filter. The output result of the third-level filter includes at least the state estimate value of the global state quantity, and the sensor data corresponding to the second combination method includes at least part of the output result of the second-level filter or the sensor data of several types of sensors. The third determining module is configured to, for each navigation state quantity, input at least a portion of the output of the third-level filter and at least a portion of the output of the second-level filter corresponding to that navigation state quantity into the fourth-level filter corresponding to that navigation state quantity, to obtain the final estimated value of the navigation state quantity output by the fourth-level filter, so as to determine the current state of the autonomous driving device based on the final estimated value of each navigation state quantity.

8. The apparatus as claimed in claim 7, characterized in that, The third determining module is specifically used to: determine the final estimated value of the navigation state quantity obtained at a historical time as prior information; determine the confidence level of the state estimate of the navigation state quantity output by each third-level filter based on the prior information and the covariance matrix of the navigation state quantity output by at least a portion of the third-level filters; determine the confidence level of the state estimate of the navigation state quantity output by the second-level filter based on the prior information and the covariance matrix of the navigation state quantity output by at least a portion of the second-level filters; and obtain the final estimated value of the navigation state quantity output by the fourth-level filter based on the state estimate of the navigation state quantity output by each third-level filter and its confidence level, as well as the state estimate of the navigation state quantity output by at least a portion of the second-level filters and its confidence level.

9. A computer-readable storage medium, characterized in that, The storage medium stores a computer program, which, when executed by a processor, implements the method described in any one of claims 1 to 6.

10. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the method described in any one of claims 1 to 6.

Images