Path planning and trajectory optimization method for mobile robot in non-flat terrain

Abstract

The invention discloses a path planning and trajectory optimization method for a mobile robot in a non-flat terrain, and the method comprises the following steps: S1, the mobile robot employs a laser radar to detect the surrounding environment, and an SLAM module constructs a three-dimensional map according to point cloud data and odometer data collected by the laser radar in real time; and S2, a trafficability analysis module performs terrain trafficability evaluation based on the generated three-dimensional map, and generates a trafficability layer accordingly. And S3, the global planning module performs path search in the trafficability layer to obtain an initial path of the mobile robot from the starting point to the target point. And S4, a trajectory optimization module optimizes the initial path to ensure that the obtained trajectory meets the kinematics constraint of the mobile robot. And S5, vertically mapping the optimized trajectory to the non-flat terrain surface to obtain a final path for direct tracking of the mobile robot. According to the method, it can be ensured that the mobile robot generates a feasible track conforming to kinematics constraints in the non-flat terrain.

Description

Technical Field

[0001] The invention relates to a path planning and trajectory optimization method for a mobile robot in a non-flat terrain, and belongs to the field of mobile robot path planning. Background Art

[0002] Mobile robots have great application value in logistics, medical treatment, rescue, exploration, military, agriculture, mining and transportation due to their high mobility, ability to carry various loads, and ability to perform dangerous tasks over long distances. At present, the autonomous navigation technology of two-dimensional indoor ground mobile robots is relatively mature, and there are many open source autonomous navigation algorithms. However, these algorithms cannot be directly applied to autonomous navigation in non-flat terrain because most of them ignore a basic fact that some attributes of the terrain (such as height) will change with the change of spatial position. Therefore, studying the autonomous navigation technology of mobile robots in non-flat terrain is of great significance to the development of mobile robot technology.

[0003] In the existing technical solutions, the autonomous navigation technology of mobile robots in non-flat terrain has the following main difficulties: (1) There are various complex terrain features in non-flat terrain, such as steep slopes and potholes, which makes it difficult to extract navigation-related passable areas from sensor data. How to perform real-time terrain passability analysis and path planning to achieve autonomous navigation of mobile robots in non-flat terrain is a technical challenge. (2) Due to the undulating characteristics of non-flat terrain and the structural characteristics of mobile robots, mobile robots cannot actively adjust their height in the vertical direction and can only passively adapt to the height changes of the terrain. Therefore, traditional trajectory optimization methods cannot be directly applied in non-flat terrain. Summary of the invention

[0004] The purpose of the present invention is to address the deficiencies of the above-mentioned prior art and to propose a path planning and trajectory optimization method for a mobile robot in a non-flat terrain, comprising the following steps:

[0005] S1. The mobile robot uses LiDAR to detect the surrounding environment, and the SLAM module builds a three-dimensional map based on the point cloud data and odometer data collected by the LiDAR in real time.

[0006] S2. The accessibility analysis module evaluates the accessibility of the terrain based on the generated three-dimensional map and generates a accessibility layer accordingly.

[0007] S3. The global planning module performs path search in the passability layer to obtain the initial path of the mobile robot from the starting point to the target point.

[0008] S4, the trajectory optimization module optimizes the initial path to ensure that the resulting trajectory meets the kinematic constraints of the mobile robot.

[0009] S5. Vertically map the optimized trajectory to the non-flat terrain surface to obtain a final path that can be directly tracked by the mobile robot.

[0010] Preferably, in step S1, the SLAM module uses the LeGO-LOAM algorithm to establish a three-dimensional map.

[0011] Preferably, in step S2, a terrain accessibility analysis is performed based on the three-dimensional map, and a accessibility layer is generated accordingly. A terrain accessibility cost is stored in each grid of the accessibility layer, and the accessibility of the local plane is evaluated by three indicators, namely slope, roughness, and sparsity.

[0012] The projection of the mobile robot in the three-dimensional map is approximated as a rectangle, and a plane is fitted in the projection area.

[0013] The slope s is calculated by the angle between the normal vector of the fitted plane and the z-axis of the global coordinate system.

[0014] The standard deviation of the terrain height values ​​within the projection area is calculated to obtain the terrain roughness r.

[0015] The sparsity λ of the terrain is obtained by calculating the vacancy ratio of point cloud data in the projection area.

