WO2021237667A1

WO2021237667A1 - Dense height map construction method suitable for legged robot planning

Info

Publication number: WO2021237667A1
Application number: PCT/CN2020/093236
Authority: WO
Inventors: 王越; 潘一源; 熊蓉
Original assignee: 浙江大学
Priority date: 2020-05-29
Filing date: 2020-05-29
Publication date: 2021-12-02

Abstract

Disclosed is a dense height map construction method suitable for legged robot planning, which method belongs to the technical field of dense height map construction. The construction method of the present invention specifically comprises: using a Gaussian distribution model to estimate a measurement value of a laser radar, and mapping the measurement value to a corresponding grid of a local map, so as to perform multi-frame data fusion, during which, according to posture information of a robot, a map grid covariance is updated, a local map is correspondingly moved, and a dynamic obstacle is removed, so as to create a complete local dense map. The method of the present invention overcomes the defects of existing dense maps having a large data volume and a large construction calculation amount, and not being updated in a timely manner, and has the advantages of convenient construction, high construction precision, etc.; by means of the method, real-time construction can be completed; and the method can be directly used for a navigation task of a legged robot.

Description

A dense height map construction method suitable for leg-foot robot planning

Technical field

The invention relates to a dense height map construction technology, in particular to a dense height map construction method suitable for leg and foot robot planning.

Background technique

Mobile robot is a comprehensive system integrating environment perception, dynamic decision-making and planning, behavior control and execution, etc. It is not only widely used in industry, agriculture, medical, service and other industries, but also in urban security, national defense and space detection. Fields and other occasions that are harmful to people and extremely dangerous have been well applied. In recent years, leg-foot robots have attracted worldwide attention and become a research hotspot due to their excellent accessibility in complex terrain. Compared with wheeled and crawler robots, the legged robot not only has the same motion control behaviors as forward, backward, and turning, but it can also achieve a "crossover" movement by controlling the footing position of the forefoot and hindfoot. This kind of operation allows the legged robot to easily pass through terrain such as trenches and low-lying land, and also allows it to climb steps and other complex scenes that traditional wheeled robots cannot pass.

For the path planning tasks of ground robots, the existing map representation methods mainly use two-dimensional grid maps to describe the external environment. Indicates whether there are obstacles in the corresponding place by whether the two-dimensional grid in the map is occupied. This representation method is simple to construct and fast to retrieve data. It is suitable for the path planning of wheeled and crawler robots, but it cannot provide height information of the environmental terrain for the footing planning of the legged robot. Therefore, in order to realize the efficient motion planning of the legged robot, a dense map that can describe the complex terrain that can meet the navigation planning of the legged robot is required. The dense map is a map formed by accumulating the data obtained by the distance sensor in real time according to the pose information of the robot. It solves the problem of small detection range and low resolution of the distance sensor, and thus can completely describe the surface shape of the scene. Dense maps are mainly used in the research of three-dimensional reconstruction and semantic maps, but due to their large amount of data, it brings greater challenges to the map storage and processing on real-time mobile robot systems.

Combining the commonly used sensors, how to construct an efficient map update system to realize the real-time storage and calculation of dense maps, and how to apply the dense maps to common route navigation are all problems and challenges that need to be solved in the current dense map.

Summary of the invention

In order to overcome the shortcomings of the prior art, the purpose of the present invention is to provide a dense height map construction method suitable for the planning of leg-foot robots, which is of great significance for realizing the long-term efficient and stable operation of the leg-foot mobile robot.

The present invention is achieved through the following technical solutions: a method for constructing a dense height map, including the following steps:

Step 1: Use lidar to obtain point cloud information of the surrounding environment at a frequency of 10-20 Hz. The point cloud information is mapped to a local height map. In the local height map, the height measurement value is a Gaussian probability distribution, which is approximately

Among them, p is the measured height value, and δ _p ² is the variance. Obtain a single measurement value from the lidar to the terrain in the lidar coordinate system S

Convert it to the corresponding height value p, specifically:

in,

Represents the inverse of the rotation matrix of the lidar coordinate system S transformed to the global map coordinate system M;

Is the distance information of SM in the global map coordinate system M; SM is the distance information from point S to point M; P is the projection matrix, and the value is [0 0 1].

Pass height value p, single measurement value

