Non-flat terrain autonomous navigation method based on path pruning and trajectory smoothing

Abstract

The invention discloses a non-flat terrain autonomous navigation method based on path pruning and trajectory smoothing. The method comprises the following steps: constructing an elevation map with three-dimensional map information as a navigation map according to robot navigation space information; based on the navigation map, performing global path search by adopting a path pruning strategy to obtain an initial path from the starting point to the target point; based on the initial path, track optimization is carried out by using a B spline curve, and a smooth track meeting robot dynamics constraints is generated; in response to an obstacle which is detected on the smooth trajectory and cannot be avoided, performing three-dimensional obstacle crossing trajectory planning based on terrain elevation information of the elevation map, and generating an obstacle crossing trajectory capable of crossing the obstacle; wherein the robot performs autonomous navigation based on the smooth track or the obstacle crossing track. According to the invention, efficient global search, smooth trajectory optimization and intelligent obstacle crossing decision of the robot in unknown and non-flat terrains can be realized.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot autonomous navigation, and particularly relates to a non-flat terrain autonomous navigation method based on path pruning and trajectory smoothing. BACKGROUND

[0002] Mobile robots play an important role in post-disaster rescue, power inspection, environmental survey, etc. Wheeled robots are difficult to pass through complex and non-flat road surfaces and are difficult to complete obstacle crossing operations; flying robots have small carrying capacity and are difficult to bear large equipment or supplies. The tracked robot with front and rear swing arms can easily overcome obstacles and successfully cross challenging terrain, while the contact area of the tracked area improves the stability of the robot operation. Although motion planning is highly mature in structured environments, there are still many challenges in non-structured scenarios facing unknown and uneven terrain for obstacle avoidance and crossing operations. The existing technical routes can be mainly divided into the following categories: The first category is a planning method based on a two-dimensional plane. This method usually relies on a laser radar or a vision sensor to construct a two-dimensional occupancy grid map and uses an A* search algorithm for global path planning. Although this method is efficient and mature in flat terrain, it essentially models the environment as a two-dimensional plane and cannot perceive and utilize the height information of the terrain, so it cannot handle any complex terrain that requires obstacle crossing and has limited application range.

[0003] The second category is a planning method based on three-dimensional space sampling. To deal with three-dimensional environments, researchers have introduced algorithms such as RRT (Rapidly Exploring Random Tree) and PRM* (Probabilistic Roadmap) to sample randomly in three-dimensional space to find a feasible path. Although this method can theoretically solve the obstacle crossing problem, it has obvious bottlenecks in practice: first, the sampling is blind and inefficient in narrow passages or complex terrain, with slow convergence speed; second, the path obtained by sampling is usually tortuous and not smooth, which is difficult to meet the dynamics constraints of the robot and often requires subsequent complex optimization.

[0004] The third category is a trajectory planning method based on learning or optimization. This method relies heavily on accurate environment representation to provide gradient information, and the construction and maintenance of such a three-dimensional map has huge computational and storage overhead, which is not conducive to real-time deployment of robots in unknown environments.

[0005] In addition, the algorithms in the prior art that focus on obstacle avoidance on flat terrain do not have obstacle crossing capability, and the algorithms that can cross obstacles are difficult to generate high-quality, smooth paths that meet the dynamics constraints in flat terrain efficiently. Although some research attempts to combine two-dimensional and three-dimensional planning, the usual way is simple and does not fundamentally solve the problems of large data volume, low planning efficiency, and seamless connection of obstacle avoidance and crossing.

[0006] Therefore, how to provide a non-flat terrain autonomous navigation method capable of realizing efficient global search, smooth trajectory optimization, and intelligent obstacle crossing decision unification is a problem that those skilled in the art urgently need to solve. SUMMARY

[0007] Therefore, the present application provides a non-flat terrain autonomous navigation method based on path pruning and trajectory smoothing, aiming to solve the problems of the prior art and realize efficient global search, smooth trajectory optimization, and intelligent obstacle crossing decision of robots in unknown non-flat terrain.

