Radar positioning method, electronic equipment, device and storage medium

Abstract

The present disclosure provides a radar positioning method, device, electronic device and storage medium, which relate to the fields of autonomous driving technology and robotics technology. The radar positioning method of the present disclosure includes: performing voxel conversion on offline point cloud data, and the converted point cloud data includes statistical parameters of the point cloud distribution in the current voxel; performing at least one step of registration on the currently acquired point cloud data to obtain a final registration result, and the final registration result includes the pose of the current point cloud frame; adding the pose of the current point cloud frame to the pose graph based on the final registration result to perform pose optimization; updating the accumulated error in the odometer subgraph based on the optimized point cloud frame pose and switching the subgraph in the global map, and performing positioning based on the subgraph in the updated global map. The present disclosure improves the accuracy of positioning, can effectively realize the compression of environmental information, and reduce storage costs; effectively saves the time of online registration and improves the registration efficiency.

Description

Technical Field

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

[0002] In recent years, SLAM (simultaneous localization and mapping) technology has become more mature and successfully applied to driverless technology, playing an increasingly important role. Since SLAM needs to process sensor data in real time, such as images and point clouds, it takes up a lot of CPU and memory resources. However, in order to reduce costs, car companies generally use on-board chips with lower computing power and smaller storage units, which undoubtedly limits the application of SLAM technology. Therefore, most driverless companies and car companies adopt offline mapping and online positioning solutions.

[0003] Due to the great influence of lighting conditions, pure visual positioning solutions are difficult to handle scenes with poor lighting conditions such as nighttime. Secondly, due to the influence of weather changes, the generated visual global map is difficult to use for later visual positioning, thus restricting the widespread application of unmanned driving technology. However, since LiDAR ranging is not affected by lighting, weather and other conditions, it has gradually become an essential sensor for realizing all-weather and all-scenario unmanned driving technology. In addition, as the manufacturing process becomes increasingly mature and the manufacturing cost gradually decreases, more and more car companies are beginning to consider installing LiDAR on their own unmanned vehicles to improve the vehicle's perception and positioning capabilities.

[0004] The following are some prior art documents.

[0005] Technology 1 "NDT-Matching" is a positioning module in the open source unmanned driving framework Autoware. First, it loads the global point cloud map generated by offline SLAM technology into the memory and sets it as the target point cloud of the NDT registration method; then it continuously receives point cloud frames and sets them as the source point cloud of the NDT registration method, and finally performs the registration operation to obtain the pose of the current frame point cloud. The above scheme will undoubtedly increase memory consumption by loading the entire global point cloud map into the memory; secondly, there is no offline conversion of the original point cloud into the NDT-Mapping required for NDT registration. The online conversion method will undoubtedly increase the registration time, reduce the registration efficiency, and affect the real-time performance of the system; finally, affected by the dynamic obstacles and potential dynamic obstacles in the global map, the point cloud pose will produce different degrees of jumps using the NDT registration method, but the system does not handle this.

[0006] Technology 2 "SmartCar-pcd-map-Tools" is a repository on github, which provides the segmentation function of the global point cloud map in large-scale scenes. It is believed that due to the density of point cloud maps, it is difficult to load all maps at once in large-scale scenes. Therefore, it is very necessary to divide the map into multiple sub-maps and realize the dynamic loading of point cloud sub-maps. First, there are many points that overlap in the global map established by SLAM, and downsampling operations need to be performed to reduce the number of points; secondly, traverse each point in the global point cloud map after downsampling and project it into a grid of a custom size divided in the XOY plane to realize the segmentation of the point cloud map. When using global map positioning, the grid point cloud of the area can be loaded according to the current position of the vehicle to reduce memory usage. This solution only realizes the segmentation function of the point cloud, and does not combine the registration algorithm used for positioning to realize the conversion function of the point cloud map.

[0007] The current online LiDAR positioning process: First, select the best mapping solution according to the scene, and use the collected LiDAR point cloud data to generate a globally consistent and high-precision point cloud map; second, load the generated point cloud map into the memory and set it as the target point cloud of the NDT registration algorithm; finally, continuously accept new LiDAR point cloud data, set it as the source point cloud of the NDT registration algorithm, and perform the registration operation to obtain the global pose information of the current frame point cloud. Affected by various factors, the above process will have the following problems when executing:

[0008] 1. The limited on-board storage unit makes it difficult to load the entire global point cloud map into memory, especially when facing large-scale scenes at the city level. Although Technology 2 provides a method to segment the global point cloud map, only the grid point cloud of the area is loaded during positioning. This dynamic loading method can effectively reduce memory usage. However, when the trajectory of the vehicle is exactly on the boundary of the two grid point clouds, due to the certain error in the vehicle's posture, it will undoubtedly cause the current grid where the vehicle is located to frequently switch between the two adjacent grids, thereby increasing the number of writes from memory to the hard disk, seriously affecting the system's operating efficiency.

[0009] 2. According to the principle of the NDT registration algorithm, when setting the target point cloud, the algorithm first voxelizes the space and projects the target point cloud into the space voxels. Finally, it traverses the space voxels to calculate statistics such as expectation and covariance. When the algorithm switches to a new target point cloud, the above initialization process is required. Therefore, when using the NDT registration algorithm for online positioning, when the vehicle switches between different grid areas, the initialization process will seriously affect the operating efficiency of the system.

[0010] 3. Affected by dynamic obstacles and potential dynamic obstacles, the point cloud map established offline is different from the current positioning scene. For example, there may be moving targets around the vehicle when collecting data, or other vehicles parked on the roadside, but when positioning, the vehicle is driven away, etc. Although the NDT registration algorithm can tolerate some of these differences, it has been tested that when using such maps for positioning, the output posture will have different degrees of jumps, which will undoubtedly affect the accuracy of the control module. Summary of the invention

[0011] The present disclosure provides a radar positioning method, device, electronic equipment and storage medium.

