[path planning] auxiliary points and multi segment Bessel smoothing RRT

In the last article on the smooth path of Bezier curve , Just a brief introduction to the principle of Bessel curve , And a preliminary test , The final smooth effect , It is still unsatisfactory , At first, a simple multi segment second-order Bessel was used , The resulting curve has a lot of sharpness ：

Then the global Bessel , That is, all the points are used at once , Get one N Jie Bessel , But there will be a drawback , That is when there are many turns , Its effect is not so good , Just like this. ：

Sometimes this happens , It's easier to press obstacles , And it is quite different from the original path .

So in the actual use of Bessel optimization path , The frequently used method can directly tell you ： Use Multistage fifth order Bessel + Insert auxiliary point Methods .

Specifically, why use the fifth order , Not fourth order , Third order , These are just the experiences that our predecessors have tried out , But I want to change it to non-level five , Ideas , The method is the same , You can adjust it according to your own needs .

The role of auxiliary points

The key here is to add auxiliary points , The reason why there are all kinds of sharp corners in the multistage second-order Bessel in the previous article , Because the junction of two Bessel curves , Different curvature , Not collinear , This leads to a sudden change in curvature , So I want to do a lot of Bessel effects , The addition of auxiliary points is the key .

Requirements for auxiliary points

Suppose that the six points required by the fifth order Bessel in each segment , From the beginning to the end, it is recorded as P0 - P5, Then the starting point of the next fifth order Bessel curve should coincide with the end point of this section , Suppose the starting point of the next paragraph is written as Q0, namely Q0 = P5, The second point after Q1 Is the auxiliary point , After the auxiliary point , Read the next four points as the remaining four points in the second paragraph , As shown in the figure below ：

Of course, the most important thing is the auxiliary point Q1 Selection of , Auxiliary points should meet several conditions ：

1）Q1 To work with P4-P5 Collinear

2）Q1 Can't be in P4-P5 middle ---> Q1 - P4 ≥ P5 - P4 ≥ Q1 - Q0

3）Q1 And Q0 Distance ratio Q2 And Q1 Small ---> || Q1 - Q0 || ≤ || Q2 - Q1 ||

4）Q1 And Q0 Is less than or equal to P5 And P4 Distance of ---> Q1 - Q0 = n * (P5 - P4), n∈(0,1]

5）P5 And Q0 coincidence ---> P5 = Q0

Then according to the above conditions , We can be in the previous RRT After the program , Plus Bessel's program , The point is still about auxiliary points Q1 The choice of , because RRT The random search distance in the program is a fixed distance , So you can follow a fixed slope , A principle construction of nearly half the length Q1, As shown below ：

The fifth order Bessel formula is also easy to derive ：

P(t) = (1 - t)^5 * P0 + 5*(1 - t)^4*t * P1 + 10*(1 - t)^3*t^2 * P2 + 10*(1 - t)^2*t^3 * P3 +

5*(1 - t)*t^4 * P4 + t^5 * P5

If there is... Left at last 3 A little bit , So the last three points , Then we can use the fourth order Bessel to add ( The last point in the upper paragraph Q0 + Auxiliary point Q1 + The last three points are five points in total , Construct fourth order Bessel ); If there is... Left 2 A little bit , Third order , By analogy, the last few points are smoothed with Bessel , The result is as follows ：

You can play a video of the whole process ：

Fifth order Bessel optimization RRT curve