CN108326849A - 一种基于改进人工势场法的多自由度机械臂动态避障路径规划方法 - Google Patents

Abstract

本发明公开一种基于改进人工势场法的多自由度机械臂动态避障路径规划方法，该方法首先利用目标在当前时刻的位置和速度信息在机械臂末端构造吸引速度，利用障碍物当前时刻和上一时刻的位置信息在机械臂上离障碍物距离最近点处构造排斥速度，然后通过笛卡尔空间与机械臂关节空间的映射关系，将吸引速度和排斥速度映射到关节空间进行矢量合成，用于机械臂动态避障规划。在机械臂运动过程中，实时检测机械臂是否陷入局部极小值点，若陷入局部极小值，则添加虚拟障碍物，使机械臂跳出局部极小值，继续运动并追踪动态目标。本发明通过避免笛卡尔空间障碍物到机械臂关节空间的映射，降低了计算复杂度，使人工势场法能够适用于多自由度机械臂。

Description

一种基于改进人工势场法的多自由度机械臂动态避障路径规 划方法

技术领域

本发明涉及多自由度机械臂路径规划领域，尤其涉及一种基于改进人工势场法的多自由度机械臂动态避障路径规划方法。

背景技术

传统工业生产中机械臂主要用于单一的流水线型任务，其运行依赖于操作工人的示教编程，是一种效率低且适应性差的方法。当机械臂工作环境变化时，其无法有效的避开障碍物可能导致严重的安全事故。随着服务型机器人的发展，人们要求机械臂能够在更加复杂的环境下工作，例如人机协作、空间站维修等。这就需要机械臂能根据环境实时规划路径，确保到达目标位姿执行任务，且在运行过程中避开所有动静态障碍物。国内外学者针对移动机器人已经提出了许多有效的路径规划方法，但由于机械臂是复杂的非线性系统，其高自由度及连杆间的耦合等性质增加了规划难度，大多数适用于移动机器人的路径规划方法并不适用于机械臂。

人工势场法是一种高效的局部路径规划方法，其基本思想是在机器人工作空间或执行空间构建虚拟势场，在目标点设置作用于全局的引力场，在障碍物设置作用于局部范围的斥力场，机器人在两场的共同作用下到达目标并避开沿途障碍。人工势场法结构简单、运用方便，在动态避障、动态目标追踪等方面具有明显优势，但主要用于移动机器人，机械臂的多连杆结构使得其应用较为困难，且存在局部极小值问题。

发明内容

本发明的目的在于针对现有技术的不足，提供一种基于改进人工势场法的多自由度机械臂动态避障路径规划方法。

本发明的目的是通过以下技术方案来实现的：

一种基于改进人工势场法的多自由度机械臂动态避障路径规划方法，其特征在于，该方法包括以下步骤：

步骤一：在机械臂末端构造吸引速度；

步骤二：在机械臂上离障碍物距离最近点处构造排斥速度；

步骤三：通过笛卡尔空间与机械臂关节空间的映射关系，将吸引速度和排斥速度映射到关节空间进行矢量合成，用于机械臂动态避障规划；

步骤四：实时检测机械臂是否陷入局部极小值点；

步骤五：若机械臂陷入局部极小值，则添加虚拟障碍物，使机械臂跳出局部极小值，继续运动并追踪动态目标。

进一步地，步骤一中所述的吸引速度由基于目标位置的吸引速度和基于目标速度的吸引速度加权合成，具体如下：

V_sum＝δ_pos*V_pos+δ_vel*V_vel

其中，V_pos为基于目标位置的吸引速度，V_vel为基于目标速度的吸引速度，v_max为提前设定的V_att的最大阈值，δ_pos和δ_vel分别为两种吸引速度的合成系数；

V_pos按如下公式计算：

V_pos＝v_pos*(P_goal-P_tool)/||P_goal-P_tool||

e_pos＝||P_goal-P_tool||

其中，P_goal为目标所在位置，P_tool为机械臂末端所在位置，(P_goal-P_tool)/||P_goal-P_tool||表示基于目标位置的吸引速度的方向为由机械臂末端指向目标，e_pos为目标与机械臂末端之间的位置误差，K_pos和D_pos为控制参数；

