[path planning] RRT adds dynamic model for trajectory planning

After adding a moving object in the previous article , You can consider adding a dynamic model to this moving object , Then the trajectory planning is carried out according to the dynamic model .

Ideas

My idea of this dynamic model is to refer to the vector method used in open source flight control to specify the speed direction , Use S Curve to plan the speed trajectory , The final effect looks ok .

S curve

Here you can catch up on the speed in open source flight control S Knowledge of curve Planning , The general idea is ： Guaranteed acceleration , Continuous speed , Change the speed with a fixed acceleration . The overall effect is ： First, add the acceleration with constant acceleration a Add to the maximum acceleration a_max, Time is T1, Then run at constant acceleration T2 Time , Then, the acceleration is increased from a_max Reduced to 0. here PIX4 An annotation diagram is also given in the source code of ：

Someone on the Internet drew a clearer picture , Let me borrow it ：

according to PIX4 Thought , The whole speed tracking process is divided into three time stages T1,T2,T3.T1 In the stage, the acceleration is increased at the set acceleration speed ,T2 The phase is constant acceleration ,T3 In the stage, the acceleration decreases with a negative constant acceleration until 0. We can judge which stage we are in every cycle , You can control . The final goal of this algorithm is that the final acceleration is 0 When , Just the speed reaches the target .

The time and end speed of each stage are high school knowledge , It's easier to deduce , And there are also source code and online analysis for reference , I feel no need to waste any more space , You can deduce it by yourself

The general procedure steps are ：

1） Calculate when the acceleration is 0 Track speed at

2） The acceleration is 0 The difference between the end speed and the target speed determines the acceleration direction

3） Calculation T1,T2,T3 Each length of time

4） Determine the current time period according to the time interval of the control cycle , And then generate acceleration and velocity

according to S The way of the curve , I wrote a function , Incoming maximum acceleration , Maximum acceleration , Current acceleration , Current speed , Target speed , Control cycle T, The acceleration and speed control quantity of the next cycle can be generated ：

Test it , The approximate speed of its generation S The effect of the curve is like this ：

In fact, the effect is not bad . Then you can use this speed S Curve function to pull the model on the trajectory .

Traction track

In the previous article , We have used RRT Search for critical path points , Then use the fifth order Bessel to smooth the curve , Each track point of the smooth curve can be obtained , So we can set up a Reach the area , For example 10 Is a circle of radius , The radius of reaching the target trajectory point 10 The track point is reached within , Then switch to the next track point for traction . Traction can be used vector The way , This is also a more convenient way for me ：

We know the target trajectory point , Therefore, we can follow the target trajectory points Calculate the remaining distance , Then we can use the calculated remaining distance , Take the maximum speed as the goal before reaching a certain remaining distance , Near the end , Just make some plans to slow down , This is a decision-making problem , Not part of this chapter ：

Of course , This is a simple speed decision , It does not affect the subsequent results .

Then the target acceleration and velocity are generated , We can decompose it into xy The partial acceleration and the partial velocity in the axial direction xy The acceleration of the axis , Speed , Position control .

Final effect , I recorded a simple video ：

RRT+ Bessel + Motion model trajectory

In this way, the overall effect is much more fun .