机器人基础之运动学逆解

概述

运动学逆解是指已知机器人末端位姿,求解各运动关节的位置,它是机器人运动规划和轨迹控制的基础。
以机械臂为例,其运动学逆解的求法主要有两类:数值解和解析解。
数值解法只能求出方程的特解,不能求出所有的解。
它的优点是计算简单,不需要进行矩阵操作;缺点是由于使用了迭代法(如牛顿迭代),实时性较差。
这里主要介绍解析解法。
研究发现,如果串联机械臂在结构上满足下面两个充分条件中的一个,就会有解析解。这两个充分条件也被称为Pieper准则,即:

  1. 三个相邻关节轴线交于一点
  2. 三个相邻关节轴线相互平行

现代绝大多数工业机械臂都满足Pieper准则的第一个充分条件,即机械臂末端的三个关节的轴线相交于一点,该点通常称为腕点。
满足第二个充分条件的机器人较少,最典型的例子是SCARA机器人。之前我所在的公司应用SCARA构型的机器人进行工件的上下料。
假设机械臂满足Pieper准则的第一个充分条件,则机械臂的运动学方程可以写为 6 0 T = 3 0 T 6 3 T {^0_6}T={^0_3}T{^3_6}T 60T=30T63T其中,   3 0 T \ {^0_3}T  30T规定了腕点的位置,   6 3 T \ {^3_6}T  63T规定了腕部的方位。因此,运动学逆解也分为两步,先求解 θ 1 \theta_1 θ1 θ 2 \theta_2 θ2 θ 3 \theta_3 θ3(腕点位置),再求解 θ 4 \theta_4 θ4 θ 5 \theta_5 θ5 θ 6 \theta_6 θ6(腕部方位)。

求解腕点位置

