硬件設(shè)計(jì)：

總體思路：

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

硬件模塊：

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

硬件選型：

硬件工作綜述：

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

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

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

尺寸：對(duì)角電機(jī)軸距10x10cm

重量：33.2g(帶電池)

軟件設(shè)計(jì)：

總體思路：

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

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

姿態(tài)解算：

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

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

通過慣性測(cè)量模塊(IMU)傳送過來的當(dāng)前飛機(jī)的三軸加速度和三軸角速度的值，這樣一個(gè)三維的向量，轉(zhuǎn)化為四維向量，然后在四維空間中做線性變換(也可以說在三維空間中旋轉(zhuǎn))后輸出，利用四元數(shù)與歐拉角的關(guān)系(一定要注意旋轉(zhuǎn)順序)，將當(dāng)前四元數(shù)轉(zhuǎn)換為歐拉角pitch、roll、yaw即得到當(dāng)前飛機(jī)的姿態(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;//在當(dāng)前載體坐標(biāo)系中，重力分量在三個(gè)軸上的分量
6. floathalfex,halfey,halfez;//當(dāng)前加速度計(jì)測(cè)得的重力加速度在三個(gè)軸上的分量與當(dāng)前姿態(tài)在三個(gè)軸上的重力分量的誤差,這里采用差積的方式
7. floatqa,qb,qc;
9. gx=gx*PI/180;//轉(zhuǎn)換為弧度制
10. gy=gy*PI/180;
11. gz=gz*PI/180;
13. //如果加速度計(jì)處于自由落體狀態(tài)，可能會(huì)出現(xiàn)這種情況，不進(jìn)行姿態(tài)解算，因?yàn)闀?huì)產(chǎn)生分母無窮大的情況
14. if(!((ax==0.0f)&&(ay==0.0f)&&(az==0.0f)))
15. {
16. //單位化加速度計(jì),意義在于在變更了加速度計(jì)的量程之后不需要修改Kp參數(shù),因?yàn)檫@里歸一化了
17. recipNorm=invSqrt(ax*ax+ay*ay+az*az);
18. ax*=recipNorm;
19. ay*=recipNorm;
20. az*=recipNorm;
22. //將當(dāng)前姿態(tài)的重力在三個(gè)軸上的分量分離出來
23. //就是方向余弦旋轉(zhuǎn)矩陣的第三列,注意是地理坐標(biāo)系(n系)到載體坐標(biāo)系(b系)的,不要弄反了.如果書上是b系到n系,轉(zhuǎn)置即可
24. //慣性測(cè)量器件測(cè)量的都是關(guān)于b系的值，為了方便，我們一般將b系轉(zhuǎn)換到n系進(jìn)行導(dǎo)航參數(shù)求解。但是這里并不需要這樣做，因?yàn)檫@里是對(duì)陀螺儀進(jìn)行補(bǔ)償
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. //計(jì)算由當(dāng)前姿態(tài)的重力在三個(gè)軸上的分量與加速度計(jì)測(cè)得的重力在三個(gè)軸上的分量的差,這里采用三維空間的差積(向量積)方法求差
30. //計(jì)算公式由矩陣運(yùn)算推導(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)于當(dāng)前姿態(tài)分離出的重力分量與當(dāng)前加速度計(jì)測(cè)得的重力分量的差值進(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)行積分運(yùn)算，只進(jìn)行比例調(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. //計(jì)算方法由矩陣運(yùn)算推導(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)時(shí)是不會(huì)拉伸的,僅有旋轉(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)換順序?yàn)閆-Y-X,參見.pdf一文,P24
90. //注意此時(shí)的轉(zhuǎn)換順序是1-2-3，即X-Y-Z。但是由于畫圖方便，作者這里做了一個(gè)轉(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%。并且注意輸入的陀螺儀必須是弧度制(這一點(diǎn)在進(jìn)入函數(shù)的時(shí)候已經(jīng)做了轉(zhuǎn)換)，否則姿態(tài)解算是錯(cuò)誤的。

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