文章目录
运动规划（Motion Planning）包括路径规划（Path Planning）和轨迹规划（Trajectory Planning）
1. 路径规划（Path Planning）
路径规划是任务层级的规划，不涉及运动速度、运动时间等变量
一般在机器人的操作空间内进行，主要包括环境建模、路径搜索和路径平滑三部分。
路径规划又分全局路径规划和局部路径规划，全局路径规划一般也称为离线规划或静态规划；局部路径规划又称在线规划或动态规划
（1）全局路径规划
主要方法有：
- 栅格法：首先建立栅格地图（四叉树Quadtree、八叉树Octree），然后使用路径搜索算法
- Dijkstra
- 深度优先法
- 宽度优先法
- etc.
- C空间法
- Voronoi图法
- 拓扑法
- 概率路径图法等等
栅格法：
A*
- Step1：将关注点A作为待处理点存入一个OpenList
- Step2：寻找A点周围所有可到达或者可通过的栅格，加入OpenList，为所有这些栅格保存A点作为父节点
- Step3：从OpenList中删除A点，将其加入CloseList
- Step4：继续搜索，选中当前OpenList中F值最小的节点，将其从OpenList中删除，然后添加到CloseList
- Step5：检查所有相邻节点，跳过已经在CloseList中的或者不可通过的节点，将它们添加到OpenList，对于新加入开启列表中的节点，把选中的栅格作为其父节点
- Step6：若某个相邻节点已在OpenList，检查现在的路径是否更好
A是目前为止最快的一种计算最短路径的算法，但它是一种较优算法，而非最优
A：存在“指数爆炸”的问题
- 搜索过程中，需要建立两个列表：OpenList（存储所有出现在最终路径中的节点）和CloseList（存储已经检查过一次且不再需要检测的节点）
- 对于一个待检查节点：①H：从起点沿产生路径移动到该点的耗费；②G：从该节点移动到终点的预算；③F=G+H：总耗费；④是否存在关闭列表；⑤其父节点位置
贪心算法、Dijkstra算法、A算法及加权A算法之间的关系：
假设
f
=
a
g
+
b
h
f=ag+bh
f=ag+bh
- a = 0 ， b = 1 a = 0，b = 1 a=0，b=1：贪心算法，只考虑离目标最近
- a = 1 ， b = ε > 1 a = 1，b = ε > 1 a=1，b=ε>1：加权A*算法，根据权重，调整贪心程度
- a = 1 ， b = 1 a = 1，b = 1 a=1，b=1：A*算法
- a = 1 ， b = 0 a = 1，b = 0 a=1，b=0：Dijkstra算法，只考虑目前已经花费的成本
（2）局部路径规划
主要方法有：
- 人工势场法（Aritificial Potential Field）
- 模糊逻辑算法
- 遗传算法
- 基于神经网络的方法
- etc.
人工势场法
- 基本思想：目标点对机器人产生引力，障碍物对机器人产生斥力；距离越近，斥力越大；距离越远，斥力越小。合力=斥力+引力，沿着势场负梯度方向搜索即可规划出机器人的无碰撞安全路径。
- 特点：
- 优点：算法简单，易于实时控制
- 缺点：易陷入局部最优且无法逃脱
问题1： 势场中可能存在引力斥力等大反向的情况，从而导致机器人陷入局部陷阱而无法逃逸
解决方案： 判断是否在目标点，如果不是，则加随机扰动，使其跳出局部最优
问题2： 目标点与某个障碍物距离很近时，斥力可能大于引力，机器人很难到达目标点
解决方案： 修改斥力场函数，使得机器人在靠近目标时，虽然斥力场增加，但距离在减小，在一定条件下减小斥力场作用而增加引力场作用
问题3： 机器人离目标点较远时，引力较大，导致忽略较小斥力，从而撞上障碍物
解决方案： 修改引力场函数，增加距离阀值，从而使其在离目标点较远时对其引力进行限制，避免过大
2. 轨迹规划（Trajectory Planning）
轨迹规划是执行层级的规划，需要包含位置、速度、加速度等信息。
轨迹规划主要包括关节空间轨迹规划和笛卡尔空间（操作空间）轨迹规划
（1）关节空间的轨迹规划
① 直线插值
θ
˙
=
θ
f
−
θ
0
t
f
\dot{\theta}=\frac{\theta_{f}-\theta_{0}}{t_{f}}
θ˙=tfθf−θ0
θ
=
θ
0
+
θ
f
−
θ
0
t
f
⋅
t
\theta=\theta_{0}+\frac{\theta_{f}-\theta_{0}}{t_{f}} \cdot t
θ=θ0+tfθf−θ0⋅t
存在的问题： 关节速度在节点处不连续，加速度无穷大
解决方案： 为了得到角度和速度都平滑的运动轨迹，一般在起点和终点处增加一段抛物线轨迹的缓冲区
② 抛物线过渡的直线插值
存在的问题： 加速度不连续，存在突变，会造成关节振动冲击
得到的轨迹不唯一，因为多解（加速度可以任意选择，故轨迹无数条）；
梯形截面速度
③ 三次多项式函数规划
三次多项式函数通式： θ ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 \theta(t)=a_0+a_{1}t+a_{2}t^2+a_{3}t^3 θ(t)=a0+a1t+a2t2+a3t3
需要满足 t 0 t_0 t0和 t f t_f tf时刻通式的位置、速度与规划目标相同
求得系数：
特点： 一、二阶微分光滑，被广泛使用，只能满足位置、速度约束，不能满足加速度约束
抛物线截面速度
④ 五次多项式函数规划
五次多项式函数通式： θ ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 + a 5 t 5 \theta(t)=a_0+a_{1}t+a_{2}t^2+a_{3}t^3+a_{4}t^4+a_{5}t^5 θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5
需要满足起始点和终止点的角度约束、速度约束和加速度约束
解得系数：
此外，还有七次插值、正余弦插值、doubleS等等……
（2）笛卡尔空间的轨迹规划
① 直线轨迹规划
姿态采用RPY角或欧拉角形式，但对于一些特殊位置，无法实现正常插值，此时应使用四元数来描述
插值方法：
- 末端运行速度v、加速度a、插补时间t，插补时间一般由机器人伺服控制周期决定，通常为ms级
- 计算两节点间直线距离 l l l
- 计算插补次数，步长
△
=
v
t
△=vt
△=vt，则插补次数为：
- 计算末端运动变量： d p = [ x 2 − x 1 n , y 2 − y 1 n , z 2 − z 1 n , α 2 − α 1 n , β 2 − β 1 n , γ 2 − γ 1 n ] T \mathrm{d} p=\left[\frac{x_{2}-x_{1}}{n}, \frac{y_{2}-y_{1}}{n}, \frac{z_{2}-z_{1}}{n}, \frac{\alpha_{2}-\alpha_{1}}{n}, \frac{\beta_{2}-\beta_{1}}{n}, \frac{\gamma_{2}-\gamma_{1}}{n}\right]^{\mathrm{T}} dp=[nx2−x1,ny2−y1,nz2−z1,nα2−α1,nβ2−β1,nγ2−γ1]T
- 由公式 p k + 1 = p k + d p , k = 1 、 2 … … n p_{k+1}=p_k+dp,k=1、2……n pk+1=pk+dp,k=1、2……n即可求出末端在下一插补点位姿，然后使用逆运动学求解关节变量 q k + 1 q_{k+1} qk+1
② 圆弧轨迹规划
略
博主Note：
- 此处的笛卡尔空间轨迹规划应该算是不考虑避障的路径规划，工业机器人中使用较多
- 实际笛卡尔空间规划如果考虑动力学规划，会增加实时的计算量，因为关节规划只计算始末点的动力学信息即可，而笛卡尔规划时需要实时计算逆运动学、雅克比、动力学等问题
- 实践中，运动控制器一般都会集成P2P的规划算法，即本文中提到的关节轨迹规划
- 如果想要更小的冲击，应该考虑速度前瞻的问题
参考文献：
- 战强老师的《机器人学：机构、运动学、动力学及运动规划》第7章
扫一扫
5347
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
