本文介绍了机器人路径规划的两种主要方法，基于搜索的A*算法和基于采样的RRT算法，以及它们的优化版本。A*算法通过启发式函数提高了搜索效率，RRT则通过随机采样寻找可行路径。轨迹优化部分讨论了多项式轨迹表示和最小snap方法，用于生成平滑的机器人运动轨迹。文章还提到了在实际应用中的安全飞行走廊和软约束处理策略。
目录
本篇文章总结自b站的视频教程机器人路径规划、轨迹优化系列课程
系该课程的个人向笔记总结。有需要的同学请自行移步的原up主的视频页。
机器人轨迹规划
planning可以分为两个部分，一个是路径规划，一个是轨迹优化。
所谓路径规划就是已知起点终点障碍物等，找到一条从起点到终点的可行路径，这条可行路径未必最短，未必最优，不同算法找到的只是一个“可行解”。
轨迹优化解决的则是在已知这条大致路径的情况下，让车辆平滑的行驶到终点的问题。
I. 路径规划
1. 基于搜索
Dijkstra算法
最难的也是最基础的路径规划算法，总的来说就是从起始点开始，广度优先遍历每一个子节点，每次都计算权重，然后对比最短权重路径。
使用该算法需要进行栅格化，当点特别多的时候，在栅格地图上就会表现为“大水漫灌”式搜索，直到到达终点。
（*思考一下rviz里面，Pose Estimation设置终点，turtle自己寻路过去的那个例子，和这个很相似，是否完全一样呢）
A*算法
提出的动机：减少收录的栅格数目，增加搜索速度。属于静态规划算法。其衍生算法D*可大幅提升效率。
F(n) = g(n) + h(n)
在Dijsktra的基础上，增加了这个启发式函数h(n)
其中h(n)的含义是当前点到终点的距离，其他的使用和Dijkstra一样。for example，利用Dijkstra进行广度优先遍历时，假设当前点有2个子节点，分别计算两个子节点到终点的距离h(n)，距离较短的那个入选。每轮都经过这样一层迭代之后，改变了Dijsktra“大水漫灌”式的搜索方式，更专注于寻找离终点近的点。两种算法的对比图如下所示。
这里还有一篇关于A*算法的比较好的总结：
路径规划之 A* 算法
目前在工程使用中，都是用A*算法，因为效率高，而且启发式函数（Heuristics）的选取也是一个值得研究的课题。
算法代码实现-python
1. 初始化
- 设置起点终点
- 设置网格大小2
- 设置机器人半径1
- 设置障碍物地图
2. class AStarPlanner
init()
class Node (x, y, cost, parent_index)
…
2. A* planning算法数据流程图
Autoware中的A*
/core_planning/waypoint_planner/src/astar_avoid
/core_planning/astar_search
/core_planning/freespace_planner/src/astar_navi
2. 基于采样
RRT
快速扩展随机树算法Rapidly Exploring Random Tree
基于采样的算法和基于搜索的算法的一大不同是，不需要进行地图栅格化。
大致的过程就是（个人理解），已知起始点和终点。从起始点开始，在规定的M范围中随机选取一个点Xrand，这个点的方向也是随机的，然后找到最近的点Xnear，父节点和Xrand之间没有obstacle的话，这条路就可以走，再寻找Xrand的下一个Xnear；如果中间有obstacle，就不能走，重新选择。循环这个过程可以得到一条到达终点的路线，其中会设置一个step，如果当前点和终点的距离小于step，就直接连到终点，如果有obstacle，继续迭代找random，如果没有就到达了终点。
其中有一个小技巧就是，可以设置每次都以10%的概率，选择终点为下一个点进行尝试，说白了就是看能不能从当前点直达终点而途中没有障碍物。反正两点之间直线最短，能直接到肯定比啥都好使。（有些细节的理解可能有偏颇的地方，暂时先这样记录，后续可更新。）
RRT的算法代码中，有一个比较有意思的点是，计算点到直线的距离。
RRT算法的结果如下所示：
可以看出由于随机选择点，结果乱七八糟纵横的，最终选出来的路径也谈不上什么最优。只是这样用线段的方式向前延伸的计算可行路径，理论上速度比基于搜索的A*算法等速度要快很多。（一个问题，这个速度的对比要看栅格化的单位和障碍物大小的对比吧，如果障碍物特别小，或者可以把栅格做到比较大呢？不过大概率栅格单位小，障碍物体积较大，单位也大，so…）
RRT*
提出的动机：能否找到一条最优的路径，在RRT找到的只是可行解的基础上。
于是，RRT*的思路就类似于基于Dijsktra的A*一样，也是类似于增加了一个启发式函数的思路。
每次找到一个新的Xnew之后，可以为当前节点重新寻找父节点，已经找到的节点能够存在必然是没有obstacle的阻隔，那么在所有符合条件的父节点，也就是可行解里面，进行回溯，将距离最近的点作为新的父节点，就可以取得更好的效果。如图所示。
需要注意的是，这些计算对比都是在蓝色圈圈的范围内进行的，太远的回溯没有太大意义，徒增计算量。
Informed RRT*
提出的动机：能否增加渐进最优的速度？
于是乎一位大佬发表了这篇论文Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic
3. 基于智能算法
II. 轨迹优化
轨迹就是带时间参数的曲线
在已经知道路径规划的基础上，让机器人平滑的从起点到终点
路径规划的算法都是比较老，而且也比较成熟的，而轨迹规划方面，近几年提出的新想法新论文比较多
1. 多项式轨迹表示方法
如图，单一函数可能无法表示一条很长的轨迹，所以需要用分段函数来表示。
根据路径长度和平均速度分配。
假定轨迹的函数为p(t)，是一个与时间相关的函数。
速度为p(t)的一阶导数，理解为斜率
加速度为p(t)的二阶导数，理解为斜率的变化方向
jerk为p(t)的三阶导数，可以理解为斜率变化的速度
snap为p(t)的四阶导数，可以理解为斜率变化的加速度
数学中，通常只有函数的前四阶导数有实际意义（其实三四阶的意义已经有点emmm…）更高阶导数一般只有数学意义。
接下来求解参数p，就可以确定轨迹
2. Minimum snap
求解过程中：
基础要求
- 两端轨迹之间连续
- 轨迹经过固定点
- 轨迹无碰撞（比如原来路径规划的waypoints无碰撞，但是将路径平滑后可能会有碰撞，必须避免）
高级要求
轨迹最顺滑、能量最优等
等式约束可表示经过固定点，不等式约束可表示无碰撞。
分别求三个min，论文Minimum snap trajectory generation and control for quadrotors中提出了这个观点。
对整段轨迹(0,T)进行积分，这里用的是snap，分成k段，其中每一段的起始时间和结束时间t(i-1)到t(i)都是分配好的。
将前面的snap表达公式代入，该函数与p无关，提出。
求解中间红框内的函数，表示为Qi，由于时间已知，因此可以计算出来，它的维度就是(n+1)(n+1)，n是多项式参数的个数，也就是多项式的个数。其中r和c表示索引从零开始时，分别对应的哪行哪列。（这个矩阵怎么乘出来的没太懂，乱七八糟）
Q是对称矩阵，且为方阵。
总之最后算出了最短的那行表达式，其中Q可以写成对角矩阵的形式，最终得到min(pT)Qp。详细推导可见上面的csdn blog。
设定约束条件，使经过某点时，满足位置，速度和加速度的要求。
连续性约束这个表示的是位置之差为0。
等式约束个数解析=>整个轨迹函数分3段表示，4个航路点，所以约束条件有：
- 每段函数的起点共3个
- 每段函数中间的连接点为固定点，有k-1个固定点
- 每段函数的终点有3个
- 中间点要求连续，一共是3(k-1)个??
一共4k+2个约束
PVA即position velocity acceleration
于是问题成为了一个二次规划问题，一定有最优解，加上等式约束或不等式约束，也可以求到最优解。
如果加了等式约束，取到的只能是某个点，如果加了不等式规划，那么一条线上的都可能被取到。
这篇文章提供了quadprog()函数 求解二次规划问题。
其中H就是刚才推导中的Q， f*X在刚才的推导中没有，赋0。
quadprog()函数中Ab表示不等式约束，代入到公式中即可。
代码运行结果示例如下：
安全飞行走廊
对不等式的构建，一般称为安全飞行走廊。
如下图，轨迹上有一个障碍物，是必须要避开的。
因此加入不等式约束，将轨迹规定在蓝色的框框里面。
相当于构建了一个走廊(corridor)，这里展示的是一个笨办法，有更新更好的方法解决这个问题。
3. 闭试求解
采用论文Richter C, Bry A, Roy N. Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments中提出的方法
A matrix中，把每一段的位置，速度，加速度都写出来。
k段函数，第一个函数的起始时间和结束时间分别是t0和t1，这些时间是提前分配好的。但中间点的PVA都是未知的，一共6k个值，每段两个点，每个点三个值。
注意Atotal中，最开始的p表示的是多项式的参数，最右边的p表示的是position。
所以
p = A*-1*d
A*-1*只和时间有关，只要得到d，就可以求出p，即多项式的所有参数。
pi(ti) = pi+1(ti)
的意思是，第i段末尾的位置和i+1段起始位置是相同的，后面的速度和加速度也是同样的道理。这里的p又表示position了。
就可以写成下面的
最右边的列向量里面是没有重复项的，每个航路点都有三个值，k段一共是k+1个航路点。
进一步得到下面的
把d转换为d’，再把d‘表示为C[dF,dP]T, 其中dF表示起点终点已知PVA的值，dP表示中间未知点的PVA值。
进一步推导：
min J 是目标函数，把p代入目标函数，根据矩阵运算法则，Q和R均为对称矩阵。
RFF,RFP,RPF,RPP四个矩阵分别是按照dF和dP的下标对R矩阵进行了分割，其中RFF和RPP都是对称矩阵。然后得到4项表达式。
根据矩阵运算法则容易知道，目标函数J最终是一个数值，同理拆分出来的4项，每一项也都是一个数值。四项中间的两项，简单推导可知，二者相差一个转置，而常数的转置还是它自己，因此两项可以合并。
min J是一个凸函数，一定可以取到最优值，类似于二次规划问题，极值点就是导数为零处。
目前公式中，dF已知，R已知，只有dP未知，对该式进行求导=>
矩阵求导，对dP进行求导，第一项与dP无关，因此求导后没有了，第二项的导数容易得到，第三项的求导看下面的矩阵求导规则。
简单变换得到dp的表达式。此时dF和dP和K全部已知，则可以求出p，也就是全部多项式的参数。
计算复杂度主要集中在求A*-1*,其他主要是矩阵运算。
下面是论文中展示的几个例子
左边是RRT*给出的路径规划点，右边是算法找出的平滑路线。
上图是点云图中的实际应用，找到的路线示例。
上图是原轨迹碰到障碍物时，可以在中途多指定几个必须经过的点，进行避障。
整体求解步骤如下：
4. 软约束
基于这篇论文Gradient-based online safe trajectory generation for quadrotor flight in complex environments
公式（2）
闭式求解的缺陷就是之前提到的障碍物的问题，这篇论文中提出了解决方法。
其目标函数就是之前提到的，通常可以用梯度下降的方法求解，有很多开源的求解器可以直接使用。
fo是碰撞目标函数，fv和fa分别是速度和加速度的目标函数。
公式（3）
等式左边的符号念做eta。
公式（4）-（6）
fs和之前一样，可以通过对dp求导，根据凸函数的性质，得到最优解。这部分的求解和前面的一样，如下图。
但是这个最优解未必是碰撞函数、速度和加速度的最优解。
公式（7）
接下来讲碰撞目标函数及其导数的求解。后面速度和加速度的求解同理。
首先引入一个penalty function惩罚函数，该惩罚函数需要满足：环境约束少时，函数值小，环境约束多时，函数值大，且由于要对函数求导，因此该函数需要可微。因此选择exponential function指数函数，公式如下：
其中d表示无人机当前位置，地图会保存无人机位置及距其最近的障碍物信息，有这个地图之后，可以通过地图快速查找到无人机当前位置和障碍物的距离以及梯度。d0是安全距离，r是自定义常数，阿尔法是系数。当d小于d0时，（d-d0)是负数，加负号为正，指数函数值趋向于很大，反之指数函数值趋向于0，相当于惩罚函数不起作用。
每一个点都要考虑，因此考虑对函数进行线积分？？？
公式（8）
ds也就是delta_s，是距离，就等于v(t)dt，这里的v(t)是xyz方向的所有速度叠加的实际速度，不是单一方向上的，这是一个离散值，无法用积分形式表达，因此离散化后用求和符号表示，这里beta_t就是delta_t，是指定的常数。
p(Tk)表示无人机在某时刻所处的位置，通过地图可以得到d和其导数？？c表示惩罚函数，
公式（9）
求fo关于dPx，dPy，dPz的导数，表示为Jo，fo的表达式都是复合函数，按照复合函数求导法则（前导后不导+后导前不导，以及先对外层函数求导，再对内层函数求导）进行求解。F是对里面的函数求导后产生的部分，
前面的eta函数，是时间*位置，就是前面多项式中的这个p(t)
然后由公式（3）可知，对dP求导时，由矩阵乘法规则，相当于只保留了相乘后矩阵的右半部分，Ldp，得到F=TLdp，T是时间。
在对速度v求导时，速度v的表达式是根号下（vx_square+vy_square+vz_square)，每次求导只能是针对某个方向，在xyz方向上分别求导。eg在x方向上求导，与yz无关，那么对根号下vx_square求导的结果就是(1/2)*根号下vx*2，后面这个2是vx_square求导之后得到的，两个2抵消，就得到vmiu/||v||。
5. 贝塞尔曲线和硬约束
6. 论文总结
7. 课程资源
Coursera 【Final Project： Self-Driving Vehicle Control】大作业
coursera Motion Planning
知乎：运动规划为什么常使用五次多项式：泛函分析
扫一扫
1517
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
