(12)发明专利申请
(10)申请公布号 CN 110712203 A(43)申请公布日 2020.01.21
(21)申请号 201910916635.9(22)申请日 2019.09.26
(71)申请人 苏州苏相机器人智能装备有限公司
地址 215000 江苏省苏州市苏州相城经济
技术开发区澄阳路116号阳澄湖国际科技创业园2号楼202室(72)发明人 匡绍龙 张建法 孙立宁 王梓豪
尹吴越 (74)专利代理机构 苏州市中南伟业知识产权代
理事务所(普通合伙) 32257
代理人 苏张林(51)Int.Cl.
B25J 9/16(2006.01)
权利要求书2页 说明书7页 附图3页
(54)发明名称
一种7自由度机械臂逆运动学求解算法(57)摘要
本发明公开了一种7自由度机械臂逆运动学求解算法，包括：采用D-H矩阵法建立机械臂的连杆坐标关系；求解给定位姿下腕关节点W在肩关节坐标系下的表达；以肩关节坐标系为参考，将肩关节点S与腕关节点W连线SW进行球坐标表示；定义臂角
求解给定末端位姿)和
时
肘关节点E的位置；给定一臂角
时求解肘关节点E在基
座标base下的齐次表达；通过肘关节点E和腕关节点W的位置求解第二关节角q2、第四关节角q4、第六关节角q6；利用全关节求解函数解第一关节角q1、第三关节角q3、第五关节角q5和第七关节角CN 110712203 Aq7；筛选最优解。本发明简化了求解过程，提高了计算效率，为冗余机械臂的运动学优化、轨迹规划、空间避障任务奠定了基础。
CN 110712203 A
权 利 要 求 书
1/2页
1.一种7自由度机械臂逆运动学求解算法，其特征在于，包括以下步骤：S10、采用D-H矩阵法建立机械臂的连杆坐标关系；S20、求解给定位姿下腕关节点W在肩关节坐标系下的表达；S30、以肩关节坐标系为参考，将肩关节点S与腕关节点W连线SW进行球坐标表示；S40、定义臂角S50、给定一臂角
求解给定末端位姿和
时肘关节点E的位置；
时求解肘关节点E在基座标base下的齐次表达；
S60、通过肘关节点E和腕关节点W的位置求解第二关节角q2、第四关节角q4、第六关节角q6；
S70、利用全关节求解函数解第一关节角q1、第三关节角q3、第五关节角q5和第七关节角q7；
S80、筛选最优解。
2.如权利要求1所述的7自由度机械臂逆运动学求解算法，其特征在于，所述步骤S80具体包括：
结合关节限位条件，以各关节角相对于初始位置转动角度最少为原则，筛选最优解。3.如权利要求1所述的7自由度机械臂逆运动学求解算法，其特征在于，所述步骤S20具体包括：
S21、定义肩关节坐标系Shoulder相对于基座标系Base的表达：Tsb＝trans([0 0 d1 1])
其中trans为一个齐次变换矩阵；S22、求解腕关节点W在基座标系的表达：W＝Tool*[0 0 -d7 1]其中，Tool为工具坐标系；S23、求解腕关节点在肩关节坐标系下的齐次表达：WS＝Tsb-1*W。
4.如权利要求3所述的7自由度机械臂逆运动学求解算法，其特征在于，所述将肩关节点S与腕关节点W连线SW进行球坐标表示，具体包括：
距离rSw＝sqrt(WS(1)2+WS(2)2+WS(3)2)仰角Wazimuth＝atan2(WS(2)，WS(1))方位角Welevation＝atan2(WS(3)，WS(1)2+WS(2)2)其中：rSw为SW长度；Wazimuth为SW与肩关节坐标系XY平面夹角；Welevation为SW在肩关节坐标系XY平面内的投影与X轴夹角；1，2，3为向量分量，其值分别等于腕关节点W在肩关节坐标系下的x坐标，y坐标，z坐标。
5.如权利要求3所述的7自由度机械臂逆运动学求解算法，其特征在于，所述步骤S70中定义了全关节求解函数ik_Axis，将q1、q3、q5、E、W和Tool送入ik_Axis求解q1、q3、q5、q7，得出最终解，共8组。
6.如权利要求1所述的7自由度机械臂逆运动学求解算法，其特征在于，所述步骤S40中，当臂角
时，肩关节点S、肘关节点E、腕关节点W组成的三角形SEW所在平面垂直于
基座标系XY平面，且肘关节点E在SW上方。
2
CN 110712203 A
权 利 要 求 书
2/2页
7.如权利要求1所述的7自由度机械臂逆运动学求解算法，其特征在于，所述步骤S50中求解肘关节点在肩坐标系下的齐次表达式时定义了Rotf矩阵，Rotf为绕某一向量旋转的变换矩阵。
8.如权利要求1所述的7自由度机械臂逆运动学求解算法，其特征在于，所述步骤S60中q2、q4、q6均存在正负两个解。
9.如权利要求7所述的7自由度机械臂逆运动学求解算法，其特征在于，所述步骤S70中q1、q3、q5分别对应q2、q4、q6的正负解有两个不同的解。
3
CN 110712203 A
说 明 书
一种7自由度机械臂逆运动学求解算法
1/7页
技术领域
[0001]本发明涉及工业机器人技术领域，特别涉及一种7自由度机械臂逆运动学求解算法。
背景技术
[0002]空间机械臂的末端位姿空间有6个变量，需要至少6个自由度以实现可达空间内的灵活控制。但是6自由度机械臂的一种末端位姿只对应有限组关节角空间的解，机械臂的操作灵活度不高。相对于6自由度机械臂，7自由度机械臂有高度的灵活性，在复杂工作空间作业、躲避奇异点、优化载荷分配及其他性能指标方面具有很大的优越性。[0003]但由于7自由度机械臂存在冗余的特征，使得逆运动学求解比较困难，计算复杂度很高。因此，需要一种冗余机械臂逆运动学求解算法，以应对上述问题。发明内容
[0004]针对现有技术的不足，本发明目的在于提供一种计算效率高的7自由度机械臂逆运动学求解算法。其采用如下技术方案：
[0005]一种7自由度机械臂逆运动学求解算法，其包括以下步骤：[0006]S10、采用D-H矩阵法建立机械臂的连杆坐标关系；[0007]S20、求解给定位姿下腕关节点W在肩关节坐标系下的表达；[0008]S30、以肩关节坐标系为参考，将肩关节点S与腕关节点W连线SW进行球坐标表示；
[0009][0010]
S40、定义臂角S50、给定一臂角
求解给定末端位姿和对肘关节点E的位置；
时求解肘关节点E在基座标base下的齐次表
达；
[0011]
S60、通过肘关节点E和腕关节点W的位置求解第二关节角q2、第四关节角q4、第六关S70、利用全关节求解函数解第一关节角q1、第三关节角q3、第五关节角q5和第七关S80、筛选最优解。
作为本发明的进一步改进，所述步骤S80具体包括：结合关节限位条件，以各关节角相对于初始位置转动角度最少为原则，筛选最优作为本发明的进一步改进，所述步骤S20具体包括：S21、定义肩关节坐标系Shoulder相对于基座标系Base的表达：Tsb＝trans([0 0 d1 1])
其中trans为一个齐次变换矩阵；S22、求解腕关节点W在基座标系的表达：
节角q6；
[0012]
节角q7；
[0013][0014][0015]
解。
[0016][0017][0018][0019][0020]
4
CN 110712203 A[0021]
说 明 书
2/7页
W＝Tool*[0 0 - d7 1]
[0022]其中，Tool为工具坐标系；[0023]S23、求解腕关节点在肩关节坐标系下的齐次表达：[0024]WS＝Tsb-1*W。
[0025]作为本发明的进一步改进，所述将肩关节点S与腕关节点W连线SW进行球坐标表示，具体包括：
[0026]距离rSw＝sqrt(WS(1)2+WS(2)2+WS(3)2)[0027]仰角Wazimuth＝atan2(WS(2)，WS(1))[0028]方位角Welevation＝atan2(WS(3)，WS(1)2+WS(2)2)[0029]其中：rSw为SW长度；Wazimuth为SW与肩关节坐标系XY平面夹角；Welevation为SW在肩关节坐标系XY平面内的投影与X轴夹角；1，2，3为向量分量，其值分别等于腕关节点W在肩关节坐标系下的x坐标，y坐标，z坐标。[0030]作为本发明的进一步改进，所述步骤S70中定义了全关节求解函数ik_Axis，将q1、q3、q5、E、W和Tool送入ik_Axis求解q1、q3、q5、q7，得出最终解，共8组。
[0031]
作为本发明的进一步改进，所述步骤S40中，当臂角时，肩关节点S、肘关节点
E、腕关节点W组成的三角形SEW所在平面垂直于基座标系XY平面，且肘关节点E在SW上方。[0032]作为本发明的进一步改进，所述步骤S50中求解肘关节点在肩坐标系下的齐次表达式时定义了Rotf矩阵，Rotf为绕某一向量旋转的变换矩阵。[0033]作为本发明的进一步改进，所述步骤S60中q2、q4、q6均存在正负两个解。[0034]作为本发明的进一步改进，所述步骤S70中q1、q3、q5分别对应q2、q4、q6的正负解有两个不同的解。
[0035]本发明的有益效果：
[0036]本发明7自由度机械臂逆运动学求解算法利用几何学，并结合球坐标系简洁快速的求解复杂的冗余机械臂逆解问题，在各种场景下能够对机械臂进行实时灵巧控制，为机器人在复杂空间作业、躲避障碍等方面提供了理论基础，使得机器人具有良好的运动性能，具有非常重要的实际应用。
[0037]上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其他目的、特征和优点能够更明显易懂，以下特举较佳实施例，并配合附图，详细说明如下。附图说明
[0038]图1是本发明中实施例中7自由度机械臂逆运动学求解算法的流程图；[0039]图2是本发明中实施例中连杆坐标关系示意图；[0040]图3是本发明中实施例中臂角的示意图。
具体实施方式
[0041]下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。[0042]如图1所示，为本发明本实施例中的7自由度机械臂逆运动学求解算法，其包括以
5
CN 110712203 A
说 明 书
3/7页
下步骤：
[0043]S10、采用D-H矩阵法建立机械臂的连杆坐标关系；[0044]如图2所示，以基座为起始端，在关节部位建立0号坐标系，依次编号至末端执行器处建立7号坐标系，共建立0、1、2、3、4、5、6、7总计8个关节部位的坐标系；所述的坐标系为D-H坐标系；
[0045]DH参数表
[0046]
机械臂处于竖直状态时。θ。i均为0°
[0048]θθθθθθθd1、d2、d3、d4、d5、d6、d7分别为1、2、3、4、5、6、7分别为各个关节旋转的角度；1-7号坐标系之间原点的偏移距离；L1、L2、L3、L4、L5、L6、L7分别为相邻坐标系z轴的垂直距离；αααααααα角与后一个1、2、3、4、5、6、7分别表示相邻坐标系前一个坐标系z轴绕其x轴旋转坐标系z轴重合；运动范围表示关节角θ的转动范围。
[0047]
[0049]
[0050][0051][0052][0053][0054][0055][0056][0057][0058][0059][0060][0061][0062][0063][0064]
此为坐标系i到坐标系i-1的齐次变换通式，i＝1，2，3，4，5，6，7。
S20、求解给定位姿下腕关节点W在肩关节坐标系下的表达；所述步骤S20具体包括：S21、定义肩关节坐标系Shoulder相对于基座标系Base的表达：Tsb＝trans([0 0 d1 1])
其中trans为一个齐次变换矩阵；S22、求解腕关节点W在基座标系的表达：W＝Tool*[0 0 - d7 1]其中，Tool为工具坐标系；S23、求解腕关节点在肩关节坐标系下的齐次表达：WS＝Tsb-1*W。S30、以肩关节坐标系为参考，将肩关节点S与腕关节点W连线SW进行球坐标表示；其中，所述将肩关节点S与腕关节点W连线SW进行球坐标表示，具体包括：距离rSw＝sqrt(WS(1)2+WS(2)2+WS(3)2)仰角Wazimuth＝atan2(WS(2)，WS(1))
6
CN 110712203 A[0065]
说 明 书
4/7页
方位角Welevation＝atan2(WS(3)，WS(1)2+WS(2)2)
[0066]其中：rSw为SW长度；Wazimuth为SW与肩关节坐标系XY平面夹角；Welevation为SW在肩关节坐标系XY平面内的投影与X轴夹角；1，2，3为向量分量，其值分别等于腕关节点W在肩关节坐标系下的x坐标，y坐标，z坐标。
[0067]为了确保机械臂能够达到指定位置且不考虑限位情况下有解，则必须满足[0068]d3+d5-rSw≥0.00001
[0069]
S40、定义臂角求解给定末端位姿和时肘关节点E的位置；
[0070]如图3所示，定义臂角为SEW平面绕SW旋转角度，定义SEW三角形所在平面为臂形
时臂形面垂直于Base的XY平面，肩关节点S、肘关节点E、腕关节点W组成的三
肘关节
面，当
角形SEW所在平面垂直于基座标系XY平面，且肘关节点E位于SW上方。则当点E的位置求解如下
[0071]angle＝acos((d32+rSw2-d52)/(2*d3*rSw))；[0072]E(1)＝d3*cos(Welevation+angle)*cos(Wazimuth)；[0073]E(2)＝d3*cos(Welevation+angle)*sin(Wazimuth)；[0074]E(3)＝d3*sin(Welevation+angle)；[0075]E(4)＝1；[0076]E′＝[E(1)E(2)E(3)E(4)]；[0077]其中：angle为∠ESW。
[0078]
S50、给定一臂角时求解肘关节点E在基座标base下的齐次表
达；
[0079][0080][0081][0082][0083][0084]
具体的，先求解肘关节点在Shoulder下的齐次表达：
Es＝Rotf[WS(1)WS(1)WS(1)，Phi]*E′(Rotf为绕某一向量旋转的变换矩阵)；
再将两个肩关节坐标系下的肘关节点转换到基座标系下：E＝Tsb*EsS60、通过肘关节点E和腕关节点W的位置求解第二关节角q2、第四关节角q4、第六关具体的，求解q2；
q2＝atan2(sqrt(E(1)2+E(2)2)，(E(3)-d1))采用余弦定理解q4得：
节角q6；
[0085][0086][0087][0088][0089][0090][0091][0092]
求解q6；
肘关节点E与末端法兰点P的连线：EP＝E-P连线长度：
7
CN 110712203 A[0093][0094][0095][0096][0097][0098][0099]
说 明 书
5/7页
rEp＝sqrt(EP(1)2+EP(2)2+EP(3)2当肘关节点、腕关节点、法兰中心点三点共线时即：d5+d7-rEP＜0.0000001时q6＝0
否则用余弦定理解出：
q6＝pi-acos((d52+d72-rEP2)/(2*d5*d7)S70、利用全关节求解函数解第一关节角q1、第三关节角q3、第五关节角q5和第七关
节角q7；
所述步骤S70中定义了全关节求解函数ik_Axis，将q1、q3、q5、E、W和Tool送入ik_
Axis求解q1、q3、q5、q7，即q＝ik_Axis(q1，q3，q5，E，W，Tool)，得出最终解，共8组。[0101]全关节函数具体求解过程如下：[0102]求解q1；
[0103][0104][0105][0106][0100]
Ai为坐标系i到坐标系i-1的齐次变换通式
Ti为坐标系i到基座标系的齐次变换通式Ti＝A1A2...Ai其中：i＝1，2，3，4，5，6，7
E在T2坐标下的齐次表达为：
[0107]
[0108]
[0109]
[0110][0111][0112][0113][0114][0115][0116]
其中：c表示cos，s表示sin；当q2≥0时，s2≥0，q2＜0时，s2＜0，以下中间变量Temp前符号同理；所以，
当q2≥0时q11＝atan2(-E(2)，-E(1))当q2＜0时q12＝atan2(E(2)，E(1))求解q3；
W在T4坐标下的齐次表达为：
8
CN 110712203 A
说 明 书
6/7页
[0117]
[0118]
[0119][0120][0121][0122][0123]
引入中间变量Temp1＝T2-1*W
当q4≥0时q31＝atan2(-Temp(2)，-Temp(1))当q4＜0时q32＝atan2(Temp(2)，Temp(1))求解q5；
P在T6坐标系下的齐次表达为：
[0124]
引入中间变量Temp2＝T4-1*P
[0126]当q6≥0时q51＝atan2(-Temp(2)，-Temp(1))[0127]当q6＜0时q52＝atan2(Temp(2)，Temp(1))[0128]求解q7；[0129]引入中间变量Temp3＝T6-1*Tool[0130]q7＝atan2(Temp(2，1)，Temp(1，1))[0131]最终求得全部8组关节角：[0132]q＝[q1 q2 q3 q4 q5 q6 q7][0133]S80、筛选最优解。[0134]具体包括：
[0135]结合关节限位条件，以各关节角相对于初始位置转动角度最少为原则，筛选最优解。将每组解各关节角度与初始角度作差后取绝对值，再将所得差值求和：
[0125][0136]
[0137][0138][0139][0140][0141][0142][0143]
i表示第i组解，j表示第j个关节角，i＝1 2 3 4 5 6 7，j＝1 2 3 4 5 6 7
Smax＝S(i)＝Max[S(1)，S(2)，S(3)，S(4)，S(5)，S(6)，S(7)]即最优解q＝q(i)其中：
q1∈[-165，165]q2∈[-115，115]q3∈[-165，165]
9
CN 110712203 A[0144]
说 明 书
7/7页
q4∈[-120，120]
[0145]q5∈[-170，170][0146]q6∈[-115，115][0147]q7∈[-170，170][0148]其中，所述步骤S60中q2、q4、q6均存在正负两个解。所述步骤S70中q1、q3、q5分别对应q2、q4、q6的正负解有两个不同的解。
[0149]本发明通过几何法简化了7自由度机械臂逆运动学求解过程，提高了计算效率，为冗余机械臂的运动学优化、轨迹规划、空间避障任务奠定了基础。
[0150]以上实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。
10
CN 110712203 A
说 明 书 附 图
1/3页
图1
11
说 明 书 附 图
2/3页
图2
12
CN 110712203 A
CN 110712203 A
说 明 书 附 图
3/3页
图3
13
因篇幅问题不能全部显示，请点此查看更多更全内容