A multi-sensor fusion positioning device and method based on radar and integrated navigation

Abstract

A kind of multi-sensor fusion positioning device and method based on laser radar and integrated navigation, by real-time monitoring and analyzing laser radar positioning and integrated navigation positioning result, the result of the fusion of two positioning methods, improve the stability of fusion positioning.Laser radar positioning, based on single-frame laser radar point cloud and global point cloud map matching of fusion RTK information, based on RTK information auxiliary laser radar initial positioning, so that laser radar positioning does not need to set additional initial parameters, automatically obtain the positioning information under map coordinate system, when laser radar positioning is lost, rely on RTK information repositioning.Integrated navigation positioning is based on the GPS information of equipment and base station, IMU provides attitude, by difference to obtain the positioning information of current position.Fusion error tolerance range laser radar and integrated navigation positioning result, improve the precision of fusion positioning.When laser radar positioning and integrated navigation positioning are both invalid, based on SLAM technology to obtain current positioning information and output fusion positioning information.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-sensor fusion positioning, and more specifically to a multi-sensor fusion positioning device and method based on radar and integrated navigation. Background Art

[0002] Positioning technology plays a vital role in autonomous driving, and using a single sensor for positioning has certain limitations. For example, LiDAR positioning is based on matching a single-frame LiDAR point cloud with a global point cloud map, calculating the coordinate conversion relationship between the radar coordinate system and the map coordinate system, and obtaining positioning information in the map coordinate system. It has high positioning accuracy for scenes with rich features, but may experience mismatching of the laser point cloud or low positioning accuracy in open areas. Combined navigation positioning is based on GPS information from RTK devices and base stations, with the IMU providing attitude and obtaining positioning information of the current position through differential analysis. It has high positioning accuracy for open areas, but has low positioning accuracy for scenes where external objects such as tall buildings and trees block the antenna signal.

[0003] Fusion of data from different sensors can leverage the strengths of each sensor and effectively improve positioning. However, existing fusion positioning methods face two major challenges: improving positioning stability and increasing fusion positioning accuracy. Fusion positioning accuracy is primarily limited by the accuracy of the fused input data and the deviations between the different input data.

[0004] To this end, we provide a multi-sensor fusion positioning device and method based on radar and integrated navigation. Summary of the Invention

[0005] The present invention provides a multi-sensor fusion positioning device and method based on radar and integrated navigation, so as to overcome the shortcomings of existing multi-sensor fusion positioning such as low stability and low precision.

[0006] The present invention adopts the following technical solutions:

[0007] A multi-sensor fusion positioning device based on radar and combined navigation includes: a laser radar positioning module, which is used to obtain a single-frame point cloud of the current position, match it with a global point cloud map, and obtain positioning information; an integrated navigation positioning module, which is used to obtain RTK equipment and base station GPS signals, and an IMU provides attitude, and obtains positioning information based on the differential result; an IMU laser radar fusion module, which is used to obtain positioning information based on SLAM technology when the laser radar positioning and combined navigation positioning fail; a positioning status monitoring module, which is used to monitor the laser radar positioning and combined navigation positioning status in real time and analyze and evaluate the positioning results; a fusion positioning module, which is used to fuse the laser radar positioning and combined navigation positioning results within an allowable error range and output fused positioning information. When the laser radar positioning and combined navigation positioning fail, the fused positioning information is output based on the IMU laser radar fusion result, and the fused positioning result is evaluated.

[0008] Furthermore, the above-mentioned lidar positioning module uses the lidar to obtain the point cloud around the vehicle, matches it with the global point cloud map integrated with RTK information, obtains the coordinate conversion relationship from the radar coordinate system to the map coordinate system, and obtains the positioning information on the map coordinate system; the combined navigation positioning module obtains the RTK equipment and base station GPS signals, and the IMU provides the attitude, subtracts the satellite signals, weakens the errors in the ionosphere and troposphere, obtains the differential positioning results, and uses the initial position as the origin to obtain the positioning information on the map coordinate system.

