WO2023116797A2

WO2023116797A2 - In-vehicle multi-sensor fusion positioning method, computer device and storage medium

Info

Publication number: WO2023116797A2
Application number: PCT/CN2022/140863
Authority: WO
Inventors: 刘现款; 陈国芳
Original assignee: 比亚迪股份有限公司
Priority date: 2021-12-22
Filing date: 2022-12-22
Publication date: 2023-06-29
Also published as: CN113970330A; WO2023116797A3; CN113970330B

Abstract

Provided in the present application are an in-vehicle multi-sensor fusion positioning method, a computer device and a storage medium. The in-vehicle multi-sensor fusion positioning method comprises: acquiring IMU data, ODO data, Lidar data and combined navigation data; performing iterative optimization on IMU mileage data and fused IMU and ODO mileage data; acquiring Lidar posture data according to the optimized IMU mileage data, the optimized fused IMU and ODO mileage data, and the Lidar data; and performing joint optimization and fusion by means of a sliding window, so as to obtain a final positioning result.

Description

Vehicle-mounted multi-sensor fusion positioning method, computer equipment and storage medium

This application claims the priority of the Chinese patent application with the application number 202111579011.6 and the application name "Vehicle-mounted multi-sensor fusion positioning method, computer equipment and storage medium" submitted to the China Patent Office on December 22, 2021, the entire content of which is incorporated by reference incorporated in this application.

technical field

The present application relates to the technical field of vehicle positioning, in particular to a vehicle multi-sensor fusion positioning method, computer equipment and storage media.

Background technique

At present, regarding the positioning technology in rail vehicles, the positioning technology that has passed the sil4 safety integrity certification mainly relies on trackside equipment, such as transponders, axle counting and other equipment systems. In order to reduce trackside equipment, GNSS and wheel speed fusion are usually used Positioning method, the positioning method fuses the information of multiple sensors for positioning, and the fusion method is to use Kalman filter, particle filter and other technologies for fusion calculation. This fusion method is difficult to improve the positioning accuracy, and at the same time, the stability of the positioning system is poor, resulting in the inability to fully utilize the advantages of each sensor.

Contents of the invention

Embodiments of the present application provide a vehicle-mounted multi-sensor fusion positioning method, computer equipment, and storage media to solve the problems that the fusion positioning method adopted in the prior art is difficult to improve the positioning accuracy and the stability of the positioning system is poor.

The first aspect of the present application provides a vehicle-mounted multi-sensor fusion positioning method, including:

Obtain IMU data, ODO data, lidar data and combined navigation data, and integrate the IMU data and the ODO data to obtain IMU mileage data and IMU&ODO fusion mileage data;

Obtain integral constraints according to the lidar data or the GNSS data in the integrated navigation data, and obtain prior constraints according to the final positioning result output in the previous cycle, iteratively optimize the IMU mileage data according to the integral constraints, and Iteratively optimizing the IMU&ODO fusion mileage data according to the prior constraints;

Obtain lidar pose data according to the optimized IMU mileage data, the optimized IMU&ODO fusion mileage data and the lidar data;

The combined navigation data, the optimized IMU&ODO fusion mileage data, and the lidar pose data are jointly optimized and fused through the sliding window residual formula to obtain the final positioning result.

The second aspect of the present application provides a computer device, including a memory, a processor, and a computer program stored in the memory and operable on the processor. When the processor executes the computer program, the implementation of the present application The method described in the first aspect.

A third aspect of the present application provides a computer-readable storage medium, where the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method as described in the first aspect of the present application is implemented.

The application provides a vehicle-mounted multi-sensor fusion positioning method, computer equipment and storage media. The vehicle-mounted multi-sensor fusion positioning method includes obtaining IMU data, ODO data, laser radar data and combined navigation data, and obtaining IMU mileage data and IMU&ODO fusion mileage data ; Iteratively optimize the IMU mileage data according to the integral constraints and iteratively optimize the IMU&ODO fusion mileage data according to the prior constraints; obtain the lidar pose data according to the optimized IMU mileage data, the optimized IMU&ODO fusion mileage data and the lidar data ; The combined navigation data, optimized IMU&ODO fusion mileage data, and lidar pose data are jointly optimized and fused through the sliding window residual formula to obtain the final positioning result. The technical solution of this application integrates IMU data and ODO data. Since ODO data does not have random walk noise, ODO data also has an inhibitory effect on the random walk noise of IMU data. After relatively simple coordinate alignment, the integral mileage noise is directly used alone smaller. The random walk parameters of the IMU are corrected by integral constraints and prior constraints, and the accuracy of the corrected IMU data in the subsequent integral mileage can be guaranteed, which ensures the initial node pose accuracy of the sliding window constraint optimization fusion process, reduces the optimization iteration cycle, and improves The calculation speed is improved, the calculation accuracy is improved at the same time, the system stability is high, and the safety of the vehicle is improved.

