姿态插值

clc;clear;close;

R0=angle2dcm(0/180*pi,0/180*pi,0/180*pi,'XYZ');
%%R1=angle2dcm(rpy(3,size(rpy,2))/180*pi,rpy(2,size(rpy,2))/180*pi,rpy(1,size(rpy,2))/180*pi,'XYZ');
R1=[0 1 0;0 0 1;1 0 0];

P0=[0,0,0];
P1=[5,5,5];

R=R0'*R1

qmax=acos((R(1,1)+R(2,2)+R(3,3)-1)/2);
qt=qmax;
w=(1/(2*sin(qt)))*[R(3,2)-R(2,3);R(1,3)-R(3,1);R(2,1)-R(1,2)];

Rt=[w(1)^2*(1-cos(qt))+cos(qt) w(1)*w(2)*(1-cos(qt))-w(3)*sin(qt) w(1)*w(3)*(1-cos(qt))+w(2)*sin(qt);w(1)*w(2)*(1-cos(qt))+w(3)*sin(qt) w(2)^2*(1-cos(qt))+cos(qt) w(2)*w(3)*(1-cos(qt))-w(1)*sin(qt);w(1)*w(3)*(1-cos(qt))-w(2)*sin(qt) w(2)*w(3)*(1-cos(qt))+w(1)*sin(qt) w(3)^2*(1-cos(qt))+cos(qt)];

for u=0:0.2:1

qt=qmax*u;
Pt=P0+(P1-P0)*u;
Rt=[w(1)^2*(1-cos(qt))+cos(qt) w(1)*w(2)*(1-cos(qt))-w(3)*sin(qt) w(1)*w(3)*(1-cos(qt))+w(2)*sin(qt);w(1)*w(2)*(1-cos(qt))+w(3)*sin(qt) w(2)^2*(1-cos(qt))+cos(qt) w(2)*w(3)*(1-cos(qt))-w(1)*sin(qt);w(1)*w(3)*(1-cos(qt))-w(2)*sin(qt) w(2)*w(3)*(1-cos(qt))+w(1)*sin(qt) w(3)^2*(1-cos(qt))+cos(qt)];

Rv=Rt*R0

T=[Rv,Pt'];
T=[T;0 0 0 1];

[ px,py,pz ] = PaintCoor( T );

end

 

 

function [ px,py,pz ] = PaintCoor( T )
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here

px=[T(1,1);T(2,1);T(3,1)];
py=[T(1,2);T(2,2);T(3,2)];
pz=[T(1,3);T(2,3);T(3,3)];

o=[T(1,4);T(2,4);T(3,4)];

hold on

quiver3(o(1),o(2),o(3),px(1),px(2),px(3),1,'b'); 
quiver3(o(1),o(2),o(3),py(1),py(2),py(3),1,'r'); 
quiver3(o(1),o(2),o(3),pz(1),pz(2),pz(3),1,'y'); 

end