硬件设计：

总体思路：

整个机架采用PCB板，将四个电机固定在PCB板的四个角，外接电池。

硬件模块：

单片机、惯性测量模块(IMU)、无线通讯模块、电机驱动模块、续流二极管、电源管理模块(稳压与充放电)、直流有刷电机、大电流放电电池、遥控器。

硬件选型：

硬件工作综述：

单片机负责整个系统的协调工作；惯性测量模块(IMU)负责测量四旋翼的姿态；无线通讯模块负责四旋翼与遥控器的通讯；电机驱动模块负责驱动电机；续流二极管负责对电机进行续流；电源管理模块中的稳压模块负责整个系统的供电，电源管理模块中的充放电模块负责对电池充电；有刷电机负责提供四旋翼的飞行动力；大电流放电电池负责四旋翼的能量来源；遥控器负责对四旋翼进行遥控和控制。

硬件设计功能模块图：

实际效果图与相关参数：

尺寸：对角电机轴距10x10cm

重量：33.2g(带电池)

软件设计：

总体思路：

惯性测量模块(IMU)测量出当前飞机的三轴加速度与三轴角速度并传送给单片机处理，由单片机进行基于四元数的姿态解算，求解出当前飞机的pitch、roll、yaw三个角度值，然后根据这三个角度经过PID控制运算，输出四路PWM控制四个直流有刷电机的加减速从而达到飞机的平衡悬停。

其中，惯性测量模块(IMU)的加速度计由于噪声比较大，所以需要对其进行滤波处理；而遥控器则是对飞机进行实时的姿态控制；最后由于四旋翼制作的特殊性，在调试PID参数阶段会频繁的烧写程序，鉴于此，笔者开发了基于NRF24L01的Bootloader技术，免除了烧写Flash的物理连线限制，可实现远程程序一键下载。

姿态解算：

姿态解算属于四旋翼制作的核心部分，如果姿态解算能够实时的反应出飞机的状态，那么对于控制来讲就相对来说比较容易了。而姿态结算所要做的事情就是两个坐标系之间的正确转化(地理坐标系与载体坐标系)，这种转化有很多种表示方法，例如欧拉角法、方向余弦矩阵法、四元数法、旋转矢量法等。笔者采用的是应用广泛的四元数法，而旋转矢量法则是一种基于四元数法的改进四元数法。

四元数本是用于描述四维空间向量的一种方法，对于他的线性变换也就是在四维空间中的拉伸和旋转，显而易见，我们用四元数的向量乘法来表示三维空间中的旋转是绰绰有余的。

通过惯性测量模块(IMU)传送过来的当前飞机的三轴加速度和三轴角速度的值，这样一个三维的向量，转化为四维向量，然后在四维空间中做线性变换(也可以说在三维空间中旋转)后输出，利用四元数与欧拉角的关系(一定要注意旋转顺序)，将当前四元数转换为欧拉角pitch、roll、yaw即得到当前飞机的姿态。

以下给出笔者姿态融合的代码，该代码网上都有，笔者在这里做了些许注释，方便理解。

[cpp]view plaincopyprint?

