四旋翼无人机设计与制作
?accx?ax??accx2?accy2?accz2?accy? ?ay? (4-19)
222accy?accy?accz??accz?a?222?zacc?acc?accyyz?(4)将地理坐标系的重力向量转换到机体坐标系可得三轴的重力量vx，vy，vz：
22222(q1q2?q0q3)?vx??q0?q1?q2?q3?v???2(qq?qq)q2?q2?q2?q212030123?y???2(q2q3?q0q1)?vz????2(q1q3?q0q2)
?2(q1q3?q0q2)????2(qq?qq)2301??2222??q0?q1?q2?q3??2(q1q3?q0q2)??0???2(q2q3?q0q1)???0??2222?q0?q1?q2?q3???1?? (4-20)
(5)将地理坐标系转换到载体坐标系下的重力向量和载体坐标系测量的加速度向量外积，得到两坐标系的误差[33]：
?ex??ax??vx??ayvz?azvy??e???a???v???av?av? (4-21)
xz??y??y??y??zx???ez????az????vz????axvy?ayvx?(6)陀螺仪误差是导致机体坐标系误差的根本原因，因此用两坐标系误差的PI来补偿陀螺仪使得机体坐标系更加准确。
??xin???x??et??????kp?e? ?yint???y?????t??zin????z???e??e?x??ki?ey?????e?z???x??y (4-22) ?z?其中：kp和ki是调整参数，在实际调试中确定。其中ki可以等于0，kp可以以0为初始值，0.01步进调节。 (7)四元数姿态更新方程[34]。
四元数微分方程为：
?=1Qω (4-23) Q2其中Q?q0?q1ib?q2jb?q3kb，ω?0??xib??yjb??zkb 将上式写成矩阵形式
–25–
四旋翼无人机设计与制作
?0??0?q?q?x?1?1????? ?q?2?2??y????q??3???z??x0??z??y?z0??x?y??z??q0??q???y???1? (4-24) ?x??q2????0???q3?对四元数一阶微分方程进行一阶毕卡算法可得：
?t?q?q?(?q??q??q?)01xint2yint3zint?02??q?q?(q??q??q?)?t10xint2zint3xint?12 (4-25) ??q?q?(q??q??q?)?t20yint1zint3xint?22??t?q3?q3?(q0?zint?q1?yint?q2?xint)?2(8)对四元数进行规范化处理，于是得下式4-21：
??q0????q1??? ??q??2???q3???q0q02?q12?q22?q32q1q02?q12?q22?q32 (4-26)
q2q02?q12?q22?q32q3q02?q12?q22?q32(9)以上得到的新的四元数代表完成了一次四元数的运算，将此四元数回到开头，将旧的四元数更新为新四元数，作为下一次四元数运算的初始数，再从(1)开始下一次的四元数运算[35]。与此同时将新的四元数更新规范化后转化成三个欧拉角得下式(4-22)，完成了姿态的初步运算[36]：
?????arcsin(2(q2q3?q0q1))?2(qq?qq) ???arctan(?21232022) (4-27)
q0?q1?q2?q3??2(qq?qq)???arctan(?21222032)q0?q1?q2?q3?–26–
四旋翼无人机设计与制作
4.6 校准载体航向角
通过三轴数字电子罗盘可以校准陀螺仪积分获得的航偏角以消除累计误差。如果电子设备干扰强烈的情况下需要暂停数字罗盘的数据融合[37]，因为这样测到的数据误差非常的大。对地磁传感器常用的校准方法之一是平面校准法[38]：使用数字罗盘前，对其进行初始化，然后进行磁场校准，将传感器水平放置并旋转一周得到新的圆周圆心位置((xmax?xmin)/2,(ymax?ymin)/2) [39]。这是对水平的xy平面的校准，校准前后对比如图4.4所示。
图 4.4 xy平面校准前后对比
同理需要对竖直平面xz进行校准，校准前后的对比图，如图4.5所示。
图 4.5 xz平面校准前后对比
bHb???HxbHyRHzb?在导航坐标系R系各轴的分量HR???，?HxTRHyT
当载体静止时，设地球磁场强度H在载体坐标系b系各个轴的分量为
HzR?根据 ?，
HR?CbRHb可得：
–27–
四旋翼无人机设计与制作
?HxR??cos??R?? ?Hy???0?HzR?????sin?sin?sin?cos??sin?cos?b??cos?sin???Hx??Hb? (4-28) sin???y?b?cos?cos??Hz????R
将4-22式求得的?、?代入上式，即可求出HxR和Hy。航向角?可由式4-24得
出：
??arctaHRHnyR x–28–
(4-29)