雙輪平衡小車
看到一篇很好的文章，圖文並茂的講述了，兩輪平衡小車的原理，並且給出了詳細的程式碼，這裡拿來分享給大家。
平衡原理
平衡小車是透過兩個電機運動下實現小車不倒下直立行走的多功能智慧小車，在外力的推拉下，小車依然保持不倒下。這麼一說可能還沒有很直觀的瞭解 究竟什麼是平衡小車，不過這個平衡小車實現的原理其實是在人們生活中的經驗 得來的。如果透過簡單的練習，一般人可以透過自己的手指把木棒直立而不倒的 放在指尖上，所以練習的時候，需要學會的兩個條件：一是放在指尖上可以移動， 二是透過眼睛觀察木棒的傾斜角度和傾斜趨勢（角速度）。透過手指的移動去抵 消木棒傾斜的角度和趨勢，使得木棒能直立不倒。這樣的條件是不可以缺一的， 實際上加入這兩個條件，控制過程中就是負反饋機制。
而世界上沒有任何一個人可以矇眼不看，就可以直立木棒的，因為沒有眼睛的負反饋，就不知道筆的傾斜角度和趨勢。
平衡小車也是這樣的過程，透過負反饋實現平衡。與上面保持木棒直立比較則相對簡單，因為小車有兩個輪子著地，車體只會在輪子滾動的方向上發生 傾斜。控制輪子轉動，抵消在一個維度上傾斜的趨勢便可以保持車體平衡了。
所以根據上述的原理，透過測量小車的傾角和傾角速度控制小車車輪的加速度來消除小車的傾角。因此，小車傾角以及傾角速度的測量成為控制小車直立 的關鍵。我們的亞博智慧平衡小車使用了測量傾角和傾角速度的整合感測器陀螺儀-MPU6050。
角度（PD 演算法）
控制平衡小車，使得它作加速運動。這樣站在小車上（非慣性系，以車輪 作為座標原點）分析倒立擺受力，它就會受到額外的慣性力，該力與車輪的加 速度方向相反，大小成正比。這樣倒立擺（如圖 2）所受到的回覆力為：
公式 1：
F = mg sin θ-ma cos θ≈mg θ-mk1θ
式1 中，由於θ很小，所以進行了線 性化。假設負反饋控制是車輪加速度 a 與偏角θ成正比，比例為 k1。如果比例k1>g，（g 是重力加速度）那麼回覆力的方向便於位移方向相反了。
而為了讓倒立擺能夠儘快回到垂直位置穩定下來，還需要增加阻尼力。增 加的阻尼力與偏角的速度成正比，方向相反，因此公式1可改為：F = mg θ-mk1 θ -mk2 θ` 按照上述倒立擺的模型，可得出控制小車車輪加速度的演算法：
公式 2：
a =k1θ+k2θ`
式2中θ為小車角度，θ`為角速度。k1 k2都是比例係數 根據上述內容，建立速度的比例微分負反饋控制，根據基本控制理論討論小車透過閉環控制保持穩定的條件（這裡需要對控制理論有基本瞭解）。假設外力干擾引起車模產生角加速度x（t）。沿著垂直於車模地盤方向進行受力分析，可 以得到車模傾角與車輪運動加速度以及外力干擾加速度a（t）x（t）之間的運動方程。
在角度反饋控制中，與角度成比例的控制量是稱為比例控制；與 角速度成比例的控制量稱為微分控制（角速度是角度的微分）。因此 上面係數 k1，k2 分別稱為比例和微分控制引數。其中微分引數相當於 阻尼力，可以有效抑制車模震盪。透過微分抑制控制震盪的思想在後 面的速度和方向控制中也同樣適用。總結控制車模直立穩定的條件如下：
（1）能夠精確測量車模傾角θ的大小和角速度θ‘的大小；
（2）可以控制車輪的加速度。
上述控制實際結果是小車與地面不是嚴格垂直，而是存在一個對 應的傾角。在重力的作用下，小車會朝著一個方面加速前進。為了保 持小車的靜止或者勻速運動需要消除這個安裝誤差。在實際小車製作 過程中需要進行機械調整和軟體引數設定。另外需要透過軟體中的速 度控制來實現速度的穩定性。在小車角度控制中出現的小車傾角偏 差，使得小車在傾斜的方向上產生加速。這個結果可以用來進行小車 的速度控制。下面將利用這個原理來調節小車的速度。
測速（模型和演算法）
假設小車在上面直立控制調節下已經能夠保持平衡了，但是由於安裝誤差，感測器實際測量的角度與車模角度有偏差，因此小車實際 不是保持與地面垂直，而是存在一個傾角。在重力的作用下，小車就 會朝傾斜的方向加速前進。控制速度只要透過控制小車的傾角就可以 實現了。具體實現需要解決三個問題：
（1）如何測量小車速度？
（2）如何透過小車直立控制實現小車傾角的改變？
（3）如何根據速度誤差控制小車傾角？
第一個問題可以透過安裝在電機輸出軸上的光碼盤來測量得到小車的車輪速度。如圖所示。利用控制微控制器的計數器測量在固定 時間間隔內速度脈衝訊號的個數可以反映電機的轉速。
第二個問題可以透過角度控制給定值來解決。給定小車直立控制 的設定值，在角度控制調節下，小車將會自動維持在一個角度。透過 前面小車直立控制演算法可以知道，小車傾角最終是跟蹤重力加速度Z 軸的角度。因此小車的傾角給定值與重力加速度Z軸角度相減，便可 以最終決定小車的傾角。
第三個問題分析起來相對比較困難，遠比直觀進行速度負反饋分 析複雜。首先對一個簡單例子進行分析。假設小車開始保持靜止，然 後增加給定速度，為此需要小車往前傾斜以便獲得加速度。在小車直 立控制下，為了能夠有一個往前的傾斜角度，車輪需要往後運動，這 樣會引起車輪速度下降（因為車輪往負方向運動了）。由於負反饋， 使得小車往前傾角需要更大。如此迴圈，小車很快就會傾倒。原本利 用負反饋進行速度控制反而成了“正”反饋。
為了保證系統穩定，往往小車傾角控制時間常數Tz很大。這樣便會引起系統產生兩個共軛極點，而且極點的實部變得很小，使得系統的速度控制會產生的震盪現象。這個現象在實際引數整定的時候可以觀察到。那麼如何消除速度控制過程中的震盪呢？要解決控制震盪問題，在前面的小車角度控制中已經有了經驗，那就是在控制反饋中增加速度微分控制。但由於車輪的速度反饋訊號 中往往存在著噪聲，對速度進行微分運算會進一步加大噪聲的影響。為此需要對上面控制方法進行改進。原系統中傾角調整過程時間常數 往往很大，因此可以將該系統近似為一個積分環節。將原來的微分環 節和這個積分環節合併，形成一個比例控制環節。這樣可以保持系統 控制傳遞函式不變，同時避免了微分計算。但在控制反饋中，只是使用反饋訊號的比例和微分，沒有誤差積分，所以最終這個速度控制是有殘差的控制。但是直接引入誤差積 分控制環節，會增加系統的複雜度，為此就不再增加積分控制，而是 透過與角度控制相結合後在進行改進。
要求小車在原地停止，速度為0。但是由於採用的是比例控制， 如果此時陀螺儀有漂移，或者加速度感測器安裝有誤差，最終小車傾 角不會最終調整到0，小車會朝著傾斜的方向恆速執行下去。注意此 時車模不會像沒有速度控制那樣加速運行了，但是速度不會最終為0。為了消除這個誤差，可以將小車傾角設定量直接積分補償在角度控制 輸出中，這樣就會徹底消除速度控制誤差。第二點，由於加入了速度 控制，它可以補償陀螺儀和重力加速度的漂移和誤差。所以此時重力 加速度感測器實際上沒有必要了。所以此時重力 加速度感測器實際上沒有必要了。此時小車在控制啟動的時候，需要保持小車的垂直狀態。此時陀螺儀的積分角度也初始化為0。當然如果電路中已經包括了重力加速度感測器，也可以保留這部分，從而提高小車的穩定性。在後面的 最終給定的控制方案中，保留了這部分的控制迴路。
轉向控制（演算法）
透過左右電機速度差驅動小車轉向消除小車距離道路中心的偏差。透過調整小車的方向，再加上車前行運動，可以逐步消除小車距 離中心線的距離差別。這個過程是一個積分過程，因此小車差動控制 一般只需要進行簡單的比例控制就可以完成小車方向控制。但是由於 小車本身安裝有電池等比較重的物體，具有很大的轉動慣量，在調整 過程中會出現小車轉向過沖現象，如果不加以抑制，會使得小車過度 轉向而倒下。根據前面角度和速度控制的經驗，為了消除小車方向控 制中的過沖，需要增加微分控制。
PID（演算法）
控制相關的軟體函式包括：
AngleCalculate：
小車傾角計算函式。根據採集到的陀螺儀和重力加速度感測器的數值 計算小車角度和角速度。如果這部分的演算法由外部一個運放實現，那麼採集得到的直接是小車的角度和角速度，這部分演算法可以省略。該函式是每 5 毫秒呼叫一次。
AngelControl：
小車直立控制函式。根據小車角度和角速度計算小車電機的控制量。直立控制是 5 毫秒呼叫一次。
SpeedControl：
小車速度控制函式。根據小車採集到的電機轉速和速度設定值，計算電 機的控制量。該函式是 100 毫秒呼叫一次。
SpeedControlOutput：
速度輸出平滑函式。由於速度是每 100 毫秒進行一次計算。為 了使得速度控制更加平滑，該函式將速度輸出變化量平均分配到 20步 5 毫秒的控制週期中。
DirectionControlOutput：
方向控制函式輸出平滑函式。將方向控制的輸出變化量平 均分配到 2 步 5 毫秒的控制週期中。
MotorOutput：
電機輸出量彙集函式。根據前面的直立控制、速度控制和方向控制所得 到的控制量進行疊加，分別得到左右兩個電極的輸出電壓控制量。對疊加後的輸出量進行飽和處理。函式呼叫週期 5 毫秒。在此請大家注意速度控制量疊加的極性是負。
MotorSpeedOut：
電機 PWM 輸出計算函式。根據左右兩個電極的輸出控制量的正負 極性，疊加上一個小的死區數值，克服車模機械靜態摩擦力。函式呼叫週期 5 毫秒。
SetMotorVoltage：
PWM 輸出函式：根據兩個電機的輸出量，計算出 PWM 控制寄存 器的數值，設定四個 PWM 控制暫存器的數值。函式呼叫週期 1 毫秒。以上 9 個函式都是在 1 毫秒中斷服務中進行被相互呼叫的。下圖顯示了這些函式之間的呼叫與引數傳遞關係。在函式附近也表明了呼叫的週期。
Chaoshengbo：
加入超聲波壁障模組：根據前方障礙物的距離檢測，一旦檢測到後， 透過直接 PWM 值輸出（ g_fchaoshengbooutput），在障礙物反方上運動，無需演算法實現。每30 毫秒呼叫一次。
整機方案
為了實現小車直立行走，需要採集如下訊號：
（1）小車傾角速度陀螺儀訊號，獲得小車的傾角和角速度。
（2） 重力加速度訊號（z軸訊號），補償陀螺儀的漂移。該訊號可以省略，有速度控制替 代。
（3） 小車電機轉速脈衝訊號，獲得小車運動速度，進行速度控制。
（4） 小車轉動速度陀螺儀訊號，獲得小車轉向角速度，進行方向控制。
在小車控制中的直立、速度和方向控制三個環節中，都使用了比例微分（PD）控制，這三種控制演算法的輸出量最終透過疊加透過電機 運動來完成：
（1）小車直立控制：使用小車傾角的PD（比例、微分）控制；
（2）小車速度控制：使用PD（比例、微分）控制；
（3）小車方向控制：使用PD（比例、微分）控制。
可透過微控制器軟體實現上述控制演算法。在上面控制過程中，車模的角度控制和方向控制都是直接將輸出電壓 疊加後控制電機的轉速實現的。而車模的速度控制本質上是透過調節 車模的傾角實現的，由於車模是一個非最小相位系統，因此該反饋控 制如果比例和速度過大，很容易形成正反饋，使得車模失控，造成系 統的不穩定性。因此速度的調節過程需要非常緩慢和平滑。
程式流程圖
AD封裝系列
不用開發板學習STM32系列
必看必讀系列
電子元器件基礎普及系列
點
在
看
，
鼓
勵
一
下
小
編
！！