Description of drawings

In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following will briefly introduce the accompanying drawings that need to be used in the description of the embodiments of the present application. Obviously, the accompanying drawings in the following description are only some embodiments of the present application , for those skilled in the art, other drawings can also be obtained according to these drawings without paying creative labor.

Fig. 1 is a schematic structural diagram of a safety computing platform in a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

FIG. 2 is a flow chart of a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

3 is a schematic diagram of IMU data security judgment in step S101 in a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

4 is a schematic diagram of ODO data security judgment in step S101 in a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

Fig. 5 is a schematic diagram of the safety judgment of integrated navigation data in step S101 in a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

FIG. 6 is a flow chart in step S101 of a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

Fig. 7 is a schematic diagram of vehicle trajectory in a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

8 is a schematic diagram of the IMU&ODO coordinate system distribution and conversion relationship in a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

FIG. 9 is a flow chart in step S102 of a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

FIG. 10 is a flow chart in step S102 of a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

FIG. 11 is a flow chart in step S103 of a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

FIG. 12 is another flow chart in step S103 of a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

Fig. 13 is a schematic diagram of point cloud feature information in step S103 in a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

FIG. 14 is a flow chart in step S104 of a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

FIG. 15 is a schematic diagram of sliding window constraint optimization fusion in step S104 in a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

Fig. 16 is a relationship diagram of information processing of the fusion positioning system in a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

Fig. 17 is a relationship diagram of information processing of the fusion positioning system in a vehicle-mounted multi-sensor fusion positioning method in an embodiment of the present application;

Fig. 18 is a schematic diagram of computer equipment in an embodiment of the present application.

Detailed ways

The following will clearly and completely describe the technical solutions in the embodiments of the present application with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all of them. Based on the embodiments in this application, all other embodiments obtained by persons of ordinary skill in the art without creative efforts fall within the protection scope of this application.

The embodiment of the present application provides a vehicle-mounted multi-sensor fusion positioning method, which can be applied to the control system of a rail vehicle. As shown in Figure 1, the control system includes a data acquisition sensor and a vehicle-mounted safety computing platform, wherein the data acquisition sensor includes a laser Radar, wheel speed sensor, inertial sensor, integrated navigation. The integrated navigation equipment includes inertial sensors, GNSS antennas, RTK antennas, and integrated navigation processors; the safety computing platform is equipped with a multi-sensor fusion positioning module software system and map data to perform fusion positioning based on information collected by multiple sensors.

In one embodiment, as shown in FIG. 2 , a vehicle-mounted multi-sensor fusion positioning method is provided, including step S101, step S102, step S103 and step S104, and the specific steps are as follows:

Step S101. Acquire IMU data, ODO data, lidar data and integrated navigation data, and integrate the IMU data and ODO data to obtain IMU mileage data and IMU&ODO mileage fusion data.

Wherein, the acquisition of IMU data is collected by an inertial sensor (IMU, Inertial Measurement Unit). As an implementation, as shown in Figure 3, the process of obtaining IMU data includes carrying out a security judgment on the IMU data. A group of inertial sensors obtain two sets of IMU data, and diagnose the acceleration and angular velocity divergence in the two sets of data. If the diagnosis is safe, calculate the average value of the acceleration and angular velocity. Among them, the diagnosis of acceleration and angular velocity divergence refers to judging whether the difference between the two sets of data is Within the preset error, it is considered safe to judge within the preset error.

Among them, the acquisition of ODO data is collected through the wheel speed sensor. As an embodiment, as shown in Figure 4, the process of obtaining ODO data includes making a safety judgment on the ODO data. The specific process is to obtain the ODO data through the wheel speed sensor. For the diagnosis of velocity and acceleration divergence in two groups of ODO data, if the diagnosis is safe, calculate the average value of velocity and acceleration. If it is within the error, it is regarded as safe.

