引言

本篇文章是记录我个人在学习机械臂时的一些想法，我只是初学者，如果各位有发现某些错误，麻烦在评论区批评或指出，我会第一时间改正，谢谢。
此篇为解析法的实现，我的另一篇博客详细说明了数值法的实现，同样都给出了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