微型四旋翼飛行器的設計與制作

那么顯然對加速度計做不做零點(diǎn)校準處理都是可行的。為什么呢？經(jīng)過(guò)我的分析，首先在這段代碼中，我們對加速度計進(jìn)行了歸一化處理，我們知道在數學(xué)當中，對數值進(jìn)行單位化意味著(zhù)長(cháng)度不變而只改變方向，對于加速度計來(lái)講，他的”長(cháng)度”就是加速度的大小，他的”方向”就是加速度的方向。所以我們對加速度計做了單位化之后，其加速度的大小我們就無(wú)從而知，但是我們利用了他的方向來(lái)進(jìn)行姿態(tài)解算。就這一點(diǎn)來(lái)講，無(wú)論我們做不做零點(diǎn)校準處理，進(jìn)來(lái)的加速度的值始終都拋棄掉大小，并關(guān)注方向，與零點(diǎn)校準處理無(wú)關(guān)。另一方面，由于我們生活在重力場(chǎng)里面，那么加速度計在靜止狀態(tài)下測量的是重力加速度，會(huì )有一個(gè)g的輸出。而我們理想的加速度計應該是輸出0，而在有加速度的時(shí)候應該輸出相應的加速度，但是現實(shí)是我們生活在一個(gè)重力場(chǎng)里面，所以必定有一個(gè)重力輸出。那么零點(diǎn)校準處理的核心就是我們對于加速度計的理解問(wèn)題，如果做了零點(diǎn)校準處理，那么我們使用的加速度計就成為了”真正的”加速度計，當有重力的時(shí)候他輸出為0，有加速度的時(shí)候就輸出加速度；當我們沒(méi)有做零點(diǎn)校準處理的時(shí)候，那么我們使用的加速度計就成了”重力”加速度計。但是細心的你其實(shí)可以發(fā)現那個(gè)并不是真正的加速度計，我將傳感器反過(guò)來(lái)放的話(huà)輸出就不是0了，而是z軸上的負值輸出。顯然這個(gè)零點(diǎn)標準處理做的不那么標準。況且這種處理方式是非常粗糙的，因為加速度計的噪聲十分的大，數據波動(dòng)非常厲害，我做了16深度的窗口滑動(dòng)濾波再加19階的kaiser窗FIR低通濾波，其輸出仍然有1~4左右的波動(dòng)?？梢?jiàn)加速度計確實(shí)不好處理，除非用Kalman濾波。

鑒于以上兩點(diǎn)原因，本人就沒(méi)有做加速度計的零點(diǎn)校準處理。當需要測量飛機的加速度大小并實(shí)現定位時(shí)，那么就需要做零點(diǎn)校準處理了。而當我們只需要解算姿態(tài)，那么加速度計就不需要做零點(diǎn)校準處理。

以上是筆者對于加速度計零點(diǎn)校準處理的愚見(jiàn)，如有錯誤，還望共同學(xué)習。

最后想說(shuō)明一點(diǎn)的是關(guān)于陀螺儀的數據轉化，筆者在最開(kāi)始編寫(xiě)姿態(tài)解算代碼時(shí)，發(fā)現角度的變化與實(shí)時(shí)姿態(tài)差了好幾個(gè)數量級，體現出來(lái)的現象就是稍微移動(dòng)一下飛機，姿態(tài)就呼呼的飛速變化。之前一直以為是姿態(tài)解算中的Kp和Ki的系數問(wèn)題，后來(lái)才發(fā)現是陀螺儀的數據沒(méi)有轉化成標準單位(°/s)輸出，沒(méi)有參看pdf上的量程單位，所以沒(méi)有做數據轉化處理，在這里提醒一下各位，不要犯筆者這種低級錯誤了。

PID控制：

