A local terrain perception method for a legged robot

Abstract

The application discloses a kind of local terrain perception methods of legged robot, comprising the following steps: S1, at least one laser radar is arranged on the legged robot, and the external parameter of its relative legged robot body coordinate system is obtained, and the point cloud obtained by laser radar scanning is converted to the legged robot body coordinate system;S2, to point cloud remove motion distortion, and convert to the legged robot gravity coordinate system, retain the point cloud in the set range;S3, the point cloud is reduced to height information matrix;S4, based on height information matrix carries out hole completion;S5, based on height information matrix removes the influence caused by the legged robot leg point cloud to height matrix;S6, to the height information matrix after processing is analyzed, calculates slope information matrix and obstacle information matrix, for the landing point planning of legged robot and obstacle avoidance.The application is low in degree of dependence on high-precision odometer, can directly observe the terrain below and around the body of legged robot, and perception is more accurate.

Description

Technical Field

[0001] This invention belongs to the field of legged robot technology, and in particular relates to a method for local terrain perception of legged robots. Background Technology

[0002] Legged robots need to perceive terrain information below and around their bodies to plan footholds and avoid obstacles in non-flat environments such as stairs. Due to the limited field of view of point cloud sensors and interference from the legs, directly observing the terrain below the body is not easy. Currently, leading legged robot manufacturers worldwide, such as Boston Dynamics and AnyBotics, use a method that relies on odometry to accumulate and stitch terrain data. This means the robot's point cloud sensors do not directly observe the terrain below the body, but only the terrain in front of or around it. The robot relies on high-precision laser / visual odometry to estimate its displacement and attitude angles in real time, stitching together a complete terrain from multiple frames of historical point cloud data during movement. In other words, the terrain currently below the body is the one scanned before reaching this position. This approach has the following drawbacks: 1) High computational cost; 2) High accuracy requirements for the odometry; 3) Odometry may fail in certain scenarios, such as laser odometry failing in overly open, narrow, or sparsely structured environments, and visual odometry failing in dark or textureless environments. Summary of the Invention

[0003] To overcome the shortcomings of existing technologies, this invention provides a local terrain perception method for legged robots, which does not rely on high-precision odometry but directly observes the terrain below and around the body, resulting in more accurate perception.

[0004] The technical solution adopted by this invention to solve its technical problem is: a method for local terrain perception of a legged robot, comprising the following steps:

[0005] S1. Set up at least one lidar on the legged robot and obtain the external parameters of the lidar relative to the body coordinate system of the legged robot. Transform the point cloud obtained by the lidar scanning to the body coordinate system of the legged robot and update the point cloud obtained by scanning periodically.

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

[0007] S3, reduce the dimensionality of the point cloud obtained in step S2 into a height information matrix;

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

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

[0010] S6. Analyze the height information matrix processed in step S5, calculate the slope information matrix and obstacle information matrix, and use them for foothold planning and obstacle avoidance of the legged robot.

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

[0012] S11, At least one lidar is installed on the legged robot so that the area around the legged robot and the area below its torso are within the scanning range.

[0013] S12 defines the coordinate system of the legged robot, with the origin at any position on the robot. The x-axis points directly in front of the robot, the y-axis points directly to the left, and the z-axis points vertically upwards. The extrinsic parameters include the rotation matrix. and displacement vector b represents the body coordinate system of the legged robot, l i This represents the i-th lidar;

[0014] S13, After receiving the point cloud data, the point cloud is transformed from the lidar coordinate system to the legged robot's body coordinate system using the following formula:

[0015]

[0016] in Let X be the coordinates of the measurement point of the i-th lidar in the lidar coordinate system. b Y b Z b The coordinates of the measurement point after transformation to the body coordinate system of the legged robot;

[0017] S14, set the point cloud collection period T, and every T time, merge the point cloud scanned by the lidar into the same point cloud after processing in step S13.

[0018] Furthermore,

[0019] Step S2 includes the following sub-steps:

[0020] S21, An IMU is installed on the legged robot, and a LiDAR is connected to a processor that can acquire the attitude angle of the IMU. The installation attitude of the IMU is consistent with that of the legged robot specimen system, so that the IMU can represent the attitude of the quadruped robot.

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

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

[0023]

