CN114299145A

CN114299145A - Grid map updating method and device, electronic equipment and storage medium

Info

Publication number: CN114299145A
Application number: CN202111633902.5A
Authority: CN
Inventors: 郑林飞
Original assignee: Beijing Yihang Yuanzhi Technology Co Ltd
Current assignee: Beijing Yihang Yuanzhi Technology Co Ltd
Priority date: 2021-12-29
Filing date: 2021-12-29
Publication date: 2022-04-08

Abstract

The disclosure provides a grid map updating method based on pose optimization, which comprises the following steps: time alignment is carried out on the point cloud data of the visual field scene of the carrier acquired by the distance measuring sensor and the pose data of the carrier acquired by the IMU so as to at least acquire the carrier pose of the current frame, and the point cloud data distortion caused by the motion of the carrier is removed; performing point cloud data segmentation to obtain ground point cloud and non-ground point cloud; carrying out first optimization processing on the carrier pose of the current frame; carrying out second optimization processing on the carrier pose of the current frame after the first optimization processing; time alignment is carried out on the optimized carrier pose of the current frame and the pose data of the carrier acquired by the IMU, and fusion processing is carried out; and updating the grid map by using the ground point cloud and the non-ground point cloud based on the carrier pose after the fusion processing. The disclosure also provides a grid map updating device, an electronic device and a readable storage medium.

Description

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

Technical Field

The present disclosure relates to an autopilot technology and a robot technology, and more particularly, to a method and an apparatus for updating a grid map based on pose optimization, an electronic device, and a storage medium.

Background

With the development of the automatic driving and autonomous mobile robot technology and the development of new application scenes, new requirements are put forward on the original technology.

With the increase of the range of motion of the unmanned vehicle, the driving speed is greatly increased, and for slam (simultaneous Localization and mapping) technology, vehicles such as automobiles must have the capability of real-time positioning and large-scale mapping, for example, the following existing technical solutions:

prior art 1: direct LiDAR metrology for 2.5D out door Environment discloses a 2.5D grid map-based inter-frame matching technique, which derives an inspiration from a visual optical flow Direct method, assumes that ground points at the same position have the same elevation value at different times, and models a scene by taking the idea of a probability grid map as a reference, but differs in that the probability value of whether the grid is occupied or not is not stored in the grid, but the elevation value of the ground point corresponding to the position of the grid point is stored, and a least square model is constructed 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, so as to optimize the six-dimensional posture of the carrier. However, the technology does not realize that the six degrees of freedom of the carrier have different influence modes on the elevation value in the grid, the six degrees of freedom are not split, the optimization variable has higher dimensionality, the iteration times of the optimization algorithm can be increased, and the program operation efficiency is influenced; secondly, the influence of dynamic obstacles is not eliminated in the technology; finally, the technology does not solve the problem of grid map storage efficiency which may occur in large-scale scenes.

Prior art 2: a Universal Grid Map Library discloses a technology for constructing a 2.5D Grid Map, when the Map moves, the technology can realize the quick update of internal data, meanwhile, the technology can provide a plurality of data layers, different types of data can be stored in the Map, and finally, the technology provides a compatible interface with the existing multiple data types, and the quick conversion of the data types can be realized. But the technology does not solve the problem of grid map storage efficiency in large-scale scenes.

The current interframe matching algorithm of the front end in the 3D laser SLAM can be divided into two categories: 1. based on point sets, classical algorithms such as ICP, NDT, etc. and their variants; 2. based on feature points, such as lines, surface features in the LOAM, etc. Various algorithms have been studied quite deeply in academia, and the characteristics and advantages of the algorithms are quite mature to be known, but in engineering application, each scheme more or less exposes certain problems.

For example, ICP algorithm, ICP registration process iteratively finds an optimal transformation matrix between two frames of point clouds at the cost of the distance between corresponding point pairs of two adjacent frames of point clouds. Firstly, in order to determine the corresponding relation of each point in two frames of point clouds, each point needs to be processed circularly, which causes higher calculation cost; secondly, the algorithm is greatly influenced by noise and abnormal values, data processing is not good, and the algorithm cannot be converged or falls into local optimum easily; finally, the algorithm strongly depends on the pose initial value, and if the initial value is not good enough, the optimization is easy to fall into local optimization.

To avoid matching between a large number of points in the ICP and reduce the computational cost, feature point-based registration methods (by extracting representative points from the current point cloud and then modeling the correspondence between feature points in two frames of point clouds) are becoming increasingly popular. However, extracting the feature points takes a certain time, and in addition, in a place where the scene is relatively single, it is difficult to extract appropriate feature points, thereby causing problems such as scene degradation; finally, mismatching between feature points due to viewpoint changes also has a large impact on pose optimization.

Disclosure of Invention

In order to solve at least one of the above technical problems, the present disclosure provides a pose optimization-based grid map updating method, apparatus, electronic device, and storage medium.

According to one aspect of the disclosure, a grid map updating method based on pose optimization is provided, which includes:

s110, performing time alignment on the point cloud data of the visual field scene of the carrier acquired by the distance measuring sensor and the pose data of the carrier acquired by the IMU to at least acquire the pose of the carrier of the current frame, and removing point cloud data distortion caused by carrier motion;

s120, dividing the point cloud data of the current frame subjected to time alignment and point cloud data distortion removal into a ground point cloud and a non-ground point cloud;

s130, performing first optimization processing on the carrier pose of the current frame based on the ground point cloud data to obtain the carrier pose after the first optimization processing;

s140, carrying out second optimization processing based on non-ground point cloud on the carrier pose of the current frame subjected to the first optimization processing to obtain a carrier pose subjected to second optimization processing;

s150, time alignment and fusion processing are carried out on the optimized carrier pose of the current frame and the pose data of the carrier acquired by the IMU, and the carrier pose of the current frame after fusion processing is output;

and S160, updating the grid map by using the ground point cloud and the non-ground point cloud based on the carrier pose after the fusion processing.

According to at least one embodiment of the present disclosure, a grid map updating method based on pose optimization, in step S110, performing time alignment to obtain at least a carrier pose of a current frame, includes: s1102, acquiring pose data of the carrier at the moment when the ranging sensor acquires the point cloud data of the current frame based on the pose data of the carrier acquired by the IMU.

