Synchronous positioning and mapping method and device based on radar, equipment and medium

Abstract

The invention relates to the technical field of three-dimensional map construction, in particular to a radar-based synchronous positioning and mapping method, device and equipment and a medium. The method comprises the following steps of: performing IMU pre-integration processing on IMU observation data and IMU state parameters to obtain an IMU residual factor; according to point cloud observation data of a radar and radar state parameters, a radar residual factor is obtained, a light beam adjustment method is applied to the radar residual factor and the IMU residual factor to obtain target state quantities, the target state quantities are the state quantity of the IMU state parameters and the state quantity of the radar state parameters, and synchronous positioning and mapping are achieved according to the two state quantities. According to the method, the IMU is used for compensating motion distortion between LiDAR frames, the accuracy of the point cloud data for robot state estimation is improved, and then the accuracy of synchronous positioning and mapping is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of three-dimensional map construction, and particularly to a radar-based simultaneous localization and mapping method, device, equipment and medium. Background Art

[0002] Accurate odometry and point cloud maps are prerequisites for mobile robots to explore autonomously, avoid obstacles and perform motion planning. Among them, the simultaneous localization and mapping technology can be used to calculate the odometry and construct the point cloud map. The existing technology estimates the state of the mobile robot through adjacent point cloud data collected by a lidar (LiDAR), and then realizes the current simultaneous localization and mapping through the state of the mobile robot. However, since the lidar is installed on the mobile robot, the movement of the mobile robot will cause motion distortion in the point cloud data collected by the lidar, that is, the accuracy of the point cloud data collected by the lidar is relatively low, resulting in relatively low accuracy of simultaneous localization and mapping.

[0003] Therefore, the existing technology still needs to be improved. Summary of the Invention

[0004] To solve the above technical problems, the present invention provides a radar-based simultaneous localization and mapping method, device, equipment and medium, which solves the problem of relatively low accuracy of simultaneous localization and mapping in the existing technology.

[0005] To achieve the above object, the present invention adopts the following technical solutions:

[0006] In a first aspect, the present invention provides a radar-based simultaneous localization and mapping method, which includes:

[0007] Obtain IMU observation data, set IMU state parameters, and perform IMU pre-integration processing on the IMU observation data and the IMU state parameters to obtain an IMU residual factor;

[0008] Obtain point cloud observation data collected by a radar, set radar state parameters, and determine a radar residual factor based on the point cloud observation data and the radar state parameters;

[0009] Apply the bundle adjustment method to the IMU residual factor and the radar residual factor to obtain a target state quantity, and realize simultaneous localization and mapping based on the target state quantity. The target state quantity includes the state quantity corresponding to the IMU state parameters and the state quantity corresponding to the radar state parameters.

[0010] In one implementation, performing IMU pre-integration processing on the IMU observation data and the IMU state parameters to obtain an IMU residual factor includes:

[0011] Determine the IMU angular velocity and IMU acceleration in the IMU observation data;

[0012] Determine the rotation zero bias, acceleration zero bias, and gravity direction in the IMU state parameters;

[0013] Perform IMU pre-integration processing on the IMU angular velocity, the IMU acceleration, the rotation zero bias, the acceleration zero bias, and the gravity direction to obtain an IMU residual factor.

[0014] In one implementation, performing IMU pre-integration processing on the IMU angular velocity, the IMU acceleration, the rotation zero bias, the acceleration zero bias, and the gravity direction to obtain an IMU residual factor includes:

[0015] Obtain a pre-integrated rotation increment and a pre-integrated rotation measurement value based on the IMU angular velocity and the rotation zero bias;

[0016] Obtain a rotation residual factor based on the pre-integrated rotation increment and the pre-integrated rotation measurement value;

[0017] Obtain a velocity residual factor based on the rotation zero bias, the acceleration zero bias, the gravity direction, and the IMU acceleration;

