1、欧拉插值
把旋转矩阵R表示成欧拉角,那么每个时刻的位姿就可以表示成三个角度,
我们每两个节点之间对三个角度值分别进行插值,我们看起来就是每两个节点剑斗士绕三个轴依次旋转,每次旋转是按照其中一个角度值的插值,这也是他的缺点就是看起来比较繁琐,冗余的运动成分大,经济性很差
2、RPY插值
rpy插值和欧拉插值的原理在我心里是一样的
%%
%欧拉插值和RPY插值
rpy0 = [0,0,0];
rpy1 = [0,0,70];
rpy2 = [0,60,70];
rpy01 = mtraj(@tpoly, rpy0, rpy1, 50);
tranimate(rpy2tr(rpy01));
rpy02 = mtraj(@tpoly, rpy0, rpy2, 50);
tranimate(rpy2tr(rpy02));
3.四元数插值
四元数姿态插值的原理在上一篇bolg里写到了,我看了一下matlab里边ctraj
(笛卡尔空间轨迹规划)用的插值方法就是四元数插值
打开matlab,在命令行输入
open trinterp
可以打开笛卡尔空间的姿态插值函数,基本思路如下
输入T0,T1得到四元数q0,q1
然后用四元数的插值方法得到其中qs,然后用qs算出Ts,输出
function T = trinterp(A, B, C)
if nargin == 3
% TRINTERP(T0, T1, s)
T0 = A; T1 = B; s = C(:)';
if length(s) == 1 && s > 1 && (s == floor(s))
% TRINTERP(T0, T1, M)
s = linspace(0, 1, s);
end
assert(all(s>=0 & s<=1), 'SMTB:trinterp:badarg', 'values of S outside interval [0,1]');
q0 = UnitQuaternion(T0);
q1 = UnitQuaternion(T1);
p0 = transl(T0);
p1 = transl(T1);
for i=1:length(s)
qr = q0.interp(q1, s(i));
pr = p0*(1-s(i)) + s(i)*p1;
T(:,:,i) = rt2tr(qr.R, pr);
end
elseif nargin == 2
% TRINTERP(T, s)
T0 = A; s = B(:)';
if length(s) == 1 && s > 1 && (s == floor(s))
% TRINTERP(T0, T1, M)
s = linspace(0, 1, s);
elseif any(s<0 | s>1)
error('SMTB:trinterp:badarg', 'values of S outside interval [0,1]');
end
q0 = UnitQuaternion(T0);
p0 = transl(T0);
for i=1:length(s)
qr = q0.interp(s(i));
pr = s(i)*p0;
T(:,:,i) = rt2tr(qr.R, pr);
end
else
error('SMTB:trinterp:badarg', 'must be 2 or 3 arguments');
end