CN102672720B - 一种三关节机械臂的控制方法 - Google Patents

Abstract

本发明公开了一种三关节机械臂的控制方法，仅基于三关节机械臂本身的结构和尺寸特点，求出三段链杆绕对应的关节轴应该转过的角度，从而将三关节机械臂的末端送至需要的位置。由于本发明的方法运算量小，运算过程简单，且本发明不需要求解逆运动学方程，而是直接获取三关节机械臂的移动方式，因此能够快速地对移动命令进行响应，将三关节机械臂的末端精确地定位至需要的位置，实现三关节机械臂机器人的活动需要，提高机器人的快速响应能力。

Description

一种三关节机械臂的控制方法

技术领域

本发明涉及机器人的机械臂控制技术，具体涉及一种三关节机械臂的控制方法。

背景技术

随着人类太空活动的增多，人类在太空的“财产”也越来越多，在这些财产中人造卫星占了绝大多数。如果这些卫星一旦发生故障，丢弃它们再发射新的卫星将付出昂贵的代价，必须设法修理后使它们重新发挥作用。但如果派宇航员去修理，又牵涉到舱外活动的问题，而且由于航天器在太空中是处于强烈宇宙辐射的环境之下，人根本无法执行任务，所以只能依靠空间机器人。目前，空间机器人技术在空间应用中正逐步发挥着重大作用，空间机器人可以替代航天员完成空间作业，搬运和组装大型安装部件，对有毒品或危险品进行处理，对卫星实施捕获、装配或修理等等。根据空间作业的需要，空间机器人上大多安装有一个或多个模仿人手臂的多自由度机械臂，机械臂包括多段链杆和连接链杆的关节，机械臂的动作主要由各个关节旋转带动链杆运动来实现，动作过程由预先编制好的程序进行控制，这些程序与机械臂的逆运动学方程的求解方法密不可分，决定机器人的智能、功能以及灵巧程度，是机械臂控制问题的关键。现有的机械臂控制方法是首先列出机械臂的运动学方程，将机械臂末端的位置和姿态表示成各关节旋转角度以及各链杆长度的函数，再根据对机械臂末端的位置和姿态的要求求解逆运动学方程，得到机械臂各个关节所需的旋转角度。但是，由于逆运动学方程存在多个解，每次计算都需要从中选择最优解，导致该方法运算量大且复杂，使得机械臂的响应缓慢，机器人执行命令的动作迟钝，对于要求机械臂响应速度快的机器人，无法满足使用需求。

发明内容

鉴于此，本发明提供了一种三关节机械臂的控制方法，不需要求解逆运动学方程，而是直接获取三关节机械臂的移动方式。

本发明三关节机械臂的控制方法包括：仅基于三关节机械臂的尺寸得到所述三关节机械臂的三段链杆绕对应的关节轴应该转过的角度，以将所述三关节机械臂的末端移动至需要的位置。

有益效果：

本发明针对机器人的三关节机械臂的控制问题，给出一种不需要求解逆运动学方程的控制方法，能够直接获取各链杆应该转过的角度。该方法仅基于三关节机械臂本身的结构特点，巧妙地求出三段链杆绕对应的关节轴应该转过的角度，且运算量小，运算过程简单，能够大幅提高三关节机械臂的快速响应能力，提高机器人的运动性能。

附图说明

图1为三关节机械臂的结构及其坐标系。

图2是O、O₂、O₃和第3关节中心在XZ平面内的投影图，投影点分别为O，A，C和B。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

图1示出了某三关节机械臂的结构，该机械臂包括三段链杆和三个关节，三个关节分别以“1”、“2”和“3”标出，第1关节与第2关节之间是第1链杆，第2关节与第3关节之间是第2链杆，剩余的是第3链杆，第3链杆的末端可根据需要安装不同的部件，如抓取部件。每个关节包括固定部和转动部，转动部可绕其中心的关节轴转动任意角度，以实现机械臂的活动需要。