[0008] In order to achieve the above purpose, the present application adopts the following technical solutions: The present application first proposes a non-flat terrain autonomous navigation method based on path pruning and trajectory smoothing, comprising the following steps: S1: constructing an elevation map with three-dimensional map information as a navigation map according to the robot navigation space information; S2: based on the navigation map, using a path pruning strategy for global path search to obtain an initial path from the starting point to the target point; S3: based on the initial path, using a B-spline curve for trajectory optimization to generate a smooth trajectory that satisfies the robot dynamics constraints; S4: in response to detecting an unevadable obstacle on the smooth trajectory, based on the terrain elevation information of the elevation map, performing three-dimensional obstacle crossing trajectory planning to generate an obstacle crossing trajectory that can cross the obstacle; wherein the robot performs autonomous navigation based on the smooth trajectory or the obstacle crossing trajectory.

[0009] Preferably, the environment point cloud data is obtained by the sensor carried by the robot, and the point cloud data is converted into an elevation map with three-dimensional grid map information as a navigation map, wherein each grid stores the elevation information corresponding to the current grid position.

[0010] Preferably, the path pruning strategy in S2 comprises the following steps: S21: for the current node, determine at least one search direction to be explored; wherein if the current node is the starting node, determine all eight adjacent directions around it as the to-be-explored directions; if the current node is not the starting node, determine at most three adjacent directions as the to-be-explored directions according to the direction of the parent node pointing to the current node; S22: for each of the at least one search direction to be explored, performing a jump search from the current node along the corresponding direction; in the jump process, for each node passed by, judging whether it has a forced neighbor; wherein, if at least one forced neighbor is identified at the current node, or the target node is reached, immediately terminating the jump along the current direction, and identifying the current node or the target node as a jump point; S23: adding all the jump points found in the step S22 to a set of nodes to be expanded; selecting a node with the minimum cost from the set of nodes to be expanded as a new current node, and repeating the steps S21 to S23 until the target node is added to the set of nodes to be expanded; S24: starting from the target node, backtracking along the parent node pointers to the start node to form the initial path.

[0011] Preferably, the step of judging whether the node passed by has a forced neighbor in the step S22 comprises: for a current node x passed by in the jump search process, judging whether there is at least one neighbor node f, such that a first path cost from a parent node p of the current node x to the neighbor node f without passing through the current node x is greater than a second path cost from the parent node p to the neighbor node f passing through the current node x; if yes, determining that the current node x has a forced neighbor f.

[0012] Preferably, the step S3 comprises the following steps: S31: obtaining initial B-spline control points by B-spline curve interpolation or fitting based on discrete path points on the initial path; S32: constructing an optimization objective function including a smoothness cost, a collision cost and a feasibility cost; S33: taking the initial B-spline control points as a starting point, solving the optimization objective function by an optimization algorithm to obtain optimized B-spline control points; S34: generating the smooth trajectory according to the optimized B-spline control points.

[0013] Preferably, the optimization objective function is composed of a smoothness cost function, a safety cost function and a feasibility cost function by weighting; wherein, the smoothness cost function is constructed in a weighted sum form of norms of first-order derivative, second-order derivative and third-order derivative with respect to the B-spline control points; the safety cost function is constructed as a function of distance between the B-spline control points and obstacles in the environment; The feasibility cost function is constructed as a constraint function with respect to first-order and second-order derivatives of the B-spline control points, for limiting the velocity and acceleration of the smooth trajectory within the dynamics limit of the robot.

[0014] Preferably, the S4 comprises the following steps: S41, defining a reachability volume of the robot and a robot body volume based on the elevation map; the reachability volume is used to constrain the contact area of the robot with the ground, and the robot body volume is used to constrain the area of the robot body for collision detection; S42: adaptive sampling is performed on an XY two-dimensional plane constituted by the three-dimensional map information, and the sampling points are projected to a three-dimensional space according to the terrain height information of the elevation map; S43: based on the reachability volume and the robot body volume, invalid sampling points and poses that collide with the terrain are removed, and valid three-dimensional sampling points are obtained; S44: based on the valid three-dimensional sampling points, a motion planning algorithm is used to generate the obstacle-crossing trajectory.

[0015] Preferably, the adaptive sampling step in the S42 comprises setting the sampling probability as an inverse function of the node density of the elevation map.

[0016] According to the above technical solution, compared with the prior art, the present application provides a non-flat terrain autonomous navigation method based on path pruning and trajectory smoothing, which has the following beneficial effects: The traditional robot autonomous navigation uses a 3D map for planning operation, and the data storage amount is large. The present application innovatively fuses a two-dimensional grid map with a 2.5-dimensional elevation map, reduces the data storage amount, and improves the search efficiency.

