A map construction and path planning method for a quadruped robot

Abstract

The present invention discloses a method for map construction and path planning of a quadruped robot, which is mainly implemented by a quadruped robot geometric model, a map builder, a quadruped robot kinematic model, and a path planner. Based on the geometric characteristics of the quadruped robot, the present invention constructs a map that simultaneously considers the position and heading angle information of the quadruped robot. The present invention represents the position and heading angle to be planned in a three-dimensional grid map, converts the navigation task of the two-dimensional plane into a three-dimensional space search problem, and improves the efficiency of path planning; the geometric shape of the quadruped robot is reasonably simplified, combined with the process of map construction, so that the path planning result is not too conservative, and safety is guaranteed at the same time; the kinematic model and movement ability of the quadruped robot are combined to construct a unique kinematic constraint, and the A* algorithm based on the kinematic model and constraints can make full use of information and plan a smooth path.

Description

Technical Field

[0001] The invention belongs to the field of robots, and in particular relates to a map construction and path planning method for a quadruped robot. Background Art

[0002] Autonomous navigation is a common task requirement for mobile robots. The robot needs to build a map describing the environment based on sensor information, set the starting point and end point in the map, and plan the path in the map. When the environment is more complex or the robot's motion characteristics are more unique, the design of environmental mapping and path planning will greatly affect the quality of autonomous navigation. Wheeled robots account for a large proportion of mobile robots, and there are many mature methods. Map construction methods include occupancy grid maps, octree maps, Euclidean distance field maps, etc., and path planning methods include Dijkstra, A*, D*, RRT and other methods.

[0003] In recent years, with the progress of research in the field of legged robots, quadruped robot technology has become more mature. Compared with wheeled robots, quadruped robots can adapt to more complex terrains and have more flexible movement capabilities, so they are gradually taking on tasks such as scene inspection. However, due to the differences in structural characteristics and movement models between quadruped robots and wheeled robots, conventional methods used for wheeled robots are not well adapted to quadruped robots.

[0004] At present, the patent (CN108646765A) proposes a path planning method and system for a quadruped robot based on an improved A* algorithm. The method includes constructing an environmental grid map and initially expanding the grid of obstacles according to the shortest distance between the center of the quadruped robot and the edge of the body. The path planning end uses the improved A* algorithm to determine the pass cost value from the initial grid point to the target grid point, and determines that the path corresponding to the minimum pass cost value from the initial grid point to the target grid point is the optimal path. Patent (CN114022824A) proposes a motion planning method for a quadruped robot for narrow environments. The main steps of this method are: constructing a passability cost map based on the environmental information obtained by the sensor; using an external global planner to determine the local target point, and then using the particle model of the quadruped robot to plan a rough optimal collision-free path on the cost map using the A* algorithm; combining the motion dynamics model of the quadruped robot to optimize the trajectory and generate an effective motion trajectory.

[0005] The above methods have made improvements based on the characteristics of quadruped robots, but there are still some problems. The main problem with the map construction method is that when the edge of the obstacle is expanded, the particle model will cause the final planning result to be too conservative or unsafe, and it is often impossible to plan the heading angle of the robot. The main problem with the path planning method is that the motion characteristics of the quadruped robot are not taken into account. That is, although the quadruped robot has omnidirectional movement capabilities, there are differences in the ability of each direction. Summary of the invention

[0006] In view of the problem that the current method does not fully consider the geometric characteristics of the quadruped robot in the map construction end, and the path planning end does not effectively combine the kinematic model, resulting in low efficiency of autonomous navigation tasks, the present invention provides a method for map construction and path planning of a quadruped robot. Based on the geometric characteristics of the quadruped robot, the present invention constructs a map that simultaneously considers the position and heading angle information of the quadruped robot; at the same time, the A* algorithm combined with the motion characteristics plans a safe, smooth and efficient path.

[0007] The objective of the present invention is achieved through the following technical solutions: a method for map construction and path planning of a quadruped robot, which is mainly implemented by a quadruped robot geometric model, a map builder, a quadruped robot kinematic model, and a path planner; after receiving a path planning task, the map builder will combine the quadruped robot geometric model and the three-dimensional point cloud to construct a three-dimensional grid map for planning, and output this map to the path planner; the path planner will, based on the quadruped robot kinematic model and in combination with the A* search algorithm of kinematic constraints, construct a global path that includes both the position information and heading angle information of the quadruped robot, and output it.