[0012] On the one hand, a radar positioning method is provided, comprising:

[0013] Perform voxel conversion on the offline point cloud data, and the converted point cloud data includes statistical parameters of the point cloud distribution in the current voxel;

[0014] Perform at least one step of registration on the currently acquired point cloud data to obtain a final registration result, wherein the final registration result includes the pose of the current point cloud frame;

[0015] Adding the pose of the current point cloud frame to the pose graph based on the final registration result to perform pose optimization;

[0016] Based on the optimized point cloud frame pose, the accumulated error in the odometer submap is updated and the submap in the global map is switched, and positioning is performed based on the updated submap in the global map.

[0017] In an optional embodiment, performing at least one step of registration on the currently acquired point cloud data to obtain a final registration result includes:

[0018] Performing a first step of registration on the currently acquired point cloud data to obtain a first registration result, wherein the first registration result includes a first position of the current point cloud frame;

[0019] Based on the relative pose between the first pose of the current point cloud frame and the odometer pose of the vehicle when performing the latest observation, and the statistical parameters, a second registration result is obtained, and the second registration result is used as the final registration result.

[0020] In an optional embodiment, performing a first step of registering the currently acquired point cloud data to obtain a first registration result includes:

[0021] The sub-image formed by the historical key frames selected in the current period is used as the first target point cloud, and the current point cloud frame is used as the first source point cloud;

[0022] An alignment operation is performed on the first target point cloud and the first source point cloud to obtain a first position of the current point cloud frame.

[0023] In an optional embodiment, the second registration is performed based on the relative pose between the pose of the current point cloud frame and the odometer pose of the vehicle when performing the latest observation, and the statistical parameters to obtain a second registration result, including:

[0024] A global subgraph composed of the point clouds at the current position of the vehicle and the adjacent area is used as the second target point cloud, and the current point cloud frame is used as the second source point cloud. The second target point cloud and the second source point cloud are aligned to obtain a second pose of the current point cloud frame.

[0025] In an optional embodiment, after performing a first step of registration on the currently acquired point cloud data to obtain a first registration result, wherein the first registration result includes a first pose of the current point cloud frame, and before performing a second step of registration, the method further includes updating the sub-image:

[0026] Get the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose;

[0027] The sub-image is updated based on the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose.

[0028] In an optional embodiment, obtaining the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose includes:

[0029] Get the latest keyframe pose, the current frame pose, and the rotation part of the current frame pose;

[0030] Based on the latest key frame pose, the current frame pose and the rotation part of the current frame pose, the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose are obtained.

[0031] In an optional embodiment, the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose are obtained by the following formula:

[0032]

[0033] θ=[RotMatToEulAng(r rel )]3

[0034] Where: Indicates t relThe square of the i-th component in ; RotMatToEulAng(·) means converting the rotation matrix into Euler angles; [·]3 means taking the third component in the vector.

[0035] In an optional embodiment, updating the accumulated error in the odometer sub-map based on the optimized point cloud frame pose and switching the sub-map in the global map include:

[0036] Obtaining an odometer pose, and obtaining a change in the current point cloud frame pose based on the odometer pose and the optimized point cloud pose;

[0037] Based on the change in the current point cloud frame pose, the accumulated error in the odometer sub-map is updated and the sub-map in the global map is switched.

[0038] In an optional embodiment, updating the accumulated error in the odometer sub-map and switching the sub-map in the global map based on the change in the current point cloud frame pose includes:

[0039] An index value corresponding to the point cloud frame is obtained based on the optimized point cloud frame pose. If the index value changes, a position corresponding to the current index value and a position adjacent to the position corresponding to the current index value are updated.

[0040] In an optional embodiment, updating the accumulated error in the odometer sub-map based on the optimized point cloud frame pose and switching the sub-map in the global map, and performing positioning based on the updated sub-map in the global map further includes:

[0041] According to the optimized point cloud frame pose, a grid index value corresponding to the optimized point cloud frame pose is obtained, and if the index value changes, the current grid and its neighboring grids are updated;

[0042] For the grid beyond the neighborhood range, if the absolute value of the difference between the grid index value beyond the neighborhood range and the current grid index value is greater than the threshold, the current grid will be deleted; otherwise, the current grid will be retained.

[0043] In an optional embodiment, the offline point cloud data is voxel-converted, and the converted point cloud data includes statistical parameters of the point cloud distribution in the current voxel, including:

[0044] The offline point cloud data is converted to voxels using the following formula:

[0045]

[0046] Among them, p n Indicates the coordinate value of the nth point falling in the current voxel; E n-1 represents the expected value of the midpoint of the current voxel before updating; E n Represents the expected value of the midpoint of the current voxel after update;

[0047]

[0048] Where: C(n-1) represents the covariance of the midpoint of the current voxel before updating, and Cn represents the covariance of the midpoint of the current voxel after updating;

[0049] The statistical parameters include the covariance and the expected value.

[0050] On the other hand, a radar positioning device is provided, comprising:

[0051] A conversion unit, used for performing voxel conversion on offline point cloud data, wherein the converted point cloud data includes statistical parameters of point cloud distribution in the current voxel;

[0052] A registration unit, used to perform at least one step of registration on the currently acquired point cloud data to obtain a final registration result, wherein the final registration result includes the pose of the current point cloud frame;

[0053] An optimization unit, configured to add the pose of the current point cloud frame to a pose graph based on the final registration result to perform pose optimization;

[0054] A positioning unit is used to update the accumulated error in the odometer sub-map based on the optimized point cloud frame pose and switch the sub-map in the global map, and perform positioning based on the updated sub-map in the global map.

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

[0056] A memory storing execution instructions;

[0057] A processor, wherein the processor executes the execution instruction stored in the memory, so that the processor executes any one of the radar positioning methods described above.