PID控制屬于自動(dòng)化領(lǐng)域，由于筆者的本科出生于自動(dòng)化專(zhuān)業(yè)，所以對于自動(dòng)控制原理有一點(diǎn)理論上的認識。P是比例，I是積分，D是微分，這是最基本的定義。對于一個(gè)系統，我們想要控制他，目前的理論是引入負反饋，這個(gè)概念相當重要，是由維納提出來(lái)的。意思是，將輸出引入到輸入端，并且用輸入減去輸出，這就是著(zhù)名的負反饋系統。很顯然，我們要做的是輸出跟隨輸入，使得系統可控。也就是說(shuō)要求輸出和輸入的誤差為0，即輸出等于輸入。在實(shí)際的系統中，輸出與輸入肯定是存在誤差的，這種誤差就通過(guò)PID來(lái)控制使得滿(mǎn)足輸出與輸入誤差為0。當系統由于干擾出現誤差時(shí)，此時(shí)的P參數就起到了“立竿見(jiàn)影”的作用，將當前系統誤差第一時(shí)間反應出來(lái)，也就是當前誤差多少，我就給你多少輸出值來(lái)補償你的誤差。這種調節方式的特點(diǎn)是快速而有勁，相應來(lái)說(shuō)就是發(fā)散且不穩定的；而D參數則具有一種預見(jiàn)性，這種預見(jiàn)性可以提前預知系統的行為，比如距離設定值是越來(lái)越遠還是越來(lái)越近，前者D的作用越強，后者D的作用越弱?？梢园l(fā)現D參數與P參數具有一定的互補性質(zhì)，P會(huì )引起發(fā)散，而D則是抑制發(fā)散，使系統非常敏感；最后I參數是積分，在連續系統中是時(shí)間的積分，在數字系統中是時(shí)間的累加。這種累加無(wú)疑會(huì )造成系統的不穩定，如果系統長(cháng)時(shí)間處于不平衡的位置，那么由于時(shí)間的累計，I的作用會(huì )變得越來(lái)越強，甚至超過(guò)了P的作用，那么系統必定失控。但是他的作用有時(shí)候確實(shí)不可忽略的：消除靜差。

在這里筆者尤其提醒大家一點(diǎn)，如果此時(shí)系統的輸出達到了我們給定的期望值，也就是說(shuō)輸出與輸入誤差為0，即現在的PID控制器輸入0，所以輸出也是0，也就是說(shuō)此時(shí)的執行機構是不會(huì )輸出的，讓設備處于自由運動(dòng)階段。而非我們認為的當你觀(guān)察到一個(gè)系統處于穩定運行并達到給定值的時(shí)候，他的執行機構是一直在輸出的，這是錯誤的。

淺談完P(guān)ID，對于四旋翼的控制，筆者采用的就是經(jīng)典控制論中的PID控制，利用的是期望姿態(tài)(pitch=0,roll=0,yaw=0)與當前姿態(tài)的誤差，通過(guò)PID的控制作用輸出四路不同的PWM驅動(dòng)電機讓飛機調整自己的姿態(tài)滿(mǎn)足當前姿態(tài)與期望姿態(tài)的誤差為0的目標，這也是PID控制器的目標。

以下是筆者的PID控制代碼：

[cpp]view plaincopyprint?

