CN105014666B - 一种多自由度机械手自主抓取逆解工程算法 - Google Patents

Abstract

本发明涉及机器人控制技术领域，具体涉及一种多自由度机械手自主抓取逆解工程算法，在求取各关节角度时，先求出腰部回转角度，进而转化为平面问题，有利于求解效率效率，并且避免了逆矩阵求解方式带来的复杂性，以及操作人员的精神负担较大、容易疲劳等问题，进一步提高了抓取效率。此外，本发明中的目标位置采用双目视觉摄像单元获取或者人工判断，以供上位机控制系统确定所述可疑目标相对于所述车体的三维坐标，并反馈回相应的控制指令，可以进一步实现对目标的精确定位及抓取，从而也进一步提高了抓取效率。

Description

一种多自由度机械手自主抓取逆解工程算法

技术领域

本发明属于机器人控制技术领域，具体涉及一种多自由度机械手自主抓取逆解工程算法。

背景技术

随着科学技术不断发展，以及机器人取代人工的领域迅速增加，各类机器人的研制已成为世界各国和军队共同关注的课题。由于机器人的作业目标质量状况不明，放置的位置随机性较大，抓取过程中随时有意外发生的可能，因此，采用机器人进行作业，降低处理难度，避免工作人员的意外发生，对提高科技整体技术水平和作业效率具有重要意义。

世界上已有的车载机械手型机器人包括履带式“手推车”、“超级手推车”MPR-800型多功能智能机器人、“安德鲁斯HD-I”机器人、“灵蜥A”和“灵蜥B”排爆机器人、“RAPTOP-EOD”中型排爆机器人等。而控制系统是机器人中至关重要的一部分，控制系统的核心是逆运动算法，其好坏程度直接影响着机器人的功能、可靠程度及操作性能等。

针对机器人作业环境的特殊情况，可靠性和效率是它的一个重要因素。如果机器人的控制系统不可靠，将会引入新的不安全因素，不但没能解决问题，反而会促发矛盾问题的升级。在多自由度的机械手操作时，通常的做法是采用单关节的远程遥控方式，这对机械手末端手爪的定位抓取造成了极大困难。操作人员必须非常熟悉排爆机器人的操作，通过远处的摄像头判断机械手末端位置，并且快速且准确的操作每个关节，使机械手末端的手爪刚好对可疑目标并进行抓取。该种方式对操作人员的精神负担较大，容易疲劳而且操作效率较低。如果采用逆矩阵求解方法，计算复杂，且求解不直观。

发明内容

本发明针对现有技术中机械手的控制系统采用单关节的遥控方式，导致机械手末端定位抓取困难，造成操作人员的精神负担较大，容易疲劳而且操作效率较低等问题，提供一种能够实现机械手多关节联动的逆运动求解工程算法。

本发明通过以下技术方案实现该目的：

本发明提出的多自由度逆解方法没有采用逆矩阵方法求解，把空间的问题简化为平面问题，简化了求逆解的过程，该方法包括以下步骤(1)至步骤(4)：

(1)根据实际的目标图像判断抓取目标点P(X,Y,Z)，且不考虑目标姿态；

(2)根据机械手的正解方法，设定4自由度机械手的各关节参数为(θ₁,θ₂,θ₃,θ₄)，第4个关节依靠人工调整，因此求出目标点与前3个关节的关系；如果是5自由度，可固定第2和第3，或者第3和第4关节的相对位置，简化为4自由度机械手；

(3)求逆解时先转腰，求出θ₁，后面的求解简化为平面求解问题；

(4)计算θ₂,θ₃。

进一步，所述步骤(1)中，目标点P(X,Y,Z)由人工设定或者由机器人视觉提供。

进一步，所述步骤(2)中，不考虑最后一个关节手爪的旋转，将目标点P(X,Y,Z)定位于机械手爪中心，在机械手臂上固定一个平面坐标系{xy}，这是为了将空间坐标系的问题简化到平面坐标系解决，减少了变量，使得空间变量求解更加简单，该平面坐标系{xy}的原点与空间基坐标系{XYZ}的原点重合，即固定在机器人的基坐标系的原点；平面坐标系{xy}的y轴与空间坐标系{XYZ}的Z轴重合，坐标系的关系如图1所示。

无论机械臂绕腰关节怎么旋转，坐标系{xy}始终与机械手处于同一平面内，该坐标系是辅助的动态平面坐标系，那么点P在该坐标系{xy}中的坐标记为(x,y),根据几何学知识可以求得该坐标如下：

