????????在看這篇文章的時候，希望大家不是粗略看一遍就完了，而是跟著我的思路去慢慢思考，這樣才會有所收獲，我也會盡量寫清楚我想表達的意思。

一、關(guān)于姿態(tài)融合

????????·首先需要提一點關(guān)鍵的，如果是采用卡爾曼濾波或者互補濾波，盡可能把采樣時間調(diào)小，這樣可以增加濾波的效果。建議采樣時間不要高于10ms，起初我在測試的時候采樣時間設(shè)置為20ms，結(jié)果卡爾曼濾波收斂速度非常慢，也就是旋轉(zhuǎn)小車讓角度突變的情況下，OLED上顯示的角度值會超過實際值，然后再慢慢回來接近實際值。包括PID控制也一樣，畢竟我們所使用的都是將這些數(shù)學(xué)線性公式轉(zhuǎn)換成離散形式的公式，采樣時間越短，效果當(dāng)然也就越接近原本數(shù)學(xué)公式所體現(xiàn)的效果了。

? ?????采樣時間包括姿態(tài)傳感器的采樣時間。MPU6050的采樣頻率一般設(shè)為200Hz，這樣采樣時間就是5ms了，可以達到一個不錯的效果，再用上MPU6050的數(shù)據(jù)中斷，每隔5ms給MCU發(fā)一個中斷信號，然后在這個中斷服務(wù)函數(shù)中讀取數(shù)據(jù)，處理數(shù)據(jù)。

????????·其次，在進行姿態(tài)融合濾波時，要注意數(shù)據(jù)極性(或者說矢量方向)，通常我們都是將加速度計2個軸取反正切值計算出的角度和陀螺儀積分計算出的角度進行濾波融合。這時候就必須保證由加速度計計算出的角度和陀螺儀計算出的角度極性必須相同。通常我們都是以加速度計算出的角度極性為準，確保陀螺儀計算的角度極性與其相同。下面這張圖是我從MPU6050數(shù)據(jù)手冊上截下來的，并定義了一下小車前后的方向，接下來我會通過2個例子說明極性的問題。

? ? ? ? ? ??

????????①假設(shè)我的小車用的是Y軸的傾角，水平時Z軸的加速度固定為g，也就是重力加速度，Y軸加速度即為0，計算出的角度也是0。如果此時小車往后轉(zhuǎn)動，那么此時重力和Z軸就不重合了，重力就可以被分解為Z軸和Y軸上兩個互相垂直的矢量，這樣通過arctan(y/z)就可以計算出Y軸當(dāng)前的傾角了（單位：弧度）。注意，此時Z軸和Y軸上的加速度都是負值，負負得正，最終計算出的角度是正的。

????????接下來再來看看陀螺儀計算的角度，由于我們使用Y軸的傾角，那么對應(yīng)的角速度也就是X軸的角速度，因為我們Y軸為傾角往后轉(zhuǎn)動小車時，就是在繞X軸運動。從MPU6050的坐標系可以看出，以Y軸為傾角往后轉(zhuǎn)動時，X軸的角速度方向是正的，也就是直接對陀螺儀輸出的數(shù)據(jù)（量程轉(zhuǎn)換后）進行積分得到的角度也是正的，這與上一步加速度計算出的角度極性相同，因此可以直接傳入進行濾波。

????????②那如果小車以X軸為傾角轉(zhuǎn)動呢，假設(shè)此時小車往后轉(zhuǎn)動，那么使用加速度計算角度和剛才是一樣的，都是負負得正（大家可以自己思考一下哈）。但是陀螺儀計算的角度呢，往后轉(zhuǎn)動小車，在Y軸上的角速度對比MPU6050的坐標系，可以發(fā)現(xiàn)角速度的方向是負的，那么通過積分得到的角度也就是負的，這就需要在姿態(tài)融合濾波前，將其取相反數(shù)，得到與加速度計算出的角度一樣的極性進行下面的姿態(tài)融合濾波。

????????如果極性沒有對應(yīng)的話，比如加速度計算的角度是正的，而陀螺儀計算的角度是負的，那么在濾波的時候，我們短時間內(nèi)是相信陀螺儀的，這樣角度就會比實際值小，隨著時間的增加，我們又更相信加速度計，因此加速度計算的角度會不斷地進行修正，因此靜止幾秒后，濾波后的角度才會慢慢接近實際值。其實歸根結(jié)底就是要根據(jù)MPU6050的坐標系來處理數(shù)據(jù)啦。

