机器人中的轨迹规划

原作者: 邢远收藏邀请

机器人2025本期导读

第一版：

机器人中的轨迹规划第二版：

波士顿轮足机器人之后，四足机器人Minitaur解锁更多“逆天”新技能

智慧机器人先驱Shakey获颁IEEE里程碑殊荣

圣地亚哥传感器路灯：能疏散交通和监测枪声

第三版：

TensoRFlow贡献者黄文坚：解读对比13个深度学习框架后的选择

第四版：

如何用数学追到完美情人？

机器人中的轨迹规划-基于构造空间中的人工势场方法“实际上机器人的轨迹规划问题一直是机器人领域最为困难的问题之一，主要原因在于随着自由度的增加，机器人轨迹规划的计算量指数增加，因而，实际应用中的机器人一般自由度都较少。

轨迹规划主要指在机器人的工作空间中寻找一条合适的路径连接出发点和终点。虽然，看起来比较简单，但是实际上机器人的轨迹规划问题一直是机器人领域最为困难的问题之一，主要原因在于随着自由度的增加，机器人轨迹规划的计算量指数增加，因而，实际应用中的机器人一般自由度都较少。

在本文章，我们将轨迹规划问题视为搜索问题，虽然本文描述的方法并不能保证解决所有机器人的轨迹规划问题，但已被证明在许多应用领域都是有效的。更为重要的是，本文所描述的犯法具有易于实现、计算量小的优点。

轨迹规划方法提供了机器人运动轨迹的一个几何表述方法，而并没有对运动过程的中动力学因素如速度、加速度进行描述。对机器人运动轨迹的动力学描述由机器人的路径规划方法实现。

本文中，我们首先给出构造空间的定义，然后将工作空间中的障碍物映射到构造空间中。然后使用基于人工势场的方法进行轨迹规划。在得到人工势场后，通过梯度下降法求解可以避开所有障碍物的优化路径。但是，使用梯度下降法进行求解时，所得到的结果可能收敛于局部最小点而不是全局最小点。为了解决局部最优问题，通过使用随机运动法摆脱局部最优解。在得到轨迹规划结果后，然后使用多项式拟合的方法产生光滑的路径。

1. 构造空间

在轨迹规划问题中，对于机器人上每一个点的一个确定性的位置描述成为一个构造，所有可能构造的集合称为构造空间。这里，我们使用机器人每个关节变量所构成的向量对机器人的构造进行描述，构造空间记作Q。

对于单旋转副机械臂，构造空间仅仅为机械臂的防线，因而，构造空间,其中表示单位圆。同样，我们也可以认为。这种情况下，我们可以使用一个变量对机械臂的构造进行描述，即关节转角;同样，对于两连杆平面机械臂，其构造空间为,可以使用两个变量对其构造进行表述，即关节转角向量。

虽然我们可以使用关节向量描述机械臂的构造，但是机器人构造的定义远不止如此。对于平面内的移动机器人，我们可以通过在机器人上构造一个坐标系来描述其构造。这是，其构造可以使用三个数来表示，即机器人位置和方向,构造空间可以表示为：。同样，这也只是移动机器人构造空间的表示方法，但是确实是最常用的方法。

在机器人工作空间中，当机器人与障碍物接触时，即发生碰撞。为了对工作空间中的碰撞进行描述，我们需要引入一些额外的定义。这里，我们使用A表示机器人本体，A(q)表示在构造q下机器人所占据的工作空间；表示工作空间中的第i个障碍物；W表示工作空间（如机器人运动的直角坐标空间）。为了使得机器人不与障碍物发生碰撞。我们需要保证机器人处于轨迹上的任何一点的构造q时，机器人均与障碍物不接触。在机器人的工作空间中，所有使得机器人与障碍物接触的机器人构造组成的空间成为障碍构造空间，定义为：

[attach]67327[/attach]

这里，定义：。

所有使得机器人与障碍物不接触的机器人构造组成的空间成为自由空间，定义为：

[attach]67328[/attach]

下图为平面内的移动刚体障碍物空间与自由空间的例子。

其中图(a)中为移动刚体（不可以转动），为障碍物；图(b)中虚线所包围的多边形即为障碍物空间。

[attach]67329[/attach]

对于平面内障碍物空间的计算可以通过上面的例子看出可以通过图示方法绘出。但是，随着自由度的增加，对障碍物空间进行显示表示的计算量将指数增加。因而，我们将研究降低计算量的一些方法构造障碍物空间。

轨迹规划的问题在于寻找一条可以避开所有障碍物的路径使得机器人可以从初始构造达到最终构造。表示为如下：