[0024] Where X b_trans Y b_trans Z b_trans To remove motion distortion from the coordinates of a point in the body coordinate system of the legged robot, Let represent the rotation matrix and displacement vector of the legged robot's body coordinate system at the j-th point timestamp relative to the legged robot's body coordinate system at the end timestamp of the point cloud, respectively.

[0025] S24. Define the gravity coordinate system of the legged robot. Its origin coincides with the origin of the body coordinate system of the legged robot. 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 body coordinate system of the legged robot, and the y-axis is perpendicular to the x-axis and z-axis respectively. The direction satisfies the right-hand rule.

[0026] S25, based on the IMU pose corresponding to the end timestamp of the point cloud, transform the point cloud after removing motion distortion in step S23 from the legged robot's body coordinate system to the legged robot's gravity coordinate system:

[0027]

[0028] Where X g Y g Z g To transform the coordinates to the quadruped robot's gravity coordinate system, The rotation matrix is ​​obtained as follows:

[0029]

[0030] Convert the IMU attitude Euler angles corresponding to the last timestamp of the point cloud into a rotation matrix, which is: Set the heading angle to zero in Euler angles, while keeping the pitch and roll angles unchanged, and then convert it to a rotation matrix, which is:

[0031] S26. Perform pass-through filtering on the point cloud obtained in step S25. Select and retain the point cloud within the target range in the x, y, and z directions of the quadruped robot's gravity coordinate system, and discard the point cloud outside the range.

[0032] Since the point cloud data 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 gravity coordinate system converted from the point cloud can ensure that the tilt angle of the ground and slopes is the same as the actual tilt angle when the robot tilts. Only retaining the point cloud within a set range can reduce the amount of computation.

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

[0034] S31, define a matrix for storing terrain height information. Define an m-row m-column empty matrix 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.

[0035] S32, reduce the 3D point cloud to a height information matrix, traverse the point cloud obtained in step S2, 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 already been assigned a value, compare the element value with 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.

[0036] Point cloud dimensionality reduction into a height information matrix reduces three-dimensional information to two dimensions, which can reduce the amount of data and transform unordered data into ordered data, making it easier for subsequent processing.

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

[0038] S41, find empty elements in the height matrix. Use the height of the element at the hole boundary as the candidate height for hole filling. Traverse the height information matrix obtained in step S3. For an 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 a hole edge element. Visit each element one by one from the hole edge element to the direction of the empty neighbor until an element with a non-empty height value is visited, or the number of visits reaches the 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 recording container as a candidate height for empty elements, and record the row and column coordinates of all empty elements in the process of visiting them one by one.

[0039] S42, select a suitable height from multiple candidate heights. The coordinates of empty elements in the empty record container may be repeated. Traverse the coordinates of empty elements in all containers and access the element in the height information matrix. If it is still empty, assign the corresponding candidate height to the element. If it has been assigned a height value by a previous record, compare the current height value with the new candidate value and take the smaller height.

[0040] S43. Repeat steps S41 and S42 until no new holes are filled, or the preset maximum number of times is reached.

[0041] Because the lidar point cloud is not dense enough, some cells in the height information matrix do not have corresponding point clouds. The height of the empty cells is filled by combining the height values ​​of the surrounding cells, in order to facilitate subsequent processing.

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

[0043] S51, set the range of the leg of the legged robot and the erosion size. Set a row and column range for the height information matrix according to the size of the legged robot, which can cover the matrix elements of the leg point cloud update of the legged robot. The number of rows is from r1 to r2 and the number of columns is from c1 to c2. Set an erosion size s according to the leg size.

[0044] S52, within the set range, perform erosion on the bulge height to erode the terrain bulge caused by the leg point cloud, copy the height information matrix to a temporary matrix, and update each element of the temporary matrix from row r1-s to r2+s and column 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 bulge of some matrix elements caused by the leg point cloud;

[0045] S53, perform dilation compensation on normal terrain, update each element of the temporary matrix from row r1-s to r2+s and column c1-s to c2+s to the maximum height value within the range of -s to s rows and -s to s columns around the current element, thereby compensating for the changes to normal terrain caused by step S52.

[0046] S54, Update the height information matrix by copying the region in the temporary matrix with rows ranging from r1 to r2 and columns ranging from c1 to c2 into the original height information matrix.