将固连在机械臂腕部的三个坐标系{4}、{5}、{6}的原点设在腕点,则腕点相对于基坐标系{0}的位置为: 0 P 6 o = 0 P 4 o = 1 0 T 2 1 T 3 2 T 3 P 4 o {^0}P_6o= {^0}P_4o={^0_1}T{^1_2}T{^2_3}T {^3}P_4o 0P6o=0P4o=10T21T32T3P4o其中,   3 P 4 o \ {^3}P_4o  3P4o即为变换矩阵   4 3 T \ {^3_4}T  43T的第4列,即 0 P 4 o = 1 0 T 2 1 T 3 2 T [ a 3 − d 4 s i n θ 3 d 4 c o s θ 3 1 ] (n) {^0}P_4o={^0_1}T{^1_2}T{^2_3}T \left[ \begin{matrix} a_3\\ -d_4 sin\theta_3\\ d_4 cos\theta_3\\ 1 \end{matrix} \right] \tag{n} 0P4o=10T21T32Ta3d4sinθ3d4cosθ31(n) [ f 1 ( θ 3 ) f 2 ( θ 3 ) f 3 ( θ 3 ) 1 ] = 3 2 T [ a 3 − d 4 s i n θ 3 d 4 c o s θ 3 1 ] (n) \left[ \begin{matrix} f_1(\theta_3) \\ f_2(\theta_3) \\ f_3(\theta_3) \\ 1 \end{matrix} \right] \tag{n}={^2_3}T \left[ \begin{matrix} a_3\\ -d_4 sin\theta_3\\ d_4 cos\theta_3\\ 1 \end{matrix} \right] f1(θ3)f2(θ3)f3(θ3)1=32Ta3d4sinθ3d4cosθ31(n)则有, f 1 ( θ 3 ) = a 3 c o s θ 3 + d 4 s i n θ 3 s i n α 3 + a 2 f_1(\theta_3)=a_3 cos\theta_3 +d_4 sin\theta_3sin\alpha_3 +a_2 f1(θ3)=a3cosθ3+d4sinθ3sinα3+a2 f 2 ( θ 3 ) = a 3 s i n θ 3 c o s α 2 − d 4 c o s θ 3 c o s α 2 s i n α 3 − d 4 s i n α 2 c o s α 3 − d 3 s i n α 2 f_2(\theta_3)=a_3 sin\theta_3 cos\alpha_2-d_4 cos\theta_3 cos\alpha_2 sin\alpha_3 -d_4 sin\alpha_2 cos\alpha_3 -d_3 sin\alpha_2 f2(θ3)=a3sinθ3cosα2d4cosθ3cosα2sinα3d4sinα2cosα3d3sinα2 f 3 ( θ 3 ) = a 3 s i n θ 3 s i n α 2 − d 4 c o s θ 3 s i n α 2 s i n α 3 + d 4 c o s α 2 c o s α 3 + d 3 c o s α 2 f_3(\theta_3)=a_3 sin\theta_3 sin\alpha_2-d_4 cos\theta_3 sin\alpha_2 sin\alpha_3 +d_4 cos\alpha_2 cos\alpha_3 +d_3 cos\alpha_2 f3(θ3)=a3sinθ3sinα2d4cosθ3sinα2sinα3+d4cosα2cosα3+d3cosα2 再令 [ g 1 ( θ 2 , θ 3 ) g 2 ( θ 2 , θ 3 ) g 3 ( θ 2 , θ 3 ) 1 ] = 2 1 T [ f 1 ( θ 3 ) f 2 ( θ 3 ) f 3 ( θ 3 ) 1 ] (n) \left[ \begin{matrix} g_1(\theta_2,\theta_3) \\ g_2(\theta_2,\theta_3) \\ g_3(\theta_2,\theta_3) \\ 1 \end{matrix} \right] \tag{n}={^1_2}T \left[ \begin{matrix} f_1(\theta_3) \\ f_2(\theta_3) \\ f_3(\theta_3) \\ 1 \end{matrix} \right] g1(θ2,θ3)g2(θ2,θ3)g3(θ2,θ3)1=21Tf1(θ3)f2(θ3)f3(θ3)1(n) 则有, g 1 ( θ 2 , θ 3 ) = c o s θ 2 f 1 − s i n θ 2 f 2 + a 1 g_1(\theta_2,\theta_3)=cos\theta_2 f_1-sin\theta_2 f_2 +a_1 g1(θ2,θ3)=cosθ2f1sinθ2f2+a1 g 2 ( θ 2 , θ 3 ) = s i n θ 2 c o s α 1 f 1 + c o s θ 2 c o s α 1 f 2 − s i n α 1 f 3 − d 2 s i n α 1 g_2(\theta_2,\theta_3)=sin\theta_2 cos\alpha_1f_1 + cos\theta_2 cos\alpha_1f_2 -sin\alpha_1 f_3 - d_2sin\alpha_1 g2(θ2,θ3)=sinθ2cosα1f1+cosθ2cosα1f2sinα1f3d2sinα1 g 3 ( θ 2 , θ 3 ) = s i n θ 2 s i n α 1 f 1 + c o s θ 2 s i n α 1 f 2 + c o s α 1 f 3 + d 2 c o s α 1 g_3(\theta_2,\theta_3)=sin\theta_2 sin\alpha_1f_1 + cos\theta_2 sin\alpha_1f_2 +cos\alpha_1 f_3 + d_2cos\alpha_1 g3(θ2,θ3)=sinθ2sinα1f1+cosθ2sinα1f2+cosα1f3+d2cosα1 因此 0 P 4 o = 1 0 T [ g 1 g 2 g 3 1 ] (n) {^0}P_4o={^0_1}T\left[ \begin{matrix} g_1\\ g_2\\ g_3\\ 1 \end{matrix} \right] \tag{n} 0P4o=10Tg1g2g31(n)由于坐标系{1}与基坐标系{0}原点重合、Z轴重合,仅存在绕Z轴的转动,因此, 1 0 T = [ cos ⁡ θ 1 − sin ⁡ θ 1 0 0 sin ⁡ θ 1 cos ⁡ θ 1 0 0 0 0 1 0 0 0 0 1 ] {^0_1}T= \left[ \begin{matrix} \cos\theta_1 & -\sin\theta_1 & 0& 0 \\ \sin\theta_1 & \cos\theta_1 & 0& 0 \\ 0 & 0 & 1 & 0\\ 0& 0& 0& 1 \end{matrix} \right] 10T=cosθ1sinθ100sinθ1cosθ10000100001进一步可得, 0 P 4 o = [ x y z 1 ] = [ cos ⁡ θ 1 − sin ⁡ θ 1 0 0 sin ⁡ θ 1 cos ⁡ θ 1 0 0 0 0 1 0 0 0 0 1 ] [ g 1 g 2 g 3 1 ] = [ g 1 c o s θ 1 − g 1 s i n θ 1 g 1 s i n θ 1 + g 1 c o s θ 1 g 3 1 ] {^0}P_4o=\left[ \begin{matrix} x\\ y\\ z\\ 1 \end{matrix} \right]=\left[ \begin{matrix} \cos\theta_1 & -\sin\theta_1 & 0& 0 \\ \sin\theta_1 & \cos\theta_1 & 0& 0 \\ 0 & 0 & 1 & 0\\ 0& 0& 0& 1 \end{matrix} \right]\left[ \begin{matrix} g_1\\ g_2\\ g_3\\ 1 \end{matrix} \right] =\left[ \begin{matrix} g_1cos\theta_1- g_1sin\theta_1\\ g_1sin\theta_1+g_1cos\theta_1\\ g_3\\ 1 \end{matrix} \right] 0P4o=xyz1=cosθ1sinθ100sinθ1cosθ10000100001g1g2g31=g1cosθ1g1sinθ1g1sinθ1+g1cosθ1g31 r = x 2 + y 2 + z 2 = g 1 2 + g 2 2 + g 3 2 r=x^2+y^2+z^2=g{^2_1}+g{^2_2}+g{^2_3} r=x2+y2+z2=g12+g22+g32 并且 z = g 3 z = g_3 z=g3代入 g 1 g_1 g1 g 2 g_2 g2 g 3 g_3 g3的表达式,可得: r = ( k 1 c o s θ 2 + k 2 s i n θ 2 ) 2 a 1 + k 3 r=(k_1 cos\theta_2 +k_2 sin\theta_2)2a_1 +k_3 r=(k1cosθ2+k2sinθ2)2a1+k3 z = ( k 1 s i n θ 2 − k 2 c o s θ 2 ) s i n α 1 + k 4 z =(k_1 sin\theta_2 - k_2 cos\theta_2)sin\alpha_1 +k_4 z=(k1sinθ2k2cosθ2)sinα1+k4 其中,
k 1 ( θ 3 ) = f 1 k_1(\theta_3)=f_1 k1(θ3)=f1 k 2 ( θ 3 ) = − f 2 k_2(\theta_3)=-f_2 k2(θ3)=f2 k 3 ( θ 3 ) = f 1 2 + f 2 2 + f 3 2 + a 1 2 + d 2 2 + 2 d 2 f 3 k_3(\theta_3)=f{^2_1}+f{^2_2}+f{^2_3} + a{^2_1} + d{^2_2} +2d_2 f_3 k3(θ3)=f12+f22+f32+a12+d22+2d2f3 k 4 ( θ 3 ) = f 3 c o s α 1 + d 2 c o s α 1 k_4(\theta_3)=f_3 cos\alpha_1 +d_2 cos\alpha_1 k4(θ3)=f3cosα1+d2cosα1 这里分三种情况进行讨论:

  1. a 1 = 0 a_1 =0 a1=0时。 r = k 3 ( θ 3 ) r=k_3 (\theta_3) r=k3(θ3),因为 r r r已知,所以可以求出 θ 3 \theta_3 θ3
  2. s i n α 1 = 0 sin\alpha_1 =0 sinα1=0时。 z = k 4 ( θ 3 ) z=k_4 (\theta_3) z=k4(θ3),因为 z z z已知,所以可以求出 θ 3 \theta_3 θ3
  3. a 1 a_1 a1 s i n α 1 sin\alpha_1 sinα1均不为0时,有 ( r − k 3 ) 2 ( 2 a 1 ) 2 + ( z − k 4 ) 2 s i n 2 α 1 = k 1 2 + k 2 2 \frac{(r-k_3)^2}{(2a_1)^2} + \frac{(z-k_4)^2}{sin^2\alpha_1} = k{^2_1}+k{^2_2} (2a1)2(rk3)2+sin2α1(zk4)2=k12+k22这里所有变量均为 θ 3 \theta_3 θ3的函数,求解关于 θ 3 \theta_3 θ3的非线性方程即可得到 θ 3 \theta_3 θ3。需要注意的是,更为准确地说,上述公式中所有变量均为 s i n θ 3 sin\theta_3 sinθ3 c o s θ 3 cos\theta_3 cosθ3的函数,可令 u = t a n θ 3 2 u=tan\frac{\theta_3}{2} u=tan2θ3 c o s θ = 1 − u 2 1 + u 2 cos\theta=\frac{1-u^2}{1+u^2} cosθ=1+u21u2 s i n θ = 2 u 1 + u 2 sin\theta=\frac{2u}{1+u^2} sinθ=1+u22u 由此,可将原非线性方程转化为关于变量 u u u的方程。解得 u u u之后再利用 θ 3 = 2 a r c t a n u \theta_3=2arctanu θ3=2arctanu得到 θ 3 \theta_3 θ3
    利用前面的公式 r = ( k 1 c o s θ 2 + k 2 s i n θ 2 ) 2 a 1 + k 3 r=(k_1 cos\theta_2 +k_2 sin\theta_2)2a_1 +k_3 r=(k1cosθ2+k2sinθ2)2a1+k3 这里代入 θ 3 \theta_3 θ3后,所有变量均为 θ 2 \theta_2 θ2的函数,类似的,求解该非线性方程即可得到 θ 2 \theta_2 θ2
    利用公式 x = g 1 c o s θ 1 − g 2 s i n θ 1 x=g_1 cos\theta_1 - g_2 sin\theta_1 x=g1cosθ1g2sinθ1代入 θ 2 \theta_2 θ2 θ 3 \theta_3 θ3,即可求得 θ 1 \theta_1 θ1
    到此,求解得到了 θ 1 \theta_1 θ1 θ 2 \theta_2 θ2 θ 3 \theta_3 θ3

