硬件設計：

總體思路：

整個(gè)機架采用PCB板，將四個(gè)電機固定在PCB板的四個(gè)角，外接電池。

硬件模塊：

單片機、慣性測量模塊(IMU)、無(wú)線(xiàn)通訊模塊、電機驅動(dòng)模塊、續流二極管、電源管理模塊(穩壓與充放電)、直流有刷電機、大電流放電電池、遙控器。

硬件選型：

硬件工作綜述：

單片機負責整個(gè)系統的協(xié)調工作；慣性測量模塊(IMU)負責測量四旋翼的姿態(tài)；無(wú)線(xiàn)通訊模塊負責四旋翼與遙控器的通訊；電機驅動(dòng)模塊負責驅動(dòng)電機；續流二極管負責對電機進(jìn)行續流；電源管理模塊中的穩壓模塊負責整個(gè)系統的供電，電源管理模塊中的充放電模塊負責對電池充電；有刷電機負責提供四旋翼的飛行動(dòng)力；大電流放電電池負責四旋翼的能量來(lái)源；遙控器負責對四旋翼進(jìn)行遙控和控制。

硬件設計功能模塊圖：

實(shí)際效果圖與相關(guān)參數：

尺寸：對角電機軸距10x10cm

重量：33.2g(帶電池)

軟件設計：

總體思路：

慣性測量模塊(IMU)測量出當前飛機的三軸加速度與三軸角速度并傳送給單片機處理，由單片機進(jìn)行基于四元數的姿態(tài)解算，求解出當前飛機的pitch、roll、yaw三個(gè)角度值，然后根據這三個(gè)角度經(jīng)過(guò)PID控制運算，輸出四路PWM控制四個(gè)直流有刷電機的加減速從而達到飛機的平衡懸停。

其中，慣性測量模塊(IMU)的加速度計由于噪聲比較大，所以需要對其進(jìn)行濾波處理；而遙控器則是對飛機進(jìn)行實(shí)時(shí)的姿態(tài)控制；最后由于四旋翼制作的特殊性，在調試PID參數階段會(huì )頻繁的燒寫(xiě)程序，鑒于此，筆者開(kāi)發(fā)了基于NRF24L01的Bootloader技術(shù)，免除了燒寫(xiě)Flash的物理連線(xiàn)限制，可實(shí)現遠程程序一鍵下載。

姿態(tài)解算：

姿態(tài)解算屬于四旋翼制作的核心部分，如果姿態(tài)解算能夠實(shí)時(shí)的反應出飛機的狀態(tài)，那么對于控制來(lái)講就相對來(lái)說(shuō)比較容易了。而姿態(tài)結算所要做的事情就是兩個(gè)坐標系之間的正確轉化(地理坐標系與載體坐標系)，這種轉化有很多種表示方法，例如歐拉角法、方向余弦矩陣法、四元數法、旋轉矢量法等。筆者采用的是應用廣泛的四元數法，而旋轉矢量法則是一種基于四元數法的改進(jìn)四元數法。

四元數本是用于描述四維空間向量的一種方法，對于他的線(xiàn)性變換也就是在四維空間中的拉伸和旋轉，顯而易見(jiàn)，我們用四元數的向量乘法來(lái)表示三維空間中的旋轉是綽綽有余的。

通過(guò)慣性測量模塊(IMU)傳送過(guò)來(lái)的當前飛機的三軸加速度和三軸角速度的值，這樣一個(gè)三維的向量，轉化為四維向量，然后在四維空間中做線(xiàn)性變換(也可以說(shuō)在三維空間中旋轉)后輸出，利用四元數與歐拉角的關(guān)系(一定要注意旋轉順序)，將當前四元數轉換為歐拉角pitch、roll、yaw即得到當前飛機的姿態(tài)。

以下給出筆者姿態(tài)融合的代碼，該代碼網(wǎng)上都有，筆者在這里做了些許注釋?zhuān)奖憷斫狻?/p>