[0016] Preferably, in step S3, the global planning module adopts a path planning algorithm based on the improvement of Informed-RRT*. In the improved Informed-RRT* algorithm, the cost evaluation strategy between two nodes is optimized to comprehensively consider the passability of the terrain and the path length, so that the cost function can more comprehensively reflect the actual travel cost of the path. In addition, the algorithm is the same as Informed-RRT* in other strategies, ensuring the efficiency and optimality of the path search process.

[0017] Preferably, in step S4, initial control points are obtained according to the initial path, an objective function based on minimizing Jerk is constructed, and continuity constraints and smoothness constraints are imposed to generate an initial trajectory.

[0018] Preferably, in step S4, by introducing the time allocation based on terrain passability and the inequality constraint based on the corridor, and constructing the trajectory guidance function based on terrain passability, the overshoot phenomenon that may occur in the trajectory optimization process is suppressed, and the optimized trajectory that meets the kinematic constraints is obtained.

[0019] The beneficial effects of the present invention are:

[0020] Compared with the existing technologies, the present invention has obvious advantages. On the one hand, the present invention integrates path planning with terrain passability analysis to ensure the efficiency and optimality of the path search process. On the other hand, the present invention formulates the trajectory optimization problem as an optimal control problem, reasonably designs the objective function, and considers terrain passability and kinematic constraints, vertically maps the two-dimensional trajectory to the non-flat terrain surface, and finally outputs a smooth path that can be directly tracked by the mobile robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 It is a flow chart of a path planning and trajectory optimization method of a mobile robot in a non-flat terrain of the present invention;

[0022] Figure 2 is a schematic flow chart of a trajectory optimization method in an embodiment of the present invention;

[0023] Figure 3 is a schematic diagram of navigation in a non-flat terrain according to an embodiment of the present invention. DETAILED DESCRIPTION

[0024] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the accompanying drawings in the embodiments of the present invention.

[0025] The present invention discloses a path planning and trajectory optimization method for a mobile robot in a non-flat terrain. Figure 1 As shown, based on the current position of the mobile robot, the following steps are iteratively executed until the target point is reached.

[0026] Step S1: The mobile robot uses a laser radar to detect the surrounding environment, and the SLAM module constructs a three-dimensional map based on the point cloud data and odometer data collected by the laser radar in real time.

[0027] The internal implementation of the SLAM module uses the LeGO-LOAM algorithm, which uses the point cloud data and odometer data of the Velodyne VLP-16 lidar (placed horizontally) as input to build a three-dimensional map of non-flat terrain.

[0028] Step S2: The accessibility analysis module evaluates the accessibility of the terrain based on the generated three-dimensional map, and generates a accessibility layer accordingly.

[0029] Generate a terrain accessibility layer based on the three-dimensional map, and store a accessibility cost in each grid of the accessibility layer. The accessibility τ of the local plane is evaluated by three indicators, namely slope, roughness, and sparsity, as shown below:

[0030]

[0031] Where ω1, ω2 and ω3 are weight factors that sum to 1; s, r and λ are the slope, roughness and sparsity costs respectively. crit 、r crit and λ crit They represent the maximum permissible values ​​of slope, roughness and sparsity cost respectively. The value range of τ is 0 to 1. The smaller the value, the more suitable the terrain is for mobile robot navigation.

[0032] The projection of the mobile robot in the map is approximated as a rectangle, and a plane is fitted in the projection area.

[0033] The slope angle s is calculated by the angle between the normal vector of the plane and the z-axis of the global coordinate system.

[0034] The standard deviation of the terrain height values ​​within the projection area is calculated to obtain the terrain roughness r.

[0035] The sparsity λ of the terrain is obtained by calculating the vacancy ratio of point cloud data in the projection area.

[0036] Step S3: The global planning module performs a path search in the passability layer to obtain an initial path for the mobile robot from the starting point to the target point.

[0037] Given a target point, the mobile robot uses the improved path planning algorithm based on Informed-RRT* to search for an initial path from the current position to the target point in the passability layer according to the current position and target point information.

[0038] In the path planning algorithm improved based on Informed-RRT*, the cost evaluation strategy between two nodes is optimized to comprehensively consider the terrain passability and path length, so that the cost function can more comprehensively reflect the actual travel cost of the path. In addition, the algorithm is the same as the Informed-RRT* algorithm in other strategies, ensuring the efficiency and optimality of the path search process. The cost function is as follows:

[0039]

[0040] Among them, κ is the weight coefficient, τ1 and τ2 are the terrain passability of the two nodes, l 12 is the Euclidean distance between two nodes.

[0041] Step S4: The trajectory optimization module optimizes the initial path to ensure that the resulting trajectory satisfies the kinematic constraints of the mobile robot.

[0042] The flow chart of trajectory optimization method is as follows: Figure 2As shown in the figure, the path points in the obtained initial path are used as the initial control points of trajectory optimization, and the objective function based on minimizing Jerk is constructed. Continuity and smoothness constraints are imposed to generate the initial trajectory. By introducing the time allocation based on terrain passability and the inequality constraints based on Corridor, and constructing the trajectory guidance function based on terrain passability, the overshoot phenomenon that may occur in the trajectory optimization process is suppressed, and the optimized trajectory that meets the kinematic constraints is obtained.

[0043] The trajectory is fitted in segments. According to the M-1 intermediate path points of the initial path, together with the starting point and the end point, the trajectory can be divided into M segments. Each segment of the trajectory is represented by a polynomial curve of the same order:

[0044]

[0045] where p i are the polynomial coefficients (i=0,1,2,…,5), and t is the time of the current trajectory segment.

[0046] The entire trajectory can be regarded as a concatenation of M segments of given time segments, expressed as:

[0047]

[0048] where p j,i is the polynomial coefficient of the jth trajectory, T is the total time of the entire trajectory, T=(T1-T0)+(T2-T1)+...+(T M -T M-1 ).

[0049] In the motion control of mobile robots, Jerk (rate of change of acceleration) is a key indicator. Minimizing it can effectively reduce the sudden change of acceleration, thereby reducing the impact and vibration during the motion process. This not only makes the trajectory smoother, but also improves the efficiency of energy utilization. Therefore, minimizing Jerk is an important goal of optimizing the trajectory of mobile robots. According to the trajectory polynomial, the position, velocity, acceleration, and jerk corresponding to any time t can be calculated. The rate of change of acceleration of any section of the trajectory can be expressed as:

[0050]

[0051] where p i is the coefficient vector of the i-th trajectory, p i =[p i0 ,p i1 ,...,p in ] T .

[0052] The trajectory optimization problem is expressed as an optimal control problem, and the minimization objective function can be expressed as:

[0053]

[0054] where p j represents the coefficient vector of the trajectory polynomial, and j represents the sequence number of each trajectory in different time periods. j is the j-th trajectory polynomial information matrix, which is specifically expressed as:

[0055]

[0056] where r and c are the row and column indices of the matrix respectively.

[0057] The objective function of a certain trajectory is expanded to the minimized Jerk objective function of the global trajectory, which is specifically expressed as:

[0058]

[0059] From the minimization objective function of the global trajectory, it can be seen that the information matrix of the entire trajectory is a symmetric matrix. Therefore, the problem of solving the objective function is modeled as a mathematical quadratic programming problem.

[0060] In order to ensure the continuity of the trajectory, continuity constraints are imposed on the trajectory. These constraints are mainly reflected in the middle connection point of the trajectory, that is, the position, velocity, acceleration, and jerk at the end of the jth trajectory segment and the beginning of the j+1th trajectory segment are equal. Therefore, the continuity constraints are constructed as follows:

[0061]

[0062] Where k is the derivative order, p j,n ,p j,m Represent the polynomial coefficients of the j-th and j+1-th trajectories respectively.

[0063] In order to ensure the smoothness of the trajectory, a smoothness constraint is imposed on the trajectory. The smoothness constraint of the trajectory means that the position of the mobile robot at the starting point, the end point and the path connection point of each two segments of the trajectory is relatively fixed, and its speed, acceleration and jerk can meet a certain specific value. Therefore, the smoothness equation constraint is constructed as follows:

[0064]

[0065] Where k is the derivative order, They represent the position, velocity, and acceleration of the pose points in the trajectory respectively.

[0066] By introducing the terrain passability index and adjusting the time of each trajectory, this method takes into account the characteristics of the terrain, such as slope, flatness and other influencing factors. The function expression is as follows:

[0067]

[0068] Where T j is the time required for the jth trajectory, t j is the time required for the jth trajectory to be allocated according to the average allocation method, λ is the weight coefficient, is the average terrain accessibility of the two path points of the jth path, τ j is the terrain accessibility of the j-th path point.

