CN115026816A - 一种基于虚拟力的机械臂末端避障方法 - Google Patents

Abstract

本发明公开了一种基于虚拟力的机械臂末端避障方法，属于机械臂运动规划技术领域。本发明的基于虚拟力的机械臂末端避障方法，利用伪距离的概念来构造防碰撞约束的方法，每个障碍都包含一个超二次曲面的解析表达式，更定性地表达了障碍物与机械手之间的约束条件，简化了仿真中繁琐的计算，基于虚拟力无碰撞约束的公式不仅简单而且容易实时更新机械臂避障碍运动。在末端避障问题求解中，改变传统方法只考虑距离的缺点，通过障碍物和机械臂末端之间不同的速度、距离和移动方向，动态排斥场可以产生平滑的虚拟力斥力，给末端执行器增加了一个避障速度，解决障碍物出现在末端期望轨迹上的问题。

Description

一种基于虚拟力的机械臂末端避障方法

技术领域

本发明涉及机械臂运动规划技术领域，更具体地说，涉及一种基于虚拟力的机械臂末端避障方法。

背景技术

如今，机械臂的轨迹规划是机器人行业一个热点的课题，在工业生产和工业制造中已经得到广泛的运用。轨迹规划的主要任务是在初始位置和最终位置之间找到一个最优的无碰撞配置。如果机械臂在非结构化环境工作过程中与周围物体发生碰撞，就会影响任务的完成质量，甚至会造成一些安全性的问题，因此避障方法已经成为机器人工作中一项重要的技能。引入决策力自适应对预先规划的轨迹进行实时过滤，保证机器人运动的平滑性和稳定性。由于在实际中，障碍物模型的复杂程度是不同的，用欧式距离表达也更加繁琐，且无法统计表达，这将导致末端轨迹跟踪精度变差。

机械臂轨迹规划主要任务为末端轨迹跟踪和避开障碍物，传统避障方法通过自运动实现关节处避障，当障碍物出现在末端期望轨迹上，轨迹跟踪任务和避障任务就会发生冲突，国内外诸多学者提出的主从任务转换的闭环控制方法，能够有效的解决此类问题，但是随着多个障碍物的出现，机械臂和障碍物之间的最小欧式距离很难检测，计算繁琐复杂，导致机械臂出现运动不连续且误差精度低的问题。

发明内容

1.要解决的技术问题

本申请旨在提供一种基于虚拟力的机械臂末端避障方法，其至少在一个方面比背景技术中说明的现有技术有利。

2.技术方案

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

一种基于虚拟力的机械臂末端避障方法，步骤为：

S1、建立机械臂运动学模型：采用标准的D-H法建立机械臂运动学模型，通过机械臂关节坐标系相对于基坐标系的齐次变换矩阵进行坐标变换：

式(1)中，i(i＝1,2,…,n)为机械臂关节序号，为了简化表达，将cosθ_i，sinθ_i，cosα_i-1，sinα_i-1缩写为cθ_i，sθ_i，cα_i-1，sα_i-1；Rot(x_i-1,α_i-1)为绕x_i-1轴转α_i-1角度变换矩阵，Trans(x_i-1,a_i-1)为沿x_i-1轴平移α_i-1的变换矩阵，Rot(z_i,θ_i)为绕z_i轴转θ_i角度变换矩阵，Trans(z_i,d_i)为绕z_i轴平移d_i的变换矩阵；机械臂末端相对于基座标系的变换矩阵通过上式各杆件的变换矩阵

得到：

S2、建立机械臂-障碍物模型：以一种简单而原始的方式统一了各种障碍的建模，取代了传统的欧式距离，计算机械臂工作空间中任意一点q_c(x,y,z)在障碍物坐标系o_obs-x_obsy_obsz_obs下的位置，用于判别机械臂连杆与障碍物伪距离大小；伪距离用来描述机械臂与空间障碍物位置关系，基本思想是通过非线性函数方程建立机械臂与障碍物之间的距离情况。

S3、计算伪距离最小值：采用向量方法来表达机械臂和障碍物之间的最小伪距离值，以清晰的表达障碍物和机械臂间的约束条件；

