一种水下电动机械臂运动学与奇异性分析

黄 忠，刘可安

（株洲中车时代电气股份有限公司， 湖南 株洲 412001）

0 引言

水下机器人是开发海洋的重要装备，其中作业型水下机器人常搭载水下机械臂进行水下打捞、援潜救生、海底设施维护与装置回收、海底生物及岩石标本的采样等方面的作业［1］。目前，广泛使用的是液压驱动水下机械臂，但是水下液压机械臂存在漏油、重量重、控制精度低、响应速度慢等缺点［2］。水下电动机械臂每个关节采用大转矩伺服电机控制，可以完全克服水下液压机械臂的缺点，并且水下电动机械臂易于向智能化方向扩展，因此水下电动机械臂是未来的发展趋势。

运动学模型是机械臂姿态控制的基础，分为正运动学和逆运动学。正运动学根据每个关节的当前位置，计算机械臂末端位姿。逆运动学顾名思义即正运动学的逆过程，根据机械臂末端位姿，求解满足该位姿条件下每个关节的位置［3］。建立机械臂运动学模型的方法有解析法和数值法。解析法直接推导求解正逆运动学方程，优点是可以得到全部逆解，且求解速度快，缺点是运动学方程推导难度较大，不具有通用性。数值法通过迭代的方式求逆解，优点是方法简单，通用性好，缺点是无法求得全部逆解，求解速度慢［4］。国内外学者对机械臂的运动学已经开展了很多研究。Hawkins等人［5］采用解析法推导了UR5和UR10工业机械臂的运动学方程。罗贵成等人［6］基于改进的D-H参数法建立了一种六自由度机械臂的运动学模型，并提出了一种逆解的选取方法。姜涛等人［7］将机械臂逆运动学问题转化为目标优化问题，基于改进的粒子群算法搜索机械臂逆解。芮宏斌等人［8］提出了基于BP神经网络求解机械臂逆运动学的方法。裴香丽［9］对一种水下机械臂建立了运动学和动力学模型。

机械臂工作空间中存在一些奇异位置［10］，当机械臂位于奇异位置时，逆运动学方程无解，即无法通过逆运动学模型计算关节位置，并且此时机械臂末端的微小移动可能导致关节的大幅运动，从而导致机械臂失控，甚至机毁人亡的事故［11］。因此必须对机械臂的奇异位置进行分析，并且在机械臂的运动过程中对奇异位置进行规避，从而保证机械臂运动安全性。张倩玉等人［12］对桌面型607串联机械臂进行了工作空间和奇异位形分析。唐家豪［13］基于雅可比矩阵分析了一种七自由度机械臂的奇异位置。本文采用解析法分析水下电动机械臂的运动学模型，并详细分析了水下电动机械臂的奇异性，提出了规避奇异位置的方法。

1 运动学模型

本文研究的水下电动机械臂的三维模型如图1所示，其包含6个关节，所有关节都是旋转关节，每个关节通过一个伺服电机驱动。建立机械臂的运动学模型首先需要建立坐标系。图2显示了整个机械臂的坐标系，坐标系的建立方法服从改进的D-H（Denavit-Hartenberg）参数［10］对坐标系的要求，O代表坐标原点。根据坐标系获得该水下电动机械臂的改进D-H参数，如表1所示。表中i为关节索引，αi-1，ai-1，di，θi分别为连杆扭角、连杆长度、偏距和关节旋转角。a1～a4，d6，θ22，θ33，θ44的定义如图2所示，a1为O1和O2之间的距离，a2为O2和O3之间的距离，a3为O3和O4之间的距离，a4为O4和O5之间的距离，d6为O5和O6之间的距离，θ22为线段O2O3与竖直线之间的锐角，θ33为线段O2O3与线段O3O4之间的钝角，θ44为线段O3O4与竖直线之间的钝角。

图1 水下电动机械臂三维模型Fig.1 Three-dimensional model of the underwater electric manipulator

图2 机械臂坐标系Fig.2 Coordinate system of the manipulator

1.1 正运动学模型

已知每个关节的位置q=[θ1，θ2，θ3，θ4，θ5，θ6]T，求解机械臂末端相对于基坐标系O0的位姿代表末端坐标系O6相对于基坐标系O0的齐次变换矩阵。矩阵中的n=[nx，ny，nz]T，o=[ox，oy，oz]T，a=[ax，ay，az]T代表机械臂末端姿态，p=[px，py，pz]T代表末端位置。求解正运动学模型，需要使用到坐标系间通用的齐次变换矩阵［10］。

基于式（1）和D-H参数，可以计算得到：

因此

1.2 逆运动学模型

从图2中可以看出该机械臂的关节2、关节3和关节4 的旋转轴相互平行，因此该机械臂满足pieper 准则［14］，具有逆运动学解析解。对于逆运动学问题，机械臂末端相对于基坐标的位姿是已知的。

