机器人学：（4）运动规划_机器人学路径规划

之前做机械臂的时候做到这里就没做了，这一部分了解的不多，早就在草稿里放着了，想着有时间完善一下，结果一直搁置，就先这样吧，有多少算多少，有缘再续……

https://blog.csdn.net/xiaozisheng2008_/article/details/114807543
https://zhuanlan.zhihu.com/p/514474615

4.1 运动规划、路径规划和轨迹规划

机器人的运动规划可以看做是包括了路径规划和轨迹规划两个步骤，目的是根据给定的任务，计算出一条可以下发给机器人控制器去执行的轨迹。

路径规划，顾名思义就是在有障碍的空间里找出一条没有碰撞的路径来，一般情况下，这些路径都由一系列的离散点来表示，相邻的点进行线性插值，确保每个离散点和中间的插值点都不会有碰撞。

轨迹规划是根据路径规划生成的离散点，计算一条连续的满足机器人运动学、动力学约束的包含时间信息的轨迹。在轨迹上按照实际的伺服周期采样位置点，下发给伺服控制器，从而使机器人运动到目标位置。

4.2 路径规划

路径规划一般是在机器人的操作空间进行的，根据可利用的环境信息完备性的不同，又可以分为全局路径规划和局部路径规划。

全局路径规划一般是基于静态环境的全局信息规划出静态的全局安全路径，也称为离线规划或静态规划。局部路径规划一般是基于机器人周围的局部环境地图并结合传感器实时采集的信息规划出机器人的动态局部安全路径，又称为在线规划或动态规划。

路径规划主要包括环境建模、路径搜索、路径平滑三部分。

4.2.1 全局路径规划

全局路径规划方法主要有栅格法、C空间法、Voronoi 图法、拓扑法和概率路径图法。

栅格法

栅格法实际上是一种环境建模方法，它把工作空间分割成规则而均匀的含二值信息的栅格。在机器人移动的过程中，栅格的尺寸和位置不变。二值信息分别表示该栅格处是否有障碍，没有障碍的栅格称为自由栅格，否则为障碍栅格。栅格的尺寸通常和机器人的基本移动步长相适应，故机器人移动转化成从一个自由栅格移动到下一个自由栅格，机器人移动的路长对应于机器人爬过的栅格数。栅格法直观且建模相对较容易，因此得到了广泛的应用。

重要参数：栅格大小

栅格大小的选取是影响规划算法性能的一个很重要的因素。栅格较小的话，由栅格地图所表示的环境信息将会非常清晰，但由于需要存储较多的信息，会增大存储开销，同时干扰信号也会随之增加，规划速度会相应降低，实时性得不到保证；反之，由于信息存储量少，抗干扰能力有所增强，规划速随之增快，但环境信息划分会变得较为模糊，不利于有效路径的规划。

4.2.2 局部路径规划

局部路径规划方法主要有人工势场法、模糊逻辑算法、遗传算法和基于神经网络的方法等。人工势场法是比较有代表性的方法。

人工势场法

https://blog.csdn.net/weixin_42301220/article/details/125155505
https://zhuanlan.zhihu.com/p/434095158

人工势场法是一种经典的机器人路径规划算法。该算法将目标和障碍物分别看做对机器人有引力和斥力的物体，在障碍物周围构建障碍物斥力势场，在目标点周围构建引力势场，类似于物理学中的电磁场。
被控对象在这两种势场组成的复合场中受到斥力作用和引力作用，斥力和引力的合力指引着被控对象的运动，搜索无碰的避障路径。

某点 $\mathbf{q}$ 处的势函数 $\mathbf{U}(\mathbf{q})$ 表达为引力势和斥力势之和，势场 $\mathbf{U}$ 的构造如下∶

$$\mathbf{U}(\mathbf{q})={\mathbf{U}}_{\mathbf{att}}(\mathbf{q})+{\mathbf{U}}_{\mathbf{rep}}(\mathbf{q})$$

路径规划问题可以被转化为一个数值优化问题，即，以初始位形${\mathbf{q}}_{\mathbf{c}}$作为起始，寻找势场$\mathbf{U}$的全局最小值。

解决该问题的最简单算法之一是梯度下降法。在这种情况下，势场$\mathbf{U}$的负梯度可被认为是位形空间中作用在机器人上的一个广义力，$\mathbf{F}=-ᐁ\mathbf{U}$，作为质点的机器人受到引力与斥力的作用。

优点： 算法简单，便于实时控制。
缺点： 存在局部最优解，容易陷入局部最小，导致规划失败。

路径规划算法一般包括基于随机采样和梯度下降两种。如果某种算法保证在一定时间内可以找到一个解，则具有完备性；如果求出来的解是最优解，则具有最优性。评价路径的优劣一般和优化目标有关系，通常除了不碰撞之外，还会设置路径长度、平滑性的优化目标，希望最终找到一条路径最短、最平滑、无碰撞的路径。比如说，两点之间，有无数中路径可以到达，但是两点直线连接则是路径最短的一条，如果这条直线还没有和周围障碍碰撞，那么就是最优的。

4.3 轨迹规划

路径规划给出机器人运动路线，是任务层级的运动；轨迹规划生成的是机器人的运动执行指令，是执行层级的运动。机器人的轨迹规划主要包括关节空间（Joint-Space）轨迹规划和操作空间（Cartesian-space）轨迹规划两大类。

4.3.1 Joint-Space

对 Joint-Space 来说进行轨迹规划需要以下步骤：

1. 确定轨迹的起点(Initial Point) - 途径点(Via Point) - 终点(Finial Point)
2. 通过 Inverse Kinematics (IK) 计算出以上所有点的 Joint Angle，即 Joint-space 下的所有点关于时间的位置.
3. 设计一条轨迹将 Joint-space 所有点都平滑的连接起来。
4. 再通过 Forword Kinematics (FK) 算出在这种情况下末端关于世界坐标的曲线。
5. 检查世界坐标下的曲线是否合理。

4.3.2 Cartesian-space

对 Cartesian-space 来说进行轨迹规划需要以下步骤：

1. 确定轨迹的起点(Initial Point) - 途径点(Via Point) - 终点(Finial Point)
2. 设计一条曲线将 Cartesian-space 所有点都平滑的连接起来。
3. 使用 IK 计算出这条曲线在 Joint-space 的曲线。
4. 检查 Joint-space 的曲线是否平滑。

4.3.3 两种方法的对比

Cartesian-space 来做轨迹规划是一种直观的对位姿轨迹的规划。但可能出现机器人的运动速度上的不平滑。
Joint-space 可以保证运动速度的平滑，但是运动位姿的轨迹可能与设计前预想的轨迹有区别，不会出现尖锐的拐角。
一般来说需要根据实际应用的情况来选择使用那种方法来设计轨迹。

4.3.4 轨迹规划算法

这里所说的轨迹规划算法，实际上就是经过多点的平滑曲线拟合算法。如果使用多项式拟合，通常需要使用三次以上的多项式来拟合。线性拟合也可以，如抛物线混合 (Parabolic Blends)。由于曲线需要经过每一个点，所以不能用贝塞尔曲线来设计轨迹。

一、多项式拟合(Polynomials Spline)

三次样条插值(Cubic Spline Interpolation)

二、抛物线混合(Parabolic Blends)

原理就是消除位置转折处的速度突变，用恒定的一个加速度，使速度在转角处逐渐改变，从而获得一个圆滑的转角。