双七自由度臂防碰撞协同控制算法研究①

江亦涵 王 挺 李亚伟 邵士亮 王 宁 毕健康 姚 辰

(*中国科学院沈阳自动化研究所机器人学国家重点实验室 沈阳 110016)

(**沈阳理工大学自动化与电气工程学院 沈阳 110159)

(***中国科学院大学 北京 100049)

(****沈阳工业大学人工智能学院 沈阳 110870)

(*****沈阳化工大学信息工程学院 沈阳 110142)

0 引言

近年来,双臂机器人由于其协调性强、可操作性强、工作效率高等特点,逐渐在制造业和服务业等领域取得应用[1-3]。双臂机器人可以完成多种复杂的任务,例如协同搬运和协调操纵。但控制器设计的难度也远大于单臂机器人,因为增加的手臂不仅导致复杂的结构和机构,而且增加了强耦合非线性以及被抓取物体产生的内力。为了解决这些问题,双臂机器人的末端位置控制得到了广泛的研究[4-10]。Liang 等人[4]通过计算双四元数对双臂机器人进行运动控制。Phukan 等人[5]设计了一个滑模控制器以协调双机械臂位置。Liu 等人[6]通过对机械臂进行体素化分割,计算出了空间中每一点相对于机械臂的可达性。Kivel 等人[7]为机械臂建立了坐标轴平行(axis-aligned bounding box,AABB)包围盒,并采用了分离轴定理法进行碰撞检测。Kim 等人[8]采用了一种基于调和函数的新型人工势函数来构建人工势场并进行轨迹规划。Palmieri 等人[10]提出了一个冗余机械手的运动规划和控制框架,采用了离线路径规划算法与在线运动控制组合的控制策略。文献[11]与文献[12]分别采用了快速搜索随机树(rapid-exploration random tree,RRT)及A*算法探索未知环境,但以上方法均只适用于机械臂在已知环境信息的情况下规避静态障碍物。

根据机器人在生产和生活中的应用可知,障碍物包括静态障碍物和动态障碍物2 种。对于大多数双臂机器人来说,两臂的工作区域存在重叠部分,因此当一个机械臂移动时,该机械臂会被另一个机械臂视为动态障碍物。因此,任何一个机器人机械手都应该避开动态障碍物以完成路径规划。李龙等人[13]提出了一种基于接近觉的机械臂避障路径规划方法,通过读取连杆运动方向上的距离传感器数据作为碰撞检测的依据,并采用基于势函数与关节空间的人工势场法解决避障路径规划问题。但该方法需要在机械臂上安装额外的传感器,增加了机械臂的成本。刘建春等人[14]采用球体和胶囊体包围盒简化机械臂模型,根据包围盒之间的相交情况判断是否发生碰撞。通过二次投影法,将包围盒投影为线段实现快速检测。但二次投影法对包围盒角度要求较高,存在失效风险。

本文针对仿人双机械臂协同运动规划中防碰撞的问题,基于实验室自行研发的双七自由度臂机器人,通过理论计算、仿真实验,提出了一种适用于冗余自由度机器人协同工作的控制算法。建立了机器人正逆运动学模型与包围盒模型,采用了基于单纯形退化的改进吉尔伯特-约翰逊-基尔特(Gilbert-Johnson-Keerth,GJK)算法设计了基于安全工作范围的改良五次多项式轨迹插补法。对此算法分别进行了Matlab 仿真、插拔钥匙实验与火炬传递实验。在环境中不存在静态障碍物仅需避免机器人自碰撞的情况下该方法具有非常快的计算速度,得到的轨迹也较为平滑,具备一定的工程应用价值。

1 自碰撞检测系统设计

1.1 正运动学模型

本文研究对象为图1 所示的双七自由度臂机器人。为准确获取其运动过程中的各关节位置,并以此为基础构建与更新包围盒,需要对其进行运动学建模。目前主流的运动学建模方法有2 种,一种是标准D-H 参数法,一种是改进D-H 参数法[15]。标准DH 参数法仅适用于串联机器人,而改进D-H 参数法适用于串联机器人、树结构机器人、并联机器人、闭回路结构机器人与移动机器人。由于本文的研究对象为6 轮移动机器人,故建立如图2 所示的机械臂坐标系,得到的改进D-H 参数如表1 所示。其中θi代表x轴转角,αi-1代表z轴转角,di-1代表x轴偏移量,ai代表z轴偏移量。

表1 MDH 参数

图1 机器人三维模型

图2 机械臂MDH 图

由于机械臂采用仿人手结构,因此对每个机械臂分别采用4 个OBB 包围盒进行碰撞模型建模。

