基于旋量和自运动的七自由度机械臂逆解算法

吴 亮，李宪华，2

(1.安徽理工大学机械工程学院 安徽 淮南 232001； 2.安徽理工大学人工智能学院 安徽 淮南 232001)

0 引言

传统串联机械臂通常采用D-H参数法[1]和旋量法[2]来建立机器人运动学模型。D-H参数法需要在每个连杆处建立坐标系，然后，得出每一个连杆坐标系相对于前一连杆坐标系的位姿转换矩阵，即得到机械臂运动学方程。D-H参数法相对于旋量法更加复杂，但目前用来分析机械臂正逆运动学十分成熟，且应用广泛[3,4]。而采用旋量法建立机器人运动学模型，只需要建立基坐标系和末端执行器坐标系，通过确定各个连杆之间的旋转或平移运动，得到机械臂末端坐标系相对于基坐标系的运动关系，旋量法相对于D-H参数法更加简明清晰，便于理解，在机器人运动学建模中使用旋量法更加方便[5,6]。

相较于传统六自由度机械臂而言，七自由度机械手臂有着显著的优点：更高的工作性能、更广泛的应用空间，因自由度数目的增加使得机械臂的操作性能有了一个质的飞跃。但是也因为比六自由度多出一个冗余关节，而增加了计算逆解的难度，七自由度机械臂肘部会产生自运动,其轨迹为圆，由自运动产生的运动变量被定义为臂角[7]，作为求取逆解的一个关键参数，对于臂角的研究至关重要。文献[8]将冗余关节角作为参数，用解析法求得七自由度机械臂逆解，然而该方法依赖于机械臂构型，如果冗余角选择不合适将导致计算十分复杂。文献[9]和文献[10]分别采用神经网络和遗传算法计算七自由度冗余机械臂逆解的解析解，然而，两种算法都很复杂，并且需要大量的计算，同时难以对机械臂进行实时控制。文献[11]通过固定七自由度机械臂中的3个关节角度将其转换四自由度，并结合代数迭代法和位姿分离法两种方法求解逆解，简化了逆解的求解过程，但由于固定了关节角度使求出的逆解不够准确。

本文以七自由度模块化机械臂为研究对象，提出一种七自由度机械臂逆解算法。首先，基于SolidWorks软件建立七自由度模块化机械臂模型，采用旋量法建立机械臂正运动学方程；然后，采用矢量法和位姿分离法两者结合求解机械臂逆解；最后，在MATLAB软件编制逆解算法程序，得出机械臂八组逆解，将八组逆解代入正运动学方程，并在Robotics Toolbox中进行仿真，便可得到八组逆解的机械臂末端位姿。仿真结果验证了运动学建模的正确性，使机械臂逆解求解更加简明，为后续机械臂动力学研究和轨迹规划提供研究基础。

1 机器人运动学模型建立

1.1 旋量理论

(1)

刚体的旋量运动可采用矩阵指数形式来表示，则有

(2)

(1-cosθ)

将坐标系T固定到机械臂末端上，则坐标系T相对于基坐标系S的位姿可由gst表示，初始状态下位姿为gst(0)，当各关节转动θi后的位姿为

(3)

1.2 机械臂运动学建模

该七自由度模块化机械臂有7个转动关节，并且后3个关节轴线交于一点(如图1)。根据该机械臂构型特点，及该机械臂某一位姿建立基坐标系S和固定在末端执行器坐标系T，如图2所示，其中d1=0.3 m，d3=0.328 m，d5=0.2765 m，d7=0.2916 m。

图1 七自由度模块化机械臂模型 图2 初始姿态机械臂构型及坐标系建立

机械臂运动学方程建立步骤为：

(1)在初始位姿下，机械臂末端执行器坐标系T相对于基坐标系S的位姿矩阵为

(4)

(2)由图2可知，各关节轴线的单位方向向量为

(3)取各关节轴线上一点q，

