CN102243620B - 六关节机械臂逆运动学问题的快速求解方法 - Google Patents

Abstract

传统的机械臂运动控制技术需要先求出机械臂的运动学方程，再根据此方程运动学方程求相应的逆运动学方程的全部解并从中选择最优解。这种运动控制技术运算复杂，实时性差，严重影响了机械臂的灵巧性。本发明针对6关节机械臂，给出一种运算更加简单的直接获取逆运动学最优解的方法，以提高机械臂快速响应能力。本发明的特点在于根据对末端的姿态和位置的要求，直接求出最优解，即先求出第5个坐标系的坐标原点和第4、5、6个关节角的旋转角度，再根据第5个坐标系原点的坐标以及第4、5、6个关节角的旋转角度求出第1、2、3个关节角的旋转角度。

Description

六关节机械臂逆运动学问题的快速求解方法

技术领域

本发明涉及一种机械臂逆运动学方程的快速求解方法。

背景技术

自1962年第一台工业机器人应用于General Motor公司的生产线以来，机器人技术不断发展、完善和成熟，目前已广泛应用于制造业、服务业、娱乐业、科学探索、医学、军事和航天等各个领域。工业机器人可以代替人类完成大量高质量要求的工作，如汽车制造，家电产品制造，化工及食品包装，物料的搬运等。汽车制造业利用机器人在汽车自动化生产线完成焊接、喷漆、装配等工作。在服务业中，各种服务用智能机器人如导游机器人、护士助手、步行功能训練机器人、导盲机器人等已逐渐进入市场。21世纪的服务用智能机器人除了可以为家庭从事简单的家务劳动和健康护理工作外，还可以从事一些劳动强度大，工作重复性强的工作，如环境保护，打扫以及公共交通服务等。在医学方面机器人主要作为医疗设备辅助医生和医务工作者从事医疗、诊断、医学研究和教学。如远程手术机器人、医用微型机器人等。机器人在军事领域的应用包括代替士兵执行拆除炸弹、扫雷、侦察和攻击等各种任务。20世纪70年代，美国率先提出了在宇宙空间利用机器人系统的概念，旨在通过机器人的应用进行设备的组装、回收、维修，以及在极其恶劣的空间环境下完成一些人类难以完成的舱外活动。目前，空间机器人研究主要集中在卫星、航天飞机以及国际空间站上使用的空间机械臂。

机器人之所以能够胜任各种工作靠的是机械臂，而机械臂的每一个动作又是由人类预先编制好的程序控制的。机器人的智能和功能以及灵巧程度决定于相应的程序，而这些程序又与机械臂的逆运动学方程的求解方法密不可分。传统的机械臂的逆运动学方程的求解通常是求出多个解，然后在其中选择最优者。这就严重影响了机器人的响应速度，使机器人的运动更加迟钝。本发明旨在针对最常用的6关节机械臂，给出一种直接获取最优解，并且运算更加简单的方法，以提高机械臂快速响应能力。

发明内容

本发明的目的是对最常用的6关节机械臂，给出一种直接获取逆运动学最优解，并且运算更加简单的方法，以提高机械臂快速响应能力。

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

本发明的有益效果在于：不需要先列出机械臂“正”运动学方程的解，再根据此方程求逆运动学方程的全部解，然后从中选择最优解，而是根据对末端的姿态和位置的要求，直接求出最优解，即先求出第5个坐标系的坐标原点和第4、5、6个关节角的旋转角度，再根据第5个坐标系原点的坐标以及第4、5、6个关节角的旋转角度求出第1、2、3个关节角的旋转角度。

附图说明

图1是6关节机械臂结构及其各关节坐标系示意图，图中，各坐标系均为右手系；Z₁，Y₂，Y₃，Y₄，Z₅，X₆与相应的转轴平行；Y₁，X₂，X₃，Y₄，X₅，X₆与相应的链杆平行；坐标系O-X₅Y₅Z₅的原点位于X₁Z₁平面之内。

具体实施方式

机械臂的结构如图1所示。以O，O₁，…，O₆分别表示第1，2，…，6个坐标系的原点，且假定

|OO₁|＝d₁，|O₂O₃|＝a₂，|O₅O₆|＝a₄