[0008] Furthermore, the geometric model of the quadruped robot is equivalently represented by a convex hull in a three-dimensional space, and the convex hull is a cylinder.

[0009] Furthermore, the map builder combines the quadruped robot geometric model and the 3D point cloud to construct a 3D grid map, including:

[0010] (2.1) After receiving the three-dimensional point cloud information, the map builder selects the point cloud within the cylindrical height range of the convex hull of the quadruped robot; the point cloud after the height screening is unified again to form a two-dimensional point cloud, and it is regarded as an obstacle in the same plane;

[0011] (2.2) The map builder constructs a three-dimensional grid map; the x and y directions of the three-dimensional grid map are consistent with the directions of the world coordinate system, and the z direction is used to represent the heading angle ψ of the quadruped robot in the world coordinate system; the feasible area in the three-dimensional grid map indicates that when the origin of the quadruped robot's body coordinate system is in these areas, the corresponding position and heading angle of the quadruped robot are safe, which converts the path planning problem of the quadruped robot into a point-to-point path planning problem; for an obstacle, the map builder calculates the equivalent obstacle area of ​​the obstacle at different heading angles, and then superimposes them in the z direction of the map to obtain the obstacle area formed by the obstacle at each heading angle of the robot; the map builder traverses the obstacles in the map, marks all grids where the equivalent obstacle areas are located as inaccessible, and completes the construction of the map with position and heading angle information.

[0012] Further, step (2.2) comprises the following steps:

[0013] (2.2.1) The map builder pre-sets the map boundary in the z direction to [0, z max ], calculate the corresponding heading angle ψ through any value z0 in the z direction:

[0014]

[0015] (2.2.2) The map builder calculates the rotation matrix of the quadruped robot body coordinate system relative to the world coordinate system through the heading angle ψ

[0016]

[0017] (2.2.3) The rotation matrix By acting on the cross-sectional shape of the convex hull representing the robot's geometric shape, we can obtain the top-down shape of the cross-sectional shape after rotating it according to the heading angle ψ. This shape is equivalent to representing the top-down shape of the quadruped robot along the heading angle in the world coordinate system.

[0018] (2.2.4) For a certain heading angle, the map builder performs a Minkowski sum on the robot's top-down shape and the obstacle shape to obtain the obstacle area equivalent to the obstacle at that heading angle.

[0019] (2.2.5) As described in formula (1), there is a conversion relationship between the z-direction value in the grid map and the heading angle ψ, so the obstacle area indicates that the quadruped robot will collide with the obstacle at this position and heading angle; the map builder traverses each grid in the map to obtain a three-dimensional grid map indicating whether any position and any heading angle in the global map is feasible.

[0020] Further, step (2.2.4) includes: the map builder obtains a two-dimensional point cloud constructed according to the convex hull height, and performs a Minkowski sum on each point of the two-dimensional point cloud and the rotated cross-sectional shape obtained in step (2.2.3) to obtain a new point set; the Minkowski sum is defined as follows:

[0021] R={p+q|p∈P,q∈Q} (3)

[0022] Among them, the set R represents the new point set obtained after the Minkowski sum; the set P represents the two-dimensional point cloud, and the element p represents a point in P; the set Q represents the point set that constitutes the cross-sectional shape under the current heading angle, and the element q represents a point in Q;

[0023] The map builder obtains the data of the points in the set R according to formula (3), and marks the grids where these points are located as obstacle areas according to the values ​​of the points in the x, y, and z directions.

[0024] Furthermore, the kinematic model of the quadruped robot includes:

[0025] (3.1) Construct the kinematic model of the quadruped robot in the form of a state space model:

[0026]

[0027]

[0028] The kinematic model includes the quadruped robot state variables S and input variables [V bx V by ω] T ; The state variable S contains the position X of the quadruped robot in the x direction in the world coordinate system w 、The robot's position in the y direction in the world coordinate system Y w , the robot's heading angle ψ in the world coordinate system; the input variables include V bx 、V by and ω, where V bx V represents the speed of the quadruped robot in the x direction in the body coordinate system. by represents the velocity in the y direction in the body coordinate system, and ω represents the angular velocity of rotation in the body coordinate system;

