Grid map updating method, device, electronic device and storage medium

Abstract

The present disclosure provides a grid map updating method based on posture optimization, including: performing time alignment on the point cloud data of the vehicle's field of view scene acquired by the ranging sensor and the vehicle's posture data acquired by the IMU to at least acquire the vehicle's posture of the current frame, and removing the point cloud data distortion caused by the vehicle's motion; performing point cloud data segmentation to acquire ground point cloud and non-ground point cloud; performing a first optimization process on the vehicle's posture of the current frame; performing a second optimization process on the vehicle's posture of the current frame after the first optimization process; performing time alignment and fusion process on the optimized vehicle posture of the current frame and the vehicle's posture data acquired by the IMU; based on the vehicle posture after the fusion process, using the ground point cloud and the non-ground point cloud to update the grid map. The present disclosure also provides a grid map updating device, an electronic device, and a readable storage medium.

Description

Technical Field

[0001] The present disclosure relates to autonomous driving technology and robotics technology, and in particular to a grid map updating method, device, electronic device and storage medium based on posture optimization. Background Art

[0002] With the development of autonomous driving and autonomous mobile robot technology and the creation of new application scenarios, new requirements have been put forward for the original technology.

[0003] As the range of unmanned vehicles increases and their speed increases, vehicles such as cars must have the ability to perform real-time positioning and large-scale mapping for SLAM (Simultaneous Localization and Mapping) technology, such as the following existing technical solutions:

[0004] Prior art 1: "DLO: Direct LiDAR Odometry for 2.5D Outdoor Environment" published an inter-frame matching technology based on 2.5D grid maps. This technology is inspired by the direct optical flow method of vision, assuming that ground points at the same location have the same elevation value at different times, and borrowing the idea of ​​probabilistic grid maps to model the scene. However, the difference is that the grid no longer stores the probability value of whether it is occupied, but stores the elevation value of the ground point corresponding to its position, and constructs a least squares model at the cost of the difference between the elevation value of the current frame and the elevation value at the same position in the historical frame, and optimizes the six-dimensional posture of the vehicle. However, this technology does not realize that the six degrees of freedom of the vehicle have different influences on the elevation value in the grid, and the six degrees of freedom are not split. The optimization variables have a higher dimension, which will increase the number of iterations of the optimization algorithm and affect the efficiency of the program. Secondly, this technology does not eliminate the influence of dynamic obstacles. Finally, this technology does not solve the problem of grid map storage efficiency that may appear in large-scale scenes.

[0005] Existing technology 2: "A Universal Grid Map Library: Implementation and Use Case for Rough Terrain Navigation" published a technology for building a 2.5D grid map. When the map moves, the technology can quickly update the internal data. At the same time, the technology can provide multiple data layers and store different types of data in the map. Finally, the technology provides a compatible interface with multiple existing data types to achieve fast conversion of data types. However, this technology does not solve the problem of grid map storage efficiency in large-scale scenarios.

[0006] The front-end frame matching algorithms in current 3D laser SLAM can be divided into two categories: 1. Based on point sets, such as classic algorithms such as ICP and NDT and their variants; 2. Based on feature points, such as line and surface features in LOAM. Academically, various algorithms have been studied in depth, and a fairly mature understanding of the characteristics and advantages of the algorithms has been formed. However, in engineering applications, each solution will more or less expose certain problems.

[0007] For example, the ICP algorithm, the ICP registration process is to iteratively find the optimal transformation matrix between two frames of point clouds at the cost of the distance between the corresponding point pairs of two adjacent frames of point clouds. First, in order to determine the correspondence between the points in the two frames of point clouds, each point needs to be processed cyclically, which will lead to a high computational cost; secondly, the algorithm is greatly affected by noise and outliers, and poor data processing can easily lead to the algorithm failing to converge or falling into a local optimum; finally, the algorithm strongly depends on the initial value of the pose. If the initial value is not good enough, it is also easy to cause the optimization to fall into a local optimum.

[0008] In order to avoid matching a large number of points in ICP and reduce computational costs, feature point-based registration methods (by extracting representative points in the current point cloud and then modeling the correspondence between feature points in two frame point clouds) are becoming more and more popular. However, the extraction of feature points itself will take a certain amount of time. In addition, it is difficult to extract suitable feature points in places where the scene is relatively simple, which leads to problems such as scene degradation. Finally, due to the change of viewpoint, the mismatch between feature points will also have a great impact on pose optimization. Summary of the invention

[0009] In order to solve at least one of the above technical problems, the present disclosure provides a grid map updating method, device, electronic device and storage medium based on posture optimization.

[0010] According to one aspect of the present disclosure, a grid map updating method based on posture optimization is provided, comprising:

[0011] S110, performing time alignment on the point cloud data of the vehicle's field of view scene acquired by the ranging sensor and the vehicle's posture data acquired by the IMU to at least acquire the vehicle's posture in the current frame, and removing point cloud data distortion caused by vehicle motion;

[0012] S120, dividing the point cloud data of the current frame after time alignment and point cloud data distortion removal into ground point cloud and non-ground point cloud;

[0013] S130, performing a first optimization process on the vehicle posture of the current frame based on the ground point cloud data to obtain a vehicle posture after the first optimization process;

[0014] S140, performing a second optimization process based on the non-ground point cloud on the vehicle posture of the current frame after the first optimization process, so as to obtain a vehicle posture after the second optimization process;

[0015] S150, time-aligning and fusing the optimized vehicle posture of the current frame with the vehicle posture data acquired by the IMU, and outputting the fused vehicle posture of the current frame;

[0016] S160: Based on the fused vehicle posture, use the ground point cloud and the non-ground point cloud to update the grid map.

[0017] According to the grid map updating method based on posture optimization of at least one embodiment of the present disclosure, in step S110, time alignment is performed to obtain at least the vehicle posture of the current frame, including: S1102, based on the posture data of the vehicle obtained by the IMU, the posture data of the vehicle is obtained when the ranging sensor obtains the point cloud data of the current frame.

[0018] According to at least one embodiment of the present disclosure, in the grid map updating method based on posture optimization, in step S110, the point cloud data distortion caused by the vehicle motion is removed, including:

[0019] S1104, convert the current frame point cloud data to the vehicle coordinate system, obtain the IMU pose corresponding to each point by difference based on the timestamp information of each point in the current frame point cloud data, and convert the IMU pose corresponding to each point to the IMU pose corresponding to the initial point in the current frame point cloud data.