According to at least one embodiment of the present disclosure, in the method for updating a grid map based on pose optimization, in step S110, removing distortion of point cloud data caused by vehicle motion includes:

and S1104, converting the current frame point cloud data into a carrier coordinate system, acquiring an IMU pose corresponding to each point in the current frame point cloud data in a difference mode based on the timestamp information of each point, and converting the IMU pose corresponding to each point into an IMU pose corresponding to an initial point in the current frame point cloud data.

According to at least one embodiment of the present disclosure, a grid map updating method based on pose optimization, S120, segmenting point cloud data of a current frame, which is subjected to time alignment and point cloud data distortion removal, into ground points and non-ground points, includes: and dividing the point cloud data of the current frame into ground points and non-ground points based on the height values of all the points in the point cloud.

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

s1302, carrying out voxel filtering on the ground point cloud data of the current frame;

s1304, projecting ground point clouds of the current frame after filtering into a grid map, and constructing a first cost function based on difference values of height values of points projected to a target grid in the point clouds and the elevation value of the target grid and probability values of the height values of the points in the target grid;

and S1306, performing iterative optimization on the first cost function to obtain an optimized carrier pose.

According to at least one embodiment of the present disclosure, a grid map updating method based on pose optimization, in which a filtered ground point cloud of a current frame is projected into a grid map, and a first cost function is constructed based on a difference between a height value of each point projected to a target grid in the point cloud and an elevation value of the target grid and a probability value of each height value in the target grid, includes:

and constructing the first cost function by using a difference value between the elevation value of the ground acquired by the ranging sensor at the current frame time and the elevation value of the ground acquired at least one frame time before the current frame time as a cost value.

According to the grid map updating method based on pose optimization in at least one embodiment of the disclosure, the first cost function is a least square function.

According to at least one embodiment of the present disclosure, a grid map updating method based on pose optimization, S140, performing a second optimization process based on non-ground point cloud on a vehicle pose of a current frame after the first optimization process is performed, so as to obtain a vehicle pose after the second optimization process, includes:

s1402, voxel filtering is carried out on the non-ground point cloud data of the current frame;

s1404, projecting the filtered non-ground point clouds of the current frame into a grid map, acquiring an elevation characteristic value in each grid, and constructing a second cost function based on a difference value between the elevation characteristic value in each grid of the current frame and the elevation characteristic value in each grid of the previous frame of the current frame;

and S1406, solving the second cost function based on the CSM algorithm to obtain the optimized carrier pose.

According to the grid map updating method based on pose optimization of at least one embodiment of the disclosure, the elevation characteristic values comprise an elevation maximum value, an elevation median value and an elevation minimum value.

According to at least one embodiment of the present disclosure, a grid map updating method based on pose optimization, in which S150, time alignment and fusion processing are performed on the optimized carrier pose of a current frame and pose data of a carrier acquired by an IMU, and the carrier pose of the current frame after fusion processing is output, includes:

s1502, performing time alignment processing on the carrier pose after the current frame is optimized based on the timestamp of the IMU sensor;

and S1504, integrating the pose acquired by the subsequent IMU sensor by taking the currently optimized carrier pose as a reference, and acquiring the carrier pose after the fusion processing of the current frame.

According to at least one embodiment of the present disclosure, the method for updating a grid map based on pose optimization, in S160, based on the vehicle pose after the fusion processing, using the ground point cloud to update the grid map, includes:

A. if the elevation value in the grid corresponding to the point cloud is NAN, and the NAN indicates that the elevation value is not stored, storing the elevation value of the current point into the grid;

B. when the existing elevation value in the grid corresponding to the point cloud is stored and the grid is updated only once, if the difference value between the elevation value of the current point and the elevation value stored in the grid is larger than a difference threshold value, setting the elevation value of the current grid to be NAN, and if the difference value is smaller than the difference threshold value, updating the probability distribution of the elevation value of the current grid by using the current value and the existing value;

C. when the elevation values in the grids corresponding to the point cloud are stored and the updating times are more than 2, if the difference value between the elevation value of the current point and the elevation value stored in the grids is more than a difference threshold value, the current point is considered as an abnormal point, and the abnormal point is directly discarded; and if the difference value is smaller than the difference threshold value, updating the probability distribution of the elevation value of the current grid by using the current value and the existing value.

According to at least one embodiment of the present disclosure, the method for updating a grid map based on pose optimization, in S160, based on the vehicle pose after the fusion processing, using the non-ground point cloud to update the grid map, includes:

a. and (3) carrying out global setting on the grid map: storing data in the grid map by adopting a quadtree data structure, and determining the depth of a tree, the size of each grid in the grid map and the size of a scene to be modeled for setting the grid map;

b. projecting the current frame point cloud into a grid map: calculating the index of the grid corresponding to each point according to the coordinate of each point, and performing recursive search on the sub-nodes corresponding to the current grid in the quadtree until the leaf nodes are found;

c. and updating the statistics stored in the current grid according to the points newly added into the grid.

According to another aspect of the present disclosure, there is provided a pose optimization-based grid map updating apparatus, including:

the system comprises a preprocessing module, a distance measuring sensor and an IMU (inertial measurement Unit), wherein the preprocessing module is used for carrying out time alignment on point cloud data of a visual field scene of a carrier acquired by the distance measuring sensor and position and pose data of the carrier acquired by the IMU so as to at least acquire the position and pose of the carrier of a current frame and remove point cloud data distortion caused by carrier motion;

a point cloud data segmentation module which segments the point cloud data of the current frame, which is subjected to time alignment and point cloud data distortion removal, into a ground point cloud and a non-ground point cloud;

the first optimization processing module is used for performing first optimization processing on the carrier pose of the current frame based on the ground point cloud data to obtain the carrier pose after the first optimization processing;

the second optimization processing module is used for performing second optimization processing on the vehicle pose of the current frame subjected to the first optimization processing based on non-ground point cloud to obtain a vehicle pose subjected to second optimization processing;

the fusion processing module is used for carrying out time alignment on the optimized carrier pose of the current frame and the pose data of the carrier acquired by the IMU, carrying out fusion processing and outputting the carrier pose of the current frame after the fusion processing;

and the updating module is used for updating the grid map by using the ground point cloud and the non-ground point cloud based on the carrier pose after the fusion processing.