S4、改进虚拟斥力函数：考虑到末端执行器的速度和运动方向，改进虚拟斥力函数，并求出负梯度函数；将步骤S3中求出的最小伪距离值带入到改进虚拟斥力函数，求出机械臂末端和障碍物之间虚拟斥力，对机械臂末端轨迹进行规划；

S5、利用算法实现末端避障任务：将步骤S4中的求出的虚拟斥力带入到机械臂末端避障方法中，实现机械臂末端避障。

进一步的，步骤S2中建立机械臂-障碍物模型，利用伪距离的概念来构造防碰撞约束的方法，每个障碍都包含一个超二次曲面的解析表达式，更定性地表达了障碍物与机械手之间的约束条件，以障碍物几何中心为原点建立坐标系，空间点到障碍物球体的超曲面伪距离解析方程为:

式中,

为障碍物坐标空间中伪距离,q_c(x,y,z)为空间中任意一点坐标位置,(x₀,y₀,z₀)为超二次曲面中心点。R_s＝r_obs+r_i，为预设障碍物安全距离，r_obs为障碍物包络体半径，r_i为模拟机械臂厚度半径。

更进一步的，步骤S2中别机械臂连杆与障碍物伪距离大小，任务空间中任意一点q_c(x,y,z)，计算出相应的伪距离，为了满足避障过程安全的进行，避障过程中最小伪距离S_p,min(q_c)始终大于障碍物包络物的预设阈值d_pm，

机械臂处于安全状态。

更进一步的，步骤S3中向量方法表示为：S_p(x,y,z)＝X^TQX+B^TX+C

其中，X＝[x,y,z]^T，Q＝diag[1/R_s ²,1/R_s ²,1/R_s ²]，B＝[-2_x0/R_s ²,-2y₀/R_s ²,-2z₀/R_s ²]^T，Q、B、C均为常量。

更进一步的，步骤S4中采用虚拟引力斥力对机械臂末端轨迹进行规划，即障碍物包络体与末端执行器之间的产生一种新颖的虚拟排斥力，对末端执行器轨迹进行修正，使其在完成障碍物出现在期望轨迹上的情境。

更进一步的，步骤S4中斥力函数为:

其中,F_dyn(x,v)为由速度和位置产生的虚拟力，S_p(x)为机械臂末端和障碍物之间的伪距离，β是一个常数，以调整速度矢量角θ的影响，θ表示当前速度矢量ν和末端执行器位置q_c(相对于障碍物的位置)之间的角度。

更进一步的，步骤S4中，基于公式

计算，当θ在

之间时，表示机器人正在接近障碍物，动态斥力场开始工作；当θ值在

以内时，机器人远离障碍物，动态斥力场不起作用。

更进一步的，步骤S5中，基于步骤S4中产生的虚拟斥力对机械臂末端速度

进行修正，对传统的避障算法修正后的运动学方程：

其中，

为末端速度矢量，雅可比矩阵J为机械臂关节角速度向量

在

的线性映射(

n，m为关节空间和操作空间的维数)，J₀为障碍物在连杆上最小欧氏距离投影点对应的雅可比矩阵，N＝I-J⁺J，I∈R^n×n为单位矩阵，J⁺表示对矩阵求伪逆，J⁺＝J^T(JJ^T)^-1；

为上述虚拟斥力F_dyn(x,v)产生的速度调整量。

更进一步的，步骤S5当障碍物出现在末端执行器期望轨迹上时，即最近距离点为机械臂末端执行器位置，改变末端运动速度轨迹，末端执行器不断向障碍物靠近，最小伪距离的减小，同时θ在

时处于虚拟斥力不断增大，产生较大的速度进行修正，作用在末端执行器使其避开期望轨迹上的障碍物。

3.有益效果

相比于现有技术，本发明的优点在于：

(1)本发明的基于虚拟力的机械臂末端避障方法，通过向量的方法获取机械臂连杆上离障碍物最近的点及最小伪距离值，根据该距离值大小与预设阈值比较，解决障碍物出现在期望轨迹时无法完成避障的问题，同时完成避障后迅速恢复到期望轨迹，实现轨迹跟踪任务，具有十分重要的现实意义；

(2)本发明的基于虚拟力的机械臂末端避障方法，利用伪距离的概念来构造防碰撞约束的方法，每个障碍都包含一个超二次曲面的解析表达式，更定性地表达了障碍物与机械手之间的约束条件，简化了仿真中繁琐的计算，基于虚拟力无碰撞约束的公式不仅简单而且容易实时更新机械臂避障碍运动。

