新手四旋翼算法总结.doc

(匿名版程序)首先,程序中一般用了两种求解姿态的方法,一种为欧拉角法,一种为四元数法(1)欧拉角法静止状态,或者总加速度只是稍微大于g时,由加计算出的值比较准确。使用欧拉角表示姿态,令Φ,θ和Φ代表ZYX欧拉角,分别称为偏航角、俯仰角和横滚角。载体坐标系下的加速度(axB,ayB,azB)和参考坐标系下的加速度(axN,ayN,azN)之间的关系可表示为(1)。其中c和s分别代表cos和sin。axB,ayB,azB就是mpu读出来的三个值。这个矩阵就是三个旋转矩阵相乘得到的,因为矩阵的乘法可以表示旋转。(1)飞行器处于静止状态,此时参考系下的加速度等于重力加速度,即(2)把(2)代入(1)可以解的(3)(4)即为初始俯仰角和横滚角,通过加速度计得到载体坐标系下的加速度即可将其解出,偏航角可以通过电子罗盘求出。(2)四元数法(通过处理单位采样时间内的角增量(mpu的陀螺仪得到的就是角增量),为了避免噪声的微分放大,应该直接用角增量-------抄的书)上匿名的程序voidIMUupdate(floatgx,floatgy,floatgz,floatax,floatay,floataz){floatnorm;//floathx,hy,hz,bx,bz;floatvx,vy,vz;//wx,wy,wz;floatex,ey,ez;//先把这些用得到的值算好floatq0q0=q0*q0;floatq0q1=q0*q1;floatq0q2=q0*q2;//floatq0q3=q0*q3;floatq1q1=q1*q1;//floatq1q2=q1*q2;floatq1q3=q1*q3;floatq2q2=q2*q2;floatq2q3=q2*q3;floatq3q3=q3*q3; if(ax*ay*az==0) return; norm=sqrt(ax*ax+ay*ay+az*az);//acc数据归一化ax=ax/norm;ay=ay/norm;az=az/norm;//estimateddirectionofgravityandflux(vandw)vx=2*(q1q3-q0q2); //四元素中xyz的vy=2*(q0q1+q2q3);vz=q0q0-q1q1-q2q2+q3q3;//errorissumofcrossproductbetweenreferencedirectionoffieldsanddirectionmeasuredbysensorsex=(ay*vz-az*vy); //向量外积在相减得到差分就是误差ey=(az*vx-ax*vz);ez=(ax*vy-ay*vx);exInt=exInt+ex*Ki; //对误差进行积分eyInt=eyInt+ey*Ki;ezInt=ezInt+ez*Ki;//adjustedgyroscopemeasurementsgx=gx+Kp*ex+exInt; //将误差PI后补偿到陀螺仪,即补偿零点漂移gy=gy+Kp*ey+eyInt;gz=gz+Kp*ez+ezInt; //这里的gz由于没有观测者进行矫正会产生漂移,表现出来的就是积分自增或自减//integratequaternionrateandnormalise //四元素的微分方程q0=q0+(-q1*gx-q2*gy-q