According to yet another aspect of the present disclosure, there is provided an electronic device including: a memory storing execution instructions; and a processor executing the execution instructions stored in the memory to cause the processor to execute any one of the grid map updating methods described above.

According to still another aspect of the present disclosure, there is provided a readable storage medium having stored therein execution instructions for implementing the grid map updating method of any one of the above when executed by a processor.

Drawings

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

Fig. 1 is a schematic flowchart of a pose optimization-based grid map updating method according to an embodiment of the present disclosure.

Fig. 2 is a schematic diagram of the middle part of two frames of IMU pose data of laser point cloud data acquired at time k according to an embodiment of the present disclosure.

Fig. 3 is a schematic diagram of point cloud distortion.

FIG. 4 is a schematic diagram of the transformation of the IMU pose to the IMU pose corresponding to the first point in the point cloud.

Fig. 5 shows a flowchart of time alignment and removal of point cloud data distortion caused by vehicle motion in a pose optimization-based grid map updating method according to an embodiment of the present disclosure.

FIG. 6 is a graphical representation of the variation of the angle θ about the x-axis, which produces a slight error in the y-axis and z-axis, respectively.

Fig. 7 shows a schematic flow chart of preprocessing the acquired point cloud data and IMU pose data according to an embodiment of the present disclosure.

Fig. 8 is a schematic flowchart of a first optimization process in a grid map updating method based on pose optimization according to an embodiment of the present disclosure.

Fig. 9 is a flowchart illustrating a second optimization process in the pose optimization-based grid map updating method according to an embodiment of the present disclosure.

Fig. 10 is a flowchart illustrating a data fusion method in a pose optimization-based grid map updating method according to an embodiment of the present disclosure.

Fig. 11 is a detailed flowchart of a pose optimization-based grid map updating method according to an embodiment of the present disclosure.

FIG. 12 is a schematic flow chart of updating a grid map using a ground point cloud.

Fig. 13 shows a flowchart of a method for updating a grid map using non-ground point clouds according to the embodiment.

Fig. 14 is a schematic diagram of a cube cut into four small cubes and repeated until the size of the small cube meets the modeling accuracy requirement.

Fig. 15 is a block diagram schematic structure of a pose optimization-based grid map updating apparatus implemented in hardware using a processing system according to an embodiment of the present disclosure.

Description of the reference numerals

1000 grid map updating device

1002 preprocessing module

1004 point cloud data segmentation module

1006 first optimization processing module

1008 second optimization processing module

1010 fusion processing module

1012 update module

1100 bus

1200 processor

1300 memory

1400 and other circuits.

Detailed Description

The present disclosure will be described in further detail with reference to the drawings and embodiments. It is to be understood that the specific embodiments described herein are for purposes of illustration only and are not to be construed as limitations of the present disclosure. It should be further noted that, for the convenience of description, only the portions relevant to the present disclosure are shown in the drawings.

It should be noted that the embodiments and features of the embodiments in the present disclosure may be combined with each other without conflict. Technical solutions of the present disclosure will be described in detail below with reference to the accompanying drawings in conjunction with embodiments.

Unless otherwise indicated, the illustrated exemplary embodiments/examples are to be understood as providing exemplary features of various details of some ways in which the technical concepts of the present disclosure may be practiced. Accordingly, unless otherwise indicated, features of the various embodiments may be additionally combined, separated, interchanged, and/or rearranged without departing from the technical concept of the present disclosure.

The use of cross-hatching and/or shading in the drawings is generally used to clarify the boundaries between adjacent components. As such, unless otherwise noted, the presence or absence of cross-hatching or shading does not convey or indicate any preference or requirement for a particular material, material property, size, proportion, commonality between the illustrated components and/or any other characteristic, attribute, property, etc., of a component. Further, in the drawings, the size and relative sizes of components may be exaggerated for clarity and/or descriptive purposes. While example embodiments may be practiced differently, the specific process sequence may be performed in a different order than that described. For example, two processes described consecutively may be performed substantially simultaneously or in reverse order to that described. In addition, like reference numerals denote like parts.

When an element is referred to as being "on" or "on," "connected to" or "coupled to" another element, it can be directly on, connected or coupled to the other element or intervening elements may be present. However, when an element is referred to as being "directly on," "directly connected to" or "directly coupled to" another element, there are no intervening elements present. For purposes of this disclosure, the term "connected" may refer to physically, electrically, etc., and may or may not have intermediate components.

The terminology used herein is for the purpose of describing particular embodiments and is not intended to be limiting. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. Furthermore, when the terms "comprises" and/or "comprising" and variations thereof are used in this specification, the presence of stated features, integers, steps, operations, elements, components and/or groups thereof are stated but does not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components and/or groups thereof. It is also noted that, as used herein, the terms "substantially," "about," and other similar terms are used as approximate terms and not as degree terms, and as such, are used to interpret inherent deviations in measured values, calculated values, and/or provided values that would be recognized by one of ordinary skill in the art.

Before describing the technical solution of the present disclosure in detail, the following explanation is made:

frame: the data of the sensor is read in the form of frames, each frame is a complete packet of sensor data at a certain time, and the sending frequency and the receiving frequency of the frame are determined by a data acquisition system used by vehicles such as vehicles. And (3) coordinate system: all coordinate systems described in the technical solutions of the present disclosure are preferably cartesian rectangular coordinate systems. Projection: the point cloud involved in the technical scheme of the disclosure is a point in a three-dimensional space, the grid map has a two-dimensional space, and the process of calculating the grid index in the point cloud and the grid map corresponding to the point cloud is called projection. Pose: the pose described in the technical scheme of the present disclosure is the position and the pose of an object in a 3D space, the position is coordinates in x, y, and z directions, and the pose is a rotation angle around x, y, and z axes, respectively. Voxel volume: the concept of a voxel described in the technical solution of the present disclosure is one small space in a three-dimensional space. Grid: the grid described in the technical solution of the present disclosure is one small space in a two-dimensional space.

The grid map updating method and the grid map updating device based on pose optimization of the present disclosure are described in detail below with reference to fig. 1 to 15.

Fig. 1 is a schematic flowchart of a pose optimization-based grid map updating method according to an embodiment of the present disclosure, and referring to fig. 1, a pose optimization-based grid map updating method S100 according to an embodiment of the present disclosure includes:

s110, performing time alignment (time stamp alignment) to at least acquire the carrier pose of a current frame and remove point cloud data distortion caused by carrier motion for point cloud data (preferably laser point cloud data) of a visual field scene of a carrier (an automatic driving automobile and the like) acquired by a distance measuring sensor and pose data (the pose data is pose data of the carrier in a three-dimensional world coordinate system and comprises the position and the pose of the carrier in a 3D space, the position is coordinates in x, y and z directions, and the pose is a rotating angle around x, y and z axes respectively) acquired by an IMU;

s130, performing first optimization processing on the carrier pose (a first set of degrees of freedom) of the current frame based on the ground point cloud data to obtain a carrier pose (a first set of degrees of freedom (roll, pitch, z)) after the first optimization processing;

s140, performing a second optimization process based on the non-ground point cloud on the vehicle pose of the current frame after the first optimization process to obtain a vehicle pose after the second optimization process (the second optimization process performs an optimization process on a second set of degrees of freedom, where the second set of degrees of freedom includes x, y, and yaw);

In the present disclosure, an autonomous vehicle or a mobile robot is used as a carrier, an IMU (inertial sensor) mounted on the carrier provides a rough pose (pose estimation) of the carrier, and a ranging sensor provides 3D point cloud information of an environmental space around the carrier. And taking the rough pose given by the IMU as an initial value, and quickly optimizing the current pose of the carrier by optimizing the matching of the current frame point cloud and the map to give a more accurate pose of the carrier.

The vehicle described in the present disclosure needs to be configured with an IMU and a range sensor, wherein the range sensor may be any sensor capable of providing ambient space 3D point cloud information, such as a mechanical 3D lidar, a solid state 3D lidar, a structured light camera, and the like.

In step S110 of the grid map updating method based on pose optimization S100 according to the preferred embodiment of the present disclosure, performing time alignment (timestamp alignment) to acquire at least a vehicle pose of a current frame includes:

s1102, acquiring pose data of the vehicle at the time when the ranging sensor acquires the point cloud data of the current frame (as described below, that is, acquiring pose data of the vehicle at the time k) based on the pose data of the vehicle acquired by the IMU.

In the implementation process of the technical scheme, two types of sensors are used, one type is an Inertial Measurement Unit (IMU) and is used for measuring the pose of a carrier; the other type is a distance measuring sensor, such as a 3D laser radar, etc., for obtaining the view scene information (i.e., the surrounding environment information) of the vehicle. Because the two types of sensors acquire different data frequencies during operation, for example, the sampling frequency of the IMU is 100Hz, and the sampling frequency of the lidar is 10Hz, it is difficult to accurately describe the carrier and the environment where the carrier is located by directly using the raw data output by the two types of sensors.

As shown in fig. 2, the laser point cloud data acquired at the time k is in the middle of two frames of IMU pose data, so that it is difficult to describe the pose of the carrier at the time k by the IMU pose data at the time t or t + 1.

Preferably, the method uses the IMU data at two adjacent moments before and after the moment k to approximately calculate the pose of the IMU at the moment k (namely the pose of the carrier at the moment k) in a linear interpolation mode, so as to realize time alignment of the two types of sensor data.

Preferably, the pose of the IMU at time k is acquired based on:

p_k＝p_t*(1-scale)+p_t+1*scale

wherein p is_kThe vehicle pose output by the IMU at the moment k is represented, and scale represents the proportion of the time length from t to k in the time length from t to t +1, namely:

preferably, in step S110 of the pose optimization-based grid map updating method S100 of the present disclosure, removing distortion of point cloud data caused by vehicle motion includes:

and S1104, converting the current frame point cloud data into a carrier coordinate system, acquiring an IMU (inertial measurement unit) pose corresponding to each point in the current frame point cloud data in an interpolation mode based on the timestamp information of each point, and converting the IMU pose corresponding to each point into an IMU pose corresponding to an initial point in the current frame point cloud data.

When the distance measuring sensor scans the surrounding environment, the carrier can also move to a certain extent, and the movement of the carrier can cause non-negligible influence on the distance measurement of the sensor because the measurement data of the distance measuring sensor can not be obtained instantly.

Referring to fig. 3, if the operating frequency of the ranging sensor is 10Hz, the operating cycle is 100ms, and when the vehicle moves along the X direction at a speed of 1m/s and scans a front target object, such as a wall, the measurement distances at the beginning and the end of one cycle are different by 10cm in the X direction. A huge measurement error is fatal to SLAM (simultaneous localization and mapping).

The technical scheme of the present disclosure preferably adopts the following method to remove the motion distortion of the ranging result: based on the timestamp information of each point in the point cloud, the corresponding IMU pose is found in an interpolation manner, and the IMU pose (IMU pose) is converted into the IMU pose corresponding to the first point in the point cloud, referring to fig. 4.

Preferably, the conversion is performed by the following formula:

wherein p is_iFor the ith point in the current frame point cloud, T_iRepresenting the IMU pose of the ith point acquired based on an interpolation mode;

representing the inverse of the IMU pose of the 1 st point (the 1 st point in the acquisition time) obtained based on interpolation in the current frame point cloud; p'_iAnd coordinate values representing the points from which distortion is removed.

Fig. 5 shows a flowchart of performing time alignment (timestamp alignment) and removing distortion of point cloud data caused by vehicle motion in a pose optimization-based grid map updating method according to an embodiment of the present disclosure.

For the pose optimization-based grid map updating method S100 according to each of the above embodiments, preferably, the dividing, at S120, the point cloud data of the current frame, which is subjected to time alignment and point cloud data distortion removal, into ground points and non-ground points includes:

and dividing the point cloud data of the current frame into ground points and non-ground points based on the height values of all the points in the point cloud.

In the method, when the carrier pose (6D) is optimized, the pose dimensionality is too high, the iteration times of the algorithm are increased, so that the convergence time is increased, and the program execution efficiency is influenced.