[0020] According to the grid map updating method based on posture optimization of at least one embodiment of the present disclosure, S120, the point cloud data of the current frame that has been time aligned and has point cloud data distortion removed is segmented into ground points and non-ground points, including: based on the height value of each point in the point cloud, the point cloud data of the current frame is segmented into ground points and non-ground points.

[0021] According to at least one embodiment of the present disclosure, in the grid map updating method based on posture optimization, S130, a first optimization process is performed on the vehicle posture of the current frame based on the ground point cloud data to obtain the vehicle posture after the first optimization process, including:

[0022] S1302, performing voxel filtering on the ground point cloud data of the current frame;

[0023] S1304, projecting the ground point cloud of the current frame after filtering into the grid map, constructing a first cost function based on the difference between the height value of each point projected to the target grid in the point cloud and the elevation value of the target grid and the probability value of the height value of each point in the target grid;

[0024] S1306: Iteratively optimize the first cost function to obtain an optimized vehicle posture.

[0025] According to at least one embodiment of the present disclosure, in the grid map updating method based on posture optimization, S1304, the ground point cloud of the current frame after filtering is projected into the grid map, and a first cost function is constructed based on the difference between the height value of each point projected to the target grid in the point cloud and the elevation value of the target grid and the probability value of the height value of each point in the target grid, including:

[0026] The first cost function is constructed by using the difference between the ground elevation value acquired by the ranging sensor at the current frame time and the ground elevation value acquired at least one frame time before the current frame time as a cost value.

[0027] According to the grid map updating method based on posture optimization of at least one embodiment of the present disclosure, the first cost function is a least squares function.

[0028] According to at least one embodiment of the present disclosure, in the grid map updating method based on posture optimization, S140, a second optimization process based on a non-ground point cloud is performed on the vehicle posture of the current frame after the first optimization process to obtain the vehicle posture after the second optimization process, including:

[0029] S1402, performing voxel filtering on the non-ground point cloud data of the current frame;

[0030] S1404, projecting the non-ground point cloud of the current frame after filtering onto the grid map, obtaining the elevation feature value in each grid, and constructing a second cost function based on the difference between the elevation feature value in each grid of the current frame and the elevation feature value in each grid of the frame before the current frame;

[0031] S1406. Solve the second cost function based on the CSM algorithm to obtain an optimized vehicle posture.

[0032] According to the grid map updating method based on posture optimization of at least one embodiment of the present disclosure, the elevation characteristic values ​​include a maximum elevation value, a median elevation value, and a minimum elevation value.

[0033] According to at least one embodiment of the present disclosure, in a grid map updating method based on posture optimization, S150, the optimized vehicle posture of the current frame is time-aligned with the posture data of the vehicle acquired by the IMU and a fusion process is performed, and the fused vehicle posture of the current frame is output, including:

[0034] S1502, performing time alignment processing on the optimized vehicle posture of the current frame based on the timestamp of the IMU sensor;

[0035] S1504: Taking the currently optimized vehicle posture as a reference, integrate the posture acquired by the subsequent IMU sensor to obtain the fused vehicle posture of the current frame.

[0036] According to at least one embodiment of the present disclosure, in the grid map updating method based on posture optimization, in S160, based on the vehicle posture after the fusion processing, the grid map is updated using the ground point cloud, including:

[0037] A. If the elevation value in the grid corresponding to the point cloud is NAN, NAN means that no elevation value is stored, then the elevation value of the current point is stored in the grid;

[0038] B. When the grid corresponding to the point cloud already has elevation values ​​stored and the grid has only been updated once, if the difference between the elevation value of the current point and the elevation value stored in the grid is greater than the difference threshold, the current grid elevation value is set to NAN; if the difference is less than the difference threshold, the current value and the existing value are used to update the probability distribution of the elevation value of the current grid;

[0039] C. When the grid corresponding to the point cloud already has elevation values ​​stored and the number of updates is greater than 2, if the difference between the elevation value of the current point and the elevation value stored in the grid is greater than the difference threshold, the current point is considered to be an abnormal point and is directly discarded; if the difference is less than the difference threshold, the current value and the existing value are used to update the probability distribution of the elevation value of the current grid.

[0040] According to at least one embodiment of the present disclosure, in the grid map updating method based on posture optimization, in S160, based on the vehicle posture after the fusion processing, the grid map is updated using the non-ground point cloud, including:

[0041] a. Global settings for the grid map: use a quadtree data structure to store data in the grid map, specify the depth of the tree, specify the size of each grid in the grid map, and set the size of the scene to be modeled by the grid map;

[0042] b. Project the current frame point cloud to the grid map: Calculate the index of the corresponding grid according to the coordinates of each point, and recursively search the child node corresponding to the current grid in the quadtree until a leaf node is found;

[0043] c. Update the statistics stored in the current grid based on the newly added points in the grid.

[0044] According to another aspect of the present disclosure, a grid map updating device based on posture optimization is provided, comprising:

[0045] A preprocessing module, which performs time alignment on the point cloud data of the vehicle's field of view scene acquired by the ranging sensor and the vehicle's posture data acquired by the IMU to at least acquire the vehicle's posture in the current frame and remove point cloud data distortion caused by vehicle motion;

[0046] A point cloud data segmentation module, which segments the point cloud data of the current frame after time alignment and point cloud data distortion removal into ground point cloud and non-ground point cloud;

[0047] A first optimization processing module, wherein the first optimization processing module performs a first optimization processing on the vehicle posture of the current frame based on the ground point cloud data to obtain the vehicle posture after the first optimization processing;

[0048] A second optimization processing module, wherein the second optimization processing module performs a second optimization processing based on a non-ground point cloud on the vehicle posture of the current frame after the first optimization processing, so as to obtain the vehicle posture after the second optimization processing;

[0049] A fusion processing module, which performs time alignment and fusion processing on the optimized vehicle posture of the current frame and the vehicle posture data acquired by the IMU, and outputs the fused vehicle posture of the current frame;

[0050] An update module is provided, wherein the update module updates a grid map based on the fused vehicle posture using the ground point cloud and the non-ground point cloud.

[0051] According to another aspect of the present disclosure, an electronic device is provided, comprising: a memory storing execution instructions; and a processor executing the execution instructions stored in the memory, so that the processor executes any one of the above-described grid map updating methods.

[0052] According to another aspect of the present disclosure, 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 grid map updating methods. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0054] Figure 1 It is a flowchart of a grid map updating method based on posture optimization according to an embodiment of the present invention.

