CN107589934A - 一种关节型机械臂逆运动学解析解的求取方法 - Google Patents

Abstract

本发明一种关节型机械臂逆运动学解析解求取方法属于工业机器人制造技术领域，涉及工业机器人领域快速获取一种肩关节朝前偏置的关节型六自由度机械臂的唯一逆运动学解析解求解方法。该方法按照D‑H参数法建立关节机械臂连杆坐标系,确定机械臂相邻关节之间的4个结构几何参数，计算相邻两坐标系的齐次坐标变换矩阵，并求其逆矩阵；求解机械臂六个关节的旋转角度表达式，利用旋转角度表达式进行逆运动学求解。该方法坐标系建模简单易懂，并且解析解能够保证该类机械臂逆运动学解要求。方法具有求解精度高、求解速度快、求解过程更加简单明了的特点。

Description

一种关节型机械臂逆运动学解析解的求取方法

技术领域

本发明属于工业机器人制造技术领域，涉及工业机器人领域快速获取一种肩关节朝前偏置的关节型六自由度机械臂的唯一逆运动学解析解求解方法。

背景技术

关节型机械臂的逆向运动学问题是在给定机械臂末端执行器坐标系相对于基坐标系的位置和姿态，以及所有机械臂连杆几何参数的情况下，求取所有机械臂关节转动的角度值，是正向运动学的逆过程。正向运动学根据机械臂的结构几何参数可以得到前后相邻关节坐标系之间的齐次坐标变换矩阵T，也即两连杆之间的位姿矩阵；若已知每个关节转动角度θ,通过将各齐次变换矩阵T依次连续右乘即可得到机械臂末端执行器的位置和姿态，所得结果唯一。而逆向运动学的求解则相对复杂，且可能具有无解或多解情况，比如末端执行器在奇异点处时无解，而反三角函数的周期性导致机械臂关节旋转角度解析解理论上的多解性。

市场上的大多数关节型六自由度机械臂的腕部都是满足Pieper准则的，即腕部相邻的三个关节旋转轴相交于一点，则腕部相邻的三个关节是解耦的。目前，机器人领域通用的坐标系建模方法是Denavit-Hartenberg参数法,简称D-H参数法，即一种为机械臂关节链中的每一个杆件建立坐标系的矩阵方法，该方法描述了相邻连杆之间的坐标方向和几何参数，直观明了。现有的逆运动学求解方法大多数都是利用数值法或几何法单独实现。例如，陶茂生与韩峰涛公开的“用于六自由度的工业机器人的逆运动学求解方法”，专利号CN105573143A，需要根据求出的六个关节旋转轴转动角度对应的八组结果，解离上一关节空间所处位置对应的各个关节旋转轴角度差值范数总和最小的解，其计算精度与计算速度不能满足现代生产的要求；朱齐丹等人公开的“一类六自由度机械臂运动学逆解的快速简便求法”，专利号CN103942427A，采用欧拉角变换矩阵进行求解，没有避免该方法的万向节锁问题。此外，采用神经网络、专家系统、模糊逻辑及遗传算法等智能算法进行逆运动学求解时，程序复杂需要高性能的计算配置，且存在稳定性不足的问题。

发明内容

本发明目的在于克服现有机械臂的逆运动学求解存在的缺陷及不足：通用求解方法坐标系建模方法单一，求解过程复杂难懂，存在多解且求解速度慢。发明一种关节型机械臂逆运动学解析解的求取方法，用于快速获取肩关节朝前偏置的关节型六自由度机械臂的唯一逆运动学求解。方法按照D-H参数法建立机械臂连杆坐标系，确定机械臂相邻关节之间的4个结构几何参数，计算相邻两坐标系的齐次坐标变换矩阵，并求其逆矩阵；求解机械臂六个关节的旋转角度表达式，利用旋转角度表达式进行逆运动学求解。该方法求解精度高、求解速度快、求解过程更加简单易懂。

