先記住一句話，直立環和速度環的結合是保持小車長期穩定平衡的前提。相信大家讀到最後一定可以明白這句話的含義。

大家都玩過的用手控制木棍直立不倒的遊戲。這是一個通過眼睛觀察木棒的傾角和傾斜趨勢（角速度），通過手指的移動去抵消木棒傾斜的角度和傾斜的趨勢使木棒能直立不倒的一個過程。典型的負反饋機制。

平衡小車也是如此，小車運動方向與傾斜方向一緻，通過測量小車的傾角和傾角速度，進而通過控制小車車輪的加速度來消除小車的傾角和傾角速度，這樣理論上不考慮電機極限的情況下，小車就可以平衡。

理想情況下，控制電機的加速度和小車傾角成正比，可以讓小車保持平衡，就是小車向傾斜的方向運動，傾斜角度越大，小車運動越快。a = k p ∗ θ a=kp*θa=kp∗θ但實際上，小車剛體繞軸旋轉時具有慣性，當小車到達傾角為零時，a輸出零，因為慣性的存在，小車還是會向另一個方向倒去。如此反複，小車将一直振蕩無法靜止。

如何消除小車繞軸轉動的慣性，也就是消除小車的轉動慣量，使小車盡可能的靜止呢？

首先分析轉動慣量，公式如下：I = ∑ i m i r i 2 I=\sum_{i}m_i{r_i}^2I=i∑mi​ri​2其中m是質量，r是質點到轉軸的垂直距離，m和r都無法為零，所以轉動慣量客觀存在。

既然不能使轉動慣量為零，換個角度，是不是可以抵消轉動慣量呢？

這裡分析一下單擺模型吧。

由于重力的作用，單擺受到和單擺角度成正比與運動方向相反的回複力；類比電機的加速度

由于空氣的作用，單擺又受到和單擺運動速度成正比與運動反向相反的阻尼力，使單擺最終平衡下來。

有沒有啟發，為了讓小車能近似靜止，還需要一個阻尼力，這個阻尼力和小車的傾角速度成正比。

所以，小車的平衡控制算法改寫為：a = k p ∗ θ k d ∗ θ ⋅ a=kp*θ kd*θ^·a=kp∗θ kd∗θ⋅一個是角度偏差，另一個是角度偏差的變化率，也就是角速度。為了簡化控制，a的輸出直接作為直流減速電機的PWM替代。這裡引入PID，關于PID的理論就不講了，網上大佬巨多。上面的公式就是一個位置式離散PD控制器。負反饋機制。

關于直立PD控制器的參數整定，也就公式中角度和角速度的參數整定，這裡根據公式原理也可以得到一些調試技巧。

比例控制：回複力

微分控制：阻尼力抵消轉動慣量

微分系數與轉動慣量有關，根據轉動慣量和力矩的公式：I = ∑ i m i r i 2 I=\sum_{i}m_i{r_i}^2I=i∑​mi​ri​2

M = ∑ i F i L i M=\sum_iF_iL_iM=i∑​Fi​Li​

小車質量M固定的情況下，重心越高（r、L越大），需要的回複力F越小，所以比例系數kp變小；轉動慣量I變大，所以微分系數kd變大。

小車重心（r、L）固定的情況下，質量M增大，需要回複力增大，所以比例系數kp增大；轉動慣量I變大，所以微分系數kd變大。

往往，我們隻使用直立環，小車隻能短暫的平衡幾秒，然後就朝着一個方向倒去了。

具體原因要從具體的平衡控制結構來分析。我們使用的傳感器是單一的MPU6050，通過DMP庫獲取數據，輸出的是角度以及角速度信号，而我們控制的是電機，不考慮電機的感受（也就是電機的速度幅值），隻要求平衡車的角度能夠保持絕對平衡。

隻有直立環的平衡小車會倒下的原因：

那麼假如小車現在平衡狀态，控制閉環出現了微小的幹擾，平衡車就會有一個方向的加速度，而此時小車角度平衡，小車平移速度沒有限制，這樣就可能超過PWM的幅值，導緻電機無法加速了，也就沒有了有效的回複力，所以小車就倒了。

所以速度環出現了，在小車平衡的同時，盡量讓小車保持靜止，就是小車速度為零。也就是對小車平移速度進行限制。

平衡小車速度環的原理：

根據經驗知道，小車的運行速度和小車的傾角是相關的，比如小車前傾，是不是直立環要使小車向前加速，讓小車保持平衡，此時小車不就有速度了嘛。如果對着速度進行閉環控制，那麼這個環不就是速度環了嘛。如果将速度環的目标設置為0，小車不就可以長期穩定平衡了嘛。

大家之前了解的電機速度PID基本上都是負反饋，但是在以直立為主的平衡車系統中，負反饋的速度控制将會導緻小車加速到下。這裡我們通過讓小車保持一定的角度，用小車平衡的過程，表示速度閉環的控制。我們可以通過串級PID的方式，如下圖所示：

