Navigation planning method for wheeled robot in non-flat environment

Abstract

The invention provides a wheeled robot navigation planning method in a non-flat environment, which combines local optimum and global optimum, disassembles a long-distance trajectory planning problem in a complex environment, firstly solves a global distance optimum trajectory to provide constraints, then locally solves a navigation problem, approaches the global optimum by using the local optimum, and finally solves the navigation problem by using the global optimum. And the solving efficiency is greatly improved. In the optimization process, the algorithm uses the geometrical characteristics of the ground surface to carry out dimension reduction on the loss function of the track, the landing characteristics of the track are considered, the method can well adapt to various complex and extreme ground surface topological structures, and the real-time performance and the landing constraint of the wheeled robot are considered while it is guaranteed that the track is approximately optimal.

Description

Technical Field

[0001] The invention belongs to the field of wheeled robots, and in particular relates to a navigation planning method for a wheeled robot in a non-flat environment. Background Art

[0002] Automatic navigation planning is one of the key issues in the field of wheeled robots. In a flat environment, a wheeled robot can use a two-dimensional grid to represent the environment and only needs to consider its own kinematic constraints. However, in a complex terrain environment with undulations, the geometric characteristics of the surface itself become important. Recently, a lot of work has focused on studying the navigation planning of wheeled robots in non-flat environments, but most of the work only considers the collision-free and safety of the trajectory on the local surface, simplifying the environmental information and kinematic constraints. However, trajectories that ignore the geometric information of the surface and kinematic constraints cannot guarantee that the robot's kinematic constraints are met. Not only that, most methods do not perform back-end optimization on the trajectory, so they can only be suboptimal in terms of safety, smoothness, and distance.

[0003] The shortcomings of the prior art methods are summarized as follows:

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

[0005] 2. In the navigation planning process in a non-flat environment, the constraints of the wheeled robot's four-wheel landing are not considered, and only the discrete path is solved. The robot's kinematic constraints are not considered in the sampling solution process, and the robot is solved as a particle.

[0006] 3. The trajectory of the wheeled robot is directly processed by spatial curve optimization, without considering the trajectory landing constraints in the trajectory optimization process. Summary of the invention

[0007] In view of the problems existing in the prior art, the present invention provides a navigation planning method for a wheeled robot in a non-flat environment, which combines local optimization with global optimization, decomposes the long-distance trajectory planning problem in a complex environment, first solves the global distance optimal trajectory to provide constraints, then solves the navigation problem locally, and uses the local optimal to approach the global optimal, which greatly improves the solution efficiency. During the optimization process, the algorithm uses the geometric characteristics of the surface to reduce the dimension of the loss function of the trajectory, takes into account the trajectory landing characteristics, and can adapt well to various complex and extreme surface topological structures, while ensuring that the trajectory is approximately optimal and taking into account real-time performance and wheeled robot landing constraints.

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

[0009] The navigation planning method of a wheeled robot in a non-flat environment comprises the following steps:

[0010] Step 1: Calculation of landmark points in non-flat environments

[0011] Search for the relay points of the navigation trajectory in the special non-flat environment represented by the Surfel map, and decompose the long-distance navigation problem into the navigation problem between relay points;

[0012] Step 2: Non-flat environment trajectory sampling

[0013] The trajectory sampling that meets the kinematic constraints is performed in the Surfel map, and the sampling process quickly approaches the target point to solve the initial trajectory;

[0014] Step 3: Non-flat trajectory optimization

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

[0016] Safe smooth kinematics feasible terms are different optimization terms in a unified optimization function.

[0017] For each control point, these three losses can be calculated. The surfel map is proposed to directly index the local information of the surface and provide constraints in the calculation process of these three loss items. That is, the surface geometric information needed in the calculation process of these three loss items can be directly indexed in the map.

[0018] Preferably, in the navigation planning method for a wheeled robot in a non-flat environment,

[0019] In step 1, the Surfel map is composed of a hash table and a linked list. The hash table indexes the map area on the XOY coordinate, and the linked list structure represents the sparse surface area in the Z direction.

[0020] Preferably, in the navigation planning method for a wheeled robot in a non-flat environment, the solution of the relay point in step 1 is divided into the following parts:

[0021] The RRT* algorithm based on the Surfel map is used to solve the path Traj with the minimum distance cost from the starting position to the target position. coarse ={S1,S2,...,S n}. Among them, S i Represents a surfel, which can be denoted by S i ={x i ,y i ,z i ,a i ,b i ,c i ,di ,σ i ,γ i ,s i ,φ i}; Records the index of the current surfel in the hash table and the local surface features;

[0022] Relay point pose calculation; Traj coarse ={S1,S2,...,S n} is evenly divided into m-1 segments, and a series of segmentation points Milestone = {S1, S 1+k ,...,S 1+mk}. 1+tk is the surfel corresponding to a segmentation point in the map; the path near the segmentation point is extended to obtain Traj subi ={S i-k ,S i-k+1 ,...,S i ,...,S i+k}. Select the midpoint of this path and use the PCA algorithm to solve the pose to get the relay point T i , and then solve the relay point sequence Milestone={T1,T2,...,T m}.

[0023] Preferably, in the navigation planning method for a wheeled robot in a non-flat environment, step 2 comprises:

[0024] The heuristic function of the sampling point 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 inspiration point, and h = max{astar,dubin}, which is the maximum value of the astar distance and the dubin distance on the Surfel map;

[0026] State point sampling: Taking a left turn as an example, assuming that the state of the current node is Indexed landing surface Surfel S i , with posture T wi , the robot's z-axis is parallel to the Surfel normal vector, and the x-axis is consistent with the direction of the vehicle head; assuming that the coordinates of the vehicle after state expansion in the horizontal plane are Then the posture obtained by state expansion on Surfel is

[0027] The value of is related to the sampling state. Take forward and left turn as an example.

[0028]

[0029] In order to ensure that the robot is as close to the ground as possible, the posture make corrections;

[0030] The corresponding space coordinate P j1 ={x j1 ,y j1 ,z j1}, the index in the Surfel map can be obtained as S j ; It can be roughly considered that the robot always moves along the ground, constructing the rotation axis right Perform rotation correction so that the z-axis of the corrected posture is aligned with S j Normal vector parallel:

[0031]

[0032] Considering that wheeled robots need to move on the ground, we Perform translation correction and adjust the posture to S j The projection 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 the pose T wj The yaw angle and spatial position can be used to get the next state

[0035] Preferably, in the navigation planning method for a wheeled robot in a non-flat environment, step three comprises:

[0036] Elastic band loss for 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 A trajectory point in the trajectory obtained for the previous sample. Q k Corresponds to S on the Surfel mapk The average plane near π k :a k x+b k y+c k z+d k = 0. In the process of calculating the gradient, for the loss term f si , only consider the control point Q i The gradient of

[0040]

[0041] Trajectory safety loss on non-flat surfaces:

[0042] The trajectory point Q to be optimized i , we extend along the normal direction of the trajectory d ext =min(d ext ,d obs ), d obs It is the distance from the trajectory point to the nearest unsafe surfel during the extension process; in this way, two child nodes can be found on both sides of the control point

[0043] For the control node Q i , we consider all child nodes Q in a fixed-length window of the initial sampled trajectory;

[0044]

[0045] Project the set Q into T wi The xoy plane is Then Convert to T iw middle,

[0046]

[0047] Because of the collection The z coordinate is 0, so only the x and y coordinates are considered, and the set is obtained

[0048]

[0049] At this point the problem can be transformed into T iw The problem of gradient calculation on the xoy plane under ;

[0050] Pair Collection Constructing a convex polygon:

[0051]

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

[0053] Constructing safety constraints

[0054]

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

[0056]

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

[0058]

[0059] Kinematic losses for trajectory on non-flat surfaces

[0060]

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

[0062] 1. The present invention proposes a new map representation Surfel Link. Surfel Link divides the surface in a non-flat environment evenly into two-dimensional space and uses a linked list to distinguish multiple layers of the surface. Each node in the linked list stores the parameterized results of the current local surface geometric features, which efficiently utilizes the memory space and is conducive to the rapid indexing of map information.