Φ _SM obtains the distance Jacobian matrix J _s and the rotating Jacobian matrix J _Φ measured by the lidar, and obtains the variance δ _p ^{2 of} the height value p.

Among them, ∑ _s represents the covariance matrix of the lidar, and ∑ _ΦIS represents the covariance matrix of the lidar with respect to the angle.

Finally, through Kalman filtering, the latest height value p obtained by the lidar and the variance (p, δ _p ² ) of the height value p are estimated with the grid corresponding to the local height map

Perform fusion, update to get the latest local map height

And the local map variance δ _h ²⁺ .

The above operations are performed on all the measured values of the lidar, so that all the measured values are mapped to the corresponding grid of the local map for fusion, thereby constructing a complete local map.

Further, the local map variance δ _h ²⁺ constitutes the grid covariance, and the specific method is:

The covariance matrix at time k is set for each grid of the local height map

Among them, δ _{x, min} ² and δ _{y, min} ² respectively represent the uncertainty of the horizontal plane of the local height map in the x and y directions,

Indicates the high degree of uncertainty of the grid.

From time k to time k+1, the robot _{moves from B 1} to B ₂ , and the covariance matrix of the local height map is from

Becomes

Get the covariance matrix updated due to the robot's movement and transfer:

Among them, Σ _r represents the uncertainty of the translational movement of the robot B ₁ to B ₂ _{, and Σ Φ} represents the uncertainty of the rotation change of the movement of _{B 1} _{to B 2.}

Further, the local map construction method further includes: when the local map moves, through the robot's positioning estimation system, obtain the displacement X _{Shift in the} _{X direction of the robot and the displacement Y Shift in the} Y direction from the previous moment, and set the local The map resolution is s, and the position offset values X _StartIndex and Y _{StartIndex are} updated according to the movement information of the robot to obtain

The relative displacement of the local map on the grid is X _{index_shift} and Y _{index_shift} :

X _startIndex = (X _startIndex -round(X _Shift /s)+L _x )%L _x (9)

Y _startIndex = (Y _startIndex -round(Y _Shift /s)+L _y )%L _y (10)

Among them, round() is a function operation for rounding decimals to obtain integers,% is a remainder operation, and L _x and _Ly represent the number of grids in the X and Y directions of the grid map, respectively. When the grid information of the map (x, y) needs to be queried, the data content of the storage location is _{queried (x search} , y _{search) through the following mapping.}

x _search =(L _x -X _startIndex +x)%L _x (11)

y _search = (L _y -Y _startIndex +y)%L _y (12)

Further, the local map construction method further includes:

The update of the point cloud information under the local map construction is directly added to the local map. For each measurement value of the lidar, the measured value to the terrain height of all points on the line segment of the lidar projection point on the ground has a height limitation h _limitation :

Wherein, h _{R_P} laser radar observation point to the height difference, L _{R_P} lidar horizontal distance of the observation point, L _{C_P} be required to limit the height of the grid to the horizontal distance of the observation point. If the height data of the local height map at the last moment exceeds the corresponding height limit, it means that the object is moving, and the height information of the corresponding point needs to be cleared.

Compared with the prior art, the beneficial effect of the present invention is that the method of the present invention uses GPU and multi-threaded parallel computing, so that the entire mapping process can be run in real time. A 2.5D dense map with feasible region information is constructed to facilitate the planning of legged robots. Only ray tracing the grid that is judged as an obstacle, instead of ray tracing all areas of the map, to determine whether the area is the afterimage of a moving obstacle. Using this strategy construction, dynamic obstacles can be effectively dealt with. The invention overcomes the shortcomings of large data volume, large construction calculation volume, and untimely update of the existing dense map, and has the advantages of convenient construction and high construction accuracy, and can complete real-time construction, and can be directly used for the navigation task of leg-foot robots. .

Description of the drawings

Fig. 1 is a schematic flow chart of the method for constructing a dense height map of the present invention.

Specific implementation method

In the following, the technical solution of the present invention will be further described with reference to the drawings and specific implementations:

As shown in Figure 1, the present invention provides a dense map construction method, including the following steps:

Convert it to the corresponding height value p, specifically:

in,

Pass height value p, single measurement value

Perform fusion, update to get the latest local map height

And the local map variance δ _h ²⁺ .