Y₃和Y₄所在直线之间的距离为a₃，O₅在X₁Z₁平面内。按照如此的机械臂结构，对于给定的末端位置(X，Y，Z)，滚动角、俯仰角、方位角分别为θ、ψ，求解逆运动学方程可分为两步，即根据末端姿态求解θ₄，θ₅和θ₆以及根据末端姿态求解(X₅，Y₅，Z₅)并由此求出关节角度θ₁，θ₂和θ₃。

1)、(X₅，Y₅，Z₅)的确定

由于机械臂末端相对于O-X₀Y₀Z₀的滚动角、俯仰角、方位角分别为

θ、ψ，则由坐标系O-X₀Y₀Z₀到末端坐标系的变换矩阵为

因此姿态矩阵可以表示为

而

(r₁₁，r₂₁，r₃₁)＝(cosθcosψ，cosθsinψ，-sinθ)

是末端坐标系横轴上的单位向量，代表末端的方向向量，记为(l，m，n)。

由于末端坐标系原点为(X，Y，Z)，第五个坐标系的原点为(X₅，Y₅，Z₅)，于是

(X₅，Y₅，Z₅)-(X，Y，Z)＝k(l，m，n)

即

$\sqrt{{(X-X_5)}^2+{(Y-Y_5)}^2+{(Z-Z_5)}^2}=a_4$

因此

$\sqrt{{\left(\mathrm{kl}\right)}^2+{\left(\mathrm{km}\right)}^2+{\left(\mathrm{kn}\right)}^2}=a_4$

$k=\±\frac{500}{\sqrt{l^2+m^2+n^2}}=\±a_4$

而(X₅，Y₅，Z₅)比(X，Y，Z)离坐标原点更近，因此

(X+kl)²+(Y+km)²+(Z+kn)²-(X²+Y²+Z²)＜0

k²(l²+m²+n²)+2k(lX+mY+nZ)＜0

k²+2k(lX+mY+nZ)＜0

因此k与lX+mY+nZ异号，于是

(X₅，Y₅，Z₅)＝(X，Y，Z)-sign(lX+mY+nZ)·a₄(l，m，n)

2)、末端坐标

(X₅，Y₅，Z₅)的坐标为

${\left(\begin{array}{c}{\cos \mathrm{\θ}}_1(a_2\cos {\mathrm{\θ}}_2+a_3\cos ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3))\\ {\sin \mathrm{\θ}}_1(a_2\cos {\mathrm{\θ}}_2+a_3\cos ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3))\\ d_1-a_2\sin {\mathrm{\θ}}_2-a_3\sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)\end{array}\right)}^T$

(X₆，Y₆，Z₆)的姿态矩阵T满足

$T^{\′}=\left(\begin{array}{ccc}1& 0& 0\\ 0& c_6& s_6\\ 0& -s_6& c_6\end{array}\right)\left(\begin{array}{ccc}{c}_5& s_5& 0\\ {-s}_5& c_5& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}{c}_{234}& 0& -s_{234}\\ 0& 1& 0\\ s_{234}& 0& c_{234}\end{array}\right)\left(\begin{array}{ccc}{c}_1& s_1& 0\\ -s_1& c_1& 0\\ 0& 0& 1\end{array}\right)$

$=\left(\begin{array}{ccc}1& 0& 0\\ 0& c_6& s_6\\ 0& -s_6& c_6\end{array}\right)\left(\begin{array}{ccc}{c}_5& s_5& 0\\ -s_5& c_5& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}{c}_1{c}_{234}& s_1{c}_{234}& -s_{234}\\ -s_1& c_1& 0\\ c_1{s}_{234}& s_1{s}_{234}& c_{234}\end{array}\right)$

$=\left(\begin{array}{ccc}1& 0& 0\\ 0& c_6& s_6\\ 0& -s_6& c_6\end{array}\right)\left(\begin{array}{ccc}{c}_1{c}_5{c}_{234}-s_1{s}_5& s_1{c}_5{c}_{234}+c_1{s}_5& {-c}_5{c}_{234}\\ -c_1{s}_5{c}_{234}-s_1{c}_5& -s_1{s}_5{c}_{234}+c_1{c}_5& s_5{s}_{234}\\ c_1{s}_{234}& s_1{s}_{234}& c_{234}\end{array}\right)$