求解腕部方位

z-y-z欧拉角

假设某坐标系分别依次绕动坐标系的Z轴、Y轴、Z轴转动 α \alpha α β \beta β γ \gamma γ角度,则旋转矩阵为 R = [ cos ⁡ α − sin ⁡ α 0 sin ⁡ α cos ⁡ α 0 0 0 1 ] [ cos ⁡ β 0 sin ⁡ β 0 1 0 − sin ⁡ β 0 cos ⁡ β ] [ cos ⁡ γ − sin ⁡ γ 0 sin ⁡ γ cos ⁡ γ 0 0 0 1 ] R=\left[ \begin{matrix} \cos\alpha& -\sin\alpha& 0 \\ \sin\alpha& \cos\alpha& 0 \\ 0 & 0 & 1 \end{matrix} \right] \left[ \begin{matrix} \cos\beta& 0 & \sin\beta\\ 0 & 1 & 0 \\ -\sin\beta& 0 & \cos\beta \end{matrix} \right]\left[ \begin{matrix} \cos\gamma& -\sin\gamma& 0 \\ \sin\gamma& \cos\gamma& 0 \\ 0 & 0 & 1 \end{matrix} \right] R=cosαsinα0sinαcosα0001cosβ0sinβ010sinβ0cosβcosγsinγ0sinγcosγ0001 具体计算得: R = [ cos ⁡ α c o s β c o s γ − s i n α s i n γ − c o s α c o s β s i n γ − s i n α c o s γ c o s α s i n β s i n α c o s β c o s γ + c o s α s i n γ − s i n α c o s β s i n γ + c o s α c o s γ s i n α s i n β − s i n β c o s γ s i n β s i n γ c o s β ] R=\left[ \begin{matrix} \cos\alpha cos\beta cos\gamma - sin\alpha sin\gamma & -cos\alpha cos\beta sin\gamma-sin\alpha cos\gamma & cos\alpha sin\beta \\ sin\alpha cos\beta cos\gamma + cos\alpha sin\gamma & -sin\alpha cos\beta sin\gamma + cos\alpha cos\gamma & sin\alpha sin\beta \\ -sin\beta cos\gamma & sin\beta sin\gamma & cos\beta \end{matrix} \right] R=cosαcosβcosγsinαsinγsinαcosβcosγ+cosαsinγsinβcosγcosαcosβsinγsinαcosγsinαcosβsinγ+cosαcosγsinβsinγcosαsinβsinαsinβcosβ R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] R=\left[ \begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{matrix} \right] R=r11r21r31r12r22r32r13r23r33 α \alpha α β \beta β γ \gamma γ可由以下公式计算得到。 β = a t a n 2 ( r 31 2 + r 32 2 , r 33 ) \beta=atan2(\sqrt{ r{^2_{31}} + r{^2_{32}}},r_{33}) β=atan2(r312+r322 ,r33) α = a t a n 2 ( r 23 , r 13 ) \alpha=atan2(r_{23},r_{13}) α=atan2(r23,r13) γ = a t a n 2 ( r 32 , − r 31 ) \gamma =atan2(r_{32},-r_{31}) γ=atan2(r32,r31)