在上述三关节机械臂结构中建立4个坐标系，O-XYZ坐标系、O₁-X₁Y₁Z₁坐标系、O₂-X₂Y₂Z₂坐标系和O₃-X₃Y₃Z₃坐标系。O-XYZ坐标系原点O位于第1关节固定部的中心，且Z轴沿第1链杆指向第2关节的固定部，X轴、Y轴和Z轴的方向符合右手系法则，且X轴与第2链杆平行；O₁-X₁Y₁Z₁坐标系原点O₁位于第1关节转动部的中心(本实施例|OO₁|＝d₀)；O₂-X₂Y₂Z₂坐标系原点O₂位于第2关节的转动部的中心；O₃-X₃Y₃Z₃坐标系原点O₃位于第3链杆末端的中心。进一步地，O₁-X₁Y₁Z₁坐标系的参考位置由O-XYZ坐标系沿Z轴平移距离d₀得到；O₂-X₂Y₂Z₂坐标系的参考位置由O₁-X₁Y₁Z₁坐标系沿第1链杆平移d₁(这里d₁为第1关节转动部的中心与第2关节固定部的中心的距离)，再沿与Y₁轴平行的方向平移到O₂得到；O₃-X₃Y₃Z₃坐标系的参考位置由O₂-X₂Y₂Z₂坐标系沿第2链杆平移d₂，再沿第3链杆平移d₃得到(这里d₂为第2关节转动部的中心与第3关节固定部的中心的距离，d₃为第3关节转动部的中心与第3链杆末端中心的距离)。因此，在上述三关节机械臂结构中，|OO₁|＝d₀，O₁与Y₂轴之间的距离为d₁，O₂与第3关节的关节轴之间的距离为d₂，O₃与第3关节的关节轴之间的距离为d₃，O₃在X₁Z₁平面内，Y₃平行于Y₂，X₃位于第3链杆的中心线上。本发明全部实施例的角度的方向依据右手系法则确定，如绕Y轴旋转时拇指指向Y轴正方向，其余四指由Z到X的旋转方向为旋转角的正向。

基于上述三关节机械臂结构的坐标系，对于给定的第3链杆末端位置(x,y,z)，机械臂控制问题的关键在于求出相应的关节角θ₁，θ₂和θ₃，θ₁为第1链杆绕第1关节轴(Z轴)应该转过的角度，θ₂为第2链杆绕第2关节轴(Y₂轴)应该转过的角度，θ₃为第3链杆绕第3关节轴应该转过的角度，由此使第3链杆的末端由初始位置(0,0,d₀+d₁+d₂+d₃)移动到给定位置(x,y,z)。使用本发明的方法快速求解：

首先，将O，O₂，第3关节中心和O₃投影在XZ平面内，投影点分别记为O，A，B和C，并连接A和C(如图2所示)，其中|AB|＝d₂，|BC|＝d₃，|AO|＝d₀+d₁；将AC投影在XY平面内，建立直角三角形容易得到

为计算第1链杆绕第1关节轴(Z轴)应该转过的角度θ₁，将第3链杆的末端投影在XY平面内，投影点记为C′，连接OC′，在以OC′为斜边的直角三角形中，实数X,Y的四象限反正切函数为：

