冗余自由度协作机器人运动规划研究

马金琦，郭冰菁，2，韩建海，2，3，李向攀，2

(1.河南科技大学机电工程学院，河南 洛阳 471003；2.河南省机器人与智能系统重点实验室，河南 洛阳 471003；3.机械装备先进制造河南省协同创新中心，河南 洛阳 471003)

1 引言

近年来，随着产品多样化、精细化、轻量化的发展，对机器人的应用提出了新的要求。机器人要满足生产线批量生产的刚性需求，也要满足客户个性定制的柔性制造。同时机器人从独立工作转向共享环境中的与人协作模式，工作环境不再需要围栏隔离，机器人能够直接与人并肩工作[1]。协作机器人对人机交互、柔顺适应等关键技术提出了更高的要求，约束空间中的机器人自主的运动规划研究是协作交互的重要研究内容。

运动特性的研究是机器人运动控制和路径规划的基础[2]。国内外对非冗余机械臂运动学的研究已相当成熟，但对冗余机械臂，特别是其逆运动学的研究相对有限。文献[3]提出固定一个关节，将机器人改为六自由度求解，但关节的减少会降低机械臂的灵活性；李宁森[4]对无特殊构型机械臂，采用伪逆法求逆解，此方法适合实时、高精度的逆运动学求解；魏延辉等[5]根据构型平面的基本理论和半解析的方法进行逆运动学求解。

常见的路径规划算法有概率路标图法(probabilistic roadmap method，PRM)[6-7]、人工势场法[8-9]、A*算法[10]、快速探索随机数法(Rapidly-exploring random trees，RRT)[11-12]。当存在未知障碍物时PRM算法的效率较低。RRT算法收敛速度快，具有概率完备性，但因其采用的是全局均匀搜索策略，无谓的消耗大量的计算资源，增加了搜索时间，收敛速度缓慢。

针对特殊构型的Sawyer机器人，本文对传统RRT算法新枝点扩展时无方向性，搜索时间长的特点进行算法改进。在新枝点的扩展上加入引力系数，使其具有方向性，根据机器人工作空间障碍物复杂程度的不同，通过调节引力系数，使新枝点扩展具有合适的搜索步长，同时利用双向搜索有效减少了路径规划时间。采用阻尼最小二乘Newton-Raphson方法对Sawyer机器人逆运动学进行数值求解。在MATLAB和ROS(机器人操作系统)平台进行仿真和实验，通过实验验证路径规划改进算法的有效性。

2 机器人构形分析

在三维空间中，机器人至少需要六个独立变量(三个位置，三个姿态)，才能完整定义机械臂的末端位姿。冗余机器人由于自由度大于6，在机械臂的奇异点回避，空间避障等方面具有优势。由于机器人的关节存在限位，在工作空间内，非冗余机器人末端执行器存在不可达点，七自由度机器人通过增加了关节个数和旋转角度，可以克服关节不可达问题。

七自由度机器人最常用的关节配置模式为Spherial-Roll-Spherial构型，即腕部与肩部是由三个轴线互相垂直、交于一点的等效球关节，肘部为旋转关节构成，多为非偏置式构型。其优点是制造简单，运动学求解方便。无偏置球关节构型如图1所示。但由于其肘关节运动范围受限，在需要执行较复杂工作，或要求机械臂工作范围尽可能大时，多采用图2所示的偏置式非球关节构型。

图2 有偏置非球形关节构型

本文以七自由度协作机器人Rethink Sawyer为研究对象，如图3所示，为有偏置非球形关节构型，肩部关节J0与J1的轴线垂直且无交点，连杆偏置，使得关节运动角度增加。

图3 7自由度机器人Sawyer

3 基于RRT的改进路径规划算法

3.1 RRT算法基本原理

RRT算法是在待搜索区域中，对地图进行分裂寻找从起始位置xinit到目标位置xgoal的随机搜索树。算法的基本原理是，以xinit为根节点，在区域内随机采样得到xrand，然后找到离xrand最近的节点xnear；在指向xrand的方向上生成给定步长ρ的新节点xnew，若新生节点与障碍物没有碰撞，则作为扩展节点加入原随机树，否则重新进行采样；当采样点距目标点xgoal距离小于设定阈值时，算法结束，否则继续扩展。其新生节点计算公式为：

(1)

图4 RRT树生长过程

RRT算法在理论上总能在空间中找到一条有效搜索路径，但新枝点的产生是随机采样，对空间区域搜索过于平均，随着空间维度与搜索区域的增加，将产生大量冗余点，使搜索效率将大幅下降。

3.2 改进RRT算法

针对传统RRT算法在新枝点扩展上无目标性，时间复杂度高的特点。基于人工势场法中目标引力的思想，提出枝点扩展具有导引的变步长双向RRT算法。目标点方向增加引力函数，枝节点朝着目标点方向扩展，无需对所有区域进行搜索，减少了对冗余点的计算；根据搜索区域的复杂度，人工调节引力参数，避免局部极小值的产生；从根节点与目标点同时扩展，缩短搜索时间，提高路径搜索效率。

