【MATLAB】5DOFs机械臂运动学正逆解（MDH）-解析法实现
引言
本篇文章是记录我个人在学习机械臂时的一些想法，我只是初学者，如果各位有发现某些错误，麻烦在评论区批评或指出，我会第一时间改正，谢谢。
此篇为解析法的实现，我的另一篇博客详细说明了数值法的实现，同样都给出了MATLAB代码，地址如下：https://blog.csdn.net/qq_43557907/article/details/125353210
一、MDH建立：
对于某宝上常见的5DOFs机械手臂，应用MDH（改进D-H）参数可建立如下坐标系：
MDH参数表：
| i | alphai-1 | ai-1 | di | thetai |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | theta1 |
| 2 | -pi/2 | 0 | 0 | theta2 |
| 3 | 0 | L2 | 0 | theta3 |
| 4 | 0 | L3 | 0 | theta4 |
| 5 | -pi/2 | 0 | 0 | theta5 |
MATLAB程序如下：
clear;
close all;
clc;
clear L;
angle=pi/180; %角度转换
L1 = 0; L2 = 80; L3 = 60; L4 = 0;
%D-H参数表
theta1 = 0; D1 = 0; A1 = 0; alpha1 = 0;
theta2 = 0; D2 = 0; A2 = 0; alpha2 = -pi/2;
theta3 = 0; D3 = 0; A3 = L2; alpha3 = 0;
theta4 = 0; D4 = 0; A4 = L3; alpha4 = 0;
theta5 = 0; D5 = 0; A5 = 0; alpha5 = -pi/2;
%% DH法建立模型,关节转角，关节距离，连杆长度，连杆转角，关节类型（0转动，1移动）,关节范围
L(1) = Link([theta1, D1, A1, alpha1, 0], 'modified');L(1).qlim =[-180*angle, 180*angle];
L(2) = Link([theta2, D2, A2, alpha2, 0], 'modified');L(2).qlim =[-180*angle, 180*angle];
L(3) = Link([theta3, D3, A3, alpha3, 0], 'modified');L(3).qlim =[-180*angle, 180*angle];
L(4) = Link([theta4, D4, A4, alpha4, 0], 'modified');L(4).qlim =[-180*angle, 180*angle];
L(5) = Link([theta5, D5, A5, alpha5, 0], 'modified');L(5).qlim =[-180*angle, 180*angle];
%% 显示机械臂
robot0 = SerialLink(L,'name','five');
theta = [0 -60 90 0 90]*angle; %初始关节角度
figure(1)
robot0.plot(theta);
title('五轴机械臂模型');
%% 运动学计算
T = robot0.fkine(theta).T; %运动学正解
t = DOF5_fkine(theta, L1, L2, L3, L4);
theta_ikine = DOF5_ikine(T, L1, L2, L3, L4);%逆解
二、运动学正解：
计算齐次变换矩阵，
function t_fkine = DOF5_fkine(theta, l1, l2, l3, l4)
c1 = cos(theta(1));c2 = cos(theta(2));c3 = cos(theta(3));c4 = cos(theta(4));c5 = cos(theta(5));
s1 = sin(theta(1));s2 = sin(theta(2));s3 = sin(theta(3));s4 = sin(theta(4));s5 = sin(theta(5));
r11 = c1*c4*c5*(c2*c3-s2*s3) - c1*s4*c5*(c2*s3+s2*c3) + s1*s5;
r12 = -c1*c4*s5*(c2*c3-s2*s3) + c1*s4*s5*(c2*s3+s2*c3) + s1*c5;
r13 = -c1*s4*(c2*c3-s2*s3) - c1*c4*(c2*s3+s2*c3);
r21 = s1*c4*c5*(c2*c3-s2-s3) - s1*s4*c5*(c2*s3+s2*c3) - c1*s5;
r22 = -s1*c4*s5*(c2*c3-s2-s3) + s1*s4*s5*(c2*s3+s2*c3) - c1*c5;
r23 = -s1*s4*(c2*c3-s2*s3) - s1*c4*(c2*s3+s2*c3);
r31 = c4*c5*(-s2*c3-c2*s3) + s4*c5*(s2*s3-c2*c3);
r32 = -c4*s5*(-s2*c3-c2*s3) - s4*s5*(s2*s3-c2*c3);
r33 = -s4*(-s2*c3-c2*s3) + c4*(s2*s3-c2*c3);
px = (-s4*l4+l3)*(c1*c2*c3-c1*s2*s3) + c4*l4*(-c1*c2*s3-c1*s2*c3) + c1*c2*l2;
py = (-s4*l4+l3)*(s1*c2*c3-s1*s2*s3) + c4*l4*(-s1*c2*s3-s1*s2*c3) + s1*c2*l2;
pz = (-s4*l4+l3)*(-s2*c3-c2*s3) + c4*l4*(s2*s3-c2*c3) - s2*l2;
t_fkine = [r11 r12 r13 px; r21 r22 r23 py; r31 r32 r33 pz; 0 0 0 1];
end
令 theta = [0 -60 90 0 90]*angle;
Transformation Matrix：
与机器人工具箱中fkine()函数得到的矩阵一致。
三、运动学逆解：
MATLAB程序：
function theta_ikine = DOF5_ikine(oT, ol1, ol2, ol3, ol4)
%5自由度逆解运算
l1 = ol1; l2 = ol2; l3 = ol3; l4 = ol4;
r11 = oT(1, 1);r12 = oT(1, 2);r13 = oT(1, 3);px = oT(1, 4);
r21 = oT(2, 1);r22 = oT(2, 2);r23 = oT(2, 3);py = oT(2, 4);
r31 = oT(3, 1);r32 = oT(3, 2);r33 = oT(3, 3);pz = oT(3, 4);
% theta1 两个解
theta1_1 = atan2(py, px) - atan2(0, (px^2 + py^2)^0.5);
%theta1_2 = atan2(py, px) - atan2(0, -(px^2 + py^2)^0.5);
s1 = sin(theta1_1);
c1 = cos(theta1_1);
% theta3 两个解
c3 = ((px*c1)^2 + 2*px*py*s1*c1 + (py*s1)^2 + pz^2 - l2^2 - l3^2) / (2*l2*l3);
theta3_1 = atan2(sqrt(1-c3^2), c3);
theta3_2 = atan2(-sqrt(1-c3^2), c3);
s3_1 = sin(theta3_1);
s3_2 = sin(theta3_2);
% theta2 两个解
s2_1 = (pz*(c3*l3+l2)+s3_1*l3*(c1*px+s1*py))/(-(c3*l3+l2)^2-(s3_1*l3)^2);
s2_2 = (pz*(c3*l3+l2)+s3_2*l3*(c1*px+s1*py))/(-(c3*l3+l2)^2-(s3_2*l3)^2);
c2_1 = ((c1*px+s1*py)+s2_1*s3_1*l3)/(c3*l3+l2);
c2_2 = ((c1*px+s1*py)+s2_2*s3_2*l3)/(c3*l3+l2);
theta2_1 = atan2(s2_1, c2_1);
theta2_2 = atan2(s2_2, c2_2);
% theta5 一个解
theta5 = atan2(s1*r11-c1*r21, s1*r12-c1*r22);
% theta4 四个解
theta4_1_1 = atan2(s2_1*c3+c2_1*s3_1, s2_1*s3_1-c2_1*c3) + atan2(sqrt((s2_1*s3_1-c2_1*c3)^2+(s2_1*c3+c2_1*s3_1)^2-r33^2), r33);
theta4_1_2 = atan2(s2_1*c3+c2_1*s3_1, s2_1*s3_1-c2_1*c3) - atan2(sqrt((s2_1*s3_1-c2_1*c3)^2+(s2_1*c3+c2_1*s3_1)^2-r33^2), r33);
theta4_2_1 = atan2(s2_2*c3+c2_2*s3_2, s2_2*s3_2-c2_2*c3) + atan2(sqrt((s2_2*s3_2-c2_2*c3)^2+(s2_2*c3+c2_2*s3_2)^2-r33^2), r33);
theta4_2_2 = atan2(s2_2*c3+c2_2*s3_2, s2_2*s3_2-c2_2*c3) - atan2(sqrt((s2_2*s3_2-c2_2*c3)^2+(s2_2*c3+c2_2*s3_2)^2-r33^2), r33);
theta_ikine = [theta1_1 theta2_1 theta3_1 theta4_1_1 theta5;
theta1_1 theta2_1 theta3_1 theta4_1_2 theta5;
theta1_1 theta2_2 theta3_2 theta4_2_1 theta5;
theta1_1 theta2_2 theta3_2 theta4_2_2 theta5];
end
结果如下：
验证： 第一组解theta = [-0.0000 -1.0472 1.5708 5.2360 1.5708];
** 第二组解theta = [-0.0000 -1.0472 1.5708 -0.0000 1.5708];
第三、四组解皆成立。
四、应用
在上面的基础上加入虚拟关节Axes6作为末端执行器，它的D-H参数为
theta6 = 0; D6 = L4; A6 = 0; alpha6 = 0; %虚拟关节
其中L4为Axes5到Axes6的距离。
1.建立{P}相对于{5}的T矩阵：
%% 手爪中心点{P}的T矩阵
T_P25 = [1 0 0 0;
0 1 0 0;
0 0 1 L4;
0 0 0 1];
2.建立{0}相对于世界坐标{W}的T矩阵：
%% 世界坐标{W}的T矩阵
T_02W= [1 0 0 0;
0 1 0 0;
0 0 1 L1;
0 0 0 1];
其中L1为世界坐标原点距离Axes0的距离。
3.建立{P}相对{W}的T矩阵：
%% {P}相对{W}的T矩阵
px = 50;py = 50; pz = 50;
T_P2W = [1 0 0 px;
0 1 0 py;
0 0 1 pz;
0 0 0 1];
T_P2W为P点在世界坐标系下的位姿，这里默认与{W}的旋转矩阵一致。
4.最后，我们可以得到0到5的T矩阵：
T = inv(T_02W)*T_P2W*inv(T_P25);
此时，可以通过逆解得到theta1-5的角度，即实现了使机械臂运动到指定坐标的任务。
例如，给定坐标{50, 50, 50},结果为
参考：
1.《机器人学导论》第四版
2.https://openrsl.blog.csdn.net/article/details/95792841
3.https://blog.csdn.net/weixin_42454034/article/details/109681661