前提

用机器人工具箱求解会方便很多，如果没有安装或不会安装，请参考
https://blog.csdn.net/AprilsHell/article/details/90722892

原理

D-H参数分别为关节扭角，连杆长度，连杆转角，连杆距离。所有都是齐次矩阵。

代码

clear
N=input('请输入自由度数：');%几个轴就是输入几
for i=1:Nalf(i)=input('alf=');%关节扭角alf(i)=alf(i)*pi/180;%转换为度a{i}=sym(input('a=','s'));%连杆长度d{i}=sym(input('d=','s'));%连杆距离t{i}=strcat('th',num2str(i));%连杆转角为符号变量th=sym(t{i});T{i}=round(trotx(alf(i)),4)*transl(a{i},0,0)*trotz(th)*transl(0,0,d{i});%套用公式
end
Tf0=eye(4);
for j=1:NT{j}Tf0=Tf0*T{j};%求运动学方程
end
Tf0