[0055] Figure 2It is a schematic diagram of an embodiment of the present disclosure in which the laser point cloud data acquired at time k is in the middle part of two frames of IMU pose data.

[0056] Figure 3 It is a schematic diagram of point cloud distortion.

[0057] Figure 4 It is a schematic diagram of converting the IMU pose to the IMU pose corresponding to the first point in the point cloud.

[0058] Figure 5 A flowchart of performing time alignment and removing point cloud data distortion caused by vehicle motion in a grid map updating method based on pose optimization according to an embodiment of the present disclosure is shown.

[0059] Figure 6 This is a schematic diagram showing how a change in the angle θ around the x-axis will produce small errors on the y-axis and z-axis.

[0060] Figure 7 A schematic diagram of a process for preprocessing acquired point cloud data and IMU pose data according to an embodiment of the present disclosure is shown.

[0061] Figure 8 It is a flowchart of the first optimization process in a grid map updating method based on posture optimization according to an embodiment of the present invention.

[0062] Fig. 9 It is a flowchart of the second optimization process in the grid map updating method based on posture optimization according to an embodiment of the present invention.

[0063] Fig.10 It is a flowchart of a data fusion method in a grid map updating method based on posture optimization according to an embodiment of the present invention.

[0064] Fig.11 It is a detailed flowchart of a grid map updating method based on posture optimization according to an embodiment of the present invention.

[0065] Fig.12 It is a flowchart of updating raster maps using ground point clouds.

[0066] Fig.13 A flowchart of a method for updating a grid map using a non-ground point cloud according to the present embodiment is shown.

[0067] Fig.14 It is a schematic diagram of dividing a cube into four small cubes, and repeating this process until the size of the small cubes meets the modeling accuracy requirements.

[0068] Fig.15It is a structural schematic block diagram of a grid map updating device based on posture optimization using a hardware implementation of a processing system according to an embodiment of the present disclosure.

[0069] Description of Reference Numerals

[0070] 1000 Grid map update device

[0071] 1002 Preprocessing Module

[0072] 1004 Point cloud data segmentation module

[0073] 1006 First Optimization Processing Module

[0074] 1008 Second Optimization Processing Module

[0075] 1010 Fusion Processing Module

[0076] 1012 Update module

[0077] 1100 Bus

[0078] 1200 processor

[0079] 1300 Memory

[0080] 1400 Other circuits. DETAILED DESCRIPTION

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

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

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

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

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

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

[0087] Before describing the technical solution of the present disclosure in detail, the following explanations are given:

[0088] Frame: The data of the sensor is read in the form of frames, and each frame is a complete package of sensor data at a certain moment. The sending frequency and receiving frequency of the frame are determined by the data acquisition system used by vehicles and other vehicles. Coordinate system: All coordinate systems described in the technical solution of the present disclosure are preferably Cartesian rectangular coordinate systems. Projection: The point cloud involved in the technical solution of the present disclosure is a point in a three-dimensional space, and the grid map has a two-dimensional space. The process of calculating the point cloud and its corresponding grid index in the grid map is called projection. Pose: The pose described in the technical solution of 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. Voxel: The concept of voxel described in the technical solution of the present disclosure is a small space in a three-dimensional space. Grid: The grid described in the technical solution of the present disclosure is a small space in a two-dimensional space.

[0089] Combined with the following Figures 1 to 15 The grid map updating method and grid map updating device based on posture optimization disclosed in the present invention are described in detail.

[0090] Figure 1 is a flowchart of a grid map updating method based on posture optimization according to an embodiment of the present disclosure, Figure 1 According to an embodiment of the present disclosure, a grid map updating method S100 based on posture optimization includes:

[0091] S110, performing time alignment (time stamp alignment) on the point cloud data (preferably laser point cloud data) of the field of view scene of the vehicle (such as an autonomous driving vehicle) acquired by the ranging sensor and the pose data of the vehicle acquired by the IMU (the pose data is the pose data of the vehicle in the three-dimensional world coordinate system, including the position and pose of the vehicle in the 3D space, the position is the coordinates in the x, y and z directions, and the pose is the rotation angle around the x, y and z axes respectively), so as to at least acquire the pose of the vehicle in the current frame and remove the point cloud data distortion caused by the vehicle motion;

[0092] S120, dividing the point cloud data of the current frame after time alignment and point cloud data distortion removal into ground point cloud and non-ground point cloud;

[0093] S130, performing a first optimization process on the vehicle posture (first group of degrees of freedom) of the current frame based on the ground point cloud data to obtain the vehicle posture (first group of degrees of freedom (roll, pitch, z)) after the first optimization process;

[0094] S140, performing a second optimization process based on the non-ground point cloud on the vehicle posture of the current frame after the first optimization process, so as to obtain the vehicle posture after the second optimization process (the second optimization process optimizes the second group of degrees of freedom, and the second group of degrees of freedom includes x, y, and yaw);

[0095] S150, time-aligning and fusing the optimized vehicle posture of the current frame with the vehicle posture data acquired by the IMU, and outputting the fused vehicle posture of the current frame;

[0096] S160: Based on the fused vehicle posture, the grid map is updated using the ground point cloud and the non-ground point cloud.

[0097] In this disclosure, an autonomous vehicle or mobile robot is used as a vehicle, and the IMU (inertial sensor) installed on the vehicle provides a rough pose (pose estimation) of the vehicle, and the ranging sensor provides 3D point cloud information of the vehicle's surrounding space. The rough pose given by the IMU is used as the initial value, and the current pose of the vehicle is quickly optimized by optimizing the matching of the current frame point cloud and the map, giving a more accurate pose of the vehicle.

[0098] The vehicle described in this disclosure needs to be equipped with an IMU and a ranging sensor, where the ranging sensor can be a mechanical 3D lidar, a solid-state 3D lidar, a structured light camera, or any other sensor that can provide 3D point cloud information of the environmental space.

[0099] In step S110 of the grid map updating method S100 based on posture optimization according to the preferred embodiment of the present disclosure, time alignment (timestamp alignment) is performed to obtain at least the vehicle posture of the current frame, including:

[0100] S1102. Based on the posture data of the vehicle obtained by the IMU, the posture data of the vehicle at the moment when the ranging sensor obtains the current frame point cloud data is obtained (as described below, that is, the posture data of the vehicle at time k is obtained).