2. voidQuadrotor_Control(constfloatExp_Pitch,constfloatExp_Roll,constfloatExp_Yaw)
3. {
4. s16outputPWM_Pitch,outputPWM_Roll,outputPWM_Yaw;
6. //---得到當前系統的誤差-->利用期望角度減去當前角度
7. g_Attitude_Error.g_Error_Pitch=Exp_Pitch-g_Pitch;
8. g_Attitude_Error.g_Error_Roll=Exp_Roll-g_Roll;
9. g_Attitude_Error.g_Error_Yaw=Exp_Yaw-g_Yaw;
11. //---傾角太大，放棄控制
12. if(fabs(g_Attitude_Error.g_Error_Pitch)>=55||fabs(g_Attitude_Error.g_Error_Roll)>=55)
13. {
14. PWM2_LED=0;//藍燈亮起
15. PWM_Set(0,0,0,0);//停機
16. return;
17. }
18. PWM2_LED=1;//藍燈熄滅
20. //---穩定指示燈,黃色.當角度值小于3°時(shí)，判定為基本穩定,黃燈亮起
21. if(fabs(g_Attitude_Error.g_Error_Pitch)<=3&&fabs(g_Attitude_Error.g_Error_Roll)<=3)
22. PWM4_LED=0;
23. else
24. PWM4_LED=1;
26. //---積分運算與積分誤差限幅
27. if(fabs(g_Attitude_Error.g_Error_Pitch)<=20)//積分分離-->在姿態(tài)誤差角小于20°時(shí)引入積分
28. {//Pitch
29. //累加誤差
30. g_Attitude_Error.g_ErrorI_Pitch+=g_Attitude_Error.g_Error_Pitch;
32. //積分限幅
33. if(g_Attitude_Error.g_ErrorI_Pitch>=PITCH_I_MAX)
34. g_Attitude_Error.g_ErrorI_Pitch=PITCH_I_MAX;
35. elseif(g_Attitude_Error.g_ErrorI_Pitch<=-PITCH_I_MAX)
36. g_Attitude_Error.g_ErrorI_Pitch=-PITCH_I_MAX;
37. }
38. if(fabs(g_Attitude_Error.g_Error_Roll)<=20)
39. {//Roll
40. //累加誤差
41. g_Attitude_Error.g_ErrorI_Roll+=g_Attitude_Error.g_Error_Roll;
43. //積分限幅
44. if(g_Attitude_Error.g_ErrorI_Roll>=ROLL_I_MAX)
45. g_Attitude_Error.g_ErrorI_Roll=ROLL_I_MAX;
46. elseif(g_Attitude_Error.g_ErrorI_Roll<=-ROLL_I_MAX)
47. g_Attitude_Error.g_ErrorI_Roll=-ROLL_I_MAX;
48. }
50. //---PID運算-->這里的微分D運算并非傳統意義上的利用前一次的誤差減去上一次的誤差得來(lái)
51. //---而是直接利用陀螺儀的值來(lái)替代微分項,這樣的處理非常好,因為巧妙利用了硬件設施,陀螺儀本身就是具有增量的效果
52. outputPWM_Pitch=(s16)(g_PID_Kp*g_Attitude_Error.g_Error_Pitch+
53. g_PID_Ki*g_Attitude_Error.g_ErrorI_Pitch-g_PID_Kd*g_MPU6050Data_Filter.gyro_x_c);
54. outputPWM_Roll=(s16)(g_PID_Kp*g_Attitude_Error.g_Error_Roll+
55. g_PID_Ki*g_Attitude_Error.g_ErrorI_Roll-g_PID_Kd*g_MPU6050Data_Filter.gyro_y_c);
56. outputPWM_Yaw=(s16)(g_PID_Yaw_Kp*g_Attitude_Error.g_Error_Yaw);
58. //---給出PWM控制量到四個(gè)電機-->X模式控制
59. //特別注意，這里輸出反相了，因為誤差是反的
60. g_motor1_PWM=g_BasePWM+outputPWM_Pitch+outputPWM_Roll+outputPWM_Yaw;
61. g_motor2_PWM=g_BasePWM-outputPWM_Pitch+outputPWM_Roll-outputPWM_Yaw;
62. g_motor3_PWM=g_BasePWM-outputPWM_Pitch-outputPWM_Roll+outputPWM_Yaw;
63. g_motor4_PWM=g_BasePWM+outputPWM_Pitch-outputPWM_Roll-outputPWM_Yaw;
65. //---PWM反向清零,因為沒(méi)有反轉
66. if(g_motor1_PWM<0)
67. g_motor1_PWM=0;
68. if(g_motor2_PWM<0)
69. g_motor2_PWM=0;
70. if(g_motor3_PWM<0)
71. g_motor3_PWM=0;
72. if(g_motor4_PWM<0)
73. g_motor4_PWM=0;
75. //---PWM限幅
76. if(g_motor1_PWM>=g_MaxPWM)
77. g_motor1_PWM=g_MaxPWM;
78. if(g_motor2_PWM>=g_MaxPWM)
79. g_motor2_PWM=g_MaxPWM;
80. if(g_motor3_PWM>=g_MaxPWM)
81. g_motor3_PWM=g_MaxPWM;
82. if(g_motor4_PWM>=g_MaxPWM)
83. g_motor4_PWM=g_MaxPWM;
85. if(g_Fly_Enable)//允許起飛,給出PWM
86. PWM_Set(g_motor1_PWM,g_motor2_PWM,g_motor3_PWM,g_motor4_PWM);
87. else
88. PWM_Set(0,0,0,0);//停機
89. }

在這段代碼中，首先得到期望值與當前值的誤差，然后經(jīng)過(guò)積分分離與抗積分飽和處理后，計算PID輸出，關(guān)鍵點(diǎn)在于三軸PID輸出與四電機的融合處理，接著(zhù)對運算結果進(jìn)行反向清零和正向限幅處理。