由于关节2、关节3 和关节4 的旋转轴相互平行，所以坐标系O4的单位x轴、y轴和z轴在坐标系O1的y轴上的投影恒为［0， 0， 1］T，对应于的第二行前三个元素恒为［0， 0， 1］T。坐标系O4的原点在坐标系O1的y坐标恒为0，如的第四列第二行元素所示。逆运动学的求解过程主要是利用了这些几何特征。该机械臂逆运动学求解过程首先求解关节1、关节5和关节6的旋转角，再求解关节2、关节3和关节4的旋转角。

1.2.1 关节1、关节5和关节6的旋转角

根据式（9）和上述的正运动学公式，可以得到：

式（10）的左边为

其中，σ3=θ2+θ3+θ4+θ22-θ33-θ44。

式（10）的右边为

其 中 ，σ4=oycos(θ1)-oxsin(θ1)；σ5=oxcos(θ1)+oysin(θ1)；σ6=nycos(θ1)-nxsin(θ1)；σ7=nxcos(θ1)+nysin(θ1)；σ8=aycos(θ1)-axsin(θ1)；σ9=axcos(θ1)+aysin(θ1)。

式（10）左右两边第二行对应元素相等，可以得到：

根据式（11）的第一个等式，可以得到：

因此可以得到关节1旋转角θ1：

其中atan2是正切逆函数，根据输入的两个参数的符号确定输出角度的符号，θ1有两个解。

根据式（11）的第三项，可以得到关节5旋转角θ5：

根据式（11）的第四项，可以得到关节6旋转角θ6：

1.2.2 关节2、关节3和关节4的旋转角

已经求解得到θ1，θ5和θ6，在式（10）的左右两边同时右乘()-1：

因为θ1，θ5，θ6已知，所以式（16）右边的所有元素都已知。令代表的第一行第四列的元素。x，y的具体计算如下：

其 中 ，σ10=nxcos(θ1) cos(θ6)+nycos(θ6) sin(θ1)-oxcos(θ1) sin(θ6)-oysin(θ1) sin(θ6)；σ11=axcos(θ1)+aysin(θ1)。

根据式（9）和式（16）右边矩阵第四列第二行和第三行元素相等，可以得到：

将上面两个方程平方后相加得到：

因此可以得到关节3旋转角θ3：

根据式（17），令：s2=sin(θ2+θ22)；s3=sin(θ3-θ33)；c2=cos(θ2+θ22)；c3=cos(θ3-θ33)。

因此，式（17）可以转换成如下形式：

令：a=a2+a3c3；b=a3s3；c=-y；d=a1-x。

因此，式（19）可以转换成如下形式：

根据式（20）可以得到关节2旋转角θ2：

根据式（9）和式（16），可以得到：

因此，可以得到关节4旋转角θ4：

2 雅可比矩阵

机械臂的雅克比矩阵可将机械臂工作空间运动映射到关节空间运动［10］，如下式所示。

式中：̇——机械臂工作空间各个维度的运动速度，其一般是一个6维列向量，前3个元素代表移动速度，后3个元素代表旋转角速度̇ ——机械臂每个关节的速度；J——雅克比矩阵，其行数等于机械臂工作空间运动维度，一般等于6，列数等于机械臂关节数量。

本文中的水下电动机械臂有6 个关节，因此其雅克比矩阵是一个6×6的方阵。

当机械臂位于奇异位置时，雅克比矩阵的行列式等于零，因此雅克比矩阵可以用来判断机械臂是否位于奇异位置，并可用于奇异位置规避。下面将推导出该水下电动机械臂的雅克比矩阵，为机械臂的奇异性分析提供理论基础。

推导机械臂的雅克比矩阵需要采用两个递推公式［10］：

式中：——坐标系Oi的角速度；——坐标系Oi到坐标系Oi+1的旋转矩阵，其可以从式（2）～式（7）获得；——第i+1 个关节的角速度̂——坐标系Oi+1的z轴的单位向量［0， 0， 1］T——坐标系Oi的线速度——坐标系Oi+1的原点在坐标系Oi上的位置向量。

在已知̇的情况下，可以很容易写出雅克比矩阵J。注意，此时的雅克比矩阵是相对于机械臂的末端坐标系的。可以通过矩阵变换将雅克比矩阵转换到相对于基坐标系的。不过对于奇异位置分析，相对于末端坐标系和相对于基坐标系的雅克比矩阵得出的结果是一样的。

3 奇异性分析

机械臂在奇异位置时，逆运动学方程无解，并且机械臂工作空间内的一点微小变化可能会引起关节角度的剧烈变化，因此需要获得奇异位置的直观表达，避免机器人失控。

从该水下电动机械臂的逆运动学方程中，可以分析出该机械臂存在3个奇异位置：

1） 肩部奇异位置。根据式（12）、式（13）可知，当px-d6ax=0 时，θ1无法求解，所以该位置是一个奇异位置。

2） 肘部奇异位置。根据式（21）可知，当ac+bd=0时，θ2无法求解，此时关节2、关节3和关节4的旋转轴共面。

3） 腕部奇异位置。根据式（15）可知，当θ5=±90°时，θ6无法求解，导致机构失控。

可以对机械臂的雅克比矩阵进行奇异值分解［15］：

