磁力把

MPU6050获取⾓度理论推导（三）---9轴融合算法

再接着上⼀篇：

姿态⾓解算---互补滤波补充(融合磁⼒计)

前⾯介绍了互补滤波法的算法流程和程序实现，但是仅仅只是融合了三轴加速度计和三轴陀螺仪的数据解算出姿态。

由于机体⽔平时，加速度计⽆法测量绕Z轴的旋转量，即偏航⾓yaw,并且磁⼒计也同样⽆法测得z轴的旋转量。

所以，我们考虑使⽤加速度计和磁⼒计同时对陀螺仪进⾏校正。这次介绍如何融合磁⼒计的数据进⾏姿态解算。

在介绍算法之前，想再提⼀点⾃⼰的感想。很多新⼿⼊门姿态解算算法时，最先接触的就是这个Mahony的互补滤波算法，也相对简单⼀

些。有的⼈觉得数学推导很⿇烦，也不愿意花时间研究代码，懵懵懂懂地把程序复制粘贴过去就随便填了⼏个参数，然后就去看现象。其实

⼤家都知道这种⽅法是不对的吧，没出问题还好，⼀旦出了错就不知所措了。很多时候，飞控出了问题，不⼀定是控制的问题，有时候问题

出在姿态解算上（“⾎与泪”的教训，我当初就是在这个地⽅被坑了很久）。得到了姿态⾓后，先看看波形，看看数据怎么样，滤波器有没

有⽤。（不想⾃⼰写程序和调试的，请⽆视这些。）

这⾥再推荐⼀个很实⽤的⼯具：匿名上位机。功能很强⼤，可以⾃⼰写程序上传数据，在上位机上看波形，⼀切OK之后再进⾏下⼀步的⼯

作。

这是匿名上位机的界⾯的截图，可以到匿名科创的官⽹上下载：

好了，开始介绍算法部分吧！

还是按照这张框图得流程来介绍：

⾸先，假设磁⼒计测量得到的三轴磁场数据为。

为⽅便运算，像此前⼀样，将数据归⼀化之后再进⾏运算。得到载体坐标系b下的标准磁场数据，

其中。

为表⽰简便，将归⼀化之后的数据还是表⽰为。

将这组载体坐标系b下测得的数据旋转到导航坐标系n下，得到。

坐标系的旋转相当于乘以⼀个旋转矩阵。

使⽤以前推导得到的结论，其为四元数表⽰的旋转矩阵：

得到：

假定理想情况下，让导航坐标系n中x轴指向正北⽅向，并设这个新的导航坐标系下磁⼒计数据为。

那么根据⾼中物理知识分析可以知道，Z轴⽅向分量（即重⼒⽅向分量）相等，⽔平⽅向分量相等。则容易得到：

若取y轴指向正北⽅向，则：

取x轴或y轴朝向正北，按照⾃⼰情况定义。我在程序中取的是x轴，所以后⾯使⽤x轴朝正北⽅向的结果来推导。

将磁⼒线在导航坐标系n中的理想输出再次旋转到载体坐标系b中，得到在b系中的理想输出：

将理想输出和原始输出作叉积得到误差e(w)，将其作为补偿项送给陀螺仪进⾏校正：

这部分只是⽤来修正YAW轴的，还要加上加速度的校正补偿项，再送给陀螺仪：

附上代码：

#defineKp10.0f//proportionalgaingovernsrateofconvergencetoaccelerometer/magnetometer

#defineKi0.008f//integralgaingovernsrateofconvergenceofgyroscopebias

#definehalfT0.001f//halfthesampleperiod采样周期的⼀半

floatq0=1,q1=0,q2=0,q3=0;//quaternionelementsreprentingtheestimatedorientation

floatexInt=0,eyInt=0,ezInt=0;//scaledintegralerror

voidIMUupdate(floatgx,floatgy,floatgz,floatax,floatay,floataz,floatmx,floatmy,floatmz)