[0018] Obtain a pre-integrated displacement increment based on the pre-integrated rotation increment, the IMU acceleration, and the acceleration zero bias;

[0019] Obtain a displacement residual factor based on the pre-integrated displacement increment;

[0020] Construct an IMU residual factor based on the rotation residual factor, the velocity residual factor, and the displacement residual factor.

[0021] In one implementation, determining a radar residual factor based on the point cloud observation data and the radar state parameters includes;

[0022] Determine the radar rotation angle, radar position, radar moving speed, and external parameters from the radar to the IMU in the radar state parameters;

[0023] Convert the point cloud observation data to the coordinate system where the IMU is located based on the radar rotation angle, the radar position, the radar moving speed, and the external parameters from the radar to the IMU to obtain point cloud observation conversion data;

[0024] Determine a radar residual factor based on the point cloud observation conversion data.

[0025] In one implementation, determining a radar residual factor based on the point cloud observation conversion data includes:

[0026] Determine the edge feature residuals and surface feature residuals of the radar based on the point cloud observation conversion data;

[0027] Obtain the radar residual factor based on the sum of the edge feature residuals and the surface feature residuals;

[0028] In one implementation, apply bundle adjustment to the IMU residual factor and the radar residual factor to obtain the target state quantity, including:

[0029] Apply bundle adjustment to the IMU residual factors corresponding to multiple frames of the IMU observation data within the sliding window and the radar residual factors corresponding to multiple frames of the point cloud observation data within the sliding window to obtain a tightly coupled joint state optimization model;

[0030] Calculate the state quantity of the IMU state parameters and the state quantity of the radar state parameters corresponding to the minimum value of the joint state optimization model to obtain the target state quantity.

[0031] In one implementation, the point cloud observation data is the observation data after motion distortion compensation, and the method of motion distortion compensation includes:

[0032] Perform motion distortion compensation on the point cloud observation data through the IMU observation data.

[0033] In a second aspect, an embodiment of the present invention further provides a radar-based simultaneous localization and mapping device, where the device includes the following components:

[0034] An IMU residual factor calculation module, configured to obtain IMU observation data, set IMU state parameters, perform IMU pre-integration processing on the IMU observation data and the IMU state parameters, and obtain the IMU residual factor;

[0035] A radar residual factor calculation module, configured to obtain point cloud observation data collected by the radar, set radar state parameters, and determine the radar residual factor based on the point cloud observation data and the radar state parameters;

[0036] A simultaneous localization and mapping module, configured to apply bundle adjustment to the IMU residual factor and the radar residual factor to obtain the target state quantity, and implement simultaneous localization and mapping based on the target state quantity, where the target state quantity includes the state quantity corresponding to the IMU state parameters and the state quantity corresponding to the radar state parameters.

[0037] In a third aspect, an embodiment of the present invention further provides a terminal device, where the terminal device includes a memory, a processor, and a radar-based simultaneous localization and mapping program stored in the memory and executable on the processor. When the processor executes the radar-based simultaneous localization and mapping program, the steps of the above-mentioned radar-based simultaneous localization and mapping method are implemented.

[0038] In a fourth aspect, an embodiment of the present invention further provides a computer-readable storage medium, on which a radar-based simultaneous localization and mapping program is stored. When the radar-based simultaneous localization and mapping program is executed by a processor, the steps of the above-mentioned radar-based simultaneous localization and mapping method are implemented.

[0039] Beneficial effects: The present invention performs IMU pre-integration processing on IMU observation data and IMU state parameters to obtain an IMU residual factor; according to the point cloud observation data of the radar and the radar state parameters, a radar residual factor is obtained, and the bundle adjustment method is applied to the radar residual factor and the IMU residual factor to obtain a target state quantity, where the target state quantity is the state quantity of the IMU state parameters and the state quantity of the radar state parameters. Simultaneous localization and mapping are achieved based on these two state quantities. The present invention uses an inertial measurement unit (IMU) to compensate for motion distortion between LiDAR frames, improving the accuracy of the point cloud data for robot state estimation, and thus improving the accuracy of simultaneous localization and mapping. Description of the Drawings

