一种带肘部偏置的七自由度机械臂的几何逆解方法

1.本发明涉及机械臂控制技术领域，尤其涉及一种带肘部偏置的七自由度机械臂的几何逆解方法。


背景技术：

2.机械臂是模仿人类手臂的某些动作功能，按固定的程序完成抓取，搬运，操作工具等任务的自动操作装置。它可以代替人类完成一些机械重复工作，来实现生产自动化，广泛应用于机械制造，电子，轻工等行业。在日益复杂的实际应用场景下，新的操作任务和操作环境对于机械臂提出了更高的要求，因此在学术和工业领域针对机械臂的研究逐渐深入。
3.肘部偏置的七自由度机械臂是工程中常见的一种机械臂，它类似人类手臂，因为其更高的自由度以及肘部偏置的结构特点，它具有高度的灵活性、容错性和可靠性，可以在完成复杂任务的同时兼顾关节避极限、避奇异、避障、优化关节力矩等性能指标，在工程中应用广泛。
4.2000年，deepak等学者提出了一系列逆运动学算法的评估指标，一个好的逆运动学算法应该从以下几个指标上进行系统的评估：
5.(1)实时性：机械臂在运动控制中通常需要实时控制，这对算法的计算量以及效率上提出了比较高的要求；
6.(2)可靠性：如果逆解存在，算法返回满足条件的可行解；如逆解不存在，则报告相应的错误；
7.(3)完备性：针对机械臂不同的末端位姿，对应的逆解可能不止一组，逆运动学算法要保证能找到所有逆解；
8.(4)稳定性：机械臂处于奇异位姿时会面临结构退化的问题，此时逆运动学算法仍能给出合理的解；
9.(5)通用性：逆运动学算法是否可以运用到不同形态结构的机械臂，且能稳定运行。
10.针对以上几点评估指标，我们对现有的一些方法进行评估。newton-raphson法是应用较多的一种典型的数值解法，它具有较好的通用性，对于不同结构特点的机械臂都能比较好的适用，但是这种方法运行时间较长，计算量大，难以用于实时性要求较高的应用场景。而基于雅可比矩阵矩阵转置或广义逆矩阵以及数值优化的方法可能会陷入局部最小值问题。数值解法容易导致发散，几乎所有的数值解法在雅可比矩阵的奇异位置附近的收敛性很差。数值解法的优点在于通用性强，适用于不同构型的机械臂，而且可以扩展一些优化方法，但是缺点也同样明显。所以我们总是优先期望得到具有封闭形式的解决方案，也即解析解法。他们相较于数值解法计算量更小，效率更高，且完备性更好，更容易获取所有可能的解。


技术实现要素：