为了在机械臂各个关节旋转的过程中及时更新包围盒的位置与角度,需要对机械臂进行正运动学计算,以确定各关节的位置坐标,分别确定第2 关节到第4 关节、第5 关节到第5 关节、第7 关节与末端抓手的向量方向与长度,以此为基础实时地更新包围盒的位置与欧拉角。通过MDH 参数表构建齐次变换矩阵,可得到:

其中cθi为cosθi的简写,sθi为sinθi的简写,θ1～θ7为第1 关节至第7 关节的旋转角度。

根据上式求各个关节相对于基坐标系的齐次变换可得

其中r11～r33可以构成3×3 旋转矩阵,px、py、pz即为第i个关节相对于基坐标系的坐标,三轴欧拉角为

其中px、py、pz与θx、θy、θz均为关于θ1～θ7的函数。

1.2 OBB 包围盒建模

基于这些数据,对于本文所研究的机械臂进行有向包围盒(oriented bounding box,OBB)建模,则依照式(1)和(2)通过DH 参数计算出手臂第2 关节变换矩阵并取其最后一列,可以得到该关节几何中心的三维坐标px、py、pz。取其前3 列并依照式(3)转换为欧拉角,可以得到关节相对基坐标系旋转的角度θx、θy、θz。以此类推,对第4 关节、第5关节、第6 关节与第7 关节进行同样的计算。由此可得所有关节的坐标以及包围盒旋转的角度,以此可以构建出高为

的OBB 包围盒,长与宽依照三维模型尺寸确定。针对本文的机器人形状参数,包围盒的长与宽取为

并按照θx、θy、θz对包围盒进行旋转,由此在机械臂旋转过程基于各关节角度实时更新包围盒的坐标与欧拉角。

除此之外,对于双臂机器人,不仅应当考虑双臂之间的自碰撞,也应当考虑双臂与躯干之间的自碰撞。但由于躯干部分相对于机器人坐标系不发生相对转动,因此生成的OBB 包围盒不需要实时更新位置与欧拉角,仅需在基坐标系的基础上增加一个固定的偏移量。针对躯干部分生成的OBB 包围盒尺寸如下:

得到了如图3 所示的机器人的包围盒建模。

图3 包围盒模型

1.3 改进GJK 碰撞检测算法

根据机械臂的包围盒模型,将双臂机器人碰撞问题转化为判断各包围盒是否与其他包围盒相交的问题。本文采用基于单纯形退化的改进GJK 算法计算包围盒之间的距离,避免了原始GJK 算法需要搜索所有原点可能出现的区域的缺点,计算量小,鲁棒性较强。

GJK 算法是由Gilbert、Johnson 和Keerthi 在1988 年开发的一种迭代算法。通过输入空间内任意两凸体的顶点集,可以计算出凸体间的欧式距离。假设存在两凸体A和B,则A和B距离d(A,B)=min{‖X-Y‖},其中X∈A,Y∈B。根据两凸体的Minkowski 差的定义,即D={X-Y|X∈A,Y∈B},两凸体间的距离可以等效为二者的Minkowski 差构成的图形与原点的距离,如图4 所示。图4(a)中2 个三角形之间的距离等价于图4(b)中2 个三角形求Minkowski 差所构成的六边形与到原点的距离。具体流程如下所示。

图4 距离等价图

(2)在凸体A中选取在方向投影长度最长的点p1,在凸体B中选取在方向投影长度最长的点p2,令v1=p1-p2,此时v1即为三维单纯形的一个顶点。

(3)判断该三维单纯形是否包含原点,若包含原点,说明发生了碰撞,算法结束。若不包含原点,继续步骤(4)。

(4)在v1、v2、v3、v4构成的三维单纯形中找出距离原点最近的面S,移除不属于这个面的点,并将向量的方向更新为面S的垂线方向,以新的向量方向重新取点,构建单纯形;重复步骤(4),直到取到的点与之前重复,此时迭代完成,未发生碰撞,计算三维单纯形与原点的距离,即为两凸体间的欧氏距离。

对于原始GJK 算法而言,虽然这种算法在数学上是完全可实现且可靠的,但实际的应用场景对于实时性的要求非常高,原始的GJK 算法在处理多面体时需要进行大量的计算,可能导致实时性受到影响。为了应对这一问题,本文采用了一种基于单纯形退化的改进GJK 算法,通过改良原始GJK 算法的距离子算法来获得更高的计算效率。

