Radar odometer mapping method, device, electronic device and storage medium

Abstract

The present disclosure provides a radar odometer mapping method, device, electronic device and storage medium, which belongs to the field of autonomous driving technology and robotics. It includes: preprocessing the point cloud frame data input by the target object to obtain the preprocessed point cloud frame data; aligning the points in the point cloud frame of the target object with the sub-image to obtain the alignment result, and using the alignment result as the basic data of the construction module of the sub-image, wherein the alignment result includes the position and posture of the current point; voxelizing the space stored in the point cloud frame, projecting the position and posture of the current point and the preprocessed point cloud frame data into the space voxel, obtaining the first update value and the second update value of the point in the current space voxel, updating the basic data of the construction module of the sub-image based on the first update value and the second update value, and constructing the sub-image based on the updated sub-image construction module basic data.

Description

Technical Field

[0001] The present disclosure relates to the field of autonomous driving technology and robotics technology, and more particularly to a radar odometer mapping method, device, electronic device, and storage medium. Background Art

[0002] In recent years, the development of driverless technology has achieved remarkable results, and SLAM (simultaneous localization and mapping) technology has played an increasingly important role in it. Due to the great influence of lighting conditions, pure visual SLAM solutions are difficult to handle scenes with poor lighting conditions such as nighttime, which restricts the application of driverless technology in nighttime environments. In order to achieve all-weather and all-scenario driverless technology, more and more car companies are beginning to consider installing laser radars on their driverless cars to improve the vehicle's perception and SLAM capabilities. However, the traditional mechanical rotating laser radar has a relatively complex manufacturing process, high cost, and its large size makes it difficult to meet mass production requirements. Therefore, the new semi-solid laser radar technology is favored by car companies. Compared with the 360-degree mechanical rotating radar, the FOV (field of view) of the semi-solid radar is only 120 degrees. To solve the problem caused by the small FOV, the current more commonly used method is to use multi-radar splicing technology. In order to understand the development status of the prior art, the present patent has searched, compared and analyzed the existing patents and papers, and screened out the following technical solutions with a relatively high relevance to the present patent.

[0003] Technology 1: "Robust Odometry and Mapping for Multi-LiDAR Systems with Online Extrinsic Calibration" proposes an odometer and mapping solution based on multiple radars. The solution consists of three stages: the data preprocessing stage, in which feature points are extracted from each frame of point cloud data according to the LOAM (LOAM: Lidar Odometry and Mapping in Real-time) method of extracting line and surface features; the initialization stage, the external parameters of multiple radars are roughly calibrated; the optimization stage, sliding window optimization is used to estimate the poses of multiple historical frames of multiple radars, and the external parameters of multiple radars are optimized at the same time. If the optimization of the external parameters meets the threshold condition, it is determined that the external parameters have converged to a relatively accurate value, and the external parameters will not be optimized in the later stage. Although this solution optimizes the external parameters of multiple radars online, so that they converge to a relatively high accuracy in a short time, the external parameters between radars are inevitably changed due to the influence of the vibration of the vehicle, and secondly, even the high-precision external parameter calibration is inevitably prone to errors. Therefore, when using this external parameter to update the map, it is inevitable that calibration errors will be introduced into the map. Technology 2: "Faster-LIO: Lightweight Tightly Coupled Lidar-Inertial Odometry Using Parallel Sparse Incremental Voxels" proposes a radar inertial odometer solution based on incremental voxels. The innovation of this solution is to propose a data structure of incremental voxels based on unordered_map, which can realize efficient storage of local maps and query of approximate nearest neighbor points. Experiments show that this data structure can make the odometer frequency reach 1000H Z However, this data structure is only applicable to the registration method based on the nearest point search, and is difficult to apply to the NDT (Normal Distribution Transform) method based on model matching. Summary of the invention

[0004] The present invention provides a radar odometer mapping method, device, electronic device and storage medium.

[0005] On the one hand, a radar odometer mapping method is provided, comprising:

[0006] Preprocessing the point cloud frame data input by the target object to obtain preprocessed point cloud frame data;

[0007] Registering the points in the point cloud frame of the target object with the sub-image to obtain a registration result, and using the registration result as basic data of the construction module of the sub-image, wherein the registration result includes the pose of the current point;

[0008] The space where the point cloud frame is stored is voxelized, the posture of the current point and the preprocessed point cloud frame data are projected into the voxels of the space, a first update value and a second update value of the point in the current space voxel are obtained, basic data of the construction module of the sub-image is updated based on the first update value and the second update value, and a sub-image is constructed based on the updated basic data of the sub-image construction module.

[0009] In an optional embodiment, obtaining the first update value and the second update value of the midpoint of the voxel in the current space includes:

[0010] Get the coordinate value of the point falling in the current space voxel in the preset coordinate system;

[0011] Obtaining the first update value based on the coordinate value of the point falling in the current space voxel in a preset coordinate system;

[0012] The second update value is obtained based on the first update value, the total number of points falling in the current space voxel, and the coordinate values ​​of the points falling in the current space voxel in a preset coordinate system.

[0013] In an optional embodiment, obtaining the coordinate value of a point falling in a voxel in the current space in a preset coordinate system includes:

[0014] Obtaining the index value of the first-layer space voxel where the current point is located, and if the index value of the first-layer space voxel meets a preset condition, obtaining the index value of the second-layer space voxel where the current point is located;

[0015] If the index value of the second-layer space voxel meets the preset condition, the current point falls in the current space voxel, and the coordinate value of the current point in the preset coordinate system is obtained.

[0016] In an optional embodiment, obtaining the index value of the first-layer space voxel where the current point is located includes obtaining the index value of the first-layer space voxel where the current point is located by using the following formula:

[0017]

[0018] Where: px, py, pz represent the coordinate values ​​of the current point p in the preset coordinate system; i 1x 、i 1y 、i 1z They respectively represent the index value of the first layer voxel where the current point p is located; r1 represents the accuracy value of the first layer voxel.

[0019] In an optional embodiment, obtaining the coordinate value of the current point in a preset coordinate system includes obtaining the index value of the second-layer space voxel where the current point is located by the following formula:

[0020]

[0021] Where: px, py, pz represent the coordinate values ​​of the current point p in the preset coordinate system; i 2x 、i 2y 、i 2z They respectively represent the index value of the second-layer voxel where the current point p is located; r2 represents the accuracy value of the second-layer voxel.

[0022] In an optional embodiment, preprocessing the point cloud frame data input by the target object includes:

[0023] Obtaining the median of the distance between the points in the current point cloud frame and the center of the target object;

[0024] When the median of the distance between the points in the current point cloud frame and the center of the target object is less than a preset median threshold, the point data in the current point cloud frame is discarded.

[0025] In an optional embodiment, obtaining the median of the distance between the points in the current point cloud frame and the center of the target object includes:

[0026] Get the distance from the point in the point cloud frame to the center of the target object;

[0027] Sort the distances from the points in the point cloud frame to the center of the target object in ascending order, and take the distance value in the middle of all the sorted points;

[0028] The median of the distance between the point in the current point cloud frame and the center of the target object is obtained according to the distance value at the middle of all the sorted points and a preset distance threshold.

[0029] In an optional embodiment, obtaining the distances from points in all point cloud frames to the center of the target object includes:

[0030] The distance from all points in the point cloud frame to the center of the target object is obtained by the following formula:

[0031] r = sqrt(px*p.x+py*p.y+pz*pz)

[0032] Where r represents the distance from the point to the center of the radar; sqrt(·) represents the square root function; px, py, and pz represent the coordinate values ​​of point p along the x, y, and z directions in the radar coordinate system, respectively.

[0033] In an optional embodiment, the preprocessing of the point cloud frame data input by the target object further includes:

[0034] Get the coordinate values ​​of the points in the point cloud frame;

