Navigation planning methods for wheeled robots in non-flat environments

Abstract

This invention provides a navigation planning method for wheeled robots in non-flat environments. It combines local and global optima, decomposing the long-distance trajectory planning problem in complex environments. First, it solves for the globally optimal trajectory to provide constraints, then solves the navigation problem locally, using local optima to approximate the global optimum, significantly improving solution efficiency. During optimization, the algorithm utilizes the geometric features of the terrain to reduce the dimensionality of the trajectory loss function, considering trajectory landing characteristics. This allows it to adapt well to various complex and extreme terrain topologies, ensuring near-optimal trajectory while also considering real-time performance and wheeled robot landing constraints.

Description

Technical Field

[0001] This invention belongs to the field of wheeled robots, and in particular relates to a navigation planning method for wheeled robots in non-flat environments. Background Technology

[0002] Automated navigation planning is one of the key issues in the field of wheeled robots. In flat environments, wheeled robots can represent the environment using a two-dimensional grid, only needing to consider their own kinematic constraints. However, in complex terrain environments with undulations, the geometric features of the surface itself become significant. Recent works have focused on navigation planning for wheeled robots in non-flat environments, but most of these works only consider the collision-free nature and safety of the trajectory on a local surface, simplifying environmental information and kinematic constraints. However, trajectories that ignore surface geometry and kinematic constraints cannot guarantee that they satisfy the robot's kinematic constraints. Moreover, most methods do not perform backend optimization of the trajectory, thus achieving only suboptimal results in terms of safety, smoothness, and distance.

[0003] The shortcomings of existing technical methods are summarized as follows:

[0004] 1. The terrain is represented by complex 3D structures such as point clouds and 3D grids without taking into account the sparse surface characteristics of the Z-axis of the wheeled robot's motion environment, resulting in a large waste of memory space.

[0005] 2. In the navigation planning process in non-flat environments, the constraints of the wheeled robot's four-wheel landing were not considered. Only discrete paths were solved. In the sampling solution process, the robot's kinematic constraints were not considered. The robot was treated as a point mass for the solution.

[0006] 3. The trajectory of the wheeled robot was directly processed using spatial curve optimization without considering trajectory landing constraints during the trajectory optimization process. Summary of the Invention

[0007] To address the problems of existing technologies, this invention provides a navigation planning method for wheeled robots in non-flat environments. It combines local and global optima, decomposing the long-distance trajectory planning problem in complex environments. First, it solves for the globally optimal trajectory to provide constraints, then solves the navigation problem locally, using local optima to approximate the global optimum, significantly improving solution efficiency. During optimization, the algorithm utilizes the geometric features of the terrain to reduce the dimensionality of the trajectory loss function, considering trajectory landing characteristics. This allows it to adapt well to various complex and extreme terrain topologies, ensuring near-optimal trajectory while also considering real-time performance and wheeled robot landing constraints.

[0008] The technical solution of the present invention is as follows:

[0009] A navigation planning method for wheeled robots in non-flat environments includes the following steps:

[0010] Step 1: Calculation of Road Marker Points in Non-flat Environments

[0011] In the special non-flat environment represented by the Surfel map, the relay points of the navigation trajectory are searched out, and the long-distance navigation problem is decomposed into a navigation problem between relay points.

[0012] Step 2: Trajectory Sampling in Non-flat Environments

[0013] Trajectory sampling that conforms to kinematic constraints is performed on the Surfel map, and the sampling process quickly approaches the target point to solve for the initial trajectory.

[0014] Step 3: Optimization of Non-flat Trajectory

[0015] The initial trajectory is optimized for safety / smoothness and dynamic feasibility.

[0016] The safe and smooth kinematic feasible terms are different optimization terms in a unified optimization function.

[0017] For each control point, three losses can be calculated. A Surfel map is proposed, which can directly index local surface information and provides constraints in the calculation of these three loss terms. That is, the surface geometric information needed to calculate these three loss terms can be directly indexed from this map.

[0018] Preferably, in the aforementioned navigation planning method for wheeled robots in non-flat environments,