[0058] On the other hand, 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 positioning methods. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] 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.

[0060] Figure 1 A schematic diagram of the overall process of the radar positioning method provided by an embodiment of the present disclosure;

[0061] Figure 2A simplified flowchart of the radar positioning method provided in the embodiment of the present disclosure with specific details

[0062] Figure 3 A schematic diagram of a voxel conversion process for offline point cloud data provided by an embodiment of the present disclosure;

[0063] Figure 4 A schematic diagram of a radar positioning method S2 provided in an embodiment of the present disclosure;

[0064] Figure 5 A schematic diagram of the scan-to-submap registration process provided by an embodiment of the present disclosure;

[0065] Figure 6 A schematic diagram of a radar positioning method S4 provided in an embodiment of the present disclosure;

[0066] Figure 7 A schematic diagram of constructing a pose graph provided in an embodiment of the present disclosure;

[0067] Figure 8 A schematic diagram of global map update provided by an embodiment of the present disclosure;

[0068] Fig. 9 Another schematic diagram of global map update provided by an embodiment of the present disclosure;

[0069] Fig.10 A schematic diagram of point cloud data preprocessing provided in an embodiment of the present disclosure;

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

[0071] 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.

[0072] 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.

[0073] 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.

[0074] 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.

[0075] 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.

[0076] 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.

[0077] Combined with the following Figures 1 to 12 The radar positioning method, radar positioning device, etc. disclosed in the present invention are described in detail.

[0078] On the one hand, see Figure 1 and Figure 2 The present disclosure provides a radar positioning method, including:

[0079] S1. Perform voxel conversion on the offline point cloud data, where the converted point cloud data includes statistical parameters of the point cloud distribution in the current voxel.

[0080] S2. Perform at least one step of registration on the currently acquired point cloud data to obtain a final registration result, which includes the position and posture of the current point cloud frame.

[0081] S3. Based on the final registration result, the pose of the current point cloud frame is added to the initial pose graph to perform pose optimization.

[0082] S4. Based on the optimized point cloud frame pose, the accumulated error in the odometer submap is updated and the submap in the global map is switched, and positioning is performed based on the submap in the updated global map.

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

[0084] The method provided by the embodiment of the present disclosure is based on two-step registration, wherein the first step of registration can effectively suppress the posture jump caused by dynamic obstacles or potential dynamic obstacles; based on the first step of registration result including the posture of the current point cloud frame, the relative posture between the posture of the current point cloud frame and the odometer posture of the vehicle when performing the latest observation, and the statistical parameters, the second step of registration is performed to obtain the second registration result; the second step of registration can effectively correct the cumulative error in the sub-graph matching; finally, based on the second registration result, the posture of the current frame is added to the posture graph to perform posture optimization; based on the optimized posture, the cumulative error in the odometer sub-graph is updated and the sub-graph in the global map is switched. The method provided by the embodiment of the present disclosure solves the posture jump caused by dynamic obstacles or potential dynamic obstacles, improves the positioning accuracy, and by performing voxel conversion on the offline point cloud data, the converted point cloud data includes the statistical parameters of the point cloud distribution in the current voxel, on the one hand, it can effectively realize the compression of environmental information and reduce the storage cost; on the other hand, by placing the initialization process of the registration in the offline processing step, it can effectively save the time of online registration and improve the registration efficiency.

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

[0086] 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.

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

[0088] 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.

[0089] 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.

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

[0091] 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.

[0092] The disclosed embodiments use an unmanned vehicle or a mobile robot as a vehicle, and the vehicle is equipped with ranging sensors such as a laser radar to provide 3D point cloud information of the surrounding environment space at different locations of the vehicle.

[0093] Figure 2 In the figure, LIDAR is radar, the frequency is 10HZ, and Global Map is the global map.

[0094] S1. Perform voxel conversion on the offline point cloud data, where the converted point cloud data includes statistical parameters of the point cloud distribution in the current voxel.

[0095] The disclosed embodiment adopts NDT-Mapping (Normal Distribution Transform) for mapping, so before mapping, the offline point cloud data is first voxel converted, and the global point cloud map provided by the mapping module is converted into NDT-Mapping, including voxel projection, statistics calculation and other operations.

[0096] For further information, see Figure 3 , first perform voxel projection, traverse each point in the Global Map, that is, the point cloud provided by the embodiment of the present disclosure, and calculate the index value of the calculator in the nested voxel. Then perform statistical calculation: traverse each voxel in the nested voxel, and calculate the statistical parameters of the Gaussian distribution according to the position information projected to the midpoint of the current voxel, that is, Figure 3 The expected value and covariance of .

[0097] S2. Perform at least one step of registration on the currently acquired point cloud data to obtain a final registration result, which includes the position and posture of the current point cloud frame.

[0098] The radar measures the distance and direction of objects in the surrounding environment by emitting laser beams, thereby determining the relative position of the vehicle and obstacles. When it emits enough laser beams, the laser points will be gathered into a point cloud, outlining the three-dimensional environment information in which it is located. This is the point cloud data provided by the embodiment of the present disclosure. For most laser radars, although the emission and reception of the laser are very fast, each point that makes up the point cloud is not generated at the same time. Generally, the data accumulated within 100ms (corresponding to a typical value of 10Hz) is output as a frame of point cloud, that is, the point cloud frame provided by the embodiment of the present disclosure.

[0099] Through at least one step of registration, the pose jump caused by dynamic obstacles or potential dynamic obstacles can be effectively suppressed; and the accumulated error in sub-image matching can be effectively corrected.

[0100] S3. Based on the final registration result, the pose of the current point cloud frame is added to the initial pose graph to perform pose optimization.

[0101] Since the final registration result includes the pose of the current point cloud frame, the pose of the current point cloud frame is inserted into the initial pose graph to optimize the initial pose.

