Quadruped robot trajectory planning method for unknown rugged terrain

Abstract

The invention belongs to the field of robot navigation, and discloses an unknown rugged terrain-oriented quadruped robot trajectory planning method, which comprises the following steps of: carrying out multi-orientation discrete sampling to obtain a two-dimensional plane pose, carrying out elliptical ring analysis iteration to solve a three-dimensional space pose, and extracting characteristics such as curvature and gradient to construct a global trafficability map; an exploration path is generated, nodes meeting spatial distribution constraints are reserved, a global footprint graph is constructed and dynamically maintained according to node risk levels, and structured reuse of historical path information is achieved; according to the spatial relationship between the target point and the footprint map, switching an exploration planning strategy, and introducing a ray casting mechanism to detect the sight accessibility so as to dynamically update the sub-target and solve the problem of local occlusion stagnation; and constructing a nonlinear trajectory optimization model containing motion anisotropy and terrain risk constraints, and generating a feasible smooth trajectory. The three-dimensional terrain risk is accurately evaluated, and the re-planning efficiency and the smoothness and safety of motion in the complex terrain are improved.

Description

Technical Field

[0001] This invention relates to the field of mobile robot navigation technology, and in particular to a trajectory planning method for quadruped robots facing unknown and rugged terrain. Background Technology

[0002] With advancements in mobile robot platforms and motion control technology, robots' mobility on rugged terrain has significantly improved. To achieve autonomous navigation, mobile robots need to plan a safe and feasible trajectory. Traditional two-dimensional planning algorithms primarily focus on obstacle avoidance and struggle to address safety risks such as tipping over in three-dimensional terrain. Therefore, trajectory planning for three-dimensional rugged terrain has become a research hotspot.

[0003] The existing technical solutions have the following drawbacks: 1. Fixed methods for assessing terrain accessibility: Most methods assign a fixed accessibility rating to each local terrain patch when assessing terrain, failing to reflect the differences in robot accessibility due to different orientations. Furthermore, the widely used 2D grid maps or 2.5D elevation maps have inherent limitations: 2D grid maps are suitable for structured indoor environments, while 2.5D maps only store a single height at each horizontal position, failing to represent real 3D structures such as overhangs and tunnels, and making it difficult to extract key geometric features such as curvature and normal vectors, resulting in incomplete terrain perception.

[0004] 2. Reliance on prior global map information: Some high-performance 3D planning methods must acquire and process complete environmental point cloud information in advance for terrain analysis in order to generate high-quality paths. This strong dependence on map information makes them unsuitable for truly unknown environments that require online exploration, severely limiting their practicality in real-world scenarios such as rescue and exploration.

[0005] 3. Lack of a global information reuse mechanism, resulting in low planning efficiency: Many navigation methods for unknown environments do not retain global traversal information of already traversed areas during the exploration process. When a task needs to backtrack or revisit an explored area, the system must perform a new path search, increasing path generation time and failing to guarantee the quality of the new path.

[0006] 4. Lack of post-processing for paths, resulting in poor trajectory quality and feasibility: Existing methods mostly focus on generating front-end paths using graph search or sampling methods, lacking subsequent trajectory optimization processes. The generated paths are often polylines, failing to consider robot kinematics (such as velocity and acceleration limits) and dynamic constraints, leading to uneven trajectories, high curvature, and compromised navigation safety and efficiency.

[0007] The present invention aims to solve the above-mentioned technical problems and provide a trajectory planning scheme that can achieve high efficiency, safety and feasible movement in unknown three-dimensional rugged terrain. Summary of the Invention

[0008] This invention aims to at least solve one of the technical problems existing in related technologies. To this end, this invention provides a trajectory planning method for quadruped robots facing unknown, rugged terrain.

