CN108287469A - 基于人工势场引力因子的机械臂最优避障控制方法及装置 - Google Patents

Abstract

本发明涉及一种基于人工势场引力因子的机械臂最优避障控制方法及装置，方法包括以下步骤：1)设置初始化模型位置和引力因子的初始值；2)采用人工势场算法计算机械臂末端所受到的引力和斥力；3)基于初始化模型位置及步骤2)，利用引力和斥力获得机械臂末端路径点集合；4)获取机械臂末端路径点集合中的点与障碍物中心位置的最短距离及机械臂末端终止位置，并基于模型约束条件获得最短避障路径模型描述；5)基于最短避障路径模型描述构建避障路径目标函数，采用最优化方法迭代求解避障路径目标函数的零值，获得机械臂最优路径点的集合。与现有技术相比，本发明具有实时性和鲁棒性好等优点，能够适应障碍物位置和大小的变化，实现最优避障。

Description

基于人工势场引力因子的机械臂最优避障控制方法及装置

技术领域

本发明属于机械臂路径规划方法，具体涉及一种基于人工势场引力因子的机 械臂最优避障控制方法及装置。

背景技术

随着工业机器人的发展，其生产技术逐步提高，成本也相对降低，这让像“人” 一样的仿生机器人(例如机械臂)进入人们生活中的门槛越来越低。比如KUKA 公司的iiwa、Kinova公司的jaco、Rethink robotics公司的Saywer等机械臂的宜人 化、灵巧化，让社会生活、工作等等变得越来越灵活和智能的美好愿望变得更为可 能。这类服务性机械臂慢慢的从一种工器业机人，人与机器人之间使用与被使用， 替代与被替代的方式转变成一种融入人的生产、生活环境当中，能够与人和谐的共 同合作的机器人。因此，如何能够让机械臂安全、便捷的与人类共同合作是值得人 们关注的重要问题，而机械臂安全避障是达到人机共融的基础和研究热点。

人工势场法最早是由Khatib提出的一种局部路径规划方法，其基本思想是将 机器人在环境中运动视为一种机器人在虚拟的人工受力场中运动，障碍物对移动机 器人产生斥力，目标点对移动机器人产生引力，斥力和引力的合力作为控制机器人 向目标点运动的力。由于目标点被设计为合力的全局最小点，因此在合力的作用下， 理论上机器人可以到达目标点，但其存在目标不可达和局部极小值问题。文献“汪 首坤,朱磊,王军政.基于导航势函数法的六自由度机械臂避障路径规划.北京理 工大学学报,2015,35(2):186-191”运用几何法与人工势场法相结合解决传统人工 势场法的局部极小值陷阱问题。运用了逆运动学的方法对机械整体进行路径规划， 然而，此方法需要从8组逆解中进行筛选，效率比较低，实时性较差。文献“王俊 龙,张国良,羊帆,敬斌等.改进人工势场法的机械臂避障路径规划.计算机工程 与应用,2013,49(21):266-270”在人工势场法陷入局部极小值时，通过添加虚拟障 碍物改变整个人工势场解决问题。该方法的实现必须在通过其他的方法得到一组 “合理”的关节角度解，但在实时应用中难以选取“合理”的解。在移动机器人避 障方面如何改进人工势场法的局部性缺点有众多的公开文献，而对于针对以人工势 场引力因子实现能够适应障碍物位置和大小的变化，满足机械臂末端在最优状态下 避障的要求尚无公开的类似文献。因此，设计一种基于人工势场引力因子的机械臂 最优避障方法，实现机械臂对于不同位置和大小的障碍物，具有多条避障路径，如 何获取最优的避障路径具有重要意义。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于人工势 场引力因子的机械臂最优避障控制方法。

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

一种基于人工势场引力因子的机械臂最优避障控制方法，包括以下步骤：

1)设置初始化模型位置和引力因子的初始值；

2)根据步骤1)，采用人工势场算法计算机械臂末端所受到的引力和斥力；

3)基于所述初始化模型位置及步骤2)，利用引力和斥力获得机械臂末端路径 点集合；