[0040] Figure 1 is the overall flowchart of the present invention;

[0041] Figure 2 is the schematic diagram of edge features in an embodiment of the present invention;

[0042] Figure 3 is the schematic diagram of surface features in an embodiment of the present invention;

[0043] Figure 4 is the system framework diagram in an embodiment of the present invention;

[0044] Figure 5 is the structure diagram of the radar-based simultaneous localization and mapping device provided by the present invention;

[0045] Figure 6 is the internal structure principle block diagram of the terminal device provided in an embodiment of the present invention. Detailed Embodiments

[0046] The technical solutions in the present invention will be clearly and completely described below in conjunction with the embodiments and the accompanying drawings of the specification. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0047] It has been found through research that accurate odometers and point cloud maps are the prerequisites for mobile robots to explore autonomously, avoid obstacles, and perform motion planning. Among them, the simultaneous localization and mapping technology can be used to calculate the odometer and construct the point cloud map. The existing technology estimates the state of the mobile robot through adjacent point cloud data collected by the lidar LiDAR, and then realizes the current simultaneous localization and mapping through the state of the mobile robot. However, since the lidar LiDAR is installed on the mobile robot, the movement of the mobile robot will cause motion distortion in the point cloud data collected by the lidar LiDAR, that is, the accuracy of the point cloud data collected by the lidar LiDAR is relatively low, resulting in relatively low accuracy of simultaneous localization and mapping.

[0048] To solve the above technical problems, the present invention provides a radar-based simultaneous localization and mapping method, device, equipment, and medium, which solves the problem of relatively low accuracy of simultaneous localization and mapping in the existing technology.

[0049] The radar-based simultaneous localization and mapping method of this embodiment can be applied to terminal devices. The terminal device can be a terminal product with control functions, such as the controller of a mobile robot, etc. In this embodiment, as Figure 1 shown, the radar-based simultaneous localization and mapping method specifically includes the following steps:

[0050] S100, obtain IMU observation data, set IMU state parameters, perform IMU pre-integration processing on the IMU observation data and the IMU state parameters to obtain an IMU residual factor;

[0051] S200, obtain point cloud observation data collected by the radar, set radar state parameters, and determine a radar residual factor based on the point cloud observation data and the radar state parameters;

[0052] S300, apply the bundle adjustment method to the IMU residual factor and the radar residual factor to obtain a target state quantity, and realize simultaneous localization and mapping based on the target state quantity. The target state quantity includes the state quantity corresponding to the IMU state parameters and the state quantity corresponding to the radar state parameters.

[0053] Based on steps S100, S200, and S300, the motion planning of the mobile robot can be realized, and the specific process is as follows:

[0054] A lidar and an inertial measurement unit (IMU) are installed on the mobile robot. The lidar is used to collect the point cloud data (i.e., point cloud observation data) around the mobile robot. Based on the point cloud observation data and the lidar state parameters, the lidar residual factor is obtained.

[0055] The IMU is used to collect the acceleration generated by the movement of the mobile robot and the angular velocity generated by the rotation. The acceleration and angular velocity collected by the IMU are the IMU observation data. The IMU observation data and the IMU state parameters are processed by IMU pre-integration to obtain the IMU residual factor.

[0056] The bundle adjustment method is applied to the IMU residual factor and the lidar residual factor to obtain the target state quantity. The target state quantity is the state quantity of the lidar state parameters and the state quantity of the IMU state parameters. Finally, based on these two state quantities and the world coordinate system, a map corresponding to the environment around the mobile robot is constructed, and the mobile robot performs motion planning based on this map.

[0057] Example 1, in this example, the state parameters are used to represent the state parameters of the IMU and the lidar:

[0058]

