Local terrain perception method for legged robot

Abstract

Disclosed in the present invention is a local terrain perception method for a legged robot, comprising the following steps: S1, providing on a legged robot at least one LiDAR, acquiring extrinsic parameters of the LiDAR with respect to a legged robot body coordinate system, and transforming to the legged robot body coordinate system point clouds scanned by the LiDAR; S2, removing motion distortion from the point clouds, transforming same to a legged robot gravity coordinate system, and keeping the point clouds within a set range; S3, reducing the dimensionality of the point clouds to a height information matrix; S4, performing hole filling on the basis of the height information matrix; S5, on the basis of the height information matrix, removing the effect of a leg point cloud of the legged robot on the height information matrix; and S6, analyzing the processed height information matrix, and computing a slope information matrix and an obstacle information matrix for the legged robot to plan footholds and avoid obstacles. The present invention has a low degree of dependency on high-precision odometers, and can directly observe terrains beneath and around legged robot bodies, allowing for more accurate perception.

Description

A local terrain perception method for legged robots Technical Field

[0001] The present invention belongs to the technical field of legged robots, and in particular relates to a local terrain perception method for a legged robot. Background Art

[0002] Legged robots need to perceive the terrain beneath and around them to plan footholds and avoid obstacles on uneven surfaces like stairs. Directly observing the terrain beneath the body is challenging due to the limited field of view of point cloud sensors and interference from the legs. Currently, leading legged robot manufacturers, such as Boston Dynamics and AnyBotics, rely on odometry for terrain accumulation and stitching. This involves using a high-precision laser / visual odometry to estimate the robot's displacement and attitude angles in real time. During motion, the robot uses multiple frames of historical point clouds to stitch together the complete terrain. This means the terrain beneath the robot is currently scanned before reaching that location. This approach has the following disadvantages: 1) high computational complexity; 2) high odometry accuracy requirements; and 3) odometry failure in certain scenarios. For example, laser odometry fails in open, narrow, or feature-free scenes, while visual odometry fails in dark or textureless scenes.

[0003] Summary of the Invention

[0004] In order to overcome the shortcomings of the existing technology, the present invention provides a local terrain perception method for a legged robot, which does not rely on a high-precision odometer, directly observes the terrain under and around the body, and has more accurate perception.

[0005] The technical solution adopted by the present invention to solve the technical problem is: a method for sensing local terrain of a legged robot, comprising the following steps:

[0006] S1, setting at least one laser radar on the legged robot, and obtaining the external parameters of the laser radar relative to the legged robot body coordinate system, converting the point cloud obtained by the laser radar scanning to the legged robot body coordinate system, and regularly updating the scanned point cloud;

[0007] S2, removing motion distortion from the point cloud obtained in step S1, and converting it to the gravity coordinate system of the legged robot, retaining the point cloud within the set range;

[0008] S3, reducing the dimension of the point cloud obtained in step S2 into a height information matrix;

[0009] S4, performing hole filling based on the height information matrix obtained in step S3;

[0010] S5, based on the height information matrix obtained in step S4, removing the influence of the leg point cloud of the leg robot on the height matrix;

[0011] S6, analyzing the height information matrix processed in step S5, and calculating the slope information matrix and the obstacle information matrix for the foothold planning and obstacle avoidance of the legged robot.

[0012] Furthermore, the step S1 includes the following sub-steps:

[0013] S11, installing at least one laser radar on the legged robot so that the periphery of the legged robot and the lower part of the torso are within the scanning range;

[0014] S12, define the coordinate system of the legged robot body. The origin of the coordinate system is any position of the legged robot, where the x-axis is facing the front of the legged robot, the y-axis is facing the left of the legged robot, and the z-axis is facing vertically upwards of the legged robot. The external parameters include the rotation matrix and displacement vector b represents the coordinate system of the legged robot body, l i represents the i-th laser radar;

[0015] S13, after receiving the point cloud data, convert the point cloud from the lidar coordinate system to the legged robot body coordinate system using the following formula:

[0016] in is the coordinate of the measurement point of the i-th laser radar in the radar coordinate system, X b 、Y b 、Z b The coordinates of the measurement point after conversion to the legged robot body coordinate system;

[0017] S14, set the period T for collecting point clouds. Every T time, merge the point clouds scanned by the laser radar into the same point cloud after being processed in step S13.

[0018] Further,

[0019] The step S2 includes the following sub-steps:

[0020] S21, an IMU is installed on the legged robot, the laser radar is connected to a processor that can obtain the IMU attitude angle, and the installation posture of the IMU is consistent with the legged robot specimen system, so that the IMU can represent the posture of the quadruped robot;

[0021] S22, combining joint motor encoders and IMU to estimate the displacement of the legged robot;

[0022] S23, input the IMU attitude data and the legged robot displacement odometer data with a frequency of not less than 100 Hz into the point cloud processing module, and remove the motion distortion of the point cloud by combining the timestamp of each point in the point cloud so that the timestamp of each point after processing is the time of the last point in the point cloud:

[0023] where X b_trans 、Y b_trans , Z b_trans The coordinates of the point after removing motion distortion in the legged robot body coordinate system are: They represent the rotation matrix and displacement vector of the legged robot body coordinate system at the timestamp of the j-th point relative to the legged robot body coordinate system at the timestamp of the end of the point cloud;

