MD-H模型运动学正解、逆解及姿态角的解算验证

---

前言

记录修正D-H模型正运动学、RPY、逆运动学解算的验证过程

---

一、MD-H模型正运动学验证

%% 正运动学验证
clear;clc;close all;
%
%杆长参数设置
L1=160;L2=449.5;L3=580;L4=200;L5=640;L6=228;
d1=L2;a2=L1;a3=L3;a4=L4;d4=L5;d6=L6;
%初始角设置
theta1=10;theta2=20;theta3=30;theta4=40;theta5=50;theta6=60;
T1=[cosd(theta1) -sind(theta1) 0 0; sind(theta1) cosd(theta1) 0 0; 0 0 1 d1; 0 0 0 1]*...
  [sind(theta2) cosd(theta2) 0 a2; 0 0 1 0; cosd(theta2) -sind(theta2) 0 0; 0 0 0 1]*...
  [cosd(theta3) -sind(theta3) 0 a3; sind(theta3) cosd(theta3) 0 0; 0 0 1 0; 0 0 0 1]*...
  [cosd(theta4) -sind(theta4) 0 a4; 0 0 1 d4; -sind(theta4) -cosd(theta4) 0 0; 0 0 0 1]*...
  [cosd(theta5) -sind(theta5) 0 0; 0 0 -1 0; sind(theta5) cosd(theta5) 0 0;0 0 0 1]*...
  [cosd(theta6) -sind(theta6) 0 0; 0 0 1 d6; -sind(theta6) -cosd(theta6) 0 0; 0 0 0 1]

s1=sind(theta1);s2=sind(theta2);s3=sind(theta3);s4=sind(theta4);s5=sind(theta5);s6=sind(theta6);
c1=cosd(theta1);c2=cosd(theta2);c3=cosd(theta3);c4=cosd(theta4);c5=cosd(theta5);c6=cosd(theta6);
s23=s2*c3+c2*s3;c23=c2*c3-s2*s3;
nx=c1*(s23*(c4*c5*c6-s4*s6)+c23*s5*c6)-s1*(-s4*c5*c6-c4*s6);
ny=s1*(s23*(c4*c5*c6-s4*s6)+c23*s5*c6)+c1*(-s4*c5*c6-c4*s6);
nz=c23*(c4*c5*c6-s4*s6)-s23*s5*c6;
ox=c1*(s23*(-c4*c5*s6-s4*c6)-c23*s5*s6)-s1*(s4*c5*s6-c4*c6);
oy=s1*(s23*(-c4*c5*s6-s4*c6)-c23*s5*s6)+c1*(s4*c5*s6-c4*c6);
oz=c23*(-c4*c5*s6-s4*c6)+s23*s5*s6;
ax=c1*(-s23*c4*s5+c23*c5)-s1*s4*s5;
ay=s1*(-s23*c4*s5+c23*c5)+c1*s4*s5;
az=-c23*c4*s5-s23*c5;
px=c1*(s23*(-d6*c4*s5+a4)+c23*(d6*c5+d4)+a3*s2+a2)-s1*d6*s4*s5;
py=s1*(s23*(-d6*c4*s5+a4)+c23*(d6*c5+d4)+a3*s2+a2)+c1*d6*s4*s5;
pz=c23*(-d6*c4*s5+a4)-s23*(d6*c5+d4)+a3*c2+d1;
T2=[nx ox ax px;...
   ny oy ay py;...
   nz oz az pz;...
   0  0  0  1 ]

验证结果：矩阵计算结果T1与代码公式计算结果T2相同

二、RPY姿态角解算验证

%% RPY姿态角验证
clear;clc;close all;
%
%杆长参数设置
L1=160;L2=449.5;L3=580;L4=200;L5=640;L6=228;
d1=L2;a2=L1;a3=L3;a4=L4;d4=L5;d6=L6;
%初始角设置
theta1=10;theta2=20;theta3=30;theta4=40;theta5=50;theta6=60;