Wherein, the acquisition of integrated navigation data is collected by integrated navigation equipment including IMU, GNSS antenna, RTK antenna, and integrated navigation processor. As an implementation, as shown in Figure 5, the process of obtaining integrated navigation data includes Data safety judgment, the specific process is to obtain two sets of combined navigation data through combined navigation equipment, combined navigation A position coordinates and speed and combined navigation B position coordinates and speed, and to diagnose the divergence of position coordinates and speed in the two sets of combined navigation data, If the diagnosis is safe, calculate the position coordinates and the average velocity. Among them, the position coordinate and velocity divergence diagnosis refers to judging whether the difference between the two sets of data is within the preset error, and it is considered safe if it is within the preset error.

Among them, as shown in Figure 6, the IMU data and ODO data are integrated to obtain IMU mileage data and IMU&ODO mileage fusion data, including:

Step S110. Integrate the IMU data to obtain the IMU mileage data.

Among them, due to the high frequency of IMU data, it is necessary to integrate the IMU data to synchronize the IMU data and ODO data. The IMU data includes accelerometer data, gyroscope data and heading angle. By integrating the acceleration, the speed and time are multiplied and the heading angle is obtained. Carry out integration to obtain mileage data.

Step S111. Integrate ODO data according to IMU data to obtain IMU&ODO fusion mileage data.

Among them, only the ODO data cannot be integrated, and the ODO data needs the heading angle, pitch angle and roll angle provided by the IMU to be integrated to obtain the IMU&ODO fusion mileage data.

Specifically, as shown in Figure 7 and Figure 8, the conversion of the offline velocity and angular velocity in the ODO coordinate system of the vehicle is as follows:

Where r _l , r _r are the radii of the left and right wheels, ω _l , ω _r are the angular velocities of the left and right wheels, and d is the left and right wheelbase;

Based on the IMU local coordinate system {I} to integrate the mileage through the dynamic equation (3), the IMU&ODO fusion integral dynamic equation is as follows:

X _k+1 = F _k X _k +G _k N _k (3)

Among them, X _k+1 is the state quantity of IMU&ODO at time K+1, X _k is the state quantity of IMU&ODO at time K, F _k is the state transition model matrix, N _k is the measurement noise vector, G _k observation matrix, the default is unit matrix;

For example: X _k = {P _xyz , V _xyz , R _rpy , bias _a , bias _ω }

C＝1+R _ryp *(ω _xyz -bias _ω )*dt

P _xyz represents the position coordinate state, V _xyz represents the velocity state, R _rpy represents the IMU direction state at time k, dt represents the time increment, a _xyz represents the acceleration information, ω _xyz represents the angular velocity information, bias _a represents the acceleration random walk, bias _ω represents a random walk with angular velocity.

The pre-integrated Jacobian matrix transfer formula is as follows:

J _k+1 = F _k J _k (4)

Covariance P transfer formula:

P _k+1 ＝F _k P _k F _k ^T +G _k QG _k ^T (5)

Where J _k and J _k+1 are the Jacobian matrix at k and k+1 respectively, P _k and P _k+1 are the covariance matrix at k and k+1 respectively, and Q is the covariance matrix of the noise signal N .

The IMU&ODO fusion mileage data is obtained through the pre-integration calculation of the above formula.

Step S102. Obtain the integral constraints according to the lidar data or the GNSS data in the integrated navigation data, and obtain the prior constraints according to the final positioning results output in the previous cycle, iteratively optimize the IMU mileage data according to the integral constraints and perform the iterative optimization on the IMU mileage data according to the prior constraints IMU&ODO fuses mileage data for iterative optimization.

Among them, since the data frequency of IMU&ODO is much higher than the frequency of radar Lidar and GNSS, the mileage integration between two frames of Lidar data or GNSS data is converted into mileage data, and the integral amount between two frames is the constraint of IMU mileage data. , the result of the latest cycle can be used as a priori constraint for IMU&ODO fusion mileage data, and the two constraints can calculate the vehicle pose state error.

Among them, as shown in Figure 9, the IMU mileage data is iteratively optimized according to the integral constraints, including:

Step S120. Comparing the lidar data or the GNSS data in the integrated navigation data with the IMU mileage data to obtain the vehicle pose state error.