我們知道四旋翼目前有兩種運行模式，一種成為+模式，一種成為x模式。前者表示四旋翼的運動(dòng)方向與其中一對電機的軸線(xiàn)重合，后者則是將前一種方式旋轉45度的結果。相對而言，x模式穩定一些。但如果要完成翻跟頭等特技動(dòng)作，可能需要用+模式。筆者觀(guān)看了網(wǎng)易公開(kāi)課關(guān)于四旋翼的TED，他們的四軸運動(dòng)方式全部是+模式。筆者在這里就不細講+模式與x模式怎么融合，這部分網(wǎng)上都有，其實(shí)也不難，想好符號和力矩關(guān)系，自己都可以寫(xiě)出來(lái)。筆者就是這么過(guò)來(lái)的。

而對于PID的參數整定來(lái)講，因為筆者制作的是小四軸，慣性小，很靈敏。所以P和D參數的耦合比大四軸嚴重很多，在調試的時(shí)候注意兩者的關(guān)系。先整定P，再整定D，然后反過(guò)來(lái)迭代P，再迭代D，直到找到一個(gè)最佳值。如果發(fā)現無(wú)論如何都找不到更好的效果時(shí)，考慮降低參數，因為可能在迭代的過(guò)程中已經(jīng)超過(guò)了極值。

加速度計濾波：

在前面的姿態(tài)解算部分已經(jīng)提到有必要對加速度計的值進(jìn)行濾波。筆者為了達到濾波的最佳效果，當沒(méi)有考慮實(shí)時(shí)性時(shí)，采用了方才討論的16深度的窗口滑動(dòng)濾波再加19階的kaiser窗FIR低通濾波，效果確實(shí)理想很多，但是代價(jià)就是延遲較為嚴重；而在考慮實(shí)時(shí)性的要求之后，筆者去除了FIR低通濾波，只用了8深度的窗口滑動(dòng)濾波。雖然效果來(lái)講肯定沒(méi)有前述的要好，但是對于姿態(tài)解算的誤差來(lái)講，靜止時(shí)波動(dòng)差不多在0.1~0.2°左右(有FIR濾波則穩定不動(dòng))。針對于本四旋翼的設計，0.1~0.2°的誤差顯得微不足道，所以就放棄了高階的FIR濾波。當然，這只是在靜止狀態(tài)下的測試，如果打開(kāi)電機，引入電機的高頻機械震動(dòng)，那么加速度計的值又會(huì )產(chǎn)生新的噪聲。筆者將四旋翼拿在手上測試他的角度變化，發(fā)現在大油門(mén)時(shí)大致有4°左右的偏差，這個(gè)誤差還是較為嚴重的。鑒于此，筆者才做FIR濾波。但是在實(shí)際飛行過(guò)程中，當只有8深度的窗口滑動(dòng)濾波時(shí)，似乎可以平衡，沒(méi)有拿在手上測試的4°誤差。所以在這里筆者就偷懶了，直接采用8深度的窗口滑動(dòng)濾波，放棄了FIR低通濾波。具體的原因，如果有網(wǎng)友愿意討論可以聯(lián)系我。

以下是筆者的8深度窗口滑動(dòng)濾波代碼，算法經(jīng)過(guò)優(yōu)化，減少了數組的移動(dòng)和求和運算，利用了循環(huán)隊列的原理避免了求和運算：

[cpp]view plaincopyprint?