[0101] During the implementation of the technical solution disclosed in the present invention, two types of sensors will be used, one is the inertial measurement unit IMU, which is used to measure the position and posture of the vehicle; the other is a range sensor, such as a 3D laser radar, which is used to obtain the vehicle's field of view scene information (i.e., surrounding environment information). Since the two types of sensors collect data at different frequencies when working, for example, the sampling frequency of the IMU is 100Hz, while the sampling frequency of the laser radar is 10Hz, it is difficult to accurately describe the vehicle and its environment by directly using the raw data output by the two types of sensors.

[0102] like Figure 2 As shown in the figure, the laser point cloud data acquired at time k is in the middle part of the two frames of IMU pose data. Therefore, it is difficult to use the IMU pose data at time t or t+1 to describe the pose of the vehicle at time k.

[0103] Preferably, the present disclosure utilizes the IMU data of two adjacent moments before and after moment k, and adopts a linear interpolation method to approximately calculate the posture of the IMU at moment k (i.e., the posture of the vehicle at moment k) to achieve time alignment of the two types of sensor data.

[0104] Preferably, the position and posture of the IMU at time k is obtained based on the following formula:

[0105] p k =p t *(1-scale)+p t+1 *scale

[0106] Among them, p k represents the vehicle posture output by the IMU at time k, and scale represents the proportion of the time length from t to k in the time length from t to t+1, that is:

[0107]

[0108] Preferably, in step S110 of the grid map updating method S100 based on posture optimization of the present disclosure, removing point cloud data distortion caused by vehicle motion includes:

[0109] S1104, convert the point cloud data of the current frame to the vehicle coordinate system, obtain the IMU pose corresponding to each point by interpolation based on the timestamp information of each point in the point cloud data of the current frame, and convert the IMU pose corresponding to each point to the IMU pose corresponding to the initial point in the point cloud data of the current frame.

[0110] When the ranging sensor scans the surrounding environment, the vehicle will also move to a certain extent. Since the measurement data of the ranging sensor cannot be obtained instantly, the movement of the vehicle will have a significant impact on the sensor's ranging.

[0111] refer to Figure 3 If the operating frequency of the ranging sensor is 10Hz, the working cycle is 100ms. When the vehicle moves along the X direction at a speed of 1m / s and scans the target object in front, such as a wall, the measured distance at the start and end of a cycle will differ by 10cm in the X direction. Huge measurement errors are fatal to SLAM (simultaneous localization and mapping).

[0112] The technical solution disclosed in the present invention preferably removes motion distortion from the ranging result in the following manner: based on the timestamp information of each point in the point cloud, find its corresponding IMU pose by interpolation, and convert it (IMU pose) to the IMU pose corresponding to the first point in the point cloud, refer to Figure 4 .

[0113] Preferably, the conversion is performed by the following formula:

[0114]

[0115] Among them, p i is the i-th point in the point cloud of the current frame, T i Represents the IMU pose of the i-th point obtained based on interpolation; Represents the inverse of the IMU pose of the first point (the first point at the acquisition time) obtained based on interpolation in the current frame point cloud; p′ i Indicates the coordinate value of the point after the distortion is removed.

[0116] Figure 5 A flowchart of performing time alignment (time stamp alignment) and removing point cloud data distortion caused by vehicle motion in a grid map update method based on pose optimization according to an embodiment of the present disclosure is shown.

[0117] For the above-mentioned grid map updating method S100 based on posture optimization, preferably, S120, segmenting the point cloud data of the current frame after time alignment and point cloud data distortion removal into ground points and non-ground points, includes:

[0118] Based on the height value of each point in the point cloud, the point cloud data of the current frame is divided into ground points and non-ground points.

[0119] In the present disclosure, when optimizing the vehicle posture (6D), since the posture dimension is too high, the number of algorithm iterations increases, thereby increasing the convergence time and affecting the program execution efficiency. The present disclosure preferably adopts a dimensionality reduction approach to deal with high-dimensional optimization problems.

[0120] The applicant has found that in the process of projecting the point cloud onto the grid map, changes in the six degrees of freedom of the vehicle's posture will affect the elevation value in the grid after projection in different ways. Among them, slight changes in the roll, pitch, and z degrees of freedom will directly affect the elevation value after projection, while changes in the yaw, x, and y degrees of freedom will change the index value of the point cloud projected onto the grid, thereby indirectly affecting the elevation value in the grid. Figure 6 As shown in the figure, the change of the angle θ around the x-axis will produce small errors on the y-axis and the z-axis respectively. When constructing a grid map, if the grid size is appropriately increased, the current point in the point cloud will still fall into the same grid when projected into the grid, and the index value will not change. Therefore, it can not only eliminate the error in the horizontal direction, but also does not affect the error value in the Z direction.

[0121] In view of the above reasons, the present disclosure decides to segment the point cloud, where the ground points have continuity in the Z direction, and the least squares problem can be constructed at the cost of elevation difference. Therefore, the present disclosure uses the ground point cloud to optimize the roll, pitch, and z degrees of freedom in the vehicle posture, and uses non-ground points to optimize the yaw, x, and y degrees of freedom.

[0122] According to the preferred embodiment of the present disclosure, the present disclosure uses a height threshold to segment the point cloud to improve the segmentation efficiency. Before segmentation, the point cloud needs to be converted to the vehicle coordinate system and segmented according to the following rules:

[0123]

[0124] Among them, pz is the value of a point in the point cloud in the z direction, h is the threshold set when segmenting the point cloud, and after the point cloud is converted to the vehicle coordinate system, the value range of h is preferably 0.5 to 0.8.

[0125] Among them, in the Cartesian rectangular coordinate system, pitch is the rotation around the X-axis (pitch angle), yaw is the rotation around the Y-axis (yaw angle), and roll is the rotation around the Z-axis (roll angle).

[0126] Figure 7 A schematic diagram of a process for preprocessing acquired point cloud data and IMU pose data according to an embodiment of the present disclosure is shown.

[0127] Figure 8 1 is a flow chart of the first optimization process in the grid map updating method based on posture optimization according to an embodiment of the present disclosure. Figure 8 S130: performing a first optimization process on the vehicle posture (first group of degrees of freedom) of the current frame based on the ground point cloud data to obtain the vehicle posture (first group of degrees of freedom (roll, pitch, z)) after the first optimization process, including:

[0128] S1302, performing voxel filtering on the ground point cloud data of the current frame;

[0129] S1304, projecting the ground point cloud of the current frame after filtering into the grid map, constructing a first cost function based on the difference between the height value (z direction) of each point projected to the target grid in the point cloud and the elevation value of the target grid and the probability value of the height value of each point in the target grid;