[0009] This invention provides a trajectory planning method for a quadruped robot facing unknown and rugged terrain, comprising the following steps: S1. Construct a global navigability map: acquire the robot's state information and environmental point cloud in real time, perform multi-orientation discrete sampling with the robot as the center, perform elliptical ring terrain analysis on the sampled two-dimensional plane pose, calculate the navigability evaluation value, and merge and update the navigability evaluation value to output a global navigability map. S2. Maintaining the global footprint map: During the robot's exploration, a fast expanding random tree algorithm based on elliptic ring terrain analysis is run to generate path nodes. The global drivability map output by S1 is queried to obtain the drivability evaluation value corresponding to the path node. Nodes that meet the spatial distribution constraints are selected as vertices to construct a weighted undirected graph and output the global footprint map. S3. Generate front-end path: Receive navigation target point, calculate the spatial distance relationship between the robot's current position, the navigation target point and the global footprint map output by S2, dynamically select a planning strategy based on the spatial distance relationship, and generate a front-end path from the robot's current position to the navigation target point; S4. Trajectory Optimization: Using the front-end path output by S3 as the initial value, and combining the terrain information in the global drivability map output by S1 with the anisotropic constraints of the quadruped robot's motion, a nonlinear optimization problem is constructed and solved, and an executable control trajectory is output.

[0010] Furthermore, in S1, elliptical ring terrain analysis and calculation are performed to obtain a accessibility evaluation value, specifically including: A three-dimensional elliptical ring region is defined with the two-dimensional planar pose obtained by the discrete sampling as the center, and environmental point clouds falling within this region are selected. Based on the selected environmental point cloud, the 3D spatial pose that best fits the terrain is solved iteratively. Based on the point cloud data in the final three-dimensional spatial pose, the terrain feature vector is calculated, and the terrain feature vector is weighted and normalized to obtain the accessibility evaluation value in the three-dimensional spatial pose.

[0011] Furthermore, the terrain feature vector includes at least curvature, slope, height difference, centroid deviation, and vacancy rate; The global traversability map output in S1 uses a weighted average update rule, and the specific formula is as follows: , In the formula: Represents the first in the global map A grid in The constantly updated accessibility evaluation value; This represents the accessibility rating of the corresponding grid cell in the current local map. This represents the historical accessibility rating of the grid before the update; For fusion weighting coefficients.

[0012] Furthermore, in S2, nodes that satisfy the spatial distribution constraints are selected according to the following rules: Calculate the horizontal distance between the path node generated by the fast expanding random tree algorithm based on elliptic ring terrain analysis and the nearest vertex already present in the global footprint map. and vertical distance ; Determine if the conditions are met: Greater than the horizontal distance threshold and less than ,at the same time Less than the vertical distance threshold If the conditions are met, the path node is retained as a candidate vertex and added to the global footprint graph; in, and These are the minimum and maximum thresholds for the horizontal distance, respectively. This is the vertical distance threshold.

[0013] Furthermore, the construction of a weighted undirected graph in S2 specifically includes: Based on the mobility evaluation value corresponding to the candidate vertex in the global mobility map, the candidate vertex is divided into low-risk vertex, medium-risk vertex and high-risk vertex; For low-risk vertices, medium-risk vertices, and high-risk vertices, respectively, a decreasing connection range threshold is set to establish edges, where no connection is established for high-risk vertices; The edge in the global footprint graph weight The calculation formula is: , in, Represents vertices and The Euclidean distance between them; and Vertices and Accessibility rating of the area; This is a weighting coefficient used to adjust the relative impact of accessibility on edge weights.

[0014] Furthermore, in step S3, the planning strategy is dynamically selected based on the spatial distance relationship, specifically including the following three scenarios: Scenario A: If the robot is located at the edge of the coverage area of ​​the global footprint map and the navigation target point is located in an unknown area, then the fast expanding random tree algorithm based on elliptical ring terrain analysis is invoked for direct exploration; Scenario B: If the navigation target point is within the coverage area of ​​the global footprint map, then a graph search algorithm is executed on the global footprint map; Scenario C: If the navigation target point is located in an unknown area and there is an area in front of the robot that has been covered by the global footprint map, a hybrid strategy is adopted: the global footprint map is used first to plan to the boundary vertex, and then the fast expanding random tree algorithm based on elliptic ring terrain analysis is called from the boundary vertex to continue exploration.

[0015] Furthermore, the fast expanding random tree algorithm based on elliptical ring terrain analysis is ER-RRT. The algorithm, and the algorithm includes a ray-projected sub-target update step during the expansion process: A three-dimensional raster map is constructed using environmental point clouds. The three-dimensional raster map discretizes the space into a voxel grid. Each voxel stores a state value representing the space occupancy status. The state value includes free, occupied, and unknown. Using the end node of the path tree as a candidate sub-target point, construct a virtual ray from the candidate sub-target point to the navigation target point; The virtual ray is discretized into discrete points, and the state value of the discrete points in the three-dimensional grid map is queried. If all discrete points are in an idle state, the candidate sub-target point is determined to be valid, and the path tree is guided to grow towards the valid candidate sub-target point.