[0102] If the positioning process lasts for a long time, more point cloud frame nodes will be inserted into the pose graph. If all nodes are optimized, a long optimization time will be required, which will seriously affect the execution efficiency of the program. To solve the above problem, the embodiment of the present disclosure believes that the pose after fusion optimization has higher accuracy and does not need to participate in the subsequent optimization. Therefore, after the fusion optimization is completed, the pose of the current frame can be fixed to reduce the number of nodes involved in the subsequent optimization, reduce the optimization time, and improve the program running efficiency.

[0103] S4. Based on the optimized point cloud frame pose, the accumulated error in the odometer submap is updated and the submap in the global map is switched, and positioning is performed based on the submap in the updated global map.

[0104] There are inevitably dynamic obstacles and potential dynamic obstacles in the scene, which have many effects on the mapping and positioning process. For example, dynamic obstacles will leave long tails in the map, but there are no identical obstacles at the same location in the scene during positioning, which causes a mismatch between the map and the scene, affecting positioning accuracy. Potential dynamic obstacles will also cause a mismatch between the map and the scene, for example, a vehicle parked on the roadside during mapping is moved away during positioning. Therefore, based on the optimized point cloud frame pose, the accumulated error in the odometer submap is updated and the submap in the global map is switched, and positioning is performed based on the submap in the updated global map.

[0105] In an alternative embodiment, see Figure 4, S2, perform at least one step of registration on the currently acquired point cloud data to obtain the final registration result, including:

[0106] S21. Perform a first step of registration on the currently acquired point cloud data to obtain a first registration result, wherein the first registration result includes a first position of the current point cloud frame.

[0107] S22, performing a second registration based on the relative pose between the first pose of the current point cloud frame and the odometer pose of the vehicle when performing the latest observation, as well as statistical parameters, to obtain a second registration result, and using the second registration result as the final registration result.

[0108] The first step of registration provided by the embodiment of the present disclosure may be registration of submaps, namely scan-to-submap registration. Scan-to-submap registration may effectively suppress posture jumps caused by dynamic obstacles or potential dynamic obstacles.

[0109] The second step of registration can be the registration of the global sub-map, that is, the scan-to-globalmap registration. The scan-to-globalmap registration can effectively correct the accumulated errors in the sub-map matching.

[0110] In an optional embodiment, S21, performing a first step of registration on the currently acquired point cloud data to obtain a first registration result, including:

[0111] The sub-image formed by the historical key frames selected in the current period is used as the first target point cloud, and the current point cloud frame is used as the first source point cloud;

[0112] The first target point cloud and the first source point cloud are aligned to obtain the first position of the current point cloud frame.

[0113] There are inevitably dynamic obstacles and potential dynamic obstacles in the scene, which have many impacts on the mapping and positioning process: dynamic obstacles will leave long tails in the map, but there are no identical obstacles at the same location in the scene during positioning, resulting in a mismatch between the map and the scene, affecting positioning accuracy; potential dynamic obstacles will also cause a mismatch between the map and the scene, for example, a vehicle parked on the roadside during mapping may be moved away during positioning.

[0114] The NDT registration algorithm adopted in the embodiment of the present disclosure can tolerate the mismatch between the map and the scene to a certain extent. This is why the embodiment of the present disclosure selects the NDT registration algorithm when positioning. However, according to tests, this mismatch will cause the posture obtained by NDT registration to jump to varying degrees, which will affect the accuracy of the control module.

[0115] To solve the above problems, the disclosed embodiment proposes a two-step registration method: scan-to-submap registration and scan-to-global registration. The point cloud data collected in the current time period has a high degree of matching with the scene, and will not cause posture jumps when used for registration. Therefore, the disclosed embodiment selects some key frames in the historical frames of the current time period according to certain rules to form a submap, and then matches the current frame with the submap to obtain the posture of the current frame. However, there will inevitably be errors in each match, and long-term operation will inevitably lead to large cumulative errors in the submap. Scan-to-submap registration mainly performs the registration of the current frame with the global map. Although the posture obtained by registration has certain jumps, there is no cumulative error in the posture, and a certain degree of accuracy is guaranteed. It can be regarded as an absolute observation of the posture of the current frame, so this posture can be used to fuse the posture obtained by scan-to-submap, and then the fused posture can be used to eliminate the cumulative error in the submap.

[0116] For further information, see Figure 5 ,When performing scan-to-submap registration, the current point cloud frame is taken as the first source point cloud, the sub-image is taken as the first target point cloud, and the NDT registration method is used to align the first source point cloud with the first target point cloud, so as to obtain the first pose of the current point cloud frame.

[0117] Sub-image management: after obtaining the first pose of the current point cloud frame, the sub-image will be updated, the relative position between the first pose of the current point cloud and the latest key frame will be calculated, and the relative distance and relative rotation angle will be calculated according to the relative pose. If the threshold condition is met, the current frame is considered to be a new key frame, otherwise it will be skipped. Determine whether the re-localization is successful. If successful, add observations and perform the second registration, that is, scan-to-globalmap registration.

[0118] In the scan-to-globalmap registration, the current point cloud frame is used as the source point cloud and the global map is used as the target point cloud. The NDT registration method is used to align the second source point cloud with the second target point cloud to obtain the absolute pose of the current point cloud frame (i.e., the second pose) and then complete the registration.

[0119] In an optional embodiment, S22, performing a second registration based on the relative pose between the pose of the current point cloud frame and the odometer pose of the vehicle when performing the latest observation, and the statistical parameters, to obtain a second registration result, including:

[0120] The global subgraph composed of the point clouds at the current position of the vehicle and the adjacent area is used as the second target point cloud, and the current point cloud frame is used as the second source point cloud. The second target point cloud and the second source point cloud are aligned to obtain the second pose of the current point cloud frame.

[0121] The embodiment of the present disclosure believes that the accumulated error in the submap will not be too large in a short time. Therefore, in order to reduce the computing cost and increase the program running efficiency, the embodiment of the present disclosure will appropriately reduce the execution frequency of the scan-to-globalmap.