[0130] S1306: Iteratively optimize the first cost function to obtain an optimized vehicle posture (first set of degrees of freedom).

[0131] In the present disclosure, when the ground point cloud is projected onto the grid map, there will inevitably be multiple points falling in the same grid. In order to reduce the influence of outliers, in S132, in the process of voxel filtering the ground point cloud data, preferably, the present disclosure uses the average height of multiple points falling in the same grid as the elevation value in the current grid, preferably obtained by the following formula:

[0132]

[0133] Among them, h ij is the elevation value stored in the grid with index i in the X direction and index j in the Y direction in the grid map; pz is the height of the midpoint of the grid (i, j); where i and j are obtained by the following formula:

[0134]

[0135]

[0136] Among them, px and py are the coordinate values ​​of the point along the X and Y directions respectively; resolutionX and resolutionY are the sizes of the grid along the X and Y directions respectively; floor() is a floor rounding function.

[0137] In the specific implementation, it is preferred to use the VoxelGrid class in the PCL point cloud library. When setting the voxel size, its horizontal size should be consistent with the size of the grid in the grid map. After the above processing, the number of points in the ground point cloud will be greatly reduced, reducing the calculation cost and improving the program running efficiency.

[0138] For step S1304 in this embodiment, preferably, since the elevation values ​​at the same position at different times are the same, when the vehicle moves, the ground height of the 3D space measured by the ranging sensor should have the same elevation value at different times. Based on this, it is preferred to use the difference between the ground elevation value measured by the ranging sensor at the current frame time (the average height of multiple points falling in the same grid is used as the elevation value in the current grid) and the elevation value scanned at least one frame time before the current frame time (the average height of multiple points falling in the same grid is used as the elevation value in the current grid) as the cost value to construct a first cost function, and the first cost function is preferably a least squares model, as follows:

[0139]

[0140] Among them, p k is the kth point in the ground point cloud, h i,j For k The mean of the elevation values ​​stored in the grid with index (i, j) in the corresponding map, Tt-1 is the pose data of the vehicle obtained after optimization at the last moment. γ, β, and α are the rotation changes of the vehicle's posture at time t relative to the posture at time t-1 around the Z, Y, and X axes, respectively. Their initial values ​​can be measured by IMU. R(·) is the rotation matrix corresponding to the rotation change. △z, △y, and △x are the translation changes of the vehicle's position at time t relative to the position at time t-1 along the Z, Y, and X axes, respectively. Their initial values ​​can be measured by IMU. g k For p k The probability value of the elevation value in the normal distribution obeyed by its corresponding grid.

[0141] In this disclosure, it is assumed that in the grid map, the elevation value stored in each grid follows a one-dimensional normal distribution. k The calculation uses the following formula:

[0142] -∞<h k <+∞

[0143] Among them, h k For p k After being projected onto the map, the corresponding elevation value; σ i ,j is the same as p k The standard deviation of the elevation values ​​stored in the grid at index (i, j) in the corresponding map.

[0144] When a ground point falls within the current grid, its elevation value h is calculated. k The probability value g in the normal distribution followed by the grid k When constructing the first cost function, in addition to considering the elevation difference Δh, it is also necessary to multiply g k . The technical solution disclosed in the present invention considers that when the ranging sensor is scanning the current frame, if a dynamic obstacle passes through the field of view, then the point cloud at the same position will show a huge difference in the Z direction compared with the point cloud of the previous frames. Therefore, when the point cloud of the current frame is projected onto the grid map, the elevation value of the current frame is quite different from the existing elevation value in the grid. A large elevation difference can easily cause the algorithm to fall into a local optimum. In order to reduce the impact of dynamic obstacles, the technical solution disclosed in the present invention takes into account the elevation value of the point in the point cloud and the probability value in the normal distribution obeyed by its corresponding grid when calculating the cost value.

[0145] Preferably, the iterative solution is performed based on the LM algorithm, and other suitable algorithms such as the Gauss-Newton algorithm may also be used for iterative optimization.

[0146] For the grid map updating method based on posture optimization of each of the above-mentioned embodiments, preferably, S1304, the ground point cloud of the current frame after filtering is projected into the grid map, and a first cost function is constructed based on the difference between the height value (z direction) of each point projected to the target grid in the point cloud and the elevation value of the target grid and the probability value of the height value of each point in the target grid, including:

[0147] The first cost function is constructed by using the difference between the ground elevation value acquired by the ranging sensor at the current frame time and the ground elevation value acquired at least one frame time before the current frame time as a cost value.

[0148] Wherein, the first cost function is preferably a least squares function.

[0149] Fig. 9 is a flow chart of the second optimization process in the grid map updating method based on posture optimization according to an embodiment of the present disclosure, with reference to Fig. 9 In the grid map updating method S100 based on posture optimization, S140, a second optimization process based on a non-ground point cloud is performed on the vehicle posture of the current frame after the first optimization process to obtain the vehicle posture after the second optimization process (the second optimization process optimizes the second group of degrees of freedom, and the second group of degrees of freedom includes x, y, and yaw), including:

[0150] S1402, performing voxel filtering on the non-ground point cloud data of the current frame;

[0151] S1404, projecting the non-ground point cloud of the current frame after filtering onto the grid map, obtaining the elevation feature value in each grid, and constructing a second cost function based on the difference between the elevation feature value in each grid of the current frame and the elevation feature value in each grid of the frame before the current frame;

[0152] S1406. Solve the second cost function based on the CSM algorithm to obtain an optimized vehicle posture (a second set of degrees of freedom).

[0153] Preferably, the elevation characteristic values ​​include the maximum elevation, the median elevation and the minimum elevation.

[0154] Unlike roll, pitch, z and other degrees of freedom, yaw, x, and y mainly affect the index value of the point cloud hitting the grid. Therefore, when using non-ground points to construct a grid map, the grid size cannot be set too large, otherwise, a slight change in posture will not cause a change in the statistics in the grid map, and it will be difficult to use the optimization algorithm for optimization. Therefore, when performing voxel filtering on non-ground points, the size of the voxel in the horizontal direction should be set to the same as the grid size, and it should not be too large. When constructing the optimization problem below, the statistical characteristics of the point cloud hitting the same grid in the vertical direction will be used. Therefore, under the premise of not destroying the original properties of the point cloud, and achieving the purpose of downsampling, the size of the voxel in the vertical direction should not be set too large. In addition to the above precautions, the process of voxel filtering for non-ground point cloud data is the same as that for ground point cloud data, and will not be repeated.