4)获取所述机械臂末端路径点集合中的点与障碍物中心位置的最短距离及机 械臂末端终止位置，并基于模型约束条件获得最短避障路径模型描述；

5)基于所述最短避障路径模型描述构建避障路径目标函数，采用最优化方法 迭代求解所述避障路径目标函数的最小零值，获得机械臂最优路径点的集合。

进一步地，所述初始化模型位置λ表示为：

λ＝{ρ₀,X_o(x,y),X_g(x,y),X_t(x,y)}

其中，ρ₀为斥力势场作用范围常数，X_o(x,y)为障碍物中心位置，X_g(x,y)为目标点位置，X_t(x,y)为机械臂末端初始位置。

进一步地，所述引力因子κ_f定义为：

κ_f＝k_a/(k_a+k_b)

其中，k_a为引力势场系数，k_b为斥力势场系数。

进一步地，所述步骤3)中，获得的机械臂末端路径点集合P_traj表示为：

P_traj＝Γ(t,κ_f,λ)

即P_traj与t、κ_f、λ相关，其中，t为机械臂末端运动的时间，κ_f为引力因子，λ为 初始化模型位置，Γ(·)为关系函数。

进一步地，所述避障路径目标函数L(κ_f)具体为：

L(κ_f)＝f²(κ_f)+g²(κ_f)

其中，中间函数f(κ_f)、g(κ_f)由最短避障路径模型描述定义：d_min为机械臂末端路径点集合中的点与障碍物中心位置 的最短距离，为机械臂末端终止位置，r为障碍物外接圆半径，X_g(x,y)为目 标点位置。

本发明还提供一种基于人工势场引力因子的机械臂最优避障控制装置，包括：

初始化模块，用于获取初始化模型位置和引力因子的初始值；

人工势场求解模块，用于根据所述初始化模型位置和引力因子，采用人工势场 算法计算机械臂末端所受到的引力和斥力；

路径求解器，用于根据所述初始化模型位置及人工势场求解模块获得的引力和斥力获得机械臂末端路径点集合；

最短避障路径模型描述模块，用于获取所述机械臂末端路径点集合中的点与障碍物中心位置的最短距离及机械臂末端终止位置，并基于模型约束条件获得最优路 径模型描述；

最短路径求解器，基于所述最短避障路径模型描述构建避障路径目标函数，采 用最优化方法迭代求解所述避障路径目标函数的最小零值，获得机械臂最优路径点 的集合。

进一步地，所述初始化模块中，初始化模型位置λ表示为：

λ＝{ρ₀,X_o(x,y),X_g(x,y),X_t(x,y)}

其中，ρ₀为斥力势场作用范围常数，X_o(x,y)为障碍物中心位置，X_g(x,y)为目标点位置，X_t(x,y)为机械臂末端初始位置。

进一步地，所述初始化模块中，引力因子κ_f定义为：

κ_f＝k_a/(k_a+k_b)

其中，k_a为引力势场系数，k_b为斥力势场系数。

进一步地，所述路径求解器中，获得的机械臂末端路径点集合P_traj表示为：

P_traj＝Γ(t,κ_f,λ)

即P_traj与t、κ_f、λ相关，其中，t为机械臂末端运动的时间，κ_f为引力因子，λ为 初始化模型位置，Γ(·)为关系函数。

进一步地，所述最短路径求解器中，避障路径目标函数L(κ_f)具体为：

L(κ_f)＝f²(κ_f)+g²(κ_f)

其中，中间函数f(κ_f)、g(κ_f)由最短避障路径模型描述定义：d_min为机械臂末端路径点集合中的点与障碍物中心位置 的最短距离，为机械臂末端终止位置，r为障碍物外接圆半径，X_g(x,y)为目标 点位置。

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

第一，本发明所涉及的机械臂最优避障算法简单，无需大量复杂的计算，具有 较好的实时性，避障路径最短，消耗能量最少。

第二，本发明通过构建并求解引力因子κ_f，能够更加直观有效地研究机器人 的最避障路径。