[0019] In step one, the Surfel map consists of a hash table and linked lists. The hash table indexes the map regions in the XOY coordinates, while the linked list structure represents the sparse surface regions in the Z direction.

[0020] Preferably, in the aforementioned navigation planning method for wheeled robots in non-flat environments, the solution for relay points in step one is divided into the following parts:

[0021] The RRT* algorithm based on the Surfel map is used to find the path Traj with the minimum distance cost from the starting pose to the target pose. coarse ={S1,S2,...,S} n}. Where S i A Surfel can be represented as S. i ={x i ,y i ,z i ,a i ,b i ,c i ,di ,σ i ,γ i ,s i ,φ i}; records the current Surfel's index in the hash table and local surface features;

[0022] Relay point pose calculation; for Traj coarse ={S1,S2,...,S} n The segment is uniformly divided into m-1 segments, resulting in a series of segmentation points Milestone = {S1, S2, ..., S3}. 1+k ,...,S 1+mk}. S 1+tk Find the Surfel corresponding to a split point on the map; extend the paths near the split point to obtain Traj. subi ={S i-k ,S i-k+1 ,...,S i ,...,S i+k By selecting the midpoint of this path segment and using the PCA algorithm to calculate the pose, the relay point T can be obtained. i Then, solve the relay point sequence Milestone={T1,T2,...,T m}

[0023] Preferably, in the aforementioned navigation planning method for wheeled robots in non-flat environments, step two includes:

[0024] The heuristic function for the sampling points is: c = g + h;

[0025] g represents the current sampling point N i = {x, y, z, yaw, g, h}, where h represents the cost of reaching the target pose from the current heuristic point, and h = max{astar, dubin}, which is the maximum value of the astar distance and dubin distance on the Surfel map;

[0026] State point sampling: Taking a left turn as an example, assume the current node's state The landing table Surfel S obtained by indexing i There is a T-shirt wi The robot's z-axis is parallel to the Surfel normal vector, and its x-axis aligns with the direction of the vehicle's head. Assume the vehicle's coordinates after state extension in the horizontal plane are [coordinates], and its pose is [pose]. The pose obtained by state expansion on Surfel is:

[0027] The value depends on the sampling state; for example, when moving forward and turning left.

[0028]

[0029] To ensure the robot conforms as closely to the ground as possible, its pose needs to be adjusted. Make corrections;

[0030] Corresponding spatial coordinates P j1 ={x j1 ,y j1 ,z j1}, S can be obtained from the index in the Surfel map. j We can approximate this by assuming the robot always moves close to the ground, thus establishing a rotation axis. right Perform rotation correction so that the z-axis of the corrected pose is aligned with the S-axis. j Parallel normal vectors:

[0031]

[0032] Considering that wheeled robots need to move on the ground, we Perform translation corrections and adjust the pose to S. j The projected distance is the height h of the robot's center of mass from the ground surface;

[0033] T wj =T wj2 ·T tran ,

[0034] Calculate pose T wj By determining the yaw angle and spatial position, the next state can be obtained.

[0035] Preferably, in the aforementioned navigation planning method for wheeled robots in non-flat environments, step three includes:

[0036] Elastic band loss on non-flat surface trajectory:

[0037]

[0038] e k =a k x k +b k y k +c k z k +d k

[0039] Q k This is a trajectory point in the previously sampled trajectory. For Q k S corresponds to the Surfel mapk The nearby average plane π k :a k x+b k y+c k z+d k The projection onto 0. During gradient calculation, for the loss term f... si Considering only control point Q i The gradient;

[0040]

[0041] Safety loss due to non-flat surface trajectory:

[0042] The trajectory point Q to be optimized i We extend d along the trajectory normal direction. ext =min(d ext ,d obs ), d obs This is the distance from the trajectory point to the nearest unsafe Surfel during the extension process; this allows us to find two child nodes on either side of the control point.

[0043] For control node Q i We consider all child nodes Q within a fixed-length window of the initial sampling trajectory;

[0044]

[0045] Project set Q onto T wi Obtained on the xoy plane Then Transform to T iw middle,

[0046]

[0047] Due to sets Since the z-coordinate is 0, we only consider the x and y coordinates to obtain the set.

[0048]

[0049] The problem can now be transformed into T. iw The problem of gradient calculation on the xoy plane;

[0050] For sets Constructing convex polygons:

[0051]

[0052] Where n zi It is by The number of hyperplanes obtained by constructing the set, Aiz and b iz It is a pair of descriptors for a hyperplane;

[0053] Construct safety constraints

[0054]

[0055] Obstacle loss term f c The calculation scheme is as follows

[0056]

[0057] The actual gradient needs to be transformed to the world coordinate system.

[0058]

[0059] Non-flat surface trajectory kinematic loss

[0060]

[0061] The present invention has the following beneficial effects:

[0062] 1. This invention proposes a novel map representation, Surfel Link. Surfel Link uniformly divides the terrain in non-flat environments into a two-dimensional space and uses linked lists to distinguish multiple layers of the terrain. Each node in the linked list stores the parameterized results of the current local terrain geometry, efficiently utilizing memory space and facilitating rapid indexing of map information.

[0063] 2. A complete solution was developed for navigation tasks of wheeled robots in multi-layered non-flat environments. First, HybridA was extended to non-flat environments, resulting in an initialization path that meets vehicle kinematic constraints and four-wheel landing conditions. Second, B-splines were used to parameterize the trajectory, optimizing its safety, smoothness, and dynamic feasibility. During optimization, the loss function of the objective function was mapped to a function representation of the local terrain, achieving dimensionality reduction of the optimization problem while approximately satisfying the four-wheel landing assumption.

[0064] Other advantages, objectives and features of the present invention will become apparent in part from the following description, and in part from those skilled in the art through study and practice of the invention. Attached Figure Description

[0065] Figure 1 The Surfel map data structure is shown in one embodiment of the navigation planning method for wheeled robots in non-flat environments proposed in this invention.

[0066] Figure 2This is a flowchart of an embodiment of the navigation planning method for wheeled robots in non-flat environments proposed in this invention;

[0067] Figure 3 This is a schematic diagram illustrating the trajectory sampling principle in a non-flat environment, as presented in one embodiment of the navigation planning method for wheeled robots in non-flat environments proposed in this invention. Detailed Implementation

[0068] The present invention will now be described in further detail with reference to the accompanying drawings, so that those skilled in the art can implement it based on the description.

[0069] like Figure 1 and Figure 2 As shown, this invention provides a navigation planning method for a wheeled robot in a non-flat environment, comprising the following steps:

[0070] 1. Road marker calculation in non-flat environments:

[0071] Search for a feasible path with the optimal distance in the Surfel space. Given the poses T of the starting and target points. star ,T end The RRT* algorithm in Surfel space is shown in the table below.

[0072]

[0073]

[0074] Generating relay points using discrete trajectories

[0075]

[0076] 2. Trajectory sampling in non-flat environments

[0077] Taking a left turn as an example, the next state after sampling is as follows: Figure 3 As shown. Assume the current node's state... The landing table Surfel S obtained by indexing i There is a T-shirt iw The robot's z-axis is parallel to the Surfel normal vector, and its x-axis aligns with the direction of the vehicle's head. Assume the vehicle's coordinates after state extension in the horizontal plane are [coordinates], and its pose is [pose]. The pose obtained by state expansion on Surfel is: The value depends on the sampling state; for example, when moving forward and turning left.

[0078]

[0079] relative pose Make corrections. Corresponding spatial coordinates The index yields SurfelS j We can approximate this as the robot always moving while remaining in contact with the ground. Constructing the axis of rotation... right Perform rotation correction so that the z-axis of the corrected pose is aligned with the S-axis. j Parallel normal vectors:

[0080]

[0081] Considering that wheeled robots need to move on the ground, Perform translation corrections and adjust the pose to S. j The projected distance is the height h of the robot's center of mass from the ground surface.

[0082]

[0083] Calculate pose T wj By determining the yaw angle and spatial position, the next state can be obtained.

[0084] In addition to the nodes expanded in the manner described above, optimal Dubin trajectories are calculated for some nodes. The start and end points of the trajectory are planar projections of the current and final states. During trajectory generation, obstacles and impassable areas in the environment are not considered. Then, the generated trajectory is discretized, and node expansion is performed on the Surfel map based on the actions of the discrete points. This node expansion method can greatly improve the efficiency of trajectory search when the current node and the end point are on the same surface level.

[0085] For state nodes and position T wi The spatial coordinates of the four landing points and their respective projections P on the corresponding Surfel can be calculated. wpro1 P wpro2 P wpro3 P wpro4 For P wpro1 P wpro2 P wpro3 Fitting to obtain a plane

[0086] π: a pj x+b pj y+c pj z+d pj =0

[0087] If P wpro4 Distance d to plane π π <d th If the current state node is 'safe', then it is a safe node; otherwise, it is an unsafe node. Of course, if the index yields four Surfel or S...j This state node is inherently insecure and is therefore rejected.

[0088] If the state node is a safe sampling point, then a heuristic function needs to be calculated. Assume the current node is... The target relay point is The heuristic function is then...

[0089] h ueven (X s ,X g ) = max(dubin(X) s ,X g ),astar(X s ,X g ))

