Matlab机器⼈⼯具箱（3-1）：五⾃由度机械臂（正逆运动学）
01 正运动学：DH表⽰法
1955年, Denavit和Hartenberg在“ASME Journal of Applied Mechanic”发表了⼀篇论⽂，这篇论⽂介绍了⼀种机器⼈表⽰和建模的⽅法，并导出了它们的运动⽅程,⽬前已成为机器⼈表⽰表⽰和机器⼈运动建模的标准⽅法
Denavit-Hartenberg（D-H）模型表⽰了对机器⼈连杆和关节进⾏建模的⼀种⾮常简单的⽅法，可⽤于任何机器⼈构型,⽽不管机器⼈的结构顺序和复杂程度。
Link()
参数可从DH参数表获得
sigma 为0，表⽰转动关节
offt 表⽰初始⾓度
modified 表⽰采⽤改进D-H法
qlim() 设定关节运动范围
SerialLink()
连接机械臂各杆件，并且返回机械臂名字，供后续使⽤
桂花油plot()
显⽰出机械臂位置。姿态
teach()
⽰教
tool() 指定⼯具端长度
ba() 指定基座位置
说明：
1. 位姿矩阵保存为3格式，与（4×4）矩阵的替换⽅法为：
（若A1为前者，A2为后者）
A2=A1.T;
A1=SE3(A2);
2. ⼯具端和基座都可以通过 ``transl```确定，
⼯具端是 右乘 ， 基座是 左乘
3. L(2).A(pi/3)
获得pi/3时，连杆2的转换矩阵
1.1 建⽴机械臂模型
%5⾃由度机械臂-单臂结构参数
d=[0,0,0,0,0];
a=[0,13,233.24,175.64,0];
alpha=[0, pi/2,0,0, pi/2];
%使⽤offt
L(1)=Link('d',d(1),'a',a(1),'alpha',alpha(1),'modified');
L(2)=Link('d',d(2),'a',a(2),'alpha',alpha(2),'offt',pi/2,'modified'); L(3)=Link('d',d(3),'a',a(3),'alpha',alpha(3),'modified');
L(4)=Link('d',d(4),'a',a(4),'alpha',alpha(4),'offt',pi/2,'modified'); L(5)=Link('d',d(5),'a',a(5),'alpha',alpha(5),'modified');
du=pi/180;
ra=180/pi;
%定义关节范围
L(1).qlim =[-170,170]*du;
L(2).qlim =[60-70,60+70]*du;%-10,130
L(3).qlim =[-70-70,-70+70]*du;%-140,0
L(4).qlim =[-70,70]*du;
L(5).qlim =[-170,170]*du;
bot=SerialLink(L,'name','五⾃由度机械臂');
%l=transl(0,0, tool)
% ach()
% bot.plot([00000])
1.2 计算位姿矩阵
simplify()
化简
% v10
clc;
clear;
syms th1 th2 th3 th4 th5;
考试的图片syms a1 a2 a3 a4 a5;
syms d1 d2 d3 d4 d5;
syms alp1 alp2 alp3 alp4 alp5;
d=[0,0,0,0,0];
a=[0,13,233.24,175.64,0];
alpha=[0, pi/2,0,0, pi/2];
% theta d a alpha
%L(1)=Link([0 d1 a1 alp1 th1],'modified');
%L(2)=Link([0 d2 a2 alp2 th2],'modified');
%L(3)=Link([0 d3 a3 alp3 th3],'modified');
%L(4)=Link([0 d4 a4 alp4 th4],'modified');
%L(5)=Link([0 d5 a5 alp5 th5],'modified');
[d1,d2,d3,d4,d5]=deal(0);
[alp1,alp2,alp3,alp4,alp5]=deal(alpha(1),alpha(2),alpha(3),alpha(4),alpha(5)); a1=0;
L(1)=Link([th1 d1 a1 alp1 0],'modified');
L(2)=Link([th2 d2 a2 alp2 0],'modified');
L(3)=Link([th3 d3 a3 alp3 0],'modified');
L(4)=Link([th4 d4 a4 alp4 0],'modified');
L(5)=Link([th5 d5 a5 alp5 0],'modified');节能知识
bot=SerialLink(L,'name','my robot');
%计算总转换矩阵
T=L(1).A(th1)*L(2).A(th2)*L(3).A(th3)*L(4).A(th4)*L(5).A(th5)
%简化
simplify(T)
02 逆运动学
逆运动学可以分为 迭代法 和 解析法（封闭解）
求封闭解有两个充分条件：
西天目山
三个相邻关节轴相交与⼀点
三个相邻关节轴平⾏
MATLAB机器⼈⼯具箱提供了两种逆运动学函数：
ikine6s()
6轴机械臂的封闭解法
ikine()
机械臂的迭代解法
默认是六轴
可以⽤于⾮六轴的情况
eg:
对于这个五轴机械臂
q2 = bot.ikine(T2,'mask',[1 1 1 1 0 1],'q0',q1);
其中
mask的“1”标记有那些关节
q0表⽰迭代的初始位置是那⾥（影响迭代速度和正确性）
迭代法往往计算速度⽐较慢，所以需要计算五轴机械臂封闭解
封闭解的计算⽅法，关键就是⽐较矩阵中每个位置，具体可以参考⼀个硕⼠论⽂《协作机器⼈零⼒控制与碰撞检测技术研究》2.1 五轴机械臂封闭解
对⽐矩阵1、2
按照
theta1 → theta5→ theta2→ (theta2+theta3)→ theta3→ (theta2+theta3+theta4) → theta4
的顺序计算
theta1 和 theta5 的计算⽅法如下图
在这⾥插⼊图⽚描述
注意求解过程⼀定要使⽤atan2()
不要⽤arctan()，arcsin()之类的
以此结果得到逆运动学函数（共四组解）：
%逆运动学求解函数
%输⼊机器⼈名称，转换矩阵（SE3）
%输出关节⾓度
%说明：函数执⾏过程中打印4组解（deg），⽬前函数返回第三组解（rad）
function [ th ]=MDH_IK(robot,T)
TT=SE3(T);%矩阵转换为SE3格式
T =inv(robot.ba)* TT *l);培训自我鉴定
%对于多个连续的问题，可设置k
% T =inv(robot.ba)*TT(k)*l);手臂抽筋
%将函数变量导⼊
% T是SE3类型函数，所以不能⽤下列写法(也可以转换为T=T.T）
%[nx ny nz]=deal(T(1,1),T(2,1),T(3,1));
%[ox oy oz]=deal(T(1,2),T(2,2),T(3,2));
%[ax ay az]=deal(T(1,3),T(2,3),T(3,3));
%[px py pz]=deal(T(1,4),T(2,4),T(3,4));
n=T.n;
o=T.o;
a=T.a;
p=T.t;
[nx ny nz]=deal(n(1),n(2),n(3));
[ox oy oz]=deal(o(1),o(2),o(3));
[ax ay az]=deal(a(1),a(2),a(3));
[px py pz]=deal(p(1),p(2),p(3));
%结构参数
syms theta1 theta2 theta3 theta4 theta5 ;
syms a1 a2 a3 d5 ;
%alpha,a,d,theta
DH_Tab =[0000;
901300;
0233.2400;
0175.6400;
90000];
%参数赋值
a0=robot.a(1);a1=robot.a(2);a2=robot.a(3);a3=robot.a(4);a4=robot.a(5);
%⾓度转弧度
%DH_Tab(1:5,1)=DH_Tab(1:5,1)/180*pi;
% alp0=DH_Tab(1,1); alp1=DH_Tab(2,1);alp2=DH_Tab(3,1);alp3=DH_Tab(4,1);alp4=DH_Tab(5,1);
%%求解过程1
theta1_1=atan2(-py,-px);
theta1_2=atan2(py,px);
theta1_2=atan2(py,px);
theta1=theta1_1;
theta5=atan2(nx*sin(theta1)-ny*cos(theta1),ox*sin(theta1)-oy*cos(theta1));
theta234=atan2(nz,nx*cos(theta1)+ny*sin(theta1));
A=px*cos(theta1)+py*sin(theta1)-a1;
炒虾仁怎么做B=pz;
theta2_1=atan2(A^2+B^2+a2^2-a3^2,sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B); theta2_2=atan2(A^2+B^2+a2^2-a3^2,-sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B); theta2=theta2_1;
theta23=atan2(pz-a2*sin(theta2),px*cos(theta1)+py*sin(theta1)-a1-a2*cos(theta2));
theta3=theta23-theta2;
theta4=theta234-theta23;%theta4不⼀样只是⼀种表⽰⽽已，差别360°，就是转⼀圈，位置是⼀样的if(theta4>pi)
theta4=theta4-2*pi;
end
if(theta4<-pi)
theta4=theta4+2*pi;
end
th1=[theta1,theta2,theta3,theta4,theta5];
for j=1:robot.n
th1(j)=th1(j)- robot.offt(j);
end
%输出
th1=th1*180/pi
%%求解过程2
theta1_1=atan2(-py,-px);
theta1_2=atan2(py,px);
theta1=theta1_2;
theta5=atan2(nx*sin(theta1)-ny*cos(theta1),ox*sin(theta1)-oy*cos(theta1));
theta234=atan2(nz,nx*cos(theta1)+ny*sin(theta1));
A=px*cos(theta1)+py*sin(theta1)-a1;
B=pz;
theta2_1=atan2(A^2+B^2+a2^2-a3^2,sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B); theta2_2=atan2(A^2+B^2+a2^2-a3^2,-sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B); theta2=theta2_1;
theta23=atan2(pz-a2*sin(theta2),px*cos(theta1)+py*sin(theta1)-a1-a2*cos(theta2));
theta3=theta23-theta2;
theta4=theta234-theta23;%theta4不⼀样只是⼀种表⽰⽽已，差别360°，就是转⼀圈，位置是⼀样的if(theta4>pi)
theta4=theta4-2*pi;
end
if(theta4<-pi)
theta4=theta4+2*pi;
end
th2=[theta1,theta2,theta3,theta4,theta5];
for j=1:robot.n
th2(j)=th2(j)- robot.offt(j);
end
%输出
th2=th2*180/pi
%%求解过程3
theta1_1=atan2(-py,-px);
theta1_2=atan2(py,px);
theta1=theta1_1;
theta5=atan2(nx*sin(theta1)-ny*cos(theta1),ox*sin(theta1)-oy*cos(theta1));
theta234=atan2(nz,nx*cos(theta1)+ny*sin(theta1));
A=px*cos(theta1)+py*sin(theta1)-a1;
B=pz;情什么初开
theta2_1=atan2(A^2+B^2+a2^2-a3^2,sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);
