CN110712203A - 一种7自由度机械臂逆运动学求解算法 - Google Patents

Abstract

本发明公开了一种7自由度机械臂逆运动学求解算法，包括：采用D‑H矩阵法建立机械臂的连杆坐标关系；求解给定位姿下腕关节点W在肩关节坐标系下的表达；以肩关节坐标系为参考，将肩关节点S与腕关节点W连线SW进行球坐标表示；定义臂角

求解给定末端位姿)和

时肘关节点E的位置；给定一臂角时求解肘关节点E在基座标base下的齐次表达；通过肘关节点E和腕关节点W的位置求解第二关节角q₂、第四关节角q₄、第六关节角q₆；利用全关节求解函数解第一关节角q₁、第三关节角q₃、第五关节角q₅和第七关节角q₇；筛选最优解。本发明简化了求解过程，提高了计算效率，为冗余机械臂的运动学优化、轨迹规划、空间避障任务奠定了基础。

Description

一种7自由度机械臂逆运动学求解算法

技术领域

本发明涉及工业机器人技术领域，特别涉及一种7自由度机械臂逆运动学求解算法。

背景技术

空间机械臂的末端位姿空间有6个变量，需要至少6个自由度以实现可达空间内的灵活控制。但是6自由度机械臂的一种末端位姿只对应有限组关节角空间的解，机械臂的操作灵活度不高。相对于6自由度机械臂，7自由度机械臂有高度的灵活性，在复杂工作空间作业、躲避奇异点、优化载荷分配及其他性能指标方面具有很大的优越性。

但由于7自由度机械臂存在冗余的特征，使得逆运动学求解比较困难，计算复杂度很高。因此，需要一种冗余机械臂逆运动学求解算法，以应对上述问题。

发明内容

针对现有技术的不足，本发明目的在于提供一种计算效率高的7自由度机械臂逆运动学求解算法。其采用如下技术方案：

一种7自由度机械臂逆运动学求解算法，其包括以下步骤：

S10、采用D-H矩阵法建立机械臂的连杆坐标关系；

S20、求解给定位姿下腕关节点W在肩关节坐标系下的表达；

S30、以肩关节坐标系为参考，将肩关节点S与腕关节点W连线SW进行球坐标表示；

S40、定义臂角

求解给定末端位姿和

对肘关节点E的位置；

S50、给定一臂角

时求解肘关节点E在基座标base下的齐次表达；

S60、通过肘关节点E和腕关节点W的位置求解第二关节角q₂、第四关节角q₄、第六关节角q₆；

S70、利用全关节求解函数解第一关节角q₁、第三关节角q₃、第五关节角q₅和第七关节角q₇；

S80、筛选最优解。

作为本发明的进一步改进，所述步骤S80具体包括：

结合关节限位条件，以各关节角相对于初始位置转动角度最少为原则，筛选最优解。

作为本发明的进一步改进，所述步骤S20具体包括：

S21、定义肩关节坐标系Shoulder相对于基座标系Base的表达：

Tsb＝trans([0 0 d₁ 1])

其中trans为一个齐次变换矩阵；

S22、求解腕关节点W在基座标系的表达：

W＝Tool*[0 0 - d₇ 1]

其中，Tool为工具坐标系；

S23、求解腕关节点在肩关节坐标系下的齐次表达：

W_S＝Tsb^-1*W。

作为本发明的进一步改进，所述将肩关节点S与腕关节点W连线SW进行球坐标表示，具体包括：

距离r_Sw＝sqrt(W_S(1)²+W_S(2)²+W_S(3)²)

仰角W_azimuth＝atan2(W_S(2)，W_S(1))

方位角W_elevation＝atan2(W_S(3)，W_S(1)²+W_S(2)²)

其中：r_Sw为SW长度；W_azimuth为SW与肩关节坐标系XY平面夹角；W_elevation为SW在肩关节坐标系XY平面内的投影与X轴夹角；1，2，3为向量分量，其值分别等于腕关节点W在肩关节坐标系下的x坐标，y坐标，z坐标。

作为本发明的进一步改进，所述步骤S70中定义了全关节求解函数ik_Axis，将q₁、q₃、q₅、E、W和Tool送入ik_Axis求解q1、q3、q5、q7，得出最终解，共8组。

