图10 四旋翼飞行器全姿态数据融合示意图
图中的符号解释：
Bg是加速度计测得的值，扩充为四元数，Bg=[0，Bgx，Bgy，Bgz]，上标B代表是在体坐标系(Body Frame)下的测量值
Bh是磁传感器计测得的值，扩充为四元数，Bh=[0，Bhx，Bhy，Bhz] Eh是参考坐标系下的固有磁场，地磁场，上标E代表是在参考坐标系(earth Frame)下的值
Eg是参考坐标系下的固有加速度，重力加速度 p，q，r是陀螺仪测得的角速度 a，b，c，d是姿态四元数Q 下图是一个全姿态互补滤波的流程：
第 11 页 共 11 页
图11 全姿态互补滤波过程
加速度计经过高斯牛顿迭代得到姿态误差速率，陀螺仪直接通过四元数微分方程得到姿态四元数速率，两个加起来积分得到姿态四元数，然后再有四元数变为欧拉角，从而对飞行器做出相应的控制。
3 电路与程序设计
3.1系统总体框图
图12 系统总体框图
本系统采用瑞萨R5F100LE作为主飞控板，通过读取各种传感器的值来控制飞行器的飞行姿态以及导航。
3.2主控芯片最小系统电路图
第 12 页 共 12 页
图13 飞控板最小系统电路图 3.3陀螺仪电路图
图14 陀螺仪电路图
程序中通过模拟I2C来读取陀螺仪的值，而由于陀螺仪会零飘，所以隔一段时间要对其进行校准。
第 13 页 共 13 页
3.4超声波电路图
图15 超声波传感器电路图
给超声波一个起始高电平后，通过定时器捕获超声波输出高电平的时间，然后乘以超声波的传输速度，即可得到高度h。 3.5 线性CCD电路图
图16 线性CCD电路图
线性CCD输出的是一个模拟量电压值，通过线性CCD输出电压值的不同从而判断是否偏离了黑线，改变滑动变阻器R2的阻值，即可改变CCD的最大输出电压值，改变LM358运放R1的阻值，即可改变CCD的曝光时间。
3.6程序流程图
第 14 页 共 14 页
图17 程序流程图
4 测试方案与测试结果
4.1测试仪器与测试方案 4.1.1测试仪器：
SS1792型可跟踪直流稳定电源，MS8260D数字万用表，DS1062E-EDU数字示波器，米尺，坐标纸。
4.1.2主要测试方案与测试结果
1、对于基本要求，分别将四旋翼飞行器放在AB区，一键启动后飞行，分别用秒表测出飞行时间，并用米尺测量偏离降落区的距离，如下表1，表2。 表1 从A区飞到B区的数据
第 15 页 共 15 页