【路径规划】使用垂距限值法与贝塞尔优化A星路径

在之前使用通用的A*算法来规划出路径之后，可以发现路径上其实是有许多的冗余点的，如下图所示：

上图中可以明显看出有一些折弯点是冗余的，因此本节内容就介绍使用垂距限值法来优化冗余点。

垂距限值法

垂距限值法的思想也是比较简单的，概括一下就是：计算当前节点到上节点与下节点连线的距离，超过一定的阈值就从总集合中删除当前点。总体思想就如它的名字一样。

这里我写成了一个函数Vertical_distance_limit：

 

当然，此函数中我又封装了一个函数，即计算垂距的函数compute_vertical_distance

垂距计算

关于垂距计算，我们可以使用向量法，如下图所示：

使用向量法计算垂距的步骤可以按照如下来进行：

1) 先计算向量last_to_next的单位向量u_last_to_next

2) 计算向量last_to_pos，之后计算last_to_pos与向量u_last_to_next的点乘，即为last_to_pos到向量u_last_to_next上的投影长度L

3) 使用向量u_last_to_next乘以L即为向量last_to_closest

4) 使用last的坐标加上last_to_closest即可求出closest的坐标

5) 之后就可以求出pos与closest的距离

使用垂距限值法的效果

在增加了垂距优化后就可以得到如下的效果：

上图中红色连线点为初始的A*算法路径点，绿色连线点为优化后的路径点

使用五阶贝塞尔优化路径

在优化了路径后，我们可以进一步使用贝塞尔曲线规划出一条更平滑的曲线：

增加运动轨迹模型

之后我们可以按照之前做RRT那样，增加速度S曲线的运动轨迹模型，最后的效果就是这样的：