(3)本发明的基于虚拟力的机械臂末端避障方法，在末端避障问题求解中，改变传统方法只考虑距离的缺点，通过障碍物和机械臂末端之间不同的速度、距离和移动方向，动态排斥场可以产生平滑的虚拟力斥力，给末端执行器增加了一个避障速度，解决障碍物出现在末端期望轨迹上的问题，同时保证机器人通过障碍物后恢复到原来的运动状态；

(4)本发明的基于虚拟力的机械臂末端避障方法，仿真结果表明，当包络体障碍物突然出现在末端执行器期望任务轨迹上时，末端执行器与障碍物最近距离在0.02m左右时，末端执行器能够平稳的避开障碍物并迅速恢复到期望轨，同时保证机械臂可以准确到达目标位置，验证了该避障方法能够实现末端避障。

附图说明

图1为本发明的避障方法流程图。

图2为7-DOF机械臂运动学模型图。

图3为机械臂最近点与障碍物球面示意图。

图4为末端斥力角度示意图。

图5为本发明方法的机械臂连杆避障过程示意图。

图6为避障过程中最小伪距离变化示意图。

图7为机械臂末端轨迹误差示意图。

图8为避障过程虚拟力变化大小。

图9为机械臂各个关节角度变化示意图。

具体实施方式

为进一步了解本发明的内容，结合附图和实施例对本发明作详细描述。

实施例

本实施例的基于虚拟力的机械臂末端避障方法，如图1所示，

S1、建立机械臂运动学模型：采用标准的D-H法建立机械臂运动学模型，通过机械臂关节坐标系相对于基坐标系的齐次变换矩阵进行坐标变换：

式(1)中，i(i＝1,2,…,n)为机械臂关节序号，为了简化表达，将cosθ_i，sinθ_i，cosα_i-1，sinα_i-1缩写为cθ_i，sθ_i，cα_i-1，sα_i-1；Rot(x_i-1,α_i-1)为绕x_i-1轴转α_i-1角度变换矩阵，Trans(x_i-1,a_i-1)为沿x_i-1轴平移α_i-1的变换矩阵，Rot(z_i,θ_i)为绕z_i轴转θ_i角度变换矩阵，Trans(z_i,d_i)为绕z_i轴平移d_i的变换矩阵；

机械臂末端相对于基座标系的变换矩阵通过上式各杆件的变换矩阵

得到：

具体参数如表1所示，

获取各个关节点在在基座标下的坐标。

机械臂末端相对于基座标系的变换矩阵通过上式各杆件的变换矩阵

得到：

机器人末端执行器速度和关节速度之间的映射关系，而反映两者之间的关系的变换矩阵称为雅可比矩阵，用公式(2)表示为:

式中，

表示避开障碍物的速度，J为机械臂关节角速度向量

在末端速度向量

的线性映射(

J∈R^n×m，n，m为关节空间和操作空间的维数)。

S2、建立机械臂-障碍物模型：在实际试验中，避障控制方法不需要精确计算，只需计算机械臂的作业空间内不与障碍物所在的包络体发生碰撞。本申请引入伪距离作为无碰撞距离判别指标替换欧氏距离，使用解析函数来表示多形状障碍物，以障碍物几何中心为原点建立坐标系，空间点到障碍物球体的超曲面伪距离解析方程为

采用理想几何球体对不规则形状的障碍物建模，即空间点到障碍物的超二次曲面伪距离表达式为

式中，

表示障碍物坐标空间中伪距离，

表示障碍物包络体形状函数，s_a＝(h₁,h₂,h₃,m,n,p)为障碍物包何体函数的几何参数，q_c(x,y,z)为空间中任意一点坐标位置,(x₀,y₀,z₀)为超二次曲面中心点。R_s＝r_obs+r_i，为预设障碍物安全距离，r_obs为给定障碍物半径，r_i为拟合连杆圆柱体最大半径。

图3展示了机械臂任务空间中任意一点q_c(x,y,z)，基于式(2)，计算出相应的伪距离，为了满足避障过程安全的进行，避障过程中最小伪距离S_p,min(q_c)始终大于障碍物包络物的预设阈值d_pm，