[0024] S24, define a gravity coordinate system for the legged robot, whose origin coincides with the origin of the coordinate system of the legged robot body, whose z-axis is opposite to the direction of gravity, whose x-axis is perpendicular to the direction of gravity and is in the same vertical plane as the x-axis of the coordinate system of the legged robot body, whose y-axis is perpendicular to the x-axis and z-axis respectively, and whose directions satisfy the right-hand rule;

[0025] S25, based on the IMU posture corresponding to the timestamp at the end of the point cloud, the point cloud after removing the motion distortion in step S23 is converted from the legged robot body coordinate system to the legged robot gravity coordinate system:

[0026] where X g 、Y g , Z g To convert the coordinates to the quadruped robot gravity coordinate system, is the rotation matrix, which is obtained as follows:

[0027] Convert the IMU attitude Euler angle corresponding to the timestamp at the end of the point cloud into a rotation matrix, which is Set the heading angle in the Euler angle to zero, keep the pitch angle and roll angle unchanged, and then convert it into a rotation matrix, which is

[0028] S26 , performing through filtering on the point cloud obtained in step S25 , intercepting the point cloud within the target range in the x, y, and z directions of the quadruped robot's gravity coordinate system and retaining the point cloud, and discarding the point cloud outside the range.

[0029] Since the point cloud of the same frame of lidar is not sampled at the same time, and the lidar moves with the robot, the original point cloud data is distorted, and an algorithm is needed to remove motion distortion; the robot's gravity coordinate system converted from the point cloud can ensure that when the robot tilts, the inclination angle of the ground, slopes, and other terrain is the same as the actual one; retaining only the point cloud within the set range can reduce the amount of calculation.

[0030] Furthermore, step S3 includes the following sub-steps:

[0031] S31, defining a matrix for storing terrain height information. Define an empty matrix with m rows and m columns to store the terrain height information around the quadruped robot. The center of the matrix corresponds to the origin of the quadruped robot's gravity coordinate system. Each element of the matrix represents an n cm × n cm square, where the row represents the x-axis direction and the column represents the y-axis direction.

[0032] S32, reduce the dimensionality of the three-dimensional point cloud into a height information matrix, traverse the point cloud obtained in step S2, and determine the element to which the point belongs in the matrix based on the x and y coordinates of each point. If the element is empty, assign the z coordinate of the current point as the height to the element. If the element has been assigned, compare the element value and the z coordinate of the current point. If the element value is smaller, update the element value to the z coordinate of the current point.

[0033] Reducing the dimension of the point cloud into a height information matrix and reducing the three-dimensional information to two dimensions can reduce the amount of data and turn the disordered data into ordered data, which is convenient for later processing.

[0034] Furthermore, step S4 includes the following sub-steps:

[0035] S41, looking for empty elements in the height matrix, using the height of the element at the hole boundary as the candidate height for filling the hole, traversing the height information matrix obtained in step S3, for an element with its own height value, if its front, back, left, or right neighbors have elements with empty height values, the current element is considered to be a hole edge element, and each element is visited one by one from the hole edge element toward the empty neighbor until an element with a non-empty height value is visited, or the number of visits reaches a preset upper limit, or the edge of the matrix is ​​visited. If an element with a non-empty height value is visited, the height difference is compared with the current hole edge element. If the absolute value of the height difference is less than a preset threshold, the smaller height value is recorded in a hole record container as the candidate height of the empty element, and the row and column coordinates of all empty elements in the process of visiting one by one are recorded;

[0036] S42: Select a suitable height from multiple candidate heights. The coordinates of empty elements in the empty record container may be repeated. The coordinates of empty elements in all containers are traversed and the element is accessed in the height information matrix. If it is still empty, the candidate height is assigned to the element. If it has been assigned a height value in the previous record, the current height value is compared with the new candidate value and the smaller height is selected.

[0037] S43, repeating steps S41 and S42 until there are no new holes to be filled or the preset upper limit is reached.

[0038] Since the LiDAR point cloud is not dense enough, some grids on the height information matrix have no corresponding point clouds. The height of the blank spaces is filled with the surrounding grids with height values ​​to facilitate subsequent processing.

[0039] Furthermore, step S5 includes the following sub-steps:

[0040] S51, setting the range of the leg of the leg robot and the erosion size, setting a row and column range for the height information matrix according to the size of the leg robot, which can cover the matrix elements of the updated point cloud of the leg of the leg robot, with the number of rows ranging from r1 to r2 and the number of columns ranging from c1 to c2, and setting an erosion size s according to the size of the leg;

[0041] S52, eroding the convex height within the set range to erode the terrain convexity caused by the leg point cloud, copying the height information matrix to a temporary matrix, and updating each element of the temporary matrix from rows r1-s to r2+s and columns c1-s to c2+s to the minimum height value within the range of -s to s rows and -s to s columns around the current element, thereby removing the abnormal convexity of certain matrix elements caused by the leg point cloud;

[0042] S53, performing expansion compensation on the normal terrain by updating each element in the temporary matrix from rows r1-s to r2+s and columns c1-s to c2+s to the maximum height value within the range from -s to s rows and -s to s columns surrounding the current element, thereby compensating for the change to the normal terrain caused by step S52;