Step S121. Adding the vehicle pose state error into the calculation process of the IMU mileage data to iteratively optimize the IMU mileage data.

Among them, the GNSS data in the lidar data or integrated navigation data can be regarded as an accurate value, and the iterative optimization of the GNSS data in the lidar data or integrated navigation data, IMU mileage data and random walk is established, and the current frame can be obtained through iterative optimization Random walk variation (vehicle pose state error), iteratively optimize the IMU mileage data during the calculation process of the IMU mileage data according to the machine walk variation, specifically when calculating the position value and velocity value, the acceleration value is replaced by the acceleration Values minus acceleration random walks, and angular velocity values replaced by angular velocity values minus angular velocity random walks.

Among them, as shown in Figure 10, the IMU&ODO fusion mileage data is iteratively optimized according to the prior constraints, including:

Step S122. Compare the final positioning result output in the previous cycle with the IMU&ODO fusion mileage data to obtain the vehicle pose state error.

Step S123. Adding the vehicle pose state error into the calculation process of the IMU&ODO fusion mileage data to iteratively optimize the IMU&ODO fusion mileage data.

Among them, since the real situation of the bias (random walk) of the IMU between two frames changes, the iterative optimization of the vehicle pose state error on the pose, speed and bias of the two frames is established. Through iterative optimization, the current frame random walk variation (vehicle pose state error) can be obtained. Through the above formula (4), the integral mileage of the random walk change can be calculated and updated in the iterative optimization process, based on the optimized random walk and IMU&ODO The fusion mileage data recalculates the IMU&ODO fusion mileage data increment after the prior constraint key frame time point, and the optimized IMU&ODO fusion mileage data can be obtained by adding the prior constraint key frame pose.

Step S103. Obtain the lidar pose data according to the optimized IMU mileage data, the optimized IMU&ODO fusion mileage data, and the lidar data.

Among them, this step includes two implementations, one implementation is to use the lidar positioning algorithm A point cloud feature frame nearest neighbor search algorithm to obtain the lidar pose data, and one implementation is to use the lidar positioning algorithm B point cloud feature frame The local map matching algorithm takes the lidar pose data.

As an implementation, as shown in Figure 11, the lidar pose data is obtained according to the optimized IMU mileage data, the optimized IMU&ODO fusion mileage data, and lidar data, including:

Step S131. Perform de-distortion calculation on the lidar data based on the IMU mileage data, and extract feature points from the lidar data to obtain feature points of the lidar data.

Step S132. Extract the local feature map according to the point cloud features and IMU&ODO fusion mileage data, quantify the local feature map in high-dimensional space, and search for the nearest neighbor feature point corresponding to the local feature map in the high-dimensional space, and obtain the position of the nearest neighbor key frame posture.

Among them, this embodiment adopts the point cloud feature frame nearest neighbor search algorithm to obtain the lidar pose data, obtains the point cloud data through the lidar device, and performs dedistortion calculation on the lidar data according to the pose data in the IMU mileage data to obtain the prediction The lidar pose, the lidar data is extracted and calculated to obtain the feature points of the lidar data, and the local feature map corresponding to the IMU&ODO fusion mileage data is extracted from the map corresponding to the point cloud feature according to the IMU&ODO fusion mileage data, and the local feature The map is quantified in high-dimensional space to obtain a set of feature points, and the nearest neighbor feature point corresponding to the feature point of the lidar data is searched in the feature point set to obtain the pose of the nearest neighbor key frame.

As an implementation, as shown in Figure 12, the lidar pose data is obtained according to the optimized IMU mileage data, the optimized IMU&ODO fusion mileage data and the lidar data, including:

Step S133. Perform de-distortion calculation on the lidar data based on the IMU mileage data, and extract feature points from the lidar data to obtain feature points of the lidar data.

Step S134. Extract the local feature map according to the point cloud features and the IMU&ODO fusion mileage data, match the feature points of the lidar data into the local feature map, and obtain the lidar pose data according to the matching result.

Among them, this embodiment adopts the point cloud feature frame and local map matching algorithm to obtain the lidar pose data, obtains the point cloud data through the lidar device, and performs de-distortion calculation on the lidar data according to the pose data in the IMU mileage data, and obtains Based on the predicted lidar pose, the feature points of the lidar data are extracted and calculated to obtain the feature points of the lidar data. According to the IMU&ODO fusion mileage data, the local feature map corresponding to the IMU&ODO fusion mileage data is extracted from the map corresponding to the point cloud features, and the laser The radar data feature points are matched to the local feature map, and the lidar pose data is obtained according to the matching results.

