机器人控制算法二之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);