[0035] When the coordinate value of the point is a null value, data cleaning is performed on the point in the point cloud frame according to the distance from the point in the point cloud frame to the center of the target object and the upper and lower limits of the distance threshold.

[0036] In an optional embodiment, preprocessing the point cloud frame data input by the target object further includes:

[0037] Get the coordinate value of a point in the current point cloud frame;

[0038] Obtaining a relative pose of a vehicle based on a point in the current point cloud frame;

[0039] Obtain the inverse of the relative pose of the vehicle based on the last point in the current frame point cloud;

[0040] The coordinate value of the point after distortion is removed is obtained based on the coordinate value of a point in the current point cloud frame, the relative posture of the vehicle at a point in the current point cloud frame, and the inverse of the relative posture of the vehicle obtained based on the last point in the current frame point cloud.

[0041] In an optional embodiment, registering the points in each point cloud frame of each target object with the sub-image to obtain a registration result includes:

[0042] Get the odometer distance of the main target object;

[0043] Get the odometer distance threshold of the main target object;

[0044] Determine a perception overlap area between the main target object and an initial sub-image according to an odometer distance of the main target object and an odometer distance threshold of the main target object;

[0045] The initial sub-image is initialized based on the perceived overlap area between the main target object and the initial sub-image.

[0046] In an optional embodiment, registering the points in each point cloud frame of each target object with the sub-image to obtain a registration result further includes:

[0047] Obtain the position and posture of the main target object at this moment, and invert the position and posture of the target object at the previous moment; obtain the position and posture of the main target object at the predicted moment; obtain other target objects to obtain the external parameter conversion of the main target object;

[0048] Based on the pose of the main target object at the current moment, the pose of the target object at the previous moment is inverted, the pose of the main target object at the predicted moment and other target objects are used to obtain the external parameter conversion of the main target object to obtain the predicted pose of the main target object at the predicted moment;

[0049] Based on the predicted pose of the main target object at the prediction moment as an initial value, the points in each point cloud frame of each target object are aligned with the sub-image to obtain an alignment result.

[0050] On the one hand, a radar odometer mapping device is provided, comprising:

[0051] A preprocessing unit, used for preprocessing the point cloud frame data input by the target object to obtain preprocessed point cloud frame data;

[0052] A registration unit, used for registering the points in each point cloud frame of each target object with the sub-image to obtain a registration result, and using the registration result as basic data of the construction module of the sub-image, wherein the registration result includes the pose of the current point;

[0053] A mapping unit is used to voxelize the space stored in the point cloud frame, project the pose of the current point and the preprocessed point cloud frame data into the voxels of the space, obtain a first update value and a second update value of the midpoint of the current space voxel, update the basic data of the construction module of the sub-image based on the first update value and the second update value, and construct the sub-image based on the updated basic data of the sub-image construction module.

[0054] In another aspect, an electronic device is provided, comprising:

[0055] a memory storing execution instructions; and

[0056] A processor, wherein the processor executes the execution instruction stored in the memory, so that the processor executes any one of the above-mentioned radar odometer mapping methods.

[0057] In another aspect, a readable storage medium is provided, wherein the readable storage medium stores execution instructions, and when the execution instructions are executed by a processor, the execution instructions are used to implement any of the above-mentioned radar odometer mapping methods. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] The accompanying drawings illustrate exemplary embodiments of the present disclosure and together with the description serve to explain the principles of the present disclosure. These drawings are included to provide a further understanding of the present disclosure and are incorporated in and constitute a part of this specification.

[0059] Figure 1 A schematic diagram of the overall process of the radar odometer mapping method provided in an embodiment of the present disclosure;

[0060] Figure 2 A simplified flowchart of the radar odometer mapping method provided in the embodiment of the present disclosure with specific details

[0061] Figure 3 A schematic diagram of the overall process of preprocessing point cloud frame data input by a target object provided in an embodiment of the present disclosure;

[0062] Figure 4 A schematic diagram of a radar odometer mapping method S1 provided in an embodiment of the present disclosure;

[0063] Figure 5 A schematic diagram of a radar odometer mapping method S11 provided in an embodiment of the present disclosure;

[0064] Figure 6 A schematic diagram of the overall process of point cloud frame data registration provided by an embodiment of the present disclosure;

[0065] Figure 7 A schematic diagram of a radar odometer mapping method S2 provided in an embodiment of the present disclosure;

[0066] Figures 8 to 11 It is a schematic block diagram of the structure of a radar odometer mapping device using a hardware implementation of a processor according to an embodiment of the present disclosure. DETAILED DESCRIPTION

[0067] The present disclosure is further described in detail below in conjunction with the accompanying drawings and implementations. It is understood that the specific implementations described herein are only used to explain the relevant content, rather than to limit the present disclosure. It should also be noted that, for ease of description, only the parts related to the present disclosure are shown in the accompanying drawings.

[0068] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments in the present disclosure can be combined with each other. The technical solution of the present disclosure will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0069] Unless otherwise specified, the exemplary embodiments / embodiments shown will be understood as providing exemplary features of various details of some ways in which the technical concept of the present disclosure can be implemented in practice. Therefore, unless otherwise specified, the features of the various embodiments / embodiments can be combined, separated, interchanged and / or rearranged without departing from the technical concept of the present disclosure.

[0070] The use of cross-hatching and / or shading in the accompanying drawings is generally used to make the boundaries between adjacent components clear. As such, unless otherwise specified, the presence or absence of cross-hatching or shading does not convey or indicate any preference or requirement for the specific materials, material properties, dimensions, proportions, commonalities between the components shown, and / or any other characteristics, attributes, properties, etc. of the components. In addition, in the accompanying drawings, the sizes and relative sizes of the components may be exaggerated for clarity and / or descriptive purposes. When the exemplary embodiments can be implemented differently, the specific process sequence can be performed in a different order than described. For example, two successively described processes can be performed substantially simultaneously or in an order opposite to the described order. In addition, the same figure numbers represent the same components.

[0071] When a component is referred to as being "on" or "over," "connected to," or "coupled to" another component, the component may be directly on, directly connected to, or directly coupled to the other component, or intervening components may be present. However, when a component is referred to as being "directly on," "directly connected to," or "directly coupled to" another component, there are no intervening components. For this purpose, the term "connected" may refer to a physical connection, an electrical connection, etc., with or without intervening components.

[0072] The terms used herein are for the purpose of describing specific embodiments, and are not intended to be restrictive. As used herein, unless the context clearly indicates otherwise, the singular forms "one (kind, person)" and "said (the)" are also intended to include plural forms. In addition, when the terms "comprise" and / or "include" and their variations are used in this specification, it is explained that there are stated features, integral bodies, steps, operations, parts, assemblies and / or their groups, but it is not excluded that there are or add one or more other features, integral bodies, steps, operations, parts, assemblies and / or their groups. It should also be noted that, as used herein, the terms "substantially", "approximately" and other similar terms are used as approximate terms and not as degree terms, so that they are used to explain the inherent deviations of the measured values, calculated values ​​and / or the values ​​provided that will be recognized by those of ordinary skill in the art.

[0073] Semi-solid radars are low-cost and meet mass production requirements, so they are widely used in unmanned vehicles. However, compared with the traditional 360° mechanical rotating radar, the FOV of the semi-solid radar is only 120°, and a single radar is difficult to meet the perception requirements. Especially when the vehicle turns, the overlap rate of the point cloud data of the current frame and the point cloud data of the historical frame will be greatly reduced, making it difficult to provide good constraints for the registration of the point cloud, reducing the registration accuracy. In order to improve the perception ability, unmanned vehicles often adopt the configuration of multiple solid-state radars. Semi-solid radars are installed in front of the vehicle, on the left and on the right of the vehicle, and a certain overlap area is ensured between the two adjacent radars. When the vehicle is driving normally on the road, it is inevitable that it will encounter a parallel vehicle. Because the two vehicles are close to each other and the FOV of the side radar is small, the side radar will be severely blocked. According to the radar ranging principle, the radar data at this time is extremely unstable. If it is input into the SLAM system, the registration accuracy will be reduced. Therefore, for small FOV radar sensors, it is necessary to detect in real time whether their measurement data is blocked.