[0009] Furthermore, the above-mentioned IMU lidar fusion module, provided with three-axis acceleration, three-axis angular velocity, and three-axis attitude information by the IMU, provides an initial matching attitude for two frames of lidar point clouds based on the IMU data, extracts the feature points of the lidar, performs inter-frame matching of the lidar point clouds, and obtains positioning information on the map coordinate system.

[0010] Furthermore, the above-mentioned positioning status monitoring module monitors and analyzes the lidar positioning and combined navigation positioning status respectively. For lidar positioning, it monitors and analyzes the laser point cloud matching effect, matching time, and lidar positioning estimated vehicle speed information; for combined navigation positioning, it monitors and analyzes the combined navigation positioning position and heading output standard deviation, RTK output status, and RTK estimated vehicle speed information.

[0011] Furthermore, the above-mentioned fusion positioning module performs fusion positioning based on the extended Kalman filter algorithm, fuses the laser radar and combined navigation positioning results within the allowable error range, outputs the fusion positioning results, and analyzes and evaluates the results.

[0012] The present invention also provides a multi-sensor fusion positioning method based on radar and integrated navigation, which specifically includes the following steps:

[0013] S10. Based on the requirements for the installation of the integrated navigation device, two measurement antennas and an RTK device host are installed on the vehicle. After the system is started, the RTK device host obtains GPS signals from the device and the base station, and the IMU provides attitude to obtain differential positioning results. With the initial position as the origin, positioning information in the map coordinate system is obtained.

[0014] S20. Install a laser radar on the top of the vehicle. After the system is started, the laser radar collects point clouds around the vehicle and inputs a single frame of point clouds into the laser radar positioning module.

[0015] S30, the lidar positioning module is based on a single-frame point cloud, which is matched with the global point cloud map integrated with RTK information. It assists the initial positioning of the lidar based on the RTK information, automatically obtains the positioning information in the map coordinate system, and relies on the RTK information for repositioning when the lidar positioning is lost;

[0016] S40, monitoring the standard deviation of the integrated navigation positioning position and heading output, the RTK output status, and the RTK estimated vehicle speed information, and analyzing whether the integrated navigation positioning result is within the allowable error range, wherein the RTK output status includes a single point solution, a floating solution, and a fixed solution, and the fixed solution is the best state;

[0017] S50, monitoring the laser point cloud matching effect, matching time, and laser radar positioning estimated vehicle speed information, and analyzing whether the laser radar positioning result is within the allowable error range;

[0018] S60, obtaining a positioning result within an allowable error range, further calculating the position, heading, and velocity information to input into a fusion positioning module, performing fusion positioning based on an extended Kalman filter algorithm, and outputting a fusion positioning result;

[0019] S70: When both lidar positioning and integrated navigation positioning fail, the IMU provides three-axis acceleration, three-axis angular velocity, and three-axis attitude information. Based on the IMU data, it provides an initial matching attitude for the two frames of lidar point clouds, extracts the feature points of the lidar, performs inter-frame matching of the lidar point clouds, obtains positioning information in the map coordinate system, inputs the extended Kalman filter algorithm for fusion positioning, and outputs the fusion positioning result.

[0020] S80: Analyze the release interval, position and heading standard deviation, and fusion positioning estimated vehicle speed information of the fusion positioning results, and evaluate the fusion positioning output.

[0021] Furthermore, when RTK information is used for repositioning in step S30, the initial coordinates of the integrated navigation positioning are set as the origin of the map coordinate system, and the map coordinate system is oriented with the x-axis facing forward, the y-axis facing left, and the z-axis facing upward; the calculation formulas of the rotation matrix R and the translation vector T between the radar coordinate system and the map coordinate system are as follows: R = R γ *R β *Rα ,in γ c is the rotation angle around the z-axis at the current moment, β c is the rotation angle around the y-axis at the current moment, α c is the rotation angle around the x-axis at the current moment; T=[t x , t y , t z ] T , where t x is the x-axis component of displacement, t y is the y-axis component of displacement, t z is the z-axis component of the displacement.