根據上面的原理圖，我們把速度和直立兩個控制器串聯起來，速度環的輸出作為直立環的輸入，直立環的輸出就是系統的輸出，這樣就可以保持平衡。

公式如下：a 1 = k p 1 ∗ e ( k ) k i 1 ∗ ∑ e ( k ) a_1=kp_1*e(k) ki_1*\sum{e(k)}a1​=kp1​∗e(k) ki1​∗∑e(k)a = k p ∗ ( θ − a 1 ) k d ∗ θ ⋅ a=kp*(θ-a_1) kd*θ^·a=kp∗(θ−a1​) kd∗θ⋅

這樣把速度控制系統輸出看做是一個角度，原本要保持角度為零就變成保持一個小的角度a1，也就是上面說過的平衡小車速度環的原理。

所以我們就有了一個這樣的算法，兩個式子合并。a = k p ∗ θ k d ∗ θ ⋅ − k p [ k p 1 ∗ e ( k ) k i 1 ∗ ∑ e ( k ) ] a=kp*θ kd*θ^·-kp[kp_1*e(k) ki_1*\sum{e(k)}]a=kp∗θ kd∗θ⋅−kp[kp1​∗e(k) ki1​∗∑e(k)]進行轉化演變：

将上面的算法，轉化成為：一個單獨的負反饋的直立環 一個單獨的正反饋的速度環，也就是大家常說的平衡車的速度環是正反饋的由來。

如果大家看完上面的内容已經躍躍欲試了，下面的圖幫你整理了制作一個平衡小車需要準備什麼硬件？需要完成具體的任務？軟件實現步驟？

關于初級玩具平衡小車的主控部分的電路模塊有以下參考，圖片來源淘寶。

如果對硬件不是很感冒的同學，完全可以購買配件組裝，或者買個成品車，重在學習控制算法嘛。

假如現在你已經擁有一輛平衡小車了，在完成電機的PWM控制、編碼器測速、MPU6050數據讀取之後，我們就開始了平衡小車PD直立環的設計。如果沒完成可以看我之前的文章。

之前的文章鍊接

電機的PWM控制：

電機編碼器測速：

MPU6050數據讀取：

單獨PD直立環最終效果：使小車能夠穩定一小段時間。原因上面講過，電機速度存在極限。

PD直立環的代碼：

/************************************************************************** 函數功能：直立環PD控制 入口參數：角度、角速度 返回 值：直立控制PWM **************************************************************************/ int balance(float Angle,float Gyro)//roll(橫滾角)（回複力）、角速度（阻尼力） { float Bias; float Balance_Kp=700,Balance_Kd=2.0;//600 kp(0 - 720) kd( 0 - 2 ) // float Balance_Kp=0,Balance_Kd=0; //===調試用 int balance; Bias=Angle-ZHONGZHI; //===求出平衡的角度中值 和機械相關 balance=Balance_Kp*Bias Gyro*Balance_Kd; //===計算平衡控制的電機PWM PD控制 kp是P系數 kd是D系數 return balance; } 123456789101112131415

PD直立環參數：

Kp增加回複力的響應速度

Kp過小，響應太慢，不能達到直立。

Kp過大，會出現大幅度低頻振蕩。

Kd消除Kp過大帶來的低頻振蕩。

Kd過大，會出現高頻抖動。

入口參數：

int balance(float Angle,float Gyro) 1

如果小車是繞MPU6050的x軸旋轉，Angle是roll值（橫滾角）， Gyro是對應的角速度

如果小車是繞MPU6050的y軸旋轉，Angle是pitch值（橫滾角），Gyro是對應的角速度

機械中值：

需要調整，每輛車因為機械原因，可能都不一樣

機械中值也就是代碼中參數ZHONGZHI，這個值根據自己MPU6050的安裝位置而定。

如果小車是繞MPU6050的x軸旋轉，ZHONGZHI是前傾和後仰臨界點的roll值（橫滾角）

如果小車是繞MPU6050的y軸旋轉，ZHONGZHI是前傾和後仰臨界點的pitch值（俯仰角）

直立環的Kp範圍确定：

如果你的電機PWM=7200為滿幅，即占空比100%。

如果kp=720，角度偏差10度，電機就到達滿轉了。我們不會容忍小車偏差10度以上，所以kp的範圍（0 - 720），但是具體情況具體分析。

直立環的kp極性确定：

這個和自己的PWM設定方向、角度方向有關，但是我們可以不考慮這些。

我們直接看現象，如果設定kp=-500，小車加速倒下，那麼kp的極性為負，當kp=500的時候，可以盡可能維持平衡。

直立環的kp大小确定：

首先，将直立環kd和速度環、轉向環的參數設置為0。

