【路径规划】RRT增加动力模型进行轨迹规划

在上篇增加了一个运动对象之后，就可以考虑给这个运动对象增加动力模型，之后按照动力模型来进行轨迹规划。

思路

这个动力模型我的思路是参照开源飞控中使用向量法指定速度方向，使用S曲线来规划速度轨迹的方式，最终做出来的效果看起来还是可以的。

S曲线

这里可以补习一下开源飞控中的速度S曲线规划的知识，总体思想为：保证加速度，速度连续，以固定加加速度来改变速度。总体的效果就是：先以恒加加速度将加速度a加到最大加速度a_max，时间为T1，之后以恒加速度运行T2时间，之后以恒加加速度将加速度从a_max减到0。这里PIX4的源码中也给出了一个注释图：

网上有人画了一个更清晰的图，我借用一下：

按照PIX4的思想，是将整个速度跟踪的过程分为三个时间阶段T1，T2，T3。T1阶段以设定的加加速度进行加速度的提升，T2阶段为恒定加速度，T3阶段以负的恒定加加速度进行加速度下降直到0。我们可以每个周期判断此时是处于哪个阶段，就可以进行控制了。最终这个算法需要达到的目标是在最后的加速度为0的时候，刚好速度达到目标。

这个各个阶段的时间以及末端速度都是高中知识，比较好推导，而且也有源码以及网上的解析可以参考，感觉没必要再浪费篇幅，可以自行推导一下

大致的程序步骤为：

1）计算当加速度为0时的轨迹速度

2）根据加速度为0时的末端速度的大小与目标速度的偏差判断加速方向

3）计算T1，T2，T3各个时间长度

4）根据控制周期的时间间隔判断当前处于哪个时间段，进而生成加速度与速度

按照S曲线的方式，我编写了一个函数，传入最大加加速度，最大加速度，当前加速度，当前速度，目标速度，控制周期T，就可以生成下个周期的加速度与速度控制量：

测试一下，它大概生成的速度S曲线的效果是这样的：

其实效果还不错。之后就可以利用这个速度S曲线函数来牵引模型在轨迹上运动。

牵引轨迹

在前面的文章中，我们已经使用了RRT搜索关键路径点，之后使用五阶贝塞尔平滑了曲线，可以得到平滑曲线的各个轨迹点，那么我们可以设置一个到达区域，比如以10为半径的圆，到达目标轨迹点的半径10以内就算到达该轨迹点，之后就切换到下一个轨迹点进行牵引。牵引的方式可以使用向量的方式，这个也是我比较顺手的方式吧：

由于目标轨迹点我们是知道的，因此可以按照目标轨迹点计算剩余路程，之后我们可以用利用计算出的剩余路程，还没有到一定的剩余路程就以最大速度为目标，接近终点时，就做一些减速的规划，这个就是一个决策的问题了，不属于本章的讨论内容：

当然，这个属于简单的一些速度决策，不影响后续的结果。

之后生成了目标加速度与速度之后，我们就可以分解成xy轴方向的分加速度与分速度进行xy轴的加速度，速度，位置的控制。

最终的效果，我录了一个简易的视频：

RRT+贝塞尔+运动模型轨迹

这样整体效果就好玩多啦。