[0016] Furthermore, the trajectory optimization of the front-end path in S4 specifically includes: The front-end path is parameterized in time, and the robot's horizontal position and orientation are represented by a fifth-order polynomial; A nonlinear optimization problem is constructed, the objective function of which is to minimize the sum of the jerk and the terrain cost on the path, wherein the terrain cost is derived from the accessibility evaluation value in the global accessibility map; Solve the nonlinear optimization problem to obtain the optimal trajectory parameters.

[0017] Furthermore, the constraints of the nonlinear optimization problem include the motion anisotropy constraint and the acceleration constraint. The specific anisotropic constraint of motion is as follows: in, , For longitudinal and lateral velocities; Maximum forward speed; This is the lateral velocity scaling factor; The acceleration constraint is specifically as follows: in, , For longitudinal and lateral acceleration, and These are the maximum longitudinal and lateral accelerations.

[0018] Furthermore, the constraints of the nonlinear optimization problem also include drivability safety constraints, boundary constraints, and continuity constraints; The specific provisions regarding the accessibility safety constraints are: any point on the trajectory The value must be below the safety threshold. ,Right now ; The boundary constraints are specifically as follows: The starting and ending states of the trajectory are consistent with the robot's current state and the target state. The continuity constraint is specifically as follows: The position, velocity, and acceleration of each trajectory segment at the connection point are continuous.

[0019] The above-described one or more technical solutions in the embodiments of the present invention have at least one of the following technical effects: A global accessibility map was constructed using elliptical ring terrain analysis, enabling accurate assessment of terrain risks. By constructing and maintaining a global footprint map, efficient reuse of path information in explored areas is achieved, greatly improving the efficiency of replanning in round-trip tasks; By integrating terrain accessibility and robot motion anisotropy constraints in trajectory optimization, the path is transformed into a smooth, kinematically feasible trajectory, improving tracking control quality and motion safety. The entire framework is based entirely on online perception, requiring no prior map, and effectively overcomes the planning stagnation problem when targets are occluded by using a ray-projection sub-target update strategy.

[0020] It not only significantly improves planning efficiency and navigation success rate in unknown and rugged terrain, but also fundamentally ensures the feasibility and safety of movement, achieving safe, efficient and reliable autonomous navigation.

[0021] Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0022] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0023] Figure 1 This is a flowchart of the present invention.

[0024] Figure 2 This is a schematic diagram illustrating the principle of constructing a local accessibility map through elliptical ring terrain analysis in this invention.

[0025] Figure 3 For the present invention, through ER-RRT A flowchart illustrating the process of selecting candidate vertices and constructing a global footprint graph. Detailed Implementation

[0026] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention. The following embodiments are used to illustrate this invention but should not be used to limit the scope of this invention.

[0027] The following is combined Figures 1 to 3 This invention describes a trajectory planning method for a quadruped robot oriented towards unknown, rugged terrain.

[0028] (a) Constructing a global traversability map The system acquires environmental point clouds generated by the lidar mounted on the quadruped robot in real time, and uses a simultaneous localization and mapping (SLT) module to estimate and output the robot's state information in real time, including its three-dimensional position in the world coordinate system. with posture .

[0029] First, a 3D raster map is constructed based on the environmental point cloud using the simultaneous localization and mapping (SLT) module. This map discretizes the 3D space into a uniform voxel grid, with each voxel storing a state value to characterize its spatial occupancy, including three states: "idle" (indicating no point cloud in the space), "occupied" (indicating point cloud in the space), and "unknown" (indicating the space has not yet been observed by the sensor, and its state is uncertain). This 3D raster map serves as the basic geometric environment representation layer, primarily used for determining the state of discrete points in subsequent ray casting methods.

[0030] Based on this, to assess the terrain accessibility near the robot, this invention employs elliptic ring analysis to construct a global accessibility map. Unlike a 3D grid map that only records occupancy status, the global accessibility map stores terrain feature vectors obtained through in-depth analysis of terrain geometry, including curvature, slope, etc. Its core is a accessibility evaluation value reflecting the terrain's safety level. The lookup index for a specific grid cell in the 3D grid map is... The lookup index for a specific grid cell in the global traversability map is... .