根据引力势能函数

(2)

求得引力函数

(3)

其中kp为引力场系数。

由此改进后新枝点计算方法为

(4)

在新枝点的扩展中，根据空间障碍物情况，调节合适的引力场系数kp。当空间中无障碍物时，增大kp提高随机树向目标点生长的概率，减小新枝点随机生长的概率；在遇到障碍物时，减小kp至合适值，增强新枝点随机扩展的能力，绕开障碍物。不但保证了避障能力，同时增强了方向的指向能力。

基于经典RRT算法，引入目标引力思想与变步长策略，虽然有效地降低了搜索过程的随机性，提升了机器人的避障能力强，但路径搜索效率较低。在此基础上，引入双向生长策略，在起始位置xinit与目标位置xgoal同时生成随机扩展树，直至两树新生枝点间的距离小于给定阈值，即可认为两树连通。

4 逆运动学求解算法

逆运动学是机器人进行运动规划的基础，其实质是已知机器人末端执行器在工作空间的位姿，求解关节空间角度的过程，设工作空间末端位姿为X=[x1，x2，x3，x4，x5，x6]，关节空间角度为q=[q1，q2，q3，q4，q5，q6，q7]，运动学逆解表达式为

q=f-1(X)

(5)

Sawyer机器人具有7自由度，关节几何构型不满足Pieper准则。本文采用Newton-Raphson法进行数值迭代求解逆运动学。由于关节空间维数大于工作空间维数，冗余机器人具有自运动性，对同一位姿会产生无穷组关节角与其对应。因此，需添加约束条件使机器人具有唯一逆解。Sawyer存在冗余自由度，雅克比矩阵J为6×7矩阵。

由于雅克比矩阵不是方阵，不能直接计算逆解，因此，使用最小二乘法的伪逆代替逆解进行计算。

在满足末端跟随误差最小的情况下，使关节速度范数最小，最小二乘法解得雅克比伪逆

J+=JT(JJT)-1

(6)

雅克比矩阵J奇异值分解

J=UΣVT

(7)

式中：U∈Rm×m与V∈Rn×n为正交矩阵，Σ∈Rm×n由J的奇异值构成。

(8)

利用伪逆解求得关节最小范数为

(9)

(10)

对式(6)进行求解得

(11)

(12)

式中：J*为奇异鲁棒性逆矩阵。奇异值分解，可得

(13)

(14)

根据奇异值的变化来确定阻尼系数的大小如式(15)，设定最小奇异值的阈值σ0，当机器人远离奇异点时，最小奇异值大于阈值σ0，此时阻尼系数u为零，使用伪逆解，在保证跟踪精度的情况下，使关节速度在合理范围；当机器人接近奇异点时，最小奇异值小于阈值σ0，此时阻尼系数不再为零，保证关节速度不会超限。

(15)

式中：u0为最大阻尼系数，σ0为最小奇异值阈值。

利用Newton-Raphson法对Sawyer机器人进行逆运动学求解释，基于机器人的微分雅克比变换：

Δq=J-1ΔX

(16)

式中：Δq为关节角度微小变量，ΔX=[dxdydzkxdθkydθkzdθ]为机器人末端位移的微小变量，其中dx，dy，dz是末端沿笛卡儿坐标系的移动量，kxdθ，kydθ，kzdθ是绕坐标系的旋转量。机器人的微分变化矩阵可以用位姿矩阵与微分变换算子Δ的乘积表示。其中，微分变换算子Δ为：

Δ=Trans(dx，dy，dz)Rot(k，dθ)-I

(17)

式中：dx，dy，dz是在坐标轴上的量，Rot(k，dθ)是绕坐标向量k旋转dθ角。

目标位姿由初始位姿Tstart与微分运动累加求解，为：

Tend=Tstart+Δ·Tstart

(18)

由式(18)求解微分变换算子Δ为：

(19)

根据式(17)可得机器人末端微小位移ΔX。

逆运动学算法流程如图5所示。

图5 逆运动算法流程图

5 仿真验证

5.1 逆运动学仿真

通过MATLAB进行数值仿真，通过给定机器人末端位姿变化，由式(5)～(19)，求解机器人末端到达指定位姿时各个关节角度，从而验证逆运动学算法。仿真条件为：给出机器人末端的初始位姿为[1.01475，0.1603，0.317;0，1.571，0]，初始位姿位于奇异构型附近，其最小奇异值趋于零，终点位姿为[0.5432，0.1603，0.9839;0，1.047，0]，位姿参数中前3项为机器人末端在x，y，z方向的位置，单位为m，后三项为绕三个坐标轴的旋转角度，单位为rad。参数初始化：u0=5，误差ε=10-5。