[0090] The dubin curve has a readily available calculation scheme, and astar is the path length of astar in the Surfel map. The algorithm is as follows:

[0091]

[0092] In Line 2, `CaculateCost` calculates the movement cost `g` and heuristic cost `h` for the current node. The heuristic cost is directly calculated using the Manhattan distance. i ={x i ,y i ,z i} represents the discrete index of the current node in the Surfel map. i This represents the index position in the linked list. The priority queue PriorityQ is implemented using a max-heap, prioritizing elements with a smaller total cost g+h, which is beneficial for the search to proceed towards the target. Line 8 performs a neighbor search for each element, unlike the direct expansion in a flat environment. Because in the Surfel map, {x i ,y i This corresponds to a linked list, and we need to filter the nodes by their y-coordinates.

[0093] 3. Non-flat trajectory optimization

[0094] The optimization function can be defined as:

[0095] f=λ1f s +λ2f c +λ3(f v +f a )

[0096] Where f s and f c These are the smooth term and the obstacle term, fv and f a These are constraints on velocity and acceleration terms. λ1, λ2, and λ3 are the optimization weights for the smoothing term, safety term, and dynamics term, respectively.

[0097] The expression for the smoothing term function is as follows:

[0098]

[0099]

[0100] e k =a k x k +b k y k +c k z k +d k

[0101] in For Q k In the average plane π k :a k x+b k y+c k z+d k The projection onto π = 0. The mean plane π. k For position T kw The xoy plane.

[0102] In the process of calculating the gradient, for the loss term f si Considering only control point Q i The gradient, because the loss term is at control point Q. i The local surface conditions in the vicinity should be taken into consideration.

[0103]

[0104] In practical optimization, since we constrain the optimization range of the control points, π is within the constraint range. k It is sufficient to approximate the surrounding local surface information and is calculated only once during the entire optimization process.

[0105] To ensure the trajectory stays as far away from obstacles as possible, we need to extract semantic information from the environment. For each control point Q... i We extend d along the trajectory normal direction. ext =min(d ext ,d obs ), where d obs This is the distance from the trajectory point to the nearest unsafe Surfel during the extension process. This allows us to find two child nodes on either side of the control point. Based on the trajectory control points and their corresponding child nodes, we can construct a corridor in a non-flat environment, where the corridor's boundaries are unsafe areas or extended edges. For control node Q... i We consider a set Q consisting of all child nodes in a window of fixed length.

[0106]

[0107] Project set Q onto T iw Obtained on the xoy plane Then Transform to T iw middle,

[0108]

[0109] Due to sets Since the z-coordinate is 0, we only consider the x and y coordinates to obtain the set.

[0110]

[0111] The problem can now be transformed into T. iw The problem of calculating the gradient on the xoy plane. Using coordinate system T... iw The origin (i.e., the control point Q on the path) i Centered on the set Constructing convex polygons:

[0112]

[0113] Where n zi It is by The number of hyperplanes obtained by constructing the set, A iz and b iz It is a hyperplane with a pair of descriptors. Therefore, the necessary and sufficient condition for the wheeled robot to be safe at this control point can be stated as: the robot's boundary point p1 is on T iw The projections onto the xoy plane all fall within the convex polygon.

[0114]

[0115] Obstacle loss term f c Can be written

[0116]

[0117] Due to control point Q i The obstacle loss gradient at point T is iw The gradient is calculated in the world coordinate system, therefore the actual gradient needs to be transformed to the world coordinate system.

[0118]

[0119] To limit the robot's speed and acceleration, we will penalize excessively large speed and acceleration terms to ensure V i ∈[-v max ,v max A i ∈[-a max ,a max The velocity control point and acceleration control point can be calculated as follows:

[0120]

[0121] The velocity loss function and acceleration loss function are calculated as follows:

[0122]

[0123] To ensure the consistency of the gradient direction, we... We make an approximation. We will apply the gradient at T. iw Projecting onto the xoy plane, we obtain This is the gradient actually used to update the control points, as shown in the figure. Although this is an approximation, because we ensure that adjacent nodes Q are within the same range when expanding nodes at the front end... i+1 Q i In coordinate system T iw The threshold for the drop on the lower z-axis (if the threshold is too large, it indicates that the path is truncated and is unsafe), and the safe node Q. i Both the roll and pitch angles are limited (in our experiment we designed them to be ±20°), therefore With T iw The angle between the xoy planes will be very small, therefore

[0124] This invention provides a navigation method for wheeled robots in non-flat environments, based on a SurfelMap map structure using Surfel and linked lists. The map employs Surfel to represent the safety and geometric features of the local terrain, and utilizes linked lists to efficiently index multi-layered surfaces, making it suitable for 3D task scenarios of wheeled robots in complex topographical terrain. Based on the map representation, a trajectory planning and optimization algorithm satisfying kinematic constraints in non-flat environments is proposed, comprising a non-flat environment landmark point calculation module, a non-flat environment trajectory sampling module, and a non-flat trajectory optimization module. The algorithm optimizes trajectory smoothness, safety, and kinematic feasibility using a time-space decoupling approach. In the optimization problem, the algorithm uses a local function approximation method to map the loss of each control point to a function representation of the terrain, ensuring that the trajectory satisfies the surface landing constraints and achieving dimensionality reduction while maintaining optimization accuracy.

[0125] Although the embodiments of the present invention have been disclosed above, they are not limited to the applications listed in the specification and embodiments. They can be applied to various fields suitable for the present invention. For those skilled in the art, other modifications can be easily made. Therefore, without departing from the general concept defined by the claims and their equivalents, the present invention is not limited to the specific details.

Claims

1. A navigation planning method for a wheeled robot in a non-flat environment, characterized in that, Includes the following steps: Step 1: Calculation of Road Marker Points in Non-flat Environments In the special non-flat environment represented by the Surfel map, the relay points of the navigation trajectory are searched out, and the long-distance navigation problem is decomposed into a navigation problem between relay points. Step 2: Trajectory Sampling in Non-flat Environments Trajectory sampling that conforms to kinematic constraints is performed on the Surfel map, and the sampling process quickly approaches the target point to solve for the initial trajectory. Step 3: Optimization of Non-flat Trajectory The initial trajectory is optimized for safety / smoothness and dynamic feasibility; The solution for the relay point in step one consists of the following parts: The RRT* algorithm based on the Surfel map is used to find the path Traj with the minimum distance cost from the starting pose to the target pose. coarse ={S1,S2,...,S} n }, where S i A Surfel can be represented as S i ={x i ,y i ,z i ,a i ,b i ,c i ,d i ,σ i ,γ i ,s i ,φ i }; records the current Surfel Indexes and local surface features in hash tables; { x i ,y i } represents the corresponding linked list, z i This indicates the index position in the linked list. Relay point pose calculation; for Traj coarse ={S1,S2,...,S} n The segment is uniformly divided into m-1 segments, resulting in a series of segmentation points Milestone = {S1, S2, ..., S3}. 1+k ,...,S 1+mk }, S 1+tk Let be the Surfel corresponding to a split point on the map; where t is an integer ranging from 1 to m, and the path near the split point is extended to obtain . Traj subi ={S i-k ,S i-k+1 ,...,S i ,...,S i+k By selecting the midpoint of this path segment and using the PCA algorithm to calculate the pose, the relay point T can be obtained. i Then, solve the relay point sequence Milestone={T1,T2,...,T m }; Step two includes: The heuristic function for the sampling points is: c = g + h; g represents the current sampling point N i ={x i ,y i ,z i The movement cost of x,yaw,g,h}, where x i ,y i ,z i Here, yaw is the vehicle's 3D coordinates, h represents the cost of reaching the target pose from the current heuristic point, and h = max{astar,dubin}, which is the maximum value of the distance between astar and dubin on the Surfel map. State point sampling: Taking a left turn as an example, assume the current node's state The landing table Surfel S obtained by indexing i There is a T-shirt wi The robot's z-axis is parallel to the Surfel normal vector, and its x-axis aligns with the direction of the vehicle's head. Assume the vehicle's coordinate pose after state extension in the horizontal plane is... The pose obtained by state expansion on Surfel is: Represents the vehicle's coordinate pose after state extension in the horizontal plane. The value depends on the sampling state; for example, when moving forward and turning left. To ensure the robot conforms as closely to the ground as possible, its pose needs to be adjusted. Make corrections; Corresponding spatial coordinates The index in the Surfel map can be obtained as S j We can approximate this by assuming the robot always moves close to the ground, thus establishing a rotation axis. Constructed vectors, z-axis, Earth's surface z-axis, relative to Perform rotation correction so that the z-axis of the corrected pose is aligned with the S-axis. j Parallel normal vectors: k1, k2, k3 are The three coordinates are θ, which is the yaw angle calculated based on the current attitude. Considering that wheeled robots need to move on the ground, we Perform translation corrections and adjust the pose to S. j The projected distance is the height h of the robot's center of mass from the ground surface; Calculate pose T wj By determining the yaw angle and spatial position, the next state can be obtained.

2. The navigation planning method for a wheeled robot in a non-flat environment as described in claim 1, characterized in that, In step one, the Surfel map consists of a hash table and a linked list. The hash table indexes the map regions in the XOY coordinates, and the linked list structure represents the sparse surface regions in the Z direction.

3. The navigation planning method for a wheeled robot in a non-flat environment as described in claim 1, characterized in that, Step three includes: Elastic band loss on non-flat surface trajectory: e i =a i x i +b i y i +c i z i +d i Q i This refers to a trajectory point in the previously sampled trajectory. For Q i S corresponds to the Surfel map i The nearby average plane π i :a i x+b i y+c i z+d i During the gradient calculation of the projection onto 0, for the loss term f... si Considering only control point Q i The gradient; Safety loss due to non-flat surface trajectory: The trajectory point Q to be optimized i We extend d along the trajectory normal direction. ext =min(d ext ,d obs ), d obs This is the distance from the trajectory point to the nearest unsafe Surfel during the extension process; this allows us to find two child nodes Q on either side of the control point. isub1 Q isub2 ; For control node Q i We consider all child nodes Q within a fixed-length window of the initial sampling trajectory; Project set Q onto T wi Obtained on the xoy plane Then Transform to T iw China T iw This indicates the transformation from the current pose to the world pose. Due to sets Since the z-coordinate is 0, we only consider the x and y coordinates to obtain the set. The problem can now be transformed into T. iw The problem of gradient calculation on the xoy plane; For sets Constructing convex polygons: P i H ={q∈R 2 :A i q≤b i }, Where n zi It is by The number of hyperplanes obtained by constructing the set, A iz and b iz It is a hyperplane with a pair of descriptors; construct security constraints. Obstacle loss term f c The calculation scheme is as follows The actual gradient needs to be transformed to the world coordinate system. Non-flat surface trajectory kinematic loss

Images