[0074] For multi-radar odometers and mapping solutions, it is generally necessary to strictly calibrate the external parameters between radars, then splice the measurement data of multiple radars together through external parameter conversion, and finally input them into the SLAM system. Alternatively, the rough calibration results of the radar external parameters are used as initial values, and they are input into the optimization system for iterative optimization to obtain accurate calibration results of the external parameters. Both of the above methods require time synchronization between multiple radars and require synchronous triggering. Secondly, no matter how high the calibration accuracy of the external parameters is, errors are inevitable. Therefore, when using this external parameter to update the map, calibration errors will inevitably be introduced into the map.

[0075] In order to improve the accuracy of odometer mapping and ensure the real-time performance of the system, laser SLAM often uses a sliding window optimization method, that is, multiple historical key frames are accumulated into one piece, which is used as a sub-image, and the current frame is aligned with the sub-image to estimate the pose of the current frame. When a new key frame is added, the historical key frame needs to be removed to maintain a certain number of key frames. The entire process of adding, removing and accumulating is very time-consuming, which seriously affects the real-time performance of laser SLAM. In view of this, the embodiment of the present disclosure provides a radar odometer mapping method that can solve the above technical problems.

[0076] Combined with the following Figures 1 to 11 The radar odometer mapping method, radar odometer mapping device, etc. disclosed in the present invention are described in detail.

[0077] See also Figure 1 and Figure 2 , a radar odometer mapping method provided by an embodiment of the present disclosure includes:

[0078] S1. Preprocessing the point cloud frame data input by the target object to obtain preprocessed point cloud frame data.

[0079] S2. Align the points in the point cloud frame of the target object with the sub-image to obtain an alignment result, and use the alignment result as basic data of the construction module of the sub-image, wherein the alignment result includes the position and posture of the current point.

[0080] S3. Voxelize the space where the point cloud frame is stored, project the pose of the current point and the preprocessed point cloud frame data into the space voxels, obtain the first update value and the second update value of the point in the current space voxel, update the basic data of the construction module of the sub-graph based on the first update value and the second update value, and construct the sub-graph based on the updated basic data of the construction module of the sub-graph.

[0081] The method provided by the embodiment of the present disclosure has at least the following beneficial effects:

[0082] The method provided by the embodiment of the present disclosure pre-processes the point cloud frame data input by the target object so that the input point cloud frame data can meet the needs of mapping, and aligns each point cloud frame of each target object with the sub-image, that is, regards the point cloud frame of each target object as an independent observation, does not involve the external parameter estimation of the target object and the stitching of the point cloud frame, can effectively avoid introducing calibration errors in the sub-image, and improves the alignment accuracy based on the point cloud data stored in multiple target objects; voxelizes the space where the point cloud frame is stored, projects the position and posture of the current point cloud frame and the pre-processed point cloud frame into the space voxel, obtains the first update value and the second update value of the midpoint of the current space voxel, updates the basic data of the construction module of the sub-image based on the first update value and the second update value, and constructs the sub-image based on the updated sub-image construction module basic data, which can realize the rapid update of the sub-image with new data and improve the accuracy of sub-image construction.

[0083] The method, device and readable storage medium provided by the embodiments of the present disclosure will be further explained and described below through optional embodiments.

[0084] The embodiments of the present disclosure are applicable to fields such as autonomous driving and mobile robots.

[0085] The disclosed embodiment uses an unmanned vehicle or a mobile robot as a vehicle, and the vehicle is equipped with multiple semi-solid laser radars to provide 3D point cloud information of the vehicle's surrounding environment in different directions. Multiple radars need to be time synchronized, but do not need to be triggered synchronously.

[0086] The disclosed embodiment uses airborne sensors to realize the odometer and mapping functions of the vehicle's outdoor scene. The vehicle can be an unmanned vehicle or a mobile robot that uses SLAM technology; the airborne sensors include multiple ranging sensors, among which the ranging sensors are semi-solid 3D laser radars, but are not limited to mechanical rotating 3D laser radars, solid-state laser radars, and other sensors that can provide 3D point cloud information of the environment space.

[0087] See also Figure 2 ,The entire radar odometry mapping process can include the following stages:

[0088] Preprocessing: Mainly process the point cloud frame data acquired by multiple radars: 1. Calculate the distance from each point in the point cloud frame to the center of the radar, sort all the distance values, and take the center value. If the center value is less than the set threshold, it is determined that the current radar is blocked by an obstacle, and the current frame point cloud is discarded. 2. Eliminate points with distance values ​​greater than the set upper distance limit and less than the set lower distance limit. The embodiment of the present disclosure believes that the ranging noise of these points is large. 3. Dedistort the current point cloud frame. 4. Perform voxel filtering on the current point cloud frame to reduce the number of points and improve the efficiency of registration.

[0089] Odometer: To avoid introducing calibration errors into the map, the disclosed embodiment treats each frame point of each radar as an independent observation and aligns it with the sub-map: 1. If the current frame is acquired by the side radar, after the registration is completed, the pose obtained by the registration is used to update the point cloud of the current frame to the sub-map; 2. If the current frame is acquired by the main radar, in addition to the above operations, the sub-map needs to be updated according to the pose of the current frame obtained by the registration.

[0090] Sub-graph management: This solution adopts the same modeling method as NDT. First, the space is voxelized, then the point cloud to be updated is projected into the corresponding space voxel, and the expectation and covariance information in the voxel is updated. If the current vehicle posture changes, it is necessary to remove some voxels that exceed the threshold boundary based on the current posture information and distance threshold.

[0091] Further, see Figure 2 , Figure 2 LIDAR1-3 in the figure represents radars 1-3. In the method provided in the embodiment of the present disclosure, there are multiple radars. Figure 2 As an example, the data of radars 1-3 are preprocessed, and point cloud data is input into the sub-map management database after preprocessing. Scan-to-submap is used for registration, and the sub-map management database is updated as the basic data of the building module after registration, and the current data is detected to be the point cloud frame data of the main radar. If so, the position of the sub-map is updated.

[0092] Before further explanation and description of the embodiments of the present disclosure, the following definitions and explanations are made:

[0093] Frame: Sensor data is read in the form of frames. Each frame is a complete packet of sensor data at a certain moment. The frequency of sending and receiving frames is determined by the system used by the vehicle.

[0094] Coordinate system: All coordinate systems disclosed herein are Cartesian rectangular coordinate systems.

[0095] Projection: The point cloud involved in the present disclosure is a point in a three-dimensional space, while the grid map has only a two-dimensional space. The process of calculating the point cloud and its corresponding grid index in the grid map can be called projection.

[0096] Posture: The posture described in the present disclosure is the position and posture of an object in 3D space, the position is the coordinates in the x, y and z directions, and the posture is the rotation angle around the x, y and z axes respectively.

[0097] Voxel: The concept of voxel is similar to that of pixel. Pixels are two-dimensional points, while voxels are small three-dimensional spaces.

[0098] Grid: The concept of grid can be compared to voxel. Voxel represents a small space in three-dimensional space, while grid represents a small space in two-dimensional space.

[0099] S1. Preprocessing the point cloud frame data input by the target object to obtain preprocessed point cloud frame data.

[0100] It should be noted that the target object provided in the embodiment of the present disclosure is a solid-state radar. It is understandable that the point cloud frame data obtained when the radar is blocked, or the point cloud frame data obtained has too much noise or the points in the point cloud frame are distorted, etc., all need to be processed to improve the accuracy of the registration.