[0031] To construct a global navigability map, it is necessary to [do something] on the horizontal plane. Discrete grid sampling is performed at a preset resolution.

[0032] The sampling method can select different ranges for sampling. The following examples illustrate the sampling method using square or circular ranges.

[0033] For a square range, if its side length is Then the location sampling is , Within the area by resolution Perform uniform grid division to obtain the coordinates of sampling points. , .in .

[0034] For a circular range, if its radius is Then, polar coordinates can be used for discrete sampling. The specific method is: first, the radial distance... from arrive By step size Discretize, angle from arrive By step size Discretize and generate polar coordinate grids ,in: , Then, each polar coordinate point is transformed back to a Cartesian coordinate system to obtain the position sampling points. , This method ensures that all sampling points fall within the circular region. The distribution is determined by both radial and angular resolution.

[0035] Obtaining sampling points at each location Then, further adjustments were made to the robot's orientation angle. Inner angle resolution Uniform discrete sampling is performed, thus defining each sampling point as a complete possible robot pose. .

[0036] For each discrete pose Elliptical ring analysis is performed, and local terrain features are extracted through iterative solutions. For example... Figure 2 As shown, the specific steps are as follows: 1. Define the elliptical annular region: based on distance Centered on the point cloud with the nearest location, and based on preset elliptical ring parameters... Define a three-dimensional elliptical annular spatial region. The length of the semi-axis of the inner ellipse is... The preferred range is (0.3m, 0.2m) to (0.5m, 0.4m); the length of the semi-axis of the outer ellipse is... The preferred range is (0.5m, 0.4m) to (1.0m, 0.8m); height range The height is set according to the robot chassis height, for example, (-0.2m, 0.5m).

[0037] 2. Iteratively solve for the 3D spatial pose (position) and attitude matrix And calculate terrain features: using the initial horizontal sampling pose. Based on this, the three-dimensional spatial pose that best fits the terrain is solved through multiple iterations, and the terrain features are calculated simultaneously.

[0038] Each iteration includes the following steps: a. Point cloud filtering: Based on the pose of the current iteration (initially...) (Corresponding initial 3D spatial pose), determine the 3D elliptical ring region, and filter out the environmental point cloud falling within this region. .

[0039] b. Feature calculation: based on the current point cloud Calculate the three-dimensional centroid Covariance matrix ;right Perform eigenvalue decomposition to obtain eigenvalues. and eigenvectors (where the vector corresponding to the smallest eigenvalue is the normal vector). ); calculate the curvature of the current iteration. ,slope Height difference Centroid deviation vacancy rate Based on the above characteristics, the feasibility evaluation value for the current iteration is calculated using weighted normalization. .

[0040] c. Pose Update: Using normal vectors Update the pose matrix of the current pose Axis, using the center of mass of Update the coordinates to the current pose position and height The process is iterated until the maximum number of iterations is reached, yielding the final three-dimensional spatial pose (position). and attitude matrix and the corresponding stable terrain feature vector at that pose. The specific meaning of the eigenvector is as follows: This is the final average height. For the final normal vector, For curvature, take the smallest eigenvalue of the covariance matrix. With the sum of eigenvalues The ratio of represents the degree of unevenness of the terrain surface; Calculate the normal vector for the slope. with vector The included angle, , indicating the angle of terrain inclination; Calculate the point cloud for the height difference. middle The difference between the maximum and minimum values ​​of the coordinates; To calculate the centroid of the point cloud, consider the centroid deviation. Horizontal position relative to sampling point The Euclidean distance; The vacancy rate is the ratio of the number of voxels occupied by point clouds within the elliptical ring region to the total number of voxels, which characterizes the integrity of the terrain structure. As a feasibility evaluation value, it is obtained by analyzing the feature set. After normalization and weighted summation, we get: .

[0041] Scalar, representing a set A specific feature in (such as curvature) ,slope (etc.). For each sampling point, It is a specific value calculated based on point computing. and It is aimed at this specific feature The preset normalization boundary values ​​are both scalar constants. For example, when (Curvature) corresponds to the preset value. and ;when When (slope) is selected, it corresponds to the preset value. and And so on. Their function is to represent the original characteristics with different physical meanings and dimensions. Mapped to the [0,1] interval for weighted aggregation. Weighting coefficients. The importance of each feature to the quadruped robot's passage safety is calibrated, and the sum is 1. The value ranges from 0 to 1, with lower values ​​indicating better accessibility.