[0047] The above steps effectively prevent the point cloud of the leg of the legged robot from being regarded as the terrain information below the legged robot, resulting in more accurate perception.

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

[0049] S61, calculate the slope component in the x-direction for each element in the matrix. Define an m×m x-direction slope information matrix, iterate through the height information matrix obtained in step 5, and calculate the x-direction slope component slopex for each element with a non-empty height value. ：

[0050]

[0051]

[0052]

[0053] Where n represents the width of the square grid area corresponding to an element, and 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 its height value is empty, and h(i+1,j) has a height value, slopex = slopex2. When h(i+1,j) does not exist or its height value is empty, and h(i-1,j) has a height value, slopex = slopex2. x =slope x1 If h(i-1, j) and h(i+1, j) do not exist or their height values ​​are empty, then the slope x =0;

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

[0055]

[0056] Where n represents the width of the square grid area corresponding to an element, and 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 its height value is empty, and when h(i, j+1) has a height value, the slope is... y =slopey2, when h(i,j+1) does not exist or its height value is empty, and h(i,j+1) has a height value, slopey = slope y1 If h(i, j-1) and h(i, j+1) do not exist or their height values ​​are empty, then the slope y =0;

[0057] S63, calculate the actual slope of each element in the matrix, define an m×m actual slope information matrix, and update it using the slope information matrices in the x and y directions:

[0058]

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

[0060] S64 defines an m×m obstacle information matrix, with the maximum walking stride length of the legged robot preset to l_Max, the maximum footing height to h_Max, and the maximum possible footing slope to s_Max:

[0061] S65, the order of accessing matrix elements in the offline obstacle analysis process;

[0062] S66. Following the search order obtained in step S65, sequentially access the corresponding elements of the height information matrix and the slope information matrix, and analyze whether the cell containing each element is an obstacle.

[0063] Furthermore, in step S65, starting from the center of the matrix, multiple search directions are preset, covering a 360° range. Based on the matrix size, the search width of each search direction is preset to w, and the search step size is n, which is consistent with the width of the cell corresponding to an element. When searching a direction, a line segment with its 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 cells passed by this line segment are the search elements of step 0. After each search step, the line segment is translated by a step size n along the search direction. The cells passed by the translated line segment are the search elements of the new step. Each search direction must translate the line segment until it completely leaves the area where the matrix is ​​located.

[0064] Furthermore, in step S66, when starting a search in each search direction, a historical search queue is first established. The maximum length of the queue is l_queue = l_Max / n, which is rounded down to the nearest integer. This represents the number of search steps corresponding to the maximum walking stride of the legged robot. When a new value enters at the end of the queue and the queue exceeds its maximum length, the value at the head of the queue is deleted.

[0065] Furthermore, in step S66, an initial height is set as the 0th value of the queue. The initial height is the negative of the height of the legged robot from the ground when it is standing. For each searched cell, if the height is greater than the maximum value in the historical search queue + h_Max, the cell is considered an obstacle, and the corresponding element of the obstacle information matrix is ​​recorded as 1. If the height is less than or equal to the maximum value in the historical search queue + h_Max, the cell is considered passable and not an obstacle. In this case, if the corresponding slope is less than s_Max, the cell is considered footable, and the corresponding element of the obstacle information matrix is ​​recorded as 0. If the corresponding slope is greater than s_Max, the cell is considered passable but not footable, and the corresponding element of the obstacle information matrix is ​​recorded as 0.1. After each search step, the maximum height of all footable cells found in this step is added to the end of the historical search queue.

[0066] The beneficial effects of this invention are that it does not rely heavily on high-precision odometers, can directly observe the terrain under the body and around the legged robot, has more accurate perception, facilitates the legged robot in planning feasible paths, and requires relatively less computation. Attached Figure Description

[0067] Figure 1 This is a simplified diagram of step S42 of the present invention.

[0068] Figure 2 This is a schematic diagram illustrating the effect of step S4 of the present invention.

[0069] Figure 3 This is a schematic diagram illustrating the effect of step S5 of the present invention.

[0070] Figure 4 This demonstrates the effect of step S6 of the present invention. Figure 1 .

[0071] Figure 5 This demonstrates the effect of step S6 of the present invention. Figure 2 .

[0072] Figure 6 To demonstrate the practical application effect of this invention Figure 1 .