式中：U∈Rm×m，V∈Rn×n，Σ∈Rm×n，U和V为正交矩阵，n为关节数，m为机械臂工作空间运动维度；λi——雅克比矩阵第i个特征值，且λ1≥λ2≥…≥λm≥0。

可以采用条件数K来衡量机械臂的灵活性［16］，条件数越大，说明机械臂可操作性越弱，越接近奇异位置。条件数可采用下式计算：

4 仿真验证

4.1 运动学模型验证

目前已经获得水下电动机械臂的正、逆运动学解析解，为了验证所建立的运动学模型的准确性，本文通过文献［17］开发的Robotics Toolbox 建立该水下电动机械臂的仿真模型，如图3所示。Robotics Toolbox是一个免费开源的机器人工具箱。可以采用其中的fkine函数和ikine函数求解机械臂的正逆运动学参数。本文将通过几个测试例子来对比本文构建的运动学模型计算结果与机器人工具箱的运动学计算结果，从而验证本文的运动学模型的准确性。

图3 水下电动机械臂仿真模型Fig.3 Simulation model of the underwater electric manipulator

4.1.1 测试1

关节旋转角为［0， 0， 0， 0， 0， 0］。

4.1.1.1 正运动学

机器人工具箱计算的末端位姿为

本文的运动学模型计算的末端位姿为

4.1.1.2 逆运动学

将计算得到的末端位姿作为输入，机器人工具箱计算的关节旋转角为［0， 0， 0， 0， 0， 0］。

本文的运动学模型计算的关节旋转角为

4.1.2 测试2

关节旋转角为［10，20，30，40，50，60］。

4.1.2.1 正运动学

机器人工具箱计算的末端位姿为

本文的运动学模型计算的末端位姿为

4.1.2.2 逆运动学

机器人工具箱计算的关节旋转角为［10，20，30，40，50，60］。

本文的运动学模型计算的关节旋转角为［10，20，30，40，50，60］，［10.000 0， -31.749 8， 148.768 9，-27.019 0， 50.000 0， 60.000 0］，［-170.000 0， 25.604 4，38.229 1， -153.833 5， -50.000 0， -120.000 0］，［-170.000 0， -19.139 6， 140.539 8， 148.599 8，-50.000 0， -120.000 0］。

4.1.3 测试3

关节旋转角为［10，-20，30，-40，50，-60］。

4.1.3.1 正运动学

机器人工具箱计算的末端位姿为

本文的运动学模型计算的末端位姿为

4.1.3.2 逆运动学

机器人工具箱计算的关节旋转角为

本文的运动学模型计算的关节旋转角为［10.000 0，-20.000 0， 30.000 0， -40.000 0， 50.000 0， -60.000 0］，［10.000 0， -71.749 8， 148.768 9， -107.019 0， 50.000 0，-60.000 0］。

4.1.4 验证结论

通过对比可以看出，本文构建的水下机械臂运动学模型是准确的，并且逆运动学方程可以获得所有满足末端位姿的关节旋转角。图4 显示了基于所构建的运动学模型进行水下机械臂笛卡尔空间的轨迹规划，可以控制水下机械臂末端沿着图中的红色轨迹运动。

图4 笛卡尔路径规划Fig.4 Cartesian path planning

4.2 奇异位置规避验证

根据奇异位置分析可知，当关节5 旋转角为±90°时，机械臂处于奇异位置。图5显示了关节5旋转角从-80°变化到80°的范围内，机械臂条件数的变化情况，此时其他关节位置保持为0°。因为当关节5 旋转角等于±90°时，雅克比矩阵的最小特征值等于零，条件数为无穷大，所以没有显示在图5中。从图5可以看出关节5旋转角等于0°时，条件数最小，此时机械臂的可操作性最强。随着关节5旋转角增大或减小，条件数呈指数级的增长。这说明机械臂的可操作性随着关节5旋转角的增大或减少而迅速减弱，且随着关节5旋转角的增大或减小而迅速接近奇异位置。因此在实际控制机械臂运动过程中，可以采用条件数来判断机械臂当前关节状态是否接近奇异位置，从而有效规避奇异位置，保证机械臂操控的安全性。

图5 关节5 旋转角与条件数的关系Fig.5 Relationship between the rotation angle of Joint 5 and the condition number

5 结束语

针对水下电动机械臂运动学模型与结构强相关的问题以及奇异位置规避问题，本文采用改进的D-H参数法建立了一种水下六自由度电动机械臂的运动学模型，推导了机械臂的正、逆运动学方程；求得了机械臂的雅可比矩阵，并基于雅可比矩阵详细分析了水下电动机械臂的奇异位置，采用条件数判断机械臂当前状态是否接近奇异位置。仿真实验结果表明本文建立的运动学模型可以根据关节位置精确计算机械臂末端位姿，也可以根据末端位姿反解机械臂关节位置；且当机械臂接近奇异位置时，条件数会呈指数级增加，因此可以采用条件数进行奇异位置规避。

目前本文只通过仿真的方法来验证所建立的水下电动机械臂运动学模型和奇异位置规避方法，后续将在真实的水下电动机械臂上进行更充分全面的验证，从而保障水下电动机械臂作业的安全性。