本发明采用的技术方案是一种关节型机械臂逆运动学解析解的求取方法，其特征是，该方法按照D-H参数法建立关节型机械臂连杆坐标系,确定机械臂相邻关节之间的4个结构几何参数，计算相邻两坐标系的齐次坐标变换矩阵，并求其逆矩阵；求解机械臂六个关节的旋转角度表达式，利用旋转角度表达式进行逆运动学求解；方法具体步骤如下：

步骤一，关节型机械臂由基座A、末端执行器G、5个连杆B、C、D、E、F和6个旋转关节1、2、3、4、5、6组成；按照D-H参数法建立机械臂连杆坐标系，坐标系包括：XOZ平面，机械臂六个旋转关节对应坐标系O₀～O₅以及机械臂末端执行器的坐标系O₆；各关节坐标系具体为：z_i轴沿i+1关节的轴线，x_i沿z_i轴和z_i-1轴的公垂线，指向背离z_i-1轴方向，y_i轴由右手直角坐标系规则确定，其中i＝1,2,3,4,5,6；将第一个关节坐标系的初始位置设置在机械臂的基座上与基坐标系{O₀:x₀,y₀,z₀}重合，基坐标系始终保持不变；

步骤二，根据机械臂相邻关节之间的4个结构几何参数：连杆转角θ_i、连杆扭角α_i、连杆长度a_i、连杆距离d_i计算相邻两坐标系的齐次坐标变换矩阵^i-1T_i,i＝1,2,...6；各几何参数的定义：相邻两连杆之间的连杆转角θ_i为x_i轴与x_i-1轴之间的夹角，绕z_i-1轴从x_i-1轴到x_i轴，符合右手规则时为正，对于转动关节，θ_i为变量；连杆扭角α_i为z_i轴与z_i-1轴之间的夹角，绕x_i轴从z_i-1轴到z_i轴，符合右手规则时为正，当两关节轴线平行时，α_i＝0，当两关节轴线垂直时，α_i＝-90°或90°；连杆长度a_i为z_i轴与z_i-1轴的公垂线长度，沿x_i轴方向测量，当两关节轴线平行时，a_i＝l_i，l_i为连杆的长度，当两关节轴线垂直时，a_i＝0；相邻两连杆之间的连杆距离d_i为x_i轴与x_i-1轴之间的距离，在z_i-1轴上测量，对于转动关节，d_i为常数；

按照连杆坐标系之间的齐次变换规则计算出相邻坐标系的各齐次变换矩阵^i-1T_i；机械臂的相邻关节坐标系间的齐次坐标变换矩阵^i-1T_i满足公式(1)：

⁰T₆＝⁰T₁ ¹T₂ ²T₃ ³T₄ ⁴T₅ ⁵T₆ (1)

其中，等式左边矩阵为末端执行器的坐标系相对于基坐标系的齐次坐标变换矩阵，为已知条件；其中，n_x,n_y,n_z分别为末端执行器坐标系{O₆:x₆,y₆,z₆}的x₆轴与基坐标系的x₀,y₀,z₀轴的夹角余弦值；o_x,o_y,o_z分别为末端执行器坐标系的y₆轴与基坐标系的x₀,y₀,z₀轴的夹角余弦值；a_x,a_y,a_z分别为末端执行器坐标系的z₆轴与基坐标系的x₀,y₀,z₀轴的夹角余弦值；p_x,p_y,p_z为末端执行器坐标系原点O₆在基坐标系中的笛卡尔坐标；

等式右边矩阵：⁰T₁、¹T₂、²T₃、³T₄、⁴T₅、⁵T₆—分别为第一、第二、第三、第四、第五、末端执行器关节坐标系相对于基坐标系的齐次坐标变换矩阵；考虑末端执行器工具长度d₆，末端执行器坐标系与第六个关节的坐标系之间的变换是：沿z₅平移d₆距离之后绕z₅旋转θ₆；

步骤三，计算机械臂的各关节旋转角度理论表达式θ_i，

首先在等式(1)的两边同时左乘矩阵再在等式两边同时右乘矩阵得：

根据矩阵相等的定义，由等式(2)两边矩阵的第3行第4列的元素相等建立方程:

p_ycosθ₁-p_xsinθ₁+a_xd₆sinθ₁-a_yd₆cosθ₁＝0 (3)