[cpp]view plaincopyprint?

2. voidIMUupdata(floatgx,floatgy,floatgz,floatax,floatay,floataz)
3. {
4. floatrecipNorm;//平方根的倒數
5. floathalfvx,halfvy,halfvz;//在當前載體坐標系中，重力分量在三個(gè)軸上的分量
6. floathalfex,halfey,halfez;//當前加速度計測得的重力加速度在三個(gè)軸上的分量與當前姿態(tài)在三個(gè)軸上的重力分量的誤差,這里采用差積的方式
7. floatqa,qb,qc;
9. gx=gx*PI/180;//轉換為弧度制
10. gy=gy*PI/180;
11. gz=gz*PI/180;
13. //如果加速度計處于自由落體狀態(tài)，可能會(huì )出現這種情況，不進(jìn)行姿態(tài)解算，因為會(huì )產(chǎn)生分母無(wú)窮大的情況
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. //將當前姿態(tài)的重力在三個(gè)軸上的分量分離出來(lái)
23. //就是方向余弦旋轉矩陣的第三列,注意是地理坐標系(n系)到載體坐標系(b系)的,不要弄反了.如果書(shū)上是b系到n系,轉置即可
24. //慣性測量器件測量的都是關(guān)于b系的值，為了方便，我們一般將b系轉換到n系進(jìn)行導航參數求解。但是這里并不需要這樣做，因為這里是對陀螺儀進(jì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. //計算由當前姿態(tài)的重力在三個(gè)軸上的分量與加速度計測得的重力在三個(gè)軸上的分量的差,這里采用三維空間的差積(向量積)方法求差
30. //計算公式由矩陣運算推導而來(lái)公式參見(jiàn)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. //積分求誤差,關(guān)于當前姿態(tài)分離出的重力分量與當前加速度計測得的重力分量的差值進(jìn)行積分消除誤差
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//不進(jìn)行積分運算，只進(jìn)行比例調節
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. //以下為四元數微分方程.將陀螺儀和四元數結合起來(lái)，是姿態(tài)更新的核心算子
59. //計算方法由矩陣運算推導而來(lái)
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. //單位化四元數,意義在于單位化四元數在空間旋轉時(shí)是不會(huì )拉伸的,僅有旋轉角度.這類(lèi)似與線(xiàn)性代數里面的正交變換
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,參見(jiàn).pdf一文,P24
90. //注意此時(shí)的轉換順序是1-2-3，即X-Y-Z。但是由于畫(huà)圖方便，作者這里做了一個(gè)轉換，即調換Z和X，所以順序沒(méi)變
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. }

注意其中的快速開(kāi)方函數來(lái)自維基百科，精度0.175%。并且注意輸入的陀螺儀必須是弧度制(這一點(diǎn)在進(jìn)入函數的時(shí)候已經(jīng)做了轉換)，否則姿態(tài)解算是錯誤的。

針對上述代碼我還想說(shuō)明一個(gè)筆者發(fā)現的問(wèn)題：有很多網(wǎng)友和許多外國的四軸代碼(CrazyFlie)在這個(gè)姿態(tài)解算的加速度計部分都做了零點(diǎn)校準處理。意思就是在開(kāi)機的時(shí)候讀取一定次數的加速度計的值，然后平均一下，得到一個(gè)初始狀態(tài)的偏移量，最后在輸出的時(shí)候加速度計減掉這個(gè)值，然后再給到姿態(tài)解算代碼部分。而筆者在剛開(kāi)始移植代碼的時(shí)候是沒(méi)有做這個(gè)零點(diǎn)校準處理的(當然，陀螺儀必須要做零點(diǎn)處理，因為陀螺儀的原理，必須要在靜止時(shí)輸出為0)，包括現在依舊沒(méi)有對加速度計做零點(diǎn)校準處理，仍然可以獲得較為實(shí)時(shí)的姿態(tài)。