V_vel按如下公式计算：

V_vel＝v_vel*(V_goal-V_tool)/||V_goal-V_tool||

e_vel＝||V_goal-V_tool||

其中，V_goal为目标运行速度，V_tool为机械臂末端运行速度，(V_goal-V_tool)/||V_goal-V_tool||表示基于目标速度的吸引速度的方向与目标和机械臂末端速度差方向相同，e_vel为目标与机械臂末端之间的速度误差，K_vel和D_vel为控制参数。

进一步地，步骤二所述排斥速度计算公式为：

其中，

(1)V_rej为当障碍物远离机械臂时的排斥速度，按如下公式计算：

V_rej＝v_rej*(P_obj-P_M)/||P_obj-P_M||

其中，P_obj表示任一时刻障碍物的位置，P_M表示任一时刻机械臂连杆上与障碍物距离最近点的位置，v_rejmax为提前设定的v_rej的最大阈值，D_min为机械臂与障碍物的最近距离，α为形变因子，α＞4，ρ为排斥速度作用范围，ρ＞0；

(2)V′_rej为当障碍物接近机械臂时的排斥速度，按如下公式计算：

V′_rej＝v_rej(m*cosγ+n*sinγ)

其中，

m＝a/||a||

n＝s×m

s＝m×V_rej/||V_rej||

其中，m、n、s为在P_M处构建坐标系的三个轴，a为排斥速度变化率，k时刻a_max表示最大容许排斥速度变化率，β为重构排斥速度前排斥速度与排斥速度变化率的夹角，γ为重构排斥速度后排斥速度与排斥速度变化率的夹角，c为形变因子；

为当障碍物接近机械臂且障碍物运动方向与最近点所在的连杆的运动方向相同时的排斥速度，按如下公式计算：

其中，R(σ)表示旋转矩阵，σ表示旋转角度，σ＜1°，表示最近点所在机械臂连杆坐标系相对于世界坐标系的齐次变换矩阵。

进一步地，步骤三所述将吸引速度和排斥速度映射到关节空间进行矢量合成，计算方法如下：

其中，为吸引速度在机械臂关节空间的映射，Q_rej为排斥速度在机械臂关节空间的映射，计算方法如下：

其中，J^-1（P_tool)表示机械臂末端雅可比矩阵的伪逆，为第i个障碍物计算得到的排斥速度在机械臂关节空间的映射，J^#(P_M)表示机械臂与障碍物最近点P_M处雅可比矩阵的伪逆。

进一步地，步骤四所述实时检测机械臂是否陷入局部极小值点的实现方法如下：

计算k-i时刻到k时刻机械臂关节角的平均值取相邻n个关节角均值求其方差如果方差小于阈值δ，则认为机械臂陷入局部极小值。

进一步地，步骤五所述添加虚拟障碍物使机械臂跳出局部极小值的实现方法如下：

选取所有障碍物中离机械臂最近的障碍物，连接目标点和机械臂上离该最近的障碍物距离最近的点，从该连线上选取一点作为虚拟障碍物中心，该点满足如下关系式：

其中，P_v为选取的虚拟障碍物中心的位置，P_obj表示所有障碍物中离机械臂距离最近的障碍物的位置，P₁表示机械臂上离该最近的障碍物的距离最近点的位置，P_goal表示目标位置。

与现有技术相比，本发明的有益效果如下：

本发明的基于改进人工势场法的多自由度机械臂动态避障路径规划方法，是在传统人工势场法的基础上进行的改进，通过直接在笛卡尔空间构造吸引速度和排斥速度，避免了笛卡尔空间障碍物到机械臂关节空间的映射，降低了计算复杂度，使人工势场法能够适用于多自由度机械臂。在吸引速度中引入基于目标位置和速度的吸引速度，使机械臂能够追踪动态目标。对排斥速度进行重构，对于运动速度比它快的障碍物，机械臂也能够避开。此外，本发明还可以解决局部极小值问题。

附图说明