作为本发明的进一步改进，所述步骤S40中，当臂角

时，肩关节点S、肘关节点E、腕关节点W组成的三角形SEW所在平面垂直于基座标系XY平面，且肘关节点E在SW上方。

作为本发明的进一步改进，所述步骤S50中求解肘关节点在肩坐标系下的齐次表达式时定义了Rotf矩阵，Rotf为绕某一向量旋转的变换矩阵。

作为本发明的进一步改进，所述步骤S60中q₂、q₄、q₆均存在正负两个解。

作为本发明的进一步改进，所述步骤S70中q₁、q₃、q₅分别对应q₂、q₄、q₆的正负解有两个不同的解。

本发明的有益效果：

本发明7自由度机械臂逆运动学求解算法利用几何学，并结合球坐标系简洁快速的求解复杂的冗余机械臂逆解问题，在各种场景下能够对机械臂进行实时灵巧控制，为机器人在复杂空间作业、躲避障碍等方面提供了理论基础，使得机器人具有良好的运动性能，具有非常重要的实际应用。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其他目的、特征和优点能够更明显易懂，以下特举较佳实施例，并配合附图，详细说明如下。

附图说明

图1是本发明中实施例中7自由度机械臂逆运动学求解算法的流程图；

图2是本发明中实施例中连杆坐标关系示意图；

图3是本发明中实施例中臂角的示意图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。

如图1所示，为本发明本实施例中的7自由度机械臂逆运动学求解算法，其包括以下步骤：

S10、采用D-H矩阵法建立机械臂的连杆坐标关系；

如图2所示，以基座为起始端，在关节部位建立0号坐标系，依次编号至末端执行器处建立7号坐标系，共建立0、1、2、3、4、5、6、7总计8个关节部位的坐标系；所述的坐标系为D-H坐标系；

DH参数表

机械臂处于竖直状态时。θ_i均为0°。

θ₁、θ₂、θ₃、θ₄、θ₅、θ₆、θ₇分别为各个关节旋转的角度；d1、d2、d3、d4、d5、d6、d7分别为1-7号坐标系之间原点的偏移距离；L1、L2、L3、L4、L5、L6、L7分别为相邻坐标系z轴的垂直距离；α₁、α₂、α₃、α₄、α₅、α₆、α₇分别表示相邻坐标系前一个坐标系z轴绕其x轴旋转α角与后一个坐标系z轴重合；运动范围表示关节角θ的转动范围。

此为坐标系i到坐标系i-1的齐次变换通式，i＝1，2，3，4，5，6，7。

S20、求解给定位姿下腕关节点W在肩关节坐标系下的表达；

所述步骤S20具体包括：

S21、定义肩关节坐标系Shoulder相对于基座标系Base的表达：

Tsb＝trans([0 0 d₁ 1])

其中trans为一个齐次变换矩阵；

S22、求解腕关节点W在基座标系的表达：

W＝Tool*[0 0 - d₇ 1]

其中，Tool为工具坐标系；

S23、求解腕关节点在肩关节坐标系下的齐次表达：

W_S＝Tsb^-1*W。

S30、以肩关节坐标系为参考，将肩关节点S与腕关节点W连线SW进行球坐标表示；

其中，所述将肩关节点S与腕关节点W连线SW进行球坐标表示，具体包括：

距离r_Sw＝sqrt(W_S(1)²+W_S(2)²+W_S(3)²)

仰角W_azimuth＝atan2(W_S(2)，W_S(1))

方位角W_elevation＝atan2(W_S(3)，W_S(1)²+W_S(2)²)

其中：r_Sw为SW长度；W_azimuth为SW与肩关节坐标系XY平面夹角；W_elevation为SW在肩关节坐标系XY平面内的投影与X轴夹角；1，2，3为向量分量，其值分别等于腕关节点W在肩关节坐标系下的x坐标，y坐标，z坐标。

为了确保机械臂能够达到指定位置且不考虑限位情况下有解，则必须满足

d₃+d₅-r_Sw≥0.00001

S40、定义臂角

求解给定末端位姿和

时肘关节点E的位置；

如图3所示，定义臂角

