机器人路径规划及轨迹优化

Mystique101

已于 2023-10-09 21:12:50 修改

阅读量5.4k

点赞数 10

文章标签：机器人

于 2023-10-09 16:38:13 首次发布

本文链接：https://blog.csdn.net/weixin_65978597/article/details/133685528

版权

路径规划+轨迹优化

路径规划

一、基于搜索的路径规划算法

1. Dijkstra算法

从起点出发，将起点放入到 open list 当中，找到起点最近的节点。先把起点本身收录到 closed list 当中。收录之后进行第二步，遍历该节点的邻接节点（一步一步往外走），需要为它这些邻接节点找到到起点的距离。如现在可以从1 $\rightarrow$ 2（2），也可以从1 $\rightarrow$ 4（1），括号内代表该节点距离起点位置。找到距离较近的节点后，将其收录到 closed list 。然后再遍历下一个节点，依次更新计算。

算法的具体流程：

栅格地图

灰色的是障碍物，红色的是机器人。通过膨胀障碍物，将机器人视为质点来规划路径。在论文中常见（Configuration Space）构型空间，就是以一定的大小将地图分成一块一块的小块。

最后一个问题是如何把栅格地图转化为有权图。可以把栅格地图中的栅格看成有权图的节点，机器人的运动范围看作是有权图中节点和节点之间的连接。这样就联系了起来。

定义：是一种以固定大小的网格划分地图，将地理信息转换为离散化的二值或多值数据。每个网格代表一个空间单元，通常用于描述城市规划、交通导航、自动驾驶等领域中的地理信息。

栅格地图的每个网格包含了有关该区域的各种属性信息，如道路交通状况、建筑物高度、道路宽度、停车位位置等。与矢量地图不同，栅格地图能够更精确地描述地理信息，并且可以通过连续的网格来表示位置之间的相对距离。

2.A*算法

A*算法提出的动机：减少收录的栅格数目，增加搜索速度。

F(n) = g(n) + h(n)

增加启发式函数（Heuristics），下图为两算法扩展及流程的对比：

启发函数必须满足三个条件：（1）它是一个启发式的函数，即能够给出当前节点到终点的估计距离；（2）它是一个实际的估计，不应大于实际距离；（3）它是合理的，即不大于从当前节点到终点的实际成本。常用的启发式函数包括曼哈顿距离函数、切比雪夫距离函数和欧几里得距离函数。

二、基于采样的路径规划

1.RRT算法

快速扩展随机数算法，从根节点长出一棵树，当树枝长到终点的时候，我们就能从终点找到一条到根节点的唯一的一条路径。有根节点和终点的设置，也需要设置步长。

小技巧：以一定概率选择终点作为采样点，可以加快算法导向终点的速度。但是如果终点和起点间有障碍物的话，每次都向终点走，那他每次都会碰到障碍物而无法到达终点。所以还是要向外围扩展，一般设置一个概率（5%或者10%）。

2.RRT* 算法

RRT*算法提出的动机：能否能找到一条最优的路径？

改进之处：

1.为当前节点重新选择父节点（路径更优）

2.范围内的节点重新连接

3.Informed RRT*算法

提出的动机：能否增加渐进最优的速度。

4.PRM（概率路线图）算法

1.构建概率路线图

（1）随机采样点

（2）将新采样点和距离小于阈值的采样点连接产生图

2.在图上寻找路径

（1）Dijkstra算法

（2）A*算法

缺点：两阶段，速度较慢，且不能找到最优路径

三、基于智能算法的路径规划

1.基于遗传算法的路径规划

会产生很多这样的路径，具体种群初始化流程：

（1）每行选择一个栅格

（2）判断相邻栅格是否连续，具体公式：首先为地图建立一个坐标系，为每一个栅格分配一个 x，y 方向上的坐标，假设两个栅格之间 x 的差或者 y 的差，最大值只差1的话，这两个栅格可以直接走过去。说明这两个栅格是相邻或者是对角的。

（3）不连续时进行插入栅格操作，直到连续。（距离比较远时，反复进行这个过程，直至连续）

1.选择

有两条路径就对应两个个体，要选择较好的个体。对两条路径进行打分，最直观的就是路径的长度，会用一个适应度来表示，就是它适不适应这个环境，对应到路径里面的话是希望这条路径越短越好。所以取路径的倒数，这样越短的路径适应度就会越大。还有一个就是路径的顺滑性，以相邻的三个点为例，它们会组成一个三角形，路径越顺滑，三角形的钝角越大，该角可以通过余弦定理来计算，然后再将它加入适应度函数，角度越大适应度越大。然后再对适应度函数求其权重，权重通过具体实验获得。有了适应度的值，就可以为每个个体计算概率，就是它被保留的概率。就是轮盘的方式，为每个概率划分具体的面积，这样概率越大，被选中的几率越大。这样也可能会保留较差的个体，它在后面可能会通过交叉和变异，会变得很好，这样就保留了多样性。