[0022] Furthermore, in the above step S40, the RTK estimated vehicle speed calculation formula is: (x l ,y l ) is the horizontal position coordinate of the previous moment, (x c ,y c ) is the horizontal position coordinate at the current moment, and Δt is the time interval between two positioning results.

[0023] Furthermore, the state vector in the above step S60 is: x and y are the x-axis and y-axis position coordinates, γ is the heading angle, b is the heading angle offset, v and ω are the vehicle speed and heading angular velocity, which are provided by lidar positioning or integrated navigation positioning. The calculation formula is as follows: Where (x l ,y l ) is the horizontal position coordinate of the previous moment, (x c ,y c ) is the horizontal position coordinate at the current moment, γ l is the heading angle at the previous moment, γ c is the heading angle at the current moment, Δt is the time interval between two positioning results; the prediction equation is: in is the estimated value of the position in the x and y axis directions, x l ,y l The position coordinates in the x and y axis directions at the previous moment. are speed, heading angle, estimated heading angular velocity, v l , γ l 、ω l are the speed, heading angle, and heading angular velocity of the previous moment respectively; and b l are the estimated value of the heading angle bias and the heading angle bias at the previous moment, respectively, and t is the extended Kalman filter estimation time interval; the state transition matrix is: Corrected state estimate: in is the estimated state, K is the Kalman gain, z is the measurement vector, and h is the measurement function; the modified covariance matrix: P = P c -KHP c , where P c is the prediction covariance matrix, and H is the measurement sensitivity matrix.

[0024] Furthermore, the calculation formula for the vehicle speed estimated by fusion positioning in the above step S80 is: (x l ,y l ) is the horizontal position coordinate of the fusion positioning at the previous moment, (x c ,y c ) is the horizontal position coordinate at the current moment, and Δt is the time interval between two positioning results.

[0025] It can be seen from the above description of the present invention that, compared with the prior art, the present invention has the following advantages:

[0026] 1. The present invention combines the two methods of laser radar and combined navigation positioning, and fully utilizes the advantages of both. When the combined navigation positioning error is large, the laser radar positioning outputs the fused positioning result; when the laser radar positioning error is large, the combined navigation positioning outputs the fused positioning result; when the errors of the laser radar positioning and the combined navigation positioning results are within the allowable range, both are input into the extended Kalman filter algorithm to output the fused positioning result; when both the laser radar positioning and the combined navigation positioning fail, the IMU and laser radar data are fused, the current positioning information is obtained based on the SLAM technology to output the fused positioning information, and the fused positioning result is analyzed and evaluated, which can effectively improve the stability of the fused positioning.

[0027] 2. The present invention monitors and analyzes the point cloud matching effect, matching time, and vehicle speed estimated by the lidar positioning, and combines the position and heading output standard deviation, RTK output status, and RTK estimated vehicle speed information in navigation positioning to obtain positioning results within the allowable error range. The position, heading, and speed information are further calculated and input into the fusion algorithm, which can effectively improve the accuracy of fusion positioning.

[0028] 3. The present invention performs lidar positioning based on a global point cloud map that integrates RTK information, which can effectively reduce the deviation of lidar and combined navigation positioning results. It assists the initial positioning of the lidar based on RTK information, so that the lidar positioning does not need to set additional initial parameters. When the lidar positioning is lost, it can rely on RTK information for repositioning, thereby improving the degree of automation of the fusion positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1This is a structural block diagram of the fusion positioning device of the present invention.

[0030] Figure 2 Schematic diagram of the process of the fusion positioning method of the present invention.

[0031] Figure 3 This is a schematic diagram of the RTK-assisted laser radar positioning process of the present invention.

[0032] Figure 4 This is a schematic diagram of the IMU laser radar fusion positioning process of the present invention. DETAILED DESCRIPTION

[0033] Refer to the following Figure 1 The following describes specific embodiments of the present invention. Numerous details are provided below to provide a comprehensive understanding of the present invention, but those skilled in the art will appreciate that the present invention can be implemented without these details. Well-known components, methods, and processes are not described in detail below.

