机器人控制算法二之matlab工具箱机械臂运动学正逆矩阵解具体分析

1.坐标系介绍

一般应用场景为右手坐标系
详细了解可参考：计算机视觉基础——欧拉角，旋转向量，四元数与旋转矩阵

2.DH建模

建模顺序：

变换矩阵求解：

转角矩阵构建形式(变换矩阵则按照以上公式相乘即可)：

//在右手系中绕X轴旋转p° 对应的矩阵Rx
        | 1    0        0 |
  Rx=   | 0   cosp   -sinp|
        | 0   sinp    cosp|
//在右手系中绕Y轴旋转h° 对应的矩阵Ry
        | cosh   0   sinh|
  Ry=   |  0     1     0 |
        |-sinh   0   cosh|
//在右手系中绕Z轴旋转b° 对应的矩阵Rz
        |cosb  -sinb   0 |
  Rz=   |sinb   cosb   0 |
        |  0     0     1 |
        
//右手系中x,y,z的平移矩阵形式：
  T = [tx  ty  tz]'   （转置）

由旋转矩阵转为欧拉角：

//假设旋转矩阵为R：
x = atan2( R(3,2), R(3,3));
y = atan2(-R(3,1), aqrt(R(3,2)^2 + R(3,3)^2));
z = atan2(R(2,1),R(1,1));
angle = [x,y,z];//获得三个方向上的角度

关于DH建模求解并进行正逆运行学仿真详细方法参考：用matlab实现机械臂正逆运动学控制

3.matlab工具箱仿真

clc
close all
clear all

%*****************************建立机械臂的仿真模型*********************************
%theta:Z轴旋转角度
%d：沿Z轴平移距离
%alpha：沿x轴旋转角度
%a：沿x轴平移距离

%参数的设定
h0=80;
l1=2500;
l2=2000;
l3=2500;

%转角
t0=0;
t1= pi/4;
t2=-pi/4;
t3=-pi/2;

%使用Link类函数，基于DH法建模（先绕z轴旋转，再沿x轴移动，再沿x轴旋转，再沿z轴平移）

L(1)=Link('revolute','d',h0,'a',0,'alpha',pi/2,'offset',0);
L(2)=Link('revolute','d',0,'a',l1,'alpha',0,'offset',t1);
L(3)=Link('revolute','d',0,'a',l2,'alpha',0,'offset',t2);
L(4)=Link('revolute','d',0,'a',l3,'alpha',0,'offset',t3);

% %           theta      d        a        alpha
% L1=Link([    0        h0       h0     pi/2 ], 'modified'); % [四个DH参数], options
% L2=Link([    0         0       l1      0], 'modified');
% L3=Link([    0         0       l2      0], 'modified');
% L4=Link([    0         0       l3      0], 'modified');
% 
% 
%使用Seriallink类函数把我们上面使用Link函数建立的连杆连成一个整体，生成一个串联机械臂模型
robot=SerialLink (L,'name','4-robot');

%使用.plot绘制出某组关节变量的机械臂三维模型
%Three_Link.plot([0,0,0])
robot.display
robot.teach

robot类属性详见下图，比较主要的是：DH参数、变换矩阵、点集、角度限制等

% ***************************绘制直线***********************************
T1=transl(2000,1800,1000);%提取平移成分
T2=transl(2000,6500,1000);%提取平移成分
T=ctraj(T1,T2,50) %生成一条卡尔路径，50个平移矩阵
Tj=transl(T);%提取平移分量，其实是相对于原点的坐标，即x,y,z轴各个方向的偏移
plot3(Tj(:,1),Tj(:,2),Tj(:,3));%绘制蓝色直线
zlim([-10000,10000]);
xlabel('x'),ylabel('y'),zlabel('z');
grid on;
q=robot.ikine(T,'q0',[0 0 0 0], 'mask',[1 1 1 0 0 0]);
hold on
robot.plot(q);%动画演示

q_=q/pi*180;

生成的笛卡尔路径转为角度，然后通过正运动学实现移动

%*****************************正运动学**********************************
%输入由三维坐标点变成各轴转角，然后先通过正运动学获取起始与末端位姿，再生成路径进行运动
%不过此处由正运动学计算出的结果不是单纯的一个点，而是一个结构体属性
% 初始值及目标值
init_ang=[0 0 0 0];
targ_ang=[0, -pi/6, -pi/5, pi/6];
step=200;

[q,qd,qdd]=jtraj(init_ang,targ_ang,step); %关节空间规划轨迹，得到机器人末端运动的[位置，速度，加速度]
T0=robot.fkine(init_ang); % 正运动学解算,求出来T0,Tf是一个SE3类，包含位姿属性
Tf=robot.fkine(targ_ang);
figure;
subplot(2,4,3); i=1:4; plot(q(:,i)); title("位置"); grid on;
subplot(2,4,4); i=1:4; plot(qd(:,i)); title("速度"); grid on;
subplot(2,4,7); i=1:4; plot(qdd(:,i)); title("加速度"); grid on;

Tc=ctraj(T0,Tf,step); % 笛卡尔空间规划轨迹，得到机器人末端运动的变换矩阵
Tjtraj=transl(Tc); %提取平移分量，其实是相对于原点的坐标，即x,y,z轴各个方向的偏移
subplot(2,4,8); plot2(Tjtraj, 'r');
title('p1到p2直线轨迹'); grid on;
subplot(2,4,[1,2,5,6]);
plot3(Tjtraj(:,1),Tjtraj(:,2),Tjtraj(:,3),"b"); grid on;  %绘制末端点
zlim([-10000,10000]);
hold on;
view(3); % 解决robot.teach()和plot的索引超出报错
qq=robot.ikine(Tc, 'q0',[0 0 0 0], 'mask',[1 1 1 0 0 0]); % 逆运动学求角度
robot.plot(qq); %控制机械臂运动

此处不同于上个直线运动，这次的话输入变成了初始各轴转角与末端位置对应的各轴夹角，然后首先通过 robot.fkine() 得到末端点的位姿矩阵，再用ctraj() 生成中间过程离散点的位姿矩阵，再通过逆运动学 robot.ikine() 求出针对每个位姿的各轴夹角，最后就可以直接控制机械臂转动相应的夹角到达每个位置，组成一个运动过程。
当然，也可以直接通过transl() 求出每个位姿矩阵对应的末端点坐标

4.运动学函数解析

机械臂运动过程流程总结如下：
常规方式时通过起点与终点位姿生成中间的插值位姿矩阵，然后逆运动学求角度，直接控制运动
位姿通过正运动学获得！

4.1 正运动学

Forward kinematics

4.2 逆运动学

4.3 空间变换及其他函数

% Creace - cracional 3t(3)矩阵::T= transl (X, Y, Z)是一个SE(3)齐次变换(4x4)，表示X, Y和Z的纯平移。
% ***************提取平移分量，其实是相对于原点的坐标，即x,y,z轴各个方向的偏移*************************
T1=transl(2000,1800,1000);%生成一个纯平移矩阵
T2=transl(2000,6500,1000);%

T=ctraj(T1,T2,50) %生成一条笛卡尔路径，50个位姿矩阵
Tj=transl(T);  %%提取平移分量，其实是相对于原点的坐标，即x,y,z轴各个方向的便宜

init_ang=[0 0 0 0];
targ_ang=[0, -pi/6, -pi/5, pi/6];
T0=robot.fkine(init_ang); %生成的时末端点的位姿
Tf=robot.fkine(targ_ang);