[0059] where X i is the state of the i-th sliding window, R IL and t IL are the extrinsic parameters from the LiDAR to the IMU, that is, R IL is the extrinsic parameter of the rotation matrix, and t IL is the extrinsic parameter of the translation matrix. The number of frames of the IMU observation data and the number of frames of the point cloud observation data in each sliding window are both several frames. Each frame of the IMU observation data corresponds to a set of state quantities of the IMU state parameters, and each frame of the point cloud observation data also corresponds to a set of state quantities of the lidar state parameters.

[0060] X0, X1, X2,..., X n in each X i is defined as:

[0061]

[0062] where R represents the lidar rotation angle, P represents the lidar position, v represents the lidar movement speed. Since the lidar is located on the mobile robot and the lidar and the mobile robot move synchronously, the lidar rotation angle, the lidar position, and the lidar movement speed also represent the rotation angle, the position, and the movement speed of the mobile robot. G represents the global coordinate system, I represents the IMU coordinate system, and GI represents the transformation from the IMU coordinate system to the global coordinate system. b gis the rotation bias of the IMU, b a is the acceleration bias of the IMU, g G is the gravity direction of the IMU.

[0063] In this embodiment, the IMU observation data is defined, including the IMU angular velocity and the IMU acceleration where t represents the time, the IMU angular velocity and the IMU acceleration both contain white noise, so:

[0064]

[0065] represents the rotation matrix from the global coordinate system to the IMU coordinate system at time t, represents the Gaussian white noise in the IMU acceleration measurement, represents the Gaussian white noise in the IMU angular velocity measurement.

[0066] In this embodiment, a method for iteratively updating the state of a system composed of a mobile robot, a lidar, and an IMU is provided:

[0067]

[0068] represents the Gaussian white noise in the IMU measurement.

[0069] Embodiment 2, based on Embodiment 1, in this embodiment, the IMU observation data in step S100 includes the IMU angular velocity and the IMU acceleration The IMU state parameters include the rotation bias b g and the acceleration bias b a and the gravity direction g G . Perform IMU pre-integration processing on the IMU observation data and the IMU state parameters to obtain an IMU residual factor, including the following specific steps S101 to S106:

[0070] S101, according to the IMU angular velocity and the rotation bias b g , obtain the pre-integrated rotation increment ΔR ij and the pre-integrated rotation measurement value

[0071]

[0072] In the formula, represents approximately equal to, k is the frame number, and δφ ij is the observation noise of the rotation pre-integration quantity.

[0073] S102. Obtain a rotation residual factor based on the pre-integrated rotation increment and the pre-integrated rotation measurement value

[0074]

[0075] Denote the ideal value or standard value of the rotation zero bias b g and denote the ideal value or standard value of the pre-integrated rotation increment ΔR ij ij ij

[0076] S103. Obtain a velocity residual factor based on the rotation zero bias b g the acceleration zero bias b a the gravity direction g G and the IMU acceleration

[0077] According to the IMU acceleration (i.e., k is the sequence number of the frame, and the frame corresponds to the time t) and the acceleration zero bias b a ij ij

[0078]

[0079] ij

[0080] ij

[0081]

[0082] ij ij ij g g g a a a

[0082] S104. Obtain the pre-integrated displacement increment Δp a a ij ij :

[0083]

[0084] S105, based on the pre-integrated displacement increment Δp ij , obtain the displacement residual factor

[0085]

[0086] wherein, Δp ij and δp ij and satisfy the following relational expression:

[0087]

[0088] S106, based on the rotation residual factor the velocity residual factor and the displacement residual factor construct the IMU residual factor

[0089]

[0090] Embodiment 3, based on Embodiment 1 or Embodiment 2, the steps S200 of this embodiment include the following specific steps S201, S202, S203, and S204:

[0091] S201, determine the radar rotation angle R, radar position P, radar moving speed v, external parameter R from the radar to the IMU IL and t IL .