????????·在對陀螺儀積分計算角度時，要先進行量程轉(zhuǎn)換，比如我的MPU6050陀螺儀量程設(shè)置為±2000dps，那么讀出陀螺儀原始數(shù)據(jù)時需要除以16.4(65536/4000)，才能得到角速度值。而通過加速度取反正切值計算出的數(shù)據(jù)單位是弧度，要轉(zhuǎn)換成角度還需要乘以(180 / π)。

????????·在讀出陀螺儀的原始數(shù)據(jù)時，最好能減去零點漂移。在靜止的時候，陀螺儀讀出的數(shù)據(jù)一般不為0，而是一個接近0的數(shù)值，比如可能是-31，那么在進行一切計算前，給陀螺儀讀出的數(shù)據(jù)加上31(減去-31)，這樣也可以確保后面進行PID運算時效果更好。具體的零點漂移數(shù)值就需要通過顯示屏或者上位機來觀察了。

二、PID控制

????????通過以下3個環(huán)的控制最終會得出三個輸出值，這三個輸出值按以下關(guān)系計算的出最終的電機PWM占空比比較值：

?

后面會再對這里進行解釋.

1.平衡PD控制

????????平衡控制是平衡小車最簡單最基本的控制，它不需要積分運算，代碼如下（示例）：

?

????????·注意平衡控制的微分項最好是陀螺儀輸出的原始數(shù)據(jù)，而不是經(jīng)過量程轉(zhuǎn)換或者濾波后的數(shù)據(jù)，這樣在角速度很小的時候才能更好地進行超調(diào)抑制，因此前面所說的減去零點漂移就很重要了。

????????·平衡控制的參數(shù)也需要注意極性，這和前面的姿態(tài)融合濾波是一樣的，必須保證角度angle和角速度gyro極性是一樣的。如果不一樣(比如：一正一負)，就需要在傳入?yún)?shù)時對其中一個取相反數(shù)，一般是改變角速度gyro極性和角度angle極性相同，否則微分項就會起反效果了(加強超調(diào)和震蕩)。

·平衡環(huán)參數(shù)整定方法：

????????①在整定之前，必須保證前面所有的數(shù)據(jù)處理都沒有問題，即旋轉(zhuǎn)小車使小車傾角突變時，濾波后得到的角度不會出現(xiàn)很明顯的滯后和超調(diào)（然后再慢慢被加速度修正），旋轉(zhuǎn)角度突變值比較大時，滯后或者超調(diào)1~2°是正常的，畢竟是離散化公式，這就體現(xiàn)了采樣時間的重要性。

????????②確定平衡的角度，需要通過顯示屏或者上位機進行觀察確定，至少精確到小數(shù)點后一位，比如我的小車是-2.7度左右平衡。

????????③計算比例系數(shù)Kp的范圍，參考以下公式：

?

(10表示10度時最大速度)。

比如我的pwm最大占空比比較值是7200，那么Kp范圍就是0~720。

????????④給一個較小的Kp值以便確定Kp的極性，比如50。如果向前傾斜小車，小車輪子會一直往前轉(zhuǎn)動，小車往后傾斜則輪子往后轉(zhuǎn)動，說明Kp極性正確。否則Kp極性就是反的，應(yīng)該是-50。

????????⑤慢慢增大Kp值，直到小車倒下時有一個明顯的回復(fù)力，表現(xiàn)出來就是小車在平衡角度附近低頻震蕩，合適的Kp值一般不會超過Kp范圍的50%。

????????⑥確定Kd的范圍。參考以下公式：

?

(2000表示陀螺儀輸出的最大原始值，這是我的平臺上觀測得到的，盡管最大32767，但實際使用時基本不會超過2000)

????????⑦將Kp設(shè)為0，確定Kd的極性，給一個較小的Kd值以便確定Kd的極性，比如0.5,。如果旋轉(zhuǎn)小車，小車輪子會向相同的方向轉(zhuǎn)動，說明Kd極性正確。否則Kd極性就是反的，應(yīng)該是-0.5。