通过奇异鲁棒性逆的Newton iteration method迭代求解，经过57次迭代后，对应终点位姿的关节角度逆解趋于稳定，如图6所示。

图6 逆运动学求解中关节角度与迭代次数关系曲线

通过逆运动学求解，最终得到的关节角是q=[-0.0112，1.0482，-0.0138，1.0489，0.0112，1.0478，0.0138]T，没有出现关节突变的现象，说明机器人在逆解过程中，各关节运动平稳，可以求解出稳定解。因此可采用所建立的逆运动学求解方法在机器人工作空间中进行运动规划，通过逆运动学解得各关节的运动路径。

为验证所求解的关节角度是否与给出的末端位姿对应，将终点机器人关节角度再次代入正运动学模型，求解得出此关节角度对应的机器人末端位姿为[0.5436，0.1603，0.9838;0，0.1047，0]，解算后的机器人位姿如图7。

图7 终点位姿模型

将逆运动学解得关节坐标带入正运动学方程，解得此时机器人末端姿态矩阵如表1所示。

表1 逆运动学仿真结果

对照设定的机器人终点位姿，验证了运动学逆解求解方法的有效性。

5.2 RRT算法验证

5.2.1 仿真

实验环境:win10，Intel Core i5，主频2.6GHz，8G内存。编译工具为MATLAB 2017a。状态空间1000*1000，其中起始节点坐标为[10，10]，目标点坐标为[900，900]，黑色斑块为障碍物群，随机设置障碍物形状及位置。

实验一：对传统RRT算法进行仿真。从起始根节点开始，在空间中以给定长度为生长步长随机进行扩展，当随机树枝点到达目标点时，扩展停止，规划过程如图8所示。从起始节点起，以20为步长，随机生成新枝点，连接起始点与目标点规划出的路径如图9。

图8 传统RRT 图9 规划路径

实验二：对改进RRT算法进行仿真。从初始根节点与目标点同时扩展，生成新枝点，当两枝点相遇时随机树停止扩展，连接起始根节点与目标点的规划过程如图10，生成改进规划路径如图11。

图10 改进RRT 图11 改进路径

表2 RRT与改进算法的特性对比

通过表2中两种算法的特性数据对比可知，基于改进RRT算法在新枝点扩展上，有效地减少了新生枝点的数量，降低了算法的时间复杂度与计算量，提高了搜索效率，路径相对传统规划方法更加平缓。

5.2.2 机械臂运动规划实验

在改进RRT算法有效性验证的基础上，设计如图13所示的狭小空间(400*400)场景，模拟机器人进行加工时的可操作任务空间，对Sawyer冗余机械臂的无碰撞运动规划能力进行验证。首先，在ROS机器人操作系统中利用Rviz(三维可视化工具)进行可视化仿真。根据机器人模型与环境模型，通过编写URDF文件，创建机器人工作空间，然后采用改进RRT算法，在具有边界约束的狭小空间下，对冗余机械臂进行运动规划。在关节空间下给定机械臂的初始臂型与终止臂型，完成机械臂的运动规划。运动规划的初始与终止臂型如图12所示，采用改进RRT算法规划的路径如图13所示。

图12 运动规划仿真过程

图13 无碰撞狭小空间的改进RRT算法搜索路径

与仿真中所构建的任务空间一致，在实际环境中利用铝型材搭建400*400的狭小空间作为障碍物，初始机械臂末端伸入框架内，机械臂无碰撞运动至框架外作为实验目标，对运动规划方法进行实验验证，实验过程如图14所示。

图14 实际环境下运动规划过程

利用改进RRT算法顺利完成狭小空间下机械臂的运动规划。初始机械臂末端伸入障碍物框架内如图14(a)；沿规划路径，首先运动至框架外，机械臂臂型如图14(b)；机械臂运动至框架顶部，绕过框架顶部与顶角如图14(c)；最终绕过框架到达终点位姿如图14(d)。通过机械臂的运动实验，验证了改进RRT算法进行路径规划的正确性，在操作过程中有效地避免了与运动约束框架碰撞，保障了加工过程的安全。对应机械臂末端在操作空间中的规划路径，冗余机器人的各关节运动轨迹如图15所示，可以看出在关节空间中冗余机器人关节运动平稳，验证了逆运动学算法的正确性，为机械臂运动控制提供了有效的关节运动参数。

图15 关节空间中的轨迹规划曲线

6 结论

本文以七自由度协作机器人Rethink Sawyer为研究对象，运用改进RRT算法在狭小操作空间内考虑存在的路径约束对机械臂进行运动规划。实验结果表明文中设计的七自由度协作机器人的逆运动学算法能够有效的对机器人关节空间和操作空间中的运动关系进行求解，逆解的计算精度高，改进RRT算法有效缩短了运动规划的时间，在复杂环境下有效避免与障碍物的碰撞，为七自由度协作机器人的运动规划提供了可行的理论方法。