2. voidIMU_Filter(void)
3. {
4. s32resultx=0;
5. statics32s_resulttmpx[ACC_FILTER_DELAY]={0};
6. staticu8s_bufferCounterx=0;
7. statics32s_totalx=0;
9. s32resulty=0;
10. statics32s_resulttmpy[ACC_FILTER_DELAY]={0};
11. staticu8s_bufferCountery=0;
12. statics32s_totaly=0;
14. s32resultz=0;
15. statics32s_resulttmpz[ACC_FILTER_DELAY]={0};
16. staticu8s_bufferCounterz=0;
17. statics32s_totalz=0;
19. //加速度計濾波
20. s_totalx-=s_resulttmpx[s_bufferCounterx];//從總和中刪除頭部元素的值，履行頭部指針職責
21. s_resulttmpx[s_bufferCounterx]=g_MPU6050Data.accel_x;//將采樣值放到尾部指針處，履行尾部指針職責
22. s_totalx+=g_MPU6050Data.accel_x;//更新總和
24. resultx=s_totalx/ACC_FILTER_DELAY;//計算平均值，并輸入到一個(gè)固定變量中
25. s_bufferCounterx++;//更新指針
26. if(s_bufferCounterx==ACC_FILTER_DELAY)//到達隊列邊界
27. s_bufferCounterx=0;
28. g_MPU6050Data_Filter.accel_x_f=resultx;
30. s_totaly-=s_resulttmpy[s_bufferCountery];
31. s_resulttmpy[s_bufferCountery]=g_MPU6050Data.accel_y;
32. s_totaly+=g_MPU6050Data.accel_y;
34. resulty=s_totaly/ACC_FILTER_DELAY;
35. s_bufferCountery++;
36. if(s_bufferCountery==ACC_FILTER_DELAY)
37. s_bufferCountery=0;
38. g_MPU6050Data_Filter.accel_y_f=resulty;
40. s_totalz-=s_resulttmpz[s_bufferCounterz];
41. s_resulttmpz[s_bufferCounterz]=g_MPU6050Data.accel_z;
42. s_totalz+=g_MPU6050Data.accel_z;
44. resultz=s_totalz/ACC_FILTER_DELAY;
45. s_bufferCounterz++;
46. if(s_bufferCounterz==ACC_FILTER_DELAY)
47. s_bufferCounterz=0;
48. g_MPU6050Data_Filter.accel_z_f=resultz;
49. }

基于NRF24L01的Bootloader：

這一塊內容屬于獨立與四旋翼開(kāi)發(fā)的部分，因為在最初設計之時(shí)，想到PID調試需要反復整定參數，就需要不斷的燒寫(xiě)程序來(lái)變更參數，這樣就需要重復的插拔連線(xiàn)，顯得麻煩。所以筆者就在無(wú)線(xiàn)模塊NRF24L01的基礎之上，開(kāi)發(fā)了Bootloader技術(shù)，使得下載程序通過(guò)無(wú)線(xiàn)模塊下載程序到Flash中，這樣極大的簡(jiǎn)化了參數整定的過(guò)程。

筆者在這里就不詳細介紹Bootloader的原理了，簡(jiǎn)單點(diǎn)說(shuō)就是在Flash中開(kāi)辟兩個(gè)區域：A區域和B區域。其中A區域稱(chēng)之為Bootloader，用以實(shí)現Flash的燒寫(xiě)工作，相當于代替了J-LINK；B區域就是我們運行代碼的區域，也就是Bootloader將要操作的Flash區域，我們的代碼就在這里運行。單片機在開(kāi)機后首先運行A區域的Bootloader代碼，這段代碼等待NRF24L01接收二進(jìn)制程序代碼，在接收的同時(shí)，就一邊將接收到的二進(jìn)制程序代碼燒寫(xiě)進(jìn)B區域中。等全部接收完畢，同時(shí)也燒寫(xiě)完畢。之后通過(guò)在匯編修改棧頂指針并跳轉到程序的APP代碼起始位置即可。

以下為Bootloader中的APP函數跳轉關(guān)鍵代碼：

[cpp]view plaincopyprint?

2. voidIAP_Load_App(u32appxaddr)
3. {
4. if(((*(vu32*)appxaddr)&0x2FFE0000)==0x20000000)//檢查棧頂地址是否合法
5. {
6. Jump_To_App=(IAP_FunEntrance)*(vu32*)(appxaddr+4);//用戶(hù)代碼區第二個(gè)字為程序開(kāi)始地址(復位地址)-->詳見(jiàn)startup.sLine61
7. //(vu32*)(appxaddr+4)-->將FLASH的首地址強制轉換為vu32的指針
8. //*(vu32*)(appxaddr+4)-->解引用該地址上存放的APP跳轉地址,即main函數入口
9. //(IAP_FunEntrance)*(vu32*)(appxaddr+4)-->將main函數入口地址強制轉換為指向函數的指針給Jump_To_App
10. MSR_MSP(*(vu32*)appxaddr);//初始化APP堆棧指針(用戶(hù)代碼區的第一個(gè)字用于存放棧頂地址)
11. Jump_To_App();//跳轉到APP,執行APP
12. }
13. }

尤其注意Jump_To_App和Jump_To_App()的用法，前提是Jump_To_App本身就是一個(gè)指向函數的指針。定義：

[cpp]view plaincopyprint?

1. typedefvoid(*IAP_FunEntrance)(void);//定義一個(gè)指向函數的指針
2. IAP_FunEntranceJump_To_App;