毕设中用到了很多代码，其中一部分我通过看书和看论文学习并实现的代码，会通过Gitee仓库分享出来，这些代码仅用于学习使用，祝各位毕业生顺利完成毕设！

毕设系列内容：毕业设计——四自由度机械臂轨迹规划

毕设(2)——机械臂正逆运动学分析

建立好运动学模型之后，就可以开始对机械臂进行运动学分析，这也是机械臂轨迹规划中重要的基础之一，B站上我看的一个课程非常好，是一位台湾的教授讲的课，网址是机器人运动学—林沛群

正向运动学分析

改进型DH的齐次变换矩阵形式如下

根据机械臂的结构几何关系，可以得到
则可以将表达式简化为以下形式

function [T05] = myfkine(Angle_T)
%运动学正解
theta1 = Angle_T(1, 1);
theta2 = Angle_T(1, 2);
theta3 = Angle_T(1, 3);
theta4 = Angle_T(1, 4);
theta5 = Angle_T(1, 5);
theta23 = theta2 + theta3;
a2 = 135; a3 = 147; a4 = 61; d5 = 131;
T01 = [cos(theta1) -sin(theta1) 0 0;
       sin(theta1) cos(theta1)  0 0;
       0           0            1 0;
       0           0            0 1];
T14 = [1 0  0 cos(theta23) * a3 + cos(theta2) * a2;
       0 0  1 0;
       0 -1 0 -sin(theta23) * a3 - sin(theta2) * a2;
       0 0  0 1];
T45 = [cos(theta5)  -sin(theta5) 0 a4;
       0            0            1 d5;
       -sin(theta5) -cos(theta5) 0 0;
       0            0            0 1];
T05 = T01 * T14 * T45;

逆向运动学

逆向运动学也是类似操作，最需要了解的内容是理论部分
代码较长，文中就不放出来了，你可以在代码仓库中找到它

验证程序

需要注意的是，机器人工具箱中的ikine函数，支持的是带有球形手腕的6轴机械臂，通常对6自由度以下的机械臂支持不好，会报错，所以逆运动学没法用工具箱来验证，只是将逆解输入到正解函数中再次求解，看看两次正解情况是否相同

test2.m

% Modified DH 建模dobot
clear, clc, close all;

%建立机器人模型
%           关节角   关节偏距  连杆长度   连杆转角   旋转关节   偏差
%           theta    d        a         alpha     sigma      offset
L(1) = Link([0       0        0         0         0          0], 'modified');
L(2) = Link([0       0        0         -pi / 2   0          0], 'modified');
L(3) = Link([0       0        135       0         0          0], 'modified');
L(4) = Link([0       0        147       0         0          0], 'modified');
L(5) = Link([0       131      61        -pi / 2   0          0], 'modified');

%连接连杆
robot = SerialLink(L, 'name', 'Dobot');

angle1 = [-pi / 6, -pi / 6, pi / 3, -pi / 6, pi / 4];
angle2 = [pi / 3, -pi / 60, pi / 30, -pi / 60, -pi / 4];

%运动学正解
fk1 = myfkine(angle1); %正解函数
fk2 = robot.fkine(angle1);%工具箱正解函数
%fk1 = myfkine(angle2); %正解函数
%fk2 = robot.fkine(angle2);%工具箱正解函数

%运动学逆解
ik1 = myikine(fk1);%逆解函数
%实际上robot.ikine函数并不可用
%ik2 = robot.ikine(fk1);%工具箱逆解函数
degik1 = rad2deg(ik1);

%再次运动学正解
ik1_fk = myfkine(ik1(1, :));

在matlab命令行中查看fk1和ik1_fk，可以看到两个齐次矩阵是一模一样的

本文到此结束，后续会继续更新的~