第三，本发明对人工势场法参数进行深入研究，构建引力因子κ_f应用于机械 臂最优避障路径规划问题，构建避障路径目标函数，针对不同障碍物大小和位置进 行最优路径规划，实现机械臂的最优避障路径，具有较好的实时性，能够让机械臂 安全、便捷的低消耗的与人类共同合作。

第四，本发明所涉及的机械臂最优避障方法可以有效提高机械臂的安全性能， 使用寿命等。

第五，本发明所涉及的机械臂最优避障方法路鲁棒性好，避障成功率高。

附图说明

图1为本发明的控制流程图；

图2为工业SCARA平面两自由度机械臂避障模型图；

图3为实施例一障碍物半径为0.4的最优路径特征示意图；

图4为实施例一障碍物半径为0.44的最优路径特征示意图；

图5为实施例二障碍物圆心位置为(0.6,0.54)的最优路径特征示意图；

图6为实施例二障碍物圆心位置为(0.6,0.6)的最优路径特征示意图；

图7为最优避障路径的机械臂关节角度变化情况示意图(r为0.4和0.44)。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范 围不限于下述的实施例。

如图1所示，本发明提供一种基于人工势场引力因子的机械臂最优避障控制方法，包括以下步骤：

1)设置初始化模型位置和引力因子的初始值。初始化模型位置λ表示为：

λ＝{ρ₀,X_o(x,y),X_g(x,y),X_t(x,y)}

其中，ρ₀为斥力势场作用范围常数，X_o(x,y)为障碍物中心位置，X_g(x,y)为目标点位置，X_t(x,y)为机械臂末端初始位置。

所述引力因子κ_f定义为：

κ_f＝k_a/(k_a+k_b)

其中，k_a为引力势场系数，k_b为斥力势场系数。当引力势场系数k_a所占比例越 大，κ_f则越大，机械臂末端避障路径弧度会变小，k_a/k_b无穷大时κ_f极值为1；反 之，机械臂末端避障路径弧度会变大，k_a/k_b无穷小时κ_f极值为0。通过定义该引 力因子κ_f，能够更加直观有效地研究机器人的最优避障路径。

2)根据步骤1)，采用人工势场算法计算机械臂末端所受到的引力F_aat和斥力 F_rep。

3)基于所述初始化模型位置及步骤2)获得的引力和斥力获得机械臂末端路 径点集合P_traj，表示为：

P_traj＝Γ(t,κ_f,λ)

即P_traj与t、κ_f、λ相关，其中，t为机械臂末端运动的时间，κ_f为引力因子，λ为 初始化模型位置，Γ(·)为关系函数。

4)获取所述机械臂末端路径点集合中的点与障碍物中心位置的最短距离及机 械臂末端终止位置，并基于模型约束条件获得最短避障路径模型描述。

最短避障路径模型描述表示为：

其中，d_min为在机械臂末端运动路径点的集合P_traj中与障碍物中心X_o(x,y)的最短距 离：d_min＝Φ_min(κ_f,λ)；为在机械臂末端点的终止位置表示为：当d_min＝r，则表示机械臂末端与障碍物外接圆处于相切的状态，存在唯一解，此 临界状态为机械臂避障的最短路径。约束运动路径最终收敛于目 标点。

5)基于所述最短避障路径模型描述构建避障路径目标函数，采用最优化方法 迭代求解所述避障路径目标函数的最小零值，满足L(κ_f)＝0，获得机械臂最优路径 点的集合以及此时的引力因子最优解此最优解满足机械臂末端在最优状态 下避障的要求。

所述避障路径目标函数L(κ_f)具体为：

L(κ_f)＝f²(κ_f)+g²(κ_f)

其中，中间函数f(κ_f)、g(κ_f)由最短避障路径模型描述定义：d_min为机械臂末端路径点集合中的点与障碍物中心位置 的最短距离，为机械臂末端终止位置，r为障碍物外接圆半径，X_g(x,y)为目 标点位置。