[0034] A multi-sensor fusion positioning device based on radar and combined navigation includes: a laser radar positioning module 10, used to obtain a single-frame point cloud of the current position, match it with a global point cloud map, and obtain positioning information; an integrated navigation positioning module 20, used to obtain RTK equipment and base station GPS signals, an IMU provides attitude, and obtains positioning information based on differential results; an IMU laser radar fusion module 30, used to obtain positioning information based on SLAM technology when the laser radar positioning and combined navigation positioning fail; a positioning status monitoring module 40, used to monitor the laser radar positioning and combined navigation positioning status in real time, and analyze and evaluate the positioning results; a fusion positioning module 50, used to fuse the laser radar positioning and combined navigation positioning results within an allowable error range, and output fused positioning information. When the laser radar positioning and combined navigation positioning fail, the fused positioning information is output based on the IMU laser radar fusion result, and the fused positioning result is evaluated.

[0035] The above-mentioned lidar positioning module 10 uses the lidar to obtain the point cloud around the vehicle, matches it with the global point cloud map integrated with RTK information, obtains the coordinate conversion relationship from the radar coordinate system to the map coordinate system, and obtains the positioning information on the map coordinate system.

[0036] The above-mentioned integrated navigation and positioning module 20 obtains the RTK equipment and base station GPS signals, and the IMU provides the attitude, subtracts the satellite signals, weakens the errors of the ionosphere and troposphere, obtains the differential positioning results, and uses the initial position as the origin to obtain the positioning information of the map coordinate system.

[0037] The above-mentioned IMU lidar fusion module 30 is provided with three-axis acceleration, three-axis angular velocity, and three-axis attitude information by the IMU. Based on the IMU data, it provides an initial matching attitude for two frames of lidar point clouds, extracts the feature points of the lidar, performs inter-frame matching of the lidar point clouds, and obtains positioning information on the map coordinate system.

[0038] The above-mentioned positioning status monitoring module 40 monitors and analyzes the laser radar positioning and combined navigation positioning status respectively. For laser radar positioning, it monitors and analyzes the laser point cloud matching effect, matching time, and laser radar positioning estimated vehicle speed information; for combined navigation positioning, it monitors and analyzes the combined navigation positioning position and heading output standard deviation, RTK output status, and RTK estimated vehicle speed information.

[0039] The fusion positioning module 50 performs fusion positioning based on the extended Kalman filter algorithm, fuses the laser radar and integrated navigation positioning results within the allowable error range, outputs the fusion positioning results, and analyzes and evaluates the results.

[0040] Reference Figure 2 The above-mentioned multi-sensor fusion positioning device based on radar and integrated navigation, and its fusion positioning method specifically include the following steps:

[0041] S10. Based on the installation requirements of the combined navigation device, two measurement antennas and an RTK device host are installed on the vehicle. After the system is started, the RTK device host obtains GPS signals from the device and the base station, and the IMU provides attitude to obtain differential positioning results. The initial position is used as the origin to obtain positioning information in the map coordinate system.

[0042] S20. Install a laser radar on the top of the vehicle. After the system is started, the laser radar collects point clouds around the vehicle and inputs single-frame point clouds into the laser radar positioning module.

[0043] S30, combined Figure 3 The laser radar positioning module is based on a single-frame point cloud and uses the NDT algorithm to match the global point cloud map integrated with RTK information. It assists the initial positioning of the laser radar based on RTK information and automatically obtains positioning information in the map coordinate system. When the laser radar positioning is lost, it can rely on RTK information for repositioning. The initial coordinates of the combined navigation positioning are set to the origin of the map coordinate system, and the map coordinate system is oriented with the x-axis facing forward, the y-axis facing left, and the z-axis facing up. The calculation formulas for the rotation matrix R and translation vector T between the radar coordinate system and the map coordinate system are as follows: R = R γ *R β *R α ,in γ c is the rotation angle around the z-axis at the current moment, β cis the rotation angle around the y-axis at the current moment, α c is the rotation angle around the x-axis at the current moment. x , t y , t z ] T , where t x is the x-axis component of displacement, t y is the y-axis component of displacement, t z is the z-axis component of the displacement.