[0101] The radar measures the distance and direction of surrounding objects by emitting laser beams, thereby determining the relative position of the vehicle and obstacles. When the radar emits enough laser beams, the laser points will be gathered into a point cloud, outlining the three-dimensional environment information, that is, the point cloud frame data provided by the embodiments of the present disclosure.

[0102] S2. Align the points in each point cloud frame of each target object with the sub-image to obtain the alignment result, and use the alignment result as the basic data of the construction module of the sub-image, wherein the alignment result includes the position and posture of the current point.

[0103] Each vehicle or mobile robot is equipped with more than one radar. By installing multiple radars, the surrounding environment can be monitored and observed 360°.

[0104] The multi-radar odometer mapping solution provided by the related technology first roughly calibrates the external parameters between multiple radars, then uses the external parameters as optimization variables, and uses the rough calibration results as initial values ​​to optimize in the multi-radar point cloud registration process, and finally uses the registered pose and optimized external parameters between multiple radars to update the current point cloud frame data of multiple radars to the sub-map. However, errors are inevitable in the radar external parameters after fine optimization. When using this external parameter to update the current frame point cloud data, the external parameter calibration error will undoubtedly be introduced into the sub-map.

[0105] The disclosed embodiment no longer performs online optimization on the external parameters of multiple radars, but instead treats the point cloud frames of multiple radars as independent observations, and aligns them with the currently maintained sub-images to obtain alignment results. The alignment results of each radar are used to update the current point cloud frame to the sub-image to expand the perception range of the sub-image.

[0106] Furthermore, the points in each point cloud frame of each radar are registered with the sub-image.

[0107] Furthermore, registering the current point cloud frame with the currently maintained sub-image includes obtaining the pose of the point in the point cloud frame based on the point cloud frame, matching the current point cloud frame with the sub-image to obtain the pose of the current point cloud frame, converting the current point cloud frame to the world coordinate system using the pose of the current point cloud frame, and accumulating the converted result with the sub-image to form a new sub-image. Therefore, the registration result includes the pose of the current point.

[0108] S3. Voxelize the space where the point cloud frame is stored, project the pose of the current point and the preprocessed point cloud frame data into the space voxels, obtain the first update value and the second update value of the point in the current space voxel, update the basic data of the construction module of the sub-graph based on the first update value and the second update value, and construct the sub-graph based on the updated basic data of the construction module of the sub-graph.

[0109] Voxel is the abbreviation of volume element, which is the smallest unit of data located on a regular grid in three-dimensional space. By voxelizing the space where the point cloud frame is stored and projecting the pose of the current point and the pre-processed point cloud frame data into the space voxel, the current point and the points in the pre-processed point cloud frame can be projected into the space voxel, which can quickly realize the update of the point cloud frame data.

[0110] It should be noted that the disclosed embodiment updates the sub-graph with the pose of the current point after registration. For the point to be updated, i.e., the current point in this embodiment, the basic data of the building module of the sub-graph is updated after the update processing based on the first update value and the second update value, so as to realize the real-time update of the point cloud.

[0111] In an optional embodiment, obtaining the first update value and the second update value of the midpoint of the voxel in the current space includes:

[0112] Get the coordinate value of the point falling in the current space voxel in the preset coordinate system;

[0113] Obtaining a first update value based on a coordinate value of a point located in a voxel of the current space in a preset coordinate system;

[0114] A second update value is obtained based on the first update value, the total number of points falling in the current space voxel, and the coordinate values ​​of the points falling in the current space voxel in the preset coordinate system.

[0115] It should be noted that the preset coordinate system provided by the present disclosure is a world coordinate system. The embodiment of the present disclosure adopts the concept of nested voxelization, that is, the space is voxelized for the first time, the voxel accuracy of the first voxelization is r1, and then the space contained in each voxel is voxelized again, the voxel accuracy of the second voxelization is r2, and r1>r2.

[0116] Furthermore, the idea of ​​nested voxelization can be implemented using a nested unordered_map structure (unordered_map is an associative container containing key values ​​with unique keys), namely: using the index of the first layer of voxels as the key and the new unordered_map structure as the value, a series of key-value pairs are constructed and inserted into the current unordered_map; for each key-value pair of the unordered_map structure, the index of the second layer of voxels is used as the key, and the data structure consisting of the number of points n, the first updated value of the point, and the second updated value of the point as members is used as the value.

[0117] In an optional embodiment, obtaining the coordinate value of a point falling in a voxel in the current space in a preset coordinate system includes:

[0118] Get the index value of the first-layer space voxel where the current point is located. If the index value of the first-layer space voxel meets the preset conditions, get the index value of the second-layer space voxel where the current point is located.

[0119] If the index value of the second-layer spatial voxel meets the preset condition, the current point falls in the current spatial voxel, and the coordinate value of the current point in the preset coordinate system is obtained.

[0120] In an optional embodiment, obtaining the index value of the first-layer space voxel where the current point is located includes obtaining the index value of the first-layer space voxel where the current point is located by using the following formula:

[0121]

[0122] Where: px, py, pz represent the coordinate values ​​of the current point p in the preset coordinate system; i 1x 、i 1y 、i 1zThey respectively represent the index value of the first layer voxel where the current point p is located; r1 represents the accuracy value of the first layer voxel.

[0123] Furthermore, when a new radar point cloud frame is input, the point cloud is first converted to the world coordinate system using the current radar pose obtained by registration. Then the converted point cloud frame is traversed, and for each point p, its index value in the first layer of voxels is calculated according to the above formula:

[0124] In an optional embodiment, obtaining the coordinate value of the current point in the preset coordinate system includes obtaining the index value of the second-layer space voxel where the current point is located by the following formula:

[0125]

[0126] Where: px, py, pz represent the coordinate values ​​of the current point p in the preset coordinate system; i 2x 、i 2y 、i 2z They respectively represent the index value of the second-layer voxel where the current point p is located; r2 represents the accuracy value of the second-layer voxel.

[0127] Furthermore, the index value of the first layer voxel where point p is located (i 1x 、i 1y 、i 1z ) as the key value of the unordered_map structure, search in the unordered_map structure, if the key value does not exist in the structure, insert a new key-value pair into the current unordered_map structure and return the inserted value; if the key value exists in the unordered_map structure, directly return the found value. Then calculate the index value of the point in the second layer of voxels according to the above formula.

[0128] In an alternative embodiment, see Figure 3 , S1 preprocesses the point cloud frame data input by the target object, which mainly includes four aspects: Occlusion judgment: Calculate the distance from each point in the current frame cloud to the center of the radar, sort all the distance values ​​and take the median distance value. If it is less than the threshold, the current radar is determined to be blocked, otherwise, vice versa. Data cleaning: Traverse each point in the current point cloud frame. If the position data of the point is a NAN value, it is removed; if the distance value from the point to the radar is less than the lower limit of the distance, or greater than the upper limit of the distance, it is removed. Distortion removal: Based on the timestamp information of each point in the point cloud frame, that is, based on the prediction of the current frame posture based on the posture of the historical frame, the points are unified to the same time to achieve the purpose of removing motion distortion. Downsampling: Perform adaptive voxel filtering on the current point cloud frame to reduce the number of points and improve the efficiency of registration.

[0129] In an alternative embodiment, see Figure 4 , S1 pre-processes the point cloud frame data input by the target object, including:

[0130] S11. Obtain the median distance between the points in the current point cloud frame and the center of the target object.

[0131] S12: When the median of the distance between the points in the current point cloud frame and the center of the target object is less than a preset median threshold, the point data in the current point cloud frame is eliminated.

[0132] The disclosed embodiment introduces a new metric: the median of the distance, which is used to determine whether the current radar is in a blocked state.

