机器人基础之运动学逆解

概述

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

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

现代绝大多数工业机械臂都满足Pieper准则的第一个充分条件，即机械臂末端的三个关节的轴线相交于一点，该点通常称为腕点。
满足第二个充分条件的机器人较少，最典型的例子是SCARA机器人。之前我所在的公司应用SCARA构型的机器人进行工件的上下料。
假设机械臂满足Pieper准则的第一个充分条件，则机械臂的运动学方程可以写为$${}_6^{0}T={}_3^{0}T{}_6^{3}T$$其中，${}_3^{0}T$规定了腕点的位置，${}_6^{3}T$规定了腕部的方位。因此，运动学逆解也分为两步，先求解${\theta }_1$、${\theta }_2$、${\theta }_3$（腕点位置），再求解${\theta }_4$、${\theta }_5$、${\theta }_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$$其中，${}^3{P}_{4}o$即为变换矩阵${}_4^{3}T$的第4列，即$${}^0{P}_{4}o={}_1^{0}T{}_2^{1}T{}_3^{2}T\left[\begin{array}{c}{a}_3\\ -d_{4}sin{\theta }_3\\ d_{4}cos{\theta }_3\\ 1\end{array}\right]\text{\hspace{1em}(n)}$$令 $$\left[\begin{array}{c}{f}_1({\theta }_3)\\ f_2({\theta }_3)\\ f_3({\theta }_3)\\ 1\end{array}\right]={}_3^{2}T\left[\begin{array}{c}{a}_3\\ -d_{4}sin{\theta }_3\\ d_{4}cos{\theta }_3\\ 1\end{array}\right]\text{\hspace{1em}(n)}$$其中，$$f_1({\theta }_3)=a_{3}cos{\theta }_3+d_{4}sin{\theta }_{3}sin{\alpha }_3+a_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$$ $$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$$ 再令$$\left[\begin{array}{c}{g}_1({\theta }_2,{\theta }_3)\\ g_2({\theta }_2,{\theta }_3)\\ g_3({\theta }_2,{\theta }_3)\\ 1\end{array}\right]={}_2^{1}T\left[\begin{array}{c}{f}_1({\theta }_3)\\ f_2({\theta }_3)\\ f_3({\theta }_3)\\ 1\end{array}\right]\text{\hspace{1em}(n)}$$ 其中，$$g_1({\theta }_2,{\theta }_3)=cos{\theta }_2{f}_1-sin{\theta }_2{f}_2+a_1$$ $$g_2({\theta }_2,{\theta }_3)=sin{\theta }_{2}cos{\alpha }_1{f}_1+cos{\theta }_{2}cos{\alpha }_1{f}_2-sin{\alpha }_1{f}_3-d_{2}sin{\alpha }_1$$ $$g_3({\theta }_2,{\theta }_3)=sin{\theta }_{2}sin{\alpha }_1{f}_1+cos{\theta }_{2}sin{\alpha }_1{f}_2+cos{\alpha }_1{f}_3+d_{2}cos{\alpha }_1$$ 因此 $${}^0{P}_{4}o={}_1^{0}T\left[\begin{array}{c}{g}_1\\ g_2\\ g_3\\ 1\end{array}\right]\text{\hspace{1em}(n)}$$由于坐标系{1}与基坐标系{0}原点重合、Z轴重合，仅存在绕Z轴的转动，因此，$${}_1^{0}T=\left[\begin{array}{cccc}\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{array}\right]\text{\hspace{1em}(n)}$$进一步可得，$${}^0{P}_{4}o=\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=\left[\begin{array}{cccc}\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{array}\right]\left[\begin{array}{c}{g}_1\\ g_2\\ g_3\\ 1\end{array}\right]=\left[\begin{array}{c}{g}_{1}cos{\theta }_1-g_{1}sin{\theta }_1\\ g_{1}sin{\theta }_1+g_{1}cos{\theta }_1\\ g_3\\ 1\end{array}\right]\text{\hspace{1em}(n)}$$令$$r=x^2+y^2+z^2=g{}_1^2+g{}_2^2+g{}_3^2$$ 并且$$z=g_3$$代入$g_1$、$g_2$、$g_3$的表达式，可得：$$r=(k_{1}cos{\theta }_2+k_{2}sin{\theta }_2)2a_1+k_3$$ $$z=(k_{1}sin{\theta }_2-k_{2}cos{\theta }_2)sin{\alpha }_1+k_4$$ 其中，
$$k_1({\theta }_3)=f_1$$ $$k_2({\theta }_3)=-f_2$$ $$k_3({\theta }_3)=f{}_1^2+f{}_2^2+f{}_3^2+a{}_1^2+d{}_2^2+2d_2{f}_3$$ $$k_4({\theta }_3)=f_{3}cos{\alpha }_1+d_{2}cos{\alpha }_1$$ 这里分三种情况进行讨论：