[0063] 2. We developed a complete solution for the navigation task of a wheeled robot in a multi-layer non-flat environment. First, we generalized HybridA to a non-flat environment and obtained an initialization path that meets the vehicle kinematic constraints and four-wheel landing conditions. Second, we used B-splines to parameterize the trajectory and optimized the safety, smoothness, and dynamic feasibility of the trajectory. During the optimization process, we mapped the loss of the objective function to the functional representation of the local surface, 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 be embodied in part through the following description, and in part will be understood by those skilled in the art through study and practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 The data structure of the Surfel map in one embodiment of the navigation planning method for a wheeled robot in a non-flat environment proposed by the present invention;

[0066] Figure 2A flowchart of an embodiment of a navigation planning method for a wheeled robot in a non-flat environment proposed by the present invention;

[0067] Figure 3 This is a schematic diagram of the non-flat environment trajectory sampling principle in one embodiment of the wheeled robot navigation planning method in a non-flat environment proposed by the present invention. DETAILED DESCRIPTION

[0068] The present invention is further described in detail below in conjunction with the accompanying drawings so that those skilled in the art can implement the invention with reference to the description.

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

[0070] 1. Calculation of landmark points in non-flat environments:

[0071] Search for a feasible path with the best distance in Surfel space. Given the starting point and the target point position T star ,T end , the RRT* algorithm in Surfel space is shown in the following table.

