前言
本文所提到的大作业是2024年下半学年所布置的机器人技术的大作业，可能不适用于之后的版本，请注意。
题目要求大概如下：1.关节型机器人，5自由度以上。 2.编程实现正问题求解（给出连杆和关节参数，求出末端执行器位置） 3.编程实现逆问题求解（给出末端执行器位置，求出连杆和关节参数） 4.有图形化界面，实现初始状态到目标状态的动画。
本文中所提的代码加报告，最后获得了85分，因为本人也是在互联网上广泛借鉴别人的写法综合而成的代码，做的不算完美，但足以完成相关要求。
下面分步给出的代码合起来就是本项目的所有代码。
准备
1.安装matlab，本人使用的是MATLAB 2024b，请自行学习MATLAB及其语言的使用方法，简单的会用就行，要求大概能看懂代码。
2.需要额外下载MATLAB的机器人工具箱，可在站内搜索如何安装，这里给出官网的链接Robotics Toolbox - Peter Corke
正文
机器人建模部分
% Modified DH
clear;
clc;
% %机器人建模
th1 = 0;
th2 = 0;
th3 = 0;
th4 = 0;
th5 = 0;
th6 = 0;
d1 = 0.1607;
d2 = 0;
d3 = 0;
d4 =0.1133;
d5 = 0.099;
d6 = 0.0936;
%连杆长度
a1 = 0;
a2 = 0;
a3 = 0.425;
a4 = 0.393;
a5 = 0;
a6 = 0;
%连杆扭角
alpha1 = 0;
alpha2 = pi/2;
alpha3 = 0;
alpha4 = 0;
alpha5 = -pi/2;
alpha6 = pi/2;
% DH parameters th关节转角 d连杆偏移 a连杆长度 alpha连杆扭角 sigma
L1 = Link([th1, d1, a1, alpha1, 0], 'modified');
L2 = Link([th2, d2, a2, alpha2, 0], 'modified');L2.offset=pi/2;
L3 = Link([th3, d3, a3, alpha3, 0], 'modified');
L4 = Link([th4, d4, a4, alpha4, 0], 'modified');L4.offset=-pi/2;
L5 = Link([th5, d5, a5, alpha5, 0], 'modified');
L6 = Link([th6, d6, a6, alpha6, 0], 'modified');
robot = SerialLink([L1, L2, L3, L4, L5, L6],'name','robot'); %SerialLink 类函数
teach(robot)
robot.display(); %显示D-H表
建议机器人建模的相关参数不要改，可能改了之后后面求逆解没有正确答案（可能，我印象中当时改了一下就出不来答案了，不知道为什么），当然你自己也可以探索一下。
外部输入theta角值
求正解，假设theta角是未知参数，在程序运行后会弹出对话框依次输入六个theta角的度数，代码如下：
%显示动画时teach的参数和参数设置的参数时对不上的，但是重新调一下teach后就好了
%轨迹规划参数设置
init_ang = [0, 0, 0, 0, 0, 0];
disp("请输入theta数据：");
%输入theta数据
inputstr = inputdlg({'°','°','°','°','°','°'},'请输入',[1 30;1 30;1 30;1 30;1 30;1 30],{'0','0','0','0','0','0'});
nums = str2double(inputstr);
targ_ang = nums*pi/180; %末端执行器的关节参数求正解部分加插值实现动画演示
利用机器人工具箱的相关函数，直接调用就能非常方便的求得运动学正解和插值实现动画演示，并将末端位姿矩阵通过控制台输出和弹出对话框输出，代码如下：
t1 = robot.fkine(targ_ang); %得到末端执行器位姿
disp("末端执行器位姿矩阵：")
t1
%实现从一个初始状态到目的状态的动画
T =(0:0.1:5);
%关节空间轨迹规划方法
[q,qd,qdd] = jtraj(init_ang,targ_ang,T); %直接得到角度、角速度、角加速度的的序列
%%显示
figure(1);
%动画显示
title('动画过程');
robot.plot(q);
%位姿矩阵赋值
a = t1.T;
astr = '';
% 获取矩阵的大小
[rows, cols] = size(a);
% 遍历矩阵的每个元素，并格式化为四位小数
for i = 1:rows
for j = 1:cols
astr = [astr, sprintf('%7.4f ', a(i, j))];
if j < cols
astr = [astr, ' '];
else
astr = [astr, ' '];
end
end
end
% 在对话框中显示矩阵
msgbox(astr, '末端执行器位姿矩阵');逆运动学求解
逆运动学求解就没有调用机器人工具箱的函数了，这里参考了csdn上某位博主的代码（忘了具体是谁了，当时都是找了一会儿的），我自己稍微改了一下，最后会将求得的逆解（就是将前面得到的末端位姿矩阵作为已知量求解输入的6组theta值，有8个可能解，其中有一组是正确解）代码如下：
%逆运动学求解
%%
nx=a(1,1);ox=a(1,2);ax=a(1,3);px=a(1,4);
ny=a(2,1);oy=a(2,2);ay=a(2,3);py=a(2,4);
nz=a(3,1);oz=a(3,2);az=a(3,3);pz=a(3,4);
% 求解关节角1
t1 = (d6*a(1,3)-px);
t2 = (d6*a(2,3)-py);
theta1_1 = atan2(t2,t1) - atan2(d4, real(sqrt(t1^2+t2^2-d4^2))) ;
theta1_2 = atan2(t2,t1) - atan2(d4, -sqrt(t1^2+t2^2-d4^2)) ;
% disp([theta1_1 theta1_2]*180/pi);
% 求解关节角5
theta5_1 = atan2(sqrt((ny*cos(theta1_1)-nx*sin(theta1_1))^2+(oy*cos(theta1_1)-ox*sin(theta1_1))^2), ax*sin(theta1_1)-ay*cos(theta1_1));
theta5_2 = atan2(-sqrt((ny*cos(theta1_1)-nx*sin(theta1_1))^2+(oy*cos(theta1_1)-ox*sin(theta1_1))^2), ax*sin(theta1_1)-ay*cos(theta1_1));
theta5_3 = atan2(sqrt((ny*cos(theta1_2)-nx*sin(theta1_2))^2+(oy*cos(theta1_2)-ox*sin(theta1_2))^2), ax*sin(theta1_2)-ay*cos(theta1_2));
theta5_4 = atan2(-sqrt((ny*cos(theta1_2)-nx*sin(theta1_2))^2+(oy*cos(theta1_2)-ox*sin(theta1_2))^2), ax*sin(theta1_2)-ay*cos(theta1_2));
%disp([theta5_1 theta5_2 theta5_3 theta5_4]*180/pi);
% 求解关节角6
theta6_1 = atan2((ox*sin(theta1_1)-oy*cos(theta1_1))/sin(theta5_1), -(nx*sin(theta1_1)-ny*cos(theta1_1))/sin(theta5_1));
theta6_2 = atan2((ox*sin(theta1_1)-oy*cos(theta1_1))/sin(theta5_2), -(nx*sin(theta1_1)-ny*cos(theta1_1))/sin(theta5_2));
theta6_3 = atan2((ox*sin(theta1_2)-oy*cos(theta1_2))/sin(theta5_3), -(nx*sin(theta1_2)-ny*cos(theta1_2))/sin(theta5_3));
theta6_4 = atan2((ox*sin(theta1_2)-oy*cos(theta1_2))/sin(theta5_4), -(nx*sin(theta1_2)-ny*cos(theta1_2))/sin(theta5_4));
% disp([theta6_1 theta6_2 theta6_3 theta6_4]*180/pi);
% 求解关节角2，3，4
q234_1 = atan2(az/sin(theta5_1), (ax*cos(theta1_1)+ay*sin(theta1_1))/sin(theta5_1));
q234_2 = atan2(az/sin(theta5_2), (ax*cos(theta1_1)+ay*sin(theta1_1))/sin(theta5_2));
q234_3 = atan2(az/sin(theta5_3), (ax*cos(theta1_2)+ay*sin(theta1_2))/sin(theta5_3));
q234_4 = atan2(az/sin(theta5_4), (ax*cos(theta1_2)+ay*sin(theta1_2))/sin(theta5_4));
%disp([q234_1 q234_2 q234_3 q234_4]*180/pi);
A_1 = d6*sin(theta5_1)*cos(q234_1)-d5*sin(q234_1)-px*cos(theta1_1)-py*sin(theta1_1);
B_1 = pz-d1-d5*cos(q234_1)-d6*sin(theta5_1)*sin(q234_1);
A_2 = -px*cos(theta1_1)-py*sin(theta1_1)-d5*sin(q234_2)+d6*sin(theta5_2)*cos(q234_2);
B_2 = pz-d1-d5*cos(q234_2)-d6*sin(theta5_2)*sin(q234_2);
A_3 = -px*cos(theta1_2)-py*sin(theta1_2)-d5*sin(q234_3)+d6*sin(theta5_3)*cos(q234_3);
B_3 = pz-d1-d5*cos(q234_3)-d6*sin(theta5_3)*sin(q234_3);
A_4 = -px*cos(theta1_2)-py*sin(theta1_2)-d5*sin(q234_4)+d6*sin(theta5_4)*cos(q234_4);
B_4 = pz-d1-d5*cos(q234_4)-d6*sin(theta5_4)*sin(q234_4);
% 关节2
theta2_1 = atan2(A_1^2+B_1^2+a3^2-a4^2, sqrt(abs(4*a3^2*(A_1^2+B_1^2)-(A_1^2+B_1^2+a3^2-a4^2)^2)))-atan2(B_1, A_1);
theta2_2 = atan2(A_1^2+B_1^2+a3^2-a4^2, -sqrt(abs(4*a3^2*(A_1^2+B_1^2)-(A_1^2+B_1^2+a3^2-a4^2)^2)))-atan2(B_1, A_1);
theta2_3 = atan2(A_2^2+B_2^2+a3^2-a4^2, sqrt(abs(4*a3^2*(A_2^2+B_2^2)-(A_2^2+B_2^2+a3^2-a4^2)^2)))-atan2(B_2, A_2);
theta2_4 = atan2(A_2^2+B_2^2+a3^2-a4^2, -sqrt(abs(4*a3^2*(A_2^2+B_2^2)-(A_2^2+B_2^2+a3^2-a4^2)^2)))-atan2(B_2, A_2);
theta2_5 = atan2(A_3^2+B_3^2+a3^2-a4^2, sqrt(abs(4*a3^2*(A_3^2+B_3^2)-(A_3^2+B_3^2+a3^2-a4^2)^2)))-atan2(B_3, A_3);
theta2_6 = atan2(A_3^2+B_3^2+a3^2-a4^2, -sqrt(abs(4*a3^2*(A_3^2+B_3^2)-(A_3^2+B_3^2+a3^2-a4^2)^2)))-atan2(B_3, A_3);
theta2_7 = atan2(A_4^2+B_4^2+a3^2-a4^2, sqrt(abs(4*a3^2*(A_4^2+B_4^2)-(A_4^2+B_4^2+a3^2-a4^2)^2)))-atan2(B_4, A_4);
theta2_8 = atan2(A_4^2+B_4^2+a3^2-a4^2, -sqrt(abs(4*a3^2*(A_4^2+B_4^2)-(A_4^2+B_4^2+a3^2-a4^2)^2)))-atan2(B_4, A_4);
% disp([theta2_1 theta2_2 theta2_3 theta2_4 theta2_5 theta2_6 theta2_7 theta2_8]*180/pi);
q23_1 = atan2(-px*cos(theta1_1)-py*sin(theta1_1)-d5*sin(q234_1)+d6*sin(theta5_1)*cos(q234_1)-a3*sin(theta2_1),pz-d1-d5*cos(q234_1)-d6*sin(theta5_1)*sin(q234_1)-a3*cos(theta2_1));
q23_2 = atan2(-px*cos(theta1_1)-py*sin(theta1_1)-d5*sin(q234_1)+d6*sin(theta5_1)*cos(q234_1)-a3*sin(theta2_2),pz-d1-d5*cos(q234_1)-d6*sin(theta5_1)*sin(q234_1)-a3*cos(theta2_2));
q23_3 = atan2(-px*cos(theta1_1)-py*sin(theta1_1)-d5*sin(q234_2)+d6*sin(theta5_2)*cos(q234_2)-a3*sin(theta2_3),pz-d1-d5*cos(q234_2)-d6*sin(theta5_2)*sin(q234_2)-a3*cos(theta2_3));
q23_4 = atan2(-px*cos(theta1_1)-py*sin(theta1_1)-d5*sin(q234_2)+d6*sin(theta5_2)*cos(q234_2)-a3*sin(theta2_4),pz-d1-d5*cos(q234_2)-d6*sin(theta5_2)*sin(q234_2)-a3*cos(theta2_4));
q23_5 = atan2(-px*cos(theta1_2)-py*sin(theta1_2)-d5*sin(q234_3)+d6*sin(theta5_3)*cos(q234_3)-a3*sin(theta2_5),pz-d1-d5*cos(q234_3)-d6*sin(theta5_3)*sin(q234_3)-a3*cos(theta2_5));
q23_6 = atan2(-px*cos(theta1_2)-py*sin(theta1_2)-d5*sin(q234_3)+d6*sin(theta5_3)*cos(q234_3)-a3*sin(theta2_6),pz-d1-d5*cos(q234_3)-d6*sin(theta5_3)*sin(q234_3)-a3*cos(theta2_6));
q23_7 = atan2(-px*cos(theta1_2)-py*sin(theta1_2)-d5*sin(q234_4)+d6*sin(theta5_4)*cos(q234_4)-a3*sin(theta2_7),pz-d1-d5*cos(q234_4)-d6*sin(theta5_4)*sin(q234_4)-a3*cos(theta2_7));
q23_8 = atan2(-px*cos(theta1_2)-py*sin(theta1_2)-d5*sin(q234_4)+d6*sin(theta5_4)*cos(q234_4)-a3*sin(theta2_8),pz-d1-d5*cos(q234_4)-d6*sin(theta5_4)*sin(q234_4)-a3*cos(theta2_8));
% 关节3
theta3_1 = q23_1 - theta2_1;
theta3_2 = q23_2 - theta2_2;
theta3_3 = q23_3 - theta2_3;
theta3_4 = q23_4 - theta2_4;
theta3_5 = q23_5 - theta2_5;
theta3_6 = q23_6 - theta2_6;
theta3_7 = q23_7 - theta2_7;
theta3_8 = q23_8 - theta2_8;
% 关节4
theta4_1 = q234_1 - q23_1;
theta4_2 = q234_1 - q23_2;
theta4_3 = q234_2 - q23_3;
theta4_4 = q234_2 - q23_4;
theta4_5 = q234_3 - q23_5;
theta4_6 = q234_3 - q23_6;
theta4_7 = q234_4 - q23_7;
theta4_8 = q234_4 - q23_8;
disp("逆解(theta)的8组可能解（°）：")
ans_theta = [
theta1_1,theta2_1,theta3_1,theta4_1,theta5_1,theta6_1;
theta1_1,theta2_2,theta3_2,theta4_2,theta5_1,theta6_1;
theta1_1,theta2_3,theta3_3,theta4_3,theta5_2,theta6_2;
theta1_1,theta2_4,theta3_4,theta4_4,theta5_2,theta6_2;
theta1_2,theta2_5,theta3_5,theta4_5,theta5_3,theta6_3;
theta1_2,theta2_6,theta3_6,theta4_6,theta5_3,theta6_3;
theta1_2,theta2_7,theta3_7,theta4_7,theta5_4,theta6_4;
theta1_2,theta2_8,theta3_8,theta4_8,theta5_4,theta6_4;
]*180/pi
% 初始化一个空字符串来存储结果
matrixStr = '';
% 获取矩阵的大小
[rows, cols] = size(ans_theta);
% 遍历矩阵的每个元素，并格式化为四位小数
for i = 1:rows
for j = 1:cols
matrixStr = [matrixStr, sprintf('%7.4f ', ans_theta(i, j))];
if j < cols
matrixStr = [matrixStr, ' '];
else
matrixStr = [matrixStr, ' '];
end
end
end
% 在对话框中显示矩阵
msgbox(matrixStr, '逆解（theta）的8组可能解（单位：°）');结果演示