[0017] The traditional path planning algorithm stores all nodes in a list for traversal, and when the environment is complex, the search efficiency is significantly reduced. The present application innovatively proposes a global planning method based on path pruning, changes the search strategy, reduces the number of traversed nodes in the list, and improves the search efficiency in complex environments.

[0018] The traditional robot autonomous navigation mostly focuses on obstacle avoidance planning on flat terrain, and a few obstacle-crossing algorithms are difficult to achieve obstacle avoidance planning that meets the dynamics constraints on flat terrain.

[0019] Therefore, the present application innovatively proposes a planning method based on B-spline optimization and sampling, which realizes obstacle avoidance and crossing planning in complex scenes. Meanwhile, the reachability volume and the robot body volume are defined in the obstacle-crossing scene to remove invalid poses that collide in the obstacle-crossing planning. BRIEF DESCRIPTION OF DRAWINGS

[0021] To more clearly illustrate the technical solutions in the embodiments of the present 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 only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0022] Figure 1 A flowchart of an autonomous navigation method for non-flat terrain based on path pruning and trajectory smoothing provided in an embodiment of the present invention; Figure 2 This is a schematic diagram of forced neighbor and jump point search in the horizontal direction provided in an embodiment of the present invention; Figure 3 This is a schematic diagram of diagonal direction search for forced neighbors and jump points provided in an embodiment of the present invention; Figure 4 A flowchart of a path-pruning-based search method provided in an embodiment of the present invention; Figure 5 A flowchart for jump point lookup provided in an embodiment of the present invention; Figure 6 This is a schematic diagram of the jump point traversal direction provided in an embodiment of the present invention; Figure 7 Schematic diagram of robot reachability volume and fuselage volume provided for embodiments of the present invention Figure 1 ; Figure 8 Schematic diagram of robot reachability volume and fuselage volume provided for embodiments of the present invention Figure 2 ; Figure 9 This is a schematic diagram comparing the robot's autonomous navigation path on uneven terrain with a real-world scene, provided as an embodiment of the present invention. Detailed Implementation

[0023] The technical solutions of 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 some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0024] This invention discloses an autonomous navigation method for non-flat terrain based on path pruning and trajectory smoothing, such as... Figure 1 As shown, it includes the following steps: S1: Construct an elevation map with three-dimensional map information as a navigation map based on the robot's navigation spatial information; S2: Based on the navigation map, a path pruning strategy is used to perform a global path search to obtain the initial path from the starting point to the target point; S3: Based on the initial path, use B-spline curves to optimize the trajectory and generate a smooth trajectory that satisfies the robot's dynamic constraints; S4: In response to the detection of an unavoidable obstacle on the smooth trajectory, a three-dimensional obstacle-crossing trajectory is planned based on the terrain elevation information of the elevation map to generate an obstacle-crossing trajectory that can cross the obstacle; wherein, the robot performs autonomous navigation based on the smooth trajectory or the obstacle-crossing trajectory.

[0025] In one embodiment, environmental point cloud data is acquired through sensors mounted on the robot, and the point cloud data is converted into an elevation map with three-dimensional grid map information as a navigation map. Each grid stores the elevation information corresponding to the current grid position.

[0026] This embodiment uses DLIO for robot localization and mapping, converting point cloud data into an elevation map with 3D map information as the basis for subsequent obstacle crossing planning. Each cell stores the height value and variance.

[0027] This embodiment combines a two-dimensional occupancy grid map and a 2.5-dimensional elevation grid map to construct a fused map as a navigation map.

[0028] In one embodiment, the path pruning strategy in S2 includes the following steps: S21: For the current node, determine at least one search direction to be explored; where, if the current node is the starting node, determine all eight adjacent directions around it as directions to be explored; if the current node is not the starting node, determine at most three adjacent directions as directions to be explored based on the direction from its parent node to the current node. S22: For each of at least one search direction to be explored, start from the current node and perform a jump search along the corresponding direction; during the jump, for each node passed through, determine whether it has a forcing neighbor; if at least one forcing neighbor is identified at the current node, or the target node is reached, the jump along the current direction is immediately terminated, and the current node or the target node is identified as a jump point. S23: Add all the jump points found in step S22 to the set of nodes to be expanded; select the node with the lowest cost from the set of nodes to be expanded as the new current node, and repeat steps S21 to S23 until the target node is added to the set of nodes to be expanded. S24: Starting from the target node, backtrack along the parent node pointer to the starting node to form the initial path.