为SEW平面绕SW旋转角度，定义SEW三角形所在平面为臂形面，当

时臂形面垂直于Base的XY平面，肩关节点S、肘关节点E、腕关节点W组成的三角形SEW所在平面垂直于基座标系XY平面，且肘关节点E位于SW上方。则当

肘关节点E的位置求解如下

angle＝acos((d₃ ²+r_Sw ²-d₅ ²)/(2*d₃*r_Sw))；

E(1)＝d₃*cos(W_elevation+angle)*cos(W_azimuth)；

E(2)＝d₃*cos(W_elevation+angle)*sin(W_azimuth)；

E(3)＝d₃*sin(W_elevation+angle)；

E(4)＝1；

E′＝[E(1)E(2)E(3)E(4)]；

其中：angle为∠ESW。

S50、给定一臂角时求解肘关节点E在基座标base下的齐次表达；

具体的，先求解肘关节点在Shoulder下的齐次表达：

E_s＝Rotf[W_S(1)W_S(1)W_S(1)，Phi]*E′

(Rotf为绕某一向量旋转的变换矩阵)；

再将两个肩关节坐标系下的肘关节点转换到基座标系下：

E＝Tsb*E_s

S60、通过肘关节点E和腕关节点W的位置求解第二关节角q₂、第四关节角q₄、第六关节角q₆；

具体的，求解q2；

q₂＝atan2(sqrt(E(1)²+E(2)²)，(E(3)-d₁))

采用余弦定理解q4得：

求解q6；

肘关节点E与末端法兰点P的连线：

EP＝E-P

连线长度：