[0122] The judgment criteria for performing registration are the same as those for sub-image update, and are also judged based on distance and angle, except that the pose of the latest keyframe is replaced by the pose of the last scan-to-globalmap execution.

[0123] That is, when performing scan-to-globalmap registration, the current point cloud frame is taken as the second source point cloud, the sub-image is taken as the second target point cloud, and the NDT registration method is used to align the second source point cloud with the second target point cloud to obtain the second pose of the current point cloud frame.

[0124] After obtaining the pose of the current point cloud frame, the sub-image is updated, and the relative pose between the second pose of the current point cloud and the latest key frame is calculated. The relative distance and relative rotation angle are calculated according to the relative pose. If the threshold condition is met, the current frame is considered to be a new key frame, otherwise it is skipped.

[0125] In an optional embodiment, after performing the first step of registration on the currently acquired point cloud data to obtain a first registration result, wherein the first registration result includes the first pose of the current point cloud frame, and before performing the second step of registration, the method further includes updating the sub-image:

[0126] Get the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose.

[0127] The sub-image is updated based on the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose.

[0128] In an optional embodiment, obtaining the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose includes:

[0129] Get the latest keyframe pose, the current frame pose, and the rotation part of the current frame pose.

[0130] Based on the latest key frame pose, the current frame pose and the rotation part of the current frame pose, the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose are obtained.

[0131] In an optional embodiment, the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose are obtained by the following formula.

[0132]

[0133] θ=[RotMatToEulAng(r rel )]3

[0134] Where: Indicates t rel The square of the i-th component in ; RotMatToEulAng(·) means converting the rotation matrix into Euler angles; [·]3 means taking the third component in the vector.

[0135] Furthermore, if d>σ d Or θ>σ θ , then the current frame is determined to be a new key frame. Generally, σ d is 0.5m, σ θ is 1°.

[0136] Furthermore, for the update of submap, the embodiment of the present disclosure adopts a sliding window algorithm for update: a certain number of key frames are maintained in the window, and when a new key frame is added to the window, if the number of key frames in the window exceeds the set threshold, the key frame farthest from the current time is deleted, and the remaining key frames form a new submap. The embodiment of the present disclosure sets the window size to 20.

[0137] In an optional embodiment, the method provided by the embodiment of the present disclosure further includes determining the rough position of the vehicle in the global map at the current moment, and then determining the grid area where the vehicle is located. The embodiment of the present disclosure adopts the more mature ScanContext solution. The principle of ScanContext is prior art, and reference may be made to relevant papers, and the embodiment of the present disclosure will not be repeated one by one.

[0138] In an alternative embodiment, see Figure 6 , S4, updating the accumulated error in the odometer sub-map based on the optimized point cloud frame pose and switching the sub-map in the global map, including:

[0139] S41, obtaining the odometer posture, and obtaining the change amount of the current point cloud frame posture based on the odometer posture and the optimized point cloud posture;

[0140] S42: based on the change in the current point cloud frame pose, update the accumulated error in the odometer sub-map and switch the sub-map in the global map.

[0141] In an optional embodiment, updating the accumulated error in the odometer sub-map and switching the sub-map in the global map based on the change in the current point cloud frame pose includes:

[0142] The index corresponding to the point cloud frame is obtained based on the optimized point cloud frame pose. If the index changes, the position corresponding to the current index and the position adjacent to the position corresponding to the current index are updated.

[0143] For further information, see Figure 7 , first construct the pose graph: take the odometer pose of the current point cloud frame as the node, that is, the first pose provided by the embodiment of the present disclosure to construct the pose graph. If the current point cloud frame adds absolute observation, that is, the second step of registration is performed, and the absolute pose, that is, the second pose, is obtained, then the second pose is added to the pose graph to optimize the pose graph. Then, the global map and sub-graph are updated through the optimized second pose of the current frame.

[0144] It should be noted that if the positioning process lasts for a long time, more point cloud frame nodes will be inserted into the pose graph. If all nodes are optimized, a long optimization time will be required, which will seriously affect the execution efficiency of the program. To solve the above problem, the embodiment of the present disclosure believes that the pose after fusion optimization has higher accuracy and does not need to participate in the subsequent optimization. Therefore, after the fusion optimization is completed, the pose of the current frame can be fixed to reduce the number of nodes involved in the subsequent optimization, reduce the optimization time, and improve the program running efficiency.

[0145] In an optional embodiment, the odometer pose is obtained, and the change in the current point cloud frame pose is obtained based on the odometer pose and the optimized point cloud frame pose.

[0146] Furthermore, the change in the current point cloud frame pose is obtained by the following formula:

[0147]

[0148] Where: is the inverse of the odometer posture; T opt is the optimized point cloud frame pose; ΔT cor is the change in the current frame pose.

[0149] Then, for the next odometer pose, you need to add the correction amount according to the following formula and then output it:

[0150] T odo =T odo ×ΔT cor

[0151] In an optional embodiment, S4, updating the accumulated error in the odometer sub-map based on the optimized point cloud frame pose and switching the sub-map in the global map, and performing positioning based on the updated sub-map in the global map, further includes:

[0152] According to the optimized point cloud frame pose, the grid index value corresponding to the optimized point cloud frame pose is calculated. If the index value changes, the current grid and its neighborhood grids are updated.

[0153] For a raster that is beyond the neighborhood range, if the absolute value of the difference between its index value and the current raster index value is greater than the threshold, the current raster will be deleted; otherwise, the current raster will be retained.

[0154] Furthermore, the global map is updated: according to the optimized posture, the corresponding grid index value is calculated. If the index value changes, the current grid and its neighboring grids are updated, such as Figure 8 shown.