如图5 所示,原始GJK 算法从底部向上迭代,依次搜索图5 中(1,2)区域,(1,3)区域与(2,3)区域中是否存在原点,而改进GJK 算法则通过将单纯形退化,排除不可能包含原点的先验区域,大幅简化了计算量,算法流程对比图如图6 所示。改进GJK算法的详细步骤如下。

图5 GJK 算法搜索区域

图6 算法流程对比图

对于如图7(a)所示的由v1、v2、v3、v4构成的三维单纯形,构造矩阵:

图7 根据指定三维单纯形构建虚拟三维单纯形

并对M进行代数余子式展开,则有:

如图7(b)～(e)所示,分别舍弃单纯形的顶点之一,并将剩余3 个顶点与原点相连,构建出4 个虚拟单纯形,此时A4,1、A4,2、A4,3、A4,4的绝对值分别与这4 个虚拟单纯形的体积成正比。在计算出了detM与所有的A4,i之后,将detM的符号与A4,i逐个进行比较,若detM与A4,i符号均相同,则说明原点在单纯形内部;若detM与A4,i符号不同,则舍弃点vi,将原有的三维单纯形退化为二维单纯形,对于图7的情况,经过计算可知:

因此,舍弃点v4,构成新的二维单纯形s1、s2、s3,如图8 所示。对于二维单纯形,取向量:

图8 二维单纯形与原点投影

将原点O按向量pO投影至单纯形所在平面中,得到点PO,此时式(5)中已计算得到的|A4,1|、|A4,2|、|A4,3|与单纯形在xOy、yOz、xOz上的投影面积成正比,选取|A4,1|、|A4,2|、|A4,3|中的最大值,并将单纯形连同点p向相应的面进行投影。对于本文列出的情况,|A4,1|=1.5,因此在yOz平面进行投影。投影后的结果如图9 所示。此时,设矩阵为

图9 yOz 面投影

得到λ1、λ2、λ3,算法结束,返回点s1、s2、s3,根据λ1、λ2、λ3得到新的搜索方向。

综上所述,可得到如图10 所示的算法流程图。

图10 单纯形退化算法流程图

2 机械臂控制方法

2.1 逆运动学求解

对于任意三维空间机械臂,末端的姿态信息可以由一个6×1 的向量α=[x,y,z,θx,θy,θz] 完全描述。而对于七自由度机械臂联立方程组可得:

在这个方程组中,共有7 个变量θ1,θ2,…,θ7以及6 个方程,变量个数大于方程个数,属于欠定方程组,因此在空间中存在无穷多解,即在笛卡尔空间中存在无穷多组满足指定位置与角度要求的手臂位姿。对于该问题,本文采用牛顿-辛普森法求取满足末端位置要求的数值解。

当机械臂末端的位置与角度仅发生微小变化,且变化耗时较短时,可以将这样的微小运动等效为机械臂末端的运动速度,这种运动称作机器人的微分运动。设d为机械臂位置的微分运动,δ为机械臂角度的微分运动,则微分运动可以表示为

记机械臂当前末端位姿为Tcurr,目标末端位姿为Tend,则

使用牛顿迭代法的步骤如下所示。

(1)根据机械臂当前姿态下各关节角度值qcurr=[q1,q2,q3,q4,q5,q6,q7] 计算出机械臂当前末端位姿Tcurr。

(2)根据机械臂目标末端姿态计算机械臂的微分运动矩阵Δ。

(3)根据机械臂的微分运动矩阵Δ与当前角度qcurr计算出机械臂的雅可比矩阵。

(4)根据式(14)求出dq的大小,其中雅可比矩阵的逆即为机械臂末端位姿矩阵的伪逆。

(5)根据当前计算出的dq,叠加到目前的关节角度中,即关节的角度变化成为qcurr=dq+qcurr。

(6)根据新的qcurr计算出新的末端姿态Tcurr,回到步骤(1),以此继续迭代。

以上是迭代计算的计算过程。此外,还需要为该迭代过程设置迭代终止条件。常见的做法是设置一个误差量E,当迭代结果与目标末端位置的差小于一定范围时即可停止迭代。此外再设置一个迭代次数上限I,当超出迭代次数上限时停止计算,并反馈一条错误信息。

2.2 机械臂轨迹规划

在完成逆解计算之后可以得到2 组关节角,分别为当前状态关节角qcurr与目标状态关节角qend。考虑到在插拔火炬钥匙与传递火炬的需求中均不存在坐标及形状已知的静态障碍物,而动态障碍物通过碰撞检测可以保证安全性,因此没有采用常用的诸如快速搜索随机树法一类的空间避障路径规划算法。为保证传递火炬与插拔钥匙过程中机械臂运动的平稳性,采用了结合包围盒欧氏距离的改进五次多项式插补方法。

