[path planning] use the vertical distance limit method and Bessel to optimize the path of a star

Before using general A* Algorithm to plan the path , It can be found that there are many redundant points on the path , As shown in the figure below ：

It is obvious from the above figure that some bending points are redundant , Therefore, this section introduces the use of vertical distance limit method to optimize redundant points .

Vertical distance limit method

The idea of vertical distance limit method is also relatively simple , To sum up, it is ： Calculate the distance from the current node to the upper node and the lower node , If it exceeds a certain threshold, the current point is deleted from the total set . The general idea is just like its name .

Here I write a function Vertical_distance_limit：

 

Of course , In this function, I encapsulate another function , That is, the function of calculating the vertical distance compute_vertical_distance

Vertical distance calculation

About vertical distance calculation , We can use vector method , As shown in the figure below ：

Use Vector method The steps of calculating the vertical distance can be carried out as follows ：

1) First calculate the vector last_to_next Unit vector u_last_to_next

2) Calculate the vector last_to_pos, Calculated after last_to_pos And vector u_last_to_next Dot product of , That is to say last_to_pos To vector u_last_to_next The projection length on the surface L

3) Using vectors u_last_to_next multiply L That's the vector last_to_closest

4) Use last Coordinates of plus last_to_closest You can find out. closest Coordinates of

5) And then you can figure out pos And closest Distance of

The effect of using the vertical distance limit method

After adding the vertical distance optimization, the following effects can be obtained ：

The red connecting point in the above figure is the initial A* Algorithm path point , The green connection point is the optimized path point

Use fifth order Bessel to optimize the path

After optimizing the path , We can further use Bezier curve to plan a smoother curve ：

Add motion trajectory model

Then we can do as before RRT like that , Increase the speed S Trajectory model of curve , The end result is this ：

A* Algorithm + Vertical distance limit method + Bessel + Motion model trajectory