????????⑧將Kp設(shè)置為先前調(diào)好的值，慢慢增大Kd，直到小車的低頻抖動消失，但注意Kd值不可過大，否則小車會出現(xiàn)高頻抖動，此時非常容易燒電機驅(qū)動芯片。合適的Kd值一般不會超過Kd范圍的40%。

????????至此，平衡環(huán)就完成了，因為目前我們沒有引入與速度相關(guān)的控制，因此小車在平衡的同時會往一個方向加速，這是平衡環(huán)在起作用，為了不讓小車倒下，但是一直加速最終還是會倒下的，而此時電機轉(zhuǎn)速可能會達到最大，因此有必要加上2個條件做保護，1是當(dāng)小車傾角大于一定值時，關(guān)閉電機；2是對pwm進行限幅，一般限制在60%的占空比即可。

2.速度PI控制

????????平衡小車的速度控制最基本功能是為了抵消平衡環(huán)產(chǎn)生的速度，讓小車保持平衡時的同時轉(zhuǎn)速為0(理論值)。速度的控制關(guān)鍵點在于消除靜差，因此需要引入積分運算，而不需要微分運算。代碼如下（示例）：

?

這個代碼是參考平衡小車之家的，下面說一些注意事項。

????????·速度PI控制是正反饋，所謂正反饋就是小車的電機有一定轉(zhuǎn)速時，編碼器數(shù)值的絕對值大于0，PI運算后的值會更大，這個值如果作用到電機PWM占空比上，會讓電機轉(zhuǎn)的更快，然后編碼器數(shù)值的絕對值變得更大，反復(fù)運算，直到PWM最大占空比。至于為什么要是正反饋，這個后面再說。

????????·在進行參數(shù)整定時，要注意電機編碼器的類型，一般有2種，霍爾編碼器和光電編碼器，霍爾編碼器精度低，在參數(shù)整定時，如果是霍爾編碼器，Kp、Ki值會比較大，而對于光電編碼器，Kp、Ki值就不需要很大了。

·速度環(huán)參數(shù)整定方法：

????????①在整定之前，需要保證平衡環(huán)已經(jīng)穩(wěn)定，判斷穩(wěn)定的方法是，讓小車保持在平衡角度附近，左右推動小車，感覺不到抖動即可。

????????②確定Kp和Ki的范圍，參考以下公式：

?

(這里的2500是在平衡環(huán)的作用下加速時達到的兩路編碼器絕對值之和。注意如果是霍爾編碼電機，大概在100~200之間，具體需要根據(jù)大家的平臺觀測。我這里用的是光電編碼器，以下參數(shù)也以光電編碼器的為例)

(至于Ki，貌似是一種經(jīng)驗，由前輩整理得出的，Ki為Kp的1/200)

????????③注釋掉平衡環(huán)以便確定Kp和Ki的極性，它們極性一般都是相同的，比如設(shè)定Kp為2.0，Ki為0.01，那么當(dāng)輕輕轉(zhuǎn)動小車輪子時，電機會開始加速，直到最大轉(zhuǎn)速，說明極性正確，因為速度控制要是正反饋。如果不是這種效果，查看Kp、Ki極性是否正確或者2路編碼器極性是否相同。

????????④加入之前注釋掉的平衡環(huán)，慢慢增大Kp的值，Ki則取對于Kp的1/200，這一步慢慢調(diào)試就可以得到不錯的效果，但注意Kp不要太大了，否則會引起抖動（超調(diào)）。一般合適的Kp值不會超過Kp范圍的60%。

????????需要注意的是，速度環(huán)是用來修正平衡環(huán)的，如果拿起小車，平衡環(huán)不會起作用（沒有修正角度），那么速度環(huán)會因為正反饋而使電機達到最大轉(zhuǎn)速。這就需要寫一個算法進行判斷小車是否被拿起了，如果被拿起則需要關(guān)閉電機，且清除速度環(huán)的積分。直到檢測到小車被放下之后才開啟電機。

????????至此，速度環(huán)就完成了，這一步需要注意很多數(shù)據(jù)的極性，還有參數(shù)的范圍，一但這些確定了，那么速度環(huán)還是非常好調(diào)的。