$A\tan 2(X,Y)=\left\{\begin{array}{cc}\mathrm{sign}\left(Y\right)\&CenterDot;\mathrm{A\; rctg}\frac{\left|Y\right|}{\left|X\right|}& X\&GreaterEqual;0\\ \mathrm{sign}\left(Y\right)\&CenterDot;(\mathrm{\&pi;}-\mathrm{A\; rctg}\frac{\left|Y\right|}{\left|X\right|})& X<0\end{array}\right.$

其中sign(Y)为符号函数，X≥0时为正，X＜0时为负。则，

θ₁＝Atan2(X,Y)

计算第2链杆绕第2关节轴(Y₂轴)应该转过的角度θ₂和第3链杆绕第3关节轴应该转过的角度θ₃，根据万能公式，

$\begin{array}{c}{\mathrm{\&theta;}}_3=\&angle;\mathrm{ABC}=\arccos \frac{{\left|\mathrm{AB}\right|}^2+{\left|\mathrm{BC}\right|}^2-{\left|\mathrm{AC}\right|}^2}{2\left|\mathrm{AB}\right|\&CenterDot;\left|\mathrm{BC}\right|}\\ =\arccos \frac{{d_2}^2+{d_3}^2-(x^2+y^2+{(d_0+d_1-z)}^2)}{{2d}_2{d}_3}\end{array}$

类似地，又有：

$\&angle;\mathrm{BAC}=\arccos \frac{{d_2}^2+(x^2+y^2+{(d_0+d_1-z)}^2)-{d_3}^2}{{2d}_2\sqrt{x^2+y^2+{(d_0+d_1-z)}^2}}$

$\&angle;\mathrm{CAO}=\arccos \frac{d_0+d_1-Z}{\sqrt{x^2+y^2+{(d_0+d_1-z)}^2}}$

因此

$\begin{array}{c}{\mathrm{\&theta;}}_2=\mathrm{\&pi;}-\&angle;\mathrm{BAO}=\mathrm{\&pi;}-\&angle;\mathrm{BAC}-\&angle;\mathrm{CAO}\\ =\mathrm{\&pi;}-\arccos \frac{{d_2}^2+(x^2+y^2+{(z-d_1-d_2)}^2)-{d_3}^2}{{2d}_2\sqrt{x^2+y^2+{(z-d_0-d_1)}^2}}-\arccos \frac{z-d_0-d_1}{\sqrt{x^2+y^2+{(z-d_0-d_1)}^2}}\end{array}$

以下以三关节机械臂第3链杆末端的给定位置为(2,-3,1)，以及d₀＝0.01，d₁＝3，d₂＝3和d₃＝3为例，利用本发明的方法求解各链杆绕相应关节轴应该转过的角度θ₁，θ₂和θ₃：

θ₁＝Atan2(x,y)＝-0.9828  (rad)，

$\left|\mathrm{AC}\right|=\sqrt{x^2+y^2+{(z-d_0-d_1)}^2}=4.1280,$

${\mathrm{\&theta;}}_3=\arccos \frac{{d_2}^2+{d_3}^2-(x^2+y^2+{(z-d_0-d_1)}^2)}{{2d}_2{d}_3}=1.5174\left(\mathrm{rad}\right),$

$\&angle;\mathrm{BAC}=\arccos \frac{{d_2}^2+(x^2+y^2+{(z-d_1-d_2)}^2)-{d_3}^2}{{2d}_2\sqrt{x^2+y^2+{(z-d_0-d_1)}^2}}=0.8121\left(\mathrm{rad}\right),$

$\&angle;\mathrm{CAO}=\arccos \frac{z-d_0-d_1}{\sqrt{x^2+y^2+{(z-d_0-d_1)}^2}}=-0.4869\left(\mathrm{rad}\right),$

θ₂＝π-∠BAC-∠CAO＝2.8164  (rad)，其中rad即弧度。

可见，采用本发明的方法，一旦给定三关节机械臂的第3链杆末端位置(x,y,z)，就能够根据三关节机械臂本身的结构特点得到各个链杆应该旋转的角度，从而将第3链杆末端精确地定位至需要的位置，实现三关节机械臂机器人的活动需要。

综上所述，以上仅为本发明的较佳实施例，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种三关节机械臂的控制方法，其特征在于，包括：仅基于三关节机械臂的尺寸得到所述三关节机械臂的三段链杆绕对应的关节轴应该转过的角度，以将所述三关节机械臂的末端移动至需要的位置；

所述三关节机械臂末端的初始位置的坐标为(0,0,d₀+d₁+d₂+d₃)，所述需要的位置的坐标为(x,y,z)，所述三关节机械臂的三段链杆绕对应的关节轴应该转过的角度分别为：第1链杆绕第1关节轴转过θ₁，第2链杆绕第2关节轴转过θ₂，并且第3链杆绕第3关节轴转过θ₃；其中，

${\mathrm{\&theta;}}_1=\left\{\begin{array}{cc}\mathrm{sign}\left(y\right)\&CenterDot;\mathrm{Arctg}\frac{\left|y\right|}{\left|x\right|}& x\&GreaterEqual;0\\ \mathrm{sign}\left(y\right)\&CenterDot;(\mathrm{\&pi;}-\mathrm{Arctg}\frac{\left|y\right|}{\left|x\right|})& x<0\end{array}\right.,$ 其中sign(y)为符号函数，x≥0时为正，x＜0时为负；

${\mathrm{\&theta;}}_2=\mathrm{\&pi;}-\arccos \frac{{d_2}^2+(x^2+y^2+{(z-d_1-d_2)}^2)-{d_3}^2}{{2d}_2\sqrt{x^2+y^2+{(z-d_0-d_1)}^2}}-\arccos \frac{z-d_0-d_1}{\sqrt{x^2+y^2+{(z-d_0-d_1)}^2}};$

${\mathrm{\&theta;}}_3=\arccos \frac{{d_2}^2+{d_3}^2-(x^2+y^2+{(d_0+d_1-z)}^2)}{{2d}_2{d}_3};$ 其中，

d₀为三关节机械臂的第1关节固定部的中心与第1关节转动部的中心之间的距离，

d₁为三关节机械臂的第1关节转动部的中心与第2关节固定部的中心之间的距离，

d₂为三关节机械臂的第2关节转动部的中心与第3关节固定部的中心之间的距离，

d₃为三关节机械臂的第3关节转动部的中心与第3链杆末端的中心之间的距离。