判断机械臂连杆与障碍物表面刚是否发生接触，机械臂是否处于安全状态。利用伪距离的概念来构造防碰撞约束的方法，每个障碍都包含一个超二次曲面的解析表达式，更定性地表达了障碍物与机械手之间的约束条件，简化了仿真中繁琐的计算，基于虚拟力无碰撞约束的公式不仅简单而且容易实时更新机械臂避障碍运动。

S3、计算伪距离最小值：采用向量方法来表达机械臂和障碍物之间的最小伪距离值；向量方法表示为：S_p(x,y,z)＝X^TQX+B^TX+C

其中，X＝[x,y,z]^T，Q＝diag[1/R_s ²,1/R_s ²,1/R_s ²]，B＝[-2_x0/R_s ²,-2y₀/R_s ²,-2z₀/R_s ²]^T，Q、B、C均为常量。通过向量的方法获取机械臂连杆上离障碍物最近的点及最小伪距离值，根据该距离值大小与预设阈值比较，解决障碍物出现在期望轨迹时无法完成避障的问题，同时完成避障后迅速恢复到期望轨迹，实现轨迹跟踪任务，具有十分重要的现实意义。

S4、改进虚拟斥力函数：采用虚拟引力斥力对机械臂末端轨迹进行规划，即障碍物包络体与末端执行器之间的产生一种新颖的虚拟排斥力，对末端执行器轨迹进行修正，传统的势场法只对末端执行器和障碍物之间的距离进行了规划，而忽略了末端执行器的速度和运动方向的影响，避障运动仅仅考虑距离是不够的，可能会造成碰撞的可能。为了解决这些问题，对上述斥力势场函数改进如下:

其中,F_dyn(x,v)为由速度和位置产生的虚拟力，S_p(x)为机械臂末端和障碍物之间的伪距离，β是一个常数，可以调整速度矢量角θ的影响，如图4所示θ表示当前速度矢量ν和末端执行器位置q_c(相对于障碍物的位置)之间的角度。并且可以使用公式计算。当θ在

之间时，表示机器人正在接近障碍物，动态斥力场开始工作；当θ值在

以内时，机器人远离障碍物，动态斥力场应该不起作用。

S5、利用算法实现末端避障任务：基于虚拟力的末端避障方法，动态势场产生的排斥力考虑了速度、位置和方向三个因素，可以提高机器人的避障行为。排斥力与到障碍物的距离s_p成反比，与速度ν成正比，当速度ν为零或s_p大于阈值时，排斥力也与速度向量ν与到障碍物的方向向量之间的角度有关，如果角度θ小于90°(即端效应器远离障碍物)，则排斥力为零。

基于上式产生的虚拟斥力对机械臂末端速度

进行修正，即：

第二项

为与避障相关的躲避速度，与机械臂连杆上距障碍物最近点有关。

为虚拟斥力产生的速度调整量，当障碍物出现在末端执行器期望轨迹上时，即最近距离点为机械臂末端执行器位置，改变末端运动速度轨迹，末端执行器不断向障碍物靠近，最小伪距离的减小，同时θ处于

虚拟斥力不断增大，产生较大的速度进行修正，作用在末端执行器使其避开期望轨迹上的障碍物。在末端避障问题求解中，改变传统方法只考虑距离的缺点，通过障碍物和机械臂末端之间不同的速度、距离和移动方向，动态排斥场可以产生平滑的虚拟力斥力，给末端执行器增加了一个避障速度，解决障碍物出现在末端期望轨迹上的问题，同时保证机器人通过障碍物后恢复到原来的运动状态。

S6、仿真实验和分析

为了验证本发明提出的避障方法的有效性及可行性，将该方法应用于LBRiiwa14R280七自由度机械臂模型中如图2，在matlab2020a软件中进行仿真实验。

末端期望轨迹：起始点位置和末端位置为(0.3,0.3,0.6)，以圆心O_rid(0.2,0.3，0.6)，期望轨迹为：

机械臂初始关节角:

机械臂连杆等效圆柱体半径R＝0.03m

预设障碍物环境：实验仿真在多个障碍物存在的情况下进行，保证避障方法的有效性，障碍物预设三个大小相同的球体包络体半径r＝0.03m，球心坐标以机械臂基座为参考系分别为