[0042] 3. Constructing and fusing a accessibility map: First, based on the current local point cloud... and robot state Define a local accessibility map The calculated local terrain features will be within the specified range. Save to local accessibility map The corresponding grid in the map. Then, the local accessibility map is... Integrate into the global accessibility map The fusion adopts a weighted average update rule: In the formula: Represents the first in the global map A grid in The constantly updated accessibility evaluation value; This represents the accessibility rating of the corresponding grid cell in the current local map. This represents the historical accessibility rating of the grid before the update; For the fusion weighting coefficient, a preferred range is 0.6 to 0.8. A larger value would be preferable. The latest observations are given higher weight because the latest point cloud data is updated as the robot moves. Including more points results in more accurate calculations.

[0043] (II) Design of a Fast Extended Random Tree Algorithm Based on Elliptic Ring Terrain Analysis (ER-RRT) ) In the global accessibility map Based on this, this invention designs a fast expanding random tree algorithm based on elliptical ring terrain analysis for exploratory path planning in unknown areas. The state nodes of the algorithm are defined as follows: ,in For node position, For node attitude, From The traversability evaluation value of the node's pose obtained from the query is used to sample and obtain the new node. Then, find its nearest neighbor node in the tree. .from arrive expansion costs From the formula Calculation, where Let be the Euclidean distance between the two nodes. represents the weighting coefficient. This cost function considers both path length and the safety of the areas traversed.

[0044] (iii) In ER-RRT The algorithm uses ray casting to dynamically update the sub-target points. To address the issue of planning stalls when the target point is obstructed by terrain features such as hills and walls, this invention addresses this problem in ER-RRT. A dynamic sub-target update mechanism based on ray projection is introduced.

[0045] The algorithm uses the terminal node of each RRT path as a candidate sub-target point. It simulates a virtual ray from this sub-target point to the final target point. This ray is discretized into a series of dense points, and the status of these discrete points is queried in a 3D raster map constructed by the simultaneous localization and mapping module based on the environmental point cloud.

[0046] If all discrete points are in an "idle" (unoccupied) state, the ray path is considered safe and unobstructed based on the current known information. In this case, the candidate sub-target is adopted as a new valid sub-target. The subsequent search by the algorithm will be mainly influenced by... The guidance is to lead, rather than rushing directly towards the final target that may be obscured.

[0047] With ER-RRT As the tree grows and the robot's exploration range expands, new valid sub-targets are constantly generated. The system continuously performs the aforementioned ray projection check, and once a node that is closer to the current sub-target and reachable by the ray is found, the sub-target is immediately updated. This mechanism ensures that even in complex occlusion environments, the planner can continuously find feasible directions of movement.

[0048] (iv) Retain ER-RRT Effective nodes to construct a global footprint graph Valid nodes are achieved through ER-RRT The nodes that satisfy the spatial distribution constraints are selected by the algorithm and added as vertices to the global footprint graph. The detailed selection method is as follows.

[0049] In ER-RRT During algorithm execution, to improve the efficiency of subsequent replanning, the system retains and filters the nodes generated by its expansion, constructing a weighted undirected graph called the global footprint graph, such as... Figure 3 As shown.

[0050] Global footprint graph is defined as ,in It is a set of vertices. It is a set of edges. Each vertex Defined as a triple: ,in: Represents vertices Position coordinates in three-dimensional space, i.e. ; This represents the walkability evaluation value of the region where the vertex is located, and its value range is... The lower the value, the better the feasibility (i.e., the lower the risk). This represents the set of edges connected to this vertex. Each edge... Recorded the vertex to its adjacent vertices The connection relationships and corresponding weights.

[0051] Vertex filtering and addition: For ER-RRT Each new node generated Calculate its relationship with the global footprint map The nearest vertex in the global footprint graph The method for obtaining the nearest vertex in the middle is well known to researchers in this field and will not be elaborated here. (The horizontal distance between...) and vertical distance Only when Greater than the horizontal distance threshold and less than ,at the same time Less than the vertical distance threshold hour, Only then were they retained and selected as candidate vertices. Add global footprint graph This rule ensures a reasonable density of vertices in the global footprint graph.