具体求解

6 0 T = 3 0 T 6 3 T {^0_6}T={^0_3}T{^3_6}T 60T=30T63T得到 6 3 T = 3 0 T − 1 6 0 T {^3_6}T={^0_3}T^{-1}{^0_6}T 63T=30T160T 再由齐次变换矩阵和旋转矩阵的关系得到 6 3 R {^3_6}R 63R。.
许多工业机器人腕部三关节配置成一个球面副,这种球面副可用z-y-z欧拉角的解出这三个关节角。因此利用上面介绍的方法可以求解出 θ 4 \theta_4 θ4 α \alpha α)、 θ 5 \theta_5 θ5 θ \theta θ)、 θ 6 \theta_6 θ6 γ \gamma γ)。

算例

设六自由度机械臂的DH参数表如下。

ia α \alpha αd θ \theta θ
1000 θ 1 \theta_1 θ1
2-30-900 θ 2 \theta_2 θ2
334000 θ 3 \theta_3 θ3
4-40-90388 θ 4 \theta_4 θ4
50900 θ 5 \theta_5 θ5
60-900 θ 4 \theta_4 θ4

腕点在基坐标系{0}中的位置向量为 0 P 4 o = [ 381.3 151.8 19.5 ] (n) {^0}P_4o= \left[ \begin{matrix} 381.3 \\ 151.8\\ 19.5 \end{matrix} \right] \tag{n} 0P4o=381.3151.819.5(n) 在坐标系{3}的位置为 3 P 4 o = [ − 40 338 0 ] (n) {^3}P_4o= \left[ \begin{matrix} -40\\ 338\\ 0 \end{matrix} \right] \tag{n} 3P4o=403380(n)坐标系{6}相对于基坐标系{0}的齐次变换矩阵为: [ 0 0.5736 0.8192 381.3 0 − 0.8192 0.5736 151.8 1 0 0 19.5 0 0 0 1 ] (n) \left[ \begin{matrix} 0& 0.5736& 0.8192& 381.3\\ 0& -0.8192& 0.5736& 151.8\\ 1& 0& 0& 19.5\\ 0& 0&0&1\\ \end{matrix} \right] \tag{n} 00100.57360.8192000.81920.573600381.3151.819.51(n)