[0133] The configuration scheme of multiple solid-state radars can effectively make up for the defect of weak perception ability of semi-solid-state radars with small FOV. However, when a vehicle is driving normally on the road, if it encounters a vehicle running parallel to it, because the two vehicles are close to each other and the FOV of the semi-solid-state radar is small, the side radar is at greater risk of being blocked. When the radar is blocked, its measurement data will accumulate to form a point cloud cluster, which is difficult to apply to the alignment task. Therefore, for radar measurement data with a small FOV, it is necessary to determine in real time whether it is blocked. The disclosed embodiment proposes to obtain the median of the distance between the point in the current point cloud frame and the center of the target object, which is used to determine the blocked state of the current radar.

[0134] In an alternative embodiment, see Figure 5 , S11, obtaining the median distance between the points in the current point cloud frame and the center of the target object, including:

[0135] S111, obtaining the distance from the point in the point cloud frame to the center of the target object;

[0136] S112, sorting the distances from the points in all the point cloud frames to the center of the target object in ascending order, and taking the distance value in the middle of all the sorted points;

[0137] S113. Obtaining the median of the distance between the point in the current point cloud frame and the center of the target object according to the distance value at the middle of all the sorted points and a preset distance threshold.

[0138] Furthermore, each point p in the point cloud is traversed, and the distance from point p to the center of the radar is calculated according to the following formula:

[0139] r = sqrt(px*p.x+py*p.y+pz*pz)

[0140] Where r represents the distance from the point to the center of the radar; sqrt(·) represents the square root function; px, py, and pz represent the coordinate values ​​of point p along the x, y, and z directions in the radar coordinate system, respectively.

[0141] Sort the distance values ​​from all points to the center of the radar in ascending order, and then take the distance value r in the middle m ,in,

[0142]

[0143] Where: m represents the index value; size represents the number of all points; represents the floor function;

[0144] The preset distance threshold is denoted as σ d , if r m <σ d , then the current radar is determined to be blocked and the current frame point cloud is removed, otherwise, the current frame point cloud is retained. d It is an empirical value, generally ranging from 1.5 to 2.5.

[0145] During the operation of the radar, it will be affected by various factors, which will increase the noise of the measurement data. If the noisy data is used in high-precision registration tasks, it will seriously affect the accuracy of the registration. Therefore, it is necessary to clean the measurement data to reduce the points with large noise and unstable ranging in the point cloud.

[0146] In an optional embodiment, preprocessing the point cloud frame data inputted by the target object further includes: obtaining coordinate values ​​of points in the point cloud frame, and when the coordinate values ​​of the points are null values, obtaining the distance from the points in the point cloud frame to the target object;

[0147] Data cleaning is performed on the points in the point cloud frame according to the distance from the point in the point cloud frame to the target object and the upper and lower limits of the distance threshold.

[0148] Traverse each point in the point cloud. If its coordinate value is empty, remove the current point from the current frame point cloud. Otherwise, calculate the distance r from each point to the center of the radar. i , if r i <σ l or i >σ h , the current point is determined to be an unstable point and is removed from the point cloud of the current frame.

[0149] Furthermore, the upper and lower limits of the distance threshold are denoted as σ l , σ h According to experience, σ is usually taken l is 0.5m, σ h It is 80.0m.

[0150] In an alternative embodiment, see Figure 3, S1, preprocessing the point cloud frame data input by the target object, and also including:

[0151] S13. Obtain the coordinate value of a point in the current point cloud frame.

[0152] S14. Obtain a relative posture of the vehicle based on a point in the current point cloud frame.

[0153] S15. Obtain the inverse of the relative posture of the vehicle obtained based on the last point in the point cloud of the current frame.

[0154] S16. Obtain the coordinate value of the point after distortion is removed based on the coordinate value of a point in the current point cloud frame, the relative posture of the vehicle based on a point in the current point cloud frame, and the inverse of the relative posture of the vehicle obtained based on the last point in the current frame point cloud.

[0155] During the operation of the ranging sensor, the vehicle will inevitably move, causing the relative position between the vehicle and the obstacle to change, which will undoubtedly increase the measurement error of the ranging sensor. In order to reduce the measurement error caused by the movement of the vehicle, the present disclosure proposes to perform a distortion removal operation on the point cloud.

[0156] The embodiment of the present disclosure assumes that the position and posture changes of the points in the point cloud frames between two adjacent time intervals are not much different, that is: Where T i is the position of the point at time i, is the position of the point at time i-1.

[0157] Therefore, the change in posture from time point i to i+1 can be expressed by the change in posture from time point i-1 to i, that is, For the data of the point cloud frame collected at time i, the relative position and posture of the vehicle at the time corresponding to each point in the point cloud is obtained by linear interpolation according to the timestamp of each point in the point cloud. For details, please refer to the following formula:

[0158] T k =I*(1-scale)+ΔT i,i+1 *scale

[0159] Where: ΔT i,i+1 represents the relative position change of the vehicle from time i to i+1; scale represents the proportion of time i to k in the period from i to i+1, that is:

[0160]

[0161] Align point clouds at different times to the same time. For details, please refer to the following formula:

[0162]

[0163] Where: pi Indicates the coordinate value of the i-th point in the current frame point cloud; T i represents the relative position of the vehicle obtained by interpolation of the i-th point; represents the inverse of the relative pose of the vehicle obtained by interpolating the last point in the point cloud of the current frame; p i ′ represents the coordinate value of the point after the distortion is removed.

[0164] In order to reduce the amount of calculation and save the calculation cost, the point cloud data output by the ranging sensor is processed by voxel filtering to achieve the purpose of downsampling. The traditional voxel filtering algorithm voxels the space according to the set accuracy. Then the point cloud data of the current frame is projected into the space voxel. The space voxels are traversed, and the center of gravity of all points falling into the current voxel is calculated, and the center of gravity represents all points in the current voxel. Although this scheme can achieve the purpose of downsampling, it loses the accuracy of the space points.

[0165] In an optional embodiment, S1, preprocessing the point cloud frame data input by the target object also includes: for all points falling into the current grid, randomly selecting one of these points as the representative value of the current grid, and no longer calculating the center of gravity of all points.

[0166] In an alternative embodiment, see Figure 6 , S2 provided by the embodiment of the present disclosure, aligning the points in each point cloud frame of each target object with the sub-graph, mainly includes the following aspects: firstly measuring the point cloud frame by radar, if the point cloud frame is obtained by the main radar measurement, then predicting the pose of the measured point cloud frame: predicting the pose of the current point cloud frame according to the pose of the historical point cloud frame and the pose change between the point cloud frames. If not, initializing the sub-graph according to the point cloud frame of the radar.

[0167] Based on the predicted pose of the current point cloud frame, the NDT method is used for registration, the predicted value is used as the initial value of the pose of the current point cloud frame, and then scan-to-submap matching is performed. For detailed description, please see below.

[0168] Further, in an optional embodiment, see Figure 7 , S2, align the points in each point cloud frame of each target object with the sub-image to obtain the alignment result, including:

[0169] S21. Obtain the odometer distance of the main target object.

[0170] S22: Obtain an odometer distance threshold of the main target object.

[0171] S23: confirming a perception overlap area between the main target object and the initial sub-image according to the odometer distance of the main target object and the odometer distance threshold of the main target object.

[0172] S24, completing initialization of the initial sub-image based on the perceived overlap area between the main target object and the initial sub-image.

[0173] It should be noted that there are many radars installed on the vehicle or robot, including the main radar and the radars installed on the side. The main target object in this embodiment refers to the main radar, and the other target objects refer to the radars installed on the side.

[0174] The traditional multi-radar odometer mapping solution first roughly calibrates the external parameters between multiple radars, then uses the external parameters as optimization variables, and uses the rough calibration results as initial values ​​to optimize during the multi-radar point cloud registration process. Finally, the registered pose and optimized external parameters between multiple radars are used to update the current point cloud frame data of multiple radars to the sub-map. The finely optimized radar external parameters are inevitably subject to errors. When using this external parameter to update the current frame point cloud data, the external parameter calibration error will undoubtedly be introduced into the sub-map.