如图1所示，实现上述基于人工势场引力因子的机械臂最优避障控制方法的装置，包括初始化模块、人工势场求解模块、路径求解器、最短避障路径模型模块和 最短避障路径求解器，其中，最短避障路径描述模块包括模型状态求解器和最短避 障路径模型描述。

本实施例以两自由度机械臂为例，但本方法不仅仅局限应用于该类机器人。如 图2所示，为工业SCARA平面两自由度机械臂避障模型。定义障碍物区域外接圆 做简化描述障碍物。机器人在执行运动指令前，根据障碍物和目标位置信息进行在 线计算规划，实现最佳的避障路径运动。

对于任意位置和大小的障碍物，确定斥力势场作用范围常数ρ₀后，机械臂末 端运动路径的变化由引力势场系数k_a、斥力势场系数k_b的比值(k_a/k_b)决定，ρ₀可 等于障碍物位置与目标点位置距离。因此该方法将人工势场法应用到机械臂最优避 障路径规划当中，具有较好实时性，能够适应障碍物位置和大小的变化，满足机械 臂末端在最优状态下避障的要求。

实施例一

第一步，如图3和图4所示，为本发明实施例一障碍物大小变化情况的最优路 径特征示意图，其中，a表示障碍点，b表示目标点，c表示运动路径。确立两个 障碍物半径分别为0.4和0.44，固定障碍物的中心点位置(0.6,0.6)，目标点的中心 点位置(0.9,0.2)，机械臂末端起始中心位置(0.2,0.2)，确定斥力势场作用范围常 数ρ₀等于障碍物与目标点距离0.5。

第二步，通过第一步初始化模型状态模型位置λ后，再给定κ_f的初始值为1。

第三步，再经过人工势场算法计算得到机械臂末端受到的引力F_aat和斥力F_rep。

第四步，利用路径求解器将人工势场算法计算得到机械臂末端受到的引力F_aat、斥力F_rep以及初始化模型位置，求解出机械臂末端路径点的集合P_traj＝Γ(t,κ_f,λ)。

第五步，根据第四步求解出多个机械臂末端路径点的集合P_traj＝Γ(t,κ_f,λ)，最短避障路径模型中的模型状态求解器求解多个d_min,的值。

第六步，结合模型约束条件得到最短避障路径模型的描述以及避障路径的目标函数L(κ_f)＝f²(κ_f)+g²(κ_f)，利用最短 路径求解器，采用最优化方法迭代寻优(本例采用梯度下降法，但最优化方法不限 于该方法)，求解L(κ_f)最小零值，满足L(κ_f)＝0，最终得出机械臂最优路径点的集 合最优路径如图3和图4所示，以及半径为0.4时最优解为半径为0.44 时最优解为

实施例二

第一步，如图5和图6所示，为本发明实施例二障碍物圆心位置变化情况的最 优路径特征示意图示意图，确立两个障碍物中心位置分别为(0.6,0.56)和(0.6,0.6)， 固定障碍物的半径大小r＝0.41，目标点的中心点位置(0.9,0.2)，机械臂末端起始 中心位置(0.2,0.2)。确定斥力势场作用范围常数ρ₀等于障碍物与目标点距离。

第二步，通过第一步初始化模型状态模型位置λ后，再给定κ_f的初始值为1。

第三步，经过人工势场算法计算得到机械臂末端受到的引力F_aat和斥力F_rep。

第四步，利用路径求解器将人工势场算法计算得到机械臂末端受到的引力F_aat、斥力F_rep以及初始化模型位置，求解出机械臂末端路径点的集合P_traj＝Γ(t,κ_f,λ)。

第五步，根据第四步求解出多个机械臂末端路径点的集合P_traj＝Γ(t,κ_f,λ)，最短避障路径模型中的模型状态求解器求解多个d_min,的值。

第六步，结合模型约束条件得到最短避障路径模型的描述以及避障路径的目标函数L(κ_f)＝f²(κ_f)+g²(κ_f)，采用最优 化方法迭代寻优，求解L(κ_f)最小零值，满足L(κ_f)＝0，最终得出机械臂最短路径 点的集合最优路径如图5和图6所示，以及障碍物中心位置为(0.6,0.56)时 最优解为障碍物中心位置为(0.6,0.6)最优解为

