机器人运动规划--基于《MODERN ROBOTICS》

机器人运动规划要解决的问题是，找到一种让机器人从初始状态运动到目标状态的运动方式，同时要能避开环境中的障碍，以及满足其他限制条件，如关节角度限制或力矩限制等。

一、运动规划概述

运动规划有一个重要的概念是构型空间，即C-space。C-space中的任意一点都与机器人构型唯一对应。free C-space表示机器人不会碰到任何障碍物，以及满足各种关节限制的自由运动构型空间，简写成：

q表示机器人构型，是一个n维向量。C表示C-space，即：

u表示机器人的控制量，是一个m维向量，即：

例如对于一个典型的6自由度机械臂来说，m=n。然而并不一定都是m=n的情况，很多情况下是m 控制量u也可以是位置、速度或力矩，根据实际情况决定。

根据上述定义，运动规划问题的一个相对广泛的定义如下：

在本章的讲述中有两个假设：
1.使用了闭环控制器，保证期望的运动都能被很好的跟随，运动的误差不在本章的考虑范围之内。
2.在运动规划过程中，机器人和环境的几何模型都能精确已知，从而可以用来估计free C-space。（在实际情况下，这一步是很难做到的）

关于运动规划，有几点需要注意：
1.路径规划和运动规划的区别
路径规划是运动规划的子问题。路径规划是纯粹的几何问题，不会考虑动力学、运动持续时间、运动的各种限制条件或控制输入等。
2.控制输入
对于m
3.在线和离线
如果环境是静态的，那么离线规划器就已足够。如果环境经常变化，例如障碍物经常出现和消失，那么需要在线规划快速得到结果。
4.最优或者满足度
可以使代价函数最小的到达目标状态，例如可以设置如下代价函数：

如果让L=1，那么就是一个时间最优的运动；如果让：

那么就是一个控制量最小的运动。
5.精确还是近似
有时候要精确运动到目标状态很难，可以设置一个近似值：

使得其在一定误差范围内接近目标状态：

6.有无障碍物
单纯是因为m

运动规划器可以根据如下性质进行区分：
1.多查询和单查询规划
当环境不变，要求机器人解决一系列不同的运动规划问题时，表示这是一个多查询规划。这种情况下，非常值得花时间建立一个用于精确描述free C-space的数据结构，从而可以在同一个环境中解决多个不同的运动规划问题。
而单查询规划则是每次都在不同的环境中解决同样的规划问题。
2.“随时”规划
“随时”规划的意思是当找到第一个解之后，规划器并不停止，而是继续寻找更优解。规划器可以在任意时刻停止，例如指定的时间到达，或者最优解已经找到。
3.完整性
如果一个运动规划器能保证在有限的时间内，找到解（如果解存在），报告失败（如果解不存在），那么运动规划器是完整的。
如果一个运动规划器在解决问题的离散化表示情况下保证能找到解（如果解存在），那么运动规划器是分辨率完整的。
如果一个运动规划器当规划时间趋近于无穷时，找到解（如果解存在）的概率趋近于1，那么运动规划器是概率完整的。
4.计算复杂度
计算复杂度表示的是一个运动规划器需要执行的时间以及需要的内存大小。这些可以由规划问题的具体描述来度量，例如C-space的维度，或表示机器人和障碍物的顶点数等。计算复杂度可以用规划器的平均情况或最复杂的情况表示。