[0092] S202, based on the radar rotation angle, the radar position, the radar moving speed, and the external parameter from the radar to the IMU, convert the point cloud observation data to the coordinate system where the IMU is located to obtain the point cloud observation conversion data.

[0093] S203, based on the point cloud observation conversion data, determine the edge feature residual and the plane feature residual

[0094] Convert the point cloud observation data to the coordinate system where the IMU is located to obtain the point cloud observation conversion data, and then convert the point cloud observation conversion data to the data p in the global coordinate system i , which is the prior art. p i is the feature point of the current LiDAR frame in the global coordinate system (the global coordinate system is the world coordinate system).

[0095] Use the distance d from a point to a line i to represent the edge feature residual The feature points in the first two LiDAR frames are projected onto the voxel map to obtain two projection points. The two projection points can form a straight line. Any point on this straight line is denoted as q, and the unit direction vector of this straight line is n. As Figure 2 shown, p i to the distance d i of this straight line:

[0096] d i = ||(1 - nn T )(p i - q)||

[0097] Use the distance d i ' from a point to a plane to represent the surface feature residual The feature points in the first five LiDAR frames are projected onto the voxel map to obtain five projection points. The five projection points can form a plane. Any point on this plane is denoted as q', and the unit normal vector of this plane is n'. As Figure 3 shown, d i ' = n' T (p i - q')[[]]END]]

[0098] S204. According to the sum of the edge feature residual and the surface feature residual , obtain the radar residual factor

[0099]

[0100] Example 4. Based on Example 1 or Example 2 or Example 3, in this example, step S300 includes the following specific steps: Apply bundle adjustment to the IMU residual factors corresponding to multiple frames of the IMU observation data within the sliding window and the radar residual factors corresponding to multiple frames of the point cloud observation data within the sliding window to obtain a tightly coupled joint state optimization model; Calculate the state quantities of the IMU state parameters and the state quantities of the radar state parameters corresponding to the minimum value of the joint state optimization model to obtain the target state quantities.

[0101] As Figure 4 shown, apply the IMU residual factors corresponding to multiple frames of the IMU observation data within the sliding window and the radar residual factors corresponding to multiple frames of the point cloud observation data within the sliding window ap

[0102] The target state quantity is the target state quantity corresponding to the minimum value of the above formula , Including X in the above formula i and X j where X i is the IMU state parameter, and X j is the radar state. is the IMU pre-integration residual between the i-th frame and the (i + 1)-th frame, represents the covariance matrix of the IMU pre-integration measurement value, is the LiDAR residual of the j-th frame, represents the covariance matrix of the LiDAR observation, is the prior term in the sliding window. In this paper, the Levenberg-Marquardt algorithm is used to handle this optimization problem, which can effectively balance the convergence speed and stability of the calculation.

[0103] Example 5, based on Example 1 or Example 2 or Example 3, this example uses the following formula to optimize the state parameter x:

[0104] For all LiDAR frames in the sliding window, the optimization problem can be modeled as:

[0105]

[0106] where N1 is the number of edge feature points and N2 is the number of surface feature points.

[0107] In summary, the present invention designs a tightly coupled LiDAR-inertial system (the LiDAR-inertial system in Figure 4 where inertia refers to the IMU). The IMU pre-integration constraint and the matching of LiDAR scans with the adaptive voxel map are designed to be tightly coupled and jointly optimized in the sliding window. It can adapt to fast motion and feature degradation, and improve the robustness and accuracy of positioning and mapping. Pre-integration can also add state constraints between any two frames, realize the association between states, and at the same time, the characteristic of only calculating the increment reduces the operation burden of the system to a certain extent and improves the real-time performance.

[0108] The present invention constructs a laser-inertial BA measurement model by minimizing the plane thickness of coplanar points and the line width of collinear points. The laser-inertial BA measurement model realizes the relative pose constraint of multi-frame states, and analytically expresses the LiDAR measurement residual, the Jacobian matrix of the IMU pre-integration constraint, and the external parameters from LiDAR to IMU.