由式(3)解得θ₁，θ₁由双参数反正切函数表示的理论表达式：

θ₁＝arctan2(p_x-a_xd₆,p_y-a_yd₆)+180·N₁ (4)

其中，N₁—由三角函数周期性产生的整数；d₆—连杆5与末端执行器之间的距离；a_x,a_y,p_x,p_y—末端执行器坐标系在基坐标系中的位置与姿态的参数；

然后，由等式(2)两边的矩阵的第1行第4列元素和第2行第4列元素分别相等，建立方程组①②：

①：

②：

将两方程左右两边同时平方再相加，消去θ₂得到由双参数反正切函数表示的θ₃理论表达式：

其中，k为中间代换参数，

N₃—由三角函数周期性产生的整数；

将方程组①②两边分别相加，由于θ₃已知，得到由双参数反正切函数表示的θ₂理论表达式：

其中，m，n，v均为中间代换参数；m＝d₁-p_z+a_zd₆；

n＝p_xcosθ₁+p_ysinθ₁-a_xd₆cosθ₁-a_yd₆sinθ₁-a₁；

v＝(a₃+d₄)cosθ₃+(a₃-d₄)sinθ₃+a₂；

N₂—由三角函数周期性产生的整数；

在等式(1)的两边同时左乘矩阵得到：

由等式(7)两边矩阵的第3行第3列元素相等，有方程：

由(8)式计算得到由双参数反正切函数表示的θ₄理论表达式：

其中，N₄—由三角函数周期性产生的整数；

由等式(7)两边矩阵的第1行第3列元素与第2行第3列元素分别相等，有方程组③④：

③：

④：

由方程组③④计算得到由双参数反正切函数表示的θ₅理论表达式：

其中，N₅—由三角函数周期性产生的整数；

由等式(7)两边矩阵的第3行第1列元素与第3行第2列元素分别相等，有方程组⑤⑥：

⑤：

⑥：

由方程组⑤⑥计算得到由双参数反正切函数表示的θ₆理论表达式：

其中，N₆—由三角函数周期性产生的整数；

步骤四，针对所述机械臂各关节的旋转角度表达式的可能多解结果，根据机械臂的实际工作范围，给定一组各个关节的转动角度值θ_i1，由正运动学解的唯一性，得到机械臂第六个关节的坐标系相对于基坐标系的由位置和姿态组成的位姿矩阵⁰T₆，将此矩阵作为已知条件，利用所述旋转角度理论表达式θ_i进行逆运动学求解得到θ_i2；

步骤五，比较给定的转动角度值θ_i1与逆运动学求解得到θ_i2值是否满足计算精度要求，若满足计算结束；若相差较大，利用所求θ₁～θ₆理论表达式进行逆运动学求解；通过调整θ₂、θ₃角度表达式中根号前面的正负号以及各表达式是否因反三角函数的周期性需要加180°·N_i，N_i为整数，使得由所述旋转角度表达式计算出的各旋转角度值与正向运动的输入相等，即逆运动学求解正确，从而确定θ₂、θ₃关节角度表达式中的正负号和所有表达式中的N_i值，即得到唯一的一组关节旋转角度表达式。

本发明的有益效果是该方法用于快速获取肩关节朝前偏置的关节型六自由度机械臂的唯一逆运动学求解，对该类构型的机械臂，坐标系建模方法简单易懂，并且解析解能够保证该类机械臂逆运动学解要求。该方法中提出的逆运动学求解过程更加简单明了，求解速度快且计算量小，求解精度高。

附图说明

图1—关节型六自由度机械臂的结构图。机械臂由基座、末端执行器与5个连杆、6个旋转关节组成；其中，A-基座，B-连杆1，C-连杆2，D-连杆3，E-连杆4，F-连杆5，G-末端执行器，a₁-连杆1的长度，a₂-连杆2的长度，a₃-连杆3的长度，d₁-基座与连杆1的距离，d₄-连杆3与连杆4的距离，d₆-连杆5与末端执行器之间的距离。