[0044] S40, monitor the standard deviation of the integrated navigation positioning position and heading output, RTK output status, and RTK estimated vehicle speed information, and analyze whether the integrated navigation positioning results are within the allowable error range. The RTK output status includes single point solution, floating solution, and fixed solution, with the fixed solution being the best state. Considering the horizontal motion of the vehicle, the RTK estimated vehicle speed calculation formula is: (x l ,y l ) is the horizontal position coordinate of the previous moment, (x c ,y c ) is the horizontal position coordinate at the current moment, and Δt is the time interval between two positioning results.

[0045] S50, monitoring the laser point cloud matching effect, matching time, and laser radar positioning estimated vehicle speed information, and analyzing whether the laser radar positioning result is within the allowable error range; the laser positioning estimated vehicle speed calculation formula is: (x l ,y l ) is the horizontal position coordinate of the previous moment, (x c ,y c ) is the horizontal position coordinate at the current moment, and Δt is the time interval between two positioning results.

[0046] S60: Obtain positioning results within an allowable error range, further calculate the position, heading, and speed information, input them into a fusion algorithm, perform fusion positioning based on an extended Kalman filter algorithm, and output a fusion positioning result.

[0047] Considering the displacement in the x and y directions and the change in heading angle, the state vector is: x and y are the x-axis and y-axis position coordinates, γ is the heading angle, b is the heading angle offset, v and ω are the vehicle speed and heading angular velocity, which are provided by lidar positioning or integrated navigation positioning. The calculation formula is as follows: Where (x l ,y l ) is the horizontal position coordinate of the previous moment, (x c ,y c ) is the horizontal position coordinate at the current moment, γ lis the heading angle at the previous moment, γ c is the heading angle at the current moment, and Δt is the time interval between two positioning results.

[0048] The prediction equation is: in is the estimated value of the position in the x and y axis directions, x l ,y l The position coordinates in the x and y axis directions at the previous moment. are speed, heading angle, estimated heading angular velocity, v l , γ l ,ω l They are the speed, heading angle, and heading angular velocity of the previous moment respectively. and b l are the heading angle bias estimate and the heading angle bias at the previous moment, respectively, and t is the extended Kalman filter estimation time interval.

[0049] The state transition matrix is:

[0050]

[0051] Corrected state estimate: in is the estimated state, K is the Kalman gain, z is the measurement vector, and h is the measurement function.

[0052] Corrected covariance matrix: P = P c -KHP c , where P c is the prediction covariance matrix, and H is the measurement sensitivity matrix.

[0053] S70, combined Figure 4 When both lidar positioning and combined navigation positioning fail, the IMU provides three-axis acceleration, three-axis angular velocity, and three-axis attitude information. Based on the IMU data, it provides the initial matching attitude for the two frames of lidar point clouds, extracts the feature points of the lidar, performs inter-frame matching of the lidar point clouds, obtains the positioning information in the map coordinate system, inputs the extended Kalman filter algorithm for fusion positioning, and outputs the fusion positioning result.

[0054] S80, analyzing the release interval of the fusion positioning results, the standard deviation of the position and heading, and the fusion positioning estimated vehicle speed information, and evaluating the fusion positioning output. The calculation formula for the fusion positioning estimated vehicle speed is: (x l ,y l ) is the horizontal position coordinate of the fusion positioning at the previous moment, (x c ,y c ) is the horizontal position coordinate at the current moment, and Δt is the time interval between two positioning results.

