机械臂逆解 python代码 机械臂运动学逆解

机器人基础之运动学逆解

• 概述
• 求解腕点位置
• 求解腕部方位

• z-y-z欧拉角
• 具体求解

• 算例

• MATLAB实现

概述

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

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

现代绝大多数工业机械臂都满足Pieper准则的第一个充分条件，即机械臂末端的三个关节的轴线相交于一点，该点通常称为腕点。
满足第二个充分条件的机器人较少，最典型的例子是SCARA机器人。之前我所在的公司应用SCARA构型的机器人进行工件的上下料。
假设机械臂满足Pieper准则的第一个充分条件，则机械臂的运动学方程可以写为其中，规定了腕点的位置，规定了腕部的方位。因此，运动学逆解也分为两步，先求解、、（腕点位置），再求解、、（腕部方位）。

求解腕点位置

将固连在机械臂腕部的三个坐标系{4}、{5}、{6}的原点设在腕点，则腕点相对于基坐标系{0}的位置为：其中，即为变换矩阵的第4列，即令 则有， 再令 则有， 因此 由于坐标系{1}与基坐标系{0}原点重合、Z轴重合，仅存在绕Z轴的转动，因此，进一步可得，令 并且代入、、的表达式，可得： 其中，

1. 当时。，因为已知，所以可以求出；
2. 当时。，因为已知，所以可以求出；
3. 当和均不为0时，有这里所有变量均为的函数，求解关于的非线性方程即可得到。需要注意的是，更为准确地说，上述公式中所有变量均为和的函数，可令 由此，可将原非线性方程转化为关于变量的方程。解得之后再利用得到。
   利用前面的公式 这里代入后，所有变量均为的函数，类似的，求解该非线性方程即可得到。
   利用公式代入和，即可求得。
   到此，求解得到了、、。

求解腕部方位

z-y-z欧拉角

假设某坐标系分别依次绕动坐标系的Z轴、Y轴、Z轴转动、、角度，则旋转矩阵为 具体计算得：令则、、可由以下公式计算得到。

具体求解

由得到 再由齐次变换矩阵和旋转矩阵的关系得到 。.
许多工业机器人腕部三关节配置成一个球面副，这种球面副可用z-y-z欧拉角的解出这三个关节角。因此利用上面介绍的方法可以求解出（）、（）、（）。

算例

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

i	a		d
1	0	0	0
2	-30	-90	0
3	340	0	0
4	-40	-90	388
5	0	90	0
6	0	-90	0

腕点在基坐标系{0}中的位置向量为 在坐标系{3}的位置为坐标系{6}相对于基坐标系{0}的齐次变换矩阵为：

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

取（弧度）。
再计算

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

取（弧度）。
再计算

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

取（弧度）。
下面计算、、。
先封装计算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

至此，运动学逆解计算完成。
六个关节角度分别为：、、、、、。