图2—关节型六自由度机械臂的结构简图及各关节坐标系。其中，1-关节1,2-关节2,3-关节3,4-关节4,5-关节5,6-关节6。各坐标系的确定采取D-H参数法，各关节均绕着该关节坐标系的z轴旋转，z_i轴沿第i+1关节的轴线，x_i沿z_i轴和z_i-1轴的公垂线，指向背离z_i-1轴的方向，y_i轴由右手直角坐标系规则确定，O_i为第i个坐标系的原点；将第一个关节坐标系的初始位置设置在机械臂的基座上与基坐标系{O₀:x₀,y₀,z₀}重合，基坐标系始终保持不变。

图3—关节型机械臂逆运动学解析解的求取方法流程图。

具体实施方式

下面结合附图和技术方案详细叙述本发明的具体实施方式。

图1是关节型六自由度机械臂机构简图，机械臂各关节坐标系如图2所示，机械臂由基座A、末端执行器G、5个连杆B、C、D、E、F和6个旋转关节1、2、3、4、5、6组成。附图3是本发明关节型机械臂逆运动学解析解的求取方法流程图，方法流程的具体步骤如下：

步骤一,借鉴D-H参数法，建立机械臂连杆坐标系,如说明书附图2所示。具体为：z_i轴沿i+1关节的轴线，x_i沿z_i轴和z_i-1轴的公垂线，指向背离z_i-1轴方向，y_i轴由右手直角坐标系规则确定，其中i＝1,2,3,4,5,6；将第一个关节坐标系的初始位置设置在机械臂的基座上与基坐标系{O₀:x₀,y₀,z₀}重合，基坐标系始终保持不变。

步骤二，根据机械臂相邻关节之间的4个结构几何参数：连杆转角θ_i、连杆扭角α_i、连杆长度a_i、连杆距离d_i。本实施例中，机械臂各连杆的D-H参数为：

i＝1时，连杆转角为θ₁，连杆距离为d₁，连杆长度为a₁，连杆扭角为-90°；

i＝2时，连杆转角为θ₂，连杆距离为0，连杆长度为a₂，连杆扭角为0°；

i＝3时，连杆转角为θ₃，连杆距离为0，连杆长度为a₃，连杆扭角为-90°；

i＝4时，连杆转角为θ₄，连杆距离为d₄，连杆长度为0，连杆扭角为90°；

i＝5时，连杆转角为θ₅，连杆距离为0，连杆长度为0，连杆扭角为-90°；

i＝6时，连杆转角为θ₆，连杆距离为d₆，连杆长度为0，连杆扭角为0°。

在本实施例中，d₆包含工具长度。

计算相邻两坐标系的齐次坐标变换矩阵^i-1T_i,i＝1,2,...6；机械臂的相邻关节坐标系间的齐次坐标变换矩阵^i-1T_i满足公式(1)，在本实施例中各变换矩阵如下：

步骤三，按公式(4)、(5)、(6)、(9)依次计算机械臂的第一个关节的旋转角度表达式θ₁，第三个关节的旋转角度表达式θ₃，第二个关节的旋转角度表达式θ₂，第四个关节的旋转角度表达式θ₄；在依次求出θ₁、θ₃、θ₂、θ₄的表达式后，第五个关节的旋转角度表达式θ₅和第六个关节的旋转角度表达式θ₆公式(10)、(11)求出。

步骤四，根据机械臂的实际工作范围，本实施例给定一组各关节的转动角度值θ_i1为：

θ₁₁＝30°,θ₃₁＝40°,θ₂₁＝20°,θ₄₁＝60°,θ₅₁＝80°,θ₆₁＝70°

求出实施例中各结构的参数值如下：

a₁＝160,a₂＝575,a₃＝130；

d₁＝440,d₄＝645,d₆＝500；

由正运动学解的唯一性，得到机械臂末端执行器的坐标系相对于基坐标系的由位置和姿态组成的位姿矩阵：

步骤五，利用所求θ₁～θ₆理论表达式进行逆运动学求解。根据旋转角度表达式计算出的各旋转角度值与正向运动的输入相等为条件，调整θ₂、θ₃角度表达式中根号前面的正负号以及判定各表达式是否需加180°·N_i,N_i为整数。当θ₃的表达式中根号前取正号，θ₂的表达式中根号前取负号，同时取