[0109] The present invention uses a voxel map to store the global point cloud map. In the voxel map, collinear or coplanar points are stored in a voxel, and the voxel index is recorded, so that all surface and line features can be found faster and more efficiently during point cloud matching.

[0110] This embodiment also provides a radar-based simultaneous localization and mapping device, as Figure 5 shown. The device includes the following components:

[0111] An IMU residual factor calculation module 01, which is used to obtain IMU observation data, set IMU state parameters, perform IMU pre-integration processing on the IMU observation data and the IMU state parameters, and obtain an IMU residual factor;

[0112] A radar residual factor calculation module 02, which is used to obtain point cloud observation data collected by the radar, set radar state parameters, and determine a radar residual factor based on the point cloud observation data and the radar state parameters;

[0113] A simultaneous localization and mapping module 03, which is used to apply bundle adjustment to the IMU residual factor and the radar residual factor to obtain a target state quantity, and perform simultaneous localization and mapping based on the target state quantity. The target state quantity includes the state quantity corresponding to the IMU state parameters and the state quantity corresponding to the radar state parameters.

[0114] Based on the above embodiment, the present invention also provides a terminal device, and its principle block diagram can be as Figure 6 shown. The terminal device includes a processor, a memory, a network interface, and a display screen connected through a system bus. Among them, the processor of the terminal device is used to provide computing and control capabilities. The memory of the terminal device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The network interface of the terminal device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, it implements a radar-based simultaneous localization and mapping method. The display screen of the terminal device can be a liquid crystal display screen or an electronic ink display screen.

[0115] Those skilled in the art can understand that Figure 6 the principle block diagram shown in

[0116] merely shows the block diagram of some structures related to the solution of the present invention, and does not constitute a limitation on the terminal device to which the solution of the present invention is applied. The specific terminal device may include more or fewer components than those shown in the figure, or combine some components, or have different component arrangements.

[0117] Obtain IMU observation data, set IMU state parameters, perform IMU pre-integration processing on the IMU observation data and the IMU state parameters to obtain an IMU residual factor;

[0118] Obtain point cloud observation data collected by a radar, set radar state parameters, and determine a radar residual factor based on the point cloud observation data and the radar state parameters;

[0119] Apply bundle adjustment to the IMU residual factor and the radar residual factor to obtain a target state quantity, and perform simultaneous localization and mapping based on the target state quantity. The target state quantity includes the state quantity corresponding to the IMU state parameters and the state quantity corresponding to the radar state parameters.

[0120] Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above methods. Among them, any reference to a memory, storage, database, or other medium used in the various embodiments provided by the present invention can include non-volatile and / or volatile memory. Non-volatile 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 (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0121] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments or equivalently replace some of the technical features. These modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A radar-based synchronous positioning and mapping method, characterized in that: include: Acquire IMU observation data, set IMU state parameters, perform IMU pre-integration processing on the IMU observation data and the IMU state parameters, and obtain an IMU residual factor; Acquire point cloud observation data collected by the radar, set radar state parameters, and determine a radar residual factor based on the point cloud observation data and the radar state parameters; A bundle adjustment method is applied to the IMU residual factor and the radar residual factor to obtain a target state quantity, and synchronous positioning and mapping are achieved based on the target state quantity, wherein the target state quantity includes a state quantity corresponding to the IMU state parameter and a state quantity corresponding to the radar state parameter.

2. The radar-based simultaneous positioning and mapping method according to claim 1, characterized in that: Performing IMU pre-integration processing on the IMU observation data and the IMU state parameters to obtain an IMU residual factor, including: Determine the IMU angular velocity and IMU acceleration in the IMU observation data; Determine the rotation bias and acceleration bias and gravity direction in the IMU state parameters; Perform IMU pre-integration processing on the IMU angular velocity, the IMU acceleration, the rotation zero bias, the acceleration zero bias, and the gravity direction to obtain an IMU residual factor.

