An improved Q-learning path planning method based on hierarchical hybrid cost map

Abstract

The present invention discloses an improved Q-learning path planning method based on a hierarchical hybrid cost map, including: based on an input point cloud, a traversability analysis of negative obstacles, inclined obstacles and road surface flatness. Then, according to the traversability of different terrains, a multi-layer cost map is established, and each layer of the cost map corresponds to a different cost function, including a traversable area map, a negative obstacle cost map, a slope cost map and a flatness cost map. A repulsive potential field is established according to the multi-layer cost map, an attractive potential field is established according to the target posture point, and the repulsive potential field and the attractive potential field are synthesized to initialize the Q table. Then, the minimum turning radius arc segment is applied to generate the Q-learning action space, and after training, a smooth path for unmanned vehicles to walk directly is obtained. The present invention takes into account rugged and complex terrain, and can plan a path for direct passage of a crawler unmanned vehicle, and assigns initial values ​​to the Q table based on the artificial potential field of the multi-layer cost, which can accelerate the convergence speed of the algorithm.

Description

Technical Field

[0001] The invention relates to an improved Q-learning path planning method based on a hierarchical hybrid cost map, and belongs to the field of robot path planning. Background Art

[0002] With the further development of science and technology, more and more functional robots have been invented and applied to various fields including military, industrial and civilian use. At present, the movement modes of robots usually include wheeled, tracked and footed. Among them, tracked robots have high movement stability and are suitable for walking on soft and unstructured roads. Especially for complex battlefield terrain, tracked robots have strong passing ability. In the process of robot autonomous navigation, path planning is a crucial link. Traditional path planning methods are suitable for structured flat and hard roads.

[0003] The environmental information in the robot's autonomous navigation can be modeled based on the laser radar point cloud information. The main methods for building models are clustering and deep learning methods. The representative method of deep learning is PointNet++, which can not only classify scene models, but also realize the semantic recognition of objects. However, due to its low operating efficiency, it has high requirements on the computing platform for practical applications. The clustering method can classify point clouds with common features into one category based on the local features of the point cloud, thereby realizing segmentation in the scene and obtaining the point cloud of the target area.

[0004] Path planning can be divided into global path planning and local path planning according to its functions. The purpose of local path planning is to make real-time path decisions based on the dense information around the carrier. For complex roads in battlefield environments, traditional local path planning methods such as the dynamic window method (DWA) tend to use obstacle avoidance technology to completely avoid negative obstacles.

[0005] In an actual battlefield with typical craters, the uneven road surface leads to low efficiency of large-load robots. Therefore, it is necessary to propose a method to model the characteristics of ground negative obstacles based on lidar point cloud information and consider the negative obstacle model in path planning to achieve higher efficiency and ensure safety. Summary of the invention

[0006] The technical problem to be solved by the present invention is to conduct traversability analysis on the road surface in a battlefield environment, establish a hierarchical mixed cost map, improve the action space and state space update method of the Q-learning algorithm, train the Q-learning model of reinforcement learning, and obtain a smooth local path with high traffic efficiency and high path safety margin.

[0007] In order to solve the above technical problems, the present invention provides an improved Q-learning path planning method based on a hierarchical hybrid cost map, which comprises the following steps:

[0008] S1: Establish negative obstacle model, slope model and flatness model. Take multi-line lidar point cloud information as input, extract features of complex terrain and conduct traversability analysis;

[0009] S2: Based on the traversability analysis results in S1, establish a traversable area map, a negative obstacle cost map, a slope cost map, and a flatness cost map, and fuse them into a mixed local cost map;

[0010] S3: According to the local cost map and local target point established in S2, potential fields are established respectively, combined into a multi-layer cost artificial potential field, and the Q table is initialized;

[0011] S4: Consider the heading information and establish the action space based on the minimum turning radius arc segment. Update the Q table according to the mixed local cost map in S2 to obtain the optimal path.

[0012] Preferably, the step S1 specifically includes the following process:

[0013] S1.1 pre-processes the point cloud data input by the laser radar, and uses a clustering method to segment the point cloud data of the horizontal plane where the unmanned vehicle is currently located as the ground point cloud according to the height threshold, and marks the grid map corresponding to the area as a passable area;

[0014] S1.2 Model typical craters, puddles, and other negative obstacles that can be approximated by elliptical cones. Assume that the maximum roll angle of the tracked unmanned vehicle is MAXROLL and the wheelbase is d tread , at the maximum rolling angle, the height of the wheel on one side from the ground is h maxroll , according to the Pythagorean theorem:

[0015] h maxroll =d tread ×sin(MAXROLL)