[0073] Figure 7 To demonstrate the practical application effect of this invention Figure 2 . Detailed Implementation

[0074] To enable those skilled in the art to better understand the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0075] A method for local terrain perception in a legged robot includes the following steps:

[0076] S1. One or more lidars are fixedly installed on the legged robot. The external parameters of the lidars relative to the body coordinate system of the legged robot are obtained in advance. The point cloud obtained by the lidar scanning is converted to the body coordinate system of the legged robot. The point cloud obtained by scanning is updated periodically. That is, every certain period, the point cloud obtained by scanning is input into the algorithm module. If there are multiple lidars, the point clouds obtained by each lidar during this period are merged and then input into the algorithm module.

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

[0078] S11, At least one lidar is installed on the legged robot so that the area around the legged robot and the area under its torso are within the scanning range; In this embodiment, several lidars for terrain point cloud scanning are installed on the same legged robot, the point cloud data is received by the same processor, the selection of lidars needs to meet the requirement that each scanning point has a timestamp, and the lidars are reasonably arranged and installed so that the scanning areas around the robot and the area under its body are complementary.

[0079] S12 defines the coordinate system of the legged robot, with the origin at any position on the robot. The x-axis points directly in front of the robot, the y-axis points directly to the left, and the z-axis points vertically upwards. The extrinsic parameters include the rotation matrix. and displacement vector b represents the body coordinate system of the legged robot, l i This represents the i-th lidar; external parameters can be obtained through calibration or by directly using more precise mechanical design parameters.

[0080] S13, After receiving the point cloud data, the point cloud is transformed from the lidar coordinate system to the legged robot's body coordinate system using the following formula:

[0081]

[0082] in Let X be the coordinates of the measurement point of the i-th lidar in the lidar coordinate system. bY b Z b The coordinates of the measurement point after transformation to the body coordinate system of the legged robot;

[0083] S14, set the point cloud collection period T, and every T time, merge the point cloud scanned by the lidar into the same point cloud after processing in step S13.

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

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

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

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

[0088] S23, input IMU attitude data and legged robot displacement odometry data at a frequency of not less than 100Hz into the point cloud processing module, combine them with the timestamp of each point in the point cloud, remove motion distortion from the point cloud, so that the timestamp of each point after processing is the same as the time of the last point in the point cloud:

[0089]

[0090] Where X b_trans Y b_trans Z b_trans To remove motion distortion from the coordinates of a point in the body coordinate system of the legged robot, Let represent the rotation matrix and displacement vector of the legged robot's body coordinate system at the j-th point timestamp relative to the legged robot's body coordinate system at the end timestamp of the point cloud, respectively.

[0091] S24. Define the gravity coordinate system of the legged robot. Its origin coincides with the origin of the body coordinate system of the legged robot. 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 body coordinate system of the legged robot, and the y-axis is perpendicular to the x-axis and z-axis respectively. The direction satisfies the right-hand rule.

[0092] S25, based on the IMU pose corresponding to the end timestamp of the point cloud, transform the point cloud after removing motion distortion in step S23 from the legged robot's body coordinate system to the legged robot's gravity coordinate system:

[0093]

[0094] Where X g Y g Z g To transform the coordinates to the quadruped robot's gravity coordinate system, The rotation matrix is ​​obtained as follows:

[0095]

[0096] Convert the IMU attitude Euler angles corresponding to the last timestamp of the point cloud into a rotation matrix, which is: Set the heading angle to zero in Euler angles, while keeping the pitch and roll angles unchanged, and then convert it to a rotation matrix, which is:

[0097] S26. Perform pass-through filtering on the point cloud obtained in step S25. Select and retain the point cloud within the target range in the x, y, and z directions of the quadruped robot's gravity coordinate system, and discard the point cloud outside the range.

[0098] S3, reduce the dimensionality of the point cloud obtained in step S2 into a height information matrix;

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

[0100] S31, define a matrix for storing terrain height information. Define an m-row m-column empty matrix 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.

[0101] S32, reduce the 3D point cloud to a height information matrix, traverse the point cloud obtained in step S2, 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 already been assigned a value, compare the element value with 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.

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

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