[0043] S54, updating the height information matrix, and copying the area with the number of rows ranging from r1 to r2 and the number of columns ranging from c1 to c2 in the temporary matrix to the original height information matrix.

[0044] The above steps effectively prevent the point cloud of the leg of the legged robot from being misinterpreted as terrain information below the legged robot, making perception more accurate.

[0045] Furthermore, step S6 includes the following sub-steps:

[0046] S61, calculate the slope component of each element in the matrix in the x direction, define an m×m x direction slope information matrix, traverse the height information matrix obtained in step 5, and calculate the x direction slope component for each element with a non-empty height value. x :

[0047] Where n represents the width of the square grid area corresponding to an element, h(i, j) is the height value of the element in the i-th row and j-th column. When h(i-1, j) does not exist or the height value is empty, and h(i+1, j) has a height value, slope x=slope x2 , when h(i+1, j) does not exist or the height value is empty, and h(i-1, j) has a height value, slope x =slope x1 , h(i-1, j) and h(i+1, j) do not exist or the height value is empty, slope x =0;

[0048] S62, calculate the slope component of each element in the matrix in the y direction, define an m×m y-direction slope information matrix, traverse the height information matrix obtained in step 5, and calculate the slope component of the y direction for each element with a non-empty height value. y :

[0049] Where n represents the width of the square grid area corresponding to an element, h(i, j) is the height value of the element in the i-th row and j-th column. When h(i, j-1) does not exist or the height value is empty, and h(i, j+1) has a height value, slope y =slope y2 , when h(i, j+1) does not exist or the height value is empty, and h(i, j-1) has a height value, slope y =slope y1 , h(i, j-1) and h(i, j+1) do not exist or the height value is empty, slope y =0;

[0050] S63, calculate the actual slope of each element in the matrix, define an m×m actual slope information matrix, and update it with the slope information matrices in the x-direction and y-direction:

[0051] Where slope(i, j) is the slope value of the element in the i-th row and j-th column of the actual slope information matrix. x (i, j) and slope y (i, j) are the slope values ​​of the elements in the i-th row and j-th column of the x-direction slope matrix and the y-direction slope matrix respectively;

[0052] S64, define an m×m obstacle information matrix, presetting the maximum walking step length of the legged robot to l_Max, the maximum landing height to h_Max, and the maximum landing slope to s_Max:

[0053] S65, matrix element access order for offline obstacle analysis process;

[0054] S66, according to the search order obtained in step S65, access the corresponding elements of the height information matrix and the slope information matrix in sequence, and analyze whether the grid where each element is located is an obstacle.

[0055] Furthermore, in step S65, multiple search directions are preset starting from the center of the matrix, and the multiple search directions cover a range of 360°. According to the matrix size, the search width of each search direction is preset to w, and the search step is n, which is consistent with the grid width corresponding to an element. When searching each direction, a line segment with a midpoint at the center of the matrix, a width of w, and perpendicular to the search direction is first obtained. The matrix elements corresponding to all the grids passed by the line segment are the search elements of step 0. For each search step, the line segment is translated along the search direction by a step length n. The grid passed by the translated line segment is the search element of the new step. For each search direction, the line segment must be translated until it completely leaves the area where the matrix is ​​located.

[0056] Furthermore, in step S66, when the search starts in each search direction, a historical search queue is first established. The maximum length of the queue is l_queue = l_Max / n, and it is rounded down to represent the number of search steps corresponding to the maximum walking step length of the legged robot. When a new value enters the end of the queue and the queue exceeds the maximum length, the value at the head of the queue will be deleted.

[0057] Furthermore, in step S66, a starting height is set as the 0th value of the queue, and the starting height is the opposite of the height of the legged robot from the ground when standing; for several grids searched in each step, if the height is greater than the maximum value in the historical search queue + h_Max, the grid is considered to be an obstacle, and the corresponding element of the obstacle information matrix is ​​recorded as 1; if the height is less than the maximum value in the historical search queue + h_Max, the grid is considered to be passable and not an obstacle. At this time, if the corresponding slope is less than s_Max, the grid is considered to be landable, and the corresponding element of the obstacle information matrix is ​​recorded as 0; if the corresponding slope is greater than s_Max, the grid is considered to be passable but not landable, and the corresponding element of the obstacle information matrix is ​​recorded as 0.1; each time a search step is completed, the maximum height of all landable grids searched in this step is added to the end of the historical search queue.

[0058] The beneficial effect of the present invention is that it does not rely heavily on a high-precision odometer, can directly observe the terrain below the body of the legged robot and the surrounding terrain, and has more accurate perception, making it easier for the legged robot to plan a feasible path with relatively little computational effort. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] FIG1 is a simplified diagram of step S42 of the present invention.

[0060] FIG2 is a schematic diagram showing the effect of step S4 of the present invention.

[0061] FIG3 is a schematic diagram showing the effect of step S5 of the present invention.

[0062] FIG. 4 is a first diagram showing the effect of step S6 of the present invention.

[0063] FIG5 is a second diagram showing the effect of step S6 of the present invention.

[0064] FIG6 is a first diagram showing the effect of the present invention in actual application scenarios.

[0065] FIG7 is a second diagram showing the effect of the present invention in actual application scenarios. DETAILED DESCRIPTION

[0066] In order to enable those skilled in the art to better understand the solutions of the present invention, the following will provide a clear and complete description of the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts should fall within the scope of protection of the present invention.