N₁＝N₃＝N₄＝N₅＝N₆＝0,N₂＝-1

得到结果如下：

θ₁₂＝30°,θ₃₂＝40°,θ₂₂＝20.0000°,θ₄₂＝60°,θ₅₂＝80°,θ₆₂＝69.9999°

由此得到唯一的一组关节旋转角度表达式，计算结果精度满足计算要求，利用已确定的表达式再进行几组正逆运算，结果同样满足精度要求。

本发明实施例的关节型六自由度机械臂的逆运动学求解方法，对此类构型的机械臂关节坐标系建模方法简单易懂，能够迅速获得唯一解，并且保证求解精度。

Claims (1)

1.一种关节型机械臂逆运动学解析解的求取方法，其特征是，该方法按照D-H参数法建立关节型机械臂连杆坐标系,确定机械臂相邻关节之间的4个结构几何参数，计算相邻两坐标系的齐次坐标变换矩阵，并求其逆矩阵；求解机械臂六个关节的旋转角度表达式，利用旋转角度表达式进行逆运动学求解；方法具体步骤如下：

步骤一，关节型机械臂由基座(A)、末端执行器(G)、5个连杆(B、C、D、E、F)和6个旋转关节(1、2、3、4、5、6)组成；按照D-H参数法建立机械臂连杆坐标系，坐标系包括：XOZ平面，机械臂六个旋转关节对应坐标系O₀～O₅以及机械臂末端执行器的坐标系O₆；各关节坐标系具体为：z_i轴沿i+1关节的轴线，x_i沿z_i轴和z_i-1轴的公垂线，指向背离z_i-1轴方向，y_i轴由右手直角坐标系规则确定，其中i＝1,2,3,4,5,6；将第一个关节坐标系的初始位置设置在机械臂的基座上与基坐标系{O₀:x₀,y₀,z₀}重合，基坐标系始终保持不变；

步骤二，根据机械臂相邻关节之间的4个结构几何参数：连杆转角θ_i、连杆扭角α_i、连杆长度a_i、连杆距离d_i计算相邻两坐标系的齐次坐标变换矩阵^i-1T_i,i＝1,2,...6；各几何参数的定义：相邻两连杆之间的连杆转角θ_i为x_i轴与x_i-1轴之间的夹角，绕z_i-1轴从x_i-1轴到x_i轴，符合右手规则时为正，对于转动关节，θ_i为变量；连杆扭角α_i为z_i轴与z_i-1轴之间的夹角，绕x_i轴从z_i-1轴到z_i轴，符合右手规则时为正，当两关节轴线平行时，α_i＝0，当两关节轴线垂直时，α_i＝-90°或90°；连杆长度a_i为z_i轴与z_i-1轴的公垂线长度，沿x_i轴方向测量，当两关节轴线平行时，a_i＝l_i，l_i为连杆的长度，当两关节轴线垂直时，a_i＝0；相邻两连杆之间的连杆距离d_i为x_i轴与x_i-1轴之间的距离，在z_i-1轴上测量，对于转动关节，d_i为常数；

按照连杆坐标系之间的齐次变换规则计算出相邻坐标系的各齐次变换矩阵^i-1T_i；机械臂的相邻关节坐标系间的齐次坐标变换矩阵^i-1T_i满足公式(1)：

⁰T₆＝⁰T₁ ¹T₂ ²T₃ ³T₄ ⁴T₅ ⁵T₆ (1)

其中，等式左边矩阵为末端执行器的坐标系相对于基坐标系的齐次坐标变换矩阵，为已知条件；其中，n_x,n_y,n_z分别为末端执行器坐标系{O₆:x₆,y₆,z₆}的x₆轴与基坐标系的x₀,y₀,z₀轴的夹角余弦值；o_x,o_y,o_z分别为末端执行器坐标系的y₆轴与基坐标系的x₀,y₀,z₀轴的夹角余弦值；a_x,a_y,a_z分别为末端执行器坐标系的z₆轴与基坐标系的x₀,y₀,z₀轴的夹角余弦值；p_x,p_y,p_z为末端执行器坐标系原点O₆在基坐标系中的笛卡尔坐标；