[0104] S41, find empty elements in the height matrix. Use the height of the element at the hole boundary as the candidate height for hole filling. Traverse the height information matrix obtained in step S3. For an 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 a hole edge element. Visit each element one by one from the hole edge element to the direction of the empty neighbor until an element with a non-empty height value is visited, or the number of visits reaches the 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 recording container as a candidate height for empty elements, and record the row and column coordinates of all empty elements in the process of visiting them one by one.

[0105] S42, select a suitable height from multiple candidate heights. The coordinates of empty elements in the empty record container may be repeated. Traverse the coordinates of empty elements in all containers and access the element in the height information matrix. If it is still empty, assign the corresponding candidate height to the element. If it has been assigned a height value by a previous record, compare the current height value with the new candidate value and take the smaller height.

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

[0107] S43. Repeat steps S41 and S42 several times until there are no more newly filled holes, or the preset maximum number of times is reached.

[0108] Step S4 fills in the height information matrix with numerous holes obtained in step S3, with the following effect: Figure 2 As shown, the void is filled, and the height of the matrix now includes terrain information and the protrusions caused by the legged robot.

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

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

[0111] S51, set the range of the leg of the legged robot and the erosion size. Set a row and column range for the height information matrix according to the size of the legged robot, which can cover the matrix elements of the leg point cloud update of the legged robot. The number of rows is from r1 to r2 and the number of columns is from c1 to c2. Set an erosion size s according to the leg size.

[0112] S52, within the set range, perform erosion on the bulge height to erode the terrain bulge caused by the leg point cloud, copy the height information matrix to a temporary matrix, and update each element of the temporary matrix from row r1-s to r2+s and column 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 bulge of some matrix elements caused by the leg point cloud;

[0113] S53, the erosion process will also change the normal terrain. To compensate for the change in normal terrain, the elements of the temporary matrix from row r1-s to r2+s and column c1-s to c2+s are updated to the maximum height value within the range of row -s to s and column -s to s around the current element, thereby compensating for the change in normal terrain caused by step S52.

[0114] S54, Update the height information matrix by copying the region in the temporary matrix with rows ranging from r1 to c2 and columns ranging from c1 to c2 into the original height information matrix.

[0115] Step S5 processes the height matrix containing terrain and leg protrusions into a height matrix that retains only terrain information. The effects of the erosion and dilation processes are as follows: Figure 3 As shown.

[0116] S6. Analyze the height information matrix processed in step S5, calculate the slope information matrix and obstacle information matrix, and use them for foothold planning and obstacle avoidance of the legged robot.

[0117] The slope information matrix can reflect the inclination of the terrain at the corresponding position of a grid, preventing the robot from stepping on unsuitable places such as the edge of stairs. The obstacle information matrix can help the robot plan feasible paths.

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

[0119] S61, calculate the slope component in the x-direction for each element in the matrix. Define an m×m x-direction slope information matrix, iterate through 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 :

[0120]

[0121]

[0122]

[0123] Where n represents the width of the square grid area corresponding to an element, and 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 its height value is empty, and when h(i+1, j) has a height value, the slope is... x =slope x 2, when h(i+1, j) does not exist or its height value is empty, and h(i-1, j) has a height value, slope x =slope x1 If h(i-1, j) and h(i+1, j) do not exist or their height values ​​are empty, then the slope x =0;

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

[0125]

[0126]

[0127]

[0128] Where n represents the width of the square grid area corresponding to an element, and 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 its height value is empty, and when h(i, j+1) has a height value, the slope is... y =slopey2, when h(i,j+1) does not exist or its height value is empty, and h(i,j-1) has a height value, slope y =slope y1 If h(i, j-1) and h(i, j+1) do not exist or their height values ​​are empty, then the slope y =0;

[0129] S63, calculate the actual slope of each element in the matrix, define an m×m actual slope information matrix, and update it using the slope information matrices in the x and y directions:

[0130]