The local map variance δ _h ²⁺ constitutes the grid covariance. In the method of updating each grid covariance of the map, since the local height map based on the M coordinate system is defined relative to the sensor/robot reference system, Whenever the robot moves relative to the inertial coordinate system I, the local map information needs to be updated according to the change of the posture estimation, including the average height h and the variance value δ _p ² . The variance and average of each grid on the map will be updated based on the uncertainty of the motion and the estimated value of the surrounding grid.

The covariance matrix at time K is set for each grid of the local height map

Indicates the high degree of uncertainty of the grid.

Becomes

Get the covariance matrix updated due to the robot's movement and transfer:

After the dense map is updated, the dense grid map with a fixed size and pixels will move along with the movement of the robot's position. In order to simplify the calculation, the dense map constructed by this method has only two translational motions along the x and y directions. In this process, a small part of the data on the edge of the dense map is deleted due to changes in the visualization area, and most of the map area only changes the coordinate position of the stored data but still retains the data content. Therefore, the common practice when the map is moving is to change the storage location of the map reserved data according to the position of the robot, but this is a huge amount of calculation for a large-size raster map. Each time the map is moved, the mutual positional relationship between the retained data on the map has never changed. Based on this discovery, after the map moves, we will not change the storage location of the map data, but update the position offset values X _StartIndex and Y _StartIndex according to the robot's movement information. When we need to query the information of different locations on the map, we can use these two values to query the data content of all rasters, specifically:

When the local map moves, through the robot's positioning estimation system, the displacement X _{Shift in the} X direction and the displacement Y _{Shift in the} Y direction of the robot from the previous moment are obtained, and the local map resolution is set to s, and according to the robot's movement information Update the position offset values X _StartIndex and Y _StartIndex to obtain

X _startIndex = (X _startIndex -round(X _Shift /s)+L _x )%L _x (9)

Y _startIndex = (Y _startIndex -round(Y _Shift /s)+L _y )%L _y (10)

x _search =(L _x -X _startIndex +x)%L _x (11)

y _search = (L _y -Y _startIndex +y)%L _y (12)

The update of the point cloud information under the local map construction is directly added to the local map, and the missing points in the new observation cannot be easily judged whether it is caused by the movement of the object. Therefore, to deal with the problem that the original point cloud needs to be eliminated due to the movement of the object, it is mainly handled by the method of ray tracing. The principle is that if a point on the ground can form a light path with the lidar at the current moment, the point is defaulted to the lidar The terrain height of each point on the line segment of the ground projection point will not block the light path, that is, all points on this line segment will have a maximum height limit, that is, for each measurement value of the lidar, the measurement value The terrain height of all points on the line segment to the lidar projection point on the ground has a height limitation h _limitation :

Wherein, h _{R_P} laser radar observation point to the height difference, L _{R_P} lidar horizontal distance of the observation point, L _{C_P} be required to limit the height of the grid to the horizontal distance of the observation point. If the height data of the local height map at the last moment exceeds the corresponding height limit, it means that the object is moving, and the height information of the corresponding point needs to be cleared. Therefore, using the ray tracing principle, and according to the real-time point cloud data of the sensor, the height limit of the grid on each light path can be calculated to realize the removal of moving objects on the dense height map.

Compared with the dense height map constructed by the method of the present invention, the feasibility of each area of the map is evaluated by geometric structure characteristics. For each grid of the map, the surface normal vector (representing the slope and curvature of the local surface) and the height difference of the nearby grids (representing the roughness of the local surface) are extracted as the geometric features of the local area.

In order to obtain the surface normal vector n _i P _i at the grid, a classical approach based on P _i as the origin, the coordinates of adjacent grid and combined height information, a fitting plane _{_{S i = n ix x + n}} iy y + n _iz z. When the sum of the distances between the fitted plane and all three-dimensional points is the smallest, the normal vector n _i can be obtained. The specific formula is as follows:

Where k represents the number of grids in the adjacent area, Q _i is a 3*K matrix with the three-dimensional coordinate information of the adjacent K points, P _i is the three-dimensional vector of the combination of the two-dimensional coordinate and height information of the grid to be evaluated, n _i =[n _ix ,n _iy ,n _iz ] is the normal vector to be solved.

The height difference of the grid can be calculated by establishing a _{grid window with P (x,y)} as the center and a size of N*N. The average height of the grid in this window is

The height of the grid P _{(x, y) is}

The height difference is:

Convert the unit normal vector into the slope information of the corresponding grid and combine the height difference, using a simple and effective evaluation formula:

Get the passable score of each grid, where (x,y) represents the location of the grid. v _slope (x,y) and v _rough (x,y) are the slope and height difference, w _s and w _r are the corresponding weights,

with

Indicates the corresponding feature threshold when the grid is passable. If the final calculated feasible region score is higher than the set feasible threshold, it means that the grid is passable; if the score is lower than the threshold, it means that the part has obstacles or the terrain is not suitable for passing.

Claims

A method for constructing a dense height map is characterized in that it comprises the following steps:

Step 1: Use lidar to obtain point cloud information of the surrounding environment at a frequency of 10-20 Hz. The point cloud information is mapped to a local height map. In the local height map, the height measurement value is a Gaussian probability distribution, which is approximately

Among them, p is the measured height value, and δ p 2 is the variance. Obtain a single measurement value from the lidar to the terrain in the lidar coordinate system S
Convert it to the corresponding height value p, specifically:

in,
Represents the inverse of the rotation matrix of the lidar coordinate system S transformed to the global map coordinate system M;
Is the distance information of SM in the global map coordinate system M; SM is the distance information from point S to point M; P is the projection matrix, and the value is [0 0 1].

Pass height value p, single measurement value
Φ SM obtains the distance Jacobian matrix J s and the rotating Jacobian matrix J Φ measured by the lidar, and obtains the variance δ p 2 of the height value p.

Among them, ∑ s represents the covariance matrix of the lidar, and ∑ ΦIS represents the covariance matrix of the lidar with respect to the angle.

Finally, through Kalman filtering, the latest height value p obtained by the lidar and the variance (p, δ p 2 ) of the height value p are estimated with the grid corresponding to the local height map
Perform fusion, update to get the latest local map height
And the local map variance δ h 2+ .

The above operations are performed on all the measured values of the lidar, so that all the measured values are mapped to the corresponding grid of the local map for fusion, thereby constructing a complete local map.
The method for constructing a dense height map according to claim 1, wherein the local map variance δ h 2+ forms a grid covariance, and the specific method is:

The covariance matrix at time k is set for each grid of the local height map

Among them, δ x, min 2 and δ y, min 2 respectively represent the uncertainty of the horizontal plane of the local height map in the x and y directions,
Indicates the high degree of uncertainty of the grid.

From time k to time k+1, the robot moves from B 1 to B 2 , and the covariance matrix of the local height map is from
Becomes
Get the covariance matrix updated due to the robot's movement and transfer:

Among them, Σ r represents the uncertainty of the translational movement of the robot B 1 to B 2 , and Σ Φ represents the uncertainty of the rotation change of the movement of B 1 to B 2.
The dense height map construction method according to claim 1, wherein the local map construction method further comprises: after the local map moves, obtaining the displacement X of the robot in the X direction from the previous time through the robot's positioning estimation system Shift Shift Y displacement and the Y-direction, setting the local map resolution is s, and the offset values Y and X StartIndex the StartIndex the mobile information update position of the robot to obtain

The relative displacement of the local map on the grid is X index_shift and Y index_shift :

X startIndex = (X startIndex -round(X Shift /s)+L x )%L x (9)

Y startIndex = (Y startIndex -round(Y Shift /s)+L y )%L y (10)

Among them, round() is a function operation for rounding decimals to obtain integers,% is a remainder operation, and L x and Ly represent the number of grids in the X and Y directions of the grid map, respectively. When the grid information of the map (x, y) needs to be queried, the data content of the storage location is queried (x search , y search) through the following mapping.

x search =(L x -X startIndex +x)%L x (11)

y search = (L y -Y startIndex +y)%L y (12)
The method for constructing a dense height map according to claim 1, wherein the method for constructing a local map further comprises:

The update of the point cloud information under the local map construction is directly added to the local map. For each measurement value of the lidar, the measured value to the terrain height of all points on the line segment of the lidar projection point on the ground has a height limitation h limitation :

Wherein, h R_P laser radar observation point to the height difference, L R_P lidar horizontal distance of the observation point, L C_P be required to limit the height of the grid to the horizontal distance of the observation point. If the height data of the local height map at the last moment exceeds the corresponding height limit, it means that the object is moving, and the height information of the corresponding point needs to be cleared.