【Thesis】Read the paper

Today I read a paper on robot path planning , and record it here.

Original: Improved A* and dynamic window method for mobile robot path planning : https://kns.cnki.net/kcms/detail/detail.aspx?dbcode=CAPJ&dbname=CAPJLAST&filename=JSJJ2020102300M&v=l5%25mmd2FDG1c37fumFRahpllN7JWV7J8kDILJRNoZEVRg8kDILJNoZEVRg

1. Ask a question

The article proposes a fusion algorithm combining A* algorithm and dynamic window method .

First, use the A* algorithm to plan a global path, and then use a key inflection point extraction algorithm to eliminate redundant inflection points and redundant nodes on the global path to ensure the global optimal path.

Then, the global path is segmented according to the key nodes, and the dynamic window method is combined to optimize each section of the local path to improve the safety and smoothness of the path.

The problems of the A* algorithm: too many inflection points, redundant nodes, close distance to obstacles, etc.

Dynamic window method : It is very easy to fall into the optimal layout, and the path finding success rate is low.

2. Propose an algorithm

(1) Optimize A* algorithm

(A) Obtain all the node sets of the path planned by the A* algorithm $U{}\left \{ Pi,1\leq i\leq n\right \}$ , which $P_{1}$ represents the starting point of the $P_{n}$ planned path and the end point of the planned path. Creating the initial value of the path contains only the start $P_{1}$ and end $P_{n}$ of key set of nodes $V\left \{ P_{1},P_{n} \right \}$ used to store key turning point in the optimization algorithm.

(B) $P_{1}$ Make a straight line connection from the beginning to $P_{3},P_{4},\cdots ,P_{m}$ determine whether the straight line $P_{1}P_{m}$ passes through obstacles. If the straight line $P_{1}P_{m}$ passes through the obstacle, the node $P_{m-1}$ is the necessary node of the path and must be the key turning point. Add the node $P_{m-1}$ to the set V; if the straight line $P_{1}P_{m}$ does not pass the obstacle Then it is determined that the node $P_{2},\cdots ,P_{m}$ is a redundant node, from $P_{m}$ continuing to connect the nodes in the node set U until the end of the path is connected $P_{n}$ , and all the key nodes are added to the set V. After the completion of key nodes selected collection $V\left \{ P_{1},P_{m-1},\cdots ,P_{m+k},P_{n} \right \}$ contains all the critical nodes, the path is assumed that the key set V of nodes contained is m.

(C) Connect all the nodes in set V in turn to complete the global optimization of the path. The steps of path global optimization are shown in the figure

(2) Optimize the dynamic window method

According to the principle of the dynamic window method, the main factors affecting the path are the heading angle and angular velocity of the mobile robot. Since the angular velocity is usually a fixed configuration value, the main factor affecting the smoothness and length of the path is the heading angle. The greater the heading angle deviation, the longer the required turning distance, and the more likely it is to cause path redundancy. Therefore, this article mainly focuses on the improvement and optimization of the heading angle, and the specific steps of local optimization are as follows.

(A) from the starting point $P_{1}$ Start, critical node set $V\left \{ P_{1},P_{m-1},\cdots ,P_{m+k},P_{n} \right \}$ in addition to the end of $P_{n}$ all the nodes as the starting point for a local path planning $S_{1},S_{2},\cdots ,S_{m-1}$ ; from the second node $P_{m-1}$ starts sequentially set $V\left \{ P_{1},P_{m-1},\cdots ,P_{m+k},P_{n} \right \}$ in all nodes as a destination local path planning $D_{1},D_{2},\cdots ,D_{m-1}$ , the global path points There are $S_{1}D_{1},S_{2}D_{2},\cdots ,S_{m-1}D_{m-1}$ m-1 sections in total.

I didn't understand here

(B) Calculate the path $S_{1}D_{1}$ , that is $P_{1}P_{m-1}$ , the inclination angle of the original line segment , and the formula for calculating the inclination angle of the line segment is as follows.

(C) Convert the inclination angle of the path into a radian value as the initial heading angle of the mobile robot. The formula for converting the inclination angle to radian value is as follows.

(D) Initialize the state parameter set L{l} of the mobile robot, where l{x,y,yaw,v,w} records the state parameters in the path planning process of the mobile robot, including position, heading angle, linear velocity, and angular velocity.

(E) According to the actual environment, set the initial linear velocity V (m/s) and initial angular velocity w (rad/s) of the mobile robot, and combine the first three steps to obtain the starting point $S_{1}(m)$ , ending point $D_{1}(m)$ , and initial heading angle yaw (rad). Get .

(F) Use the dynamic window method for local path planning, and $l_{i}$ update all the state parameters of the robot to the state parameter set L, record all the nodes of the path and the pose information of the robot. After each path is completed, the state parameter set becomes Indicates the latest status parameter of the mobile robot.

(G) Calculate the inclination angle of the next path according to formula (b) , and at the same time convert the yaw in the last state parameter l of the robot into an angle value, and the formula for converting the radian value into an angle value is as follows.

Why is the heading angle of the robot set to yaw/2 here? ? ?

(H) Repeat (g),

The pictures and texts are all from the paper, which is for learning only, and the infringement is deleted.