{

floatnorm;

floathx,hy,hz,bx,bz;

floatwx,wy,wz;

floatvx,vy,vz;

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;

if(mx*my*mz==0)

return;

norm=sqrt(ax*ax+ay*ay+az*az);//acc数据归⼀化

ax=ax/norm;

ay=ay/norm;

az=az/norm;

norm=sqrt(mx*mx+my*my+mz*mz);//mag数据归⼀化

mx=mx/norm;

my=my/norm;

mz=mz/norm;

//mx=0;

//my=0;

//mz=0;

hx=2*mx*(0.5-q2q2-q3q3)+2*my*(q1q2-q0q3)+2*mz*(q1q3+q0q2);

hy=2*mx*(q1q2+q0q3)+2*my*(0.5-q1q1-q3q3)+2*mz*(q2q3-q0q1);

hz=2*mx*(q1q3-q0q2)+2*my*(q2q3+q0q1)+2*mz*(0.5-q1q1-q2q2);

bx=sqrt((hx*hx)+(hy*hy));

bz=hz;

//estimateddirectionofgravityandflux(vandw)估计重⼒⽅向和流量/变迁

vx=2*(q1q3-q0q2);//四元素中xyz的表⽰

vy=2*(q0q1+q2q3);

vz=q0q0-q1q1-q2q2+q3q3;

wx=2*bx*(0.5-q2q2-q3q3)+2*bz*(q1q3-q0q2);

wy=2*bx*(q1q2-q0q3)+2*bz*(q0q1+q2q3);

wz=2*bx*(q0q2+q1q3)+2*bz*(0.5-q1q1-q2q2);

//errorissumofcrossproductbetweenreferencedirectionoffieldsanddirectionmeasuredbynsors

//ex=(ay*vz-az*vy);//向量外积在相减得到差分就是误差

//ey=(az*vx-ax*vz);

//ez=(ax*vy-ay*vx);

ex=(ay*vz-az*vy)+(my*wz-mz*wy);

ey=(az*vx-ax*vz)+(mz*wx-mx*wz);

ez=(ax*vy-ay*vx)+(mx*wy-my*wx);

exInt=exInt+ex*Ki;//对误差进⾏积分

eyInt=eyInt+ey*Ki;

ezInt=ezInt+ez*Ki;

//adjustedgyroscopemeasurements

gx=gx+Kp*ex+exInt;//将误差PI后补偿到陀螺仪，即补偿零点漂移

gy=gy+Kp*ey+eyInt;

gz=gz+Kp*ez+ezInt;//这⾥的gz由于没有观测者进⾏矫正会产⽣漂移，表现出来的就是积分⾃增或⾃减

//integratequaternionrateandnormali//四元素的微分⽅程

q0=q0+(-q1*gx-q2*gy-q3*gz)*halfT;

q1=q1+(q0*gx+q2*gz-q3*gy)*halfT;

q2=q2+(q0*gy-q1*gz+q3*gx)*halfT;

q3=q3+(q0*gz+q1*gy-q2*gx)*halfT;

//normaliquaternion

norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);

q0=q0/norm;

q1=q1/norm;

q2=q2/norm;

q3=q3/norm;

Q_ANGLE.z=atan2(2*q1*q2+2*q0*q3,-2*q2*q2-2*q3*q3+1)*57.3;//yaw

Q_ANGLE.y=asin(-2*q1*q3+2*q0*q2)*57.3;//pitch

Q_ANGLE.x=atan2(2*q2*q3+2*q0*q1,-2*q1*q1-2*q2*q2+1)*57.3;//roll

}

注意：这⾥这个函数我们直接调⽤，的时候需要把原始数据处理⼀下才能使⽤。⾓度

转成弧度/秒

#defineGyro_R0.0010653f//即数据乘以该参数

调⽤该函数的时候：

IMUupdate(floatgx*Gyro_R,floatgy*Gyro_R,floatgz*Gyro_R,

floatax,floatay,floataz,

floatmx,floatmy,floatmz);