[0016] Assume that the negative obstacle approximate elliptical cone equation is:

[0017]

[0018] Where a and b are the major and minor axes of the ground projection ellipse.

[0019] According to the maximum rolling angle, the height of the wheel on one side from the ground is h maxroll , draw contour lines on the negative obstacle approximate ellipse surface, project all ellipses onto the plane where the ground is located, and obtain ellipse contour lines with equal major and minor axis ratios on the plane. Draw rays every 5° outward from the intersection of the major axis and the minor axis of the innermost ring as the center point, and the point where the ellipse contour line intersects is set as P i,j, where \(i\in N\) and \(j = 1,2,\cdots,72\), and \(N\) is the number of elliptical rings. Calculate the lengths of the intercepted rays of adjacent elliptical rings. The judgment condition for a point to be a passable point is as follows:

[0020]

[0021] S1.3 For positive obstacles such as gentle slopes and convex hulls, establish an inclined plane model. Let the point cloud coordinates within the slope area be \((x i ,y i ,z i ), where \(i = 1,2,\cdots,N\) and \(N\) is the number of point clouds involved in the slope obstacle. Use the least squares method to perform plane fitting on the point cloud. Let the fitted plane equation be:

[0022] z = Ax + By + C

[0023] According to the least squares method, let the target equation be \(E\), and find \(A\), \(B\), and \(C\) to minimize \(E\):

[0024]

[0025] Take the partial derivatives of \(A\), \(B\), and \(C\) in the above formula and solve the system of equations to obtain the values of \(A\), \(B\), and \(C\).

[0026] The normal vector of the known fitted plane is The normal vector of the plane where the unmanned vehicle is located is Let the angle between the inclined plane and the ground be \(\theta\). Then, the passability of the inclined plane can be judged according to the following formula:

[0027]

[0028] If \(\theta\lt MAXROLL\), the inclined plane is determined to be passable.

[0029] S1.4 Rasterize the local point cloud map, characterize the flatness with the root mean square of the distance from each grid to the fitted ground plane, and establish a flatness model. Let the flatness of the grid be \(d rough , which can be calculated by the following formula:

[0030]

[0031] where \(d x,y is the distance from each grid to the fitted ground plane.

[0032] Preferably, the step S2 specifically includes the following process:

[0033] S2.1 According to the passability analysis results of negative obstacles, inclined plane obstacles, and step obstacles in S1, construct a passability area raster map, and the grid values are as follows:

[0034]

[0035] S2.2 Based on the passability of negative obstacles in S1 and considering the safety margin, the cost of passing caused by negative obstacles can be expressed as:

[0036]

[0037] where d tread Indicates the wheelbase of the unmanned vehicle;

[0038] S2.3 Based on the analysis of the passability of slope obstacles in S1, the pass cost of slope obstacles is defined as:

[0039]

[0040] S2.4 builds a flatness cost map based on the flatness analysis in S1, and its peer cost will be defined according to the following function:

[0041]

[0042] ROUGHFACTOR is a scaling factor that describes the flat-ground obstacle-crossing capability of a tracked unmanned vehicle. Generally, obstacles with an elevation difference exceeding 1 / 2 of the vehicle width cannot be crossed.

[0043] S2.5 integrates the traversable area map, negative obstacle cost map, slope cost map, and flatness cost map to establish a mixed local cost map. Let a1, a2, and a3 be mixed cost coefficients, and the traversable cost function is:

[0044]

[0045] Among them, a1+a2+a3=1.

[0046] Preferably, the step S3 specifically includes the following process:

[0047] S3.1 takes the insurmountable obstacles of the mixed local cost map in S2 as the repulsive force source and establishes the repulsive potential field U rep (q), and its electrostatic potential field model is:

[0048]

[0049] Where ρ0 is the maximum distance that the obstacle area affects the motion, and ρ(q) is the minimum distance from the obstacle area to the position q.

[0050] S3.2 Local waypoint Waypoint is used as the target point to establish the attractive potential field U att (q), and its electrostatic potential field model is:

[0051]

[0052] where ρ g (q) is the position from q to the target pose q g The Euclidean distance of .

[0053] S3.3 Superimpose the repulsive potential field and the attractive potential field to obtain the artificial potential field based on multi-layer cost as follows:

[0054] U q =U att (q)+U rep (q)

[0055] The potential field is gridded and normalized, and assigned to the Q table as the initial value.

[0056] Preferably, the step S4 specifically includes the following process:

[0057] S4.1 establishes an action space based on the minimum turning radius arc segment, sets four actions: forward, backward, left turn, and right turn, sets the state transition space in eight directions, and defines the instant reward of the action space as:

[0058]

[0059] S4.2 trains the Q table model and plans a smooth path based on the arc segment with the minimum turning radius.

[0060] Compared with the prior art, the advantages of the present invention are as follows:

[0061] First, the terrain analysis method proposed in the present invention extracts the ground plane in advance, and classifies the complex terrain into negative obstacles, slope obstacles and step obstacles, and establishes an elliptical cone model for negative obstacles, which improves the real-time performance of the algorithm and greatly expands the applicability of the algorithm.

[0062] Secondly, the hierarchical hybrid cost map established for different scenarios enables the path planned by the planner to meet the safe passage conditions of the unmanned vehicle, and considering its certain obstacle crossing ability, the planned path can improve the passage efficiency of the carrier.

[0063] Finally, the artificial potential field is used to initialize the Q-table of the Q-learning algorithm, and its action space and state space are improved, so that the Q-learning algorithm can directly output a smooth path, thereby improving the practicality of the algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 It is the overall block diagram of the system of the present invention;

[0065] Figure 2 Determine the passable area of ​​the typical negative obstacle approximation elliptical cone of the present invention;

[0066] Figure 3 The present invention has 4 action spaces and 8 heading state spaces based on the minimum turning radius arc segment. DETAILED DESCRIPTION

[0067] The present invention will be further explained below in conjunction with the accompanying drawings and specific implementation methods. It should be understood that the following specific implementation methods are only used to illustrate the present invention and are not intended to limit the scope of the present invention.

[0068] Example 1: Figure 1 As shown, an improved Q-learning path planning method based on a hierarchical mixed cost map comprises the following steps:

[0069] Step S1: Establish negative obstacle model, slope model and flatness model. Take multi-line laser radar point cloud information as input, extract features of complex terrain, and perform traversability analysis.

[0070] S1.1 pre-processes the point cloud data input by the laser radar, and uses a clustering method to segment the point cloud data of the horizontal plane where the unmanned vehicle is currently located as the ground point cloud according to the height threshold, and marks the grid map corresponding to the area as a passable area;

[0071] S1.2 Model typical craters, puddles, and other negative obstacles that can be approximated by elliptical cones. Assume that the maximum roll angle of the tracked unmanned vehicle is MAXROLL and the wheelbase is d tread , at the maximum rolling angle, the height of the wheel on one side from the ground is h maxroll According to the Pythagorean theorem, we can get:

[0072] h maxroll =d tread ×sin(MAXROLL)

[0073] Assume that the negative obstacle approximate elliptical cone equation is:

[0074]

[0075] Where a and b are the major and minor axes of the ground projection ellipse.

[0076] According to the maximum rolling angle, the height of the wheel on one side from the ground is h maxroll , draw contour lines on the negative obstacle approximate ellipse surface, such as Figure 2 As shown in the figure, all ellipses are projected onto the plane where the ground is located to obtain ellipse contour lines with equal proportions of major and minor axes on the plane. With the intersection of the major axis and minor axis of the innermost ring as the center point, rays are drawn outward every 5°, and the point where the ellipse contour line intersects is set as P i,j, where \(i\in N\) and \(j = 1,2,\cdots,72\), and \(N\) is the number of elliptical rings. Calculate the lengths of the rays intercepted by adjacent elliptical rings. The judgment condition for this intersection point to be a passable point is:

[0077]

[0078] S1.3 For positive obstacles such as gentle slopes and convex hulls, establish a slope model. Let the point cloud coordinates in the slope area be \((x i ,y i ,z i ), where \(i = 1,2,\cdots,N\), and \(N\) is the number of point clouds involved in the slope obstacle. Use the least squares method to perform plane fitting on the point cloud. Let the fitted plane equation be:

[0079] z = Ax + By + C

[0080] According to the least squares method, let the target equation be \(E\), and find \(A\), \(B\), and \(C\) to minimize \(E\):

[0081]

[0082] Take the partial derivatives of \(A\), \(B\), and \(C\) in the above formula and solve the system of equations to obtain the values of \(A\), \(B\), and \(C\).

[0083] The normal vector of the known fitted plane is The normal vector of the plane where the unmanned vehicle is located is Let the angle of the slope relative to the ground be \(\theta\). Then, the passability of this slope can be judged according to the following formula:

[0084]

[0085] If \(\theta\lt MAXPOLL\), then this slope is judged to be passable.

[0086] S1.4 Rasterize the local point cloud map, characterize the flatness with the root mean square of the distance from each grid to the fitted ground plane, and establish a flatness model. Let the flatness of the grid be \(d rough , which can be calculated by the following formula:

[0087]

[0088] where \(d x,y is the distance from each grid to the fitted ground plane.

[0089] Step S2: According to the passability analysis results in S1, establish a passable area map, a negative obstacle cost map, a slope pass cost map, and a flatness cost map, and fuse them into a hybrid local cost map.

[0090] S2.1 Based on the passability analysis results of negative obstacles, slope obstacles, and step obstacles in S1, a passability area grid map is constructed. The grid values ​​are as follows:

[0091]

[0092] S2.2 Based on the passability of negative obstacles in S1 and considering the safety margin, the cost of passing caused by negative obstacles can be expressed as:

[0093]

[0094] where d tread Indicates the wheelbase of the unmanned vehicle;

[0095] S2.3 Based on the analysis of the passability of slope obstacles in S1, the pass cost of slope obstacles is defined as:

[0096]

[0097] S2.4 builds a flatness cost map based on the flatness analysis in S1, and its peer cost will be defined according to the following function:

[0098]

[0099] ROUGHFACTOR is a scaling factor that describes the flat-ground obstacle-crossing capability of a tracked unmanned vehicle. Generally, obstacles with an elevation difference exceeding 1 / 2 of the vehicle width cannot be crossed.

[0100] S2.5 integrates the traversable area map, negative obstacle cost map, slope cost map, and flatness cost map to establish a mixed local cost map. Let a1, a2, and a3 be mixed cost coefficients, and the traversable cost function is:

[0101]

[0102] Among them, a1+a2+a3=1.

[0103] Step S3: According to the local cost map and local target point established in S2, potential fields are established respectively, combined into a multi-layer cost artificial potential field, and the Q table is initialized.

[0104] S3.1 takes the insurmountable obstacles of the mixed local cost map in S2 as the repulsive force source and establishes the repulsive potential field U rep (q), and its electrostatic potential field model is:

[0105]

[0106] Where ρ0 is the maximum distance that the obstacle area affects the motion, and ρ(q) is the minimum distance from the obstacle area to the position q.

[0107] S3.2 Local waypoint Waypoint is used as the target point to establish the attractive potential field U att (q), and its electrostatic potential field model is:

[0108]

[0109] where ρ g (q) is the position from q to the target pose q g The Euclidean distance of .

[0110] S3.3 Superimpose the repulsive potential field and the attractive potential field to obtain the artificial potential field based on multi-layer cost as follows:

[0111] U q =U att (q)+U rep (q)

[0112] The potential field is gridded and normalized, and assigned to the Q table as the initial value.

[0113] Step S4: Consider the heading information and establish an action space based on the minimum turning radius arc segment. Update the Q table according to the mixed local cost map in S2 to obtain the optimal path.

[0114] S4.1 As Figure 3 As shown in the figure, an action space based on the minimum turning radius arc segment is established, four actions of forward, backward, left turn and right turn are set, a state transition space of eight directions is set, and the instant reward of the action space is defined as:

[0115]

[0116] S4.2 trains the Q table model and plans a continuous smooth path based on the minimum turning radius arc segment.

[0117] It should be noted that the above embodiments are merely preferred embodiments of the present invention and are not intended to limit the protection scope of the present invention. Equivalent replacements or substitutions made on the basis of the above technical solutions all fall within the protection scope of the present invention.

Claims

1. An improved Q-learning path planning method based on hierarchical hybrid cost map, characterized in that: It includes the following steps: S1: Establish a negative obstacle model, an inclined plane model, and a flatness model. Using the multi-line lidar point cloud information as input, extract the features of the complex terrain and conduct passability analysis. S2: According to the passability analysis results in S1, establish a passable area map, a negative obstacle cost map, an inclined plane pass cost map, and a flatness cost map, and fuse them into a hybrid local cost map. S3: Based on the hybrid local cost map and the local target points established in S2, establish potential fields respectively, combine them into a multi-layer cost artificial potential field, and initialize the Q-table. S4: Considering the heading information, establish an action space based on the circular arc segment with the minimum turning radius, update the Q-table according to the hybrid local cost map in S2, and obtain the optimal path. The establishment methods of the negative obstacle model, inclined plane model, and flatness model in step S1 are as follows: S1.1 Preprocess the point cloud data input by the lidar. According to the height threshold, use the clustering method to segment the point cloud data of the horizontal plane where the unmanned vehicle is currently located as the ground point cloud, and mark the corresponding grid map of this area as a passable area. S1.2 Model typical craters and puddles with negative obstacles that can be approximated by elliptical cones. Assume that the maximum roll angle of the tracked unmanned vehicle is MAXROLL and the wheelbase is d. tread , at the maximum rolling angle, the height of the wheel on one side from the ground is h maxroll , according to the Pythagorean theorem: h maxroll =d tread ×sin(MAXROLL) Let the equation of the negative obstacle approximate elliptical cone surface be: where a and b are the major axis and minor axis of the ellipse projected on the ground, According to the maximum rolling angle, the height of the wheel on one side from the ground is h maxroll , draw contour lines on the negative obstacle approximate ellipse surface, project all ellipses onto the plane where the ground is located, and obtain ellipse contour lines with equal major and minor axis ratios on the plane. Draw rays every 5° outward from the intersection of the major axis and the minor axis of the innermost ring as the center point, and the point where the ellipse contour line intersects is set as P i,j , i∈N, j=1,2,…,72, where N is the number of elliptical rings. Calculate the length of the rays intercepted by adjacent elliptical rings. The judgment condition for the intersection point to be a passable point is: S1.3 For the gentle slope and convex hull positive obstacles, establish the slope model, and set the point cloud coordinates in the slope area as (x i ,y i ,z i ), i = 1, 2…, N, where N is the number of point clouds involved in the slope obstacle. The point cloud is fitted with a plane using the least squares method. The fitted plane equation is assumed to be: z = Ax + By + C According to the least squares method, set the target equation as E, and find A, B, and C to minimize E: Take the partial derivatives of A, B, and C in the above formula and solve the system of equations to obtain the values of A, B, and C. The normal vector of the known fitting plane is The plane where the unmanned vehicle is located is fitted using the least squares method, and its normal vector is Assuming that the angle of the inclined plane relative to the ground is θ, the passability of the inclined plane can be determined according to the following formula: If θ < MAXROLL, then this inclined plane is determined to be passable. S1.4 Rasterize the local point cloud map, use the root mean square of the distance from each grid to the fitted ground plane to characterize the flatness, and establish a flatness model. Let the flatness of the grid be d rough , can be calculated by the following formula: where d x,y For the distance from each grid to the fitted ground plane, the ground plane equation where the unmanned vehicle is located is A e +B e +C e +D e =0, then d x,y =(x d ,y d ,z d ) can be calculated using the following equation: The establishment method of the hybrid local cost map in step S2 is as follows: S2.1 According to the passability analysis results of negative obstacles, inclined plane obstacles, and step obstacles in S1, construct a passable area grid map, and the grid values are as follows: S2.2 According to the passability of negative obstacles in S1, considering the safety margin, the passing cost caused by negative obstacles can be expressed as: where d tread Indicates the wheelbase of the unmanned vehicle; S2.3 According to the passability analysis of inclined plane obstacles in S1, define the passing cost of slope obstacles as: S2.4 According to the flatness analysis in S1, establish a flatness cost map, and its passing cost will be defined according to the following function: where ROUGHFACTOR is a scale factor describing the flat ground obstacle-crossing ability of the tracked unmanned vehicle. It is set that obstacles with an elevation difference exceeding half of the vehicle body width cannot be crossed. S2.5 Synthesize the passable area map, negative obstacle cost map, inclined plane cost map, and flatness cost map to establish a hybrid local cost map. Let a1, a2, and a3 be the hybrid cost coefficients, and the passing cost function is: where a1 + a2 + a3 = 1.

2. The improved Q-learning path planning method based on hierarchical hybrid cost map according to claim 1, characterized in that: The method for establishing the artificial potential field based on the multi-layer cost in step S3 is as follows: S3.1 Take the insurmountable obstacles of the mixed local cost map in S2 as the repulsive force source to establish a repulsive potential field U rep (q), and its electrostatic potential field model is: where ρ0 is the maximum distance at which the obstacle area affects the movement, and ρ(q) is the minimum distance from the obstacle area to the pose q. S3.2 Local waypoint Waypoint is used as the target point to establish the attractive potential field U att (q), and its electrostatic potential field model is: where ρ g (q) is the position from q to the target pose q g The Euclidean distance of S3.3 Superimpose the repulsive potential field and the attractive potential field to obtain the artificial potential field based on multi-layer cost as follows: U q =U att (q)+U rep (q) Grid the potential field and normalize the values, and assign them to the Q-table as the initial value.

3. The improved Q-learning path planning method based on hierarchical hybrid cost map according to claim 1, characterized in that: The improved Q-learning in step S4 is specifically described as follows: S4.1 establishes an action space based on the minimum turning radius arc segment, sets four actions: forward, backward, left turn, and right turn, sets the state transition space in eight directions, and defines the instant reward of the action space as: S4.2 trains the Q table model and plans a continuous smooth path based on the minimum turning radius arc segment.

Images