$=\left(\begin{array}{ccc}{c}_1{c}_5{c}_{234}-s_1{s}_5& s_1{c}_5{c}_{234}+c_1{s}_5& -c_5{s}_{234}\\ -c_1{s}_5{c}_6{c}_{234}+c_1{s}_6{s}_{234}-s_1{c}_5{c}_6& -s_1{s}_5{c}_6{c}_{234}+s_1{s}_6{s}_{234}+c_1{c}_5{c}_6& s_5{c}_6{s}_{234}+s_6{c}_{234}\\ c_1{s}_5{s}_6{c}_{234}+c_1{c}_6{s}_{234}+s_1{c}_5{s}_6& s_1{s}_6{c}_{234}{s}_5+s_1{c}_6{s}_{234}-c_1{c}_5{s}_6& -s_5{s}_6{s}_{234}+c_6{c}_{234}\end{array}\right)$

其中c_i＝cosθ_i，，s_i＝sinθ_i，(i＝1，2，…，6)

由此得X₆的方向向量，即上述矩阵的第一行向量为

(c₁c₅c₂₃₄-s₁s₅ s₁c₅c₂₃₄+c₁s₅-c₅s₂₃₄)

因此(X₆，Y₆，Z₆)坐标系的原点坐标为

$\left(\begin{array}{c}{X}_5\\ Y_5\\ Z_5\end{array}\right)+a_4\left(\begin{array}{c}{c}_1{c}_5{c}_{234}-s_1{s}_5\\ s_1{c}_5{c}_{234}+c_1{s}_5\\ -c_5{s}_{234}\end{array}\right)$

$=\left(\begin{array}{c}{\cos \mathrm{\θ}}_1(a_2\cos {\mathrm{\θ}}_2+a_3\cos ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3))\\ {\sin \mathrm{\θ}}_1(a_2\cos {\mathrm{\θ}}_2+a_3\cos ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3))\\ d_1-a_2\sin {\mathrm{\θ}}_2-a_3\sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)\end{array}\right)+a_4\left(\begin{array}{c}{c}_1{c}_5{c}_{234}-s_1{s}_5\\ s_1{c}_5{c}_{234}+c_1{s}_5\\ -c_5{s}_{234}\end{array}\right)$

$=\left(\begin{array}{c}{c}_1(a_2{c}_2+a_3{c}_{23})+a_4(c_1{c}_{234}{c}_5-s_1{s}_5)\\ s_1(a_2{c}_2+a_3{c}_{23})+a_4(s_1{c}_{234}{c}_5+c_1{s}_5)\\ d_1-a_2{s}_2-a_3{s}_{23}-a_4{s}_{234}{c}_5\end{array}\right)$

因此

$\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)=T^{\′}\left(\begin{array}{c}{X}_6\\ Y_6\\ Z_6\end{array}\right){+}^{\′}\left(\begin{array}{c}{X}_5\\ Y_5\\ Z_5\end{array}\right)+a_4\left(\begin{array}{c}{c}_1{c}_5{c}_{234}-s_1{s}_5\\ s_1{c}_5{c}_{234}+c_1{s}_5\\ {-c}_5{s}_{234}\end{array}\right)$

$=\left(\begin{array}{ccc}{c}_1{c}_{234}{c}_5-s_1{s}_5& {-c}_1{s}_5{c}_{234}{c}_6+c_1{s}_{234}{s}_6-s_1{c}_5{c}_6& c_1{s}_6{c}_{234}{s}_5+c_1{s}_{234}{c}_6+s_1{c}_5{s}_6\\ s_1{c}_{234}{c}_5+c_1{s}_5& -s_1{s}_5{c}_{234}{c}_6+s_1{s}_{234}{s}_6+c_1{c}_5{c}_6& s_1{s}_6{c}_{234}{s}_5+s_1{s}_{234}{c}_6-c_1{c}_5{s}_6\\ -s_{234}{c}_5& s_5{s}_{234}{c}_6+c_{234}{s}_6& -s_5{s}_{234}{s}_6+c_{234}{c}_6\end{array}\right)\left(\begin{array}{c}{X}_6\\ Y_6\\ Z_6\end{array}\right)$