[0175] In order to improve the mapping accuracy of multi-radar odometers and avoid introducing external parameter calibration errors in sub-maps, the disclosed embodiment no longer performs online optimization of the external parameters of multiple radars, but instead treats the point cloud frames of multiple radars as independent observations, and aligns them with the currently maintained sub-maps respectively. The current point cloud frames are updated to the sub-maps using their respective alignment results to expand the perception range of the sub-maps.

[0176] Based on the above analysis, the S2 provided in the embodiment of the present disclosure, registering the points in each point cloud frame of each target object with the sub-image to obtain a registration result, further includes:

[0177] That is, the disclosed embodiment maintains a sub-graph, which should have a sufficiently large perception overlap area with each radar. In view of the wide field of view of the forward radar and the large perception overlap area with each side radar, the forward radar is taken as the main target object, and the point cloud frame of the forward radar is used as the initial data for radar odometer mapping, and the forward radar point cloud frame and its registration result are used to update the sub-graph, until the perception overlap area between the sub-graph and each side radar becomes large enough, and the point cloud frames of each side radar also participate in the multi-radar odometer mapping process.

[0178] As an example, the present disclosure uses the main radar odometer distance d m As a criterion for measuring whether there is sufficient perception overlap between the sub-image and the radars on each side. m >σ o It is determined that there is enough perceptual overlap, that is, the subgraph completes the initialization process, where σ o It is the odometer distance threshold, which is usually 80 to 100 meters.

[0179] In an optional embodiment, S2, registering the points in each point cloud frame of each target object with the sub-image to obtain a registration result, further includes:

[0180] Obtain the position and posture of the main target object at this moment, and invert the position and posture of the target object at the previous moment; obtain the position and posture of the main target object at the predicted moment; obtain other target objects to obtain the external parameter conversion of the main target object;

[0181] Based on the pose of the main target object at this moment, the pose of the target object at the previous moment is inverted, the pose of the main target object at the prediction moment and other target objects are used to obtain the external parameter conversion of the main target object to obtain the predicted pose of the main target object at the prediction moment;

[0182] Based on the predicted pose of the main target object at the prediction moment as the initial value, the points in each point cloud frame of each target object are aligned with the sub-image to obtain the alignment result.

[0183] Point cloud registration is essentially a least squares problem. The solution process often uses iterative optimization methods such as the GN algorithm (GN algorithm is a classic graph theory algorithm proposed by Grivan and Newman) and the LM algorithm (Levenbeerg-Marquardt algorithm), and a good initial value needs to be given. The embodiment of the present disclosure predicts the position and posture of each radar through the above algorithm.

[0184] The embodiment of the present disclosure assumes that the change in the radar posture between two adjacent time intervals is not much different, that is: Based on this assumption, the position and posture of the radar at time i+1 can be predicted.

[0185] If the current radar is the main radar, then:

[0186] Where: T i 、T i+1 Respectively represent the radar poses at time i and i+1; It means to invert the radar's posture at time i-1.

[0187] If the current radar is a side radar and the two historical frames are not discarded due to occlusion, the above formula can be used to predict the pose at time i+1; otherwise, it is necessary to use the pose of the main radar historical frame to interpolate the pose of the main radar at the prediction time, and right-multiply the side radar to the external parameter of the main radar to obtain the pose of the side radar at the prediction time, that is:

[0188] Where: T i ' +1 Indicates the interpolated main radar pose at the prediction time; Indicates the external parameter conversion from side radar to main radar.

[0189] The registration method of the disclosed embodiment adopts the NDT method, takes the predicted pose of the current frame as the initial value, and uses the Levenberg-Marquarelt method to iteratively optimize the pose of the current frame until the pose converges or reaches the maximum number of iterations, and finally outputs the optimized radar pose and the point cloud data of the current frame of the radar to the sub-image management module. The registration method of the disclosed embodiment can also use the Gauss-Newton algorithm for iterative optimization.

[0190] In an optional embodiment, the method provided by the embodiment of the present disclosure also includes updating the position of the point in the point cloud frame.

[0191] As the vehicle continues to move, the overlap between some voxels in the sub-image and the current frame point cloud continues to decrease, and the accumulated point cloud data causes the memory usage of the sub-image to continue to increase. Therefore, the embodiment of the present disclosure considers real-time removal of voxels in the sub-image that have no overlap or a small overlap with the current frame point cloud.

[0192] The embodiment of the present disclosure uses the registration posture of the main radar as the posture of the vehicle, and calculates the index value of the first layer voxel corresponding to the projection of the current position into the sub-image, traverses the voxels of the first layer, and if the distance between the index of the voxel and the index of the voxel where the current position is located exceeds the threshold σ i , then delete the voxel, where σ i The distance threshold d i The ratio of the first layer voxel accuracy value r1 is rounded to get the integer value, where d i Generally it is 80 to 100 meters.

[0193] Figures 8 to 11 It is a schematic block diagram of the structure of a radar odometer mapping device using a hardware implementation of a processor according to an embodiment of the present disclosure.

[0194] The radar odometer mapping device may include corresponding modules for executing each or several steps in the above flowchart. Therefore, each step or several steps in the above flowchart may be executed by the corresponding module, and the device may include one or more of these modules. The module may be one or more hardware modules specifically configured to execute the corresponding steps, or implemented by a processor configured to execute the corresponding steps, or stored in a computer-readable medium for implementation by a processor, or implemented by some combination.

[0195] The hardware structure of the radar odometer mapping device of the present disclosure can be implemented using a bus architecture. The bus architecture may include any number of interconnected buses and bridges, depending on the specific application and overall design constraints of the hardware. The bus 1100 connects various circuits including one or more processors 1200, memory 1300, and / or hardware modules together. The bus 1100 may also connect various other circuits 1400 such as peripherals, voltage regulators, power management circuits, external antennas, etc.

[0196] The bus 1100 may be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, or an Extended Industry Standard Component (EISA) bus, etc. The bus may be divided into an address bus, a data bus, a control bus, etc. For ease of representation, the figure only uses one connecting line, but does not mean that there is only one bus or one type of bus.

[0197] Any process or method description in the flowchart or otherwise described herein can be understood as a module, fragment or portion of a code representing one or more executable instructions for implementing the steps of a specific logical function or process, and the scope of the preferred embodiments of the present disclosure includes other implementations, in which the functions may not be performed in the order shown or discussed, including performing the functions in a substantially simultaneous manner or in a reverse order according to the functions involved, which should be understood by a person skilled in the art of the present disclosure. The processor performs the various methods and processes described above. For example, the method implementation in the present disclosure can be implemented as a software program, which is tangibly contained in a machine-readable medium, such as a memory. In some embodiments, part or all of the software program can be loaded and / or installed via a memory and / or a communication interface. When the software program is loaded into the memory and executed by the processor, one or more steps in the method described above can be performed. Alternatively, in other embodiments, the processor can be configured to perform one of the above methods in any other appropriate manner (e.g., by means of firmware).

[0198] The logic and / or steps represented in the flowchart or otherwise described herein may be embodied in any readable storage medium for use by an instruction execution system, apparatus, or device (such as a computer-based system, a system including a processor, or other system that can fetch instructions from and execute instructions on an instruction execution system, apparatus, or device), or in combination with such instruction execution systems, apparatuses, or devices.