The applicant researches and discovers that the elevation value in the projected grid can be influenced in different modes by the change of 6 degrees of freedom of the carrier pose in the process of projecting the point cloud to the grid map. Wherein, the elevation value after projection is directly influenced by the tiny change of roll, pitch and z degrees of freedom, and the elevation value in the grid is indirectly influenced by the change of yaw, x and y degrees of freedom and the change of the index value of the point cloud projected in the grid. As shown in fig. 6, when the grid map is constructed, if the grid size is increased properly, the current point in the point cloud will fall into the same grid when projected into the grid, and the index value will not change, so that the error in the horizontal direction can be eliminated, and the error value in the Z direction will not be affected.

In view of the above, the present disclosure decides to perform a segmentation process on a point cloud, where ground points have continuity in the Z-direction, and a least squares problem can be constructed at the cost of elevation difference. The present disclosure thus uses ground point clouds to optimize roll, pitch, z degrees of freedom in vehicle pose, and non-ground points to optimize yaw, x, y degrees of freedom.

According to a preferred embodiment of the present disclosure, the present disclosure segments the point cloud using a height threshold to improve segmentation efficiency. Before segmentation, the point cloud is converted into a carrier coordinate system, and is segmented according to the following rules:

p.z is the value of the point in the point cloud in the z direction, h is the threshold value set when the point cloud is segmented, and the value range of h after the point cloud is converted to the carrier coordinate system is preferably 0.5-0.8.

In a cartesian rectangular coordinate system, pitch is a rotation around the X-axis (pitch angle), yaw is a rotation around the Y-axis (yaw angle), and roll is a rotation around the Z-axis (roll angle).

Fig. 8 is a schematic flowchart of a first optimization process in a grid map updating method based on pose optimization according to an embodiment of the present disclosure. Referring to fig. 8, S130, performing a first optimization process on a vehicle pose (a first set of degrees of freedom) of the current frame based on the ground point cloud data to obtain a vehicle pose (a first set of degrees of freedom (roll, pitch, z)) after the first optimization process, includes:

s1304, projecting the filtered ground point clouds of the current frame into a grid map, and constructing a first cost function based on the difference value between the height value (z direction) of each point projected to a target grid in the point clouds 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, performing iterative optimization on the first cost function to obtain an optimized carrier pose (a first set of freedom degrees).

In the present disclosure, when projecting the ground point cloud onto the grid map, it is necessary that a plurality of points fall on the same grid, and in order to reduce the influence of abnormal values, in S132, in the process of performing voxel filtering on the ground point cloud data, it is preferable that the present disclosure uses the height average of the plurality of points falling on the same grid as the height value in the current grid, and is preferably obtained by the following formula:

wherein h is_ijThe elevation values stored in grids with the index of the X direction being i and the index of the Y direction being j in the grid map are obtained; p.z is the height at the midpoint of grid (i, j); wherein i and j are obtained by the following formula:

p.x and p.y are coordinate values of the point along the X, Y direction respectively; resolutionX and resolutionY are the dimensions of the grid along the direction X, Y, respectively; floor () is a floor function.

In particular implementation, preferably, a VoxelGrid class in the PCL point cloud library is used. When the voxel size is set, the size in the horizontal direction thereof should be made to coincide with the size of the grid in the grid map. After the processing, the number of the points in the ground point cloud is greatly reduced, the calculation cost is reduced, and the program operation efficiency is improved.

For step S1304, it is preferable that the altitude values at the same positions at different times are the same, so that the ground heights of the 3D space measured by the ranging sensors should have the same altitude values at different times while the vehicle is moving. Based on this, a first cost function is preferably constructed using as a cost value a difference between an elevation value of the ground measured by the ranging sensor at the current frame time (a height average of a plurality of points falling on the same grid is taken as the elevation value in the current grid) and an elevation value scanned at least one frame time before the current frame time (a height average of a plurality of points falling on the same grid is taken as the elevation value in the current grid), the first cost function being preferably a least-squares model as follows:

wherein p is_kFor the k point in the ground point cloud, h_i,jIs a reaction of with p_kMean value, T, of the elevation values stored in the grid indexed (i, j) in the corresponding map_t-1For the pose data of the carrier obtained after the optimization at the previous moment, gamma, beta and alpha are rotation variation quantities of the pose of the carrier at the moment t relative to the pose of the carrier at the moment t-1 around the Z, Y, X axis respectively, and initial values of the rotation variation quantities can be measured by an IMU.R (-) is a rotation matrix corresponding to the rotation variation respectively. Δ z, Δ y, Δ x are the amount of translational change along axis Z, Y, X for the position of the carrier at time t relative to the position at time t-1, respectively, and their initial values can be measured by the IMU. g_kIs p_kIs the probability value in the normal distribution to which its corresponding grid follows.

It is assumed in this disclosure that the elevation values stored in each grid in the grid map follow a one-dimensional normal distribution for the above-mentioned g_kThe following formula is used for the calculation of (c):

-∞＜h_k＜+∞

wherein h is_kIs p_kAfter the elevation value is projected to a map, the corresponding elevation value is obtained; sigma_iJ is and p_kThe standard deviation of the elevation values stored in the grid indexed (i, j) in the corresponding map.

When the ground point falls in the current grid, the elevation value h of the ground point is calculated_kProbability value g in a normal distribution to which the grid is subjected_k. When the first cost function is constructed, the height difference delta h is considered and multiplied by g_k. According to the technical scheme, when the ranging sensor scans the current frame, if a dynamic obstacle passes through the field of view, the point clouds at the same position are greatly different in the Z direction compared with the point clouds of the previous frames, and therefore when the cloud clouds of the current frame are projected into the grid map, the difference between the elevation value of the current frame and the existing elevation value in the grid is large. The algorithm is easy to fall into local optimization due to large elevation difference, and influence brought by dynamic obstacles is weakened.

Preferably, the iterative solution is performed based on the LM algorithm, and other suitable algorithms such as the gauss-newton algorithm may be used for iterative optimization.

For the pose optimization-based grid map updating method of each embodiment, preferably, in S1304, projecting the filtered ground point clouds of the current frame into the grid map, and constructing a first cost function based on a difference between a height value (z direction) of each point projected to the target grid in the point clouds and an elevation value of the target grid and a probability value of each point in the target grid, including:

and constructing a first cost function by using the difference value between the elevation value of the ground acquired by the ranging sensor at the current frame time and the elevation value of the ground acquired at least one frame time before the current frame time as a cost value.

Among them, the first cost function is preferably a least squares function.