[0067] A method for local terrain perception of a legged robot comprises the following steps:

[0068] S1. One or more laser radars are fixedly installed on the legged robot. The external parameters of the laser radar relative to the legged robot's main body coordinate system are obtained in advance. The point cloud obtained by the laser radar scan is converted to the legged robot's main body coordinate system. The scanned point cloud is regularly updated. That is, the scanned point cloud is input into the algorithm module at a certain period. If there are multiple laser radars, the point clouds obtained by each laser radar during this period are merged and then input into the algorithm module.

[0069] Specifically, step S1 includes the following sub-steps:

[0070] S11. Install at least one laser radar on the legged robot so that the area around the legged robot and the area below the torso are within the scanning range. In this embodiment, several laser radars for terrain point cloud scanning are installed on the same legged robot, and the point cloud data is received by the same processor. The laser radars are selected so that each scanning point has a timestamp, and the laser radars are reasonably arranged and installed so that the scanning areas around the robot and below the torso are complementary.

[0071] S12, define the coordinate system of the legged robot body. The origin of the coordinate system is any position of the legged robot, where the x-axis is facing the front of the legged robot, the y-axis is facing the left of the legged robot, and the z-axis is facing vertically upwards of the legged robot. The external parameters include the rotation matrix and displacement vector b represents the coordinate system of the legged robot body, li represents the i-th laser radar; the external parameters can be obtained through calibration, or more accurate mechanical design parameters can be directly used;

[0072] S13, after receiving the point cloud data, convert the point cloud from the lidar coordinate system to the legged robot body coordinate system using the following formula:

[0073] in is the coordinate of the measurement point of the i-th laser radar in the radar coordinate system, X b 、Y b , Z b The coordinates of the measurement point after conversion to the legged robot body coordinate system;

[0074] S14, set the period T for collecting point clouds. Every T time, merge the point clouds scanned by the laser radar into the same point cloud after being processed in step S13.

[0075] S2, removing motion distortion from the point cloud obtained in step S1, and converting it to the gravity coordinate system of the legged robot, retaining the point cloud within the set range;

[0076] Specifically, step S2 includes the following sub-steps:

[0077] S21, a 9-axis IMU (gyroscope) is installed on the legged robot, and the lidar is connected to a processor that can obtain the IMU attitude angle. The processor can obtain the three attitude angles of the IMU, and the installation posture of the IMU is consistent with the legged robot specimen system, so that the IMU can represent the posture of the quadruped robot;

[0078] S22, combining joint motor encoders and IMU to estimate the displacement of the legged robot;

[0079] S23, input the IMU attitude data and the legged robot displacement odometer data with a frequency of not less than 100 Hz into the point cloud processing module, and remove the motion distortion of the point cloud by combining the timestamp of each point in the point cloud so that the timestamp of each point after processing is the time of the last point in the point cloud:

[0080] where X b_trans 、Y b_trans , Z b_trans The coordinates of the point after removing motion distortion in the legged robot body coordinate system are: They represent the rotation matrix and displacement vector of the legged robot body coordinate system at the timestamp of the j-th point relative to the legged robot body coordinate system at the timestamp of the end of the point cloud;

[0081] S24, define a gravity coordinate system for the legged robot, whose origin coincides with the origin of the coordinate system of the legged robot body, whose z-axis is opposite to the direction of gravity, whose x-axis is perpendicular to the direction of gravity and is in the same vertical plane as the x-axis of the coordinate system of the legged robot body, whose y-axis is perpendicular to the x-axis and z-axis respectively, and whose directions satisfy the right-hand rule;

[0082] S25, based on the IMU posture corresponding to the timestamp at the end of the point cloud, the point cloud after removing the motion distortion in step S23 is converted from the legged robot body coordinate system to the legged robot gravity coordinate system:

[0083] where X g 、Y g , Z g To convert the coordinates to the quadruped robot gravity coordinate system, is the rotation matrix, which is obtained as follows:

[0084] Convert the IMU attitude Euler angle corresponding to the timestamp at the end of the point cloud into a rotation matrix, which is Set the heading angle in the Euler angle to zero, keep the pitch angle and roll angle unchanged, and then convert it into a rotation matrix, which is

[0085] S26 , performing through filtering on the point cloud obtained in step S25 , intercepting the point cloud within the target range in the x, y, and z directions of the quadruped robot's gravity coordinate system and retaining the point cloud, and discarding the point cloud outside the range.

[0086] S3, reducing the dimension of the point cloud obtained in step S2 into a height information matrix;

[0087] Specifically, step S3 includes the following sub-steps:

[0088] S31, defining a matrix for storing terrain height information. Define an empty matrix with m rows and m columns to store the terrain height information around the quadruped robot. The center of the matrix corresponds to the origin of the quadruped robot's gravity coordinate system. Each element of the matrix represents an n cm × n cm square, where the row represents the x-axis direction and the column represents the y-axis direction.

[0089] S32, reduce the dimensionality of the three-dimensional point cloud into a height information matrix, traverse the point cloud obtained in step S2, and determine the element to which the point belongs in the matrix based on the x and y coordinates of each point. If the element is empty, assign the z coordinate of the current point as the height to the element. If the element has been assigned, compare the element value and the z coordinate of the current point. If the element value is smaller, update the element value to the z coordinate of the current point.