[0055] In summary, the multi-sensor fusion positioning device and method based on LiDAR and integrated navigation improves the stability and accuracy of fused positioning by real-time monitoring and analysis of LiDAR and integrated navigation positioning results. LiDAR positioning is based on matching a single-frame LiDAR point cloud with a global point cloud map fused with RTK information. RTK information is used to assist LiDAR initial positioning, eliminating the need to set additional initial parameters for LiDAR positioning and automatically obtaining positioning information in the map coordinate system. If LiDAR positioning is lost, RTK information can be used for re-positioning. Integrated navigation positioning uses GPS information from the device and base station, while the IMU provides attitude information, obtaining current positioning information through differential analysis. Fusion of LiDAR and integrated navigation positioning results within the allowable error range improves the stability and accuracy of fused positioning. If both LiDAR and integrated navigation positioning fail, the IMU and LiDAR data are fused, and the current positioning information is obtained and output as a fused positioning information based on SLAM technology.

[0056] The above is only a specific implementation of the present invention, but the design concept of the present invention is not limited to this. Any non-substantial changes to the present invention using this concept shall be deemed as an infringement of the protection scope of the present invention.

Claims

1. A multi-sensor fusion positioning device based on radar and integrated navigation, characterized in that: include: The laser radar positioning module is used to obtain a single-frame point cloud of the current position, match it with the global point cloud map, and obtain positioning information. The laser radar positioning module uses the laser radar to obtain the point cloud around the vehicle, matches it with the global point cloud map integrated with RTK information, obtains the coordinate conversion relationship from the radar coordinate system to the map coordinate system, and obtains the positioning information on the map coordinate system; The integrated navigation and positioning module obtains the RTK equipment and base station GPS signals, and the IMU provides attitude. It subtracts the satellite signals to reduce the errors in the ionosphere and troposphere, obtains the differential positioning results, and uses the initial position as the origin to obtain the positioning information of the map coordinate system; The IMU laser radar fusion module is used to obtain positioning information based on SLAM technology when the laser radar positioning and integrated navigation positioning fail. The IMU laser radar fusion module uses the three-axis acceleration, three-axis angular velocity, and three-axis attitude information provided by the IMU to provide an initial matching attitude for two frames of laser radar point clouds based on the IMU data, extract the feature points of the laser radar, perform inter-frame matching of the laser radar point clouds, and obtain positioning information in the map coordinate system; A positioning status monitoring module is used to monitor the status of lidar positioning and integrated navigation positioning in real time and analyze and evaluate the positioning results. The positioning status monitoring module monitors and analyzes the status of lidar positioning and integrated navigation positioning respectively. For lidar positioning, the module monitors and analyzes the laser point cloud matching effect, matching time, and lidar positioning estimated vehicle speed information. For integrated navigation positioning, the module monitors and analyzes the output standard deviation of the integrated navigation positioning position and heading, RTK output status, and RTK estimated vehicle speed information. The fusion positioning module is used to fuse the lidar positioning and integrated navigation positioning results within the allowable error range and output fused positioning information; when the lidar positioning and integrated navigation positioning fail, the fusion positioning information is output based on the IMU lidar fusion result, and the fusion positioning result is evaluated; the fusion positioning module performs fusion positioning based on the extended Kalman filter algorithm, fuses the lidar and integrated navigation positioning results within the allowable error range, outputs the fusion positioning result, and analyzes and evaluates the result.