(4)基于旋量理论计算机器人运动学正解，结合式(1)至(3)即可得机械臂运动学正解为

(5)

图3 机械臂模型简图

2 七自由度机械臂逆运动学求解

(6)

(7)

同时，可以通过给定一个臂角φ值，求得E点位置。

(8)

(1)关节角θ4的求解

在ΔESW中，根据余弦定理有，可求出

(9)

式中：LSE=d3，LEW=d5。再由∠SEW与θ4的关系可以求出：θ4=±[π-∠ESW]。

(2)关节角θ2的求解

由图3可知，θ2可由以下式(10)求出

(10)

(3)关节角θ1的求解

(11)

(4)关节θ3的求解

θ3的求解与θ1是类似的，在关节2建立坐标系o2-x2y2z2，如图2所示，θ3可由式(12)求出

(12)

(5)关节角θ5、θ6、θ7的求解

已知θ1、θ2、θ3、θ4，根据旋量理论公式(3)，根据等式相等原则可以求出

θ5=arcsin(R1/s6)

(13)

θ6=±arccos(R2)

(14)

θ7=arcsin(R3/s6)

(15)

式中

R1=-r13(s3c2c1+s1c3)+r23(c1c3-s1s3c2)+s2s3r33/c2

R2=r13(s4c3c2c1+s2c1-s1s3s4)+r23(s1s4c2c3+s1s2+c1c3s4)+r33(c2-s2s4c3)/c2

R3=r12(s4c3c2c1+s2c1-s1s3s4)+r22(s1s4c2c3+s1s2+c1c3s4)+r32(c2-s2s4c3)/c2

3 逆解算法验证

以上述正逆运动学分析为基础，在MATLAB软件中编制逆解算法，用来求解机械臂逆解。由于臂角的确定要结合避障来讨论，该机械臂运动分析暂不涉及避障，所以为求解逆解时，臂角用随机的值确定。

在机器人工作空间范围内，任意给出7个关节角度，这里取θ1=π/10，θ2=π/3，θ3=π/6，θ4=π/3，θ5=π/12，θ6=π/4，θ7=π/3，并给定臂角为φ=π/6。

(1)将各关节角度带入式(3)，即可求得机械臂末端位姿矩阵为

表1 机械臂8组逆解

(2)利用本文提出的逆解算法，共得到 8 组逆运动学的解，如表1所示。

图4 机械臂8组逆解位姿图

将上述求得的8组逆解带入正运动学方程式(3)中，并在MATLAB Robotics Toolbox中进行仿真，设置时间t=1 s，便可得到机械臂末端位姿和末端轨迹线，如图4所示。

通过表格1和图4可以发现，第一组解就是仿真前给出的7个关节角度，并且8幅图的末端执行器位姿与正运动学方程所得到的位姿是相同的，但是8幅位姿图的末端轨迹线却有所不同，表明机械臂可以通过8组逆解角度反求机械臂末端位姿，也证明了本文所提出的正逆运动学建模和逆解算法的正确性。本文所提出的逆解算法意义鲜明，运算速度快，准确性高，可以用于七自由度冗余机械臂的实时控制。

4 结论

本文通过旋量法和自运动对七自由度机械臂逆解进行分析，得到以下结论。

(1)以七自由度模块化机械臂为研究对象，基于SolidWorks建立七自由度模块化机械臂模型，采用旋量理论建立机械臂正运动学方程。

(2)基于矢量法和位姿分离法两者结合求解机械臂逆解，在MATLAB软件中编写逆解算法程序，得到8组逆解，将8组逆解代入正运动学方程并结合MATLAB Robotics Toolbox仿真验证，得出末端执行器位姿与正运动学所求位姿相符合。

(3)该逆解算法能够准确清晰表达八组逆解的几何意义，降低了运算过程，使机械臂逆解求解更加简明。MATLAB Robotics Toolbox仿真结果验证了逆解算法的正确性，为后续机械臂动力学研究和轨迹规划提供研究基础。