[0090] S4, performing hole filling based on the height information matrix obtained in step S3;

[0091] Specifically, step S4 includes the following sub-steps:

[0092] S41, looking for empty elements in the height matrix, using the height of the element at the hole boundary as the candidate height for filling the hole, traversing the height information matrix obtained in step S3, for an element with its own height value, if its front, back, left, or right neighbors have elements with empty height values, the current element is considered to be a hole edge element, and each element is visited one by one from the hole edge element toward the empty neighbor until an element with a non-empty height value is visited, or the number of visits reaches a preset upper limit, or the edge of the matrix is ​​visited. If an element with a non-empty height value is visited, the height difference is compared with the current hole edge element. If the absolute value of the height difference is less than a preset threshold, the smaller height value is recorded in a hole record container as the candidate height of the empty element, and the row and column coordinates of all empty elements in the process of visiting one by one are recorded;

[0093] S42: Select a suitable height from multiple candidate heights. The coordinates of empty elements in the empty record container may be repeated. The coordinates of empty elements in all containers are traversed and the element is accessed in the height information matrix. If it is still empty, the candidate height is assigned to the element. If it has been assigned a height value in the previous record, the current height value is compared with the new candidate value and the smaller height is selected.

[0094] As shown in Figure 1, let x be the row and y be the column. The elements (2,3), (3,6), and (3,7) are empty. There are 8 on both sides of (2,3), so the candidate height is 8. There are 7 and 8 on the top and bottom. Assuming the height difference threshold is 3, the height difference between 7 and 8 is within the threshold, so the candidate height is 7. The smaller of the two candidate heights is used to fill the holes. For the holes of the elements (3,6) and (3,7), the left and right sides are 8 and 9, which are within the height difference threshold, so the candidate height is 8. The height difference between the top and bottom is 6 and 4, which exceeds the threshold and no candidate height is generated, so the only candidate height is 8.

[0095] S43, repeating steps S41 and S42 several times until there are no new holes to be filled or the preset upper limit is reached.

[0096] Step S4 fills the height information matrix with a large number of holes obtained in step S3. The effect is shown in FIG2 . The holes are filled, and the height of the matrix now includes the terrain information and the protrusions caused by the legs of the legged robot.

[0097] S5, based on the height information matrix obtained in step S4, removing the influence of the leg point cloud of the leg robot on the height matrix;

[0098] Specifically, step S5 includes the following sub-steps:

[0099] S51, setting the range of the leg of the leg robot and the erosion size, setting a row and column range for the height information matrix according to the size of the leg robot, which can cover the matrix elements of the updated point cloud of the leg of the leg robot, with the number of rows ranging from r1 to r2 and the number of columns ranging from c1 to c2, and setting an erosion size s according to the size of the leg;

[0100] S52, eroding the convex height within the set range to erode the terrain convexity caused by the leg point cloud, copying the height information matrix to a temporary matrix, and updating each element of the temporary matrix from rows r1-s to r2+s and columns c1-s to c2+s to the minimum height value within the range of -s to s rows and -s to s columns around the current element, thereby removing the abnormal convexity of certain matrix elements caused by the leg point cloud;

[0101] S53: The erosion process will also cause the normal terrain to change. The normal terrain is then dilated to compensate for the change. Each element in the temporary matrix from rows r1-s to r2+s and columns c1-s to c2+s is updated to the maximum height value within the range from rows -s to s and columns -s to s around the current element, thereby compensating for the change in the normal terrain caused by step S52.

[0102] S54, updating the height information matrix, and copying the area with the number of rows ranging from r1 to r2 and the number of columns ranging from c1 to c2 in the temporary matrix to the original height information matrix.

[0103] In step S5, the height matrix including the terrain and leg protrusions is processed into a height matrix retaining only the terrain information. The effects of the erosion and dilation processes are shown in FIG3 .

[0104] S6, analyzing the height information matrix processed in step S5, calculating the slope information matrix and the obstacle information matrix for the foothold planning and obstacle avoidance of the legged robot.

[0105] The slope information matrix can reflect the inclination of the terrain at the corresponding position of a grid, preventing the robot from stepping on the edge of stairs and other unsuitable places for landing. The obstacle information matrix can help the robot plan a feasible path.

[0106] Specifically, step S6 includes the following sub-steps:

[0107] S61, calculate the slope component of each element in the matrix in the x direction, define an m×m x direction slope information matrix, traverse the height information matrix obtained in step 5, and calculate the x direction slope component for each element with a non-empty height value. x :

[0108] Where n represents the width of the square grid area corresponding to an element, h(i, j) is the height value of the element in the i-th row and j-th column. When h(i-1, j) does not exist or the height value is empty, and h(i+1, j) has a height value, slope x =slope x2 , when h(i+1, j) does not exist or the height value is empty, and h(i-1, j) has a height value, slope x =slope x1 , h(i-1, j) and h(i+1, j) do not exist or the height value is empty, slope x =0;

[0109] S62, calculate the slope component of each element in the matrix in the y direction, define an m×m y-direction slope information matrix, traverse the height information matrix obtained in step 5, and calculate the slope component of the y direction for each element with a non-empty height value. y :

[0110] Where n represents the width of the square grid area corresponding to an element, h(i, j) is the value of the element in the i-th row and j-th column.