[0029] (3.2) According to the input variables in the kinematic model constructed in step (3.1), set the kinematic constraints of the quadruped robot:

[0030]

[0031] -c≤ω≤c, c>0 (7)

[0032] ((V bx) 2 +(V by ) 2 )·ω 2 ≤d,d>0 (8) Among them, a represents the maximum forward speed of the quadruped robot in the x direction in the body coordinate system, b represents the maximum lateral speed of the quadruped robot in the y direction in the body coordinate system; therefore, equation (6) forms an ellipse for the linear velocity constraint of the robot; c represents the maximum rotation angular velocity of the quadruped robot in the body coordinate system, and d represents the constraint on the centripetal acceleration of the quadruped robot in order to prevent it from tipping over.

[0033] Furthermore, the path planner receives the three-dimensional grid map constructed by the map builder, and plans a path from the grid where the starting point is located to the grid where the end point is located, including:

[0034] (4.1) The quadruped robot is transformed into a particle in the grid. For any feasible grid in the grid map, the cost of the A* algorithm is expressed as:

[0035] f(n)=g(n)+h(n) (9)

[0036] g(n+1)=g(n)+cost e

[0037] Among them, f(n) is the estimate of the minimum cost from the starting grid to the end grid via the current grid n; g(n) is the minimum cumulative cost from the starting grid to the current grid n; cost e represents the cost of reaching grid n+1 from grid n; h(n) is the minimum estimated cost of the path from the current grid n to the destination grid;

[0038] (4.2) When the point representing the current position and heading angle of the quadruped robot in the three-dimensional grid map is at a height of z0', its heading angle at that time is calculated as:

[0039]

[0040] (4.3) When the quadruped robot moves from one grid to a neighboring grid along any of the eight feasible directions, there will be a deviation angle between the direction of travel and the heading angle.

[0041]

[0042] Among them, i represents eight directions of travel;

[0043] (4.4) Under the kinematic constraint that the maximum linear velocity is an ellipse, calculate the deflection angle The maximum feasible speed in the corresponding direction of travel |vn |For:

[0044]

[0045] (4.5) By dividing the distance l between the two grid centers by the maximum feasible speed, we get the cost cost of reaching grid n+1 from the current grid n. e ;

[0046] (4.6) Combined with the A* algorithm with kinematic constraints, in the map constructed by the map builder, repeat steps (4.1) to (4.5), continuously update f(n) of each grid, and finally search for the grid where the target position and heading angle are located, and output the path.

[0047] Furthermore, in step (4.1), the straight-line distance from the current grid n to the end grid divided by the maximum linear velocity of the quadruped robot is selected as h(n).

[0048] The beneficial effect of the present invention is that the present invention fully combines the geometric shape and kinematic characteristics of the quadruped robot to plan an efficient, safe and smooth global path. It includes the following features:

[0049] (1) The position and heading angle to be planned are represented in a three-dimensional grid map, which transforms the two-dimensional navigation task into a three-dimensional space search problem, thus improving the efficiency of path planning;

[0050] (2) The geometric shape of the quadruped robot is reasonably simplified and combined with the map construction process to ensure that the path planning result is not too conservative while ensuring safety;

[0051] (3) The kinematic model and motion capabilities of the quadruped robot are combined to construct unique kinematic constraints. The A* algorithm based on the kinematic model and constraints can make full use of the information and plan a smooth path. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 A schematic diagram of a map construction and path planning method for a quadruped robot of the present invention;

[0053] Figure 2 This is a schematic diagram of the working principle of the map builder of the present invention;

[0054] Figure 3 Schematic diagram of the kinematic model of the quadruped robot of the present invention;

[0055] Figure 4 A schematic diagram of the speed constraint of the quadruped robot of the present invention;

[0056] Figure 5 A schematic diagram of calculating grid cost for the present invention;

[0057] Figure 6 Output path diagram for the path planner of the present invention. DETAILED DESCRIPTION

[0058] The present invention will be further described below in conjunction with the accompanying drawings.