实施例一和实施例二，讨论了针对不同圆心位置和半径的障碍物，机械臂末端 最优避障路径。对于不同的末端路径，针对基于工业SCARA机器人的平面模型对 机械臂的关节路径进行实例求解分析，关节路径通过机器人逆运动学计算得到。

选择实施例一中的机械臂末端最优避障路径为例，其中障碍物半径r分别为 0.4和0.44，中心位置为(0.6，0.6)，目标点为(0.9,0.2)，机械臂末端起始位置(0.2,0.2)。 图2所示平面两自由度机械臂模型大臂和小臂长度分别为0.4m和0.35m。设置避 障模型坐标系原点在机械臂坐标系的(-0.5，0.3)，从而使机械臂能够在运动空间 范围内完成避障运动任务。通过控制机械臂的两个关节电机，使得机械臂末端精确 跟踪最优避障路径。

在实施例一中，当障碍物半径为0.4和0.44时，设置斥力势场作用范围常数ρ₀＝0.5等，由基于人工势场引力因子的机械臂最优避障控制方法可以计算出其相对 应的最优引力因子分别为1和0.9075，得到机械臂末端的最优避障路径。

如图3所示，当障碍物半径为r＝0.4时，障碍没有阻挡机械臂末端到目标点的 直线运动。如图4所示，而当r＝0.44时，机械臂末端路径先沿着x轴运动，然后 开始y轴负方向的位移，成弧线避开障碍到达目标点。

通过机器人逆运动学计算得到两种情况的机械臂关节角度变化情况如图7所 示，显示了大臂关节角度q₁和小臂关节角度q₂的变化情况，可以看到虚线为r＝0.4 情况，机械臂小臂关节先增大后减小，大臂关节先减小再增大，从而实现机械臂末 端的直线运动。在机械臂开始运动时，图中两种情况的关节运动基本相同。当r＝0.44 情况下机械臂末端在t∈[0.2,1]时间段内，相比r＝0.4情况机械臂小臂关节增大更多 的角度，而同时大臂减小更多的角度，这主要是因为当r＝0.44情况的机械臂末端 出现弧线路径，使得机械臂需要摆动更多的角度实现避障运动。

本方法在两个实施例的水平两自由度的机械臂中，实验结果表明，当障碍物半 径变大时，最优解逐渐减小，机械臂末端的路径弧度也会变大，保持着避开障 碍物的最短路径，最终停止在目标中心点位置。在机械臂执行运动任务前通过对κ_f求最优解，实现针对于不同半径大小障碍物的最优路径；当障碍物位置向上移动时， 保持斥力势场系数k_b不变，引力势场系数k_a逐渐变大，最优解的值变大，机械 臂末端路径弧度逐渐变小，但仍然能够保持平滑的路径，在最短路径下到达并停止 在目标中心点位置。在实际的机械臂避障实验中，能够通过改变κ_f的值，实现适 应运动障碍物的最优避障路径。

基于人工势场引力因子的机械臂最优避障控制方法将人工势场法应用到机械 臂最优避障路径规划当中，不仅仅适用在水平面两自由度的机械臂中，同样适用于 其他多自由度工业机器人的避障，同时思路也适用于3D空间机器人避障。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员 无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领 域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的 实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (10)

1.一种基于人工势场引力因子的机械臂最优避障控制方法，其特征在于，包括以下步骤：

1)设置初始化模型位置和引力因子的初始值；

2)根据步骤1)，采用人工势场算法计算机械臂末端所受到的引力和斥力；

3)基于所述初始化模型位置及步骤2)，利用引力和斥力获得机械臂末端路径点集合；

4)获取所述机械臂末端路径点集合中的点与障碍物中心位置的最短距离及机械臂末端终止位置，并基于模型约束条件获得最短避障路径模型描述；

5)基于所述最短避障路径模型描述构建避障路径目标函数，采用最优化方法迭代求解所述避障路径目标函数的零值，获得机械臂最优路径点的集合。