$\left\{\begin{array}{c}x=OA+AB=L_{1}cos{\mathrm{\θ}}_2+L_{2}cos({\mathrm{\θ}}_3-{\mathrm{\θ}}_2)\\ y=AD-CD=L_{1}sin{\mathrm{\θ}}_2-L_{2}sin({\mathrm{\θ}}_3-{\mathrm{\θ}}_2)\end{array}\right.$

最终要求得点P在机器人基坐标系{XYZ}中的坐标(X,Y,Z)，点P在平面动坐标系{xy}与机器人基坐标系{X,Y,Z}满足以下关系式： $\left\{\begin{array}{c}X=x{\mathrm{cos\θ}}_1\\ Y={\mathrm{xsin\θ}}_1\\ Z=y\end{array}\right.,$ 所以点P在机器人基坐标系下的坐标为： $\left\{\begin{array}{c}X=L_{1}cos{\mathrm{\θ}}_{1}cos{\mathrm{\θ}}_2+L_{2}cos{\mathrm{\θ}}_{1}cos({\mathrm{\θ}}_3-{\mathrm{\θ}}_2)\\ Y=L_{1}sin{\mathrm{\θ}}_1\cos {\mathrm{\θ}}_2+L_2\sin {\mathrm{\θ}}_1\cos ({\mathrm{\θ}}_3-{\mathrm{\θ}}_2)\\ Z=L_1\sin {\mathrm{\θ}}_2-L_2\sin ({\mathrm{\θ}}_3-{\mathrm{\θ}}_2)\end{array}\right.$

进一步，所述步骤(3)、(4)中，已知机器人末端的三维坐标(X,Y,Z)的值，求解关节θ₁,θ₂,θ₃的值，多功能水下爬行机器人运动学逆解几何图解图如图1所示。

在机器人基坐标系下可以解的θ₁的表达式θ₁＝arctan(Y/X)(根据机械臂的空间位置信息求取角度)；

由正解的结论可得：

X²+Y²+Z²＝L₁ ²+L₂ ²+2L₁L₂(cos(θ₃-θ₂)cos(θ₂)-sin(θ₃-θ₂)sin(θ₂))

X²+Y²+Z²＝L₁ ²+L₂ ²+2L₁L₂ cos(θ₃-θ₂+θ₂)＝L²

可得 ${\mathrm{cos\θ}}_3=\frac{L^2-L_1^2-L_2^2}{2L_1{L}_2},$

利用三角形的余弦定理可解得根据实际可取 ${\mathrm{\θ}}_3=\mathrm{\π}\±arccos\frac{L_1^2+L_2^2-L^2}{2L_1{L}_2},$ 取 ${\mathrm{\θ}}_3=\mathrm{\π}-arccos\frac{L_1^2+L_2^2-L^2}{2L_1{L}_2}$

将包含θ₂,θ₃的y-O-x平面

θ₂＝∠1+∠2，根据余弦定理 $\begin{array}{c}{L_2}^2={L_1}^2+L^2-2L_{1}L\cos (\∠1)\\ \∠1=\arccos \frac{{L_1}^2+L^2-{L_2}^2}{2L_{1}L}\end{array},$

$\∠2=arctan\frac{Z}{\sqrt{X^2+Y^2}};$ 因此，

${\mathrm{\θ}}_2=\∠1+\∠2=arccos\frac{{L_1}^2+L^2-{L_2}^2}{2L_{1}L}+arctan\frac{Z}{\sqrt{X^2+Y^2}}.$

其中，求出θ₁,θ₂,θ₃后，根据目标姿态，人工调整手爪姿态，实现合理位姿抓取。

相对于现有技术，本发明的有益效果为：本发明的多自由度机械手自主抓取逆解工程算法，在求取各关节角度时，采用先转腰且不考虑手爪姿态，再求取其他关节角度的方法，先求出腰部回转角度，进而转化为平面问题进行求解其他关节角度，大大提高了求解效率，并且避免了逆矩阵求解方式带来的复杂性，以及操作人员的精神负担较大、容易疲劳等问题，进一步提高了抓取效率。

附图说明

图1是本发明的多自由度机械手运动学正解几何图解示意图。

图2是本发明的简化成平面的关节参数求解示意图。

具体实施方式

以下结合附图及具体实施例对本发明进行详细描述。

实施例1。

一种多自由度机械手自主抓取逆解工程算法，把空间的问题简化为平面问题，简化了求逆解的过程，该方法包括以下步骤(1)至步骤(4)：

(1)根据实际的目标图像判断抓取目标点P(X,Y,Z)，且不考虑目标姿态；

(2)根据机械手的正解方法，设定4自由度机械手的各关节参数为(θ₁,θ₂,θ₃,θ₄)，第4个关节依靠人工调整，因此求出目标点与前3个关节的关系；如果是5自由度，可固定第2和第3，或者第3和第4关节的相对位置，简化为4自由度机械手；

(3)求逆解时先转腰，求出θ₁，后面的求解简化为平面求解问题；

(4)计算θ₂,θ₃。

进一步，所述步骤(1)中，目标点P(X,Y,Z)由人工设定或者由机器人视觉提供。

进一步，所述步骤(2)中，不考虑最后一个关节手爪的旋转，将目标点P(X,Y,Z)定位机械手爪中心，在机械手臂上固定一个平面坐标系{xy}，这是为了将空间坐标系的问题简化到平面坐标系解决，减少了变量，使得空间变量求解更加简单，该平面坐标系{xy}的原点与空间基坐标系{XYZ}的原点重合，即固定在机器人的基坐标系的原点；平面坐标系{xy}的y轴与空间坐标系{XYZ}的Z轴重合，坐标系的关系如图1所示。

无论机械臂绕腰关节怎么旋转，坐标系{xy}始终与机械手处于同一平面内，该坐标系是辅助的动态平面坐标系，那么点P在该坐标系{xy}中的坐标记为(x,y),根据几何学知识可以求得该坐标如下：

$\left\{\begin{array}{c}x=OA+AB=L_{1}cos{\mathrm{\θ}}_2+L_{2}cos({\mathrm{\θ}}_3-{\mathrm{\θ}}_2)\\ y=AD-CD=L_{1}sin{\mathrm{\θ}}_2-L_{2}sin({\mathrm{\θ}}_3-{\mathrm{\θ}}_2)\end{array}\right.$

最终要求得点P在机器人基坐标系{XYZ}中的坐标(X,Y,Z)，点P在平面动坐标系{xy}与机器人基坐标系{X,Y,Z}满足以下关系式： $\left\{\begin{array}{c}X=x{\mathrm{cos\θ}}_1\\ Y={\mathrm{xsin\θ}}_1\\ Z=y\end{array}\right.,$ 所以点P在机器人基坐标系下的坐标为： $\left\{\begin{array}{c}X=L_{1}cos{\mathrm{\θ}}_{1}cos{\mathrm{\θ}}_2+L_{2}cos{\mathrm{\θ}}_{1}cos({\mathrm{\θ}}_3-{\mathrm{\θ}}_2)\\ Y=L_{1}sin{\mathrm{\θ}}_1\cos {\mathrm{\θ}}_2+L_2\sin {\mathrm{\θ}}_1\cos ({\mathrm{\θ}}_3-{\mathrm{\θ}}_2)\\ Z=L_1\sin {\mathrm{\θ}}_2-L_2\sin ({\mathrm{\θ}}_3-{\mathrm{\θ}}_2)\end{array}\right.$

进一步，所述步骤(3)、(4)中，已知机器人末端的三维坐标(X,Y,Z)的值，求解关节θ₁,θ₂,θ₃的值，多功能水下爬行机器人运动学逆解几何图解图如图1所示。

在机器人基坐标系下可以解的θ₁的表达式θ₁＝arctan(Y/X)(根据机械臂的空间位置信息求取角度)；

由正解的结论可得：

X²+Y²+Z²＝L₁ ²+L₂ ²+2L₁L₂(cos(θ₃-θ₂)cos(θ₂)-sin(θ₃-θ₂)sin(θ₂))

X²+Y²+Z²＝L₁ ²+L₂ ²+2L₁L₂cos(θ₃-θ₂+θ₂)＝L²

可得 ${\mathrm{cos\θ}}_3=\frac{L^2-L_1^2-L_2^2}{2L_1{L}_2},$

利用三角形的余弦定理可解得根据实际可取 ${\mathrm{\θ}}_3=\mathrm{\π}\±arccos\frac{L_1^2+L_2^2-L^2}{2L_1{L}_2},$ 取 ${\mathrm{\θ}}_3=\mathrm{\π}-arccos\frac{L_1^2+L_2^2-L^2}{2L_1{L}_2}$

将包含θ₂,θ₃的y-O-x平面

θ₂＝∠1+∠2，根据余弦定理 $\begin{array}{c}{L_2}^2={L_1}^2+L^2-2L_{1}L\cos (\∠1)\\ \∠1=\arccos \frac{{L_1}^2+L^2-{L_2}^2}{2L_{1}L}\end{array},$

$\∠2=arctan\frac{Z}{\sqrt{X^2+Y^2}};$

因此， ${\mathrm{\θ}}_2=\∠1+\∠2=arccos\frac{{L_1}^2+L^2-{L_2}^2}{2L_{1}L}+arctan\frac{Z}{\sqrt{X^2+Y^2}}.$

其中，求出θ₁,θ₂,θ₃后，根据目标姿态，人工调整手爪姿态，实现合理位姿抓取。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (6)

1.一种多自由度机械手自主抓取逆解工程算法，其特征在于，包括以下步骤：

(1)根据实际的目标图像判断抓取目标点P(X,Y,Z)，且不考虑目标姿态；

(2)根据机械手的正解方法，设定4自由度机械手的各关节参数为(θ₁,θ₂,θ₃,θ₄)，第4个关节依靠人工调整，因此求出目标点与前3个关节的关系；如果是5自由度，可固定第2和第3个关节，或者第3和第4个关节的相对位置，简化为4自由度机械手；

(3)求逆解时先转腰，求出θ₁的数值，后面的求解简化为平面求解问题；

(4)计算θ₂,θ₃的数值。

2.根据权利要求1所述的多自由度机械手自主抓取逆解工程算法，其特征在于，所述步骤(1)中目标点P(X,Y,Z)由人工设定或者由机器人视觉提供。

3.根据权利要求1所述的多自由度机械手自主抓取逆解工程算法，其特征在于，所述步骤(2)中，不考虑最后一个关节手爪的旋转，将目标点P(X,Y,Z)定位机械手爪中心，在机械手臂上固定一个平面坐标系{xy}，该平面坐标系{xy}的原点与空间基坐标系{XYZ}的原点重合，即固定在机器人的基坐标系的原点；平面坐标系{xy}的y轴与空间坐标系{XYZ}的Z轴重合；

无论机械臂绕腰关节怎么旋转，坐标系{xy}始终与机械手处于同一平面内，该坐标系是辅助的动态平面坐标系，那么点P在该坐标系{xy}中的坐标记为(x,y),根据几何学知识可以求得该坐标如下：

$\left\{\begin{array}{c}x=OA+AB=L_{1}cos{\mathrm{\θ}}_2+L_{2}cos({\mathrm{\θ}}_3-{\mathrm{\θ}}_2)\\ y=AD-CD=L_{1}sin{\mathrm{\θ}}_2-L_{2}sin({\mathrm{\θ}}_3-{\mathrm{\θ}}_2)\end{array}\right.$

最终要求得点P在机器人基坐标系{XYZ}中的坐标(X,Y,Z)，点P在平面动坐标系{xy}与机器人基坐标系{X,Y,Z}满足以下关系式：所以点P在机器人基坐标系下的坐标为：

其中，O点为机械臂回转中心，OZ为机械臂回转轴线，X-O-Y平面为机械臂回转平面，OD为大臂回转中心到小臂回转中心的距离L1(称为大臂)，DP为小臂回转中心到达目标点中心的距离L2(称为小臂)，OA和AB分别为OD和DP在X-O-Y平面的投影，AD为小臂回转中心到达X-O-Y平面的距离，PB为P点到X-O-Y平面的距离，CD为D点与P点到达X-O-Y平面的距离差值。

4.根据权利要求1所述的多自由度机械手自主抓取逆解工程算法，其特征在于，所述步骤(3)中，已知机器人末端的三维坐标(X,Y,Z)的值，求解关节θ₁,θ₂,θ₃的值，根据机械臂的空间位置信息求取角度，在机器人基坐标系下可以解的θ₁的表达式为：

θ₁＝arctan(Y/X)；

由正解的结论可得：

$\begin{array}{c}{X}^2+Y^2+Z^2={L_1}^2+{L_2}^2+2L_1{L}_2\left(\cos \right({\mathrm{\θ}}_3-{\mathrm{\θ}}_2\left)\cos \right({\mathrm{\θ}}_2)-\sin ({\mathrm{\θ}}_3-{\mathrm{\θ}}_2\left)\sin \right({\mathrm{\θ}}_2\left)\right)\\ X^2+Y^2+Z^2={L_1}^2+{L_2}^2+2L_1{L}_2\cos \left({\mathrm{\θ}}_3-{\mathrm{\θ}}_2+{\mathrm{\θ}}_2\right)=L^2\end{array},$

可得

利用三角形的余弦定理可解得：根据实际可取选取

将包含θ₂,θ₃的y-O-x平面

θ₂＝∠1+∠2，根据余弦定理：

L₂ ²＝L₁ ²+L²-2L₁Lcos(∠1)

$\∠1=\arccos \frac{{L_1}^2+L^2-{L_2}^2}{2L_{1}L}$

$\∠2=\arctan \frac{Z}{\sqrt{X^2+Y^2}};$

因此，

其中，求出θ₁,θ₂,θ₃后，根据目标姿态，人工调整手爪姿态，实现合理位姿抓取；L为P点到达O点的距离，∠1为P点与机械臂回转中心O连线与大臂L1的夹角，∠2为P点与机械臂回转中心O连线与X-O-Y平面中X轴的夹角。

5.根据权利要求1所述的多自由度机械手自主抓取逆解工程算法，其特征在于，求出θ₁,θ₂,θ₃后，根据目标姿态，人工调整手爪姿态，实现合理位姿抓取。

6.根据权利要求4所述的多自由度机械手自主抓取逆解工程算法，其特征在于，计算θ₂,θ₃不使用逆矩阵求解方法，采用先转腰且不考虑手爪姿态，再求取其他关节角度的方法。