[0059] like Figure 1 As shown, a method for mapping and path planning of a quadruped robot of the present invention receives a navigation task of the quadruped robot, which is mainly implemented by a geometric model of the quadruped robot, a map builder of the quadruped robot, a kinematic model of the quadruped robot, and a path planner of the quadruped robot. After receiving the navigation planning task, the map builder will combine the geometric model of the quadruped robot and the three-dimensional point cloud acquired by the sensor to construct a three-dimensional grid map for planning, and output the map to the path planner; the path planner will construct a global path containing both the position information and heading angle information of the quadruped robot based on the kinematic model of the quadruped robot and the A* search algorithm of the kinematic constraints, and output it.

[0060] Specifically, the steps include:

[0061] (1) Construct a geometric model of the quadruped robot, including:

[0062] The quadruped robot is composed of various complex mechanical structures. In the navigation task, the quadruped robot needs to be simplified with a reasonable geometric model. The present invention considers using a convex hull in a three-dimensional space to equivalently represent the geometric appearance of the quadruped robot. The convex hull is a cylinder in three-dimensional space. The height of the cylinder is the distance from the highest point of the body to the ground when the quadruped robot is standing normally. The cross-section of the cylinder is an octagon, which is a tighter convex hull of the shape of the quadruped robot when it is standing normally. Thus, the geometric shape of the quadruped robot during map construction is represented by a convex hull in a three-dimensional space.

[0063] (2) Construct a quadruped robot map builder, and use the convex hull representing the geometric model of the quadruped robot in step (1) and the point cloud information obtained by the sensor to construct a three-dimensional grid map.

[0064] (2.1) After receiving the 3D point cloud information, the map builder first filters the 3D point cloud data according to the height of the cylinder representing the convex hull of the quadruped robot. Specifically, only the point cloud within the cylinder height range is retained. For the path planning problem in the two-dimensional plane, the point cloud within the cylinder height range should be regarded as an obstacle area, while obstacles above this range will not affect the path planning. Therefore, the point cloud after the height screening can be unified again to form a two-dimensional point cloud, and it can be regarded as an obstacle in the same plane.

[0065] (2.2) The map builder constructs a three-dimensional grid map. The x and y directions of the three-dimensional grid map are consistent with the directions of the world coordinate system, and the z direction is used to represent the heading angle ψ of the quadruped robot in the world coordinate system. The feasible area in the three-dimensional grid map indicates that when the origin of the quadruped robot's body coordinate system is in these areas, the corresponding position and heading angle of the quadruped robot are safe, which converts the path planning problem of the quadruped robot into a point-to-point path planning problem. Figure 2 As shown in the figure, for an obstacle, the map builder calculates the equivalent obstacle area of ​​the obstacle at different heading angles, and then superimposes them in the z direction of the map to obtain the obstacle area formed by the obstacle at each heading angle of the robot. The map builder traverses the obstacles in the map according to the above method, marks all grids where the equivalent obstacle area is located as impassable, and completes the construction of the map with position and heading angle information. It includes the following steps:

[0066] (2.2.1) The map builder pre-sets the map boundary in the z direction to [0, z max ], calculate the corresponding heading angle ψ through any value z0 in the z direction:

[0067]

[0068] (2.2.2) The map builder calculates the rotation matrix of the quadruped robot body coordinate system relative to the world coordinate system through the heading angle ψ

[0069]

[0070] (2.2.3) The rotation matrix By acting on the cross-sectional shape of the convex hull representing the robot's geometric shape, we can obtain the top-down shape of the cross-sectional shape after rotating it according to the heading angle ψ. This shape equivalently represents the top-down shape of the quadruped robot along the heading angle in the world coordinate system.

[0071] (2.2.4) For a certain heading angle, the map builder performs a Minkowski sum on the robot’s top-down shape and the obstacle shape to obtain the obstacle area equivalent to the obstacle at that heading angle.

[0072] Specifically, the map builder obtains a two-dimensional point cloud constructed according to the convex hull height, and performs a Minkowski sum on each point of the two-dimensional point cloud and the rotated cross-sectional shape obtained in step (2.2.3) to obtain a new point set. The Minkowski sum is defined as follows:

[0073] R={p+q|p∈P,q∈Q} (3)

[0074] Among them, the set R represents the new point set obtained after the Minkowski sum; the set P represents the two-dimensional point cloud, and the element p represents a point in P; the set Q represents the point set that constitutes the cross-sectional shape under the current heading angle, and the element q represents a point in Q.