[0069] Construct the mathematical model of the Corridor and impose inequality constraints on the position of each sampling point so that the optimized trajectory will not deviate too much from the initial path. The function expression is as follows:

[0070]

[0071] where x min 、x max They are the upper and lower limits of the x-axis of the Corridor; min ,y max They are the upper and lower bounds of the y-axis of the Corridor respectively.

[0072] A guide term is introduced to measure the error between the trajectory and the expected trajectory, while considering the passability of non-flat terrain. The improved global trajectory minimization objective function is shown below. The problem of solving this objective function can still be modeled as a mathematical quadratic programming problem.

[0073]

[0074] where p s (t) is the desired trajectory, α is the weight coefficient of the guidance term, which is used to adjust the proportion of smoothness and target curve tracking, β is the weight coefficient of terrain passability, τ is terrain passability, and D is the unit matrix.

[0075] Step S5: After the trajectory optimization is completed, the terrain surface height information corresponding to each two-dimensional path point of the optimized trajectory is obtained from the three-dimensional point cloud, and the path points of the two-dimensional optimized trajectory are Vertical mapping to waypoints in 3D space Right now This completes effective trajectory optimization and obtains the final path that can be directly tracked by the mobile robot in non-flat terrain, such as Figure 3 shown.

[0076] The embodiments described above are only descriptions of the preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Without departing from the concept of the present invention, various modifications and improvements made to the technical solutions of the present invention by ordinary technicians in this field should fall within the scope of protection determined by the claims of the present invention.

Claims

1. A path planning and trajectory optimization method for a mobile robot in a non-flat terrain, characterized in that: The steps include: S1. The mobile robot uses laser radar to detect the surrounding environment, and the SLAM module builds a three-dimensional map based on the point cloud data and odometer data collected by the laser radar in real time; S2, the accessibility analysis module evaluates the accessibility of the terrain based on the generated three-dimensional map, and generates a accessibility layer accordingly; S3, the global planning module performs path search in the passability layer to obtain the initial path of the mobile robot from the starting point to the target point; S4, the trajectory optimization module optimizes the initial path to ensure that the obtained trajectory meets the kinematic constraints of the mobile robot; S5. Vertically map the optimized trajectory to the non-flat terrain surface to obtain a final path that can be directly tracked by the mobile robot.

2. A method for path planning and trajectory optimization of a mobile robot in a non-flat terrain as claimed in claim 1, characterized in that: In step S3, the global planning module uses the improved path planning algorithm based on Informed-RRT* to optimize the cost evaluation strategy between two nodes, taking into account the terrain's passability and path length, so that the cost function can more comprehensively reflect the actual cost of the path.

3. The method for path planning and trajectory optimization of a mobile robot in a non-flat terrain as claimed in claim 1, characterized in that: In step S4, the trajectory optimization module uses the path points of the initial path as the initial control points of trajectory optimization, constructs an objective function based on minimizing Jerk, imposes continuity constraints, smoothness constraints and corridor-based inequality constraints, and introduces time allocation and trajectory guidance functions based on terrain passability to obtain an optimized trajectory that meets kinematic constraints.

4. A method for path planning and trajectory optimization of a mobile robot in a non-flat terrain as claimed in claim 3, characterized in that The time allocation based on terrain passability is adjusted by introducing terrain passability indicators. This method takes into account the characteristics of the terrain, such as slope, roughness and other influencing factors.

5. A method for path planning and trajectory optimization of a mobile robot in a non-flat terrain as claimed in claim 3, characterized in that Trajectory guidance function based on terrain passability,In the optimization objective, in addition to optimizing the acceleration change rate (Jerk), a guidance term that measures the error between the trajectory and the expected trajectory,while considering the passability of non-flat terrain,is also needed.

6. A method for path planning and trajectory optimization of a mobile robot in a non-flat terrain as claimed in claim 1, characterized in that: In step S5, the terrain surface height information corresponding to each two-dimensional path point of the optimized trajectory is obtained from the three-dimensional point cloud, and the path point of the two-dimensional optimized trajectory is Vertical mapping to waypoints in 3D space ,Right now ,Therefore, the trajectory optimization that meets the kinematic constraints in non-flat terrain is completed, and the final path that can be directly tracked by the mobile robot is obtained.

Images