硬件設(shè)計：

總體思路：

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

硬件模塊：

單片機、慣性測量模塊(IMU)、無線通訊模塊、電機驅(qū)動模塊、續(xù)流二極管、電源管理模塊(穩(wěn)壓與充放電)、直流有刷電機、大電流放電電池、遙控器。

硬件選型：

硬件工作綜述：

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

硬件設(shè)計功能模塊圖：

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

尺寸：對角電機軸距10x10cm

重量：33.2g(帶電池)

軟件設(shè)計：

總體思路：

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

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

姿態(tài)解算：

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

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

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

以下給出筆者姿態(tài)融合的代碼，該代碼網(wǎng)上都有，筆者在這里做了些許注釋，方便理解。

[cpp]view plaincopyprint?

2. voidIMUupdata(floatgx,floatgy,floatgz,floatax,floatay,floataz)
3. {
4. floatrecipNorm;//平方根的倒數(shù)
5. floathalfvx,halfvy,halfvz;//在當前載體坐標系中，重力分量在三個軸上的分量
6. floathalfex,halfey,halfez;//當前加速度計測得的重力加速度在三個軸上的分量與當前姿態(tài)在三個軸上的重力分量的誤差,這里采用差積的方式
7. floatqa,qb,qc;
9. gx=gx*PI/180;//轉(zhuǎn)換為弧度制
10. gy=gy*PI/180;
11. gz=gz*PI/180;
13. //如果加速度計處于自由落體狀態(tài)，可能會出現(xiàn)這種情況，不進行姿態(tài)解算，因為會產(chǎn)生分母無窮大的情況
14. if(!((ax==0.0f)&&(ay==0.0f)&&(az==0.0f)))
15. {
16. //單位化加速度計,意義在于在變更了加速度計的量程之后不需要修改Kp參數(shù),因為這里歸一化了
17. recipNorm=invSqrt(ax*ax+ay*ay+az*az);
18. ax*=recipNorm;
19. ay*=recipNorm;
20. az*=recipNorm;
22. //將當前姿態(tài)的重力在三個軸上的分量分離出來
23. //就是方向余弦旋轉(zhuǎn)矩陣的第三列,注意是地理坐標系(n系)到載體坐標系(b系)的,不要弄反了.如果書上是b系到n系,轉(zhuǎn)置即可
24. //慣性測量器件測量的都是關(guān)于b系的值，為了方便，我們一般將b系轉(zhuǎn)換到n系進行導(dǎo)航參數(shù)求解。但是這里并不需要這樣做，因為這里是對陀螺儀進行補償
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)的重力在三個軸上的分量與加速度計測得的重力在三個軸上的分量的差,這里采用三維空間的差積(向量積)方法求差
30. //計算公式由矩陣運算推導(dǎo)而來公式參見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)分離出的重力分量與當前加速度計測得的重力分量的差值進行積分消除誤差
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//不進行積分運算，只進行比例調(diào)節(jié)
46. {
47. g_integralFBx=0.0f;
48. g_integralFBy=0.0f;
49. g_integralFBz=0.0f;
50. }
52. //直接應(yīng)用比例調(diào)節(jié)，修正陀螺儀的值
53. gx+=g_twoKp*halfex;
54. gy+=g_twoKp*halfey;
55. gz+=g_twoKp*halfez;
56. }
58. //以下為四元數(shù)微分方程.將陀螺儀和四元數(shù)結(jié)合起來，是姿態(tài)更新的核心算子
59. //計算方法由矩陣運算推導(dǎo)而來
60. //.1
61. //q=-*qxOmega式中左邊是四元數(shù)的倒數(shù),右邊的x是四元數(shù)乘法,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ù),意義在于單位化四元數(shù)在空間旋轉(zhuǎn)時是不會拉伸的,僅有旋轉(zhuǎn)角度.這類似與線性代數(shù)里面的正交變換
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. //四元數(shù)到歐拉角轉(zhuǎn)換，轉(zhuǎn)換順序為Z-Y-X,參見.pdf一文,P24
90. //注意此時的轉(zhuǎn)換順序是1-2-3，即X-Y-Z。但是由于畫圖方便，作者這里做了一個轉(zhuǎn)換，即調(diào)換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. }

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

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