[0075] The map builder obtains the data of the points in the set R according to formula (3), and marks the grids where these points are located as obstacle areas according to the values ​​of the points in the x, y, and z directions.

[0076] (2.2.5) As described in formula (1), there is a conversion relationship between the z-direction value and the heading angle ψ in the grid map, so the obstacle area indicates that the quadruped robot will collide with the obstacle at this position and heading angle. The map builder traverses each grid in the map to obtain a three-dimensional grid map indicating whether any position and heading angle in the global map are feasible.

[0077] The present invention combines the geometric shape of the quadruped robot into the map construction process by reasonable simplification, so that the planning result is not overly conservative while ensuring safety. The present invention creatively represents the position and heading angle to be planned in a three-dimensional grid map, transforms the navigation task of the two-dimensional plane into a three-dimensional space search problem, and improves the efficiency and quality of planning.

[0078] (3) Figure 3 As shown, a kinematic model of a quadruped robot is constructed, including:

[0079] (3.1) Construct the kinematic model of the quadruped robot in the form of a state space model:

[0080]

[0081]

[0082] The kinematic model includes the quadruped robot state variables S and input variables [V bx V by ω] T The state variable S contains the position X of the quadruped robot in the x direction in the world coordinate system. w 、The robot's position in the y direction in the world coordinate system Y w , the robot's heading angle ψ in the world coordinate system. The input variables include V bx 、V by and ω, where V bx V represents the speed of the quadruped robot in the x direction in the body coordinate system. by represents the velocity in the y direction in the body coordinate system, and ω represents the angular velocity of rotation in the body coordinate system.

[0083] (3.2) Figure 4As shown, according to the input variables in the kinematic model constructed in step (3.1), the kinematic constraints of the quadruped robot are set:

[0084]

[0085] -c≤ω≤c, c>0 (7)

[0086] ((V bx ) 2 +(V by ) 2 )·ω 2 ≤d,d>0 (8) Among them, a represents the maximum forward speed of the quadruped robot in the x direction in the body coordinate system, and b represents the maximum lateral speed of the quadruped robot in the y direction in the body coordinate system; therefore, equation (6) forms an ellipse for the robot's linear velocity constraint. c represents the maximum rotation angular velocity of the quadruped robot in the body coordinate system, and d represents the constraint on the centripetal acceleration of the quadruped robot in order to prevent it from tipping over.

[0087] (4) Construct a quadruped robot path planner, receive the three-dimensional grid map constructed in step (2), and plan a path from the grid where the starting point is located to the grid where the end point is located. Including:

[0088] (4.1) The quadruped robot is transformed into a particle in the grid. For any feasible grid in the grid map, the cost of the A* algorithm is expressed as:

[0089] f(n)=g(n)+h(n) (9)

[0090] Among them, f(n) is an estimate of the minimum cost from the starting grid to the end grid via the current grid n.

[0091] g(n) is the minimum cumulative cost from the starting grid to the current grid n. For a new grid n+1, g(n+1) can be expressed as g(n)+cost e Among them, cost e represents the cost of reaching grid n+1 from grid n. By setting the cost of the starting point g(0) = 0, g(n) can be continuously calculated during the path search process.

[0092] h(n) is the minimum estimated cost of the path from the current grid n to the end grid, h * (n) is the minimum actual cost of the path from the current grid n to the end grid. As long as the estimated cost h(n) is greater than the actual cost h * If (n) is small, the optimality of the A* algorithm can be guaranteed. Therefore, the straight-line distance from the current grid n to the end grid divided by the maximum linear speed of the quadruped robot is selected as h(n):

[0093]

[0094] Among them, P goal Indicates the location of the end grid in the three-dimensional grid map, P n Indicates the position of the current grid n in the grid map, P goal -P n Represents the straight-line distance between two grids, V max Indicates the maximum linear speed of the robot.

[0095] (4.2) When the point representing the current position and heading angle of the quadruped robot in the three-dimensional grid map is at a height of z0', its heading angle at that time can be calculated as:

[0096]

[0097] (4.3) Figure 5 As shown in the figure, the x and y directions of the grid map are consistent with the x and y directions of the world coordinate system. Therefore, when the quadruped robot moves from one grid to the neighboring grid in any of the eight feasible directions, there will be a deviation angle between the travel direction and the heading angle. This deflection angle can be calculated for:

[0098]

[0099] Among them, the serial number i represents eight travel directions, No. 0 represents the positive direction of the world coordinate system x-axis, No. 1 represents the direction at a 45° angle to the positive direction of the world coordinate system x-axis, No. 2 represents the positive direction of the world coordinate system y-axis, No. 3 represents the direction at a 45° angle to the positive direction of the world coordinate system y-axis, No. 4 represents the negative direction of the world coordinate system x-axis, No. 5 represents the direction at a 45° angle to the negative direction of the world coordinate system x-axis, No. 6 represents the negative direction of the world coordinate system y-axis, and No. 7 represents the direction at a 45° angle to the negative direction of the world coordinate system y-axis.

[0100] (4.4) Under the kinematic constraint that the maximum linear velocity is an ellipse, the deflection angle obtained from step (4.3) is Calculate the deflection angle The maximum feasible speed in the corresponding direction of travel |v n |For:

[0101]

[0102] (4.5) By dividing the distance l between the two grid centers by the maximum feasible speed, we get the cost cost of reaching grid n+1 from the current grid n. e :

[0103] cost e =l / |v n | (14).

[0104] (4.6) Combined with the A* algorithm with kinematic constraints, in the map constructed in step (2), repeat the search process of steps (4.1) to (4.5) above, continuously update f(n) of each grid, and finally search for the grid where the target position and heading angle are located, output the path, and the path planning of the quadruped robot navigation task is completed. Figure 6 An output path of an embodiment is shown. The path consists of connected grids. The x and y positions of the grids represent the expected robot position of the path. The z position of the grid can be converted into a heading angle by formula (1).

[0105] The present invention constructs unique kinematic constraints through the kinematic model and actual motion ability of the quadruped robot. The A* algorithm based on the kinematic model and constraints can make full use of this information to plan in the constructed three-dimensional grid map.