MATLAB实现

先封装两个函数calT和getKG。

function Matrix_T = calT(DH_parameter, start_row, end_row)
% 输入参数为DH参数表,参数表有四列,参数表的行数是运动轴的个数。比如六自由度机械臂的参数表为6行4列
% 第一列是Z轴偏置;
% 第二列是Z轴偏转角;
% 第三列是X轴偏置;
% 第四列是X轴偏转角;
Matrix_T = eye(4);
for row_index = start_row + 1 : end_row
    % 先绕X轴转theta_X, 再沿X轴平移trans_X,将两个坐标系的Z轴重合
    theta_X = DH_parameter(row_index, 2);
    trans_X = DH_parameter(row_index, 1);
    T_X = rotX(theta_X) * transX(trans_X);
    
    % 先绕Z轴转theta_Z, 再沿Z轴平移trans_Z,将两个坐标系的X轴重合
    theta_Z = DH_parameter(row_index, 4);
    trans_Z = DH_parameter(row_index, 3);
    T_Z = rotZ(theta_Z) * transZ(trans_Z);
   
    Matrix_T = Matrix_T * T_X * T_Z;
end
function [k1, k2, k3, k4, g1, g2, g3] = getKG(DH_parameter, P_3)
% 计算k和g
% DH_parameter: DH参数表
% P_3:腕点坐标系3中的位置