障碍球1(零空间处)：O_obs1＝(0.3,0,0.4)；

障碍球2(零空间处)：O_obs2＝(0,0.1,0.6)；

障碍球3(末端轨迹处)：O_obs3＝(0.1,0.3,0.6)；

所以在零空间处和末端期望轨迹上都有障碍物出现；

伪距离阈值d_pm＝3,对应欧氏距离为0.06m；

所以在零空间处和末端期望轨迹上都O_obs1有障碍物出现。

从图5可知，机械臂通过构型的变化按照期望轨迹成功避开所有障碍物，结合图6，仿真时间在0-0.9s内机械臂与障碍物之间最小伪距离远大于伪距离阈值，此时障碍物对机械臂末端轨迹跟踪精度没有影响，按照期望轨迹继续运动，图7可以看出轨迹误差在0.02m以内，跟踪精度较高优于传统的避障方法。末端执行器能够平稳的避开障碍物并迅速恢复到期望轨，同时保证机械臂可以准确到达目标位置，验证了该避障方法能够实现末端避障。

如图5所示，T＝0.9s时机械臂末端进入障碍物3危险区域，由图8可知机械臂末端和障碍物之间的虚拟斥力迅速增大，虚拟斥力给机械臂末端产生一个位置增量，牺牲末端轨迹高精度跟踪开始迅速进行避障措施，保持最小伪距离在2左右，出现小幅度抖动是由于障碍物2和3预设位置距离较近，机械臂紧急避障导致，符合实际情况，同时证明了本发明提出的方法具有较高的灵敏度及有效性，相比于传统避障方法有更好的实用性。当仿真时间T＝2.3-3s最小伪距离逐渐增加，机械臂离开障碍物危险区域，避障运动开始减弱，末端轨迹跟踪误差逐渐减小最终收敛至预设的期望轨迹，回到起始点，完成仿真。

同时从图9的机械臂关节角度图中可以看出整个运动过程没有出现抖震现象，关节位置平稳，图9为机械臂关节速度图仿真开始出现的现象T＝0-0.5s可能是由于受机械臂模型本身一些不确定性参数和方法的适应性问题，关节速度出现了小幅度线性变化，导致Y轴上的实际轨迹误差突然增大，该问题需要在后期的科研中进一步解决，随后机械臂迅速恢复避障运动，运动速度连续且光滑平稳，仿真验证了方法的有效性。

以上示意性的对本发明及其实施方式进行了描述，该描述没有限制性，附图中所示的也只是本发明的实施方式之一，实际的结构并不局限于此。所以，如果本领域的普通技术人员受其启示，在不脱离本发明创造宗旨的情况下，不经创造性的设计出与该技术方案相似的结构方式及实施例，均应属于本发明的保护范围。

Claims (9)

1.一种基于虚拟力的机械臂末端避障方法，其特征在于，步骤为：

S1、建立机械臂运动学模型：采用标准的D-H法建立机械臂运动学模型，通过机械臂关节坐标系相对于基坐标系的齐次变换矩阵进行坐标变换：

式中，i(i＝1,2,…,n)为机械臂关节序号，cosθ_i，sinθ_i，cosα_i-1，sinα_i-1缩写为cθ_i，sθ_i，cα_i-1，sα_i-1；Rot(x_i-1,α_i-1)为绕x_i-1轴转α_i-1角度变换矩阵，Trans(x_i-1,a_i-1)为沿x_i-1轴平移α_i-1的变换矩阵，Rot(z_i,θ_i)为绕z_i轴转θ_i角度变换矩阵，Trans(z_i,d_i)为绕z_i轴平移d_i的变换矩阵；

机械臂末端相对于基座标系的变换矩阵通过上式各杆件的变换矩阵

得到：

S2、建立机械臂-障碍物模型：计算机械臂工作空间中任意一点q_c(x,y,z)在障碍物坐标系o_obs-x_obsy_obsz_obs下的位置，用于判别机械臂连杆与障碍物伪距离大小；

S3、计算伪距离最小值：采用向量方法来表达机械臂和障碍物之间的最小伪距离值；