2.根据权利要求1所述的基于人工势场引力因子的机械臂最优避障控制方法，其特征在于，所述初始化模型位置λ表示为：

λ＝{ρ₀,X_o(x,y),X_g(x,y),X_t(x,y)}

其中，ρ₀为斥力势场作用范围常数，X_o(x,y)为障碍物中心位置，X_g(x,y)为目标点位置，X_t(x,y)为机械臂末端初始位置。

3.根据权利要求1所述的基于人工势场引力因子的机械臂最优避障控制方法，其特征在于，所述引力因子κ_f定义为：

κ_f＝k_a/(k_a+k_b)

其中，k_a为引力势场系数，k_b为斥力势场系数。

4.根据权利要求1所述的基于人工势场引力因子的机械臂最优避障控制方法，其特征在于，所述步骤3)中，获得的机械臂末端路径点集合P_traj表示为：

P_traj＝Γ(t,κ_f,λ)

即P_traj与t、κ_f、λ相关，其中，t为机械臂末端运动的时间，κ_f为引力因子，λ为初始化模型位置，Γ(·)为关系函数。

5.根据权利要求1所述的基于人工势场引力因子的机械臂最优避障控制方法，其特征在于，所述避障路径目标函数L(κ_f)具体为：

L(κ_f)＝f²(κ_f)+g²(κ_f)

其中，中间函数f(κ_f)、g(κ_f)由最优路径模型描述定义：d_min为机械臂末端路径点集合中的点与障碍物中心位置的最短距离，为机械臂末端终止位置，r为障碍物外接圆半径，X_g(x,y)为目标点位置。

6.一种基于人工势场引力因子的机械臂最优避障控制装置，其特征在于，包括：

初始化模块，用于获取初始化模型位置和引力因子的初始值；

人工势场求解模块，用于根据所述初始化模型位置和引力因子，采用人工势场算法计算机械臂末端所受到的引力和斥力；

路径求解器，用于根据所述初始化模型位置及人工势场求解模块获得的引力和斥力获得机械臂末端路径点集合；

最短避障路径描述模块，用于获取所述机械臂末端路径点集合中的点与障碍物中心位置的最短距离及机械臂末端终止位置，并基于模型约束条件获得最短避障路径模型描述；

最短路径求解器，基于所述最短避障路径模型描述构建避障路径目标函数，采用最优化方法迭代求解所述避障路径目标函数的零值，获得机械臂最优路径点的集合。

7.根据权利要求6所述的基于人工势场引力因子的机械臂最优避障控制装置，其特征在于，所述初始化模块中，初始化模型位置λ表示为：

λ＝{ρ₀,X_o(x,y),X_g(x,y),X_t(x,y)}

其中，ρ₀为斥力势场作用范围常数，X_o(x,y)为障碍物中心位置，X_g(x,y)为目标点位置，X_t(x,y)为机械臂末端初始位置。

8.根据权利要求6所述的基于人工势场引力因子的机械臂最优避障控制装置，其特征在于，所述初始化模块中，引力因子κ_f定义为：

κ_f＝k_a/(k_a+k_b)

其中，k_a为引力势场系数，k_b为斥力势场系数。

9.根据权利要求6所述的基于人工势场引力因子的机械臂最优避障控制装置，其特征在于，所述最短路径求解器中，获得的机械臂末端路径点集合P_traj表示为：

P_traj＝Γ(t,κ_f,λ)

即P_traj与t、κ_f、λ相关，其中，t为机械臂末端运动的时间，κ_f为引力因子，λ为初始化模型位置，Γ(·)为关系函数。

10.根据权利要求6所述的基于人工势场引力因子的机械臂最优避障控制装置，其特征在于，所述最短路径求解器中，避障路径目标函数L(κ_f)具体为：

L(κ_f)＝f²(κ_f)+g²(κ_f)

其中，中间函数f(κ_f)、g(κ_f)由最短避障路径模型描述定义：d_min为机械臂末端路径点集合中的点与障碍物中心位置的最短距离，为机械臂末端终止位置，r为障碍物外接圆半径，X_g(x,y)为目标点位置。