[0155] Unlike ground points, non-ground points do not have a continuous property in the vertical direction, and due to viewpoint reasons, there will be occlusions. Therefore, it is difficult to use a statistic to describe the distribution of point clouds in the grid. The technical solution disclosed in the present invention preferably uses statistics such as the maximum, median, and minimum values ​​of elevation values ​​to project non-ground points into the grid map, and count the maximum, median, and minimum values ​​of the elevation values ​​of the points falling into the grid, and make a difference between the three and the original statistics in the grid to construct a second cost function.

[0156] Preferably, the second cost function is constructed as:

[0157]

[0158] in, Indicates the maximum, median and minimum elevation values ​​stored in the grid with map index (i, j) (i.e., the point cloud of the frame before the current frame is projected onto the grid);

[0159] in, It indicates the maximum, median and minimum elevation values ​​in the grid obtained by statistics after the current frame point cloud is projected onto the (i, j) grid;

[0160] m and n represent the maximum values ​​of the grid index in the X and Y directions of the grid map, respectively.

[0161] In step S1406 of the present disclosure, preferably, the remaining problem is equivalent to the 2D laser positioning and mapping problem. Preferably, the technical solution of the present disclosure adopts the more mature method in 2D laser SLAM - CSM (Correlative Scan Matching) frame matching algorithm, but different from the original method, in the present disclosure, the problem is not modeled as the maximum a posteriori probability, but the minimum cost value, and the calculation of the cost value is given by the above step S1404.

[0162] Fig.10 1 is a flow chart of a data fusion method in a grid map updating method based on posture optimization according to an embodiment of the present disclosure. Fig.10 S150: Time-aligning and fusing the optimized vehicle pose of the current frame with the pose data of the vehicle acquired by the IMU, and outputting the fused vehicle pose of the current frame, including:

[0163] S1502, performing time alignment processing on the optimized vehicle posture of the current frame based on the timestamp of the IMU sensor;

[0164] S1504: Taking the currently optimized vehicle posture as a reference, integrate the posture acquired by the subsequent IMU sensor to obtain the fused vehicle posture of the current frame.

[0165] In step S1502, based on the optimized vehicle posture, an IMU odometer output that matches its timestamp is found. The specific implementation method is the same as the time alignment method in step S1104.

[0166] Step S1504 mainly realizes the fusion output of the IMU odometer and the optimized vehicle posture obtained in the above step (based on the posture data obtained by the ranging sensor such as the laser odometer). Preferably, the fusion output is obtained based on the following formula:

[0167]

[0168] Among them, T t is the vehicle pose output by the ranging sensor (such as a laser odometer) at time t (the vehicle pose is the pose after the first optimization process and the second optimization process); is the inverse of the vehicle posture output by the IMU odometer calculated in step S1502; T imu,t+k is the position of the vehicle at time t+k.

[0169] Fig.11 It is a detailed flowchart of a grid map updating method based on posture optimization according to an embodiment of the present invention.

[0170] Fig.12 This is a flowchart of how to update a raster map using a ground point cloud. Fig.12 Preferably, in S160, based on the fused vehicle posture, the grid map is updated using the ground point cloud, including:

[0171] A. If the elevation value in the grid corresponding to the point cloud is NAN, NAN means that no elevation value is stored, then the elevation value of the current point is stored in the grid;

[0172] B. When the grid corresponding to the point cloud already has elevation values ​​stored and the grid has only been updated once, if the difference between the elevation value of the current point and the elevation value stored in the grid is greater than the difference threshold, the current grid elevation value is set to NAN; if the difference is less than the difference threshold, the current value and the existing value are used to update the probability distribution of the elevation value of the current grid;

[0173] C. When the grid corresponding to the point cloud already has elevation values ​​stored and the number of updates is greater than 2, if the difference between the elevation value of the current point and the elevation value stored in the grid is greater than the difference threshold, the current point is considered to be an abnormal point and is directly discarded; if the difference is less than the difference threshold, the current value and the existing value are used to update the probability distribution of the elevation value of the current grid.

[0174] Fig.12 In the formula, count indicates the number of times the current grid in the grid map is hit, δh indicates the difference between the elevation value formed after the point in the current frame point cloud falls into the grid and the existing elevation value in the grid; ε indicates the elevation value difference threshold, and the empirical value is preferably 0.1m (other reasonable values ​​can also be taken); NAN indicates that the elevation value stored in the grid is an invalid value, that is, the current grid has never been updated.

[0175] Preferably, in S160, based on the fused vehicle posture, the grid map is updated using the non-ground point cloud, including:

[0176] a. Global settings for the grid map: use a quadtree data structure to store data in the grid map, specify the depth of the tree, specify the size of each grid in the grid map, and set the size of the scene to be modeled by the grid map;

[0177] b. Project the current frame point cloud to the grid map: Calculate the index of the corresponding grid according to the coordinates of each point, and recursively search the child node corresponding to the current grid in the quadtree until a leaf node is found;

[0178] c. Update the statistics stored in the current grid based on the newly added points in the grid.

[0179] Fig.13 A flowchart of a method for updating a grid map using a non-ground point cloud according to the present embodiment is shown.

[0180] The technical solution disclosed in the present invention updates the grid map with the ground point cloud data and non-ground point cloud data of the current frame respectively according to the vehicle posture after fusion processing of the current frame. Due to the influence of dynamic obstacles, the ground point grid map needs to follow certain rules when updating; the non-ground point grid map has a high resolution and a large amount of stored data. In order to improve the efficiency of storage and search, the present invention preferably uses a quadtree data structure to construct the grid map.

[0181] For non-ground point clouds, based on the above analysis, in order to ensure the optimization effect of yaw, x, y and other degrees of freedom, the resolution of the grid map for modeling should be larger. When modeling areas of the same area, the number of grids in the grid map corresponding to the ground point cloud and the non-ground point cloud will differ by several orders of magnitude. The problems caused by this are: 1. Increased storage cost; 2. Difficulty in grid search.

[0182] In view of the above problems, the present disclosure draws on the solution ideas of Octomap (octree map). Octomap mainly models spatial bodies, while the present disclosure focuses on "surfaces". Fig.14 As shown, a cube is divided into four small cubes, and this is repeated until the size of the small cubes meets the modeling accuracy requirements. For data storage, the present disclosure adopts a quadtree data structure. The initial cube can be regarded as a root node, and the four small cubes are its child nodes. The smallest cube is a leaf node of the quadtree structure. Unlike the octree, each small cube no longer stores the information of the corresponding position occupied, but the statistics proposed in step S1404.