运动规划方法概述：
1.完整方法
这样的方法重点在于精确完整的表示出free C-space的几何拓扑，确保完整性，除了一些简单或低自由度的问题，绝大多数free C-space在数学上或计算上难以推倒。
2.网格方法
这样的方法将free C-space离散成网格，在网格中搜索一条从开始到目标的运动路径。
这样的方法比较容易实现，也能返回优化的解，但是随着构型空间维度的增长，需要的存储空间和搜索时间会呈指数增长，这就限制了该方法只能用于低维问题。
3.采样方法
通常的采样方法需要一个随机或确定性函数从C-space中选取样本，一个函数去判断样本是否处于free C-space中，一个函数用于确定前一个可用空间样本的最接近值，以及一个本地规划器，尝试连接或移动到上一个样本中的新样本。
这种方法比较容易实现，是概率完整的规划器，甚至可以解决高自由度规划问题，但是求解出来的并不是最优解，而且很难确定计算复杂度。
4.虚拟势场法
虚拟势场在机器人上产生力，将其拉向目标，并将其推离障碍物。
这种方法比较容易实现，甚至可以解决高自由度的运动规划问题，能够快速评估，通常允许在线实现。缺点是势函数的局部极小，即机器人可能会陷入吸引力和排斥力相互抵消的状态，但机器人并不处于目标状态。
5.非线性优化
运动规划问题可以转换成非线性优化问题，只需要将路径或控制量表示成有限数量的设计参数，例如多项式或傅里叶级数的系数。问题将会转变成求解这些设计参数，使得代价函数最小而且满足控制、障碍和目标的要求。
虽然这些方法可以产生近似最优解，但它们需要对解进行初始猜测。由于目标函数和可行解空间一般不是凸的，优化过程可能会远离可行的解决方案，更不用说最优的解决方案了。
6.平滑化
通常由规划器求解出的运动不平滑，通常需要在运动规划器求解出结果后进行平滑化处理。

近年来的主要趋势是采用易于实现且能处理高维问题的抽样方法。

二、运动规划基础

2.1 构型空间障碍物
构型空间与机械臂的关节空间、笛卡尔空间、任务空间都不相同。为了更容易理解构型空间的概念，以一个平面连杆为例，如下图所示。机器人的构型表示成q:

在机器人的工作空间存在A, B, C三个障碍物，机器人的C-space由一部分平面表示，如最右边的图，表示的范围如下：

实际上机器人的C-space是一个圆环，因为矩形边界：

与边界：

是连接在一起的，而同样边界

与边界

也是连接在一起的。这里为了直观，将圆环铺平看起来像一个矩形。

在上图的C-space中可以看出，A, B, C三个障碍物被表示成了C-obstacles. C-obstacles的内部就对应于机器人工作空间中的障碍物。
图中，分别在工作空间和C-space表示了一条从start到end的路径。

上图a中，圆形代表一个圆形移动机器人，三角形代表障碍物。图b表示C-space，黑实线以外的部分就是free C-space。

同理，上图a中三角形代表只能平移的移动机器人，长方形代表障碍物。图b是其C-space，黑色实线外的部分是free C-space。

如果将上述三角形机器人改成可旋转机器人，那么其C-space如上图。可见当自由度增加时，C-space的描述将会变得非常复杂。

2.2 与障碍物的距离及碰撞检测
首先要在C-space里定义距离以及碰撞。
给定一个C-obstacle B 和一个机器人构型q，定义机器人和障碍物的距离为：

则碰撞的定义如下：

距离检测算法就是要确定：

比较流行的距离检测算法叫做：GJK(Gilbert-Johnson-Keerthi)。该算法能有效计算用三角形网格表示的两个凸体之间的距离。
任何机器人或障碍物都可以看作多个凸体的并集。将此算法拓展用于机器人、图形和游戏物理引擎的许多距离测量算法和碰撞检测例程中。

2.3 图和树
主要用于表示C-space，这里不做详细分析。

2.4 图搜索
当free C-space被表示成图，那么就可以用图搜索的方法来找到最优路径。

三、完整路径规划器

这种方法需要将free C-space精确的表示出来，在数学和计算上非常复杂，实际应用中不可能实现。
如下图是一个简单的例子，可以看一下这种方法是如何实现的。

a. 方形机器人（参考点在图中画出）的起始和目标位置如图，在环境中有三角形和矩形两个障碍物。
b. 画出C-space。
c. 连接C-free中的可见路线图。
d. 加入起始点和目标点的路线图。
e. 在所有的路线图中搜寻最短的一条。
f. 机器人沿着这条路径运动。

四、网格法

五、采样法

六、虚拟势场法

七、非线性优化

八、平滑化