s1=sind(theta1);s2=sind(theta2);s3=sind(theta3);s4=sind(theta4);s5=sind(theta5);s6=sind(theta6);
c1=cosd(theta1);c2=cosd(theta2);c3=cosd(theta3);c4=cosd(theta4);c5=cosd(theta5);c6=cosd(theta6);
s23=s2*c3+c2*s3;c23=c2*c3-s2*s3;
nx=c1*(s23*(c4*c5*c6-s4*s6)+c23*s5*c6)-s1*(-s4*c5*c6-c4*s6);
ny=s1*(s23*(c4*c5*c6-s4*s6)+c23*s5*c6)+c1*(-s4*c5*c6-c4*s6);
nz=c23*(c4*c5*c6-s4*s6)-s23*s5*c6;
ox=c1*(s23*(-c4*c5*s6-s4*c6)-c23*s5*s6)-s1*(s4*c5*s6-c4*c6);
oy=s1*(s23*(-c4*c5*s6-s4*c6)-c23*s5*s6)+c1*(s4*c5*s6-c4*c6);
oz=c23*(-c4*c5*s6-s4*c6)+s23*s5*s6;
ax=c1*(-s23*c4*s5+c23*c5)-s1*s4*s5;
ay=s1*(-s23*c4*s5+c23*c5)+c1*s4*s5;
az=-c23*c4*s5-s23*c5;
px=c1*(s23*(-d6*c4*s5+a4)+c23*(d6*c5+d4)+a3*s2+a2)-s1*d6*s4*s5;
py=s1*(s23*(-d6*c4*s5+a4)+c23*(d6*c5+d4)+a3*s2+a2)+c1*d6*s4*s5;
pz=c23*(-d6*c4*s5+a4)-s23*(d6*c5+d4)+a3*c2+d1;
T2=[nx ox ax px;...
   ny oy ay py;...
   nz oz az pz;...
   0  0  0  1 ];
pitch=atan2(ax,sqrt(ay^2+az^2))*180/pi;
if pitch==90
   yaw=0;
   roll=atan2(ny,-nz)*180/pi;
elseif pitch==-90
   yaw=0;
   roll=atan2(-ny,nz)*180/pi;   
else
    roll=atan2(-ay/cosd(pitch),az/cosd(pitch))*180/pi;
    yaw=atan2(-ox/cosd(pitch),nx/cosd(pitch))*180/pi;
end

XYZ=[px py pz]
RPY=[roll pitch yaw]

验证结果：计算结果与MATLAB机器人工具箱的仿真结果相同

三、MD-H模型逆运动学验证

%% 逆运动学验证
clear;clc;close all;
L1=160;L2=449.5;L3=580;L4=200;L5=640;L6=228;
d1=L2;a2=L1;a3=L3;a4=L4;d4=L5;d6=L6;
%初始角设置
w1=60;w2=50;w3=40;w4=30;w5=20;w6=10;

T=[cosd(w1) -sind(w1) 0 0; sind(w1) cosd(w1) 0 0; 0 0 1 d1; 0 0 0 1]*...
  [sind(w2) cosd(w2) 0 a2; 0 0 1 0; cosd(w2) -sind(w2) 0 0; 0 0 0 1]*...
  [cosd(w3) -sind(w3) 0 a3; sind(w3) cosd(w3) 0 0; 0 0 1 0; 0 0 0 1]*...
  [cosd(w4) -sind(w4) 0 a4; 0 0 1 d4; -sind(w4) -cosd(w4) 0 0; 0 0 0 1]*...
  [cosd(w5) -sind(w5) 0 0; 0 0 -1 0; sind(w5) cosd(w5) 0 0;0 0 0 1]*...
  [cosd(w6) -sind(w6) 0 0; 0 0 1 d6; -sind(w6) -cosd(w6) 0 0; 0 0 0 1];
nx=T(1,1);ox=T(1,2);ax=T(1,3);px=T(1,4);
ny=T(2,1);oy=T(2,2);ay=T(2,3);py=T(2,4);
nz=T(3,1);oz=T(3,2);az=T(3,3);pz=T(3,4);

%theta1
Theta1=atan2(-(d6*ay-py),-(d6*ax-px))*180/pi;
%theta3
s1=sind(Theta1);c1=cosd(Theta1);
f1=c1*ax+s1*ay;
f2=c1*px+s1*py-a2;
f3=pz-d1;
k1=2*a3*d4;
k2=-2*a3*a4;
k3=a4^2+d4^2+a3^2-(-f1*d6+f2)^2-(az*d6-f3)^2;
Theta3=(atan2(k3,sqrt(k1^2+k2^2-k3^2))-atan2(k2,k1))*180/pi;
%theta2
c3=cosd(Theta3);s3=sind(Theta3);
g1=f2-d6*f1;
g2=f3-d6*az;
g3=a4*c3-d4*s3+a3;
Theta2=(atan2(g3,sqrt(g1^2+g2^2-g3^2))-atan2(g2,g1))*180/pi;
%theta5
c2=cosd(Theta2);s2=sind(Theta2);
s23=s2*c3+c2*s3;c23=c2*c3-s2*s3;
Theta5=acosd(f1*c23-az*s23);
%theta4
s5=sind(Theta5);c5=cosd(Theta5);
Theta4=asind((-s1*ax+c1*ay)/s5);
%theta6
c4=cosd(Theta4);
h1=-s1*nx+c1*ny;
h2=-s1*ox+c1*oy;
h3=-c4;
Theta6=(atan2(h3,-sqrt(h1^2+h2^2-h3^2))-atan2(h2,h1))*180/pi;

THETA=[Theta1,Theta2,Theta3,Theta4,Theta5,Theta6]

验证结果：计算结果与初始设置的角度相同