图1是本发明的基于改进人工势场法的多自由度机械臂动态避障路径规划方法的流程图；

图2是吸引速度构造示意图；

图3是排斥速度构造示意图；

图4是虚拟障碍物模型构造示意图；

图5为机械臂与障碍物最近距离随时间变化的曲线图；

图6为机械臂末端与目标距离随时间变化的曲线图。

具体实施方式

下面根据附图和优选实施例详细描述本发明，本发明的目的和效果将变得更加明白，以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明基于改进人工势场法的多自由度机械臂动态避障路径规划方法，用于多自由度机械臂在动态环境下的路径规划。首先利用目标在当前时刻的位置和速度信息在机械臂末端构造吸引速度，利用障碍物当前时刻和上一时刻的位置信息在机械臂上离障碍物距离最近点处构造排斥速度，然后通过笛卡尔空间与机械臂关节空间的映射关系，将吸引速度和排斥速度映射到关节空间进行矢量合成，用于机械臂动态避障规划。在机械臂运动过程中，实时检测机械臂是否陷入局部极小值点，若陷入局部极小值，则添加虚拟障碍物，使机械臂跳出局部极小值，继续运动并追踪动态目标。整个系统流程图如图1所示。

步骤一：在机械臂末端构造吸引速度。

如图2所示为吸引速度构造示意图，目标在当前时刻的位置为P_goal，速度为V_goal，机械臂末端在当前时刻的位置为P_tool，速度为V_tool。定义基于目标位置的吸引速度为：

V_pos＝v_pos*(P_goal-P_tool)/||P_goal-P_tool||

e_pos＝||P_goal-P_tool||

其中，(P_goal-P_tool)/||P_goal-P_tool||表示基于目标位置吸引速度的方向由机械臂末端指向目标，e_pos为目标与机械臂末端之间的位置误差，K_pos和D_pos为控制参数。

定义基于目标速度的吸引速度为：

V_vel＝v_vel*(V_goal-V_tool)/||V_goal-V_tool||

e_vel＝||V_goal-V_tool||

其中，(V_goal-V_tool)/||V_goal-V_tool||表示基于目标速度吸引速度的方向与目标和机械臂末端速度差方向相同，e_vel为目标与机械臂末端之间的速度误差，K_vel和D_vel为控制参数。

则由基于目标位置的吸引速度和基于目标速度的吸引速度加权合成的吸引速度为：

V_sum＝δ_pos*V_posδ_vel*V_vel

其中，v_max为提前设定的V_att的最大阈值，δ_pos和δ_vel分别为两种吸引速度的合成系数。

步骤二：在机械臂上离障碍物距离最近点处构造排斥速度。

如图3所示为排斥速度构造示意图，障碍物在k时刻和k-1时刻的位置分别为和k时刻和k-1时刻机械臂上离障碍物距离最近点位置分别为和k时刻和k-1时刻最近点处的排斥速度和计算公式如下：

V_rej＝v_rej*(P_obj-P_M)/||P_obj-P_M|| (4)

其中，P_obj和P_M表示任一时刻障碍物和最近点的位置，v_rejmax为提前设定的v_rej的最大阈值，D_min为机械臂与障碍物最近距离，α为形变因子，α>4，ρ为排斥速度作用范围，ρ>0。

k时刻排斥速度的变化率为：

则可计算k时刻排斥速度与排斥速度变化率的夹角为：

当β≥π/2时表示障碍物远离机械臂，此时使用式(4)计算排斥速度；当0＜β＜π/2时表示障碍物接近机械臂，此时需要重新构造排斥速度。在处构建坐标系M-mns，其中m＝a^k/||a^k||、n＝s×m，定义重构后的排斥速度为：

其中，a_max表示最大容许排斥速度变化率，γ为重构后排斥速度与排斥速度变化率的夹角，为k时刻的v_rej，由式(4)计算得到。

当β＝0时，表示障碍物接近机械臂且障碍物运动方向与最近点所在的连杆的运动方向相同，此时令排斥速度绕最近点所在机械臂连杆坐标系的Z轴旋转，计算方法如下：

