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

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

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

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

PID控制：

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

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

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

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

[cpp]view plaincopyprint?

2. voidQuadrotor_Control(constfloatExp_Pitch,constfloatExp_Roll,constfloatExp_Yaw)
3. {
4. s16outputPWM_Pitch,outputPWM_Roll,outputPWM_Yaw;
6. //---得到當(dāng)前系統(tǒng)的誤差-->利用期望角度減去當(dā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;//藍(lán)燈亮起
15. PWM_Set(0,0,0,0);//停機(jī)
16. return;
17. }
18. PWM2_LED=1;//藍(lán)燈熄滅
20. //---穩(wěn)定指示燈,黃色.當(dāng)角度值小于3°時(shí)，判定為基本穩(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. //---積分運(yùn)算與積分誤差限幅
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運(yùn)算-->這里的微分D運(yùn)算并非傳統(tǒng)意義上的利用前一次的誤差減去上一次的誤差得來
51. //---而是直接利用陀螺儀的值來替代微分項(xiàng),這樣的處理非常好,因?yàn)榍擅罾昧擞布O(shè)施,陀螺儀本身就是具有增量的效果
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控制量到四個電機(jī)-->X模式控制
59. //特別注意，這里輸出反相了，因?yàn)檎`差是反的
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反向清零,因?yàn)闆]有反轉(zhuǎn)
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);//停機(jī)
89. }

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

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

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

加速度計(jì)濾波：

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

以下是筆者的8深度窗口滑動濾波代碼，算法經(jīng)過優(yōu)化，減少了數(shù)組的移動和求和運(yùn)算，利用了循環(huán)隊(duì)列的原理避免了求和運(yù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. //加速度計(jì)濾波
20. s_totalx-=s_resulttmpx[s_bufferCounterx];//從總和中刪除頭部元素的值，履行頭部指針職責(zé)
21. s_resulttmpx[s_bufferCounterx]=g_MPU6050Data.accel_x;//將采樣值放到尾部指針處，履行尾部指針職責(zé)
22. s_totalx+=g_MPU6050Data.accel_x;//更新總和
24. resultx=s_totalx/ACC_FILTER_DELAY;//計(jì)算平均值，并輸入到一個固定變量中
25. s_bufferCounterx++;//更新指針
26. if(s_bufferCounterx==ACC_FILTER_DELAY)//到達(dá)隊(duì)列邊界
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)容屬于獨(dú)立與四旋翼開發(fā)的部分，因?yàn)樵谧畛踉O(shè)計(jì)之時(shí)，想到PID調(diào)試需要反復(fù)整定參數(shù)，就需要不斷的燒寫程序來變更參數(shù)，這樣就需要重復(fù)的插拔連線，顯得麻煩。所以筆者就在無線模塊NRF24L01的基礎(chǔ)之上，開發(fā)了Bootloader技術(shù)，使得下載程序通過無線模塊下載程序到Flash中，這樣極大的簡化了參數(shù)整定的過程。

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

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

[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ū)第二個字為程序開始地址(復(fù)位地址)-->詳見startup.sLine61
7. //(vu32*)(appxaddr+4)-->將FLASH的首地址強(qiáng)制轉(zhuǎn)換為vu32的指針
8. //*(vu32*)(appxaddr+4)-->解引用該地址上存放的APP跳轉(zhuǎn)地址,即main函數(shù)入口
9. //(IAP_FunEntrance)*(vu32*)(appxaddr+4)-->將main函數(shù)入口地址強(qiáng)制轉(zhuǎn)換為指向函數(shù)的指針給Jump_To_App
10. MSR_MSP(*(vu32*)appxaddr);//初始化APP堆棧指針(用戶代碼區(qū)的第一個字用于存放棧頂?shù)刂?
11. Jump_To_App();//跳轉(zhuǎn)到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;