$+\left(\begin{array}{c}{c}_1(a_2{c}_2+a_3{c}_{23})+a_4(c_1{c}_{234}{c}_5-s_1{s}_5)\\ s_1(a_2{c}_2+a_3{c}_{23})+a_4(s_1{c}_{234}{c}_5+c_1{s}_5)\\ d_1-a_2{s}_2-a_3{s}_{23}-a_4{s}_{234}{c}_5\end{array}\right)$

3)、转角的确定

由于

$\left(\begin{array}{c}{X}_5\\ Y_5\\ Z_5\end{array}\right)=\left(\begin{array}{c}{\cos \mathrm{\θ}}_1(a_2\cos {\mathrm{\θ}}_2+a_3\cos ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3))\\ \sin {\mathrm{\θ}}_1(a_2\cos {\mathrm{\θ}}_2+a_3\cos ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3))\\ d_1-a_2\sin {\mathrm{\θ}}_2-a_3\sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)\end{array}\right)$

因此

${\cos \mathrm{\θ}}_1=\frac{X_5}{\sqrt{X_5^2+Y_5^2}},$ ${\sin \mathrm{\θ}}_1=\frac{Y_5}{\sqrt{X_5^2+Y_5^2}}$

记

${\mathrm{\Δ}}_{c1}=\frac{X_5}{\sqrt{X_5^2+Y_5^2}},$ ${\mathrm{\Δ}}_{s1}=\frac{Y_5}{\sqrt{X_5^2+Y_5^2}}$

则

θ₁＝sign(Δ_s1)Arc cosΔ_c1

由此得

(a₂cosθ₂+a₃cos(θ₂+θ₃))²+(a₂sinθ₂+a₃sin(θ₂+θ₃))²=X₅ ²+Y₅ ²+(d₁-Z₅)²

a₂ ²+a₃ ²+2a₂a₃cosθ₃=X₅ ²+Y₅ ²+(d₁-Z₅)²

${\mathrm{\θ}}_3=\±\mathrm{Arc}\cos \frac{{X_5}^2+{Y_5}^2+{(d_1-Z_5)}^2-{a_2}^2-{a_3}^2}{{2a}_2{a}_3}$

且