S4、改进虚拟斥力函数：考虑到末端执行器的速度和运动方向，改进虚拟斥力函数，并求出负梯度函数；将步骤S3中求出的最小伪距离值带入到改进虚拟斥力函数，求出机械臂末端和障碍物之间虚拟斥力，对机械臂末端轨迹进行规划；

S5、利用算法实现末端避障任务：将步骤S4中的求出的虚拟斥力带入到机械臂末端避障方法中，实现机械臂末端避障。

2.如权利要求1所述的基于虚拟力的机械臂末端避障方法，其特征在于，所述步骤S2中建立机械臂-障碍物模型，利用伪距离的概念来构造防碰撞约束的方法，每个障碍都包含一个超二次曲面的解析表达式，更定性地表达了障碍物与机械手之间的约束条件，以障碍物几何中心为原点建立坐标系，空间点到障碍物球体的超曲面伪距离解析方程为:

式中,

为障碍物坐标空间中伪距离，(x₀,y₀,z₀)为超二次曲面中心点；R_s＝r_obs+r_i，为预设障碍物安全距离，r_obs为障碍物包络体半径，r_i为模拟机械臂厚度半径。

3.如权利要求2所述的基于虚拟力的机械臂末端避障方法，其特征在于，所述步骤S2中判别机械臂连杆与障碍物伪距离大小，任务空间中任意一点q_c(x,y,z)，计算出相应的伪距离，为了满足避障过程安全的进行，避障过程中最小伪距离S_p,min(q_c)始终大于障碍物包络物的预设阈值d_pm，

机械臂处于安全状态。

4.如权利要求3所述的基于虚拟力的机械臂末端避障方法，其特征在于，所述步骤S3中向量方法表示为：S_p(x,y,z)＝X^TQX+B^TX+C

其中，X＝[x,y,z]^T，Q＝diag[1/R_s ²,1/R_s ²,1/R_s ²]，B＝[-2_x0/R_s ²,-2y₀/R_s ²,-2z₀/R_s ²]^T，Q、B、C均为常量。

5.如权利要求4所述的基于虚拟力的机械臂末端避障方法，其特征在于，所述步骤S4中采用虚拟引力斥力对机械臂末端轨迹进行规划，即障碍物包络体与末端执行器之间的产生一种新颖的虚拟排斥力，对末端执行器轨迹进行修正，使其在完成障碍物出现在期望轨迹上的情境。

6.如权利要求5所述的基于虚拟力的机械臂末端避障方法，其特征在于，所述步骤S4中斥力函数为:

其中,F_dyn(x,v)为由速度和位置产生的虚拟力，S_p(x)为机械臂末端和障碍物之间的伪距离，β是一个常数，以调整速度矢量角θ的影响，θ表示当前速度矢量ν和末端执行器位置q_c(相对于障碍物的位置)之间的角度。

7.如权利要求6所述的基于虚拟力的机械臂末端避障方法，其特征在于，所述步骤S4中，基于公式

计算，当θ在

之间时，表示机器人正在接近障碍物，动态斥力场开始工作；当θ值在

以内时，机器人远离障碍物，动态斥力场不起作用。

8.如权利要求6所述的基于虚拟力的机械臂末端避障方法，其特征在于，所述步骤S5中，基于步骤S4中产生的虚拟斥力对机械臂末端速度

进行修正，对传统的避障算法修正后的运动学方程：

其中，

为末端速度矢量，雅可比矩阵J为机械臂关节角速度向量

在

的线性映射(

J∈R^n×m，n，m为关节空间和操作空间的维数)，J₀为障碍物在连杆上最小欧氏距离投影点对应的雅可比矩阵，N＝I-J⁺J，I∈R^n×n为单位矩阵，J⁺表示对矩阵求伪逆，J⁺＝J^T(JJ^T)^-1；

为上述虚拟斥力F_dyn(x,v)产生的速度调整量。

9.如权利要求8所述的基于虚拟力的机械臂末端避障方法，其特征在于，所述步骤S5当障碍物出现在末端执行器期望轨迹上时，即最近距离点为机械臂末端执行器位置，改变末端运动速度轨迹，末端执行器不断向障碍物靠近，最小伪距离的减小，同时θ在

时处于虚拟斥力不断增大，产生较大的速度进行修正，作用在末端执行器使其避开期望轨迹上的障碍物。

Images