[0183] To reduce storage costs, if all child nodes of a node do not store information, the current node does not need to be expanded, and only the current node is used to represent its child nodes. For the four child nodes of the parent node in the quadtree, they can be divided according to their index values ​​along the X and Y directions. The correspondence between the rectangular coordinates of the point in the point cloud and the grid index is calculated by the following formula:

[0184]

[0185]

[0186] Among them, px and py are the coordinate values ​​of the point along the X and Y directions respectively; resolutionX and resolutionY are the sizes of the grid along the X and Y directions respectively; [.] is the rounding function.

[0187] Based on the index value, it is preferably calculated in which child node of the current node according to the following formula:

[0188]

[0189] Among them, indexX and indexY represent the index values ​​of the grid; m and n represent the maximum index values ​​of the grid map represented by the current node along the X and Y directions; childNum represents the sequence number of the child node of the current node.

[0190] Compared with the prior art, the technical solution disclosed in the present invention has the following advantages: (1) The technical solution disclosed in the present invention recognizes that different degrees of freedom of the vehicle affect the elevation values ​​stored in the grid map in different ways. Changes in degrees of freedom such as roll, pitch, and z can directly affect the elevation value after the point cloud is projected onto the grid map. However, degrees of freedom such as x, y, and yaw mainly affect the corresponding index value after the point cloud is projected onto the grid map. Therefore, if the size of the grid in the grid map is appropriately increased, the influence of x, y, yaw, etc. on the elevation value in the grid can be suppressed. In view of this, the present solution divides the point cloud into ground points and non-ground points, and uses different point clouds to optimize each degree of freedom respectively. (2) In order to eliminate the influence of dynamic obstacles, the technical solution disclosed in the present invention assumes that the elevation value stored in each grid in the map obeys a one-dimensional normal distribution when constructing a ground point grid map. When calculating the cost value of matching the current frame point cloud with the grid map, not only the difference in elevation value is considered, but also the probability value of the elevation value of the current point in the grid is considered. (3) For non-ground point grid maps, the resolution is high and the amount of stored data is large. In order to improve storage and search efficiency, the technical solution disclosed in the present invention adopts a quadtree data structure to construct a grid map.

[0191] According to an embodiment of the present disclosure, a grid map updating device 1000 based on posture optimization includes:

[0192] A preprocessing module 1002, which performs time alignment on the point cloud data of the vehicle's field of view scene acquired by the ranging sensor and the vehicle's posture data acquired by the IMU to at least acquire the vehicle's posture in the current frame and remove point cloud data distortion caused by vehicle motion;

[0193] A point cloud data segmentation module 1004, which segments the point cloud data of the current frame after time alignment and point cloud data distortion removal into ground point cloud and non-ground point cloud;

[0194] A first optimization processing module 1006, which performs a first optimization processing on the vehicle posture of the current frame based on the ground point cloud data to obtain the vehicle posture after the first optimization processing;

[0195] A second optimization processing module 1008, which performs a second optimization processing based on the non-ground point cloud on the vehicle posture of the current frame after the first optimization processing, so as to obtain the vehicle posture after the second optimization processing;

[0196] A fusion processing module 1010, which performs time alignment and fusion processing on the optimized vehicle posture of the current frame and the vehicle posture data acquired by the IMU, and outputs the fused vehicle posture of the current frame;

[0197] The updating module 1012 updates the grid map based on the fused vehicle posture using the ground point cloud and the non-ground point cloud.

[0198] The grid map updating device based on posture optimization disclosed in the present invention can be implemented in a manner based on a computer software program architecture.

[0199] Fig.15 It is a structural schematic block diagram of a grid map updating device based on posture optimization using a hardware implementation of a processing system according to an embodiment of the present disclosure.

[0200] The grid map updating 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 of 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.

[0201] The hardware structure can be implemented using a bus architecture. The bus architecture can include any number of interconnecting 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 can also connect various other circuits 1400 such as peripherals, voltage regulators, power management circuits, external antennas, etc.

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

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

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

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

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

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

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

[0209] 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-mentioned grid map updating method.

[0210] 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-mentioned grid map updating method.

[0211] In the description of this specification, the description with reference to the terms "one embodiment / method", "some embodiments / methods", "example", "specific example", or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment / method or example are included in at least one embodiment / method or example of the present application. In this specification, the schematic representations of the above terms are not necessarily the same embodiment / method or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments / methods or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments / methods or examples described in this specification and the features of the different embodiments / methods or examples, unless they are contradictory.

[0212] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In the description of this application, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0213] 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 grid map updating method based on posture optimization, characterized in that: include: S110, performing time alignment on the point cloud data of the vehicle's field of view scene acquired by the ranging sensor and the vehicle's posture data acquired by the IMU to at least acquire the vehicle's posture in the current frame, and removing point cloud data distortion caused by vehicle motion; S120, dividing the point cloud data of the current frame after time alignment and point cloud data distortion removal into ground point cloud and non-ground point cloud; S130, performing a first optimization process on the vehicle posture of the current frame based on the ground point cloud data to obtain a vehicle posture after the first optimization process; S140, performing a second optimization process based on the non-ground point cloud on the vehicle posture of the current frame after the first optimization process, so as to obtain a vehicle posture after the second optimization process; S150, time-aligning and fusing the optimized vehicle posture of the current frame with the vehicle posture data acquired by the IMU, and outputting the fused vehicle posture of the current frame; as well as S160, based on the fused vehicle posture, using the ground point cloud and the non-ground point cloud to update the grid map; Among them, S130, performing a first optimization process on the vehicle posture of the current frame based on the ground point cloud data to obtain the vehicle posture after the first optimization process, including: S1302, performing voxel filtering on the ground point cloud data of the current frame; S1304, projecting the ground point cloud of the current frame after filtering into the grid map, and constructing a first cost function based on the difference between the height value of each point projected to the target grid in the point cloud and the elevation value of the target grid and the probability value of the height value of each point in the target grid; and S1306, iteratively optimizing the first cost function to obtain the optimized vehicle posture; Among them, S140, the vehicle posture of the current frame after the first optimization processing is subjected to a second optimization processing based on the non-ground point cloud to obtain the vehicle posture after the second optimization processing, including: S1402, voxel filtering of the non-ground point cloud data of the current frame; S1404, projecting the non-ground point cloud of the current frame after filtering into the grid map, obtaining the elevation feature value in each grid, and constructing a second cost function based on the difference between the elevation feature value in each grid of the current frame and the elevation feature value in each grid of the frame before the current frame; and S1406, solving the second cost function based on the CSM algorithm to obtain the optimized vehicle posture.