[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 Figure 3 Assume that the current node status is Indexed landing surface Surfel S i , with posture T iw The robot's z-axis is parallel to the Surfel normal vector, and the x-axis is consistent with the direction of the vehicle head. Assume that the coordinates of the vehicle after state expansion in the horizontal plane are Then the posture obtained by state expansion on Surfel is The value of is related to the sampling state. Take forward and left turn as an example.

[0078]

[0079] Position Make corrections. The corresponding space coordinates The index can be obtained by SurfelS j . It can be roughly considered that the robot always moves along the ground. Construct the rotation axis right Perform rotation correction so that the z-axis of the corrected posture is aligned with S j Normal vector parallel:

[0080]

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

[0082]

[0083] Calculate the pose T wj The yaw angle and spatial position can be used to get the next state

[0084] In addition to the nodes expanded in the above way, the optimal Dubin trajectory is also calculated for some nodes. The starting point and end point of the trajectory are the planar projections of the current state and the end state. In the process of trajectory generation, we do not consider obstacles and impassable areas in the environment. Then, we discretize the generated trajectory and expand the nodes in the Surfel map according to the actions of the discrete points. If the current node and the end point are on the same surface level, this node expansion method can greatly improve the efficiency of trajectory search.

[0085] For the state node and pose T wi , the spatial coordinates of the four-wheel landing points and their respective projections on the corresponding Surfel can be calculated wpro1 , P wpro2 , P wpro3 , P wpro4 . wpro1 , P wpro2 , P wpro3 Fitting to obtain plane

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

[0087] If P wpro4 The distance d to the plane π π <d th , then the current state node is a safe node, otherwise it is an unsafe node. Of course, if the four Surfel or Sj It is not safe in itself. This state node is an unsafe state node and will not be adopted.

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

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

[0090] The Dubin curve has a ready-made calculation scheme, and astar is the length of the astar path in the Surfel map. The algorithm is as follows

[0091]

[0092] CaculateCost in Line2 is used to calculate the movement cost g and heuristic cost h of the current node. The heuristic cost is directly calculated using the Manhattan distance. i ={x i ,y i ,z i}, which is the discrete index of the current node in the Surfel map. i Indicates the index position in the linked list. The priority queue PriorityQ is implemented using a large root heap. Elements with a smaller total cost g+h are prioritized, which is conducive to searching in the target direction. Line8 searches for neighbor nodes for each element, which is different from direct expansion in a flat environment. Because in the Surfel map, {x i ,y i} corresponds to a linked list, and the vertical coordinates of the nodes in the linked list need to be filtered.

[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 are the smoothness term and the obstacle term, respectively, and fv and f a are the constraints of velocity and acceleration terms. λ1,λ2,λ3 are the optimization weights of smoothness term, safety term and dynamics term respectively.

[0097] The smoothing function expression is as follows:

[0098]

[0099]

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

[0101] in Q k In the mean plane π k :a k x+b k y+c k z+d k = Projection on 0. Average plane π k is the posture T kw The xoy plane.

[0102] In the process of calculating the gradient, for the loss term f si , only consider the control point Q i The gradient of , because the loss term is at the control point Q i The nearby local surface is taken into consideration.

[0103]

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

[0105] In order to ensure that the trajectory is as far away from obstacles as possible, we need to extract the semantic information of the environment. i , we extend along the normal direction of the trajectory d ext =min(d ext ,d obs ), where d obs It is the distance from the trajectory point to the nearest unsafe Surfel during the extension process. In this way, two child nodes can be found on both sides of the control point. According to the trajectory control points and the corresponding child nodes, we can construct a corridor in a non-flat environment, and the boundary of the corridor is the unsafe area or the extended edge. i , we consider the set Q consisting of all child nodes in a window of fixed length.

[0106]

[0107] Project the set Q into T iw The xoy plane is Then Convert to T iw middle,

[0108]

[0109] Because of the collection The z coordinate is 0, so only the x and y coordinates are considered, and the set is obtained

[0110]

[0111] At this point the problem can be transformed into T iw The gradient calculation problem on the xoy plane under . iw The origin (i.e. the control point Q on the path i ) as the center, the set Constructing a convex polygon:

[0112]

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

[0114]

[0115] Obstacle loss term f c Can write

[0116]

[0117] Since the control point Q i The obstacle loss gradient at T iw Therefore, the actual gradient needs to be transformed to the world coordinate system,

[0118]

[0119] In order to limit the speed and acceleration of the robot, we will penalize excessive speed and acceleration to ensure that 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 speed loss function and acceleration loss function are calculated as follows:

[0122]

[0123] In order to ensure the consistency of the gradient direction, we As an approximation, we will use the gradient at T iw Projecting on the xoy plane, we get This is the actual gradient used to update the control point, as shown in the figure. Although this is an approximate process, we will ensure that the two adjacent nodes Q i+1 , Q i In the coordinate system T iw The threshold of the drop on the lower z-axis (if the threshold is too large, it means that the path is truncated and unsafe), and the safety node Q i The roll and pitch angles are limited (in the experiment we designed them to be ±20°), so With T iw The angle of the xoy surface will be very small, so

[0124] The present invention provides a navigation method for a wheeled robot in a non-flat environment, based on a map structure SurfelMap of Surfel and linked lists. The map uses Surfel to characterize the safety and geometric features of the local surface, and uses linked lists to efficiently index multi-layer surfaces, which is suitable for three-dimensional task scenarios of wheeled robots under complex topological terrains. On the basis of map representation, a trajectory planning and optimization algorithm that meets kinematic constraints in a non-flat environment is proposed, which includes a non-flat environment landmark point solution module, a non-flat environment trajectory sampling module, and a non-flat trajectory optimization module. The algorithm optimizes the smoothness, safety, and kinematic feasibility of the trajectory by time-space decoupling. In the optimization problem, the algorithm uses a local function approximation method to map the loss of each control point to the function representation of the terrain, which not only ensures that the trajectory meets the surface landing constraints, but also achieves dimensionality reduction of the problem while ensuring the optimization accuracy.

[0125] Although the embodiments of the present invention have been disclosed as above, they are not limited to the applications listed in the specification and the implementation modes. They can be fully applied to various fields suitable for the present invention. For those familiar with the art, additional modifications can be easily implemented. Therefore, without departing from the general concept defined by the claims and the scope of equivalents, the present invention is not limited to specific details.

Claims

1. A navigation planning method for a wheeled robot in a non-flat environment, characterized in that: The following steps are involved: Step 1: Calculation of landmark points in non-flat environments Search for the relay points of the navigation trajectory in the special non-flat environment represented by the Surfel map, and decompose the long-distance navigation problem into the navigation problem between relay points; Step 2: Non-flat environment trajectory sampling The trajectory sampling that meets the kinematic constraints is performed in the Surfel map, and the sampling process quickly approaches the target point to solve the initial trajectory; Step 3: Non-flat trajectory optimization The initial trajectory is optimized for safety / smoothness and dynamic feasibility.

2. The navigation planning method for a wheeled robot in a non-flat environment as claimed in claim 1, characterized in that: In the step 1, the Surfel map is composed of a hash table and a linked list. The hash table is used to index the map area on the XOY coordinate, and the linked list structure represents the sparse surface area in the Z direction.

3. The navigation planning method for a wheeled robot in a non-flat environment as claimed in claim 1, characterized in that: The solution of the relay point in step 1 is divided into the following parts: The RRT* algorithm based on the Surfel map is used to solve the path Traj with the minimum distance cost from the starting position to the target position. coarse ={S1,S2,...,S n }, where S i Represents a surfel, which can be denoted by 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's index in the hash table and local surface features; Relay point pose calculation; Traj coarse ={S1,S2,...,S n } is evenly divided into m-1 segments, and a series of segmentation points Milestone = {S1, S 1+k ,...,S 1+mk },S 1+tk is the surfel corresponding to a segmentation point in the map; the path near the segmentation point is extended to obtain Traj subi ={S i-k ,S i-k+1 ,...,S i ,...,S i+k }, select the midpoint of the path point and use PCA The algorithm solves the pose and obtains the relay point T i , and then solve the relay point sequence Milestone={T1,T2,...,T m }.

4. The navigation planning method for a wheeled robot in a non-flat environment as claimed in claim 3, characterized in that: The second step comprises: The heuristic function of the sampling point is: c = g + h; 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 inspiration point, and h = max{astar,dubin}, which is the maximum value of the astar distance and the dubin distance on the Surfel map; State point sampling: Taking left turn as an example, assuming the state of the current node is X i ={x i w ,y i w ,z i w ,θ i w }, the landing surface Surfel S obtained by indexing i , with pose T wi , the robot's z-axis is parallel to the Surfel normal vector, and the x-axis is consistent with the direction of the vehicle head; assuming that the coordinates of the vehicle after state expansion in the horizontal plane are Then the posture obtained by state expansion on Surfel is The value of is related to the sampling state. Take forward and left turn as an example. In order to ensure that the robot is as close to the ground as possible, the posture make corrections; The corresponding space coordinates The index in the Surfel map can be obtained by S j ; It can be roughly considered that the robot always moves along the ground, constructing the rotation axis To T wj1 Perform rotation correction so that the z-axis of the corrected posture is aligned with S j Normal vector parallel: Considering that wheeled robots need to move on the ground, we Perform translation correction and adjust the posture to S j The projection distance is the height h of the robot's center of mass from the ground surface; Calculate the pose T wj The yaw angle and spatial position can be used to get the next state 5. The navigation planning method for a wheeled robot in a non-flat environment as claimed in claim 4, characterized in that: The step three comprises: Elastic band loss for non-flat surface trajectory: e k =a k x k +b k y k +c k z k +d k Q k is a trajectory point in the previously sampled trajectory, Q k Corresponds to S on the Surfel map k The average plane near π k :a k x+b k y+c k z+d k = 0, in the process of calculating the gradient, for the loss term f si , only consider the control point Q i The gradient of Trajectory safety loss on non-flat surfaces: The trajectory point Q to be optimized i , we extend along the normal direction of the trajectory d ext =min(d ext ,d obs ), d obs It is the distance from the trajectory point to the nearest unsafe surfel during the extension process; in this way, two child nodes can be found on both sides of the control point For the control node Q i , we consider all child nodes Q in a fixed-length window of the initial sampled trajectory; Project the set Q into T wi The xoy plane is obtained Then Convert to T iw middle, Because of the collection The z coordinate is 0, so only the x and y coordinates are considered, and the set is obtained At this point the problem can be transformed into T iw The problem of gradient calculation on the xoy plane under ; Pair Collection Constructing a convex polygon: P i H ={q∈R 2 :A i q≤b i }, Where n zi Is The number of hyperplanes constructed by the set, A iz and b iz is a hyperplane pair of descriptors; Constructing safety constraints Obstacle loss term f c The calculation scheme is The actual gradient needs to be transformed into the world coordinate system Kinematic losses for trajectory on non-flat surfaces

Images