龙猫的博客

以前在做规划仿真时，用的时贝塞尔曲线，但贝塞尔曲线有一个缺点，就是所有的点都参与计算，在规划中，大部分的情况，计算出来的路径点都是比较曲折的，用标准的贝塞尔就会与计算出来的路径点贴合度不高，这种情况下，当时是写了分段贝塞尔，每5个点为一组来分段计算贝塞尔。大体是按照上述的递推公式来编写递归函数，当然，这个递归函数有一些细节需要注意一下。由于 k = 1 时，就会退化为分段函数，因此 k 至少也要为 2。，因此许多的规划算法也都采用B样条的方式来平滑曲线。P 点就是控制点，也就是规划时的关键路径点。

Dubins路径

vfh局部路径规划绕障

在之前使用通用的A*算法来规划出路径之后，可以发现路径上其实是有许多的冗余点的，如下图所示：上图中可以明显看出有一些折弯点是冗余的，因此本节内容就介绍使用垂距限值法来优化冗余点。垂距限值法的思想也是比较简单的，概括一下就是：计算当前节点到上节点与下节点连线的距离，超过一定的阈值就从总集合中删除当前点。总体思想就如它的名字一样。这里我写成了一个函数Vertical_distance_limit：正在上传…重新上传取消当然，此函数中我又封装了一个函数，即计算垂距的函数compute_vertical_dista

在上篇增加了一个运动对象之后，就可以考虑给这个运动对象增加动力模型，之后按照动力模型来进行轨迹规划。这个动力模型我的思路是参照开源飞控中使用向量法指定速度方向，使用S曲线来规划速度轨迹的方式，最终做出来的效果看起来还是可以的。这里可以补习一下开源飞控中的速度S曲线规划的知识，总体思想为：保证加速度，速度连续，以固定加加速度来改变速度。总体的效果就是：先以恒加加速度将加速度a加到最大加速度a_max，时间为T1，之后以恒加速度运行T2时间，之后以恒加加速度将加速度从a_max减到0。这里PIX4的源码中也给出

在上一篇的路径规划文章中，使用多段五阶贝塞尔曲线优化RRT搜索后的路径，虽然视频上看起来还好，但还是不够好玩，为了让其有趣一些，突然想增加一个运动对象(例如一个三角形)，在生成平滑曲线后，沿着平滑曲线运动，这个是本期想要实现的目标，在实现之后，后面可以考虑再增加一些运动模型来使得这个运动对象符合生活中的一些运动对象。首先我们使用一个三角形来表示一个运动对象的话，需要找到一个画三角形的方法，这个需要借助 matlab 的 fill 函数，这个函数的作用大致是这样：给出几个点的横纵坐标，然后使用 fill 函数

在上一篇关于贝塞尔曲线平滑路径的文章中，只是大致的介绍了一下贝塞尔曲线的原理，并且初步测试了一下，最终平滑的效果，还是不尽人意的，最开始是使用了简易的多段二阶贝塞尔，得到的曲线有很多的尖锐：然后使用了全局的贝塞尔，也就是所有的点全一次用上，得到一个N阶贝塞尔，但是这样就会有一个缺点，那就是当转弯很多的时候，它的效果就并不那么好了，就像这样：有时就会出现上述的情况，比较容易压到障碍，并且与原路径差别较大。那么在实际的贝塞尔优化路径的使用中，经常使用的方法可以直接告诉大家：使用多段五阶贝塞尔 + 插辅助点的方法

快速扩展随机数(RRT)算法，是近十几年应用比较广泛的一种运动规划算法。它的大致原理为：原始的RRT算法通过一个初始点作为根节点，通过随机采样，增加叶子节点的方式，生成一个随机扩展数，当随机树中的叶子节点包含了目标点或进入了目标区域，边可以在随机树中通过回溯的方式，找到这条从初始点到目标点的路径。RRT总体是一种基于概率采样的搜索方法，通过状态空间的随机采样点，把搜索导向空白区域，从而孕照到一条从起始点到目标点的规划路径，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，当然它也有自身的缺点

A*算法，A*（A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。算法中的距离估算值与实际值越接近，最终搜索速度越快。从上章节的RRT算法的效果来看，A*算法相较于RRT的效果是要好许多的。如果说有个地图的缝隙很小，对于RRT算法来说，是比较难找到出口的，就像下面这个地图：因为RRT是一种随机搜索，但A*的话，遇到这种地图，虽然也要迭代许多次，但是是可以最终找到出口的，而且最终搜索出来的路径就会好许多。原理与实现A*算法的原理，如果是

贝塞尔曲线一般是用于二维图形的一种数学曲线，一般是用于一些矢量图的设计，不过在路径规划中，也可以应用上，例如之前的RRT随机搜索算法，因为是随机搜索，因此得到的路径点的曲折度是很大的，除了RRT，在其他的搜索算法得到路径点之后，依然也可以使用贝塞尔曲线来优化路径，使其更加平滑。一阶贝塞尔与二阶贝塞尔对于一阶贝塞尔的话，最终得到的曲线还是一条直线，不过还是可以列一下通式方便后续推导：对上图，B1为一阶贝塞尔的点，则B1点随时间 t 的坐标为：B1(t) = P1 + t * (