等式右边矩阵：⁰T₁、¹T₂、²T₃、³T₄、⁴T₅、⁵T₆—分别为第一、第二、第三、第四、第五、末端执行器关节坐标系相对于基坐标系的齐次坐标变换矩阵；考虑末端执行器工具长度d₆，末端执行器坐标系与第六个关节的坐标系之间的变换是：沿z₅平移d₆距离之后绕z₅旋转θ₆；

步骤三，计算机械臂的各关节旋转角度理论表达式θ_i，

首先在等式(1)的两边同时左乘矩阵再在等式两边同时右乘矩阵得：

<mrow> <mmultiscripts> <mi>T</mi> <mn>2</mn> <mrow> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </mmultiscripts> <mmultiscripts> <mi>T</mi> <mn>1</mn> <mrow> <mo>-</mo> <mn>1</mn> </mrow> <mn>0</mn> </mmultiscripts> <msub> <mmultiscripts> <mi>T</mi> <mn>0</mn> </mmultiscripts> <mn>6</mn> </msub> <mmultiscripts> <mi>T</mi> <mn>6</mn> <mrow> <mo>-</mo> <mn>1</mn> </mrow> <mn>5</mn> </mmultiscripts> <mo>=</mo> <msub> <mmultiscripts> <mi>T</mi> <mn>2</mn> </mmultiscripts> <mn>3</mn> </msub> <msub> <mmultiscripts> <mi>T</mi> <mn>3</mn> </mmultiscripts> <mn>4</mn> </msub> <msub> <mmultiscripts> <mi>T</mi> <mn>4</mn> </mmultiscripts> <mn>5</mn> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>

根据矩阵相等的定义，由等式(2)两边矩阵的第3行第4列的元素相等建立方程:

p_ycosθ₁-p_xsinθ₁+a_xd₆sinθ₁-a_yd₆cosθ₁＝0 (3)

由式(3)解得θ₁，θ₁由双参数反正切函数表示的理论表达式：

θ₁＝arctan2(p_x-a_xd₆,p_y-a_yd₆)+180·N₁ (4)

其中，N₁—由三角函数周期性产生的整数；d₆—连杆5与末端执行器之间的距离；a_x,a_y,p_x,p_y—末端执行器坐标系在基坐标系中的位置与姿态的参数；

然后，由等式(2)两边的矩阵的第1行第4列元素和第2行第4列元素分别相等，建立方程组①②：

①：

②：

将两方程左右两边同时平方再相加，消去θ₂得到由双参数反正切函数表示的θ₃理论表达式：

<mrow> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>=</mo> <mi>arctan</mi> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>a</mi> <mn>3</mn> </msub> <mo>,</mo> <msub> <mi>d</mi> <mn>4</mn> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mi>arctan</mi> <mn>2</mn> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mo>&amp;PlusMinus;</mo> <msqrt> <mrow> <msubsup> <mi>a</mi> <mn>3</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>d</mi> <mn>4</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msup> <mi>k</mi> <mn>2</mn> </msup> </mrow> </msqrt> <mo>)</mo> </mrow> <mo>+</mo> <mn>180</mn> <mo>&amp;CenterDot;</mo> <msub> <mi>N</mi> <mn>3</mn> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>

其中，k为中间代换参数，

N₃—由三角函数周期性产生的整数；