[0029] This embodiment defines forced neighbors, jump points, and pruning rules to selectively expand only certain nodes in the raster map and add them to a list for subsequent processing. In complex scenarios, it can reduce the maintenance cost and number of operations of the queue, and is faster than the A* method.

[0030] The path pruning method proposed in this embodiment reduces the data storage volume of the Openlist and the number of Openlist operations by filtering jump points and adding them to the Openlist, thereby improving the efficiency of path search.

[0031] In one embodiment, during the hopping process, the step of determining whether a node has a forcing neighbor includes: For the current node x encountered during the jump search, determine whether there exists at least one neighbor node f such that the cost of the first path from the parent node p of the current node x to the neighbor node f without passing through the current node x is greater than the cost of the second path from the parent node p to the neighbor node f via the current node x. If it exists, then it is determined that the current node x has a forcing neighbor f.

[0032] The following is combined with Figures 2-3 Explain the steps to determine if a node along the path has a forcing neighbor: The set of explorable points is recorded as an Openlist. A heuristic function is used to determine the cost from each point to the destination. Forced neighbors f, jump points x, and the parent node p of node x are also defined. By searching for forced neighbors, relevant jump points can be quickly obtained as key nodes for path searching.

[0033] Force Neighbor: The cost of any path from parent node p without passing through any node is greater than the cost of reaching node f via x. .

[0034] The determination of a forced neighbor is divided into horizontal directions (such as...) Figure 1 (as shown) and diagonal direction (as shown) Figure 2 (As shown in the image) There are two types. The red area represents the area where the obstacle is located, node f is the forced neighbor of node x, and node x is the next hop point reached by the parent node p.

[0035] The specific process of the path pruning-based search method is as follows: Figure 4As shown, the search ends when the Openlist has been completely traversed. First, the starting point and target point are input. The starting point S is added to the Openlist. Starting from the starting point, a jump point that meets the conditions is searched, and this jump point is added to the Openlist as the next target position for subsequent traversal. This continues until the target node E is found, at which point a complete search path is formed. The shortest path is then found by backtracking.

[0036] The process of finding the jump point is as follows: Figure 5 As shown, the traversal direction is as follows Figure 6 As shown. The starting point S needs to traverse the surrounding 8 directions (4 linear directions and 4 diagonal directions). Other jump points need to determine the 3 traversable directions based on the position of the parent node and the current jump point. Then, according to the definition and search method of forced neighbors, the next jump point is found and added to the Openlist for subsequent traversal.

[0037] In one embodiment, S3 uses B-spline optimization path pruning to generate a global path, obtaining a feasible trajectory that satisfies robot dynamics constraints. This includes the following steps: S31: Based on the discrete path points on the initial path, obtain the initial B-spline control points through B-spline curve interpolation or fitting; S32: Construct an optimization objective function that includes smoothness cost, collision cost, and feasibility cost; S33: Starting from the initial B-spline control points, the optimization objective function is solved using an optimization algorithm to obtain the optimized B-spline control points; S34: Generate a smooth trajectory based on the optimized B-spline control points.

[0038] This embodiment uses a global path obtained through path pruning and B-spline curves to optimize the trajectory by using the derivative information of control points, thereby improving the smoothness and feasibility of the trajectory.

[0039] In one embodiment, the optimization objective function is a weighted average of a smoothness cost function, a safety cost function, and a feasibility cost function; wherein... The smoothness cost function is constructed as a weighted sum of the norms of the first, second, and third derivatives with respect to the B-spline control points; The safety cost function is constructed as a function of the distance between the B-spline control points and obstacles in the environment; The feasibility cost function is constructed as a constraint function of the first and second derivatives of the B-spline control points, which is used to limit the velocity and acceleration of the smooth trajectory within the robot's dynamic limits.

[0040] The specific execution process of this embodiment includes: First, based on the time step t, the derivative information of the starting and target points, and the global trajectory points obtained from the front-end search, the control points of the B-spline curve are obtained. ,use The third-order uniform B-spline basis function is recursively defined as follows, where the value of i ranges from [0, N]:

[0041] The curve trajectory is Q ,in, To form the node vectors of the 3rd order B-spline, the time step of each node is 1. Using the convex hull property of B-spline curves, it can be seen that optimizing the derivative information of control points Q can achieve the optimization of the global trajectory curve Q. The optimization is as follows. The first, second, and third derivatives of the control points are:

[0042] Define the optimization objective function as follows: ; in Indicates the smoothness of the trajectory. This represents the collision cost of a trajectory, used to determine the safety of the trajectory. This is a feasibility analysis of the trajectory.

[0043] Smoothness: By utilizing the convex hull property of B-splines, the trajectory can be smoothed by minimizing the derivatives v, a, and Jerk of the control points.

[0044]

[0045] Safety: When encountering obstacles, a collision-free trajectory is generated for the collision segment using a global planning method. Control points In a collision-free trajectory In spline curves, Calculate the current distance As the distance increases, the collision penalty decreases; a safety threshold is set at... The piecewise function for collision penalty is defined as follows:

[0046] .

[0047] Feasibility: By ensuring that the robot's speed, acceleration, etc. are within the expected range, this invention designs a feasibility cost function to analyze the dynamic feasibility of the robot's movement process. The following cost function is used to apply a penalty term so that the robot meets the expected requirements.

[0048]

[0049] As new obstacles are discovered during the autonomous exploration process, the objective function... Since the problem can adapt to changes, the L-BFGS quasi-Newton method, which approximates the Hessian inverse from the gradient information, is chosen to solve the optimization problem, which can quickly respond to obstacle information.

[0050] In one embodiment, obstacle avoidance planning for flat terrain is achieved based on path pruning and B-spline optimization. For areas where obstacle avoidance planning is not feasible, a 3D adaptive sampling strategy is used, setting the passable volume and fuselage volume, such as... Figures 7-8 As shown, invalid poses are eliminated by utilizing the collision region between the pose and the elevation map, and an obstacle-crossing planning trajectory is generated. S4 includes the following steps: S41. Based on the elevation map, define the robot's reachability volume and body volume; the reachability volume is used to constrain the contact area between the robot and the ground. The robot body needs to be in contact with the ground, so a path suspended in the air cannot be generated; the body volume is used to constrain the area where the robot body performs collision detection, representing the area where the robot body cannot collide with the surrounding environment. S42: Perform adaptive sampling on the XY two-dimensional plane composed of three-dimensional map information, and project the sampling points into three-dimensional space according to the terrain height information of the elevation map; S43: Based on the reachability volume and fuselage volume, invalid sampling points and poses that collide with the terrain are eliminated to obtain valid three-dimensional sampling points; S44: Based on effective 3D sampling points, a motion planning algorithm is used to generate obstacle-crossing trajectories.

[0051] In one embodiment, the adaptive sampling step in S42 includes setting the sampling probability as an inverse function of the elevation map node density, which reduces sampling in flat areas while increasing the number of samples in narrow areas, effectively improving the problem of insufficient sampling in narrow regions. The sampling points are expanded into three-dimensional space using location and height information obtained from the 2.5D elevation map. Finally, Lazy-PRM* is used to obtain feasible paths.

[0052] This embodiment uses a 2.5D elevation grid map instead of the commonly used 3D map for subsequent obstacle crossing planning. Simultaneously, considering the ground constraints of the tracked robot, it defines the reachable volume that must maintain ground contact and the body volume that must remain collision-free, to eliminate invalid poses in subsequent obstacle crossing planning.

[0053] This invention utilizes Model Predictive Control (MPC) to achieve trajectory tracking, and the effectiveness of the algorithm has been verified in real-world scenarios. For example... Figure 9The path planning diagrams (shown above) and real-world images (shown below) for four scenarios—outdoor scene, indoor flat corridor scene, indoor staircase scene, and obstacle path scene—shown in (a)~(d) demonstrate that the present invention can effectively cope with complex environments and provides a feasible global path planning method.

[0054] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.

[0055] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. An autonomous navigation method for non-flat terrain based on path pruning and trajectory smoothing, characterized in that, The method includes the following steps: S1: Construct an elevation map with three-dimensional map information as a navigation map based on the robot's navigation spatial information; S2: Based on the navigation map, a path pruning strategy is used to perform a global path search to obtain the initial path from the starting point to the target point; S3: Based on the initial path, use B-spline curves to optimize the trajectory and generate a smooth trajectory that satisfies the robot's dynamic constraints; S4: In response to detecting an unavoidable obstacle on the smooth trajectory, a three-dimensional obstacle-crossing trajectory is planned based on the terrain elevation information of the elevation map to generate an obstacle-crossing trajectory that can cross the obstacle; wherein, the robot performs autonomous navigation based on the smooth trajectory or the obstacle-crossing trajectory.