[0155] When the vehicle track swings on the boundary line between two adjacent grids, if the above update strategy is followed, the current grid and its neighboring grids will be continuously updated, increasing the frequency of writing from memory to hard disk. To solve the above problem, the disclosed embodiment optimizes the grid update strategy: if the index value changes, a new grid and its neighboring grids are loaded. For grids beyond the neighborhood range, if the absolute value of the difference between its index value and the current grid index value is greater than a threshold (generally the threshold is 2), it is deleted. Otherwise, the current grid is temporarily retained in memory, such as Fig. 9 shown.

[0156] In large-scale scenarios at the city level, there are many engineering problems when unmanned vehicles use lidar positioning. For example, the capacity of the on-board storage unit is limited, making it difficult to load all global maps used for positioning into the memory; the initialization process of various registration algorithms is time-consuming, making it difficult to ensure real-time operation of the system.

[0157] To solve the above problems, the embodiment of the present disclosure proposes a new map form: NDT-Mapping, which is a nested voxel data structure. First, to solve the problem of limited capacity of the on-board storage unit, the embodiment of the present disclosure adopts a method of dynamically loading maps (i.e., technology 2 in the background technology): in the XOY plane, the space is divided into many grids of custom sizes, and each point cloud in the global map is traversed according to the location information of the point in the map and the size of the grid, and it is projected into the corresponding grid. When positioning, only the grid where the vehicle is located and the grids in its neighborhood are loaded into the memory. Secondly, to solve the problem that the initialization process of the registration algorithm is time-consuming, the embodiment of the present disclosure, based on the principle of the adopted NDT registration algorithm, voxelizes the space contained in the above-mentioned divided grid, traverses all points belonging to the current grid, and projects the point cloud into the corresponding voxel according to its location information and the size of the voxel, and calculates the expected and covariance statistics of the point distribution in the current voxel according to the recursive formula.

[0158] The nested voxel data structure proposed above can be implemented using nested unordered_map (an associative container in C language, which is a prior art). That is, for the first-level unordered_map structure, the grid index is used as the key and the new unordered_map structure is used as the value; for the second-level unordered_map structure, the index of the voxel contained in the current grid is used as the key and the custom structure is used as the value, where the structure contains the statistics required for NDT algorithm registration. The following is the entire process of voxel projection:

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

[0160] Get the coordinate value of the point that falls in the current space voxel in the preset coordinate system.

[0161] 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;

[0162] 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.

[0163] 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.

[0164] 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.

[0165] First, traverse each point p in the global point cloud map and calculate its index in the grid according to the following formula:

[0166]

[0167] Where: px, py represent the coordinate values ​​of point p in the world coordinate system; i 1x 、i 1y They represent the index values ​​of the grid where point p is located; r1 represents the size of the custom grid; Represents the floor function.

[0168] Will (i 1x ,i 1y ) as the key value and search it in the first-level unordered_map structure. If the key value does not exist in the structure, a new key-value pair is inserted into the unordered_map structure and the inserted value is returned; if the key value exists in the structure, the found value is returned directly.

[0169] Secondly, calculate the index of the current point in the voxel according to the following formula:

[0170]

[0171] Where: i 2x 、i 2y 、i 2z Respectively represent the index value of the voxel where point p is located; r2 represents the size of the custom voxel; then (i 2x ,i 2y ,i 2z ) as the key and search in the value returned in the previous step. If the key does not exist in the structure, insert a new key-value pair into the current unordered_map and return the inserted value; if the key value exists in the structure, return the found value directly.

[0172] However, considering the principle of the NDT registration algorithm, the structure corresponding to each voxel should contain statistics such as the number of points n, expected E and covariance C. In order to improve the conversion efficiency of point cloud map to NDT-Mapping, in an optional embodiment, S1, the offline point cloud data is voxel-converted, and the converted point cloud data includes statistical parameters of the point cloud distribution in the current voxel, including:

[0173] The offline point cloud data is converted to voxels using the following formula:

[0174]

[0175] Among them, p n Indicates the coordinate value of the nth point falling in the current voxel; E n-1 represents the expected value of the midpoint of the current voxel before updating; E n Represents the expected value of the midpoint of the current voxel after update;

[0176]

[0177] Where: C(n-1) represents the covariance of the midpoint of the current voxel before updating, and Cn represents the covariance of the midpoint of the current voxel after updating;

[0178] Among them, statistical parameters include covariance and expected value.

[0179] The above formula is used to improve the efficiency of voxel conversion of offline point cloud data.

[0180] In an optional embodiment, the method provided by the embodiment of the present disclosure also includes preprocessing of point cloud data.

[0181] In an alternative embodiment, see Fig. 9 ,The preprocessing of point cloud data mainly includes three aspects: ,Data cleaning: traverse each point cloud in the current point cloud frame, ,if the position data of the point cloud is a NAN value, the current point cloud is ,removed; If the distance value from the point to the radar is ,less than the lower limit of distance, or greater than the upper limit of distance, the current point cloud is ,removed.

[0182] Distortion removal: Based on the timestamp information of each point in the point cloud frame, that is, the prediction of the current frame pose based on the pose of the historical frame, the points are unified to the same moment to achieve the purpose of removing motion distortion.

[0183] Downsampling: Perform adaptive voxel filtering on the current point cloud frame to reduce the number of points and improve the registration efficiency.

[0184] In an optional embodiment, preprocessing the point cloud frame data inputted by the target object includes: obtaining the distance from a point in the point cloud frame to the center of the target object;

[0185] Sort the distances from all 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;

[0186] 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.

[0187] Furthermore, the distance from the point in the point cloud frame to the center of the target object is obtained, including: traversing each point p in the point cloud, and calculating the distance from the point p to the center of the radar according to the following formula:

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

[0189] 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.

[0190] 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.

[0191] 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;

[0192] 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.

[0193] 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.

[0194] 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.

[0195] In an optional embodiment, preprocessing the point cloud frame data further includes:

[0196] Get the coordinate value of a point in the current point cloud frame.

[0197] Get the relative pose of a vehicle based on a point in the current point cloud frame.

[0198] Get the inverse of the relative pose of the vehicle based on the last point in the point cloud of the current frame.

[0199] The coordinate value of the point after distortion removal is obtained 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.

[0200] 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.

[0201] 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.

[0202] 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:

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

[0204] 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:

[0205]

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

[0207]

[0208] Where: p iIndicates 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.

[0209] 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 the points in the current voxel.

[0210] Figures 11 to 12 It is a schematic block diagram of the structure of a radar positioning device using a hardware implementation of a processor according to an embodiment of the present disclosure.

[0211] The radar positioning 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 modules in these modules. The module may be one or more hardware modules specially 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.

[0212] The hardware structure of the radar positioning 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.

[0213] 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.

[0214] 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).

[0215] 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.

[0216] 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.

[0217] 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.

[0218] 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.

[0219] 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.

[0220] See also Fig.11 , a radar positioning device 100, comprising:

[0221] The conversion unit 101 is used to perform voxel conversion on the offline point cloud data, and the converted point cloud data includes statistical parameters of the point cloud distribution in the current voxel.

[0222] The registration unit 102 is used to perform at least one step of registration on the currently acquired point cloud data to obtain a final registration result, which includes the position and posture of the current point cloud frame.

[0223] The optimization unit 103 is used to add the pose of the current point cloud frame to the initial pose graph based on the final registration result to perform pose optimization.

[0224] The positioning unit 104 is used to update the accumulated error in the odometer sub-map based on the optimized point cloud frame pose and switch the sub-map in the global map, and perform positioning based on the sub-map in the updated global map.

[0225] See also Fig.10 The registration unit 102 includes: a first registration unit 1021 and a second registration unit 1022 .

[0226] The first registration unit 1021 is used to perform a first step of registration on the currently acquired point cloud data to obtain a first registration result, wherein the first registration result includes a first position of the current point cloud frame.

[0227] The second registration unit 1022 is used to perform a second-step registration based on the relative pose between the first pose of the current point cloud frame and the odometer pose of the vehicle when performing the latest observation, as well as statistical parameters, to obtain a second registration result, and use the second registration result as the final registration result.

[0228] The first registration unit 1021 is used to use the sub-image formed by the historical key frames selected in the current time period as the first target point cloud and the current point cloud frame as the first source point cloud.

[0229] The first target point cloud and the first source point cloud are aligned to obtain the first position of the current point cloud frame.

[0230] The second registration unit 1022 is used to align the second target point cloud and the second source point cloud with the global sub-graph composed of the point cloud in the current position of the vehicle and the adjacent area as the second target point cloud and the current point cloud frame as the second source point cloud to obtain the second pose of the current point cloud frame.

[0231] In an optional embodiment, the device provided by the embodiment of the present disclosure also includes a sub-image updating unit, which is used to update the sub-image and obtain the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose.

[0232] The sub-image is updated based on the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose.

[0233] In an optional embodiment, the sub-image updating unit is used to obtain the latest key frame pose, the current frame pose and the rotation part of the current frame pose.

[0234] Based on the latest key frame pose, the current frame pose and the rotation part of the current frame pose, the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose are obtained.

[0235] In an optional embodiment, the sub-image updating unit is used to obtain the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose by the following formula:

[0236]

[0237] θ=[RotMatToEulAng(r rel )]3

[0238] Where: Indicates t rel The square of the i-th component in ; RotMatToEulAng(·) means converting the rotation matrix into Euler angles; [·]3 means taking the third component in the vector.

[0239] In an optional embodiment, the positioning unit 104 includes:

[0240] The first acquisition unit is used to acquire the odometer posture, and obtain the change amount of the current point cloud frame posture based on the odometer posture and the optimized point cloud posture.

[0241] The second acquisition unit is used to update the accumulated error in the odometer sub-map and switch the sub-map in the global map based on the change in the current point cloud frame pose.

[0242] The second acquisition unit is used to acquire the index corresponding to the point cloud frame based on the optimized point cloud frame posture, and if the index changes, update the position corresponding to the current index and the position adjacent to the position corresponding to the current index.

[0243] The second acquisition unit is used to obtain the grid index value corresponding to the optimized point cloud frame pose according to the optimized point cloud frame pose, and if the index value changes, update the current grid and its neighboring grids.

[0244] For the grid beyond the neighborhood range, if the absolute value of the difference between the grid index value beyond the neighborhood range and the current grid index value is greater than the threshold, the current grid will be deleted; otherwise, the current grid will be retained.

[0245] The conversion unit 101 is used to perform voxel conversion on the offline point cloud data using the following formula:

[0246]

[0247] Among them, p n Indicates the coordinate value of the nth point falling in the current voxel; E n-1 represents the expected value of the midpoint of the current voxel before updating; E n Represents the expected value of the midpoint of the current voxel after update;

[0248]

[0249] Where: C(n-1) represents the covariance of the midpoint of the current voxel before updating, and Cn represents the covariance of the midpoint of the current voxel after updating;

[0250] Among them, statistical parameters include covariance and expected value.

[0251] 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.

[0252] 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.