T_12 = calT(DH_parameter, 1, 2);
T_23 = calT(DH_parameter, 2, 3);

F_Matrix = T_23 * P_3;
G_Matrix = T_12 * F_Matrix;

f1 = F_Matrix(1, 1);
f2 = F_Matrix(2, 1);
f3 = F_Matrix(3, 1);

g1 = G_Matrix(1, 1);
g2 = G_Matrix(2, 1);
g3 = G_Matrix(3, 1);

a1 = DH_parameter(2, 1);
alpha1 = DH_parameter(2, 2);
d2 = DH_parameter(2, 3);

k1 = f1;
k2 = -f2;
k3 = f1^2 + f2^2 + f3^2 + a1^2 + d2^2 + 2*d2*f3;
k4 = f3*cos(alpha1) + d2*cos(alpha1);

下面是主程序。

syms theta1 theta2 theta3 theta4 theta5 theta6 

DH_parameter = [
    0, 0, 0, theta1;
    -30, -90/180*pi, 0, theta2;
    340, 0, 0, theta3;
    -40, -90/180*pi, 338, theta4;
    0, 90/180*pi,0, theta5;
    0, -90/180*pi, 0, theta6;
    ];
    
P_3 = [-40; 338; 0; 1];
P_O = [381.3; 151.8; 19.5];
x = P_O(1, 1);
z = P_O(3, 1);
r = P_O'* P_O;
a1 = DH_parameter(2, 1);
alpha1 = DH_parameter(2, 2);
d2 = DH_parameter(2, 3);

[k1, k2, k3, k4, g1, g2, g3] = getKG(DH_parameter, P_3);

% 计算theta3
s = '(r-k3)^2/4/a1^2+(z-k4)^2/sin(alpha1)^2=k1^2+k2^2'
s = subs(s, 'r', r);
s = subs(s, 'z', z);
s = subs(s, 'a1', a1);
s = subs(s, 'alpha1', alpha1);
s = subs(s, 'k1', k1);
s = subs(s, 'k2', k2);
s = subs(s, 'k3', k3);
s = subs(s, 'k4', k4);
% 变量替换
s = subs(s, 'cos(theta3)', '(1-u^2)/(1+u^2)');
s = subs(s, 'sin(theta3)', '2*u/(1+u^2)');
% 求解非线性方程
u = solve(s, 'u');
theta3 = double(2 * atan(u));

计算得:

theta3 =

    2.8628
    2.6414
    0.2646
    0.0432

θ 3 = 0.0432 \theta_3=0.0432 θ3=0.0432(弧度)。
再计算 θ 2 \theta_2 θ2

