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

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

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

以上是筆者對于加速度計零點校準處理的愚見，如有錯誤，還望共同學習。

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

PID控制：

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

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

淺談完PID，對于四旋翼的控制，筆者采用的就是經(jīng)典控制論中的PID控制，利用的是期望姿態(tài)(pitch=0,roll=0,yaw=0)與當前姿態(tài)的誤差，通過PID的控制作用輸出四路不同的PWM驅動電機讓飛機調(diào)整自己的姿態(tài)滿足當前姿態(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. //---得到當前系統(tǒng)的誤差-->利用期望角度減去當前角度
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. //---穩(wěn)定指示燈,黃色.當角度值小于3°時，判定為基本穩(wěn)定,黃燈亮起
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°時引入積分
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運算并非傳統(tǒng)意義上的利用前一次的誤差減去上一次的誤差得來
51. //---而是直接利用陀螺儀的值來替代微分項,這樣的處理非常好,因為巧妙利用了硬件設施,陀螺儀本身就是具有增量的效果
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控制量到四個電機-->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反向清零,因為沒有反轉
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)過積分分離與抗積分飽和處理后，計算PID輸出，關鍵點在于三軸PID輸出與四電機的融合處理，接著對運算結果進行反向清零和正向限幅處理。

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

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

加速度計濾波：

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

以下是筆者的8深度窗口滑動濾波代碼，算法經(jīng)過優(yōu)化，減少了數(shù)組的移動和求和運算，利用了循環(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;//計算平均值，并輸入到一個固定變量中
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：

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

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

以下為Bootloader中的APP函數(shù)跳轉關鍵代碼：

[cpp]view plaincopyprint?

2. voidIAP_Load_App(u32appxaddr)
3. {
4. if(((*(vu32*)appxaddr)&0x2FFE0000)==0x20000000)//檢查棧頂?shù)刂肥欠窈戏?wbr />
5. {
6. Jump_To_App=(IAP_FunEntrance)*(vu32*)(appxaddr+4);//用戶代碼區(qū)第二個字為程序開始地址(復位地址)-->詳見startup.sLine61
7. //(vu32*)(appxaddr+4)-->將FLASH的首地址強制轉換為vu32的指針
8. //*(vu32*)(appxaddr+4)-->解引用該地址上存放的APP跳轉地址,即main函數(shù)入口
9. //(IAP_FunEntrance)*(vu32*)(appxaddr+4)-->將main函數(shù)入口地址強制轉換為指向函數(shù)的指針給Jump_To_App
10. MSR_MSP(*(vu32*)appxaddr);//初始化APP堆棧指針(用戶代碼區(qū)的第一個字用于存放棧頂?shù)刂?
11. Jump_To_App();//跳轉到APP,執(zhí)行APP
12. }
13. }

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

[cpp]view plaincopyprint?

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