|
|
别的啰啰嗦嗦的代码就不贴了，对于会做OSD的来说都是小菜
贴核心点的，姿态解算的微分方程
我这个是自己推导的简化方程，属于有误差的公式
而四元数则不存在理论误差的
我这个的计算精度跟姿态计算频率有关，频率越高，误差越小
跟其他的算法相比，特点是计算量小得多、便于和加速度计融合
实际试验表明，只要精确校准了传感器，全姿态误差是在可接受的范围内（测试视频见我另外一个贴，里面就是用这个方程做的，那个视频融合参数没调好，受加速度影响较大）
这两天在对比和四元数算法的差异
我这个方程的特点是，只要运动角度不太大（接近90度或连续滚转），它的往复累计误差是相当小的，不加修正都可以归零
而四元数则不然，虽然每次运动角度都不大，但反复运动若干次后，不可避免的计算误差就出来了（估计是三角函数和双精度数的误差反复乘积造成的，因为每次姿态解算，它的乘除次数太多了）
但四元数的特点是，经过校准传感器后，在全姿态或连续滚转情况下误差并不大
可以说各有千秋吧
有兴趣的可以看看，说不定会有所启发
=================================================================
double dd,xa,ya,za;
double gxx,gyy,gzz;
double w,dw1,dw2,th; //ω,dω1,dω2,θ
double cw,sw,sth; //cos(ω),sin(ω),sin(θ)
double sa;
//gxx,gyy,gzz:从上次计算到这次计算之间陀螺积分出来的角度值
//za：Z轴角度，z轴垂直时为90度，水平时为0度,-90~+90
//xa:横滚轴和水平面夹角，-90~90度
//ya:俯仰轴和水平面夹角，-90~90度
//absd()双精度数的取绝对值
//--------------------计算x角-----------------------------
//计算ωx引起的x角变化量
if( za<0 ) w=-gxx;
else w=gxx;
th=ya+PI2;
sw=sin(w);cw=cos(w);sth=sin(th);sa=sin(xa);
if( absd(sth)<=absd(sa) ) //sth=sa;
dw1=asin( sa*cw )-xa;
else dw1=asin( sw*sqrt(sth*sth-sa*sa)+sa*cw )-xa;
//计算ωz引起的x角变化量
if( ya<0 ) w=-gzz;
else w=gzz;
th=za+PI2;
sw=sin(w);cw=cos(w);sth=sin(th);
if( absd(sth)<=absd(sa) ) //sth=sa;
dw2=asin( sa*cw)-xa;
else dw2=asin( sw*sqrt(sth*sth-sa*sa)+sa*cw)-xa;
//计算x角的陀螺计算值
xa+=dw1+dw2;
//--------------------计算y角-----------------------------
//计算ωy引起的y角变化量
if( za<0 ) w=-gyy;
else w=gyy;
th=xa+PI2;
sw=sin(w);cw=cos(w);sth=sin(th);sa=sin(ya);
if( absd(sth)<absd(sa) ) //sth=sa;
dw1=asin( sa*cw)-ya;
else dw1=asin( sw*sqrt(sth*sth-sa*sa)+sa*cw)-ya;
//------------计算ωz引起的y角变化量--------------
if( xa<0 ) w=gzz;
else w=-gzz;
th=za+PI2;
sw=sin(w);cw=cos(w);sth=sin(th);
if( absd(sth)<absd(sa) ) //sth=sa;
dw2=asin( sa*cw)-ya;
else dw2=asin( sw*sqrt(sth*sth-sa*sa)+sa*cw)-ya;
//计算y角的陀螺计算值
ya+=dw1+dw2;
//--------------------计算z角-----------------------------
sw=sin(x_angle);sw*=sw;
cw=sin(y_angle);cw*=cw;
sth=sqrt(sw+cw); //sth=sqrt(sinx*sinx+siny*siny)
dd=asin(sth)+PI2;
if( dd>PI2 ) dd=PI-dd;
if( z_acc<0 ) dd*=-1;
za=dd;
和加速度融合是自己搞的一种正交滤波器
滤波器参数还在调整中，要不断试飞寻找合适参数，麻烦得很
估计近期会出测试视频 |
欢迎继续阅读楼主其他信息
|
/2 
发表于 2011-6-25 01:21
收藏
楼主