2. voidIMUupdata(floatgx,floatgy,floatgz,floatax,floatay,floataz)
3. {
4. floatrecipNorm;//平方根的倒数
5. floathalfvx,halfvy,halfvz;//在当前载体坐标系中，重力分量在三个轴上的分量
6. floathalfex,halfey,halfez;//当前加速度计测得的重力加速度在三个轴上的分量与当前姿态在三个轴上的重力分量的误差,这里采用差积的方式
7. floatqa,qb,qc;
9. gx=gx*PI/180;//转换为弧度制
10. gy=gy*PI/180;
11. gz=gz*PI/180;
13. //如果加速度计处于自由落体状态，可能会出现这种情况，不进行姿态解算，因为会产生分母无穷大的情况
14. if(!((ax==0.0f)&&(ay==0.0f)&&(az==0.0f)))
15. {
16. //单位化加速度计,意义在于在变更了加速度计的量程之后不需要修改Kp参数,因为这里归一化了
17. recipNorm=invSqrt(ax*ax+ay*ay+az*az);
18. ax*=recipNorm;
19. ay*=recipNorm;
20. az*=recipNorm;
22. //将当前姿态的重力在三个轴上的分量分离出来
23. //就是方向余弦旋转矩阵的第三列,注意是地理坐标系(n系)到载体坐标系(b系)的,不要弄反了.如果书上是b系到n系,转置即可
24. //惯性测量器件测量的都是关于b系的值，为了方便，我们一般将b系转换到n系进行导航参数求解。但是这里并不需要这样做，因为这里是对陀螺仪进行补偿
25. halfvx=g_q1*g_q3-g_q0*g_q2;
26. halfvy=g_q0*g_q1+g_q2*g_q3;
27. halfvz=g_q0*g_q0-0.5f+g_q3*g_q3;
29. //计算由当前姿态的重力在三个轴上的分量与加速度计测得的重力在三个轴上的分量的差,这里采用三维空间的差积(向量积)方法求差
30. //计算公式由矩阵运算推导而来公式参见http://en.wikipedia.org/wiki/Cross_product中的Mnemonic部分
31. halfex=(ay*halfvz-az*halfvy);
32. halfey=(az*halfvx-ax*halfvz);
33. halfez=(ax*halfvy-ay*halfvx);
35. //积分求误差,关于当前姿态分离出的重力分量与当前加速度计测得的重力分量的差值进行积分消除误差
36. if(g_twoKi>0.0f)
37. {
38. g_integralFBx+=g_twoKi*halfex*CNTLCYCLE;//Ki积分
39. g_integralFBy+=g_twoKi*halfey*CNTLCYCLE;
40. g_integralFBz+=g_twoKi*halfez*CNTLCYCLE;
41. gx+=g_integralFBx;//将积分误差反馈到陀螺仪上，修正陀螺仪的值
42. gy+=g_integralFBy;
43. gz+=g_integralFBz;
44. }
45. else//不进行积分运算，只进行比例调节
46. {
47. g_integralFBx=0.0f;
48. g_integralFBy=0.0f;
49. g_integralFBz=0.0f;
50. }
52. //直接应用比例调节，修正陀螺仪的值
53. gx+=g_twoKp*halfex;
54. gy+=g_twoKp*halfey;
55. gz+=g_twoKp*halfez;
56. }
58. //以下为四元数微分方程.将陀螺仪和四元数结合起来，是姿态更新的核心算子
59. //计算方法由矩阵运算推导而来
60. //.1
61. //q=-*qxOmega式中左边是四元数的倒数,右边的x是四元数乘法,Omega是陀螺仪的值(即角速度)
62. //2
63. //.
64. //[q0][0-wx-wy-wz][q0]
65. //.
66. //[q1][wx0wz-wy][q1]
67. //.=*
68. //[q2][wy-wz0wx][q2]
69. //.
70. //[q3][wzwy-wx0][q3]
71. gx*=(0.5f*CNTLCYCLE);
72. gy*=(0.5f*CNTLCYCLE);
73. gz*=(0.5f*CNTLCYCLE);
74. qa=g_q0;
75. qb=g_q1;
76. qc=g_q2;
77. g_q0+=(-qb*gx-qc*gy-g_q3*gz);
78. g_q1+=(qa*gx+qc*gz-g_q3*gy);
79. g_q2+=(qa*gy-qb*gz+g_q3*gx);
80. g_q3+=(qa*gz+qb*gy-qc*gx);
82. //单位化四元数,意义在于单位化四元数在空间旋转时是不会拉伸的,仅有旋转角度.这类似与线性代数里面的正交变换
83. recipNorm=invSqrt(g_q0*g_q0+g_q1*g_q1+g_q2*g_q2+g_q3*g_q3);
84. g_q0*=recipNorm;
85. g_q1*=recipNorm;
86. g_q2*=recipNorm;
87. g_q3*=recipNorm;
89. //四元数到欧拉角转换，转换顺序为Z-Y-X,参见.pdf一文,P24
90. //注意此时的转换顺序是1-2-3，即X-Y-Z。但是由于画图方便，作者这里做了一个转换，即调换Z和X，所以顺序没变
91. g_Yaw=atan2(2*g_q1*g_q2+2*g_q0*g_q3,g_q1*g_q1+g_q0*g_q0-g_q3*g_q3-g_q2*g_q2)*180/PI;//Yaw
92. g_Roll=asin(-2*g_q1*g_q3+2*g_q0*g_q2)*180/PI;//Roll
93. g_Pitch=atan2(2*g_q2*g_q3+2*g_q0*g_q1,-2*g_q1*g_q1-2*g_q2*g_q2+1)*180/PI;//Pitch
94. }

注意其中的快速开方函数来自维基百科，精度0.175%。并且注意输入的陀螺仪必须是弧度制(这一点在进入函数的时候已经做了转换)，否则姿态解算是错误的。

针对上述代码我还想说明一个笔者发现的问题：有很多网友和许多外国的四轴代码(CrazyFlie)在这个姿态解算的加速度计部分都做了零点校准处理。意思就是在开机的时候读取一定次数的加速度计的值，然后平均一下，得到一个初始状态的偏移量，最后在输出的时候加速度计减掉这个值，然后再给到姿态解算代码部分。而笔者在刚开始移植代码的时候是没有做这个零点校准处理的(当然，陀螺仪必须要做零点处理，因为陀螺仪的原理，必须要在静止时输出为0)，包括现在依旧没有对加速度计做零点校准处理，仍然可以获得较为实时的姿态。