Fig. 9 is a schematic flowchart of a second optimization process in a grid map updating method based on pose optimization according to an embodiment of the present disclosure, and referring to fig. 9, in a grid map updating method based on pose optimization S100, S140, a second optimization process based on non-ground point cloud is performed on a vehicle pose of a current frame after the first optimization process is performed, so as to obtain a vehicle pose after the second optimization process (the second optimization process performs an optimization process on a second set of degrees of freedom, where the second set of degrees of freedom includes x, y, and yaw), where the method includes:

and S1406, solving the second cost function based on the CSM algorithm to obtain the optimized carrier pose (a second group of freedom degrees).

Preferably, the elevation features include a maximum elevation value, a median elevation value, and a minimum elevation value.

Unlike the roll, pitch, z, etc. degrees of freedom, yaw, x, y mainly affect the index value of the point cloud hitting the grid. Therefore, when the non-ground points are used for constructing the grid map, the grid size cannot be set to be too large, otherwise, the pose slightly changes, and the statistical quantity in the grid map cannot be changed, so that the optimization algorithm is difficult to adopt. Therefore, when the voxel filtering process is performed on the non-ground point, the dimension of the voxel in the horizontal direction should be set to be the same as the grid size, and should not be excessively large. When an optimization problem is constructed, the statistical characteristics of the point clouds hitting the same grid in the vertical direction are used, so that the purpose of down-sampling is achieved on the premise that the original attributes of the point clouds are not damaged, and the size of the voxel in the vertical direction is not suitable to be set too large. Except for the above-mentioned considerations, the process of voxel filtering on non-ground point cloud data is the same as the process of voxel filtering on ground point cloud data, and is not repeated.

Unlike ground points, non-ground points do not have a continuous property in the vertical direction, and due to the viewpoint, there is a case of occlusion, so it is difficult to describe the distribution of the point cloud in the grid using one statistic. The technical scheme of the disclosure preferably uses statistics such as maximum value, median value and minimum value of elevation values, projects non-ground points to a grid map, counts the maximum value, median value and minimum value of the elevation values of the points falling into the grid, and makes a difference between the three values and the original statistics value in the grid, so as to construct a second cost function.

Preferably, the second cost function is constructed as:

wherein the content of the first and second substances,

the method comprises the steps of representing the maximum elevation value, the median elevation value and the minimum elevation value of a grid with a map index (i, j), wherein the maximum elevation value, the median elevation value and the minimum elevation value are stored in the grid (namely after point clouds of a frame before a current frame are projected to the grid);

wherein the content of the first and second substances,

showing the maximum elevation, the median elevation and the maximum elevation in the grid obtained by statistics after the cloud of the current frame point is projected into the (i, j) gridA small value;

m, n represent the maximum values of the grid index in the grid map along the X and Y directions, respectively.

In step S1406 of the present disclosure, the remaining problem is preferably equated to a 2D laser positioning mapping problem. Preferably, the technical solution of the present disclosure adopts a relatively mature method in 2D laser SLAM, namely csm (relative Scan matching) interframe matching algorithm, but is different from the original method in that, in the present disclosure, the problem is not modeled to find the maximum posterior probability, but the minimum cost value is found, and the calculation of the cost value is given by the above step S1404.

Fig. 10 is a flowchart illustrating a data fusion method in a pose optimization-based grid map updating method according to an embodiment of the present disclosure. Referring to fig. 10, S150, performing time alignment and fusion processing on the optimized carrier pose of the current frame and the pose data of the carrier acquired by the IMU, and outputting the fused carrier pose of the current frame includes:

Step S1502 finds an IMU odometer output matching the timestamp thereof according to the optimized carrier pose, and the specific implementation is the same as the time alignment in step S1104.

Step S1504 mainly implements fusion output of the IMU odometer and the optimized vehicle pose (pose data acquired based on the ranging sensor such as the laser odometer) acquired in the above step, and preferably, the fusion output is acquired based on the following formula:

wherein, T_tVehicle pose (the first vehicle pose) output for a time t ranging sensor (e.g. a laser odometer)Pose after optimization and second optimization);

is the inverse of the vehicle pose output by the IMU odometer calculated in step S1502; t is_imu,t+kThe pose of the carrier at the moment t + k.

FIG. 12 is a schematic flow chart of updating a grid map using a ground point cloud. Referring to fig. 12, preferably, in S160, based on the vehicle pose after the fusion process, the updating the grid map using the ground point cloud includes:

In fig. 12, count represents the number of times that the current grid in the grid map is hit, and δ h represents the difference between the elevation value formed after the point in the current point cloud falls into the grid and the elevation value already in the grid; ε represents a difference threshold for elevation values, with the empirical value preferably taken to be 0.1m (other reasonable values may be taken); NAN indicates that the elevation value stored in the grid is invalid, i.e., the current grid has never been updated.

Preferably, in S160, based on the vehicle pose after the fusion processing, the updating the grid map by using the non-ground point cloud includes:

According to the technical scheme, the grid map is updated by using the ground point cloud data and the non-ground point cloud data of the current frame respectively according to the carrier pose after fusion processing of the current frame. Due to the influence of dynamic obstacles, a certain rule needs to be followed when the ground point grid map is updated; the non-ground point grid map has high resolution and large stored data volume, and in order to improve the efficiency of storage and search, the grid map is preferably constructed by adopting a quadtree data structure.

For non-ground point clouds, based on the above analysis, in order to ensure the optimization effect of the degrees of freedom of yaw, x, y, etc., the resolution of the grid map modeled by the point clouds should be larger. The areas with the same area are modeled, and the number of grids in the grid map corresponding to the ground point cloud and the non-ground point cloud is different by several orders of magnitude. The problems that arise from this are: 1. the storage cost is increased; 2. grid finding is difficult.

In view of the above, the present disclosure takes reference to the solution idea of Octomap (octree map). OctreeImage models mostly the bounding volume, while the process focus of this disclosure is "face". As shown in fig. 14, a cube is cut into four small cubes, and the process is repeated until the size of the small cube meets the modeling accuracy requirement. For data storage, the quadtree data structure is adopted in the method, the initial cube can be regarded as a root node, the four separated small cubes are child nodes of the cube, and the minimum cube is a leaf node of the quadtree structure. Unlike the octree, each small cube no longer stores therein information whose corresponding position is occupied, but the statistics presented in step S1404.