2. A multi-sensor fusion positioning method based on radar and integrated navigation, characterized in that: The specific steps include: S10. Based on the requirements for the installation of the integrated navigation device, two measurement antennas and an RTK device host are installed on the vehicle. After the system is started, the RTK device host obtains GPS signals from the device and the base station, and the IMU provides attitude to obtain differential positioning results. With the initial position as the origin, positioning information in the map coordinate system is obtained. S20. Install a laser radar on the top of the vehicle. After the system is started, the laser radar collects point clouds around the vehicle and inputs a single frame of point clouds into the laser radar positioning module. S30, the lidar positioning module is based on a single-frame point cloud, which is matched with the global point cloud map integrated with RTK information. It assists the initial positioning of the lidar based on the RTK information, automatically obtains the positioning information in the map coordinate system, and relies on the RTK information for repositioning when the lidar positioning is lost; S40, monitoring the standard deviation of the integrated navigation positioning position and heading output, the RTK output status, and the RTK estimated vehicle speed information, and analyzing whether the integrated navigation positioning result is within the allowable error range, wherein the RTK output status includes a single point solution, a floating solution, and a fixed solution, and the fixed solution is the best state; S50, monitoring the laser point cloud matching effect, matching time, and laser radar positioning estimated vehicle speed information, and analyzing whether the laser radar positioning result is within the allowable error range; S60, obtaining a positioning result within an allowable error range, further calculating the position, heading, and velocity information to input into a fusion positioning module, performing fusion positioning based on an extended Kalman filter algorithm, and outputting a fusion positioning result; S70: When both lidar positioning and integrated navigation positioning fail, the IMU provides three-axis acceleration, three-axis angular velocity, and three-axis attitude information. Based on the IMU data, it provides an initial matching attitude for the two frames of lidar point clouds, extracts the feature points of the lidar, performs inter-frame matching of the lidar point clouds, obtains positioning information in the map coordinate system, inputs the extended Kalman filter algorithm for fusion positioning, and outputs the fusion positioning result. S80: Analyze the release interval, position and heading standard deviation, and fusion positioning estimated vehicle speed information of the fusion positioning results, and evaluate the fusion positioning output.

3. The multi-sensor fusion positioning method based on radar and integrated navigation according to claim 2, characterized in that: When RTK information is used for repositioning in step S30, the initial coordinates of the integrated navigation positioning are set as the origin of the map coordinate system, and the map coordinate system is oriented with the x-axis facing forward, the y-axis facing left, and the z-axis facing upward; the calculation formulas of the rotation matrix R and the translation vector T between the radar coordinate system and the map coordinate system are as follows: R = R γ *R β *R α ,in γ c is the rotation angle around the z-axis at the current moment, β c is the rotation angle around the y-axis at the current moment, α c is the rotation angle around the x-axis at the current moment; T=[t x , t y , t z] T , where t x is the x-axis component of displacement, t y is the y-axis component of displacement, t z is the z-axis component of the displacement.

4. The multi-sensor fusion positioning method based on radar and integrated navigation according to claim 2, characterized in that: In step S40, the RTK estimated vehicle speed calculation formula is: (x l ,y l ) is the horizontal position coordinate at the previous moment, (x c ,y c ) is the horizontal position coordinate at the current moment, and Δt is the time interval between two positioning results.

5. The multi-sensor fusion positioning method based on radar and integrated navigation according to claim 2, characterized in that: The state vector in step S60 is: x and y are the x-axis and y-axis position coordinates, γ is the heading angle, b is the heading angle offset, v and ω are the vehicle speed and heading angular velocity, which are provided by lidar positioning or integrated navigation positioning. The calculation formula is as follows: Where (x l ,y l ) is the horizontal position coordinate at the previous moment, (x c ,y c ) is the horizontal position coordinate at the current moment, γ l is the heading angle at the previous moment, γ c is the heading angle at the current moment, Δt is the time interval between two positioning results; the prediction equation is: in is the estimated value of the position in the x and y axis directions, x l ,y l is the position coordinate in the x and y axis directions at the previous moment; are speed, heading angle, estimated heading angular velocity, v l , γ l 、ω l are the speed, heading angle, and heading angular velocity of the previous moment respectively; and b l are the estimated value of the heading angle bias and the heading angle bias at the previous moment, respectively; t is the extended Kalman filter estimation time interval; the state transition matrix is: , correct the state estimate: in is the estimated state, K is the Kalman gain, z is the measurement vector, and h is the measurement function; the modified covariance matrix: P = P c -KHP c , where P c is the prediction covariance matrix, and H is the measurement sensitivity matrix.

6. The multi-sensor fusion positioning method based on radar and integrated navigation according to claim 2, characterized in that: The calculation formula for estimating vehicle speed by fusion positioning in step S80 is: (x l ,y l ) is the horizontal position coordinate of the fusion positioning at the previous moment, (x c ,y c ) is the horizontal position coordinate at the current moment, and Δt is the time interval between two positioning results.

Images