其中，R(σ)表示旋转矩阵，σ表示旋转小角度，表示最近点所在机械臂连杆坐标系相对于世界坐标系的齐次变换矩阵，V_rej可由式(4)计算得到。综上可得排斥速度计算公式为：

步骤三：通过笛卡尔空间与机械臂关节空间的映射关系，将吸引速度和排斥速度映射到关节空间进行矢量合成，用于机械臂动态避障规划。

笛卡尔空间与机械臂关节空间的映射由机械臂雅可比矩阵实现，吸引速度在机械臂关节空间的映射为：

其中，J^-1(P_tool)表示机械臂末端雅可比矩阵的伪逆，V_att由步骤一中的式(3)计算得到。排斥速度在机械臂关节空间的映射为：

其中，J^＃(P_M)表示机械臂与障碍物最近点P_M处雅可比矩阵的伪逆，V_rej由步骤二中的式(9)计算得到。对于多个障碍物的情况，可将每个障碍物计算得到的排斥速度映射到机械臂关节空间后矢量合成：

其中表示由第i个障碍物计算得到的排斥速度在机械臂关节空间的映射。则吸引速度和排斥速度在关节空间的矢量合成为：

步骤四：实时检测机械臂是否陷入局部极小值点。

计算k-i时刻到k时刻机械臂关节角的平均值取相邻n个关节角均值求其方差如果方差小于阈值δ，则认为机械臂陷入局部极小值。

步骤五：若机械臂陷入局部极小值，则添加虚拟障碍物，使机械臂跳出局部极小值，继续运动并追踪动态目标。

如图4所示为构造虚拟障碍物模型，Link表示机械臂连杆，P₁和P₂分别表示机械臂上离障碍物1和障碍物2距离最近的点，且障碍物1在所有障碍物中离机械臂最近。连接最近点P₁与目标点P_obj，从该连线上选取一点P_v作为虚拟障碍物中心，该点满足如下关系式：

其中，P_goal表示目标位置，P_obj表示所有障碍物中离机械臂距离最近的障碍物。

图5和图6给出了本发明的方法的运行结果，给定条件为：两个球形动态障碍物，位置分别为(-0.45，-0.30，0.36)和(0.20，-0.725，0.35)，运动速度分别为0.24m/s和0.16m/s，运动方向分别为(1，0，0)和(-1，1，0)；一个球形静态障碍物，位置为(-0.16，-0.2，0.35)。障碍物半径都为0.02m。目标初始位置为(-0.15，-0.40，0.27)，以0.04m/s的速度向(1，0.1，0)方向运动。一个七自由度机械臂，总长0.94m，初始位姿为(0，0，0，π/2，0，π/2，0)，初始运动速度为(0，0，0，0，0，0，0)，末端初始位置为(-0.32，0，0.34)，末端最大可运行速度为0.1m/s，则机械臂与障碍物最近距离随时间的变化曲线如图5所示，机械臂末端与目标距离随时间的变化曲线如图6所示。从结果可以看出，本发明的方法能够很好实现机械臂避开环境中动静态障碍物并追踪动态目标的功能，即使动态障碍物速度高于自身。

Claims (6)

1.一种基于改进人工势场法的多自由度机械臂动态避障路径规划方法，其特征在于，该方法包括以下步骤：

步骤一：在机械臂末端构造吸引速度；

步骤二：在机械臂上离障碍物距离最近点处构造排斥速度；

步骤三：通过笛卡尔空间与机械臂关节空间的映射关系，将吸引速度和排斥速度映射到关节空间进行矢量合成，用于机械臂动态避障规划；

步骤四：实时检测机械臂是否陷入局部极小值点；

步骤五：若机械臂陷入局部极小值，则添加虚拟障碍物，使机械臂跳出局部极小值，继续运动并追踪动态目标。

2.根据权利要求1所述的方法，其特征在于，步骤一中所述的吸引速度由基于目标位置的吸引速度和基于目标速度的吸引速度加权合成，具体如下：

V_sum＝δ_pos*V_pos+δ_vel*V_vel