[0111] The height value of the pixel. When h(i, j-1) does not exist or the height value is empty, and h(i, j+1) has a height value,

[0112] slope y =slope y2 , when h(i, j+1) does not exist or the height value is empty, and h(i, j-1) has a height value,

[0113] slope y =slope y1 , h(i, j-1) and h(i, j+1) do not exist or the height value is empty, slope y =0;

[0114] S63, calculate the actual slope of each element in the matrix, define an m×m actual slope information matrix, and update it with the slope information matrices in the x-direction and y-direction:

[0115] Where slope(i, j) is the slope value of the element in the i-th row and j-th column of the actual slope information matrix. x (i, j) and slope y (i, j) are the slope values ​​of the elements in the i-th row and j-th column of the x-direction slope matrix and the y-direction slope matrix respectively;

[0116] S64, define an m×m obstacle information matrix, presetting the maximum walking step length of the legged robot to l_Max, the maximum landing height to h_Max, and the maximum landing slope to s_Max:

[0117] S65, offline calculation of the matrix element access order during the obstacle analysis process; in this step S65, starting from the center of the matrix, multiple search directions are preset, such as 16 search directions, covering a 360° range. The adjacent search directions of the 16 search directions differ by 22.5°. Based on the matrix size, the search width of each search direction is preset to w, and the search step length is n, which is consistent with the grid width corresponding to an element. When searching each direction, a line segment with a midpoint at the center of the matrix and a width of w and perpendicular to the search direction is first obtained. The matrix elements corresponding to all grids passed by the line segment are the search elements of step 0. For each search step, the line segment is translated along the search direction by a step length n. The grids passed by the translated line segment are the search elements of the next step. For each search direction, the line segment is translated until it completely leaves the area where the matrix is ​​located. Since the calculation in this step is repetitive and the search order is the same each time, the grid search order is saved offline after the first calculation to reduce the amount of calculation and increase real-time performance.

[0118] S66, according to the search order obtained in step S65, access the corresponding elements of the height information matrix and the slope information matrix in sequence, and analyze whether the grid where each element is located is an obstacle;

[0119] In step S66, at the beginning of each search direction, a historical search queue is first established. The maximum length of the queue is l_queue = l_Max / n, rounded down to the nearest integer, representing the number of search steps corresponding to the maximum walking step length of the legged robot. When a new value enters the end of the queue and the queue exceeds the maximum length, the value at the head of the queue is deleted.

[0120] In step S66, a starting height is set as the zeroth value in the queue. The starting height is the inverse of the legged robot's height from the ground when standing. For each grid searched in each step, if the height > the maximum value in the historical search queue + h_Max, the grid is considered an obstacle, and the corresponding element in the obstacle information matrix is ​​recorded as 1. If the height <= the maximum value in the historical search queue + h_Max, the grid is considered passable and not an obstacle. In this case, if the corresponding slope is less than s_Max, the grid is considered landable, and the corresponding element in the obstacle information matrix is ​​recorded as 0. If the corresponding slope is greater than s_Max, the grid is considered passable but not landable, and the corresponding element in the obstacle information matrix is ​​recorded as 0.1. After each search step, the maximum height of all landable grids searched in this step is added to the end of the historical search queue. Because grids searched in different search directions may partially overlap, when the elements of the obstacle information matrix are not assigned for the first time, the old and new values ​​must be compared. Only when the new value is less than the old value can the value be updated.

[0121] Steps S61-S63 obtain the slope of the corresponding position of each element, and the effect is shown in Figure 4. The darker the color, the greater the slope; Steps S64-S66 obtain whether there is an obstacle at the corresponding position of each element. Black represents an obstacle. When there is no obstacle and when there is an obstacle on the stairs, the effects are shown in Figure 5 respectively.

[0122] The effects of the present invention in actual scene applications are shown in Figures 6 and 7. The legged robot can perform obstacle avoidance and landing point planning based on the height information matrix, slope information matrix, and obstacle information matrix.

[0123] The above specific embodiments are used to illustrate the present invention rather than to limit the present invention. Any modifications and changes made to the present invention within the spirit of the present invention and the protection scope of the claims shall fall within the protection scope of the present invention.

Claims

1. A method for local terrain perception of a legged robot. It is characterized in that The following steps are involved: S1, setting at least one laser radar on the legged robot, and obtaining the external parameters of the laser radar relative to the legged robot body coordinate system, converting the point cloud obtained by scanning the laser radar to the legged robot body coordinate system, and regularly updating the scanned point cloud; S2, removing motion distortion from the point cloud obtained in step S1, and converting it to the gravity coordinate system of the legged robot, retaining the point cloud within a set range; S3, reducing the dimension of the point cloud obtained in step S2 into a height information matrix; S4, performing hole completion based on the height information matrix obtained in step S3; S5, based on the height information matrix obtained in step S4, removing the influence of the leg point cloud of the leg robot on the height matrix; S6, analyzing the height information matrix processed in step S5, calculating the slope information matrix and the obstacle information matrix for foothold planning and obstacle avoidance of the legged robot.