2. The grid map updating method based on posture optimization according to claim 1, characterized in that: In step S110, time alignment is performed to obtain at least the vehicle pose of the current frame, including: S1102, based on the vehicle's posture data obtained by the IMU, obtain the vehicle's posture data at the moment when the ranging sensor obtains the current frame point cloud data.

3. The grid map updating method based on posture optimization according to claim 2 is characterized in that: In step S110, the point cloud data distortion caused by the vehicle motion is removed, including: S1104, convert the current frame point cloud data to the vehicle coordinate system, obtain the IMU pose corresponding to each point by difference based on the timestamp information of each point in the current frame point cloud data, and convert the IMU pose corresponding to each point to the IMU pose corresponding to the initial point in the current frame point cloud data.

4. The grid map updating method based on posture optimization according to claim 1, characterized in that: S120, dividing the point cloud data of the current frame after time alignment and point cloud data distortion removal into ground points and non-ground points, including: Based on the height value of each point in the point cloud, the point cloud data of the current frame is divided into ground points and non-ground points.

5. The grid map updating method based on posture optimization according to claim 1, characterized in that: S1304, projecting the ground point cloud of the current frame after filtering into the grid map, constructing a first cost function based on the difference between the height value of each point projected to the target grid in the point cloud and the elevation value of the target grid and the probability value of the height value of each point in the target grid, including: The first cost function is constructed by using the difference between the ground elevation value acquired by the ranging sensor at the current frame time and the ground elevation value acquired at least one frame time before the current frame time as a cost value.

6. The grid map updating method based on posture optimization according to claim 1, characterized in that: The first cost function is a least squares function.

7. The grid map updating method based on posture optimization according to claim 1, characterized in that: The elevation characteristic values ​​include a maximum elevation value, a median elevation value, and a minimum elevation value.

8. The grid map updating method based on posture optimization according to any one of claims 1 to 4, characterized in that: S150, time-aligning and fusing the optimized vehicle pose of the current frame with the pose data of the vehicle acquired by the IMU, and outputting the fused vehicle pose of the current frame, including: S1502, performing time alignment processing on the optimized vehicle posture of the current frame based on the timestamp of the IMU sensor; and S1504: Taking the currently optimized vehicle posture as a reference, integrate the posture acquired by the subsequent IMU sensor to obtain the fused vehicle posture of the current frame.

9. The grid map updating method based on posture optimization according to any one of claims 1 to 4, characterized in that: In S160, based on the fused vehicle posture, the grid map is updated using the ground point cloud, including: A. If the elevation value in the grid corresponding to the point cloud is NAN, NAN means that no elevation value is stored, then the elevation value of the current point is stored in the grid; B. When the grid corresponding to the point cloud already has elevation values ​​stored and the grid has only been updated once, if the difference between the elevation value of the current point and the elevation value stored in the grid is greater than the difference threshold, the current grid elevation value is set to NAN; if the difference is less than the difference threshold, the current value and the existing value are used to update the probability distribution of the elevation value of the current grid; C. When the grid corresponding to the point cloud already has elevation values ​​stored and the number of updates is greater than 2, if the difference between the elevation value of the current point and the elevation value stored in the grid is greater than the difference threshold, the current point is considered to be an abnormal point and is directly discarded; if the difference is less than the difference threshold, the current value and the existing value are used to update the probability distribution of the elevation value of the current grid.

10. The grid map updating method based on posture optimization according to claim 9, characterized in that: In S160, based on the fused vehicle posture, the grid map is updated using the non-ground point cloud, including: a. Global settings for the grid map: use a quadtree data structure to store data in the grid map, specify the depth of the tree, specify the size of each grid in the grid map, and set the size of the scene to be modeled by the grid map; b. Project the current frame point cloud to the grid map: Calculate the index of the corresponding grid according to the coordinates of each point, and recursively search the child node corresponding to the current grid in the quadtree until a leaf node is found; and c. Update the statistics stored in the current grid based on the newly added points in the grid.

11. A grid map updating device based on posture optimization, characterized in that: include: A preprocessing module, which performs time alignment on the point cloud data of the vehicle's field of view scene acquired by the ranging sensor and the vehicle's posture data acquired by the IMU to at least acquire the vehicle's posture in the current frame and remove point cloud data distortion caused by vehicle motion; A point cloud data segmentation module, which segments the point cloud data of the current frame after time alignment and point cloud data distortion removal into ground point cloud and non-ground point cloud; A first optimization processing module, wherein the first optimization processing module performs a first optimization processing on the vehicle posture of the current frame based on the ground point cloud data to obtain the vehicle posture after the first optimization processing; A second optimization processing module, wherein the second optimization processing module performs a second optimization processing based on a non-ground point cloud on the vehicle posture of the current frame after the first optimization processing, so as to obtain the vehicle posture after the second optimization processing; A fusion processing module, which performs time alignment and fusion processing on the optimized vehicle posture of the current frame and the vehicle posture data acquired by the IMU, and outputs the fused vehicle posture of the current frame; as well as An updating module, wherein the updating module updates a grid map using the ground point cloud and the non-ground point cloud based on the fused vehicle posture; Among them, performing a first optimization process on the vehicle posture of the current frame based on the ground point cloud data to obtain the vehicle posture after the first optimization process, including: S1302, performing voxel filtering on the ground point cloud data of the current frame; S1304, projecting the ground point cloud of the current frame after filtering into the grid map, and constructing a first cost function based on the difference between the height value of each point projected to the target grid in the point cloud and the elevation value of the target grid and the probability value of the height value of each point in the target grid; and S1306, iteratively optimizing the first cost function to obtain the optimized vehicle posture; Among them, the vehicle posture of the current frame after the first optimization processing is subjected to a second optimization processing based on the non-ground point cloud to obtain the vehicle posture after the second optimization processing, including: S1402, voxel filtering of the non-ground point cloud data of the current frame; S1404, projecting the non-ground point cloud of the current frame after filtering into the grid map, obtaining the elevation feature value in each grid, and constructing a second cost function based on the difference between the elevation feature value in each grid of the current frame and the elevation feature value in each grid of the frame before the current frame; and S1406, solving the second cost function based on the CSM algorithm to obtain the optimized vehicle posture.

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

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

Images