其中，V_pos为基于目标位置的吸引速度，V_vel为基于目标速度的吸引速度，v_max为提前设定的V_att的最大阈值，δ_pos和δ_vel分别为两种吸引速度的合成系数。

V_pos按如下公式计算：

V_pos＝v_pos*(P_goal-P_tool)/||P_goal-P_tool||

e_pos＝||P_goal-P_tool||

其中，P_goal为目标所在位置，P_tool为机械臂末端所在位置，(P_goal-P_tool)/||P_goal-P_tool||表示基于目标位置的吸引速度的方向为由机械臂末端指向目标，e_pos为目标与机械臂末端之间的位置误差，K_pos和D_pos为控制参数。

V_vel按如下公式计算：

V_vel＝v_vel*(V_goal-V_tool)/||V_goal-V_tool||

e_vel＝||V_goal-V_tool||

其中，V_goal为目标运行速度，V_tool为机械臂末端运行速度，(V_goal-V_tool)/||V_goal-V_tool||表示基于目标速度的吸引速度的方向与目标和机械臂末端速度差方向相同，e_vel为目标与机械臂末端之间的速度误差，K_vel和D_vel为控制参数。

3.根据权利要求1所述的方法，其特征在于，步骤二所述排斥速度计算公式为：

其中，

(1)V_rej为当障碍物远离机械臂时的排斥速度，按如下公式计算：

V_rej＝v_rej*(Po_bj-P_M)/||P_obj-P_M||

其中，P_obj表示任一时刻障碍物的位置，P_M表示任一时刻机械臂连杆上与障碍物距离最近点的位置，v_{rej max}为提前设定的v_rej的最大阈值，D_min为机械臂与障碍物的最近距离，α为形变因子，α＞4，ρ为排斥速度作用范围，ρ＞0；

(2)V′_rej为当障碍物接近机械臂时的排斥速度，按如下公式计算：

V′_rej＝v_rej(m*cosγ+n*sinγ)

其中，

m＝a/||a||

n＝s×m

s＝m×V_rej/||V_rej||

其中，m、n、s为在P_M处构建坐标系的三个轴，a为排斥速度变化率，k时刻a_max表示最大容许排斥速度变化率，β为重构排斥速度前排斥速度与排斥速度变化率的夹角，γ为重构排斥速度后排斥速度与排斥速度变化率的夹角，c为形变因子；

为当障碍物接近机械臂且障碍物运动方向与最近点所在的连杆的运动方向相同时的排斥速度，按如下公式计算：

其中，R(σ)表示旋转矩阵，σ表示旋转角度，σ＜1°，表示最近点所在机械臂连杆坐标系相对于世界坐标系的齐次变换矩阵。

4.根据权利要求1和2所述的方法，其特征在于，步骤三所述将吸引速度和排斥速度映射到关节空间进行矢量合成，计算方法如下：

其中，为吸引速度在机械臂关节空间的映射，Q_rej为排斥速度在机械臂关节空间的映射，计算方法如下：

其中，J^-1(P_tool)表示机械臂末端雅可比矩阵的伪逆，为第i个障碍物计算得到的排斥速度在机械臂关节空间的映射，J^#(P_M)表示机械臂与障碍物最近点P_M处雅可比矩阵的伪逆。

5.根据权利要求1所述的方法，其特征在于，步骤四所述实时检测机械臂是否陷入局部极小值点的实现方法如下：

计算k-i时刻到k时刻机械臂关节角的平均值取相邻n个关节角均值求其方差如果方差小于阈值δ，则认为机械臂陷入局部极小值。

6.根据权利要求1所述的方法，其特征在于，步骤五所述添加虚拟障碍物使机械臂跳出局部极小值的实现方法如下：

选取所有障碍物中离机械臂最近的障碍物，连接目标点和机械臂上离该最近的障碍物距离最近的点，从该连线上选取一点作为虚拟障碍物中心，该点满足如下关系式：

其中，P_v为选取的虚拟障碍物中心的位置，P_obj表示所有障碍物中离机械臂距离最近的障碍物的位置，P₁表示机械臂上离该最近的障碍物的距离最近点的位置，P_goal表示目标位置。