[0131] 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 slopey (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;

[0132] S64 defines an m×m obstacle information matrix, with the maximum walking stride length of the legged robot preset to l_Max, the maximum footing height to h_Max, and the maximum possible footing slope to s_Max:

[0133] S65, the offline calculation of the matrix element access order in 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 16 search directions are 22.5° apart from each other. According to the matrix size, the search width of each search direction is preset to w, and the search step size is n, which is consistent with the width of the cell corresponding to an element. When searching a direction, a line segment with its midpoint at the center of the matrix, a width of w, and perpendicular to the search direction is obtained. The matrix elements corresponding to all the cells passed by this line segment are the search elements of step 0. After each search step, the line segment is translated by a step size n along the search direction. The cells passed by the translated line segment are the search elements of the new step. Each search direction must be translated until the line segment is completely away from the matrix area. Since the calculation in this step is repetitive and the search order is the same each time, the search order of the cells is saved offline after the first calculation to reduce the amount of calculation and increase real-time performance.

[0134] S66. Following the search order obtained in step S65, sequentially access the corresponding elements of the height information matrix and the slope information matrix, and analyze whether the cell containing each element is an obstacle.

[0135] In step S66, when the search begins in each search direction, a historical search queue is first set up. The maximum length of the queue is l_queue=l_Max / n, and it is rounded down to the nearest integer, which represents the number of search steps corresponding to the maximum walking stride of the legged robot. When a new value enters at the end of the queue and the queue exceeds the maximum length, the value at the head of the queue will be deleted.

[0136] In step S66, an initial height is established as the 0th value in the queue. The initial height is the negative of the height of the legged robot from the ground when it is standing. For each cell searched in each step, if the height is greater than the maximum value in the historical search queue + h_Max, the cell is considered an obstacle, and the corresponding element in the obstacle information matrix is ​​recorded as 1. If the height is less than or equal to the maximum value in the historical search queue + h_Max, the cell is considered passable and not an obstacle. In this case, if the corresponding slope is less than s_Max, the cell is considered footable, and the corresponding element in the obstacle information matrix is ​​recorded as 0. If the corresponding slope is greater than s_Max, the cell is considered passable but not footable, and the corresponding element in the obstacle information matrix is ​​recorded as 0.1. After each search step, the maximum height of all footable cells found in this step is added to the end of the historical search queue. Because some cells found in different search directions overlap, when the elements of the obstacle information matrix are not being assigned values ​​for the first time, it is necessary to compare the old value with the new value. Only when the new value is less than the old value can it be updated.

[0137] Steps S61-S63 yielded the slope at the corresponding position of each element, as shown in the following figure. Figure 4 As shown, the darker the color, the steeper the slope; steps S64-S66 determine whether there are obstacles at the location corresponding to each element, with black representing obstacles. The effects of having obstacles and having obstacles on the stairs are as follows: Figure 5 As shown.

[0138] The effects of this invention in practical applications are as follows: Figure 6 , Figure 7 As shown, the legged robot can perform obstacle avoidance and foothold planning based on the height information matrix, slope information matrix, and obstacle information matrix.

[0139] The above specific embodiments are used to explain and illustrate the present invention, but not to limit the present invention. Any modifications and changes made to the present invention within the spirit and scope of the claims shall fall within the protection scope of the present invention.

Claims

1. A method for local terrain perception in a legged robot, characterized in that, Includes the following steps: S1. Set up at least one lidar on the legged robot and obtain the external parameters of the lidar relative to the body coordinate system of the legged robot. Transform the point cloud obtained by the lidar scanning to the body coordinate system of the legged robot and update the point cloud obtained by scanning periodically. S2, remove motion distortion from the point cloud obtained in step S1, and convert it to the gravity coordinate system of the legged robot, retaining the point cloud within the set range; S3, reduce the dimensionality of the point cloud obtained in step S2 into a height information matrix; S4, perform hole filling based on the height information matrix obtained in step S3; S5, based on the height information matrix obtained in step S4, remove the influence of the leg point cloud of the legged robot on the height matrix; Step S5 includes the following sub-steps: S51, set the range and erosion size of the legged robot's legs. Based on the size of the legged robot, set a row and column range for the height information matrix that can cover the matrix elements updated for the legged robot's leg point cloud. The row range is [number missing]. arrive The range of column numbers is arrive And set a corrosion size s based on the leg size; S52, within a set range, performs erosion on the bulge height, eroding away the terrain bulges caused by the leg point cloud, copies the height information matrix to a temporary matrix, and then... arrive OK, arrive Each element in the column is updated to the elements surrounding the current element. arrive OK, arrive The minimum height value within the column range is used to remove abnormal bulges in some matrix elements caused by leg point clouds; S53 performs dilation compensation on normal terrain, and uses a temporary matrix. arrive OK, arrive Each element in the column is updated to the elements surrounding the current element. arrive OK, arrive The maximum height value within the column range is used to compensate for the changes to normal terrain caused by step S52. S54, Update the height information matrix, and set the number of rows in the temporary matrix to the range of . arrive The range of column numbers is arrive The region is copied into the original height information matrix. S6. Analyze the height information matrix processed in step S5, calculate the slope information matrix and obstacle information matrix, and use them for foothold planning and obstacle avoidance of the legged robot.

2. The method for local terrain perception of a legged robot according to claim 1, characterized in that, Step S1 includes the following sub-steps: S11, At least one lidar is installed on the legged robot so that the area around the legged robot and the area below its torso are within the scanning range. S12 defines the coordinate system of the legged robot, with the origin at any position on the robot. The x-axis points directly in front of the robot, the y-axis points directly to the left, and the z-axis points vertically upwards. The extrinsic parameters include the rotation matrix. and displacement vector , b represents the body coordinate system of the legged robot. This represents the i-th lidar; S13, After receiving the point cloud data, the point cloud is transformed from the lidar coordinate system to the legged robot's body coordinate system using the following formula: , in , , Let be the coordinates of the measurement point of the i-th lidar in the lidar coordinate system. , , The coordinates of the measurement point after transformation to the body coordinate system of the legged robot; S14, set the point cloud collection period T, and every T time, merge the point cloud scanned by the lidar into the same point cloud after processing in step S13.

3. The method for local terrain perception of a legged robot according to claim 1, characterized in that, Step S2 includes the following sub-steps: S21, An IMU is installed on the legged robot, and a LiDAR is connected to a processor that can acquire the attitude angle of the IMU. The installation attitude of the IMU is consistent with that of the legged robot specimen system, so that the IMU can represent the attitude of the quadruped robot. S22, combining joint motor encoders and IMU to estimate the displacement of the legged robot; S23, input IMU attitude data and legged robot displacement odometry data at a frequency of not less than 100Hz into the point cloud processing module, combine them with the timestamp of each point in the point cloud, remove motion distortion from the point cloud, so that the timestamp of each point after processing is the same as the time of the last point in the point cloud: , in , , To remove motion distortion from the coordinates of a point in the body coordinate system of the legged robot, , Let represent the rotation matrix and displacement vector of the legged robot's body coordinate system at the j-th point timestamp relative to the legged robot's body coordinate system at the end timestamp of the point cloud, respectively. S24. Define the gravity coordinate system of the legged robot. Its origin coincides with the origin of the body coordinate system of the legged robot. 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 body coordinate system of the legged robot, and the y-axis is perpendicular to the x-axis and z-axis respectively. The direction satisfies the right-hand rule. S25, based on the IMU pose corresponding to the end timestamp of the point cloud, transform the point cloud after removing motion distortion in step S23 from the legged robot's body coordinate system to the legged robot's gravity coordinate system: , in , , To transform the coordinates to the quadruped robot's gravity coordinate system, The rotation matrix is ​​obtained as follows: , Convert the IMU attitude Euler angles corresponding to the last timestamp of the point cloud into a rotation matrix, which is: Set the heading angle to zero in Euler angles, while keeping the pitch and roll angles unchanged, and then convert it to a rotation matrix, which is: ; S26. Perform pass-through filtering on the point cloud obtained in step S25. Select and retain the point cloud within the target range in the x, y, and z directions of the quadruped robot's gravity coordinate system, and discard the point cloud outside the range.

4. The method for local terrain perception of a legged robot according to claim 1, characterized in that, Step S3 includes the following sub-steps: S31, define a matrix for storing terrain height information. Define an m-row m-column empty matrix 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. S32, reduce the 3D point cloud to a height information matrix, traverse the point cloud obtained in step S2, 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 already been assigned a value, compare the element value with 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 local terrain perception of a legged robot according to claim 1, characterized in that, Step S4 includes the following sub-steps: S41, find empty elements in the height matrix. Use the height of the element at the hole boundary as the candidate height for hole filling. Traverse the height information matrix obtained in step S3. For an 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 a hole edge element. Visit each element one by one from the hole edge element to the direction of the empty neighbor until an element with a non-empty height value is visited, or the number of visits reaches the 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 recording container as a candidate height for empty elements, and record the row and column coordinates of all empty elements in the process of visiting them 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. Traverse the coordinates of empty elements in all containers and access the element in the height information matrix. If it is still empty, assign the corresponding candidate height to the element. If it has been assigned a height value by a previous record, compare the current height value with the new candidate value and take the smaller height. S43. Repeat steps S41 and S42 until there are no more holes to fill, or the preset maximum number of times is reached.

6. The method for local terrain perception of a legged robot according to claim 1, characterized in that, Step S6 includes the following sub-steps: S61, calculate the slope component in the x-direction for each element in the matrix. Define an m×m x-direction slope information matrix. Iterate through the height information matrix obtained in step 5, and calculate the x-direction slope component for each element with a non-empty height value. : , Where n represents the width of the square grid area corresponding to an element. Let be the height value of the element in the i-th row and j-th column. Does not exist or has an empty height value. When there is a height value, ,when Does not exist or has an empty height value. When there is a height value, , and None of them exist or their height values ​​are empty. =0; S62, calculate the slope component in the y-direction for each element in the matrix. Define an m×m y-direction slope information matrix, iterate through the height information matrix obtained in step 5, and calculate the y-direction slope component for each element with a non-empty height value. : , Where n represents the width of the square grid area corresponding to an element. Let be the height value of the element in the i-th row and j-th column. Does not exist or has an empty height value. When there is a height value, ,when Does not exist or has an empty height value. When there is a height value, , and None of them exist or their height values ​​are empty. =0; S63, calculate the actual slope of each element in the matrix, define an m×m actual slope information matrix, and update it using the slope information matrices in the x and y directions: , in This represents the slope value of the element in the i-th row and j-th column of the actual slope information matrix. and These are the slope values ​​of the elements in the i-th row and j-th column of the slope matrix in the x-direction and y-direction directions, respectively. S64 defines an m×m obstacle information matrix, with the maximum walking stride of the legged robot preset to be... The maximum landing height is The maximum possible slope for landing is : S65, the order of accessing matrix elements in the offline obstacle analysis process; S66. Following the search order obtained in step S65, sequentially access the corresponding elements of the height information matrix and the slope information matrix, and analyze whether the cell containing each element is an obstacle.

7. The method for local terrain perception of a legged robot according to claim 6, characterized in that: In step S65, starting from the center of the matrix, multiple search directions are preset, covering a 360° range. Based on the matrix size, the search width for each search direction is preset to be... The search step size is The width of the cell corresponding to an element is the same. When searching in each direction, a line with its midpoint at the center of the matrix and a width of [missing information] is first obtained. The line segment perpendicular to the search direction, and the matrix elements corresponding to all the cells passed through by this line segment, are the search elements for step 0. For each search step, the line segment is shifted one step along the search direction. The cells that the translated line segment passes through are the search elements for the next step. Each search direction requires translating the line segment until it is completely outside the region of the matrix.

8. The method for local terrain perception of a legged robot according to claim 6, characterized in that: In step S66, when starting a search in each search direction, a historical search queue is first established, with a maximum queue length of [value missing]. = / The value is rounded down to the nearest integer, representing the number of search steps corresponding to the maximum walking stride of the legged robot. When a new value enters at the end of the queue and the queue exceeds its maximum length, the value at the head of the queue is deleted.

9. The method for local terrain perception of a legged robot according to claim 8, characterized in that: In step S66, an initial height is established as the 0th value in the queue. The initial height is the negative of the height of the legged robot from the ground when it is standing. For each search of several cells, if the height is greater than the maximum value in the historical search queue, then... If the height is less than or equal to the maximum value of the historical search queue, then the cell is considered an obstacle, and the corresponding element in the obstacle information matrix is ​​recorded as 1; if the height is less than or equal to the maximum value of the historical search queue, then the cell is considered an obstacle. If the slope is less than 1, then the grid is considered passable and not an obstacle. If a square is deemed a suitable landing spot, the corresponding element in the obstacle information matrix is ​​set to 0. If the corresponding slope is greater than... If a grid is deemed passable but not habitable, the corresponding element of the obstacle information matrix is ​​recorded as 0.

1. After each search step, the maximum height of all habitable grids found in this step is added to the end of the historical search queue.

Images