2. The method for sensing local terrain of a legged robot according to claim 1, It is characterized in that The step S1 includes the following sub-steps: S11, setting at least one laser radar on the legged robot so that the surrounding area and the lower part of the torso of the legged robot are within the scanning range; S12, define the coordinate system of the legged robot body, the origin of the coordinate system is any position of the legged robot, where the x-axis is directly in front of the legged robot, the y-axis is directly to the left of the legged robot, and the z-axis is vertically upward of the legged robot. The external parameters include the rotation matrix and displacement vector b represents the body coordinate system of the legged robot, l i represents the i-th laser radar; S13, after receiving the point cloud data, the point cloud is converted from the laser radar coordinate system to the legged robot body coordinate system by the following formula: in is the coordinate of the measurement point of the i-th laser radar in the radar coordinate system, X b , Y b , Z b The coordinates of the measurement point after being converted to the legged robot body coordinate system; S14, setting a period T for collecting point clouds, and merging the point clouds scanned by the laser radar into the same point cloud after being processed in step S13 every T time.

3. The method for local terrain perception of a legged robot according to claim 1, It is characterized in that The step S2 includes the following sub-steps: S21, an IMU is installed on the legged robot, the laser radar is connected to a processor that can obtain the IMU attitude angle, and the installation attitude of the IMU is consistent with the legged robot specimen system, so that the IMU is used to represent the attitude of the quadruped robot; S22, combining joint motor encoders and IMU to estimate the displacement of the legged robot; S23, inputting the IMU attitude data and the foot-type robot displacement odometer data with a frequency of not less than 100 Hz into the point cloud processing module, combining the timestamp of each point in the point cloud, removing the motion distortion of the point cloud, so that the timestamp of each point after processing is the time of the end point of the point cloud: Where X b_trans , Y b_trans , Z b_trans The coordinates of the point after removing motion distortion in the legged robot body coordinate system are: They represent the rotation matrix and displacement vector of the legged robot body coordinate system at the j-th point timestamp relative to the legged robot body coordinate system at the end timestamp of the point cloud; S24, define a gravity coordinate system of the legged robot, whose origin coincides with the origin of the coordinate system of the legged robot body, the z-axis is opposite to the direction of gravity, the x-axis is perpendicular to the direction of gravity and is in the same vertical plane as the x-axis of the coordinate system of the legged robot body, the y-axis is perpendicular to the x-axis and the z-axis respectively, and the direction satisfies the right-hand rule; S25, according to the IMU posture corresponding to the timestamp at the end of the point cloud, the point cloud after removing the motion distortion in step S23 is converted from the legged robot body coordinate system to the legged robot gravity coordinate system: Where X g , Y g , Z g To convert the coordinates to the gravity coordinate system of the quadruped robot, is the rotation matrix, which is obtained as follows: Convert the IMU attitude Euler angle corresponding to the timestamp at the end of the point cloud into a rotation matrix, which is Set the heading angle in the Euler angle to zero, keep the pitch angle and roll angle unchanged, and then convert it into a rotation matrix, which is S26, performing through filtering on the point cloud obtained in step S25, intercepting the point cloud within the target range in the x, y, and z directions of the gravity coordinate system of the quadruped robot and retaining the point cloud, and discarding the point cloud outside the range.

4. The method for sensing local terrain of a legged robot according to claim 1, It is characterized in that The step S3 includes the following sub-steps: S31, defining a matrix for storing terrain height information, defining an empty matrix with m rows and m columns, storing the terrain height information around the quadruped robot, the center of the matrix corresponds to the origin of the gravity coordinate system of the quadruped robot, each element of the matrix represents a square of n cm × n cm, wherein the row represents the direction of the x-coordinate axis, and the column represents the direction of the y-coordinate axis; S32, reduce the dimensionality of the three-dimensional point cloud into a height information matrix, traverse the point cloud obtained in step S2, and determine the element to which the point belongs in the matrix according to the x and y coordinates of each point. If the element is empty, assign the z coordinate of the current point as the height to the element. If the element has been assigned, compare the element value and the z coordinate of the current point. If the element value is smaller, update the element value to the z coordinate of the current point.

5. The method for sensing local terrain of a legged robot according to claim 1, It is characterized in that The step S4 includes the following sub-steps: S41, find the empty elements of the height matrix, use the element height of the hole boundary as the candidate height for filling the hole, traverse the height information matrix obtained in step S3, for the element with its own height value, if its front, back, left, and right neighbors have elements with empty height values, then the current element is considered to be a hole edge element, and each element is visited one by one from the hole edge element to the empty neighbor direction until an element with a non-empty height value is visited, or the number of visits reaches a preset upper limit, or the edge of the matrix has been visited. If an element with a non-empty height value is visited, compare the height difference with the current hole edge element. If the absolute value of the height difference is less than the preset threshold, record the smaller height value in a hole record container as the candidate height of the empty element, and record the row and column coordinates of all empty elements in the process of visiting one by one; S42, select a suitable height from multiple candidate heights. The coordinates of empty elements in the empty record container may be repeated. The coordinates of empty elements in all containers are traversed, and the element is accessed in the height information matrix. If it is still empty, the corresponding candidate height is assigned to the element. If the element has been assigned a height value by the previous record, the current height value is compared with the new candidate value, and the smaller height is selected; S43, repeating steps S41 and S42 until there is no new hole to be filled or the preset upper limit number of times is reached.