$\left\{\begin{array}{c}{a}_2\cos {\mathrm{\θ}}_2+a_3(\cos {\mathrm{\θ}}_2\cos {\mathrm{\θ}}_3-\sin {\mathrm{\θ}}_2\sin {\mathrm{\θ}}_3)=\sqrt{X_5^2+Y_5^2}\\ a_2\sin {\mathrm{\θ}}_2+a_3(\sin {\mathrm{\θ}}_2\cos {\mathrm{\θ}}_3+\cos {\mathrm{\θ}}_2\sin {\mathrm{\θ}}_3)=d_1-Z_5\end{array}\right.$

$\left\{\begin{array}{c}(a_2+a_3\cos {\mathrm{\θ}}_3)\cos {\mathrm{\θ}}_2-a_3\sin {\mathrm{\θ}}_3\sin {\mathrm{\θ}}_2=\sqrt{X_5^2+Y_5^2}\\ a_3\sin {\mathrm{\θ}}_3\cos {\mathrm{\θ}}_2+(a_2+a_3\cos {\mathrm{\θ}}_3)\sin {\mathrm{\θ}}_2=d_1-Z_5\end{array}\right.$

$\left(\begin{array}{cc}{a}_2+a_3\cos {\mathrm{\θ}}_3& -a_3\sin {\mathrm{\θ}}_3\\ a_3\sin {\mathrm{\θ}}_3& a_2+a_3\cos {\mathrm{\θ}}_3\end{array}\right)\left(\begin{array}{c}{\cos \mathrm{\θ}}_2\\ {\sin \mathrm{\θ}}_2\end{array}\right)=\left(\begin{array}{c}\sqrt{X_5^2+Y_5^2}\\ d_1-Z_5\end{array}\right)$

$J=a_2^2+a_3^2{\cos }^2{\mathrm{\θ}}_3+{2a}_2{a}_3\cos {\mathrm{\θ}}_3+a_3^2{\sin }^2{\mathrm{\θ}}_3$

$=a_2^2+a_3^2+{2a}_2{a}_3\cos {\mathrm{\θ}}_3$

$={X_5}^2+{Y_5}^2+{(d_1-Z_5)}^2$

$\left\{\begin{array}{c}{\cos \mathrm{\θ}}_2=\frac{\sqrt{X_5^2+Y_5^2}(a_2+a_3\cos {\mathrm{\θ}}_3)+(d_1-Z_5)a_3\sin {\mathrm{\θ}}_3}{{X_5}^2+{Y_5}^2+{(d_1-Z_5)}^2}\\ {\sin \mathrm{\θ}}_2=\frac{(d_1-Z_5)(a_2+a_3\cos {\mathrm{\θ}}_3)-\sqrt{X_5^2+Y_5^2}{a}_3\sin {\mathrm{\θ}}_3}{{X_5}^2+{Y_5}^2+{(d_1-Z_5)}^2}\end{array}\right.$

令

$\left\{\begin{array}{c}{\mathrm{\Δ}}_{c2}=\frac{\sqrt{X_5^2+Y_5^2}(a_2+a_3\cos {\mathrm{\θ}}_3)+(d_1-Z_5)a_3\sin {\mathrm{\θ}}_3}{{X_5}^2+{Y_5}^2+{(d_1-Z_5)}^2}\\ {\mathrm{\Δ}}_{s2}=\frac{(d_1-Z_5)(a_2+a_3\cos {\mathrm{\θ}}_3)-\sqrt{X_5^2+Y_5^2}{a}_3\sin {\mathrm{\θ}}_3}{{X_5}^2+{Y_5}^2+{(d_1-Z_5)}^2}\end{array}\right.$

由此得

θ₂＝sign(Δ_s2)Arc cosΔ_c2

${\mathrm{\θ}}_4=\mathrm{Arc}\sin \left(\frac{\sin \mathrm{\θ}}{\cos {\mathrm{\θ}}_5}\right)-({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)$

卫星姿态矩阵为

也可以表示为

$T=\left(\begin{array}{ccc}{c}_1& -s_1& 0\\ s_1& c_1& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}{c}_{234}& 0& s_{234}\\ 0& 1& 0\\ -s_{234}& 0& c_{234}\end{array}\right)\left(\begin{array}{ccc}{c}_5& -s_5& 0\\ s_5& c_5& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& c_6& -s_6\\ 0& s_6& c_6\end{array}\right)$

$=\left(\begin{array}{ccc}{c}_1& -s_1& 0\\ s_1& c_1& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}{c}_{234}& 0& s_{234}\\ 0& 1& 0\\ {-s}_{234}& 0& c_{234}\end{array}\right)\left(\begin{array}{ccc}{c}_5& -s_5{c}_6& s_5{s}_6\\ s_5& c_5{c}_6& -c_5{s}_6\\ 0& s_6& c_6\end{array}\right)$

$=\left(\begin{array}{ccc}{c}_1& {-s}_1& 0\\ s_1& c_1& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}{c}_5{c}_{234}& -s_5{c}_6{c}_{234}+s_6{s}_{234}& s_5{s}_6{c}_{234}+c_6{s}_{234}\\ s_5& c_5{c}_6& -c_5{s}_6\\ -c_5{s}_{234}& s_5{c}_6{s}_{234}+s_6{c}_{234}& -s_5{s}_6{s}_{234}+c_6{c}_{234}\end{array}\right)$

因此

$=\left(\begin{array}{ccc}{c}_{234}{c}_5& -s_5{c}_{234}{c}_6+s_{234}{s}_6& s_6{c}_{234}{s}_5+s_{234}{c}_6\\ s_5& c_5{c}_6& -s_6{c}_5\\ -s_{234}{c}_5& s_5{s}_{234}{c}_6+c_{234}{s}_6& -s_5{s}_{234}{s}_6+c_{234}{c}_6\end{array}\right)$

即

cosθ₂₃₄cosθ₅＝cosθ₁cosθcosψ+sinθ₁cosθsinψ

              ＝cosθcos(ψ-θ₁)                                (1)

$-\sin {\mathrm{\θ}}_5\cos {\mathrm{\θ}}_{234}\cos {\mathrm{\θ}}_6+\sin {\mathrm{\θ}}_{234}\sin {\mathrm{\θ}}_6$

${\sin \mathrm{\θ}}_6\cos {\mathrm{\θ}}_{234}\sin {\mathrm{\θ}}_5+\sin {\mathrm{\θ}}_{234}\cos {\mathrm{\θ}}_6$

sinθ₅＝-sinθ₁cosθcosψ+cosθ₁cosθsinψ

      ＝cosθsin(ψ-θ₁)                                        (4)

sinθ₂₃₄cosθ₅＝sinθ                            (7)

1)当cos(ψ-θ₁)≠0时

(7)式的两端除以(1)式的两端得

${\mathrm{tg\θ}}_{234}=\frac{\mathrm{tg\θ}}{\cos (\mathrm{\ψ}-{\mathrm{\θ}}_1)}$

${\mathrm{\θ}}_4=\mathrm{Arctg}\frac{\mathrm{tg\θ}}{\cos (\mathrm{\ψ}-{\mathrm{\θ}}_1)}-({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)$

由(4)得

θ₅＝Arcsin(cosθsin(ψ-θ₁))

(5)，(6)式的两端分别乘

然后相加得

(5)，(6)式的两端分别乘

然后相加得

因此

$=-\mathrm{sign}\left(\mathrm{\θ}(\mathrm{\ψ}-{\mathrm{\θ}}_1)\right)\mathrm{Arc}\cos \left(\frac{\cos (\mathrm{\ψ}-{\mathrm{\θ}}_1)}{\left|\cos {\mathrm{\θ}}_5\right|}\right)$

$=-\sin g\left(\mathrm{\θ}(\mathrm{\ψ}-{\mathrm{\θ}}_1)\right)\mathrm{Arc}\cos \frac{\cos (\mathrm{\ψ}-{\mathrm{\θ}}_1)}{\sqrt{1-{\cos }^2\mathrm{\θ}{\sin }^2(\mathrm{\ψ}-{\mathrm{\θ}}_1)}}$

2)当cos(ψ-θ₁)＝0时

由(4)仍有θ₅＝Arc sin(cosθsin(ψ-θ₁))。

若sinθ＝0，由(4)式

sinθ₅＝cosθsin(ψ-θ₁)

sin²θ₅＝cos²θsin²(ψ-θ₁)＝cos²θ＝1

cos²θ₅＝0

而另一方面，利用cos(ψ-θ₁)＝0情况下的相应等式(加“’”表示)

得

利用矩阵变换求解得

而cos²θ₅＝0，于是

由于右端后二分量为零，因此有无穷多解。取s₂₃₄＝0得

因此

θ₄＝-(θ₂+θ₃)，θ₅＝0

若sinθ≠0，由(7)知cosθ₅≠0，因此由(1)得

cosθ₂₃₄＝0

${\mathrm{\θ}}_4=\frac{\mathrm{\π}}{2}-({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)$

由(5’)，(6’)得

而由(5’)

本发明未详尽描述的技术内容均为公知技术。

Claims (2)

1.一种六关节机械臂逆运动学问题的快速求解方法，其特征在于：各坐标系均为右手系；Z₁，Y₂，Y₃，Y₄，Z₅，X₆与相应的转轴平行；Y₁，X₂，X₃，Y₄，X₅，X₆与相应的链杆平行；坐标系O-X₅Y₅Z₅的原点位于X₁Z₁平面之内；先求出第5个坐标系的坐标原点和第4、5、6个关节角的旋转角度，再根据第5个坐标系原点的坐标以及第4、5、6个关节角的旋转角度求出第1、2、3个关节角的旋转角度。 

2.根据权利要求1所述的6关节机械臂逆运动学问题的快速求解方法，其特征在于：θ₁，θ₂，θ₃，θ₄，θ₅，θ₆分别表示第1，2，3，4，5，6个关节角，O₁，…，O₆分别表示第1，2，…，6个坐标系的原点，O是固定坐标系原点，且 

|OO₁|=d₁，|O₂O₃|=a₂，|O₅O₆|=a₄， 

Y₃和Y₄所在直线之间的距离为a₃，O₅在X₁Z₁平面内；按照如此的机械臂结构，对于给定的末端位置(X，Y，Z)，以及末端相对于坐标系O-X₀Y₀Z₀的滚动角、俯仰角、方位角分别为

θ、ψ，求解逆运动学问题包括根据末端姿态求解θ₄，θ₅和θ₆以及根据末端姿态求解(X₅，Y₅，Z₅)并由此求出关节角度θ₁，θ₂和θ₃： 

1)、(X₅，Y₅，Z₅)的确定 

由于机械臂末端相对于O-X₀Y₀Z₀的滚动角、俯仰角、方位角分别为

θ、ψ，则由坐标系O-X₀Y₀Z₀到末端坐标系的变换矩阵为 

因此姿态矩阵可以表示为 

而 

(r₁₁，r₂₁，r₃₁)=(cosθcosψ，cosθsinψ，-sinθ) 

是末端坐标系横轴上的单位向量，代表末端的方向向量，记为(l，m，n)；由于末端坐标系原点为(X，Y，Z)，第五个坐标系的原点为(X₅，Y₅，Z₅)，于是 

(X₅，Y₅，Z₅)-(X，Y，Z)=k(l，m，n) 

即 

因此 

而(X₅，Y₅，Z₅)比(X，Y，Z)离坐标原点更近，因此 

(X+kl)²+(Y+km)²+(Z+kn)²-(X²+Y²+Z²)<0 

k²(l²+m²+n²)+2k(lX+mY+nZ)<0 

k²+2k(lX+mY+nZ)<0 

因此k与lX+mY+nZ异号，于是 

(X₅，Y₅，Z₅)=(X，Y，Z)-sign(lX+mY+nZ)·a₄(l，m，n) 

2)、末端坐标 