选着一条从到的路径满足：,。

接下来我们将在构造空间内通过计算一系列离散的构造点进行轨迹规划，然后根据得到离散的构造点拟合光滑连续的运行轨迹。

2. 基于构造空间人工势场的轨迹规划方法

在1中，我们提到，在机器人轨迹规划过程中，建立自由空间显式的表达式往往是不可能的。一个可行的方法是在轨迹规划的过程中，通过一定的搜索算法不断搜寻自由空间，其中最为常用的搜索算法是基于人工势场的方法。

这里，首先我们将讨论在机器人的构造空间中建立人工势场，但由于此时往往无法计算人工势场的梯度，接下来我们将在机器人工作空间中建立人工势场并进行轨迹规划，之后将工作空间中的轨迹映射到构造空间中。

基于人工势场的方法基本思路如下：首先机器人被看作构造空间中的一个点，人工势场U充满整个工作空间。人工势场U的构造方法可以使得机器人被吸引前往终点，远离障碍物空间。如果人工势场构造的合理，那么人工势场将在终点处取得全局极小值。

总的来说，人工势场U如电场一样，包含一个元件吸引机器人，另外一个元件排斥机器人。人工势场如下：

[attach]67330[/attach]

在上述假设下，轨迹规划问题可以看作一个寻找人工势场U全局最小点的优化问题，优化问题的初始值为。解决上述优化问题最简单的方法是梯度下降法，这里，人工势场U的负梯度方向可以看作是作用在机器人上的外力（在构造空间中），如下式所示。

[attach]67331[/attach]

接下来我们将讨论上述人工势场中引力场和斥力场的构造方法。

首先讨论引力场的构造方法，在构造引力场时，需要注意引力场需要随着距离终点距离的增加而增加。因而，引力场最简单的选择方法为其随着距离终点距离线性增加，即锥形引力场。但是，这种情况下，引力场除终点外，其各处梯度均为常数，而在终点处梯度为0，因而，其梯度并不连续，这有可能会导致系统不稳定。我们希望，引力场在处处光滑，同时随着距离终点距离增加而增大。这里定义为构造q与终点的距离，。定义引力场为：

[attach]67332[/attach]

其中为引力场的权重系数，此时引力场为抛物球形。对于构造空间中的任意一点,引力场在该点处的梯度为：

[attach]67333[/attach]

这里，可以看出，此时引力场的梯度处处连续，并且随着距离终点距离的增大而线性增加。此时，引力场的负梯度方向由当前构造指向终点。

需要注意的时，在抛物球形引力场下，引力场梯度随着距离终点距离增大而增大，因而，如果起始点距离终点非常远，那么此时引力将会非常大。因而，我们通常结合锥形引力场与抛物球形引力场结构，当构造q距离终点较远时，采用锥形引力场；当构造q距离终点较近时，采用抛物球形引力场。此时，引力场，定义为：

[attach]67334[/attach]

引力场负梯度为：

[attach]67335[/attach]

接下来我们讨论斥力场的构造方法：

在对斥力场进行构造时，需要保证机器人与障碍物相互排斥，从而使得两者永不接触；同时，当机器人远离障碍物时，障碍物对机器人的斥力应该逐渐减小。因而，构造斥力场最简单的一个方法为使得斥力场在障碍物边界处趋于无穷，在距离障碍物一定距离时趋于0。这里定义为当前构造与障碍物边界的最短距离，那么斥力场可以构造为：

[attach]67336[/attach]

其中，为斥力场作用范围；为斥力场的权重系数。如果障碍物空间为凸空间，那么斥力场对机器人的斥力可以使用斥力场的负梯度表示，即：

[attach]67337[/attach]

如果障碍物空间为凸空间，当前构造q与障碍物最短距离的负梯度可以描述为：

其中，b为障碍物空间上与当前构造q距离最短的点。

如果障碍物空间为非凸空间，那么当前构造q与障碍物最短距离可能并不处处光滑，因而，上述斥力可能并不连续，下图为一个障碍物空间为非凸空间的例子。

[attach]67338[/attach]

这里，障碍物空间包含两个矩形障碍物。因而，对于上图虚线左侧的机器人构造，距离当前构造q的最短距离在CB1上；而当当前构造处于虚线右侧时，距离当前构造的最短距离位于CB2上，因而，机器人所受斥力在虚线处会产生不连续。

对于上述不连续问题解决的最简单的方法即为确保任意两个障碍物产生的斥力场不重复即可。