然後，試湊法 二分法，從小到大調到小車出現大幅度低頻振動，确定kp。不用糾結參數調試，有一個感覺好的就用，找到最好的很不容易，浪費時間。

直立環的kd極性确定：

這個和自己的PWM設定方向、角速度方向有關，但是我們可以不考慮這些。

我們直接看現象，如果設定kp=0，設置kd=-2，小車加速倒下，那麼kd的極性為負，當kd=2的時候，可以盡可能維持平衡。

直立環的kd大小确定：

在之前直立kp的基礎上，試湊法 二分法，從小到大調試kd大小，一直到小車出現高頻的劇烈抖動。

根據工程經驗，将直立環的kp和kd同時乘以0.6，就是我們最終要的直立環的參數。

PD直立環 PI速度環的效果：使小車長期的平衡下來。

終極效果：幾乎靜止不動，但是沒必要花時間調試，重在學習理論和實踐過程。

根據上面的控制理論，我們将小車的控制系統劃分為負反饋的PD直立環 正反饋的PI速度環。

平衡小車，顧名思義平衡為主，其他為輔，為了讓直立環處于主導地位，對速度環進行适當的低通濾波處理，也就是上面理論部分的公式，對速度環的kp處理。

PI速度環的代碼：

/************************************************************************** 函數功能：速度環PI控制 修改前進後退速度 入口參數：左輪編碼器、右輪編碼器 返回 值：速度控制PWM **************************************************************************/ int velocity(int encoder_left,int encoder_right) { static float Velocity=0,Encoder_Least=0,Encoder=0,Movement=0; static float Encoder_Integral=0; float Velocity_Kp=-180,Velocity_Ki=Velocity_Kp/200.0;//-185 -0.925 // float Velocity_Kp=0, Velocity_Ki=Velocity_Kp/200.0; //===調試用 //=============速度PI控制器=======================// Encoder_Least =(encoder_left encoder_right)-0; //===獲取最新速度偏差==測量速度（左右編碼器之和）-目标速度（此處為零） Encoder *= 0.8; //===一階低通濾波器 Encoder = Encoder_Least*0.2; //===一階低通濾波器 Encoder_Integral =Encoder; //===積分出位移 積分時間：5ms Encoder_Integral=Encoder_Integral-Movement; //===接收遙控器數據，控制前進後退 if(Encoder_Integral>10000) Encoder_Integral=10000; //===積分限幅 if(Encoder_Integral<-10000) Encoder_Integral=-10000; //===積分限幅 Velocity = Encoder*Velocity_Kp Encoder_Integral*Velocity_Ki; //===速度控制 return Velocity; } 1234567891011121314151617181920212223

入口參數：

int velocity(int encoder_left,int encoder_right) 1

首先，encoder_left、encoder_right代表的是左右電機速度量的意義。

實際上，encoder_left、encoder_right為單位時間内（比如5ms），左右電機編碼器分别的變化量，通過定時器編碼器模式采集，可正可負。

Encoder_Least =(encoder_left encoder_right)-0; 1

上面一句代碼是算法中求速度偏差的，有些同學不理解為什麼是encoder_left encoder_right表示當前小車速度。關于小車速度有這樣一個公式s p e e d = ( l e f t S p e e d r i g h t S p e e d ) / 2 speed=(leftSpeed rightSpeed)/2speed=(leftSpeed rightSpeed)/2這裡隻是沒有除以2而已，仔細想想，有必要除以2嗎？當然是沒有必要，我們調試的最終參數是參與量化的。

速度環的Kp範圍确定：

通過實驗測得，小車滿速時，單位時間内（5ms）小車的左右編碼器變化之和可達100，（不同的電機可能不一樣），假定速度偏差達到50%電機滿轉。

100/2=50,7200/50=144，也就是kp最大144，但是這裡假設速度偏差達到50%電機滿轉的情況，所以具體情況具體分析。

速度環的Kp極性确定：

正反饋的效果，是把情況變得更劇烈。

這裡先關閉直立環和轉向環，設置kp=-80，用手轉動一側電機，如果兩個電機開始向相同的方向加速，即為正反饋過程，若不是kp為正值。

速度環的kp大小确定：

首先，打開之前調好的直立環。

試湊法 二分法，從小到大調整kp，(公式中提到ki=kp/200)，直到使得小車的響應迅速又穩定，此時，平衡小車的第一部分就調試結束了，小車可以長期穩定的直立了。

到這裡，極簡版的直立平衡小車的理論和實踐部分就結束了，個人感覺，這篇文章不像其他的文章隻介紹如何實現一個平衡小車。這裡從始至終都在向大家傳達直立環、速度環的原理、内在機制，這樣就可以更好的了解一個系統。

更多精彩资讯请关注tft每日頭條，我们将持续为您更新最新资讯!