3. The radar-based simultaneous positioning and mapping method according to claim 2, characterized in that: Performing IMU pre-integration processing on the IMU angular velocity, the IMU acceleration, the rotation bias, the acceleration bias, and the gravity direction to obtain an IMU residual factor includes: Obtaining a pre-integrated rotation increment and a pre-integrated rotation measurement value according to the IMU angular velocity and the rotation zero bias; Obtaining a rotation residual factor according to the pre-integrated rotation increment and the pre-integrated rotation measurement value; Obtaining a velocity residual factor according to the rotation bias, the acceleration bias, the gravity direction, and the IMU acceleration; Obtaining a pre-integrated displacement increment according to the pre-integrated rotation increment, the IMU acceleration, and the acceleration zero bias; Obtaining a displacement residual factor according to the pre-integrated displacement increment; An IMU residual factor is constructed according to the rotation residual factor, the velocity residual factor and the displacement residual factor.

4. The radar-based simultaneous positioning and mapping method according to claim 1, characterized in that: Determining a radar residual factor according to the point cloud observation data and the radar state parameter, including: Determine the radar rotation angle, radar position, radar moving speed, and external parameters from radar to IMU in the radar status parameters; According to the radar rotation angle, the radar position, the radar moving speed, and the external parameters from the radar to the IMU, the point cloud observation data is converted to the coordinate system where the IMU is located to obtain point cloud observation conversion data; A radar residual factor is determined based on the point cloud observation conversion data.

5. The radar-based simultaneous positioning and mapping method according to claim 4, characterized in that: Determining the radar residual factor based on the point cloud observation conversion data includes: Determine the edge feature residual and the surface feature residual of the radar according to the point cloud observation conversion data; A radar residual factor is obtained according to the sum of the edge feature residual and the surface feature residual.

6. The radar-based simultaneous positioning and mapping method according to claim 1, characterized in that: Applying the bundle adjustment method to the IMU residual factor and the radar residual factor to obtain the target state quantity includes: Applying bundle adjustment to the IMU residual factors corresponding to the multiple frames of the IMU observation data within the sliding window and the radar residual factors corresponding to the multiple frames of the point cloud observation data within the sliding window to obtain a tightly coupled joint state optimization model; The state quantity of the IMU state parameter and the state quantity of the radar state parameter corresponding to when the joint state optimization model obtains the minimum value are calculated to obtain the target state quantity.

7. The radar-based simultaneous positioning and mapping method according to claim 1, characterized in that: The point cloud observation data is observation data after motion distortion compensation, and the motion distortion compensation method includes: Motion distortion compensation is performed on the point cloud observation data through the IMU observation data.

8. A radar-based synchronous positioning and mapping device, characterized in that: The device comprises the following components: An IMU residual factor calculation module is used to obtain IMU observation data, set IMU state parameters, perform IMU pre-integration processing on the IMU observation data and the IMU state parameters, and obtain an IMU residual factor; A radar residual factor calculation module is used to obtain point cloud observation data collected by the radar, set radar state parameters, and determine the radar residual factor based on the point cloud observation data and the radar state parameters; The synchronous positioning and mapping module is used to apply the bundle adjustment method to the IMU residual factor and the radar residual factor to obtain a target state quantity, and realize synchronous positioning and mapping according to the target state quantity, wherein the target state quantity includes a state quantity corresponding to the IMU state parameter and a state quantity corresponding to the radar state parameter.

9. A terminal device, characterized in that: The terminal device includes a memory, a processor, and a radar-based simultaneous positioning and mapping program stored in the memory and executable on the processor. When the processor executes the radar-based simultaneous positioning and mapping program, the steps of the radar-based simultaneous positioning and mapping method as described in any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a radar-based simultaneous positioning and mapping program. When the radar-based simultaneous positioning and mapping program is executed by the processor, the steps of the radar-based simultaneous positioning and mapping method as described in any one of claims 1 to 7 are implemented.

Images