Among them, LiDAR positioning algorithm A and LiDAR positioning algorithm B are used in conjunction with each other. LiDAR positioning algorithm A can be used to detect LiDAR positioning algorithm B. For example, as shown in Figure 13, the feature points obtained by LiDAR positioning algorithm A are For a certain feature point in the local feature map, the detection of the laser radar positioning algorithm B can accurately obtain the specific position as a certain position before and after the feature point, and whether the detection of the laser radar positioning algorithm B obtained by the safety judgment is within the laser radar positioning Within the threshold range of algorithm A, if it is within the threshold range, it is determined that lidar positioning algorithm B has passed the safety judgment, and the positioning result of lidar positioning algorithm B is output.

Step S104. The combined navigation data, the optimized IMU&ODO fusion mileage data, and the lidar pose data are jointly optimized and fused through the sliding window to obtain the final positioning result.

Among them, the lidar pose data and the integrated navigation data include pose data and noise covariance data, which are jointly optimized and fused by means of a sliding window, and a joint residual function is created by combining the current cycle and the historical data of the previous n cycles to perform The joint optimization iteratively solves the state information of the state node of cycle n+1 (the position and attitude are Xk, and the velocity is Vk). For example, as shown in Figure 15, setting the sliding window can fuse 5 cycles, and the current cycle is 5. , 2, 3, 4, 5 cycle data to get the final positioning result, the current cycle is 6, according to the 2nd, 3, 4, 5, 6 cycle data to get the final positioning result, and so on, get the positioning of each cycle result.

Among them, as shown in Figure 14, the combined navigation data, optimized IMU&ODO fusion mileage data, and lidar pose data are jointly optimized and fused through sliding windows to obtain the final positioning results, including:

Step S141. Obtain the sliding window residual formula and jointly optimize iterative residual function according to the integrated navigation data of the current period, the optimized IMU&ODO fusion mileage data, the lidar pose data and the historical data of the previous n periods.

Step S142. Calculate the final positioning result according to the joint residual function and the joint optimization iterative residual function.

Among them, the sliding window residual formula is:

dt represents the time difference between two nodes, cov(L _kn ) represents the covariance matrix of lidar data, cov(G _kn ) represents the covariance matrix of integrated navigation data, cov(X _kn ) represents the covariance matrix of IMU&ODO fusion mileage data, X _k is the first The pose of the K period, X _kn is the pose of the Knth period, V _k is the velocity of the Knth period, V _kn is the velocity of the Knth period, I is the identity matrix, L _kn is the lidar data of the Knth period, G _kn is the combined navigation data of the Knth cycle;

The joint optimization iterative residual function is:

min{f([X _kn ...X _k ],[V _kn ...V _k ])} (7)

The covariance matrix corresponding to each sensor noise is used as the weight information of each residual item, which can be adaptive to the residual function in the dynamic sliding frame. For example, when the lidar noise cov(L _kn ) increases, the weight of the optimization residual corresponding to the first item decreases. When the integrated navigation noise cov(G _kn ) decreases (high positioning accuracy), the weight of the optimization residual corresponding to the second item increases, and the corresponding optimization variable is also greatly affected by the integrated navigation positioning.

In addition, the sensor information of n+1 cycles in the sliding frame is involved in the calculation of the residual function equation of formula 6, which will affect the optimized positioning state of n+1 nodes. The third term of the residual function equation constrains n+ 1 node positioning state, under the premise that the accuracy of the first n states is guaranteed, the state error of the n+1th state can be controlled within the error range of the linearized dynamic model. Sensor data linkage solution.

In one embodiment, before step S104, safety judgment is also performed on the combined navigation data, the optimized IMU&ODO fusion mileage data, and the lidar pose data.

Specifically, the integrated navigation data, the optimized IMU&ODO fusion mileage data, and the lidar pose data are all in the same coordinate system, and the difference between the integrated navigation data, the optimized IMU&ODO fusion mileage data, and the lidar pose data is judged Whether it is within the error range, if it is within the error range, pass the safety judgment.

As shown in Figure 16 and Figure 17, the technical solution of the present application is specifically described below through specific examples:

Integrated navigation device A performs device status detection and outputs the first group of combined navigation data, and combined navigation device B performs device status detection to output the second group of combined navigation data, and performs safety judgment on the first group of combined navigation data and the second group of combined navigation data, Detect whether the gap between the two sets of data is within the allowable error range, and output the combined navigation data when the safety judgment is passed.

The laser radar device detects the device state and outputs point cloud data, and the point cloud feature map information is stored in the secure computing platform.

IMU device A performs device status detection and outputs the first set of IMU data, and IMU device B performs device status detection and outputs the second set of IMU data, and performs security judgment on the first group of IMU data and the second group of IMU data to detect whether the gap between the two sets of data is Within the allowable error range, the IMU data is output when the safety judgment is passed.

ODO device A performs device status detection and outputs the first set of ODO data, ODO device B performs device status detection and outputs the second set of ODO data, and performs safety judgments on the first set of ODO data and the second set of ODO data to detect whether the gap between the two sets of data is ODO data is output when the safety judgment is passed within the allowable error range.

Integrate the acceleration of the IMU data to obtain the multiplication of speed and time, and integrate the heading angle to obtain the mileage data to obtain the IMU mileage data.

Combining the above formulas (1) to (5), the ODO data is integrated according to the heading angle, pitch angle and roll angle provided by the IMU to obtain the IMU&ODO fusion mileage data.

The IMU&ODO safety judgment is performed on the IMU mileage data and the IMU&ODO fusion mileage data. When the two data are consistent, the two data are output through the judgment.

The IMU mileage data and the IMU&ODO fusion mileage data are fused and integrated to obtain two frames of radar data. The amount of integration between the two frames is the constraint of the IMU mileage data. The results of the latest period of the sliding window constraint optimization fusion output can be used as a priori constraints for IMU&ODO fusion mileage data. The two constraints can calculate the residual, because the real situation of the bias (random walk) of the IMU between two frames changes. , to establish the iterative optimization of the state quantity residual of IMU&ODO fusion mileage data on the pose, velocity and random walk of two frames. Through iterative optimization, the random walk variation of the current frame can be obtained, the IMU mileage data can be optimized according to the random walk variation of the current frame, and the integral mileage that changes with the bias can be calculated and updated through the formula (4) in the iterative optimization process. Based on the optimized bias and IMU&ODO fusion mileage data, recalculate the fusion integral mileage increment after the prior constraint key frame time point, and add the prior constraint key frame pose to obtain the optimized IMU&ODO fusion mileage data.

According to the pose data in the IMU mileage data, de-migrate the point cloud data. Assuming that the lidar is moving linearly during the process of collecting lidar data, then according to the pose of the vehicle at the beginning of S1 frame acquisition and the end of S1 frame The pose is linearly interpolated to realize the offset calculation. Get the predicted lidar pose, extract and calculate the feature points from the lidar data, extract the local feature map corresponding to the IMU&ODO fusion mileage data from the point cloud features according to the IMU&ODO fusion mileage data, and carry out the local feature map in the high-dimensional space Quantize to obtain the feature point set, search the nearest neighbor feature point corresponding to the feature point of the lidar data in the feature point set, and obtain the pose of the nearest neighbor key frame. And match the lidar data feature points to the local feature map, and get the lidar pose data according to the matching result. Determine whether the lidar pose data is within the safe threshold range of the pose of the nearest neighbor key frame, and output the lidar pose data within the safe range.

Judging whether the difference between the integrated navigation data, the optimized IMU&ODO fusion mileage data, and the lidar pose data is within the error range, and the safety judgment is passed within the error range.

The combined navigation data, optimized IMU&ODO fusion mileage data, and lidar pose data are jointly optimized and fused through the sliding window residual formula (6) and formula (7) to obtain the final positioning result.

In this embodiment, the safety diagnosis of each sensor used and the safety diagnosis of the fusion result ensure the safety and integrity monitoring of the entire calculation process. Compared with the existing scheme, the security is higher; all fusion calculations in this embodiment are based on When the safety diagnosis result is good, it can ensure the safety integrity of the final positioning result and reduce the failure rate.

In this embodiment, the fusion results of IMU&ODO, integrated navigation and laser radar positioning results are used as prior constraints, and fed back to IMU&ODO fusion optimization constraints, so as to correct the random walk parameters of the IMU, and the accuracy of the corrected IMU data in the subsequent integral mileage can be guaranteed. , which ensures the accuracy of the initial node pose during the sliding window constraint optimization fusion process. The optimization iteration cycle is reduced and the calculation speed is improved.

