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

---

前言

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

---

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

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

T1=[cosd(theta1) 0 -sind(theta1) L1*cosd(theta1);sind(theta1) 0 cosd(theta1) L1*sind(theta1);0 -1 0 L2;0 0 0 1]*...
  [sind(theta2) cosd(theta2) 0 L3*sind(theta2);-cosd(theta2) sind(theta2) 0 -L3*cosd(theta2);0 0 1 0;0 0 0 1]*...
  [cosd(theta3) 0 -sind(theta3) L4*cosd(theta3);sind(theta3) 0 cosd(theta3) L4*sind(theta3);0 -1 0 0;0 0 0 1]*...
  [cosd(theta4) 0 sind(theta4) 0;sind(theta4) 0 -cosd(theta4) 0;0 1 0 L5;0 0 0 1]*...
  [cosd(theta5) 0 -sind(theta5) 0;sind(theta5) 0 cosd(theta5) 0;0 -1 0 0;0 0 0 1]*...
  [cosd(theta6) -sind(theta6) 0 0;sind(theta6) cosd(theta6) 0 0;0 0 1 L6;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*(-d6*s23*c4*s5+c23*(d6*c5+d4)+a3*s23+a2*s2)-s1*d6*s4*s5+a1*c1;
Py=s1*(-d6*s23*c4*s5+c23*(d6*c5+d4)+a3*s23+a2*s2)+c1*d6*s4*s5+a1*s1;
Pz=-d6*c23*c4*s5-s23*(d6*c5+d4)+a3*c23+a2*c2+d1;
T2=[Nx Ox Ax Px;
    Ny Oy Ay Py;
    Nz Oz Az Pz;
    0  0  0  1]

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

二、RPY姿态角解算验证

%% RPY姿态角验证(T矩阵转换为RPY)
clear;clc;close all;
%杆长参数设置
L1=160;L2=449.5;L3=580;L4=200;L5=640;L6=228;
a1=L1;d1=L2;a2=L3;a3=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*(-d6*s23*c4*s5+c23*(d6*c5+d4)+a3*s23+a2*s2)-s1*d6*s4*s5+a1*c1;
Py=s1*(-d6*s23*c4*s5+c23*(d6*c5+d4)+a3*s23+a2*s2)+c1*d6*s4*s5+a1*s1;
Pz=-d6*c23*c4*s5-s23*(d6*c5+d4)+a3*c23+a2*c2+d1;
TT=[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机器人工具箱的仿真结果相同

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

%% 逆运动验证
%正运动采用矩阵计算，避免参数与逆运动学参数混淆
%逆运动学公式中凡是带根号的，都默认为有两组解

clear;clc;close all;
L1=160;L2=449.5;L3=580;L4=200;L5=640;L6=228;
a1=L1;d1=L2;a2=L3;a3=L4;d4=L5;d6=L6;
%初始角度设置
w1=10;w2=20;w3=30;w4=40;w5=50;w6=60;
T=[cosd(w1) 0 -sind(w1) L1*cosd(w1);sind(w1) 0 cosd(w1) L1*sind(w1);0 -1 0 L2;0 0 0 1]*...
  [sind(w2) cosd(w2) 0 L3*sind(w2);-cosd(w2) sind(w2) 0 -L3*cosd(w2);0 0 1 0;0 0 0 1]*...
  [cosd(w3) 0 -sind(w3) L4*cosd(w3);sind(w3) 0 cosd(w3) L4*sind(w3);0 -1 0 0;0 0 0 1]*...
  [cosd(w4) 0 sind(w4) 0;sind(w4) 0 -cosd(w4) 0;0 1 0 L5;0 0 0 1]*...
  [cosd(w5) 0 -sind(w5) 0;sind(w5) 0 cosd(w5) 0;0 -1 0 0;0 0 0 1]*...
  [cosd(w6) -sind(w6) 0 0;sind(w6) cosd(w6) 0 0;0 0 1 L6;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-a1;
f3=pz-d1;
k1=2*a2*d4;
k2=-2*a2*a3;
k3=d4^2+a3^2+a2^2-(-d6*f1+f2)^2-(d6*az-f3)^2;
theta3=(atan2(k3,sqrt(k1^2+k2^2-k3^2))-atan2(k2,k1))*180/pi;
%theta2
s3=sind(theta3);c3=cosd(theta3);
g1=-d6*f1+f2;
g2=-d6*az+f3;
g3=-d4*s3+a3*c3+a2;
theta2=(atan2(g3,sqrt(g1^2+g2^2-g3^2))-atan2(g2,g1))*180/pi;
%theta5
s2=sind(theta2);c2=cosd(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
s4=sind(theta4);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]

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