2. The autonomous navigation method for non-flat terrain based on path pruning and trajectory smoothing according to claim 1, characterized in that, The robot acquires environmental point cloud data through its onboard sensors, and converts the point cloud data into an elevation map with three-dimensional grid map information as a navigation map. Each grid stores the elevation information corresponding to the current grid position.

3. The autonomous navigation method for non-flat terrain based on path pruning and trajectory smoothing according to claim 1, characterized in that, The path pruning strategy in S2 includes the following steps: S21: For the current node, determine at least one search direction to be explored; where, if the current node is the starting node, determine all eight adjacent directions around it as directions to be explored; if the current node is not the starting node, determine at most three adjacent directions as directions to be explored based on the direction from its parent node to the current node. S22: For each of the at least one search direction to be explored, start from the current node and perform a jump search along the corresponding direction; during the jump, for the nodes passed through, determine whether there is a forcing neighbor; if at least one forcing neighbor is identified at the current node, or the target node is reached, the jump along the current direction is immediately terminated, and the current node or the target node is identified as a jump point. S23: Add all the jump points found in step S22 to the set of nodes to be expanded; select the node with the lowest cost from the set of nodes to be expanded as the new current node, and repeat steps S21 to S23 until the target node is added to the set of nodes to be expanded. S24: Starting from the target node, backtrack along the parent node pointer to the starting node to form the initial path.

4. The autonomous navigation method for non-flat terrain based on path pruning and trajectory smoothing according to claim 3, characterized in that, During the hopping process, the step of determining whether a node has a forcing neighbor during the S22 process includes: For the current node x encountered during the jump search process, determine whether there is at least one neighbor node f such that the first path cost from the parent node p of the current node x to the neighbor node f without passing through the current node x is greater than the second path cost from the parent node p to the neighbor node f via the current node x. If it exists, then it is determined that the current node x has a forcing neighbor f.

5. The autonomous navigation method for non-flat terrain based on path pruning and trajectory smoothing according to claim 1, characterized in that, S3 includes the following steps: S31: Based on the discrete path points on the initial path, obtain the initial B-spline control points through B-spline curve interpolation or fitting; S32: Construct an optimization objective function that includes smoothness cost, collision cost, and feasibility cost; S33: Starting from the initial B-spline control points, the optimization objective function is solved using an optimization algorithm to obtain the optimized B-spline control points; S34: Generate the smooth trajectory based on the optimized B-spline control points.

6. The autonomous navigation method for non-flat terrain based on path pruning and trajectory smoothing according to claim 5, characterized in that, The optimization objective function is composed of a weighted average of a smoothness cost function, a safety cost function, and a feasibility cost function; wherein... The smoothness cost function is constructed as a weighted sum of the norms of the first, second, and third derivatives of the B-spline control points; The safety cost function is constructed as a function of the distance between the B-spline control points and obstacles in the environment; The feasibility cost function is constructed as a constraint function with respect to the first and second derivatives of the B-spline control points, used to limit the velocity and acceleration of the smooth trajectory within the dynamic limits of the robot.

7. The autonomous navigation method for non-flat terrain based on path pruning and trajectory smoothing according to claim 1, characterized in that, S4 includes the following steps: S41. Based on the terrain height information, define the robot's reachability volume and body volume; the reachability volume is used to constrain the contact area between the robot and the ground, and the body volume is used to constrain the area where the robot body performs collision detection; S42: Perform adaptive sampling on the XY two-dimensional plane formed by the three-dimensional map information, and project the sampling points into three-dimensional space according to the terrain height information of the elevation map; S43: Based on the reachability volume and fuselage volume, invalid sampling points and poses that collide with the terrain are eliminated to obtain valid three-dimensional sampling points; S44: Based on effective three-dimensional sampling points, a motion planning algorithm is used to generate the obstacle-crossing trajectory.

8. The autonomous navigation method for non-flat terrain based on path pruning and trajectory smoothing according to claim 7, characterized in that, The adaptive sampling step in S42 includes setting the sampling probability as the inverse function of the elevation map node density.

Images