在机器人实际应用中,各个关节角的角度、角速度和角加速度都不能发生突变,否则会大大加重电机的负载,严重时可能导致电机损坏。五次多项式插补可以解决三次多项式插值方法中角速度变化不平滑、角加速度容易发生突变的问题。

五次多项式插补方程可以写作如下形式:

在此方程的基础上添加对起始点与终点处角度、角速度、角加速度的约束方程。

解方程得:

在双臂机器人正常平稳工作时,五次多项式插补可以较为轻松地满足机械臂运动的各项需要,但发生不在路径规划中的包围盒碰撞时,仍难以避免电机速度与加速度的突变。因此需要对五次多项式得到的轨迹点逐个进行测试,在发生碰撞或包围盒间最小距离小于安全值时取前一个轨迹点,重新进行五次多项式插值。

3 仿真与实验验证

3.1 碰撞检测算法仿真

为验证本文采用的改进GJK 算法的高效性,在空间中多次随机生成2 个正四面体、正方体、正八面体以及正十二面体,分别应用分离轴定理(separating axis theorem,SAT)算法、原始GJK 算法与改进GJK 算法进行距离计算。结果如图11 所示。

图11 算法耗时对比

从图中不难看出,随着多面体形状变得复杂,SAT 算法耗时上升较快,而GJK 算法更适合用于处理复杂的三维物体。对于各种形状、位于空间中随机位置的多面体,使用改进GJK 算法进行碰撞检测耗时均比原始GJK 算法少5%～25%,应用于机械臂的碰撞检测系统可以大幅提升实时性。

3.2 机械臂控制仿真

为了验证本文提出方法的可行性,在CoppeliaSim软件中设定了一条有碰撞发生的机械臂运动轨迹,并使用其提供的Matlab API 进行辅助计算。在机械臂由初始状态运动至图12 所示的终止状态的过程中,机械臂由的初始状态运动至

图12 终止状态机器人位姿

将这段运动进行不同方式的插值,并在Matlab中计算手臂各关节与其余包围盒的距离,同时记录各关节参数变化状况。参数变化状况如图13 所示。

图13 均匀插值各关节运动过程

由图13 可以看出,采用均匀插值法进行轨迹规划时,起始阶段与终止阶段均存在较大的角速度突变。如图14 所示,采用五次多项式插值时,起始阶段的角速度变化较为平滑,但在发生了规划之外的自碰撞时,仍然不可避免地产生了较大的加速度。如图15所示,采用了基于安全工作范围的改良五次多项式轨迹插补法之后,得到了较好的运动轨迹,各关节角度变化平滑,角速度与角加速度无突变。

图14 五次多项式插值各关节运动过程

图15 改良五次多项式插值各关节运动过程

3.3 真实场景协同工作实验

在火炬传递项目中,为了体现双臂机器人协同工作的能力,进行了插拔火炬点火钥匙的实验。在该实验中,机器人左手夹持一直径为5mm的杆型钥匙,右手夹持的火炬侧面开有一直径为5.2 mm的小孔,机器人的插拔钥匙过程完全自主进行,多次实验均能顺利完成指定工作。实验过程如图16 所示。

图16 协同工作实验

此外,机器人还与冰壶机器人进行了传递火炬实验。在冰壶机器人位姿不变的基础上,由双臂机器人对冰壶机器人所持火炬进行定位,并通过引导手臂末端位姿达到指定位置,使得夹爪所持火炬能够尽可能接近冰壶机器人所持火炬,进而达到火炬传递的目标,实验过程如图17 所示。

图17 与冰壶机器人火炬传递实验

实验中,虽然机器人顺利完成了指定的任务,但由于冰壶机器人属于动态障碍物,难以进行包围盒建模与碰撞检测,故仍存在碰撞风险,后续在这类多机器人协作的动态障碍物密集的环境,仍需安装力传感器以保证安全工作。

4 结论

本文针对机器人安全工作要求,为双七自由度臂机器人设计了一套控制系统,通过采用OBB 包围盒建模与基于单纯形退化的改进GJK 算法求欧氏距离相结合的方法,获得了良好的避碰撞能力。通过牛顿-辛普森迭代法以及改良五次多项式插值法,可以获得平稳、无突变的机械臂末端运动轨迹。机器人在实际场景中进行了插拔火炬点火钥匙与传递火炬实验,均能较好地完成指定工作。实验结果表明,所设计的双七自由度臂机器人控制系统具有良好的控制精度、较高的工作可靠性和较广的环境适应性,能够满足双机械臂协同工作的需求。