Robotics-trajectory Planning (Introduction)

Trajectory planning is a top-level problem in robotics, and its main goal is to plan the robot to move from A to B and avoid all obstacles.

1. The object of the trajectory plan

The trajectory planning object is map, after the robot obtains the map through the slam, it can select any two points in the maps for trajectory planning. For the time being, three-dimensional map is not considered, and the map mainly has the following types:

　　

As shown above, the two-dimensional graph can be divided into the power of the node graph and the right to heavy grid diagram. Take the flight robot movement as an example: if the energy consumed by each part of the field is equivalent, you can use a grid diagram to model it. If the energy consumed by moving in different areas is different, you should use a node graph to model it.

2, trajectory planning methods 2.1, Grassfire method

Grassfire method is one of the simplest trajectory planning algorithms. Its data structure consists of three parts:

1.map, record the starting point, the end point, the position of the obstacle

2.distancetoStart, the distance between the square and the initial point is recorded after each exploratory step

3.parent, the parent node of each node is logged

Algorithm steps:

1. Get map

2. Initialize the Distancetostart, set the starting point to current, and set Distance_ to 0

3, enter the cycle if current = destination jump out of the loop

Set the node that corresponds to the minimum value in Distancetostart to current

Distance_ = distance_+1

Find all neighboring nodes of current

Set the Distancetostart of the neighboring node to Distance_,parent set to current

Set Current's Distance_ to infinity

4. Reverse trajectory based on parent map

　　　　　　　　

Obviously, this grassfire algorithm is extremely computationally expensive, because it needs to traverse almost every lattice in the grid, even if the destination is clearly above, the grassfire algorithm still needs to be explored down. The red area of the figure represents the explored point, the blue is candidate, the green is the starting point, and the yellow is the end point. Obviously, the algorithm is isotropic.

2.2. A * algorithm

The biggest disadvantage of the grassfire algorithm is that there is no use of the end point relative to the starting point relationship information. Thus leading to the same-sex exploration strategy. If you can add a trend that pulls the candidate toward the target point, you can reduce the number of points explored.

The data structure of a * algorithm consists of five parts:

1. Map

2, G: equivalent to Distancetostart;

3, H: A measurement operator for measuring the absolute distance from the current point to the target point

4, F:g+h, synthesis of the current point to the starting distance and distance to the end

5, parent, for recording traces

Algorithm steps:

1. Initialize, get the map, and set the starting point to current. Get h map,distance_ = 0

2. Enter the loop if current is destination then exit the loop

Distance_ = Distance_ +1;

Gets the adjacent point of the current point

Sets the G map value of the adjacent point to Distance_

Sets the F map value of the adjacent point to G+h

Select the lowest point of the F map value as current

Set the current F map value to infinity

3, according to the parent map, reverse trajectory

　　

The results are as shown above.

Obviously, due to the introduction of H map, the algorithm will first explore points that are closer to the relative endpoint. obj = distance to start + distance to end represents the last distance to walk. Each time you select the smallest obj and finally reach the end, the total distance must be the smallest.

3. Summary

For flying robots, each move consumes a lot of energy. A good trajectory planning algorithm can ensure the aircraft less detours and avoid obstacles. Therefore, the reduction of computational capacity must be in the premise of less detours. The energy consumed by the movement is about 10 times times the energy consumed by the calculation.

Robotics-trajectory Planning (Introduction)