(X₅，Y₅，Z₅)的坐标为 

(X₆，Y₆，Z₆)的姿态矩阵T满足 

其中c_i=cosθ_i，，s_i=sinθ_i，(i＝1，2，…，6) 

由此得X₆的方向向量，即上述矩阵的第一行向量为 

(c₁c₅c₂₃₄-s₁s₅s₁c₅c₂₃₄+c₁s₅-c₅s₂₃₄) 

因此(X₆，Y₆，Z₆)坐标系的原点坐标为 

因此 

3)、转角的确定 

由于

因此 

记

则 

θ₁=sign(△_s1)Arc cos△_c1

由此得 

(a₂cosθ₂+a₃cos(θ₂+θ₃))²+(a₂sinθ₂+a₃sin(θ₂+θ₃))²=X₅ ²+Y₅ ²+(d₁-Z₅)²

a₂ ²+a₃ ²+2a₂a₃cosθ₃=X₅ ²+Y₅ ²+(d₁-Z₅)²

且 

=X₅ ²+Y₅ ²+(d₁-Z₅)²

令 

由此得 

θ₂=sign(△_s2)Arc cos△_c2

卫星姿态矩阵为 

也可以表示为 

因此 

即 

cosθ₂₃₄cosθ₅＝cosθ₁cosθcosψ+sinθ₁cosθsinψ 

