《机器人动力学与控制》第七章——路径规划与避障 7.2 关节空间下使用人工势场法进行路径规划

7.2.0 前言

开始阅读本文之前需要搞清楚本文的两个概念：

1. 工作空间：用xyz和三个姿态角描述机器人末端执行器最终位置和姿态的坐标系空间
2. 关节空间：用各个关节角的大小描述机器人最终位置姿态的坐标系空间

上一节我们并没有具体讲无障碍位形空间的描述方法，那是因为要得到一个完整的无障碍位形空间描述是不切实际的，想象一下一个工作空间里面，怎么描述除掉障碍物后剩下的所有位形空间。因此一般的路径规划做法是搞一个搜索算法，能够在搜索路径的时候一步步地探索无障碍位形空间。这样的一个搜索算法需要一个搜索策略，其中比较著名的一个策略便是“人工势场法”。本节就会介绍这个方法。先讲关节空间下的基本原理，接着我们会发现，在关节空间里面求解势场的梯度是不可行的，因此我们在7.3节介绍如何在工作空间里面建立势场，然后将势场的影响映射到位形空间里面去。

7.2.1 人工势场法的原理

势场法的基本原理如下：关节空间下机器人的一个位形被视为一个质点（这里有点抽象，由于是在关节空间，机器人的一个位形由n个关节参数描述，类似于n维空间里的一个点），该质点受到一个人工势场U的影响。我们要做的是，建立一个势场，该势场的建立规则是吸引点运动到最终目标位置 $q_{final}$ , 同时受到障碍物 $\mathcal{Q}\mathcal{O}$ 的排斥，（障碍物在工作空间里的描述比较好理解，在关节空间是一个n维区域。机器人在工作空间不碰撞到障碍物就等同于n维空间下机器人点不经过n维障碍区域）。如果势场U建立得合适的话，势场将会有一个全局最小值在 $q_{final}$ 处，并且没有局部最小值，当该点成功被吸引到$q_{final}$的时候，那么关节空间下，机器人的末端执行器恰好到达了我们想要的位置和姿态（在工作空间下，机器人同时也到达了目标位置，只不过起点和终点之间的运动我们并不确定，也就是说，机器人在工作空间里的路径不是一条规则的轨迹）。基本原理不复杂，但不幸的是，正如我们下面会讲到的，目前来说还很难建立一个这样的势场。

一般来讲，一个势场包括了一个吸引机器人去到最终位置 $q_{final}$ 的吸引势场以及排斥机器人接近障碍 $\mathcal{Q}\mathcal{O}$ 的排斥势场，用数学表达式表示:

有了这个式子之后，路径规划问题就变成了一个优化问题。即：在U里面找到一个以初始位置 $q_{init}$ 为开端的全局最小势场点，使得机器人在势场的作用下运动到最终位置$q_{final}$。一个解决该问题的最简单的一个算法就是梯度下降法。该算法里面把U的负梯度看成作用在机器人上的一个力：

注意，这个力不是工作空间下面末端执行器真正受到的力。

在本节剩下的部分，我们将会介绍几种经典的吸引势场和排斥势场的选择，以及一个可以用来在场里面进行路径规划的梯度下降算法。

7.2.2 吸引场

吸引场需要具备几个条件。首先，吸引场Uatt应该随着与$q_{final}$距离的增加单调递增。一个最简单的吸引势场就是随着距离线性增加的势场，专业术语称之为圆锥形势阱（Conic Well Potential）。该势场的值（类似于场强的概念）处处都为1，唯独原点处为0。因此该吸引势场有一个不连续点在原点处，这样会导致稳定性问题（怎么理解会导致稳定性的问题？）。我们更加偏好于一个具有连续差分值的势场，并且随着目标位置的接近吸引力慢慢变小。满足这样条件的一个最简单势场就是吸引力随着与 $q_{final}$ 距离增加而平方增长的势场。用ρf（q）表示当前位置q与最终位置 $q_{final}$ 的欧几里得距离：

因此具有二次关系的一个吸引势场可以定义为：

ζ 是一个比例参数，该势场被称为抛物线势阱（Parabolic Well Potential）。我们已知

因此上述吸引场的梯度为：

对于其中式7.6至式7.7的过渡

因此对于抛物线势阱，由吸引势场引起的吸引力是一个方向指向$q_{final}$，大小与到$q_{final}$的距离线性相关的向量。