theta3 = 0.0432;
% 计算theta2
s = 'r = (k1 * cos(theta2) + k2 * sin(theta2)) * 2 * a1 + k3';
s = subs(s, 'a1', a1);
s = subs(s, 'r', r);
s = subs(s, 'k1', k1);
s = subs(s, 'k2', k2);
s = subs(s, 'k3', k3);
s = subs(s, 'theta3', theta3);
% 变量替换
s = subs(s, 'cos(theta2)', '(1-u^2)/(1+u^2)');
s = subs(s, 'sin(theta2)', '2*u/(1+u^2)');
% 求解非线性方程
u = solve(s, 'u');
theta2 = double(2 * atan(u));

计算得:

theta2 =

   -0.9058
   -0.8272

θ 2 = − 0.9058 \theta_2=-0.9058 θ2=0.9058(弧度)。
再计算 θ 1 \theta_1 θ1

theta3 = 0.0432;
theta2 = -0.9058;
s = 'x = cos(theta1) * g1 - sin(theta1) * g2';
s = subs(s, 'x', x);
s = subs(s, 'g1', g1);
s = subs(s, 'g2', g2);
s = subs(s, 'theta3', theta3);
s = subs(s, 'theta2', theta2);
% 变量替换
s = subs(s, 'cos(theta1)', '(1-u^2)/(1+u^2)');
s = subs(s, 'sin(theta1)', '2*u/(1+u^2)');
% 求解非线性方程
u = solve(s, 'u');
theta1 = double(2 * atan(u));

计算得:

   theta1 =

    0.3795
   -0.3795

θ 1 = 0.3795 \theta_1=0.3795 θ1=0.3795(弧度)。
下面计算 θ 4 \theta_4 θ4 θ 5 \theta_5 θ5 θ 6 \theta_6 θ6
先封装计算ZYZ欧拉角的函数

function [alpha, beta, gamma] = calZYZEulerInv(T_Matrix)
% 输入参数为DH参数表,参数表有四列,参数表的行数是运动轴的个数。比如六自由度机械臂的参数表为6行4列
% T_Matrix:齐次变换矩阵;
% alpha:第一次Z轴转角;
% beta:Y轴转角;
% gamma:第二次Z轴转角;

R = T_Matrix(1:3, 1:3);

r31 = R(3, 1);
r32 = R(3, 2);
r33 = R(3, 3);
r13 = R(1, 3);
r23 = R(2, 3);

beta = double(atan2(sqrt(r31^2 + r32^2), r33));
alpha = double(atan2(r23, r13));
gamma = double(atan2(r32, -r31));

下面是主程序。

% 坐标系{6}相对于坐标系{0}
T_06 = [
 0, 0.5736, 0.8192, 381.3;
 0, -0.8192, 0.5736, 151.8;
 1, 0, 0, 19.5;
 0, 0, 0,1]; 
 
DH_parameter(1, 4) = 0.3795; % theta1
DH_parameter(2, 4) = -0.9058; % theta2
DH_parameter(3, 4) = 0.0432; % theta3

T_03 = calT(DH_parameter, 0, 3); % 坐标系{3}相对于坐标系{0}
T_36 = T_03 \ T_06; % 坐标系{6}相对于坐标系{3}
[alpha, beta, gamma] = calZYZEulerInv(T_36);

计算得到结果。

alpha =

   0.8626


beta =

   1.3394


gamma =

  -1.5708

至此,运动学逆解计算完成。
六个关节角度分别为: θ 1 = 21. 7 ∘ \theta_1=21.7^\circ θ1=21.7 θ 2 = − 51. 9 ∘ \theta_2=-51.9^\circ θ2=51.9 θ 3 = 2. 5 ∘ \theta_3=2.5^\circ θ3=2.5 θ 4 = 49. 4 ∘ \theta_4=49.4^\circ θ4=49.4 θ 5 = 76. 7 ∘ \theta_5=76.7^\circ θ5=76.7 θ 6 = − 9 0 ∘ \theta_6=-90^\circ θ6=90

相关推荐
©️2020 CSDN 皮肤主题: 数字20 设计师:CSDN官方博客 返回首页