r_Ep＝sqrt(EP(1)²+EP(2)²+EP(3)²

当肘关节点、腕关节点、法兰中心点三点共线时即：

d₅+d₇-r_EP＜0.0000001时

q₆＝0

否则用余弦定理解出：

q₆＝pi-acos((d₅ ²+d₇ ²-r_EP ²)/(2*d₅*d₇)

S70、利用全关节求解函数解第一关节角q₁、第三关节角q₃、第五关节角q₅和第七关节角q₇；

所述步骤S70中定义了全关节求解函数ik_Axis，将q₁、q₃、q₅、E、W和Tool送入ik_Axis求解q1、q3、q5、q7，即q＝ik_Axis(q1，q3，q5，E，W，Tool)，得出最终解，共8组。

全关节函数具体求解过程如下：

求解q1；

A_i为坐标系i到坐标系i-1的齐次变换通式

T_i为坐标系i到基座标系的齐次变换通式T_i＝A₁A₂...A_i

其中：i＝1，2，3，4，5，6，7

E在T2坐标下的齐次表达为：

其中：c表示cos，s表示sin；

当q₂≥0时，s₂≥0，q₂＜0时，s₂＜0，以下中间变量Temp前符号同理；

所以，

当q₂≥0时q₁₁＝atan2(-E(2)，-E(1))

当q₂＜0时q₁₂＝atan2(E(2)，E(1))

求解q3；

W在T4坐标下的齐次表达为：

引入中间变量Temp₁＝T₂ ^-1*W

当q₄≥0时q₃₁＝atan2(-Temp(2)，-Temp(1))

当q₄＜0时q₃₂＝atan2(Temp(2)，Temp(1))

求解q5；

P在T6坐标系下的齐次表达为：

引入中间变量Temp₂＝T₄ ^-1*P

当q₆≥0时q₅₁＝atan2(-Temp(2)，-Temp(1))

当q₆＜0时q₅₂＝atan2(Temp(2)，Temp(1))

求解q7；

引入中间变量Temp₃＝T6^-1*Tool

q₇＝atan2(Temp(2，1)，Temp(1，1))

最终求得全部8组关节角：

q＝[q1 q2 q3 q4 q5 q6 q7]

S80、筛选最优解。

具体包括：

结合关节限位条件，以各关节角相对于初始位置转动角度最少为原则，筛选最优解。将每组解各关节角度与初始角度作差后取绝对值，再将所得差值求和：

i表示第i组解，j表示第j个关节角，i＝1 2 3 4 5 6 7，j＝1 2 3 4 5 6 7

S_max＝S(i)＝Max[S(1)，S(2)，S(3)，S(4)，S(5)，S(6)，S(7)]

即最优解q＝q(i)

其中：

q₁∈[-165，165]

q₂∈[-115，115]

q₃∈[-165，165]

q₄∈[-120，120]

q₅∈[-170，170]

q₆∈[-115，115]

q₇∈[-170，170]

其中，所述步骤S60中q₂、q₄、q₆均存在正负两个解。所述步骤S70中q₁、q₃、q₅分别对应q₂、q₄、q₆的正负解有两个不同的解。

本发明通过几何法简化了7自由度机械臂逆运动学求解过程，提高了计算效率，为冗余机械臂的运动学优化、轨迹规划、空间避障任务奠定了基础。

以上实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。

Claims (9)

1.一种7自由度机械臂逆运动学求解算法，其特征在于，包括以下步骤：

S10、采用D-H矩阵法建立机械臂的连杆坐标关系；

S20、求解给定位姿下腕关节点W在肩关节坐标系下的表达；

S30、以肩关节坐标系为参考，将肩关节点S与腕关节点W连线SW进行球坐标表示；

S40、定义臂角

求解给定末端位姿和

时肘关节点E的位置；

S50、给定一臂角

时求解肘关节点E在基座标base下的齐次表达；

S60、通过肘关节点E和腕关节点W的位置求解第二关节角q₂、第四关节角q₄、第六关节角q₆；

S70、利用全关节求解函数解第一关节角q₁、第三关节角q₃、第五关节角q₅和第七关节角q₇；

S80、筛选最优解。

2.如权利要求1所述的7自由度机械臂逆运动学求解算法，其特征在于，所述步骤S80具体包括：

结合关节限位条件，以各关节角相对于初始位置转动角度最少为原则，筛选最优解。

3.如权利要求1所述的7自由度机械臂逆运动学求解算法，其特征在于，所述步骤S20具体包括：

S21、定义肩关节坐标系Shoulder相对于基座标系Base的表达：

Tsb＝trans([0 0 d₁ 1])

其中trans为一个齐次变换矩阵；

S22、求解腕关节点W在基座标系的表达：

W＝Tool*[0 0 -d₇ 1]

其中，Tool为工具坐标系；

S23、求解腕关节点在肩关节坐标系下的齐次表达：

W_S＝Tsb^-1*W。

4.如权利要求3所述的7自由度机械臂逆运动学求解算法，其特征在于，所述将肩关节点S与腕关节点W连线SW进行球坐标表示，具体包括：

距离r_Sw＝sqrt(W_S(1)²+W_S(2)²+W_S(3)²)

仰角W_azimuth＝atan2(W_S(2)，W_S(1))

方位角W_elevation＝atan2(W_S(3)，W_S(1)²+W_S(2)²)

其中：r_Sw为SW长度；W_azimuth为SW与肩关节坐标系XY平面夹角；W_elevation为SW在肩关节坐标系XY平面内的投影与X轴夹角；1，2，3为向量分量，其值分别等于腕关节点W在肩关节坐标系下的x坐标，y坐标，z坐标。

5.如权利要求3所述的7自由度机械臂逆运动学求解算法，其特征在于，所述步骤S70中定义了全关节求解函数ik_Axis，将q₁、q₃、q₅、E、W和Tool送入ik_Axis求解q1、q3、q5、q7，得出最终解，共8组。

6.如权利要求1所述的7自由度机械臂逆运动学求解算法，其特征在于，所述步骤S40中，当臂角

时，肩关节点S、肘关节点E、腕关节点W组成的三角形SEW所在平面垂直于基座标系XY平面，且肘关节点E在SW上方。

7.如权利要求1所述的7自由度机械臂逆运动学求解算法，其特征在于，所述步骤S50中求解肘关节点在肩坐标系下的齐次表达式时定义了Rotf矩阵，Rotf为绕某一向量旋转的变换矩阵。

8.如权利要求1所述的7自由度机械臂逆运动学求解算法，其特征在于，所述步骤S60中q₂、q₄、q₆均存在正负两个解。

9.如权利要求7所述的7自由度机械臂逆运动学求解算法，其特征在于，所述步骤S70中q₁、q₃、q₅分别对应q₂、q₄、q₆的正负解有两个不同的解。

Images