[0199] For the purposes of this specification, a "readable storage medium" may be any device that can contain, store, communicate, propagate or transmit a program for use with or in conjunction with an instruction execution system, device or apparatus. More specific examples of readable storage media (a non-exhaustive list) include the following: an electrical connection with one or more wirings (electronic device), a portable computer disk case (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and editable read-only memory (EPROM or flash memory), a fiber optic device, and a portable read-only memory (CDROM). In addition, the readable storage medium may even be paper or other suitable medium on which the program can be printed, since the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, deciphering or, if necessary, processing in another suitable manner, and then stored in a memory.

[0200] It should be understood that the various parts of the present disclosure can be implemented in hardware, software, or a combination thereof. In the above-mentioned embodiments, multiple steps or methods can be implemented in software stored in a memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, it can be implemented by any one of the following technologies known in the art or their combination: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0201] A person skilled in the art may understand that all or part of the steps of implementing the above-mentioned implementation method may be completed by instructing related hardware through a program, and the program may be stored in a readable storage medium, which, when executed, includes one or a combination of the steps of the implementation method.

[0202] In addition, each functional unit in each embodiment of the present disclosure may be integrated into a processing module, or each unit may exist physically separately, or two or more units may be integrated into one module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. If the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a readable storage medium. The storage medium may be a read-only memory, a disk or an optical disk, etc.

[0203] refer to Figure 8 According to one embodiment of the present disclosure, a radar odometer mapping device 1000 is also provided, comprising:

[0204] The preprocessing unit 101 is used to preprocess the point cloud frame data input by the target object to obtain preprocessed point cloud frame data;

[0205] A registration unit 102 is used to register the points in each point cloud frame of each target object with the sub-image to obtain a registration result, and use the registration result as basic data of the construction module of the sub-image, wherein the registration result includes the position and posture of the current point;

[0206] The mapping unit 103 is used to voxelize the space where the point cloud frame is stored, project the pose of the current point and the preprocessed point cloud frame data into the space voxels, obtain the first update value and the second update value of the point in the current space voxel, update the basic data of the construction module of the sub-graph based on the first update value and the second update value, and construct the sub-graph based on the updated basic data of the construction module of the sub-graph.

[0207] See also Fig. 9 , the mapping unit 103 includes:

[0208] The first acquisition subunit 1031 is used to acquire the coordinate value of a point located in the current space voxel in a preset coordinate system.

[0209] The second acquisition subunit 1032 is used to obtain a first update value based on the coordinate value of the point falling in the current space voxel in the preset coordinate system.

[0210] The third acquisition subunit 1033 is used to obtain a second update value based on the first update value, the total number of points falling in the current space voxel, and the coordinate values ​​of the points falling in the current space voxel in the preset coordinate system.

[0211] The first acquisition subunit 1031 includes:

[0212] A first acquisition module is used to acquire the index value of the first-layer space voxel where the current point is located, and if the index value of the first-layer space voxel meets a preset condition, then acquire the index value of the second-layer space voxel where the current point is located;

[0213] The second acquisition module is used to obtain the coordinate value of the current point in the preset coordinate system if the index value of the second-layer space voxel meets the preset condition and the current point falls in the current space voxel.

[0214] The second acquisition subunit 1032 is used to obtain the index value of the first-layer spatial voxel where the current point is located by the following formula:

[0215]

[0216] Where: px, py, pz represent the coordinate values ​​of the current point p in the preset coordinate system; i 1x 、i 1y 、i1z They respectively represent the index value of the first layer voxel where the current point p is located; r1 represents the accuracy value of the first layer voxel.

[0217] The third acquisition subunit 1033 is used to obtain the index value of the second-layer space voxel where the current point is located by the following formula:

[0218]

[0219] Where: px, py, pz represent the coordinate values ​​of the current point p in the preset coordinate system; i 2x 、i 2y 、i 2z They respectively represent the index value of the second-layer voxel where the current point p is located; r2 represents the accuracy value of the second-layer voxel.

[0220] See also Fig.10 , the pre-processing unit 101 comprises:

[0221] The first preprocessing subunit 1011 is used to obtain the median of the distance between the points in the current point cloud frame and the center of the target object;

[0222] The second preprocessing subunit 1012 is used to remove point data in the current point cloud frame when the median of the distance between the points in the current point cloud frame and the center of the target object is less than a preset median threshold.

[0223] A first preprocessing subunit 1011 is used to obtain the distance from a point in the point cloud frame to the center of the target object;

[0224] Sort the distances from the points in the point cloud frame to the center of the target object in ascending order, and take the distance value in the middle of all sorted points;

[0225] The median distance between the point in the current point cloud frame and the center of the target object is obtained based on the distance value at the middle of all sorted points and the preset distance threshold.

[0226] The first preprocessing subunit 1011 is used to obtain the distance from all points in the point cloud frame to the center of the target object by the following formula:

[0227] r = sqrt(px*p.x+py*p.y+pz*pz)

[0228] Where r represents the distance from the point to the center of the radar; sqrt(·) represents the square root function; px, py, and pz represent the coordinate values ​​of point p along the x, y, and z directions in the radar coordinate system, respectively.

[0229] See also Fig.10 , the preprocessing unit 101 further includes:

[0230] The third preprocessing subunit 1013 is used to obtain the coordinate values ​​of the points in the point cloud frame;

[0231] The fourth preprocessing subunit 1014 is used to perform data cleaning on the points in the point cloud frame according to the distance from the point in the point cloud frame to the center of the target object and the upper and lower distance threshold values ​​when the coordinate value of the point is a null value.

[0232] See also Fig.10 , the preprocessing unit 101 further includes:

[0233] The fifth pre-processing sub-unit 1015 is used to obtain the coordinate value of a point in the current point cloud frame;

[0234] A sixth preprocessing subunit 1016 is used to obtain a relative posture of a vehicle based on a point in the current point cloud frame;

[0235] The seventh preprocessing subunit 1017 is used to obtain the inverse of the relative posture of the vehicle obtained based on the last point in the point cloud of the current frame;

[0236] The eighth preprocessing subunit 1018 is used to obtain the coordinate value of the point after distortion is removed based on the coordinate value of a point in the current point cloud frame, the relative posture of the vehicle at a point in the current point cloud frame, and the inverse of the relative posture of the vehicle obtained based on the last point in the current frame point cloud.

[0237] See also Fig.11 , the registration unit 102 comprises:

[0238] The fourth acquisition subunit 1021 is used to acquire the odometer distance of the main target object;

[0239] A fifth acquisition subunit 1022 is used to acquire an odometer distance threshold of a primary target object;

[0240] A sixth acquisition sub-unit 1023, configured to determine a perception overlap area between the main target object and the initial sub-image according to the odometer distance of the main target object and the odometer distance threshold of the main target object;

[0241] The seventh acquisition sub-unit 1024 is used to complete the initialization of the initial sub-image based on the perceived overlap area between the main target object and the initial sub-image.

[0242] See also Fig.11 , the registration unit 102 further includes:

[0243] The eighth acquisition subunit 1025 acquires the position and posture of the main target object at this moment, inverts the acquired position and posture of the target object at the previous moment; acquires the position and posture of the main target object at the prediction moment; acquires other target objects to obtain the external parameter conversion of the main target object;

[0244] A ninth acquisition subunit 1026, based on the posture of the main target object at the current moment, inverses the posture of the target object at the previous moment, the posture of the main target object at the prediction moment, and the external parameter conversion of the main target object obtained by other target objects to obtain the predicted posture of the main target object at the prediction moment;

[0245] The tenth acquisition subunit 1027 aligns the points in each point cloud frame of each target object with the sub-image based on the predicted posture of the main target object at the prediction moment as an initial value to obtain a registration result.

[0246] The present disclosure also provides an electronic device, including: a memory, the memory storing execution instructions; and a processor or other hardware module, the processor or other hardware module executes the execution instructions stored in the memory, so that the processor or other hardware module executes the above method.

[0247] The present disclosure also provides a readable storage medium, in which execution instructions are stored. When the execution instructions are executed by a processor, they are used to implement the above method.