????????·關(guān)于控制小車前進后退的話，理解了速度環(huán)的原理就很簡單了，速度環(huán)利用比例和積分運算來修正平衡環(huán)產(chǎn)生的速度，如果要控制小車前進或后退的話，那么只需要給比例項或者積分項加上或減去一個值，這樣速度環(huán)就不會修正速度到0了，而是一個非0值，這樣就能控制小車前進后退了。

3.轉(zhuǎn)向PD控制

????????小車在前進后退或者直立時，由于兩邊的電機電氣屬性可能不完全相同，導(dǎo)致一邊轉(zhuǎn)的快一些，這樣小車就會“走歪掉”或者“原地慢慢轉(zhuǎn)圈圈”。這時候就需要引入轉(zhuǎn)向環(huán)控制了。代碼如下（示例）：

????????轉(zhuǎn)向控制可以有多種方法，可以使用電機編碼器、Z軸角速度，或者兩者結(jié)合，這里我只使用了Z軸角速度。

????????·轉(zhuǎn)向控制可以只用比例P來控制，比例項即Z軸角速度原始數(shù)據(jù)。但考慮到需要遙控轉(zhuǎn)向，而且這里使用比例控制或者微分控制沒什么區(qū)別，都是一個系數(shù)乘以一個偏差，因此將Z軸角速度作為微分項，而比例項則是用于遙控控制小車轉(zhuǎn)向。

·轉(zhuǎn)向環(huán)參數(shù)整定方法：

????????①先整定微分系數(shù)，確定Kd的范圍，參考以下公式(和平衡環(huán)的Kd一樣)：

????????②注釋掉平衡環(huán)和速度環(huán)，以便確定Kd的極性，比如Kd設(shè)為0.5，那么將小車輪子接觸地面且繞Z軸旋轉(zhuǎn)小車時，小車輪子會產(chǎn)生一個扭矩抵抗Z軸角速度的變化（轉(zhuǎn)起來比較吃力），那么說明Kd極性正確。如果小車反而加速轉(zhuǎn)動（轉(zhuǎn)起來很輕松），則說明極性反了。

????????③加上注釋掉的平衡環(huán)和速度環(huán)，慢慢調(diào)大Kd的值，直到小車直立或者前進后退時有一個較好的抑制轉(zhuǎn)向效果。合適的Kd值一般不會超過Kd范圍的60%。

????????④比例項則是用來控制遙控小車轉(zhuǎn)向的角速度的，注意轉(zhuǎn)向時要讓微分項失效，即讓Kd為0。比例項越大則控制小車轉(zhuǎn)的速度越快。

????????·這種方法我測試并不能使小車在直立時有一個很好的抑制轉(zhuǎn)向的效果，只能說較好，如果要更好的效果，還需要結(jié)合電機編碼器。

4.開頭式子的解釋

????????首先Balance_Pwm就沒什么好說的了，用于修正角度讓小車處于平衡狀態(tài)。對于Velocity_Pwm先前說是正反饋，電機轉(zhuǎn)的越快，通過PI運算后這個值會越大，因此這里是減去，因為在平衡環(huán)的作用下，如果小車往一邊加速度，那么Balance_Pwm會越來越大的，Balance_Pwm越來越大意味著電機轉(zhuǎn)速越快，這時候Velocity_Pwm就會對它進行修正，讓電機最終的PWM值不至于一直增大。而Turn_Pwm看前面的符號就很直觀了，小車直立時，Balance_Pwm和Velocity_Pwm都是接近0的，這時候如果控制小車轉(zhuǎn)向，那么Turn_Pwm必然是一個比較大的值，代入式子后計算出2個電機的PWM比較值，最終會有一個電機的轉(zhuǎn)向是向后，另一個向前，這樣就達到了小車原地轉(zhuǎn)向的效果。如果邊前進邊轉(zhuǎn)向的話，那就是一個電機轉(zhuǎn)的快，一個轉(zhuǎn)的慢，就達到便前進邊轉(zhuǎn)向的效果了。

????????以上由Up的一些理解、經(jīng)驗和網(wǎng)上資料總結(jié)而來，希望能給大家一些幫助。Up最近事情比較多，也是抽時間把這些總結(jié)出來了，因為時間比較緊，可能某些地方會出錯誤，如果有什么不合理或者錯誤的地方還請指出。