我们注意到当q逐渐靠近 $q_{final}$ 时，吸引力Fatt（q）收敛于0，但当q逐渐远离于 $q_{final}$ 时，吸引力的增长随着距离的增加是没有界的。如果当初始位置距离 $q_{final}$ 非常远的话，吸引力会非常大。鉴于这个原因，我们选择把圆锥形势阱与抛物线势阱结合来构成一个复合势阱，当距离较大的时候圆锥势阱起作用，距离较小时抛物线势阱起作用，这样构成的复合势阱使得势阱保持连续性的同时也满足了在较远距离时具有合理大小的吸引力。以下是这样一个复合势阱的定义：

此时吸引力为：

这个力不是真正意义上的力，而是一个梯度的度量。

上述定义的势阱在距离边界d处保持了吸引力的连续性，在pf(q) = d 时，都满足：

7.2.3 排斥场

与吸引场类似，排斥场也要满足一些条件。首先，排斥场需要排斥机器人，避免机器人碰撞到障碍物。另外，当机器人与排斥场距离较远时，排斥场对机器人的排斥力或者运动产生的影响要尽可能的小。一种选择是定义一个排斥场，在障碍物边缘的时候排斥力变成无穷大，在距离障碍物到一定距离时排斥力变为0。如果我们定义一个机器人到障碍物的距离p0（大于p0时排斥场不对机器人产生作用），满足该定义的一个排斥场为:

其中 p(q) 是位形空间下机器人距离障碍物的最短距离，n是决定排斥场影响大小的比例系数。如果 $\mathcal{Q}\mathcal{O}$ 只包括一个凸形障碍区域，那么上述排斥场对应排斥力是排斥场的负梯度，为：

并且此时有

其中b是障碍物边界上距离q最近的一个点。如果 $\mathcal{Q}\mathcal{O}$ 不是凸形的，那么p就不是处处可导了，也就直接导致了力向量的不连续性。下图展现了这样的一个情况：

假设有左右两个障碍物，那么机器人在中线左边的时候左边的障碍物占主导作用排斥力朝右，在右边的时候排斥力朝左。当机器人经过中线的时候，就会产生一个力的不连续性。针对这种情况有多种解决方法，其中最简单的一种就是定义斥力场的时候保证各个障碍物产生的场不重合。

7.2.4 梯度下降规划

梯度下降是解决机器人路径规划问题的一个经典的算法。原理很简单，从初始位置开始，机器人朝着梯度下降最快的方向走一个小距离，到达一个新的位置姿态。接着计算该处的场，并寻找此处的梯度下降最快的方向，走下一个一小步。如此往复直到机器人到达终点位置。梯度下降算法可以这么表示：

上述表示中，带有上标i的q并不是指机器人的第i个关节，而是指规划出来的第i个位置点。最终规划结束的结果为：

αi则表示第i个位置点处的步长，它的方向由一个单位长度的最大梯度下降的方向向量决定。αi 的大小的选择也有讲究，它需要小到使得整个规划过程中机器人不碰撞到障碍物，但是太小计算时间也会相应地变长，所以 αi 要在不碰撞到障碍物的前提下上尽可能的大。在运动规划问题里面，αi 的选择通常是基于先验信息临时决定的，或是基于到最近障碍物的距离，亦或是基于到目标位置的距离。我们可以在一些优化问题的文献中找到各种系统的方法来确定 αi 的值。但是事实上想要完美的到达 $q_{final}$ 几乎是不可能的，因此我们把判断运动是否结束的条件从

换成

只是里面的 e 需要根据实际情况定义得足够小。
梯度下降算法的一个问题就是局部最小值的存在，当选取了合适的 αi 值之后，梯度下降算法可以保证定位到一个区域的最小势场点，但是并不能保证这是全局最小势场点还是局部最小势场点。因此也就有可能不能保证我们的机器人运动到最终位置。下一节我们会用一个例子来讲解这个现象。

该路径规划思路的一个主要难点就是如何在关节空间下评估梯度 $\rho$ 以及梯度变化率 $\nabla \rho$ ，一般情况下机器人同时具备旋转自由度和平移自由度，这让我们关节空间下梯度的评估更为困难。我们会在下一节深入探讨这些问题。

参考引用

翻译自《robot dynamics and control》
作者：Mark W Spong，Seth Hutchinson， and M. Vidyasagar