将方程组①②两边分别相加，由于θ₃已知，得到由双参数反正切函数表示的θ₂理论表达式：

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>=</mo> <mi>arctan</mi> <mn>2</mn> <mrow> <mo>(</mo> <mi>n</mi> <mo>+</mo> <mi>m</mi> <mo>,</mo> <mi>n</mi> <mo>-</mo> <mi>m</mi> <mo>)</mo> </mrow> <mo>-</mo> <mi>arctan</mi> <mn>2</mn> <mrow> <mo>(</mo> <mi>v</mi> <mo>,</mo> <mo>&amp;PlusMinus;</mo> <msqrt> <mrow> <msup> <mrow> <mo>(</mo> <mi>n</mi> <mo>+</mo> <mi>m</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mi>m</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <msup> <mi>v</mi> <mn>2</mn> </msup> </mrow> </msqrt> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>+</mo> <mn>180</mn> <mo>&amp;CenterDot;</mo> <msub> <mi>N</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>

其中，m，n，v均为中间代换参数；m＝d₁-p_z+a_zd₆；

n＝p_xcosθ₁+p_ysinθ₁-a_xd₆cosθ₁-a_yd₆sinθ₁-a₁；

v＝(a₃+d₄)cosθ₃+(a₃-d₄)sinθ₃+a₂；

N₂—由三角函数周期性产生的整数；

在等式(1)的两边同时左乘矩阵得到：

<mrow> <mmultiscripts> <mi>T</mi> <mn>4</mn> <mrow> <mo>-</mo> <mn>1</mn> </mrow> <mn>3</mn> </mmultiscripts> <mmultiscripts> <mi>T</mi> <mn>3</mn> <mrow> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </mmultiscripts> <mmultiscripts> <mi>T</mi> <mn>2</mn> <mrow> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </mmultiscripts> <mmultiscripts> <mi>T</mi> <mn>1</mn> <mrow> <mo>-</mo> <mn>1</mn> </mrow> <mn>0</mn> </mmultiscripts> <mmultiscripts> <mi>T</mi> <mn>6</mn> <mn>0</mn> <mn>0</mn> </mmultiscripts> <mo>=</mo> <msub> <mmultiscripts> <mi>T</mi> <mn>4</mn> </mmultiscripts> <mn>5</mn> </msub> <msub> <mmultiscripts> <mi>T</mi> <mn>5</mn> </mmultiscripts> <mn>6</mn> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow>

由等式(7)两边矩阵的第3行第3列元素相等，有方程：

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>a</mi> <mi>y</mi> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>1</mn> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>4</mn> </msub> <mo>-</mo> <msub> <mi>a</mi> <mi>x</mi> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>4</mn> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>1</mn> </msub> <mo>-</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>a</mi> <mi>z</mi> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>2</mn> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>3</mn> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>4</mn> </msub> <mo>-</mo> <msub> <mi>a</mi> <mi>z</mi> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>2</mn> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>3</mn> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>4</mn> </msub> <mo>+</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>a</mi> <mi>x</mi> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>2</mn> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>1</mn> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>3</mn> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>4</mn> </msub> <mo>+</mo> <msub> <mi>a</mi> <mi>y</mi> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>2</mn> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>3</mn> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>1</mn> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>4</mn> </msub> <mo>-</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>a</mi> <mi>x</mi> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>2</mn> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>1</mn> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>3</mn> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>4</mn> </msub> <mo>-</mo> <msub> <mi>a</mi> <mi>y</mi> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>2</mn> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>1</mn> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>3</mn> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>4</mn> </msub> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>

由(8)式计算得到由双参数反正切函数表示的θ₄理论表达式：

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;theta;</mi> <mn>4</mn> </msub> <mo>=</mo> <mi>arctan</mi> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>a</mi> <mi>y</mi> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>a</mi> <mi>x</mi> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>1</mn> </msub> <mo>,</mo> <msub> <mi>a</mi> <mi>z</mi> </msub> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mo>(</mo> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mrow> <mo>(</mo> <msub> <mi>a</mi> <mi>x</mi> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>a</mi> <mi>y</mi> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mi>cos</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> <mo>)</mo> <mo>+</mo> <mn>180</mn> <mo>&amp;CenterDot;</mo> <msub> <mi>N</mi> <mn>4</mn> </msub> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>

其中，N₄—由三角函数周期性产生的整数；

由等式(7)两边矩阵的第1行第3列元素与第2行第3列元素分别相等，有方程组③④：

③：

④：

由方程组③④计算得到由双参数反正切函数表示的θ₅理论表达式：

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;theta;</mi> <mn>5</mn> </msub> <mo>=</mo> <mi>arctan</mi> <mn>2</mn> <mrow> <mo>(</mo> <mo>(</mo> <msub> <mi>a</mi> <mi>x</mi> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>a</mi> <mi>y</mi> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <msub> <mi>sin&amp;theta;</mi> <mn>4</mn> </msub> <mo>-</mo> <msub> <mi>a</mi> <mi>z</mi> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>4</mn> </msub> <mi>sin</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>a</mi> <mi>x</mi> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>a</mi> <mi>y</mi> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <msub> <mi>cos&amp;theta;</mi> <mn>4</mn> </msub> <mi>cos</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> <mo>,</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <msub> <mi>a</mi> <mi>x</mi> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>a</mi> <mi>y</mi> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>1</mn> </msub> <mo>)</mo> <mi>sin</mi> <mo>(</mo> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>)</mo> <mo>+</mo> <msub> <mi>a</mi> <mi>z</mi> </msub> <mi>cos</mi> <mo>(</mo> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>)</mo> <mo>)</mo> <mo>+</mo> <mn>180</mn> <mo>&amp;CenterDot;</mo> <msub> <mi>N</mi> <mn>5</mn> </msub> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>

其中，N₅—由三角函数周期性产生的整数；

由等式(7)两边矩阵的第3行第1列元素与第3行第2列元素分别相等，有方程组⑤⑥：

⑤：

⑥：

由方程组⑤⑥计算得到由双参数反正切函数表示的θ₆理论表达式：

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;theta;</mi> <mn>6</mn> </msub> <mo>=</mo> <mi>arctan</mi> <mn>2</mn> <mrow> <mo>(</mo> <mo>(</mo> <msub> <mi>n</mi> <mi>y</mi> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>n</mi> <mi>x</mi> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <msub> <mi>cos&amp;theta;</mi> <mn>4</mn> </msub> <mo>-</mo> <msub> <mi>n</mi> <mi>z</mi> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>4</mn> </msub> <mi>sin</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>n</mi> <mi>x</mi> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>n</mi> <mi>y</mi> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <msub> <mi>sin&amp;theta;</mi> <mn>4</mn> </msub> <mi>cos</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> <mo>,</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <msub> <mi>o</mi> <mi>y</mi> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>o</mi> <mi>x</mi> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>1</mn> </msub> <mo>)</mo> <msub> <mi>cos&amp;theta;</mi> <mn>4</mn> </msub> <mo>-</mo> <msub> <mi>o</mi> <mi>z</mi> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>4</mn> </msub> <mi>sin</mi> <mo>(</mo> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>o</mi> <mi>x</mi> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>o</mi> <mi>y</mi> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <msub> <mi>sin&amp;theta;</mi> <mn>4</mn> </msub> <mi>cos</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> <mo>)</mo> <mo>+</mo> <mn>180</mn> <mo>&amp;CenterDot;</mo> <msub> <mi>N</mi> <mn>6</mn> </msub> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>

其中，N₆—由三角函数周期性产生的整数；

步骤四，针对所述机械臂各关节的旋转角度表达式的可能多解结果，根据机械臂的实际工作范围，给定一组各个关节的转动角度值θ_i1，由正运动学解的唯一性，得到机械臂第六个关节的坐标系相对于基坐标系的由位置和姿态组成的位姿矩阵⁰T₆，将此矩阵作为已知条件，利用所述旋转角度理论表达式θ_i进行逆运动学求解得到θ_i2；

步骤五，比较给定的转动角度值θ_i1与逆运动学求解得到θ_i2值是否满足计算精度要求，若满足计算结束；若相差较大，利用所求θ₁～θ₆理论表达式进行逆运动学求解；通过调整θ₂、θ₃角度表达式中根号前面的正负号以及各表达式是否因反三角函数的周期性需要加180°·N_i，N_i为整数，使得由所述旋转角度表达式计算出的各旋转角度值与正向运动的输入相等，即逆运动学求解正确，从而确定θ₂、θ₃关节角度表达式中的正负号和所有表达式中的N_i值，即得到唯一的一组关节旋转角度表达式。