In the fusion process of IMU and ODO in this embodiment, ODO does not have random walk noise, and ODO also has an inhibitory effect on the random walk noise of IMU, and the integrated mileage noise is smaller when used directly after relatively simple coordinate alignment.

The application provides a vehicle-mounted multi-sensor fusion positioning method. The vehicle-mounted multi-sensor fusion positioning method includes obtaining IMU data, ODO data, lidar data and combined navigation data, and obtaining IMU mileage data and IMU&ODO fusion mileage data; The mileage data is iteratively optimized and the IMU&ODO fusion mileage data is iteratively optimized according to the prior constraints; the lidar pose data is obtained according to the optimized IMU mileage data, the optimized IMU&ODO fusion mileage data and the lidar data; the combined navigation data, The optimized IMU&ODO fusion mileage data and lidar pose data are jointly optimized and fused through the sliding window residual formula to obtain the final positioning result. The technical solution of this application integrates IMU data and ODO data. Since ODO data does not have random walk noise, ODO data also has an inhibitory effect on the random walk noise of IMU data. After relatively simple coordinate alignment, the integral mileage noise is directly used alone smaller. The random walk parameters of the IMU are corrected by integral constraints and prior constraints, and the accuracy of the corrected IMU data in the subsequent integral mileage can be guaranteed, which ensures the initial node pose accuracy of the sliding window constraint optimization fusion process, reduces the optimization iteration cycle, and improves The calculation speed is improved, and the calculation accuracy is improved at the same time, and the system stability is high.

In one embodiment, a computer device is provided. The computer device may be a server, and its internal structure may be as shown in FIG. 18 . The computer device includes a processor, memory, network interface and database connected by a system bus. Wherein, the processor of the computer device is used to provide calculation and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, computer programs and databases. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage medium. The database of the computer equipment is used to store the data used in the vehicle passenger flow monitoring method of the above embodiment. The network interface of the computer device is used to communicate with an external terminal via a network connection. When the computer program is executed by the processor, a vehicle-mounted multi-sensor fusion positioning method is realized.

In one embodiment, a computer device is provided, including a memory, a processor, and a computer program stored on the memory and operable on the processor. When the processor executes the computer program, the vehicle-mounted multi-sensor fusion in the above-mentioned embodiments is realized. positioning method.

In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored, and when the computer program is executed by a processor, the vehicle-mounted multi-sensor fusion positioning method in the foregoing embodiments is implemented.

Those of ordinary skill in the art can understand that realizing all or part of the processes in the methods of the above embodiments can be completed by instructing related hardware through computer programs, and the computer programs can be stored in a non-volatile computer-readable storage medium , when the computer program is executed, it may include the procedures of the embodiments of the above-mentioned methods. Wherein, any references to memory, storage, database or other media used in the various embodiments provided in the present application may include non-volatile and/or volatile memory. Nonvolatile memory can include read only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in many forms such as Static RAM (SRAM), Dynamic RAM (DRAM), Synchronous DRAM (SDRAM), Double Data Rate SDRAM (DDRSDRAM), Enhanced SDRAM (ESDRAM), Synchronous Chain Synchlink DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

Those skilled in the art can clearly understand that for the convenience and brevity of description, only the division of the above-mentioned functional units and modules is used for illustration. In practical applications, the above-mentioned functions can be assigned to different functional units, Completion of modules means that the internal structure of the device is divided into different functional units or modules to complete all or part of the functions described above.

The above-described embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: it can still implement the foregoing embodiments Modifications to the technical solutions described in the examples, or equivalent replacements for some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the various embodiments of the application, and should be included in the Within the protection scope of this application.

Claims

A vehicle-mounted multi-sensor fusion positioning method, characterized in that it includes:

Obtain IMU data, ODO data, lidar data and combined navigation data, and integrate the IMU data and the ODO data to obtain IMU mileage data and IMU&ODO fusion mileage data;

Obtain integral constraints according to the lidar data or the GNSS data in the integrated navigation data, and obtain prior constraints according to the final positioning result output in the previous cycle, iteratively optimize the IMU mileage data according to the integral constraints, and Iteratively optimizing the IMU&ODO fusion mileage data according to the prior constraints;