1. 当$a_1=0$时。$r=k_3({\theta }_3)$，因为$r$已知，所以可以求出${\theta }_3$；
2. 当$sin{\alpha }_1=0$时。$z=k_4({\theta }_3)$，因为$z$已知，所以可以求出${\theta }_3$；
3. 当$a_1$和$sin{\alpha }_1$均不为0时，有$$\frac{(r-k_3{)}^2}{(2a_1{)}^2}+\frac{(z-k_4{)}^2}{si{n}^2{\alpha }_1}=k{}_1^2+k{}_2^2$$这里所有变量均为${\theta }_3$的函数，求解关于${\theta }_3$的非线性方程即可得到${\theta }_3$。需要注意的是，更为准确地说，上述公式中所有变量均为$sin{\theta }_3$和$cos{\theta }_3$的函数，可令$$u=tan\frac{{\theta }_3}{2}$$ $$cos\theta =\frac{1-u^2}{1+u^2}$$ $$sin\theta =\frac{2u}{1+u^2}$$ 由此，可将原非线性方程转化为关于变量$u$的方程。解得$u$之后再利用$${\theta }_3=2arctanu$$得到${\theta }_3$。
   利用前面的公式$$r=(k_{1}cos{\theta }_2+k_{2}sin{\theta }_2)2a_1+k_3$$ 这里代入${\theta }_3$后，所有变量均为${\theta }_2$的函数，类似的，求解该非线性方程即可得到${\theta }_2$。
   利用公式$$x=g_{1}cos{\theta }_1-g_{2}sin{\theta }_1$$代入${\theta }_2$和${\theta }_3$，即可求得${\theta }_1$。
   到此，求解得到了${\theta }_1$、${\theta }_2$、${\theta }_3$。

求解腕部方位

z-y-z欧拉角

假设某坐标系分别依次绕动坐标系的Z轴、Y轴、Z轴转动$\alpha$、$\beta$、$\gamma$角度，则旋转矩阵为 $$R=\left[\begin{array}{ccc}\cos \alpha & -\sin \alpha & 0\\ \sin \alpha & \cos \alpha & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos \beta & 0& \sin \beta \\ 0& 1& 0\\ -\sin \beta & 0& \cos \beta \end{array}\right]\left[\begin{array}{ccc}\cos \gamma & -\sin \gamma & 0\\ \sin \gamma & \cos \gamma & 0\\ 0& 0& 1\end{array}\right]$$ 具体计算得：$$R=\left[\begin{array}{ccc}\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{array}\right]\text{\hspace{1em}(n)}$$令$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\text{\hspace{1em}(n)}$$则$\alpha$、$\beta$、$\gamma$可由以下公式计算得到。$$\beta =atan2(\sqrt{r{}_{31}^2+r{}_{32}^2},r_{33})$$ $$\alpha =atan2(r_{23},r_{13})$$ $$\gamma =atan2(r_{32},-r_{31})$$

具体求解

由$${}_6^{0}T={}_3^{0}T{}_6^{3}T$$得到$${}_6^{3}T={}_3^0{T}^{-1}{}_6^{0}T$$ 再由齐次变换矩阵和旋转矩阵的关系得到 ${}_6^{3}R$。.
许多工业机器人腕部三关节配置成一个球面副，这种球面副可用z-y-z欧拉角的解出这三个关节角。因此利用上面介绍的方法可以求解出${\theta }_4$（$\alpha$）、${\theta }_5$（$\theta$）、${\theta }_6$（$\gamma$）。

算例

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

i	a	$\alpha$	d	$\theta$
1	0	0	0	${\theta }_1$
2	-30	-90	0	${\theta }_2$
3	340	0	0	${\theta }_3$
4	-40	-90	388	${\theta }_4$
5	0	90	0	${\theta }_5$
6	0	-90	0	${\theta }_4$

腕点在基坐标系{0}中的位置向量为 $${}^0{P}_{4}o=\left[\begin{array}{c}381.3\\ 151.8\\ 19.5\end{array}\right]\text{\hspace{1em}(n)}$$ 在坐标系{3}的位置为$${}^3{P}_{4}o=\left[\begin{array}{c}-40\\ 338\\ 0\end{array}\right]\text{\hspace{1em}(n)}$$坐标系{6}相对于基坐标系{0}的齐次变换矩阵为：$$\left[\begin{array}{cccc}0& 0.5736& 0.8192& 381.3\\ 0& -0.8192& 0.5736& 151.8\\ 1& 0& 0& 19.5\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(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

取${\theta }_3=0.0432$（弧度）。
再计算${\theta }_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

取${\theta }_2=-0.9058$（弧度）。
再计算${\theta }_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

取${\theta }_1=0.3795$（弧度）。
下面计算${\theta }_4$、${\theta }_5$、${\theta }_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

至此，运动学逆解计算完成，六个关节角度分别为：${\theta }_1=21.7^{\circ }$、${\theta }_2=-51.9^{\circ }$、${\theta }_3=2.5^{\circ }$、${\theta }_4=49.4^{\circ }$、${\theta }_5=76.7^{\circ }$、${\theta }_6=-90^{\circ }$。