11.本发明的目的在于针对已有的技术现状，提供一种带肘部偏置的七自由度机械臂的几何逆解方法。
12.为达到上述目的，本发明采用如下技术方案：
13.一种带肘部偏置的七自由度机械臂的几何逆解方法，包括以下步骤：
14.s1、建立七自由度机械臂的dh坐标系，标定其dh参数，dh参数包括：α
i-1
、a
i-1
、θi、di，α
i-1
分别为0、-π/2、π/2、-π/2、π/2、-π/2、π/2，a
i-1
均为0，θi分别为θ2、θ2、θ3、θ4、θ5、θ6、θ7，di分别为d1、0、d3、d4、d5、0、d7；
15.其中，i表示第i个坐标系，i为1～7，α
i-1
表示z
i-1
轴与zi轴间夹角，a
i-1
表示z
i-1
轴与zi轴间距离，θi表示x
i-1
轴与xi轴间夹角，di表示x
i-1
轴与xi轴间距离；
16.s2、给定七自由度机械臂的期望末端位姿
17.将基座位置记为b，肩部关节位置记为s，腕部关节位置记为w，肘部关节位置记为e，其偏置点位置记为e
′
，肘部关节没有偏置时w所处在的点记为w
′
；
18.将平面sew
′
与bs共面时的平面sew定义为基准臂角面，sew平面绕sw轴任意旋转，定义sew平面与基准臂角面的夹角为ψ，ψ的夹角范围为[-π，π]；
[0019]
s3、定义b点坐标pb＝[0，0，0]
t
，则向量定义s点坐标
[0020]
由七自由度机械臂的期望末端位姿取第四列前三行元素得到末端位置取前三行前三列元素得到末端姿态由向量的条件，得到w点坐标且向量向量进而得到向量
[0021]
构造坐标系来描述任意臂角平面sew，构造步骤如下：
[0022]
s311、构造sew
′
平面，求取
[0023]
取单位向量z0＝[0，0，1]
t
，在空间中构造sew
′
平面：
[0024][0025]
取直线sw
′
上一点，其位置记为d，且de
⊥
sw
′
，此时z
∑
即为向量的方向向量；
[0026]
若z0与sw
′
共线，则直接取y
∑
＝[0，1，0]
t
；
[0027]
由已知条件求解的长度：
[0028][0029]
由上式解得进而得到
[0030]
s312、构造sew平面
[0031]
取方向上的单位向量sew平面构造公式表述为：
[0032][0033]
将结果排列得到∑＝[x
∑
，y
∑
，z
∑
]；
[0034]
求取七自由度机械臂的逆解，包括如下步骤：
[0035]
s321：计算θ4[0036]
由余弦定理，得到：
[0037][0038]
引入一个控制参数控制参数e用于反映肘关节相对于sw
′
的位置关系，进而得到：
[0039]
θ4＝e(π-acos(cos∠sew
′
))；
[0040]
s322：描述臂角基准面
[0041]
当θ
1-3
＝0时，臂角平面处于臂角基准面上，计算得出w的位置：
[0042][0043]
向量将向量代入sew平面构造公式，得到臂角基准面的描述为
[0044]
s323：求解θ
1-3
[0045]
将代入sew平面构造公式，得到θ
1-3
分别取θ1，θ2，θ3时的臂角平面，记作此时的臂角平面旋转描述为与旋转矩阵rot(x，ψ)的运算其中rot(x，ψ)表示沿着x轴逆时针旋转角度ψ；
[0046]
记s处的旋转变换为rs，则得到rs·
∑0＝∑d，且∑0，∑d是正交矩阵，则有
[0047]
s处的旋转变换为：rs＝rot(z，θ1)rot(y，θ2)rot(z，θ3)，即：
[0048][0049]
其中，si＝sinθi，ci＝cosθi(i＝1，...，7)，*表示可忽略的项；
[0050]
由上式得到：
[0051]
cosθ2＝r
s33
；
[0052][0053][0054]
引入一个控制参数控制参数s用于反映大臂相对于bs的位置关系，进而得到：
[0055]
θ2＝s
·
acos(r
s33
)；
[0056]
θ1＝atan2(s
·rs23
，s
·rs13
)；
[0057]
θ3＝atan2(s
·rs32
，-s
·rs31
)；
[0058]
s324：求解θ
5-7
[0059]
记w处的旋转变换为rw，则有以下等式成立：
[0060][0061]
其中，rs·
rot(y，θ4)为正交矩阵，则有：
[0062][0063]
w处的旋转变换描述为：rw＝rot(z，θ5)rot(y，θ6)rot(z，θ7)，即：
[0064][0065]
其中，si＝sinθi，ci＝cosθi(i＝1，...，7)，*表示可忽略的项。
[0066]
由上式得到：
[0067]
cosθ6＝r
w33
；
[0068][0069][0070]
引入一个控制参数控制参数w用于反映手腕的翻转情况，进而得到：
[0071]
θ6＝w
·
acos(r
w33
)；
[0072]
θ5＝atan2(w
·rw23
，w
·rw13
)；
[0073]
θ7＝atan2(w
·rw32
，-w
·rw31
)。
[0074]
本发明的有益效果为：
[0075]
本发明提出的是一种带肘部偏置的七自由度机械臂的几何逆解方法，利用臂角以及空间中的基本几何结构关系，在现有的代数解法基础上，扩展出一套针对带肘部偏置的七自由度机械臂这种特殊结构的几何逆解方法，相较于现有的基于代数法的解析解法，这套方法具有计算量小，效率高，计算精度可靠，且实现方式更容易等优点，可以作为带肘部偏置的七自由度机械臂的逆解方法上的一种补充。
附图说明
[0076]
图1为本发明七自由度机械臂的几何逆解流程示意图；
[0077]
图2为本发明七自由度机械臂dh坐标系；
[0078]
图3为本发明七自由度机械臂简化示意图；
[0079]
图4为臂角ψ＝0rad时的八组逆解结果；
[0080]
图5为臂角ψ＝π/4rad时的八组逆解结果；
[0081]
图6为正运动学验算结果。
[0082]
标注说明：
具体实施方式
[0083]
下面结合附图对本发明作进一步说明。
[0084]
1、具体实施方案
[0085]
1.1、dh参数标定
[0086]
首先，标定七自由度机械臂的dh参数。
[0087]
如图1所示，建立七自由度机械臂的dh坐标系，图中，z
0-z7为各坐标系的z轴，各坐标系的x轴方向均为从纸平面向外，y轴方向可由右手系原则标定。
[0088]
其中，i表示第i个坐标系，i为1～7，α
i-1
(i＝1，...，7)表示z
i-1
轴与zi轴间夹角，a
i-1
(i＝1，...，7)表示z
i-1
轴与zi轴间距离，θi(i＝1，...，7)表示x
i-1
轴与xi轴间夹角，di(i＝1，...，7)表示x
i-1
轴与xi轴间距离。
[0089]
最终得到dh参数如下表1所示：
[0090]
iα
i-1ai-1
θ
idi
100θ1d12-π/20θ203π/20θ3d34-π/20θ4d45π/20θ5d56-π/20θ607π/20θ7d7[0091]
表1：dh参数表
[0092]
1.2、自运动模式分析以及三维空间绘图
[0093]
给定一个期望末端位姿也即给定末端的六个约束，而机械臂有七个关节自由度，此时机械臂在满足末端位姿时，还有一个自运动，即肘关节做圆周运动。如果仅分析七个关节的转动，机械臂的前三个关节和后三个关节可视为两个球形关节，它的运动模式是，先绕z1轴旋转θ1，再绕z2轴旋转θ2，然后绕z3轴旋转θ3，可以等效为先绕z1轴旋转θ1，再绕y1轴旋转θ2，然后绕z1轴旋转θ3，也即zyz转角。
[0094]
将基座位置记为b，肩部关节位置记为s，腕部关节位置记为w，肘部关节位置记为e，其偏置点位置记为e
′
，肘部关节没有偏置时w所处在的点记为w
′
，该七自由度机械臂的简化示意图如图2所示。
[0095]
将平面sew
′
与bs共面时的平面sew定义为基准臂角面，因为有自运动的存在，sew
平面可以绕sw轴任意旋转，定义sew平面与基准臂角面的夹角为ψ，ψ的夹角范围为[-π，π]。引入臂角这个约束条件可以形成第七个约束，从而确定出机械臂七个关节转角的值。臂角可以直观的反映肘关节在空间中的位置，可以通过设定臂角达到避关节极限和避障的目的。
[0096]
1.3、逆解过程
[0097]
根据dh参数表及机械臂末端位姿可以获取到空间中几个点和向量的信息，定义b点坐标pb＝[0，0，0]
t
，容易得到向量s点坐标
[0098]
由末端位姿取第四列前三行元素得到末端位置取前三行前三列元素得到末端姿态由向量的条件，可以得到w点坐标且向量向量可以得到向量
[0099]
由于sew平面中e点位置是不确定的，直接构造臂角平面sew难度较大，所以采取先构造sew
′
平面的方法来求取加上已知条件共同确定臂角平面sew。由以上条件我们可以构造坐标系来描述任意臂角平面sew，构造方法如下：
[0100]
步骤1：构造sew
′
平面，求取
[0101]
取单位向量z0＝[0，0，1]
t
，在空间中构造sew
′
平面：
[0102][0103]
d点在直线sw
′
上，且de
⊥
sw
′
，此时z
∑
即为向量的方向向量。
[0104]
若z0与sw
′
共线，则直接取y
∑
＝[0，1，0]
t
。由已知条件求解的长度：
[0105][0106]
由上式可以解得可以得到
[0107]
步骤2：构造sew平面
[0108]
取方向上的单位向量sew平面构造公式表述为：
[0109][0110]
将结果排列得到∑＝[x
∑
，y
∑
，z
∑
]。
[0111]
在完成上述准备工作之后，开始求取七自由度机械臂的逆解，分为以下几个步骤：
[0112]
步骤1：计算θ4[0113]
由余弦定理可以得到由于θ4可以分为上下两种臂形，引入一个控制参数控制参数e可以反映肘关节相对于sw
′
的位置关系，进而得到：
[0114]
θ4＝e(π-acos(cos∠sew
′
))。
[0115]
步骤2：描述臂角基准面
[0116]
当θ
1-3
＝0时，臂角平面处于臂角基准面上，此时可以算出w的位置：
[0117][0118]
向量将向量代入sew平面构造公式得到臂角基准面的描述为
[0119]
步骤3：求解θ
1-3
[0120]
将代入sew平面构造公式，得到θ
1-3
分别取θ1，θ2，θ3时的臂角平面，记作此时的臂角平面旋转可以描述为与旋转矩阵rot(x，ψ)的运算其中rot(x，ψ)表示沿着x轴逆时针旋转角度ψ。
[0121]
记s处的旋转变换为rs，则可以得到rs·
∑0＝∑d，且∑0，∑d是正交矩阵，则有
[0122]
s处的旋转变换可以描述为：rs＝rot(z，θ1)rot(y，θ2)rot(z，θ3)，即：
[0123][0124]
其中，si＝sinθi，ci＝cosθi(i＝1，...，7)，*表示可忽略的项。
[0125]
由上式容易得到cos2＝r
s33
，由于θ2可以分为左右两种臂形，引入一个控制参数控制参数s可以反映大臂相对于bs的位置关系，进而得到：
[0126]
θ2＝s
·
acos(r
s33
)；
[0127]
θ1＝atani2(s
·rs23
，s
·rs13
)；
[0128]
θ3＝atan2(s
·rs32
，-s
·rs31
)。
[0129]
步骤4：求解θ
5-7
[0130]
记w处的旋转变换为rw，则有以下等式成立：
[0131][0132]
其中，rs·
rot(y，θ4)为正交矩阵，则有：
[0133][0134]
w处的旋转变换可以描述为：rw＝rot(z，θ5)rot(y，θ6)rot(z，θ7)，即：
[0135][0136]
其中，si＝sinθi，ci＝cosθi(i＝1，...，7)，*表示可忽略的项。
[0137]
由上式容易得到cosθ6＝r
w33
，由于θ6可以分为两种臂形，引入一个控制参数控制参数w可以反映手腕的翻转情况，进而得到：
[0138]
θ6＝w
·
acos(r
w33
)；
[0139]
θ5＝atan2(w
·rw23
，w
·rw13
)；
[0140]
θ7＝atan2(w
·rw32
，-w
·rw31
)。
[0141]
综上所述，给定末端位姿和臂角ψ，可以求解出八组逆解，若给定三个控制参数e，s，w，则可以唯一确定一组逆解。以上的分析是逆解的整体思路和方法。
[0142]
2、实施方案中的关键点
[0143]
2.1、dh参数标定及dh坐标系建系关乎后续计算的难度和复杂度，它是整个逆解过程的一个基础工作，合理的dh坐标系建系对后续的工作有极大的帮助。
[0144]
2.2、臂角平面的数学描述是该实施方案的核心，如何通过简单的几何关系找出比较合理的臂角平面描述是逆解过程中非常关键的一步。
[0145]
2.3、机械臂关节坐标系旋转关系描述以及构建尽可能简单的旋转矩阵关系式是得出逆解结果的核心步骤，对于简化逆解计算起到了关键作用。
[0146]
3、实施方案达到的效果
[0147]
为了验证以上实施方案的可行性以及效果，在python环境下进行演算，按图1所示的dh坐标系，给出一组肘部偏置的七自由度机械臂dh参数为：d1＝0.07m，d3＝0.32m，d4＝0.12m，d5＝0.32m，d7＝0.07m。任取一组位于机械臂工作空间内的末端位姿：
[0148][0149]
臂角取值ψ＝0rad和ψ＝π/4rad时，分别计算出八组逆解如图3和图4所示。分别对这16组逆解进行验算得到的结果如图5所示。
[0150]
计算结果表明，该实施方案计算出的逆解结果与机械臂末端位姿一致，且计算误差极小，计算时间约为1ms左右，有比较好的效果。
[0151]
当然，以上仅为本发明较佳实施方式，并非以此限定本发明的使用范围，故，凡是在本发明原理上做等效改变均应包含在本发明的保护范围内。