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

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

文章图片

2.DH建模建模顺序：

文章图片

变换矩阵求解：

文章图片

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

//在右手系中绕X轴旋转p° 对应的矩阵Rx | 100 | Rx=| 0cosp-sinp| | 0sinpcosp| //在右手系中绕Y轴旋转h° 对应的矩阵Ry | cosh0sinh| Ry=|010 | |-sinh0cosh| //在右手系中绕Z轴旋转b° 对应的矩阵Rz |cosb-sinb0 | Rz=|sinbcosb0 | |001 |//右手系中x,y,z的平移矩阵形式： T = [txtytz]'（转置）

由旋转矩阵转为欧拉角：

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

文章图片

//假设旋转矩阵为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); % %thetadaalpha % L1=Link([0h0h0pi/2 ], 'modified'); % [四个DH参数], options % L2=Link([00l10], 'modified'); % L3=Link([00l20], 'modified'); % L4=Link([00l30], '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 正运动学
【机器人控制算法|机器人控制算法二之matlab工具箱机械臂运动学正逆矩阵解具体分析】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); %