[0248] Those skilled in the art should understand that the above embodiments are only for the purpose of clearly illustrating the present disclosure, and are not intended to limit the scope of the present disclosure. For those skilled in the art, other changes or modifications may be made based on the above disclosure, and these changes or modifications are still within the scope of the present disclosure.

Claims

1. A radar odometer mapping method, characterized in that: include: Preprocessing the point cloud frame data input by the target object to obtain preprocessed point cloud frame data; Registering the points in the point cloud frame of the target object with the sub-image to obtain a registration result, and using the registration result as basic data of the construction module of the sub-image, wherein the registration result includes the pose of the current point; and voxelize the space where the point cloud frame is stored, project the position and posture of the current point and the preprocessed point cloud frame data into the space voxel, obtain the first update value and the second update value of the midpoint of the current space voxel, update the basic data of the construction module of the sub-graph based on the first update value and the second update value, and construct the sub-graph based on the updated basic data of the sub-graph construction module; The registering the points in the point cloud frame of the target object with the sub-image to obtain the registration result includes: Acquire the odometer distance of the main target object; acquire the odometer distance threshold of the main target object; confirm the perception overlap area between the main target object and the initial sub-graph according to the odometer distance of the main target object and the odometer distance threshold of the main target object; complete the initialization of the initial sub-graph based on the perception overlap area between the main target object and the initial sub-graph; The registering the points in the point cloud frame of the target object with the sub-image to obtain the registration result also includes: Obtain the pose of the main target object at this moment, and invert the pose of the target object at the previous moment; obtain the pose of the main target object at the predicted moment; obtain other target objects to obtain the external parameter conversion of the main target object; obtain the predicted pose of the main target object at the predicted moment based on the pose of the main target object at this moment, invert the pose of the target object at the previous moment, the pose of the main target object at the predicted moment, and the external parameter conversion of the main target object obtained by other target objects; based on the predicted pose of the main target object at the predicted moment as an initial value, align the points in each point cloud frame of each target object with the sub-image to obtain the alignment result; The method of obtaining the first update value and the second update value of the midpoint of the current space voxel includes: obtaining the coordinate value of the point falling in the current space voxel in a preset coordinate system; obtaining the first update value based on the coordinate value of the point falling in the current space voxel in the preset coordinate system; obtaining the second update value based on the first update value, the total number of points falling in the current space voxel, and the coordinate value of the point falling in the current space voxel in the preset coordinate system.

2. The radar odometer mapping method according to claim 1, characterized in that: The obtaining of the coordinate value of a point falling in the current space voxel in a preset coordinate system includes: Obtaining the index value of the first-layer space voxel where the current point is located, and if the index value of the first-layer space voxel meets a preset condition, obtaining the index value of the second-layer space voxel where the current point is located; If the index value of the second-layer space voxel meets the preset condition, the current point falls in the current space voxel, and the coordinate value of the current point in the preset coordinate system is obtained; The index value of the first-layer spatial voxel where the current point is located is obtained by the following formula: Where: px, py, pz represent the coordinate values ​​of the current point p in the preset coordinate system; i 1x 、i 1y 、i 1z They represent the index value of the first layer voxel where the current point p is located; r1 represents the precision value of the first layer voxel; The index value of the second-layer space voxel where the current point is located is obtained by the following formula: Where: px, py, pz represent the coordinate values ​​of the current point p in the preset coordinate system; i 2x 、i 2y 、i 2z They respectively represent the index value of the second-layer voxel where the current point p is located; r2 represents the accuracy value of the second-layer voxel.

3. The radar odometer mapping method according to claim 1, characterized in that: The preprocessing of the point cloud frame data input by the target object includes: Get the median distance between the points in the current point cloud frame and the center of the target object; When the median of the distance between the point in the current point cloud frame and the center of the target object is less than a preset median threshold, the point data in the current point cloud frame is eliminated; Get the coordinate values ​​of the points in the point cloud frame; When the coordinate value of the point is a null value, data cleaning is performed on the point in the point cloud frame according to the distance from the point in the point cloud frame to the center of the target object and the upper limit of the distance threshold and the lower limit of the distance threshold; Get the coordinate value of a point in the current point cloud frame; Obtaining a relative pose of a vehicle based on a point in the current point cloud frame; Obtain the inverse of the relative pose of the vehicle based on the last point in the current frame point cloud; The coordinate value of the point after distortion is removed is obtained based on the coordinate value of a point in the current point cloud frame, the relative posture of the vehicle at a point in the current point cloud frame, and the inverse of the relative posture of the vehicle obtained based on the last point in the current frame point cloud.

4. The radar odometer mapping method according to claim 3, characterized in that: The step of obtaining the median of the distance between a point in the current point cloud frame and the center of the target object includes: Get the distance from the point in the point cloud frame to the center of the target object; Sort the distances from the points in the point cloud frame to the center of the target object in ascending order, and take the distance value in the middle of all the sorted points; Obtaining the median of the distance between the point in the current point cloud frame and the center of the target object according to the distance value at the middle of all the sorted points and a preset distance threshold; Among them, the distance from all points in the point cloud frame to the center of the target object is obtained by the following formula: r = sqrt(px*p.x+py*p.y+pz*pz) Where r represents the distance from the point to the center of the radar; sqrt(·) represents the square root function; px, py, and pz represent the coordinate values ​​of point p along the x, y, and z directions in the radar coordinate system, respectively.

5. A radar odometer mapping device, characterized in that: include: A preprocessing unit, used for preprocessing the point cloud frame data input by the target object to obtain preprocessed point cloud frame data; A registration unit, used to register the points in each point cloud frame of each target object with the sub-image to obtain a registration result, and use the registration result as basic data of the construction module of the sub-image, wherein the registration result includes the pose of the current point; and A mapping unit, configured to voxelize the space stored in the point cloud frame, project the pose of the current point and the preprocessed point cloud frame data into the space voxels, obtain a first update value and a second update value of the midpoint of the current space voxel, update the basic data of the construction module of the sub-graph based on the first update value and the second update value, and construct the sub-graph based on the updated basic data of the sub-graph construction module; The step of registering the points in each point cloud frame of each target object with the sub-image to obtain a registration result includes: Acquire the odometer distance of the main target object; acquire the odometer distance threshold of the main target object; confirm the perception overlap area between the main target object and the initial sub-graph according to the odometer distance of the main target object and the odometer distance threshold of the main target object; complete the initialization of the initial sub-graph based on the perception overlap area between the main target object and the initial sub-graph; The registering the points in each point cloud frame of each target object with the sub-image to obtain the registration result also includes: Obtain the pose of the main target object at this moment, and invert the pose of the target object at the previous moment; obtain the pose of the main target object at the predicted moment; obtain other target objects to obtain the external parameter conversion of the main target object; obtain the predicted pose of the main target object at the predicted moment based on the pose of the main target object at this moment, invert the pose of the target object at the previous moment, the pose of the main target object at the predicted moment, and the external parameter conversion of the main target object obtained by other target objects; based on the predicted pose of the main target object at the predicted moment as an initial value, align the points in each point cloud frame of each target object with the sub-image to obtain the alignment result; The obtaining of the first updated value and the second updated value of the midpoint of the voxel in the current space includes: Obtain the coordinate value of the point falling in the current space voxel in the preset coordinate system; obtain the first update value based on the coordinate value of the point falling in the current space voxel in the preset coordinate system; obtain the second update value based on the first update value, the total number of points falling in the current space voxel and the coordinate value of the point falling in the current space voxel in the preset coordinate system.

6. An electronic device, characterized in that: include: A memory storing execution instructions; as well as A processor, wherein the processor executes the execution instructions stored in the memory, so that the processor executes the radar odometer mapping method according to any one of claims 1 to 4.

7. A readable storage medium, characterized in that: The readable storage medium stores execution instructions, which are used to implement the radar odometer mapping method according to any one of claims 1 to 4 when executed by the processor.

Images