|
|
发表于 2011-11-26 13:35:50
|
显示全部楼层
其实四元数就是旋转矢量法的单子样形式，多子样旋转矢量法比四元数计算更精确（这里不是说传感器的误差，而是算法本身误差）
但是我觉得，MEMS的陀螺仪，用四元数就够了，本身误差就很大了。
我不是很懂，MEMS输出的是角速度，而多子样对应的应该是角增量，如果要用多子样四元数，那么还要对时间积分之后，才是角增量。如此在MEMS上的意义是不是真的那么大？
对于四元数看《惯性导航》里的推导过程，结合上面5imx的帖子会比较好理解。我的算法没有用多子样，我觉得不是很有必要。
网上的关于三子样四元数的帖子。
科创论坛有
http://bbs.kechuang.org/read-kc-tid-36679-page-e.html
本板块老帖178楼也有
http://www.ourdev.cn/bbs/bbs_content.jsp?bbs_sn=1148130&bbs_page_no=1&bbs_id=1025
另外这个是原本的四元数。
http://bbs.5imx.com/bbs/viewthread.php?tid=504301&highlight=%CB%C4%D4%AA%CA%FD
以下是科创版的三子样四元数，这个写得比较清晰。
void init_Q(void)
{////////pitch roll yaw
q0=cos(roll/2)*cos(pitch/2)*cos(yaw/2)+sin(roll/2)*sin(yaw/2)*sin(yaw/2);
}
void angle() //姿态矩阵
{
float DX,DX1,DX2,DX3,DY,DY1,DY2,DY3,DZ,DZ1,DZ2,DZ3;
float model,h0,h1,h2,h3; //h表示增量四元数
float q0,q1,q2,q3; //q表示更新前的四元数
float model_d,d0,d1,d2,d3; //d表示更新后的四元数
float T11,T12,T13,T21,T22,T23,T31,T32,T33;
//三子样采样优化 D表示角增量
DX=DX1+DX2+DX3+(9/20)*(DY1*DZ3+DZ1*DY3)+(27/40)*(DY2*(DZ3-DZ1)+DX2*(DX3-DX1));
DY=DY1+DY2+DY3+(9/20)*(DY1*DY3+DX1*DZ3)+(27/40)*(DZ2*(DX3-DX1)+DY2*(DY3-DY1));
DZ=DZ1+DZ2+DZ3+(9/20)*(DZ1*DZ3+DY1*DX3)+(27/40)*(DX2*(DY3-DY1)+DY2*(DZ3-DZ1));
//计算增量四元数h
model=sqrt(DX*DX+DY*DY+DZ*DZ);
if(model==0)
{
h0=1;
h1=0;
h2=0;
h3=0;
}
else
{
h0=cos(model/2);
h1=(DX/model)*sin(model/2);
h2=(DY/model)*sin(model/2);
h3=(DZ/model)*sin(model/2);
}
//四元数更新
d0=q0*h0-q1*h1-q2*h2-q3*h3;
///////////////////////////
d1=q1*h0+q0*h1-q3*h2+q2*h3;
d2=q2*h0+q3*h1+q0*h2-q1*h3;
d3=q3*h0-q2*h1+q1*h2+q0*h3;
//进行规范化
model_d=sqrt(d0*d0+d1*d1+d2*d2+d3*d3);
d0=d0/model_d;
d1=d1/model_d;
d2=d2/model_d;
d3=d3/model_d;
//将更新和规范化之后的四元数d赋给q,以便下一次的更新
q0=d0;
q1=d1;
q2=d2;
q3=d3;
//将四元数转化成方向余弦矩阵
T11=q0*q0+q1*q1-q2*q2-q3*q3;
T12=2*(q1*q2-q0*q3);
T13=2*(q1*q3+q0*q2);
T21=2*(q1*q2+q0*q3);
T22=q0*q0-q1*q1+q2*q2-q3*q3;
T23=2*(q2*q3-q0*q1);
T31=2*(q1*q3-q0*q2);
T32=2*(q2*q3+q0*q1);
T33=q0*q0-q1*q1-q2*q2+q3*q3;
//提取姿态角
pitch=asin(T32);
if(T22==0&&T12>0)
yaw=pi/2;
if(T22==0&&T12<0)
yaw=-pi/2;
if(T22>0&&T12>0)
yaw=atan(-T31/T33);
if(T22>0&&T12<0)
yaw=atan(-T31/T33);
if(T22<0&&T12>0)
yaw=atan(-T31/T33)+2*pi;
if(T22<0&&T12<0)
yaw=atan(-T31/T33)-2*pi;
} |
|
楼主
