参考：
六轴机器人matlab写运动学逆解函数（改进DH模型）_JojenZz的博客-优快云博客_matlab运动学逆解
六轴机器人轨迹规划之五段位置s曲线插补_JojenZz的博客-优快云博客_六轴机器人路径规划
dh参数
1. 标准的STD_DH dh建模
Trans zi-1（di）:沿着zi-1轴平移di； Rot zi-1（
）:沿着zi-1轴旋转 ；Trans xi（ai）:沿着xi轴配平移 ai ；Rot xi-1（
）沿着xi-1轴旋转
2.改进型MOD_DH dh建模
3. 标准的STD_DH dh建模与改进型MOD_DH dh建模 区别比较：
标准DH参数和改进DH参数之间的差异是连接到连杆的坐标系的位置和齐次变换的顺序。
3.1标准的DH参数原点 Oi 坐标建立在末端，改进的DH参数的原点 Oi 坐标建立在首端
变换矩阵：
4.dh建模 参考ur改进的DH建模
把初始位置竖起来：
5.验证dh参数是否准确
5.1.验证dh参数是否准确
d：连杆偏距 ； a：杆长 ；alpha：连杆转角 ； offset初始关节角度；
clear,clc,close all;
%建立机器人DH参数，初始状态为竖直状态
% d：连杆偏距 ； a：杆长 ；alpha：连杆转角 ； offset初始关节角度；
L1=Link('d',144,'a',0,'alpha',0,'modified');
L2=Link('d',0,'a',0,'alpha',pi/2,'offset',-pi/2,'modified');
L3=Link('d',0,'a',-264,'alpha',0,'modified');
L4=Link('d',106,'a',-236,'alpha',0,'offset',-pi/2,'modified');
L5=Link('d',114,'a',0,'alpha',pi/2,'modified');
L6=Link('d',67,'a',0,'alpha',-pi/2,'modified');
%建立机器人
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','Arm6') ;
% 初始角度 都为0
Theta=[0 0 0 0 0 0];
%初始角度换算成弧度
Theta=Theta/180*pi;
%求正解的齐次变换矩阵
forwarda=robot.fkine(Theta)
W=[-1000,+1000,-1000,+1000,-1000,+1000];
%显示三维动画
robot.plot(Theta,'tilesize',150,'workspace',W);
%显示roll/pitch/yaw angles，GUI可调界面
robot.teach(forwarda,'rpy' ) 
转换成rpy姿态
rpy2r：rpy转换成旋转矩阵
r2rpy：旋转矩阵转换成rpy
R=rpy2r(0.1,0.2,0.3,'xyz')
R=r2rpy(R,'xyz')
6.正解：
ForwardSolver_MDH.m
function [T06,Pos]=ForwardSolver_MDH(theta)
%% 建立机器人DH参数，初始状态为竖直状态
%% d：连杆偏距 ； a：杆长 ；alpha：连杆转角 ； offset初始关节角度；
%% L1=Link('d',144,'a',0,'alpha',0,'modified');
%% L2=Link('d',0,'a',0,'alpha',pi/2,'offset',-pi/2,'modified');
%% L3=Link('d',0,'a',-264,'alpha',0,'modified');
%% L4=Link('d',106,'a',-236,'alpha',0,'offset',-pi/2,'modified');
%% L5=Link('d',114,'a',0,'alpha',pi/2,'modified');
%% L6=Link('d',67,'a',0,'alpha',-pi/2,'modified');
%% [alpha,a, d,offset] ******DH参数*****
%% 参数 1：连杆转角alpha ； 参数2：杆长a ； 参数3：连杆偏距d ； 参数4：初始关节角度offset；
DH_JXB =[ 0 0 144 0;
90 0 0 -90;
0 -264 0 0;
0 -236 106 -90;
90 0 114 0;
-90 0 67 0
];
d=DH_JXB(1:6,3);
a0=DH_JXB(1,2); a1=DH_JXB(2,2);a2=DH_JXB(3,2);a3=DH_JXB(4,2);a4=DH_JXB(5,2); a5=DH_JXB(6,2);
DH_JXB(1:6,1)=DH_JXB(1:6,1)/180*pi;
alp0=DH_JXB(1,1); alp1=DH_JXB(2,1);alp2=DH_JXB(3,1);alp3=DH_JXB(4,1);alp4=DH_JXB(5,1); alp5=DH_JXB(6,1);
offset=[0 -90 0 -90 0 0];
theta=(theta+offset)*pi/180;
theta1=theta(1);theta2=theta(2);theta3=theta(3);theta4=theta(4);theta5=theta(5);theta6=theta(6);
T01=[ cos(theta1), -sin(theta1), 0, a0;
sin(theta1)*cos(alp0), cos(theta1)*cos(alp0),-sin(alp0),-d(1)*sin(alp0);
sin(theta1)*sin(alp0), cos(theta1)*sin(alp0), cos(alp0), d(1)*cos(alp0);
0, 0, 0, 1
];
T12=[ cos(theta2), -sin(theta2), 0, a1;
sin(theta2)*cos(alp1), cos(theta2)*cos(alp1),-sin(alp1),-d(2)*sin(alp1);
sin(theta2)*sin(alp1), cos(theta2)*sin(alp1), cos(alp1), d(2)*cos(alp1);
0, 0, 0, 1
];
T23=[ cos(theta3), -sin(theta3), 0, a2;
sin(theta3)*cos(alp2), cos(theta3)*cos(alp2),-sin(alp2),-d(3)*sin(alp2);
sin(theta3)*sin(alp2), cos(theta3)*sin(alp2), cos(alp2), d(3)*cos(alp2);
0, 0, 0, 1
];
T34=[ cos(theta4), -sin(theta4), 0, a3;
sin(theta4)*cos(alp3), cos(theta4)*cos(alp3),-sin(alp3),-d(4)*sin(alp3);
sin(theta4)*sin(alp3), cos(theta4)*sin(alp3), cos(alp3), d(4)*cos(alp3);
0, 0, 0, 1
];
T45=[ cos(theta5), -sin(theta5), 0, a4;
sin(theta5)*cos(alp4), cos(theta5)*cos(alp4),-sin(alp4),-d(5)*sin(alp4);
sin(theta5)*sin(alp4), cos(theta5)*sin(alp4), cos(alp4), d(5)*cos(alp4);
0, 0, 0, 1
];
T56=[ cos(theta6), -sin(theta6), 0, a5;
sin(theta6)*cos(alp5), cos(theta6)*cos(alp5),-sin(alp5),-d(6)*sin(alp5);
sin(theta6)*sin(alp5), cos(theta6)*sin(alp5), cos(alp5), d(6)*cos(alp5);
0, 0, 0, 1
];
disp('Homogeneous transformation matrix T06:')
T06=T01*T12*T23*T34*T45*T56;
%% 求末端位置
X=T06(1,4);Y=T06(2,4);Z=T06(3,4);
%% 求末端姿态Rotations about X, Y, Z axes (for a robot gripper)
R=T06;
if abs(abs(R(1,3)) - 1) < eps % when |R13| == 1
% singularity
rpy(1) = 0; % roll is zero
if R(1,3) > 0
rpy(3) = atan2( R(3,2), R(2,2)); % R+Y
else
rpy(3) = -atan2( R(2,1), R(3,1)); % R-Y
end
rpy(2) = asin(R(1,3));
else
rpy(1) = -atan2(R(1,2), R(1,1));
rpy(3) = -atan2(R(2,3), R(3,3));
rpy(2) = atan(R(1,3)*cos(rpy(1))/R(1,1));
end
RPY=rpy*180/pi;
Rall=RPY(1);Pitch=RPY(2);Yaw=RPY(3);
Pos=[X,Y,Z,Rall,Pitch,Yaw];
end7. 6关节逆解
InverseSolver_MDH.m
function AllSloverTheta =InverseSolver_MDH(Pos)
%#codegen
%% coder.extrinsic 声明外部函数。
%% 在仿真期间，代码生成器会为外部函数的调用生成代码，但不会生成函数的内部代码。
coder.extrinsic('disp');
%% 建立机器人DH参数，初始状态为竖直状态
%% L1=Link('d',144,'a',0,'alpha',0,'modified');
%% L2=Link('d',0,'a',0,'alpha',pi/2,'offset',-pi/2,'modified');
%% L3=Link('d',0,'a',-264,'alpha',0,'modified');
%% L4=Link('d',106,'a',-236,'alpha',0,'offset',-pi/2,'modified');
%% L5=Link('d',114,'a',0,'alpha',pi/2,'modified');
%% L6=Link('d',67,'a',0,'alpha',-pi/2,'modified');
AllSloverTheta = zeros(8,6);A=zeros(1,4);
%% [alpha,a, d,offset]
DH_JXB =[0 0 144 0;
90 0 0 -90;
0 -264 0 0;
0 -236 106 -90;
90 0 114 0;
-90 0 67 0];
p2=DH_JXB(2,4);p4=DH_JXB(4,4);
a2=DH_JXB(3,2);a3=DH_JXB(4,2);d1=DH_JXB(1,3);d4=DH_JXB(4,3);d5=DH_JXB(5,3);
X=Pos(1);Y=Pos(2);Z=Pos(3);
gama=Pos(4)*pi/180;beta=Pos(5)*pi/180;alpha=Pos(6)*pi/180;
theta7=0;a6=0;afa6=0;d7=DH_JXB(6,3);
T67=[ cos(theta7),-sin(theta7),0,a6;
sin(theta7)*cos(afa6),cos(theta7)*cos(afa6),-sin(afa6),-sin(afa6)*d7;
sin(theta7)*sin(afa6),cos(theta7)*sin(afa6),cos(afa6),cos(afa6)*d7;
0,0,0,1
];
T_goat=[ cos(beta)*cos(gama),-cos(beta)*sin(gama),sin(beta),X;
sin(alpha)*sin(beta)*cos(gama)+cos(alpha)*sin(gama), ...
-sin(alpha)*sin(beta)*sin(gama)+cos(alpha)*cos(gama), -sin(alpha)*cos(beta),Y;
-cos(alpha)*sin(beta)*cos(gama)+sin(alpha)*sin(gama),...
cos(alpha)*sin(beta)*sin(gama)+sin(alpha)*cos(gama), cos(alpha)*cos(beta),Z;
0,0,0,1
];
T06=T_goat/T67;
nx=T06(1,1);ny=T06(2,1);
ox=T06(1,2);oy=T06(2,2);
ax=T06(1,3);ay=T06(2,3);az=T06(3,3);
px=T06(1,4);py=T06(2,4);pz=T06(3,4);
k=0;
ForJudgment=px^2+py^2-d4^2;
if ForJudgment<-1e-6
disp('Out of workspace Unable to solve');
else
if ForJudgment>=-1e-6&&ForJudgment<0
ForJudgment=0;
end
theta1_1=atan2(py,px)-atan2(-d4,sqrt(ForJudgment));
theta1_2=atan2(py,px)-atan2(-d4,-sqrt(ForJudgment));
S5_1=sqrt((-sin(theta1_1)*nx+cos(theta1_1)*ny)^2+(-sin(theta1_1)*ox+cos(theta1_1)*oy)^2);
theta5_1=atan2(S5_1,sin(theta1_1)*ax-cos(theta1_1)*ay);
S5_2=-sqrt((-sin(theta1_1)*nx+cos(theta1_1)*ny)^2+(-sin(theta1_1)*ox+cos(theta1_1)*oy)^2);
theta5_2=atan2(S5_2,sin(theta1_1)*ax-cos(theta1_1)*ay);
S5_3=sqrt((-sin(theta1_2)*nx+cos(theta1_2)*ny)^2+(-sin(theta1_2)*ox+cos(theta1_2)*oy)^2);
theta5_3=atan2(S5_3,sin(theta1_2)*ax-cos(theta1_2)*ay);
S5_4=-sqrt((-sin(theta1_2)*nx+cos(theta1_2)*ny)^2+(-sin(theta1_2)*ox+cos(theta1_2)*oy)^2);
theta5_4=atan2(S5_4,sin(theta1_2)*ax-cos(theta1_2)*ay);
S234 = [0; 0; 0; 0];C234 = [0; 0; 0; 0];
B = [0; 0; 0; 0];B1 = [0; 0; 0; 0];B2 = [0; 0; 0; 0];
C = [0; 0; 0; 0];
theta2 = [0; 0; 0; 0; 0; 0; 0; 0];
theta23 = [0; 0; 0; 0; 0; 0; 0; 0];
theta234 = [0; 0; 0; 0; 0; 0; 0; 0];
theta3 = [0; 0; 0; 0; 0; 0; 0; 0];
theta4 = [0; 0; 0; 0; 0; 0; 0; 0];
if abs(S5_1)>1e-6
theta6_1=atan2((-sin(theta1_1)*ox+cos(theta1_1)*oy)/S5_1,(sin(theta1_1)*nx-cos(theta1_1)*ny)/S5_1);
S234(1)=-az/S5_1;C234(1)=-(cos(theta1_1)*ax+sin(theta1_1)*ay)/S5_1;theta234(1)=atan2(S234(1),C234(1));
B1(1)=cos(theta1_1)*px+sin(theta1_1)*py-d5*S234(1);B2(1)=pz-d1+d5*C234(1);
A(1)=-2*B2(1)*a2;B(1)=2*B1(1)*a2;C(1)=B1(1)^2+B2(1)^2+a2^2-a3^2;
if A(1)^2+B(1)^2-C(1)^2>=0
theta2(1)=atan2(B(1),A(1))-atan2(C(1),sqrt(A(1)^2+B(1)^2-C(1)^2));
theta2(2)=atan2(B(1),A(1))-atan2(C(1),-sqrt(A(1)^2+B(1)^2-C(1)^2));
theta23(1)=atan2((B2(1)-a2*sin(theta2(1)))/a3,(B1(1)-a2*cos(theta2(1)))/a3);
theta23(2)=atan2((B2(1)-a2*sin(theta2(2)))/a3,(B1(1)-a2*cos(theta2(2)))/a3);
theta4(1)=theta234(1)-theta23(1);
theta4(2)=theta234(1)-theta23(2);
theta3(1)=theta23(1)-theta2(1);
theta3(2)=theta23(2)-theta2(2);
AllSloverTheta(k+1,:)=[theta1_1 theta2(1)-p2*pi/180 theta3(1) theta4(1)-p4*pi/180 theta5_1 theta6_1];
AllSloverTheta(k+2,:)=[theta1_1 theta2(2)-p2*pi/180 theta3(2) theta4(2)-p4*pi/180 theta5_1 theta6_1];
k=k+2;
end
end
if abs(S5_2)>1e-6
theta6_2=atan2((-sin(theta1_1)*ox+cos(theta1_1)*oy)/S5_2,(sin(theta1_1)*nx-cos(theta1_1)*ny)/S5_2);
S234(2)=-az/S5_2;C234(2)=-(cos(theta1_1)*ax+sin(theta1_1)*ay)/S5_2;theta234(2)=atan2(S234(2),C234(2));
B1(2)=cos(theta1_1)*px+sin(theta1_1)*py-d5*S234(2);B2(2)=pz-d1+d5*C234(2);
A(2)=-2*B2(2)*a2;B(2)=2*B1(2)*a2;C(2)=B1(2)^2+B2(2)^2+a2^2-a3^2;
if A(2)^2+B(2)^2-C(2)^2>=0
theta2(3)=atan2(B(2),A(2))-atan2(C(2),sqrt(A(2)^2+B(2)^2-C(2)^2));
theta2(4)=atan2(B(2),A(2))-atan2(C(2),-sqrt(A(2)^2+B(2)^2-C(2)^2));
theta23(3)=atan2((B2(2)-a2*sin(theta2(3)))/a3,(B1(2)-a2*cos(theta2(3)))/a3);
theta23(4)=atan2((B2(2)-a2*sin(theta2(4)))/a3,(B1(2)-a2*cos(theta2(4)))/a3);
theta4(3)=theta234(2)-theta23(3);
theta4(4)=theta234(2)-theta23(4);
theta3(3)=theta23(3)-theta2(3);
theta3(4)=theta23(4)-theta2(4);
AllSloverTheta(k+1,:)=[theta1_1 theta2(3)-p2*pi/180 theta3(3) theta4(3)-p4*pi/180 theta5_2 theta6_2];
AllSloverTheta(k+2,:)=[theta1_1 theta2(4)-p2*pi/180 theta3(4) theta4(4)-p4*pi/180 theta5_2 theta6_2];
k=k+2;
end
end
if abs(S5_3)>1e-6
theta6_3=atan2((-sin(theta1_2)*ox+cos(theta1_2)*oy)/S5_3,(sin(theta1_2)*nx-cos(theta1_2)*ny)/S5_3);
S234(3)=-az/S5_3;C234(3)=-(cos(theta1_2)*ax+sin(theta1_2)*ay)/S5_3;theta234(3)=atan2(S234(3),C234(3));
B1(3)=cos(theta1_2)*px+sin(theta1_2)*py-d5*S234(3);B2(3)=pz-d1+d5*C234(3);
A(3)=-2*B2(3)*a2;B(3)=2*B1(3)*a2;C(3)=B1(3)^2+B2(3)^2+a2^2-a3^2;
if A(3)^2+B(3)^2-C(3)^2>=0
theta2(5)=atan2(B(3),A(3))-atan2(C(3),sqrt(A(3)^2+B(3)^2-C(3)^2));
theta2(6)=atan2(B(3),A(3))-atan2(C(3),-sqrt(A(3)^2+B(3)^2-C(3)^2));
theta23(5)=atan2((B2(3)-a2*sin(theta2(5)))/a3,(B1(3)-a2*cos(theta2(5)))/a3);
theta23(6)=atan2((B2(3)-a2*sin(theta2(6)))/a3,(B1(3)-a2*cos(theta2(6)))/a3);
theta4(5)=theta234(3)-theta23(5);
theta4(6)=theta234(3)-theta23(6);
theta3(5)=theta23(5)-theta2(5);
theta3(6)=theta23(6)-theta2(6);
AllSloverTheta(k+1,:)=[theta1_2 theta2(5)-p2*pi/180 theta3(5) theta4(5)-p4*pi/180 theta5_3 theta6_3];
AllSloverTheta(k+2,:)=[theta1_2 theta2(6)-p2*pi/180 theta3(6) theta4(6)-p4*pi/180 theta5_3 theta6_3];
k=k+2;
end
end
if abs(S5_4)>1e-6
theta6_4=atan2((-sin(theta1_2)*ox+cos(theta1_2)*oy)/S5_4,(sin(theta1_2)*nx-cos(theta1_2)*ny)/S5_4);
S234(4)=-az/S5_4;C234(4)=-(cos(theta1_2)*ax+sin(theta1_2)*ay)/S5_4;theta234(4)=atan2(S234(4),C234(4));
B1(4)=cos(theta1_2)*px+sin(theta1_2)*py-d5*S234(4);B2(4)=pz-d1+d5*C234(4);
A(4)=-2*B2(4)*a2;B(4)=2*B1(4)*a2;C(4)=B1(4)^2+B2(4)^2+a2^2-a3^2;
if A(4)^2+B(4)^2-C(4)^2>=0
theta2(7)=atan2(B(4),A(4))-atan2(C(4),sqrt(A(4)^2+B(4)^2-C(4)^2));
theta2(8)=atan2(B(4),A(4))-atan2(C(4),-sqrt(A(4)^2+B(4)^2-C(4)^2));
theta23(7)=atan2((B2(4)-a2*sin(theta2(7)))/a3,(B1(4)-a2*cos(theta2(7)))/a3);
theta23(8)=atan2((B2(4)-a2*sin(theta2(8)))/a3,(B1(4)-a2*cos(theta2(8)))/a3);
theta4(7)=theta234(4)-theta23(7);
theta4(8)=theta234(4)-theta23(8);
theta3(7)=theta23(7)-theta2(7);
theta3(8)=theta23(8)-theta2(8);
AllSloverTheta(k+1,:)=[theta1_2 theta2(7)-p2*pi/180 theta3(7) theta4(7)-p4*pi/180 theta5_4 theta6_4];
AllSloverTheta(k+2,:)=[theta1_2 theta2(8)-p2*pi/180 theta3(8) theta4(8)-p4*pi/180 theta5_4 theta6_4];
k=k+2;
end
end
if k>0
AllSloverTheta=AllSloverTheta*180/pi;%将弧度转化成角度
for i=1:k
for j=1:6
if AllSloverTheta(i,j)<=-180
AllSloverTheta(i,j)=AllSloverTheta(i,j)+360;%将角度限定在-180—+180
elseif AllSloverTheta(i,j)>180
AllSloverTheta(i,j)=AllSloverTheta(i,j)-360;%将角度限定在-180—+180
end
end
end
else
disp('Singular position Unable to solve');
end
end
end限位
8.建立机器人
MDH_Verify.m
clear,clc,close all;
%% 建立机器人DH参数，初始状态为竖直状态
% d：连杆偏距 ； a：杆长 ；alpha：连杆转角 ； offset初始关节角度；
L1=Link('d',144,'a',0,'alpha',0,'modified');
L2=Link('d',0,'a',0,'alpha',pi/2,'offset',-pi/2,'modified');
L3=Link('d',0,'a',-264,'alpha',0,'modified');
L4=Link('d',106,'a',-236,'alpha',0,'offset',-pi/2,'modified');
L5=Link('d',114,'a',0,'alpha',pi/2,'modified');
L6=Link('d',67,'a',0,'alpha',-pi/2,'modified');
%建立机器人
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','Arm6')
% 初始角度 都为0
%Theta=[0 0 0 0 0 0];
Theta=[-30.0000 -58.2807 30.0000 -61.7193 -30.0000 -30.0000];
Theta=Theta/180*pi; %换算成弧度
forwarda=robot.fkine(Theta) %求正解的齐次变换矩阵
W=[-1000,+1000,-1000,+1000,-1000,+1000]; %限制坐标轴的范围
robot.plot(Theta,'tilesize',150,'workspace',W); %显示三维动画
robot.teach(forwarda,'rpy' ) %显示roll/pitch/yaw angles，GUI可调界面9. 验证正逆解的结果 利用机器人工具箱
ToolBoxVerify_MDH.m
clear,clc,close all;
%% 建立机器人DH参数，初始状态为竖直状态
% 连杆偏移d,连杆长度a,连杆扭转角alpha modified：改进dh参数来表示的
L1=Link('d',144,'a',0,'alpha',0,'modified');
L2=Link('d',0,'a',0,'alpha',pi/2,'offset',-pi/2,'modified');
L3=Link('d',0,'a',-264,'alpha',0,'modified');
L4=Link('d',106,'a',-236,'alpha',0,'offset',-pi/2,'modified');
L5=Link('d',114,'a',0,'alpha',pi/2,'modified');
L6=Link('d',67,'a',0,'alpha',-pi/2,'modified');
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','Arm6');
%% 正解，给定关节角，求末端位姿，样例
% Theta = [0 0 0 0 0 0]; %给定6个关节角度值
% Theta=[30 30 30 30 30 30];
% Theta=[-30 -30 -30 -30 -30 -30];
%%调用正解函数，后面不加分号表示会打印输出变量
% [T60,Pos]=ForwardSolver_MDH(Theta) % Pos=[X,Y,Z,Rall,Pitch,Yaw];
%% 逆解，给定末端位姿求关节角，(X,Y,Z单位为mm)，(Rall,Pitch,Yaw单位为deg)
Pos=[308.0304 -367.2397 524.1307 43.8979 -25.6589 56.3099]; %给定末端位姿
% Pos=[300 400 300 0 0 0];
AllSloverTheta=InverseSolver_MDH(Pos) %根据末端位姿求所有解
Theta=AllSloverTheta(1,1:end) %选择第一组解，PTP程序是默认为第一组解
%% 验证正逆解的结果
Theta=Theta/180*pi; %换算成弧度
forwarda=robot.fkine(Theta) %求正解的齐次变换矩阵
%位置
X=forwarda(1).t(1);
Y=forwarda(1).t(2);
Z=forwarda(1).t(3);
W=[-1000,+1000,-1000,+1000,-1000,+1000];%限制坐标轴的范围 w：限制plot工作空间
robot.plot(Theta,'tilesize',150,'workspace',W); %显示三维动画 w：限制工作空间 tilesize：限制地板方块的尺寸
q1=robot.ikine(forwarda)*180/pi %求逆解验证关节角 给齐次变换矩阵forwarda 反求角度值
rpy=tr2rpy(forwarda, 'xyz')*180/pi %求末端姿态，工具法兰为绕XYZ轴旋转；齐次变换矩阵反求rpy 'xyz'：绕XYZ轴的顺序进行旋转
robot.teach(forwarda,'rpy' ) %显示roll/pitch/yaw angles，GUI可调界面 调用教学模型来实时观看机械臂变化 'rpy'：rpy显示否则欧拉角显示命令行输入 ： format compact %紧凑显示输出
================================================================
利用三角函数知识，我们可以计算出它的旋转矩阵
每一列变换后的坐标系 XB YB ZB （向量）
-------------------------------------------------------------------------------------------------------------------------
Rz（30）变换矩阵
转过30度的向量，绿色坐标系：
XB YB ZB （向量）
-----------------------------------------------------------------------
红色坐标系：
XA XA XA （向量）
YA YA YA （向量）
ZA ZA ZA （向量）
分别红色变换矩阵的每列向量与转过角度的蓝色的向量之间的夹角余弦值。
即：
Rz（30）
XA 与XB 的夹角的余弦值 XA与YB 的夹角的余弦值 XA 与ZB 的夹角的余弦值
YA 与XB 的夹角的余弦值 YA 与YB 的夹角的余弦值 YA与ZB 的夹角的余弦值
ZA 与XB 的夹角的余弦值 ZA与YB 的夹角的余弦值 ZA与ZB 的夹角的余弦值
---------------------------------------------------------------------------------------------------------------------------------
Rz（30）
第一列
第一行 cos(30) = XB与XA的夹角30，
（需要计算XB的模在XA上的投影，即 cos(30)*XB，提取 cos(30) ）
第二行 -sin(30) = YB与XA的夹角,cos(90+30)=-sin(30)
第三行 0 = ZB与XA的夹角90=cos(90)=0
第二列
第一行 sin(30) = XB与YA的夹角cos(60)=sin(30)
第二行 cos(30)= YB与YA的夹角30
第三行 0 =ZB与YA的夹角90=cos(90)=0
第三列
第一行 0 = XB与ZA的夹角90,cos(90)=0
第二行 0 = YB与ZA的夹角90,cos(90)=0
第三行 1 = ZB与ZA的夹角0,cos(0)=1
------------------------------------------------------------------------------------------------------------------
XB*XA （两个向量相乘，等于 XB的模在XA上的投影与XA的乘积）
XB的模在XA上的投影=cos(30)XB (30度是XB与XA的夹角)
课程：
通过一个里程计算法项目方式讲解坐标变换算法原理
https://edu.youkuaiyun.com/course/detail/39299?spm=1001.2014.3001.5507
扫一扫
530
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