[0052] and These are the minimum and maximum thresholds for the horizontal distance, respectively.

[0053] Vertex risk classification: based on candidate vertices Location from The feasibility evaluation value queried in the middle Based on the risk level of the region where the vertex is located, the vertices are divided into three categories: low-risk nodes ( ,in The first threshold) is located in the low-risk area, and the medium-risk node ( ,in The second threshold) is located in the medium-risk area, and the high-risk node ( It is located in a high-risk area.

[0054] Edge creation: candidate vertices Establish edges with neighboring vertices. The connection range threshold is set according to the vertex risk type: low-risk vertices use a larger range. Medium-risk vertices use a smaller range High-risk vertices will not be connected. weight The calculation formula is: ,in, Represents vertices and The Euclidean distance between them and Vertices and Accessibility rating of the area. where is the weight coefficient, used to adjust the relative influence of accessibility on edge weights. This constructs a weighted undirected graph structure for the global footprint, where nodes in low-risk regions are densely connected, forming an efficient access network.

[0055] (v) Design a front-end path generation mechanism and dynamically select planning strategies. When the system receives a new navigation target point, the front-end path generation mechanism selects the path generation method based on the relationship between the target point and the robot's position. First, it calculates the vertex on the global footprint map closest to the target point. The decision-making logic of this mechanism is based on two distances: the robot's distance to the nearest vertex. distance ,as well as Distance to the target point .according to and With preset threshold and Based on the comparison relationship, select the corresponding path generation method. Parameters It is used to determine whether a robot is... Whether it is close to the threshold, parameter It is used to determine the target point and Whether it is close to the threshold.

[0056] 1. Scenario A (Direct Exploration): If and This indicates that the robot is already within the known environmental boundary, and the target point is located in an unknown area. At this point, the system directly invokes ER-RRT. The algorithm starts from the robot's current position and plans a path to the target point.

[0057] 2. Case B (Graph Search): If This indicates that the target point is very close to the known vertex. If the target is located in an already explored area, the system can assume it is. In this case, the system directly queries the global footprint map. Using a graph search algorithm, a path from the robot's current position to... The optimal path is the front-end path.

[0058] 3. Case C (Mixed Strategy): If and This indicates that the target point is located in a distant, unknown area, and that there is an already explored area in front of the robot. At this point, the system adopts a two-stage hybrid strategy: In the first stage, the global footprint map is used to plan the route from the robot's current position to... The path; the robot will arrive along this path. Then, the second phase is automatically triggered, invoking ER-RRT. The algorithm from Set off and continue exploring and planning the path to the final destination.

[0059] (vi) Trajectory optimization by combining terrain and motion constraints By ER-RRT The front-end path generated by the global footprint map is usually a polyline, which does not consider motion continuity. This invention transforms it into a smooth, dynamically feasible trajectory through trajectory optimization.

[0060] First, parameterize the front-end path in time. The robot's horizontal position... and orientation Express each using a fifth-order polynomial. Let the total trajectory time be... Divide it into Each segment is a time interval, and each segment is an optimization variable.

[0061] Construct a nonlinear optimization problem where the decision variables are polynomial coefficients. and various periods of time The objective function is: in For jerk. For trajectory points The accessibility evaluation value at the location, and These are the weighting coefficients.

[0062] Apply the following constraints: 1. Anisotropic Motion Constraint: Reflects the characteristic that the lateral mobility of a quadruped robot is weaker than its longitudinal mobility.

[0063] in, , For longitudinal and lateral velocities; Maximum forward speed; This is the lateral velocity scaling factor.

[0064] 2. Acceleration constraints: in, , For longitudinal and lateral acceleration, and These are the maximum longitudinal and lateral accelerations.

[0065] 3. Boundary condition constraints: The starting and ending states (position, velocity, acceleration) of the trajectory must be consistent with the robot's current state and the target state.

[0066] 4. Accessibility safety constraints: for any point on the trajectory The value must be below the safety threshold. ,Right now .

[0067] 5. Continuity constraint: The position, velocity, and acceleration of each trajectory segment at the connection point must be continuous.

[0068] This optimization problem is solved using numerical optimization methods (such as the augmented Lagrangian method combined with the L-BFGS algorithm) to obtain smooth, safe, and optimal trajectory parameters that satisfy the robot's motion capabilities. Finally, the optimized trajectory is sent to the quadruped robot's underlying motion controller for execution.

[0069] The above method can be deployed on computing platforms equipped with Intel Core i7 / i9 or Intel NUC. In simulation and physical experiments, the robot carrier can be a Unitree A1 or Aliengo robot. In multiple complex and rugged terrain scenarios, such as hills, ruins, and gravel roads, the method of this invention shows significant advantages over existing methods in terms of navigation success rate, average path risk, and planning time. In particular, when planning to explore already explored areas, the efficiency of replanning can be improved by two orders of magnitude due to the reuse of the global footprint map.

[0070] This invention also provides a trajectory planning system for a quadruped robot facing unknown rugged terrain, comprising: an omnidirectional drivability mapping module and an ER-RRT. The system includes a planning module, a global footprint map construction and maintenance module, a front-end path decision module, a trajectory optimization module, and a trajectory execution control module. These modules can be integrated into the robot's main control computer.

[0071] The above embodiments are merely preferred embodiments of the present invention. Those skilled in the art can adjust the parameters (such as elliptic ring parameters, various thresholds, weight coefficients, polynomial order) within the scope defined by the claims, or apply them to other types of mobile robots (such as wheeled or tracked robots), all of which fall within the protection scope of the present invention.

[0072] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A trajectory planning method for a quadruped robot facing unknown rugged terrain, characterized in that, Includes the following steps: S1. Construct a global navigability map: acquire the robot's state information and environmental point cloud in real time, perform multi-orientation discrete sampling with the robot as the center, perform elliptical ring terrain analysis on the sampled two-dimensional plane pose, calculate the navigability evaluation value, and merge and update the navigability evaluation value to output a global navigability map. S2. Maintain the global footprint map: Run the fast expanding random tree algorithm based on elliptic ring terrain analysis to generate path nodes, query the global drivability map output by S1 to obtain the drivability evaluation value corresponding to the path node, select the nodes that meet the spatial distribution constraints as vertices to construct a weighted undirected graph, and output the global footprint map. S3. Generate front-end path: Receive navigation target point, calculate the spatial distance relationship between the robot's current position, the navigation target point and the global footprint map output by S2, dynamically select a planning strategy based on the spatial distance relationship, and generate a front-end path from the robot's current position to the navigation target point; S4. Trajectory Optimization: Using the front-end path output by S3 as the initial value, and combining the terrain information in the global drivability map output by S1 with the anisotropic constraints of the quadruped robot's motion, a nonlinear optimization problem is constructed and solved, and an executable control trajectory is output.

2. The trajectory planning method for a quadruped robot facing unknown rugged terrain according to claim 1, characterized in that, The elliptical ring terrain analysis and calculation performed in S1 to obtain the accessibility evaluation value specifically includes: A three-dimensional elliptical ring region is defined with the two-dimensional planar pose obtained by the discrete sampling as the center, and environmental point clouds falling within this region are selected. Based on the selected environmental point cloud, the 3D spatial pose that best fits the terrain is solved iteratively. Based on the point cloud data in the final three-dimensional spatial pose, the terrain feature vector is calculated, and the terrain feature vector is weighted and normalized to obtain the accessibility evaluation value in the three-dimensional spatial pose.

3. The trajectory planning method for a quadruped robot facing unknown rugged terrain according to claim 2, characterized in that, The terrain feature vector includes at least curvature, slope, height difference, centroid deviation, and vacancy rate; The global traversability map output in S1 uses a weighted average update rule, and the specific formula is as follows: ， In the formula: Represents the first in the global map A grid in The constantly updated accessibility evaluation value; This represents the accessibility rating of the corresponding grid cell in the current local map. This represents the historical accessibility rating of the grid before the update; For fusion weighting coefficients.

4. The trajectory planning method for a quadruped robot facing unknown rugged terrain according to claim 1, characterized in that, The specific rules for selecting nodes that satisfy the spatial distribution constraints in S2 are as follows: Calculate the horizontal distance between the path node generated by the fast expanding random tree algorithm based on elliptic ring terrain analysis and the nearest vertex already present in the global footprint map. and vertical distance ; Determine if the conditions are met: Greater than the horizontal distance threshold and less than ,at the same time Less than the vertical distance threshold If the conditions are met, the path node is retained as a candidate vertex and added to the global footprint graph; in, and These are the minimum and maximum thresholds for the horizontal distance, respectively. This is the vertical distance threshold.

5. The trajectory planning method for a quadruped robot facing unknown rugged terrain according to claim 4, characterized in that, The construction of the weighted undirected graph in S2 specifically includes: Based on the mobility evaluation value corresponding to the candidate vertex in the global mobility map, the candidate vertex is divided into low-risk vertex, medium-risk vertex and high-risk vertex; For low-risk vertices, medium-risk vertices, and high-risk vertices, respectively, a decreasing connection range threshold is set to establish edges, where no connection is established for high-risk vertices; The edge in the global footprint graph weight The calculation formula is: , in, Represents vertices and The Euclidean distance between them; and Vertices and Accessibility rating of the area; This is a weighting coefficient used to adjust the relative impact of accessibility on edge weights.

6. The trajectory planning method for a quadruped robot facing unknown rugged terrain according to claim 1, characterized in that, In step S3, the planning strategy is dynamically selected based on the spatial distance relationship, specifically including the following three scenarios: Scenario A: If the robot is located at the edge of the coverage area of ​​the global footprint map and the navigation target point is located in an unknown area, then the fast expanding random tree algorithm based on elliptical ring terrain analysis is invoked for direct exploration; Scenario B: If the navigation target point is within the coverage area of ​​the global footprint map, then a graph search algorithm is executed on the global footprint map; Scenario C: If the navigation target point is located in an unknown area and there is an area in front of the robot that has been covered by the global footprint map, a hybrid strategy is adopted: the global footprint map is used first to plan to the boundary vertex, and then the fast expanding random tree algorithm based on elliptic ring terrain analysis is called from the boundary vertex to continue exploration.

7. The trajectory planning method for a quadruped robot facing unknown rugged terrain according to claim 6, characterized in that, The fast expanding random tree algorithm based on elliptic ring terrain analysis is ER-RRT. The algorithm, and the algorithm includes a ray-projected sub-target update step during the expansion process: A three-dimensional raster map is constructed using environmental point clouds. The three-dimensional raster map discretizes the space into a voxel grid. Each voxel stores a state value representing the space occupancy status. The state value includes free, occupied, and unknown. Using the end node of the path tree as a candidate sub-target point, construct a virtual ray from the candidate sub-target point to the navigation target point; The virtual ray is discretized into discrete points, and the state value of the discrete points in the three-dimensional grid map is queried. If all discrete points are in an idle state, the candidate sub-target point is determined to be valid, and the path tree is guided to grow towards the valid candidate sub-target point.

8. The trajectory planning method for a quadruped robot facing unknown rugged terrain according to claim 1, characterized in that, The trajectory optimization of the front-end path in S4 specifically includes: The front-end path is parameterized in time, and the robot's horizontal position and orientation are represented by a fifth-order polynomial; A nonlinear optimization problem is constructed, the objective function of which is to minimize the sum of the jerk and the terrain cost on the path, wherein the terrain cost is derived from the accessibility evaluation value in the global accessibility map; Solve the nonlinear optimization problem to obtain the optimal trajectory parameters.

9. The trajectory planning method for a quadruped robot facing unknown rugged terrain according to claim 8, characterized in that, The constraints of the nonlinear optimization problem include the motion anisotropy constraint and the acceleration constraint. The specific anisotropic constraint of motion is as follows: in, , For longitudinal and lateral velocities; Maximum forward speed; This is the lateral velocity scaling factor; The acceleration constraint is specifically as follows: in, , For longitudinal and lateral acceleration, and These are the maximum longitudinal and lateral accelerations.

10. The trajectory planning method for a quadruped robot facing unknown rugged terrain according to claim 8, characterized in that, The constraints of the nonlinear optimization problem also include drivability safety constraints, boundary constraints, and continuity constraints; The specific provisions regarding the accessibility safety constraints are: any point on the trajectory The value must be below the safety threshold. ,Right now ; The boundary constraints are specifically as follows: The starting and ending states of the trajectory are consistent with the robot's current state and the target state. The continuity constraint is specifically as follows: The position, velocity, and acceleration of each trajectory segment at the connection point are continuous.

Images