Obtain lidar pose data according to the optimized IMU mileage data, the optimized IMU&ODO fusion mileage data and the lidar data; and

The combined navigation data, the optimized IMU&ODO fusion mileage data, and the lidar pose data are jointly optimized and fused through a sliding window to obtain a final positioning result.
The vehicle-mounted multi-sensor fusion positioning method according to claim 1, wherein said integrating said IMU data and said ODO data to obtain IMU mileage data and IMU&ODO mileage fusion data comprises:

integrating the IMU data to obtain IMU mileage data; and

The ODO data is integrated according to the IMU data to obtain the IMU&ODO fusion mileage data.
The vehicle-mounted multi-sensor fusion positioning method according to claim 2, wherein the iterative optimization of the IMU mileage data according to the integral constraint comprises:

Comparing the lidar data or the GNSS data in the integrated navigation data with the IMU mileage data to obtain a vehicle pose state error; and

The vehicle pose state error is added to the calculation process of the IMU mileage data to iteratively optimize the IMU mileage data.
The vehicle-mounted multi-sensor fusion positioning method according to claim 2 or 3, wherein the iterative optimization of the IMU&ODO fusion mileage data according to the prior constraints includes:

The final positioning result output in the previous cycle is compared with the IMU&ODO fusion mileage data to obtain the vehicle pose state error; and

The vehicle pose state error is added to the calculation process of the IMU&ODO fusion mileage data, to iteratively optimize the IMU&ODO fusion mileage data.
The vehicle-mounted multi-sensor fusion positioning method according to any one of claims 1 to 4, wherein the laser radar is obtained according to the optimized IMU mileage data, the optimized IMU&ODO fusion mileage data and the laser radar data Pose data, including:

Performing de-distortion calculation on the lidar data based on the IMU mileage data, and extracting feature points from the lidar data to obtain feature points of the lidar data; and

Extract local feature maps based on point cloud features and IMU&ODO fusion mileage data, quantify the local feature maps in high-dimensional space, and search the nearest neighbor feature points corresponding to the lidar data feature points in high-dimensional space to obtain the nearest neighbor key The pose of the frame.
The vehicle-mounted multi-sensor fusion positioning method according to any one of claims 1 to 4, wherein the laser radar is obtained according to the optimized IMU mileage data, the optimized IMU&ODO fusion mileage data and the laser radar data Pose data, including:

Performing de-distortion calculation on the lidar data based on the IMU mileage data, and extracting feature points from the lidar data to obtain feature points of the lidar data; and

The local feature map is extracted according to the point cloud features and the IMU&ODO fusion mileage data, the lidar data feature points are matched to the local feature map, and the lidar pose data is obtained according to the matching result.
The vehicle-mounted multi-sensor fusion positioning method according to any one of claims 1 to 6, wherein the combined navigation data, the optimized IMU&ODO fusion mileage data, the laser radar pose data The final positioning results are obtained by joint optimization and fusion through sliding windows, including:

According to the integrated navigation data of the current period, the optimized IMU&ODO fusion mileage data, the lidar pose data and the historical data of the previous n periods, the sliding window residual formula and the jointly optimized iterative residual function are obtained; and

A final positioning result is calculated according to the joint residual function and the joint optimization iterative residual function.
The vehicle-mounted multi-sensor fusion positioning method according to claim 7, characterized in that,

The sliding window residual formula is:

Among them, dt represents the time difference between two nodes, cov(L kn ) represents the covariance matrix of lidar data, cov(G kn ) represents the covariance matrix of integrated navigation data, cov(X kn ) represents the covariance matrix of IMU&ODO fusion mileage data, X k is the pose of the Kth cycle, X kn is the pose of the Knth cycle, V k is the velocity of the Kth cycle, V kn is the velocity of the Knth cycle, I is the identity matrix, and L kn is the lidar of the Knth cycle Data, G kn is the combined navigation data of the Knth period; and

The joint optimization iterative residual function is:

min{f([X kn ...X k ],[V kn ...V k ])}.
A computer device, comprising a memory, a processor, and a computer program stored in the memory and operable on the processor, characterized in that, when the processor executes the computer program, the computer program according to claims 1 to 1 is implemented. The method described in any one of 8.
A computer-readable storage medium storing a computer program, wherein the computer program implements the method according to any one of claims 1 to 8 when executed by a processor.