6. The method for sensing local terrain of a legged robot according to claim 1, It is characterized in that The step S5 includes the following sub-steps: S51, setting the range of the leg of the leg robot and the corrosion size, setting a row and column range for the height information matrix according to the size of the leg robot, which can cover the matrix elements of the leg robot leg point cloud update, and the row number range For r 1 to r 2 , the column number range is c 1 to c 2 , and set a corrosion size s according to the leg size; S52, corrode the convex height within the set range, corrode the terrain convexity caused by the leg point cloud, copy the height information matrix to a temporary matrix, and convert the temporary matrix r 1 -s to r 2 +s line, c 1 -s to c 2 + Each element in the s column is updated to the minimum height value in the range of -s to s rows and -s to s columns around the current element, thereby removing the abnormal protrusions of some matrix elements caused by the leg point cloud; S53, perform expansion compensation on the normal terrain, and convert the temporary matrix r 1 -s to r 2 +s line, c 1 -s to c 2 Each element in the +s column is updated to the maximum height value in the range of -s to s rows and -s to s columns around the current element, thereby compensating for the change in the normal terrain caused by step S52; S54, update the height information matrix, and set the number of rows in the temporary matrix to r 1 to r 2 , the column number range is c 1 to c 2 The area is copied to the original height information matrix.

7. The method for sensing local terrain of a legged robot according to claim 1, It is characterized in that The step S6 comprises the following sub-steps: S61, calculate the slope component of each element in the matrix in the x direction, define an m×m x direction slope information matrix, traverse the height information matrix obtained in step 5, and calculate the slope component slope in the x direction for each element with a non-empty height value x : Where n represents the width of the square grid area corresponding to an element, h(i, j) is the height value of the element in the i-th row and j-th column. When h(i-1, j) does not exist or the height value is empty, and h(i+1, j) has a height value, slope x =slope x2 , when h(i+1, j) does not exist or the height value is empty, and h(i-1, j) has a height value, slope x =slope x1 , h(i-1, j) and h(i+1, j) do not exist or the height value is empty, slope x =0; S62, calculate the slope component of each element in the matrix in the y direction, define an m×m y direction slope information matrix, traverse the height information matrix obtained in step 5, and calculate the slope component slope in the y direction for each element with a non-empty height value y : Where n represents the width of the square grid area corresponding to an element, h(i, j) is the height value of the element in the i-th row and j-th column. When h(i, j-1) does not exist or the height value is empty, and h(i, j+1) has a height value, slope y =slope y2 , when h(i, j+1) does not exist or the height value is empty, and h(i, j-1) has a height value, slope y =slope y1 , h(i, j-1) and h(i, j+1) do not exist or the height value is empty, slope y =0; S63, calculate the actual slope of each element in the matrix, define an m×m actual slope information matrix, and update it with the slope information matrices in the x-direction and the y-direction: Where slope(i, j) is the slope value of the element in the i-th row and j-th column of the actual slope information matrix. x (i, j) and slope y (i, j) are the slope values ​​of the elements in the i-th row and j-th column of the x-direction slope matrix and the y-direction slope matrix respectively; S64, define an m×m obstacle information matrix, preset the maximum walking step length of the legged robot to be l_Max, the maximum landing height to be h_Max, and the maximum landing slope to be s_Max: S65, matrix element access order of offline obstacle analysis process; S66, according to the search order obtained in step S65, the corresponding elements of the height information matrix and the slope information matrix are accessed in sequence to analyze whether the grid where each element is located is an obstacle.

8. The method for sensing local terrain of a legged robot according to claim 7, Features: In step S65, multiple search directions are preset starting from the center of the matrix, and the multiple search directions cover a range of 360°. According to the matrix size, the search width of each search direction is preset to w, and the search step length is n, which is consistent with the grid width corresponding to an element. When searching each direction, a line segment with a midpoint at the center of the matrix, a width of w, and perpendicular to the search direction is first obtained. The matrix elements corresponding to all the grids passed by the line segment are the search elements of step 0. For each search step, the line segment is translated along the search direction by a step length of n, and the grid passed by the translated line segment is the search element of the new step. For each search direction, the line segment must be translated until it completely leaves the area where the matrix is ​​located.

9. The method for sensing local terrain of a legged robot according to claim 7, Features: In step S66, when starting the search in each search direction, a historical search queue is first established. The maximum length of the queue is l_queue=l_Max / n, and it is rounded down to represent the number of search steps corresponding to the maximum walking step length of the legged robot. When a new value enters the end of the queue and the queue exceeds the maximum length, the value at the head of the queue will be deleted.

10. The method for sensing local terrain of a legged robot according to claim 9, Features: In the step S66, a starting height is set as the 0th value of the queue, and the starting height is the opposite of the height of the legged robot from the ground when it is standing; for a number of grids searched in each step, if the height> the maximum value in the historical search queue+h_Max, the grid is considered to be an obstacle, and the corresponding element of the obstacle information matrix is ​​recorded as 1; if the height<=the maximum value of the historical search queue+h_Max, the grid is considered to be passable and not an obstacle. At this time, if the corresponding slope is less than s_Max, the grid is considered to be landable, and the corresponding element of the obstacle information matrix is ​​recorded as 0; if the corresponding slope is greater than s_Max, the grid is considered to be passable but not landable, and the corresponding element of the obstacle information matrix is ​​recorded as 0.1; each time a search step is completed, the maximum height of all landable grids searched in this step is added to the end of the historical search queue.

Images