[0106] The above embodiments are used to illustrate the present invention rather than to limit the present invention. Any modification and change 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 mapping and path planning of a quadruped robot, characterized in that: It is mainly implemented by the quadruped robot geometric model, map builder, quadruped robot kinematic model, and path planner. After receiving the path planning task, the map builder will combine the quadruped robot geometric model and 3D point cloud to build a 3D grid map for planning, and output this map to the path planner. The path planner will build a global path that contains both the position information and heading angle information of the quadruped robot based on the kinematic model of the quadruped robot and the A* search algorithm of kinematic constraints, and output it. The specific steps include the following: (1) The geometric model of the quadruped robot is equivalently represented by a convex hull in a three-dimensional space, where the convex hull is a cylinder; (2) The map builder combines the quadruped robot geometric model and the 3D point cloud to construct a 3D grid map, including: (2.1) After receiving the three-dimensional point cloud information, the map builder selects the point cloud within the cylindrical height range of the convex hull of the quadruped robot; the point cloud after the height screening is unified again to form a two-dimensional point cloud, and it is regarded as an obstacle in the same plane; (2.2) The map builder constructs a three-dimensional grid map; the x and y directions of the three-dimensional grid map are consistent with the directions of the world coordinate system, and the z direction is used to represent the heading angle ψ of the quadruped robot in the world coordinate system; the feasible area in the three-dimensional grid map indicates that when the origin of the quadruped robot's body coordinate system is in these areas, the corresponding position and heading angle of the quadruped robot are safe, which converts the path planning problem of the quadruped robot into a point-to-point path planning problem; for an obstacle, the map builder calculates the equivalent obstacle area of ​​the obstacle at different heading angles, and then superimposes them in the z direction of the map to obtain the obstacle area formed by the obstacle at each heading angle of the robot; the map builder traverses the obstacles in the map, marks all grids where the equivalent obstacle areas are located as inaccessible, and completes the construction of the map with position and heading angle information; (2.2.1) The map builder pre-sets the map boundary in the z direction to [0, z max ], calculate the corresponding heading angle ψ through any value z0 in the z direction: (2.2.2) The map builder calculates the rotation matrix of the quadruped robot body coordinate system relative to the world coordinate system through the heading angle ψ (2.2.3) The rotation matrix By acting on the cross-sectional shape of the convex hull representing the robot's geometric shape, we can obtain the top-down shape of the cross-sectional shape after rotating it according to the heading angle ψ. This shape is equivalent to representing the top-down shape of the quadruped robot along the heading angle in the world coordinate system. (2.2.4) For a certain heading angle, the map builder performs a Minkowski sum on the robot's top-down shape and the obstacle shape to obtain the obstacle area equivalent to the obstacle at that heading angle. (2.2.5) The map builder traverses each grid in the map to obtain a three-dimensional grid map indicating whether any position and heading angle in the global map is feasible; In step (2.2.4), the map builder obtains a two-dimensional point cloud constructed according to the convex hull height, and performs a Minkowski sum on each point of the two-dimensional point cloud and the rotated cross-sectional shape obtained in step (2.2.3) to obtain a new point set; the Minkowski sum is defined as follows: R={p+q|p∈P,q∈Q} (3) Among them, the set R represents the new point set obtained after the Minkowski sum; the set P represents the two-dimensional point cloud, and the element p represents a point in P; the set Q represents the point set that constitutes the cross-sectional shape under the current heading angle, and the element q represents a point in Q; The map builder uses the data of the points in the set R obtained by formula (3) to mark the grids where these points are located as obstacle areas according to the values ​​of the points in the x, y, and z directions; (3) Kinematic model of quadruped robot, including: (3.1) Construct the kinematic model of the quadruped robot in the form of a state space model: The kinematic model includes the quadruped robot state variables S and input variables [V bx V by ω] T ; The state variable S contains the position X of the quadruped robot in the x direction in the world coordinate system w 、The robot's position in the y direction in the world coordinate system Y w , the robot's heading angle ψ in the world coordinate system; the input variables include V bx 、V by and ω, where V bx represents the velocity of the quadruped robot in the x direction in the body coordinate system, V by represents the velocity in the y direction in the body coordinate system, and ω represents the angular velocity of rotation in the body coordinate system; (3.2) According to the input variables in the kinematic model constructed in step (3.1), set the kinematic constraints of the quadruped robot: -c≤ω≤c, c>0 (7) ((V bx ) 2 +(V by ) 2 )·ω 2 ≤d,d>0 (8) Among them, a represents the maximum forward speed of the quadruped robot in the x direction in the body coordinate system, b represents the maximum lateral speed of the quadruped robot in the y direction in the body coordinate system; therefore, equation (6) forms an ellipse for the robot's linear velocity constraint; c represents the maximum rotation angular velocity of the quadruped robot in the body coordinate system, and d represents the constraint on the centripetal acceleration of the quadruped robot in order to prevent it from tipping over; (4) A path planner receives the three-dimensional grid map constructed by the map builder and plans a path from the grid where the starting point is located to the grid where the end point is located, including: (4.1) The quadruped robot is transformed into a particle in the grid. For any feasible grid in the grid map, the cost of the A* algorithm is expressed as: f(n)=g(n)+h(n) (9) g(n+1)=g(n)+cost e Among them, f(n) is the estimate of the minimum cost from the starting grid to the end grid via the current grid n; g(n) is the minimum cumulative cost from the starting grid to the current grid n; cost e represents the cost of reaching grid n+1 from grid n; h(n) is the minimum estimated cost of the path from the current grid n to the destination grid; (4.2) When the point representing the current position and heading angle of the quadruped robot in the three-dimensional grid map is at a height of z0', its heading angle at that time is calculated as: (4.3) When the quadruped robot moves from one grid to a neighboring grid along any of the eight feasible directions, there will be a deviation angle between the travel direction and the heading angle. Among them, i represents eight directions of travel; (4.4) Under the kinematic constraint that the maximum linear velocity is an ellipse, calculate the deflection angle The maximum feasible speed in the corresponding direction of travel |v n |For: (4.5) By dividing the distance l between the two grid centers by the maximum feasible speed, we get the cost cost of reaching grid n+1 from the current grid n. e ; (4.6) Combined with the A* algorithm with kinematic constraints, in the map constructed by the map builder, repeat steps (4.1) to (4.5), continuously update f(n) of each grid, and finally search for the grid where the target position and heading angle are located, and output the path.

2. The method for mapping and path planning of a quadruped robot as claimed in claim 1, characterized in that: In step (4.1), the straight-line distance from the current grid n to the end grid divided by the maximum linear velocity of the quadruped robot is selected as h(n).

Images