在得到上述引力场和斥力场的基础上，接下来我们使用梯度下降法求解机器人运行的轨迹。梯度下降法的基本思路为：由初始构造出发，沿着势场的负梯度方向前进一端距离，然后在得到的新的构造处，重复上述步骤，直至达到最终构造点，其计算过程如下：

[attach]67339[/attach]

在上述过程中，表示第i次迭代后的当前构造，最终所规划得到的路径为包含各次迭代结果的构造序列。；上述迭代过程中的系数决定了第i次迭代时的前进步距；需要注意的是，的选择不能过小以至于计算量过大也不可以过大，从而使得系统不稳定。其选择方法可以参考最优化方法。

梯度下降法的一个常见的问题是其可能收敛于局部极小点而不是全局极小点。这也就意味着，在上述轨迹规划方法，最终我们可能找不到一条从初始构造到最终构造的轨迹。下图是一个局部极小点的例子。可以看到，最终结果并没有收敛于。

[attach]67340[/attach]

上述基于人工势场法的轨迹规划方法中最为困难的地方在于与障碍物最短距离以及其负梯度的计算。一般情况下，如果机器人中既有转动，同时又有平动，那么计算过程将更为复杂。

机器人中的轨迹规划-基于工作空间中的人工势场方法在构造空间中定义时，由于机器人可以看作一个质点，因而，定义过程相对简单。但是，在工作空间中，机器人是一个有一定体积的实体而不能使用质点表示，因而，定义过程会更加复杂。

根据上一篇文章《机器人轨迹规划-基于构造空间中的人工势场方法》我们知道，通常情况下，在构造空间，我们很难给出障碍物空间的一个显示描述，同时，也很难计算当前构造距离障碍物的最短距离及其负梯度。实际上，对于一个曲面，曲面外一点到其最短距离很多情况下都不可以显示表达出来。因而，即使我们可以得到障碍物空间的显示描述，也可能无法计算当前构造与障碍物空间的最短距离。

为了解决上述问题，我们将寻求在机器人工作空间W构造空间Q中定义人工势场。由于工作空间仅为低维的空间()，因而，在其中定义人工势场相对于构造空间将会容易得多。

接下来我们首先在工作空间中定义人工势场，人工势场需要能够保证其在重点处取得极小值，并且随着距离障碍物距离的缩短，人工势场增大。然后，我们将在工作空间中得到的人工势场映射到构造空间中，最后在构造空间中应用梯度下降法求解机器人运行轨迹。

工作空间中的人工势场：

与在构造空间中定义人工势场一样，在工作空间中定义人工势场，我们需要使得其在终点处取得极小值，同时，随着距离障碍物距离的缩短而增大。在构造空间中定义时，由于机器人可以看作一个质点，因而，定义过程相对简单。但是，在工作空间中，机器人是一个有一定体积的实体而不能使用质点表示，因而，定义过程会更加复杂。在评估人工势场对一个实体的影响时，需要沿着被评估实体进行积分，因而，计算过程会相当复杂。另外i一种定义方法，是在机器人上选择一些评估点，然后对这些评估点建立人工势场。然后将各个评估点的人工势场叠加得到最终人工势场，此时，考察人工势场中单点的行为将相对容易。

这里，我们使用表示机器人上的评估点，然后，对评估点定义人工势场。对于一个n-自由度的机械臂，我们可以使用DH坐标系的原点或者连杆的质心构成评估点。我们使用表示在构造q下第i个评估点的位置，因而，对于每一个评估点，我们可以定义人工势场为：

[attach]67341[/attach]

上述人工市场为锥形和抛物球形相结合的人工市场，在评估点处达到极小值。如果评估点的选择能够包含机器人所有自由度，当每个评估点均达到其极小值时，机器人也到达其最终构造。

在上述定义基础上，工作空间中评估点所产生的引力为：

[attach]67342[/attach]

在构造斥力场时，同样我们选择一些评估点：，定义第j评估点的斥力场为：

[attach]67343[/attach]

其中，为第j个评估点与障碍物的最短距离，为障碍物空间的作用范围。因而，此时每个评估点所受的斥力为：

[attach]67344[/attach]

其中，表示最短距离在工作空间中的梯度；如果b为障碍物空间中距离当前评估点最近的点，那么,其梯度为：

[attach]67345[/attach]

但是，这里需要注意的是，上述斥力评估点的选择并不能保证机器人和障碍物并不接触。下图是一个例子。在图中，斥力评估点距离障碍物均有一定的距离，因而，此时障碍物对机器人的斥力可能较小，因而，两者可能发生碰撞。