In order to reduce the storage cost, if all the child nodes of a node do not store information, the current node does not need to be expanded, and only the current node represents the child nodes. The four children nodes of the parent node in the quadtree can be divided according to their index values along the direction X, Y. The corresponding relation between the rectangular coordinates of the points in the point cloud and the grid index is calculated by the following formula:

p.x and p.y are coordinate values of the point along the X, Y direction respectively; resolutionX and resolutionY are the dimensions of the grid along the direction X, Y, respectively; [. is a rounding function.

Based on the index value, it is preferably calculated in the several children of the current node as follows:

wherein indexX and indexY denote index values of the grid; m and n represent the maximum index values of the grid map represented by the current node along the direction X, Y; childNum denotes the sequence number of the child node of the current node.

Compared with the prior art, the technical scheme of the disclosure has the following advantages: (1) the technical scheme of the disclosure recognizes that different degrees of freedom of the carrier have different influence modes on the elevation values stored in the grid map. The change of the degrees of freedom such as roll, pitch, z, etc. can directly affect the elevation value of the point cloud after being projected on the grid map, however, the degrees of freedom such as x, y, yaw, etc. mainly affect the corresponding index value of the point cloud after being projected on the grid map. Therefore, if the size of the grid in the grid map is appropriately increased, the influence of x, y, yaw, and the like on the elevation values 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 optimizes the respective degrees of freedom using different point clouds. (2) In order to eliminate the influence of dynamic obstacles, the technical scheme of the disclosure assumes that the elevation values stored in each grid in the map obey one-dimensional normal distribution when constructing the grid map of the ground points. When calculating the cost value of matching the current frame point cloud and the grid map, not only the difference of the elevation values but also the probability value of the elevation value of the current point in the grid are considered. (3) For the non-ground point grid map, the resolution ratio is high, the stored data volume is large, and in order to improve the storage and search efficiency, the technical scheme of the invention adopts a quadtree data structure to construct the grid map.

The grid map updating device 1000 based on pose optimization according to one embodiment of the present disclosure includes:

the preprocessing module 1002 is used for performing time alignment on the point cloud data of the visual field scene of the carrier acquired by the ranging sensor and the pose data of the carrier acquired by the IMU (inertial measurement unit) so as to at least acquire the pose of the carrier of the current frame and remove the distortion of the point cloud data caused by the motion of the carrier;

a point cloud data division module 1004, wherein the point cloud data division module 1004 divides the point cloud data of the current frame subjected to time alignment and point cloud data distortion removal into a ground point cloud and a non-ground point cloud;

the first optimization processing module 1006, based on the ground point cloud data, performs first optimization processing on the vehicle pose of the current frame to obtain a vehicle pose after the first optimization processing;

the second optimization processing module 1008 is used for performing second optimization processing based on non-ground point cloud on the carrier pose of the current frame subjected to the first optimization processing by the second optimization processing module 1008 to obtain a carrier pose subjected to the second optimization processing;

the fusion processing module 1010 is used for performing time alignment on the optimized carrier pose of the current frame and the pose data of the carrier acquired by the IMU and performing fusion processing on the time alignment and the pose data, and outputting the fused carrier pose of the current frame;

and an updating module 1012, wherein the updating module 1012 updates the raster map by using the ground point cloud and the non-ground point cloud based on the carrier pose after the fusion processing.

The grid map updating device based on pose optimization can be realized in a computer software program architecture-based mode.

The grid map updating apparatus may include corresponding modules for performing each or several of the steps of the above-described flowcharts. Thus, each step or several steps in the above-described flow charts may be performed by a respective module, and the apparatus may comprise one or more of these modules. The modules may be one or more hardware modules specifically configured to perform the respective steps, or implemented by a processor configured to perform the respective steps, or stored within a computer-readable medium for implementation by a processor, or by some combination.

The hardware architecture may be implemented using a bus architecture. The bus architecture may include any number of interconnecting buses and bridges depending on the specific application of the hardware and the overall design constraints. The bus 1100 couples various circuits including the one or more processors 1200, the memory 1300, and/or the hardware modules together. The bus 1100 may also connect various other circuits 1400, such as peripherals, voltage regulators, power management circuits, external antennas, and the like.

The bus 1100 may be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, an Extended Industry Standard Architecture (EISA) bus, or the like. The bus may be divided into an address bus, a data bus, a control bus, etc. For ease of illustration, only one connection line is shown, but no single bus or type of bus is shown.

Any process or method descriptions in flow charts or otherwise described herein may be understood as representing modules, segments, or portions of code which include one or more executable instructions for implementing specific logical functions or steps of the process, and the scope of the preferred embodiments of the present disclosure includes other implementations in which functions may be executed out of order from that shown or discussed, including substantially concurrently or in reverse order, depending on the functionality involved, as would be understood by those reasonably skilled in the art of the implementations of the present disclosure. The processor performs the various methods and processes described above. For example, method embodiments in the present disclosure may be implemented as a software program tangibly embodied in a machine-readable medium, such as a memory. In some embodiments, some or all of the software program may be loaded and/or installed via memory and/or a communication interface. When the software program is loaded into memory and executed by a processor, one or more steps of the method described above may be performed. Alternatively, in other embodiments, the processor may be configured to perform one of the methods described above by any other suitable means (e.g., by means of firmware).

The logic and/or steps represented in the flowcharts or otherwise described herein may be embodied in any readable storage medium for use by or in connection with an instruction execution system, apparatus, or device, such as a computer-based system, processor-containing system, or other system that can fetch the instructions from the instruction execution system, apparatus, or device and execute the instructions.

For the purposes of this description, a "readable storage medium" can be any means that can contain, store, communicate, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of the readable storage medium include the following: an electrical connection (electronic device) having one or more wires, a portable computer diskette (magnetic device), a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable read-only memory (CDROM). In addition, the readable storage medium may even be paper or another suitable medium upon which the program is printed, as the program can be electronically captured, via for instance optical scanning of the paper or other medium, then compiled, interpreted or otherwise processed in a suitable manner if necessary, and then stored in the memory.

It should be understood that portions of the present disclosure may be implemented in hardware, software, or a combination thereof. In the above embodiments, the various steps or methods may 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, any one or combination of the following techniques, which are known in the art, may be used: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application specific integrated circuit having an appropriate combinational logic gate circuit, a Programmable Gate Array (PGA), a Field Programmable Gate Array (FPGA), or the like.

It will be understood by those skilled in the art that all or part of the steps of the method implementing the above embodiments may be implemented by hardware that is instructed to be associated with a program, which may be stored in a readable storage medium, and which, when executed, includes one or a combination of the steps of the method embodiments.

In addition, each functional unit in the embodiments of the present disclosure may be integrated into one processing module, or each unit may exist alone physically, or two or more units are integrated into one module. The integrated module can be realized in a hardware mode, and can also be realized in a software functional module mode. The integrated module, if implemented in the form of a software functional module and sold or used as a separate product, may also be stored in a readable storage medium. The storage medium may be a read-only memory, a magnetic or optical disk, or the like.

The present disclosure also provides an electronic device, including: a memory storing execution instructions; and a processor or other hardware module executing the execution instructions stored in the memory, so that the processor or other hardware module executes the grid map updating method.

The present disclosure also provides a readable storage medium, in which an execution instruction is stored, and the execution instruction is executed by a processor to implement the grid map updating method.

In the description herein, reference to the description of the terms "one embodiment/implementation," "some embodiments/implementations," "an example," "a specific example," or "some examples," etc., means that a particular feature, structure, material, or characteristic described in connection with the embodiment/implementation or example is included in at least one embodiment/implementation or example of the present application. In this specification, the schematic representations of the terms described above are not necessarily the same embodiment/mode or example. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments/modes or examples. Furthermore, the various embodiments/aspects or examples and features of the various embodiments/aspects or examples described in this specification can be combined and combined by one skilled in the art without conflicting therewith.

Furthermore, the terms "first", "second" and "first" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one such feature. In the description of the present application, "plurality" means at least two, e.g., two, three, etc., unless specifically limited otherwise.

It will be understood by those skilled in the art that the foregoing embodiments are merely for clarity of illustration of the disclosure and are not intended to limit the scope of the disclosure. Other variations or modifications may occur to those skilled in the art, based on the foregoing disclosure, and are still within the scope of the present disclosure.

Claims

1. A grid map updating method based on pose optimization is characterized by comprising the following steps:

s150, time alignment and fusion processing are carried out on the optimized carrier pose of the current frame and the pose data of the carrier acquired by the IMU, and the carrier pose of the current frame after fusion processing is output; and

2. The grid map updating method based on pose optimization of claim 1, wherein in step S110, performing time alignment to obtain at least the vehicle pose of the current frame comprises:

s1102, acquiring pose data of the carrier at the moment when the ranging sensor acquires the point cloud data of the current frame based on the pose data of the carrier acquired by the IMU.

3. The grid map updating method based on pose optimization according to claim 2, wherein in step S110, removing point cloud data distortion caused by vehicle motion comprises:

4. The pose optimization-based grid map updating method according to claim 1, wherein the step S120 of segmenting the point cloud data of the current frame, which is subjected to time alignment and point cloud data distortion removal, into ground points and non-ground points comprises:

5. The grid map updating method based on pose optimization according to claim 4, wherein the step S130 of performing a first optimization process on the vehicle pose of the current frame based on the ground point cloud data to obtain a vehicle pose after the first optimization process comprises:

s1304, projecting ground point clouds of the current frame after filtering into a grid map, and constructing a first cost function based on difference values of height values of points projected to a target grid in the point clouds and the elevation value of the target grid and probability values of the height values of the points in the target grid; and

6. The grid map updating method based on pose optimization according to claim 5, wherein the step S1304 of projecting the cloud of ground points 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 a target grid in the cloud of points and the elevation value of the target grid and the probability value of the height value of each point in the target grid comprises:

7. The pose-optimization-based grid map updating method according to claim 6, wherein the first cost function is a least square function;

preferably, S140, performing a second optimization process based on a non-ground point cloud on the vehicle pose of the current frame after the first optimization process is performed, to obtain a second optimized vehicle pose, includes:

s1404, projecting the filtered non-ground point clouds of the current frame into a grid map, acquiring an elevation characteristic value in each grid, and constructing a second cost function based on a difference value between the elevation characteristic value in each grid of the current frame and the elevation characteristic value in each grid of the previous frame of the current frame; and

s1406, solving the second cost function based on the CSM algorithm to obtain the optimized carrier pose;

preferably, the elevation characteristic values comprise maximum elevation values, median elevation values and minimum elevation values;

preferably, S150, performing time alignment and fusion processing on the optimized carrier pose of the current frame and the pose data of the carrier acquired by the IMU, and outputting the fused carrier pose of the current frame, includes:

s1502, performing time alignment processing on the carrier pose after the current frame is optimized based on the timestamp of the IMU sensor; and

s1504, integrating the pose acquired by the subsequent IMU sensor by taking the current optimized carrier pose as a reference to acquire the carrier pose after the fusion processing of the current frame;

preferably, in S160, updating the grid map by using the ground point cloud based on the fused vehicle pose includes:

C. when the elevation values in the grids corresponding to the point cloud are stored and the updating times are more than 2, if the difference value between the elevation value of the current point and the elevation value stored in the grids is more than a difference threshold value, the current point is considered as an abnormal point, and the abnormal point is directly discarded; if the difference value is smaller than the difference value threshold value, updating the probability distribution of the elevation value of the current grid by using the current value and the existing value;

preferably, in S160, updating the grid map using the non-ground point cloud based on the fused vehicle pose includes:

b. projecting the current frame point cloud into a grid map: calculating the index of the grid corresponding to each point according to the coordinate of each point, and performing recursive search on the sub-nodes corresponding to the current grid in the quadtree until the leaf nodes are found; and

8. A grid map updating device based on pose optimization is characterized by comprising:

the fusion processing module is used for carrying out time alignment on the optimized carrier pose of the current frame and the pose data of the carrier acquired by the IMU, carrying out fusion processing and outputting the carrier pose of the current frame after the fusion processing; and

9. An electronic device, comprising:

a memory storing execution instructions; and

a processor executing execution instructions stored by the memory to cause the processor to perform the method of any of claims 1 to 7.

10. A readable storage medium having stored therein execution instructions, which when executed by a processor, are configured to implement the method of any one of claims 1 to 7.