=cosθcos(ψ-θ₁)  (1) 

-sinθ₅cosθ₂₃₄cosθ₆+sinθ₂₃₄sinθ₆

sinθ₆cosθ₂₃₄sinθ₅s+sinθ₂₃₄cosθ₆

sinθ₅=-sinθ₁cosθcosψt+cosθ₁cosθsinψ 

=cosθsin(ψ-θ₁)  (4) 

sinθ₂₃₄cosθ₅=sinθ  (7) 

(i)当cos(ψ-θ₁)≠0时 

(7)式的两端除以(1)式的两端得 

由(4)得 

θ₅=Arcsin(cosθsin(ψ-θ₁)) 

(5)，(6)式的两端分别乘

然后相加得 

(5)，(6)式的两端分别乘

然后相加得 

因此 

(ii)cos(ψ-θ₁)=0时 

由(4)仍有θ₅=Arcsin(cosθsin(ψ-θ₁))；

若sinθ=0，由(4)式 

sinθ₅=cosθsin(ψ-θ₁) 

sin²θ₅=cos²θsin²(ψ-θ₁)=cos²θ=1 

cos²θ₅=0 

而另一方面，利用cos(ψ-θ₁)=0情况下的相应等式 

得 

利用矩阵变换求解得 

而cos²θ₅=0，于是 

由于右端后二分量为零，因此有无穷多解；取s₂₃₄=0得

因此 

若sinθ≠0，由(7)知cosθ₅≠0，因此由(1)得 

cosθ₂₃₄=0 

由(5’)，(6’)得 

而由(5’) 

。

Images