[0253] 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 positioning method, characterized in that: include: Perform voxel conversion on the offline point cloud data, and the converted point cloud data includes statistical parameters of the point cloud distribution in the current voxel; Perform at least one step of registration on the currently acquired point cloud data to obtain a final registration result, wherein the final registration result includes the pose of the current point cloud frame; Inserting the pose of the current point cloud frame into the initial pose graph based on the final registration result to perform pose optimization; as well as Based on the optimized point cloud frame pose, the accumulated error in the odometer sub-map is updated and the sub-map in the global map is switched, and positioning is performed based on the updated sub-map in the global map; The step of performing at least one step of registration on the currently acquired point cloud data to obtain a final registration result includes: Performing a first step of registration on the currently acquired point cloud data to obtain a first registration result, wherein the first registration result includes a first position of the current point cloud frame; Performing a second-step registration based on the relative pose between the first pose of the current point cloud frame and the odometer pose of the vehicle when performing the latest observation, and the statistical parameters, to obtain a second registration result, and using the second registration result as the final registration result; The step of performing a first registration on the currently acquired point cloud data to obtain a first registration result includes: Taking the sub-image formed by the historical key frames selected in the current period as the first target point cloud and the current point cloud frame as the first source point cloud; and Performing an alignment operation on the first target point cloud and the first source point cloud to obtain the first pose of the current point cloud frame to achieve the first step of registration; The second registration is performed based on the relative pose between the first pose of the current point cloud frame and the odometer pose of the vehicle when performing the latest observation, and the statistical parameters to obtain the second registration result, including: Taking the global subgraph composed of the point cloud of the vehicle's current position and the adjacent area as the second target point cloud, taking the current point cloud frame as the second source point cloud, aligning the second target point cloud and the second source point cloud to obtain the second pose of the current point cloud frame to achieve the second-step registration; Wherein, the first step of registration is scan-to-submap registration, and the second step of registration is scan-to-globalmap registration; Wherein, updating the accumulated error in the odometer sub-map based on the optimized point cloud frame pose and switching the sub-map in the global map include: Obtain the odometer pose, obtain the change in the current point cloud frame pose based on the odometer pose and the optimized point cloud frame pose; and update the accumulated error in the odometer subgraph and switch the subgraph in the global map based on the change in the current point cloud frame pose.

2. The radar positioning method according to claim 1, characterized in that: After performing the first step of registration on the currently acquired point cloud data to obtain the first registration result, and before performing the second step of registration, the method further includes updating the sub-image formed by the historical key frame: Obtaining the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose; and The sub-graph formed by the historical key frames is updated based on the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose.

3. The radar positioning method according to claim 2, characterized in that: Get the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose, including: Get the latest keyframe pose, the current frame pose, and the rotation part of the current frame pose; and Based on the latest key frame pose, the current frame pose and the rotation part of the current frame pose, the relative distance and relative rotation angle between the latest key point cloud frame pose and the current point cloud frame pose are obtained.

4. The radar positioning method according to claim 1, characterized in that: Based on the optimized point cloud frame pose, the accumulated error in the odometer sub-map is updated and the sub-map in the global map is switched, including: Obtaining an odometer pose, and obtaining a change in a current point cloud frame pose based on the odometer pose and the optimized point cloud pose; and An index value corresponding to the point cloud frame is obtained based on the optimized point cloud frame pose. If the index value changes, a position corresponding to the current index value and a position adjacent to the position corresponding to the current index value are updated.

5. The radar positioning method according to claim 4, characterized in that: Based on the optimized point cloud frame pose, the accumulated error in the odometer sub-map is updated and the sub-map in the global map is switched, and positioning is performed based on the sub-map in the updated global map, which also includes: According to the optimized point cloud frame pose, obtaining a grid index value corresponding to the optimized point cloud frame pose, and if the index value changes, updating the current grid and its neighboring grids; and For the grid beyond the neighborhood range, if the absolute value of the difference between the grid index value beyond the neighborhood range and the current grid index value is greater than the threshold, the current grid will be deleted; otherwise, the current grid will be retained.

6. A radar positioning device, characterized in that: include: A conversion unit, used for performing voxel conversion on offline point cloud data, wherein the converted point cloud data includes statistical parameters of point cloud distribution in the current voxel; A registration unit, used to perform at least one step of registration on the currently acquired point cloud data to obtain a final registration result, wherein the final registration result includes the pose of the current point cloud frame; An optimization unit, configured to insert the pose of the current point cloud frame into the initial pose graph based on the final registration result to perform pose optimization; as well as A positioning unit, used to update the accumulated error in the odometer sub-map based on the optimized point cloud frame pose and switch the sub-map in the global map, and perform positioning based on the updated sub-map in the global map; The step of performing at least one step of registration on the currently acquired point cloud data to obtain a final registration result includes: Performing a first step of registration on the currently acquired point cloud data to obtain a first registration result, wherein the first registration result includes a first position of the current point cloud frame; Performing a second-step registration based on the relative pose between the first pose of the current point cloud frame and the odometer pose of the vehicle when performing the latest observation, and the statistical parameters, to obtain a second registration result, and using the second registration result as the final registration result; The step of performing a first registration on the currently acquired point cloud data to obtain a first registration result includes: Taking the sub-image formed by the historical key frames selected in the current period as the first target point cloud and the current point cloud frame as the first source point cloud; and Performing an alignment operation on the first target point cloud and the first source point cloud to obtain the first pose of the current point cloud frame to achieve the first step of registration; The second registration is performed based on the relative pose between the first pose of the current point cloud frame and the odometer pose of the vehicle when performing the latest observation, and the statistical parameters to obtain the second registration result, including: Taking the global subgraph composed of the point cloud of the vehicle's current position and the adjacent area as the second target point cloud, taking the current point cloud frame as the second source point cloud, aligning the second target point cloud and the second source point cloud to obtain the second pose of the current point cloud frame to achieve the second-step registration; Wherein, the first step of registration is scan-to-submap registration, and the second step of registration is scan-to-globalmap registration; Wherein, the updating of the accumulated error in the odometer sub-graph based on the optimized point cloud frame pose and the switching of the sub-graph in the global map include: Obtain the odometer pose, obtain the change in the current point cloud frame pose based on the odometer pose and the optimized point cloud frame pose; and update the accumulated error in the odometer subgraph and switch the subgraph in the global map based on the change in the current point cloud frame pose.

7. 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 positioning method according to any one of claims 1 to 5.

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

Images