1. 項目概述從“會跑”到“站得住”的跨越搞智能車競賽的兄弟們都清楚單車組也叫獨輪車或平衡車組別是整個比賽里技術門檻最高、也最“秀”的一個方向。別的車四個輪子或者兩個輪子著地首要任務是跑得快、拐得穩(wěn)而單車組你的車得先學會“站”起來。這個“站”就是平衡控制。我當年帶隊做這個項目第一周全組人都在跟陀螺儀和加速度計的數據較勁車不是往前“啪”一下拍地上就是往后猛仰過去場面一度十分滑稽。所以今天我們不聊具體代碼和電路就深挖一下這個讓車“站起來”的核心——平衡控制的理論基礎。這就像練武招式代碼可以千變萬化但內功心法理論不通永遠成不了高手。這篇文章就是幫你把這份“內功心法”捋明白讓你知道每一個參數調整背后到底在跟物理世界的哪些規(guī)律較勁。2. 核心需求解析平衡的本質是什么在深入數學公式之前我們得先想明白我們要解決的到底是個什么問題。一輛兩輪自平衡車你可以把它想象成一個倒立擺。一根桿子車身底下用一個輪子或兩個并排很近的輪子等效為一個支點支撐著。這個系統(tǒng)天然是不穩(wěn)定的就像你用手指頂著一根棍子稍微一偏重力就會把它拉倒。我們的目標就是通過控制底部的輪子前后移動來對抗重力的影響讓這根“棍子”車身始終保持豎直或者穩(wěn)定在一個我們期望的傾斜角度上。所以平衡控制的核心需求可以分解為兩個層次靜態(tài)平衡讓車在靜止狀態(tài)下保持直立。這是最基本也是最難的一步因為任何微小的擾動如地面不平、電機微小的力矩波動都會破壞平衡。動態(tài)平衡在靜態(tài)平衡的基礎上讓車能夠按照我們的指令前進、后退、轉彎。這時平衡控制不僅要抵抗重力還要處理因運動產生的慣性力、離心力等額外的干擾。為了實現這些我們需要三樣東西感知知道車現在“歪”了多少、決策計算需要讓輪子怎么動來糾正這個“歪”、執(zhí)行驅動電機產生準確的力矩。理論分析主要聚焦在“決策”環(huán)節(jié)即如何根據“感知”到的姿態(tài)信息計算出“執(zhí)行”機構需要的控制量。3. 動力學建模把物理世界“翻譯”成數學方程理論分析的第一步是建模。我們需要用數學方程來描述這個倒立擺系統(tǒng)的運動規(guī)律。這是整個控制理論的基石模型建得準后面的控制器設計才有意義。常見的建模方法有牛頓-歐拉法和拉格朗日法。對于智能車這樣的簡單系統(tǒng)牛頓-歐拉法更直觀。3.1 模型簡化與假設為了抓住主要矛盾我們通常先建立一個簡單的平面模型只考慮車子在前進方向上的俯仰運動忽略側傾和橫擺并做以下合理假設車身和輪子都是剛體。電機驅動輪子且電機響應足夠快我們可以認為控制輸入直接是輪子的轉矩。車體繞輪軸的轉動慣量、質量分布等參數可以通過測量或估算得到。3.2 建立運動方程我們設θ車體與豎直方向的夾角俯仰角。θ0表示完全直立前傾為正后仰為負。x輪子中心點的水平位移。τ電機作用在輪子上的轉矩控制輸入。M,m分別為輪子質量和車體質量。R,L分別為輪子半徑和車體質心到輪軸的距離。I_w,I_b分別為輪子繞其軸心的轉動慣量和車體繞其質心的轉動慣量。通過對輪子和車體分別進行受力分析并應用牛頓第二定律和轉動定律我們可以推導出描述θ和x的兩個耦合的非線性微分方程。這個過程涉及一些矢量運算和受力分解是理論分析中最“硬核”的部分。最終得到的方程形式大致如下(M m) * x? m*L*cosθ * θ? - m*L*sinθ * (θ?)2 (τ / R) (m*L*cosθ) * x? (I_b m*L2) * θ? - m*g*L*sinθ -τ這里x?和θ?分別是位移和角度的二階導即加速度和角加速度θ?是角速度的一階導。g是重力加速度。這兩個方程清晰地展示了系統(tǒng)的非線性存在sinθ,cosθ,(θ?)2項和耦合性x?和θ?相互影響。注意很多同學看到這一堆符號和方程就頭大。其實關鍵在于理解其物理意義第一個方程本質是輪子水平方向的“力平衡”控制轉矩τ轉化為了輪子前進的力。第二個方程是車體繞輪軸的“力矩平衡”重力m*g*L*sinθ是導致傾倒的主要力矩而電機轉矩τ和輪子加速度帶來的慣性力共同作用來平衡它。耦合項m*L*cosθ * θ?意味著車體的轉動會影響到輪子的平動反之亦然。3.3 線性化處理上面的非線性方程直接用來設計控制器非常困難。好在對于平衡控制我們主要關心的是θ在0度直立點附近的小范圍運動。此時我們可以應用小角度假設當θ很小時sinθ ≈ θcosθ ≈ 1并且(θ?)2作為高階小量可以忽略。這樣復雜的非線性方程就被簡化成了線性的狀態(tài)空間方程形式如下狀態(tài)向量 X [x, θ, x?, θ?]? 狀態(tài)方程X? A * X B * u 輸出方程Y C * X D * u其中u就是我們的控制量τ。矩陣A和B中的元素全部由系統(tǒng)的物理參數M, m, L, I等構成變成了常數。這一步線性化是至關重要的它使得我們可以運用成熟且強大的線性系統(tǒng)控制理論如PID控制、狀態(tài)反饋、LQR等來系統(tǒng)性地設計控制器。4. 控制器設計PD控制的登場與局限對于剛入門的隊伍最直接、最常用的控制器就是PD控制器比例-微分控制。它不依賴于精確的數學模型屬于“經典控制理論”的范疇靠的是經驗和調試。4.1 PD控制原理PD控制器針對的是單輸入單輸出系統(tǒng)。在我們的平衡控制中輸入是期望的直立角度通常為0輸出是測量得到的實際角度θ?？刂破鞲鶕`差e θ_desired - θ這里θ_desired0來生成控制量電機PWM占空比或轉矩指令。其控制律為u Kp * e Kd * (de/dt)比例項 (Kp)與當前角度誤差成正比。車體往哪邊倒就命令輪子往哪邊加速以產生一個恢復力矩。Kp越大糾正的力度越強反應越快。但過大容易引起超調和振蕩。微分項 (Kd)與角度誤差的變化率即角速度θ?成正比。它起到“阻尼”的作用能夠預測車的傾倒趨勢。當車體正在快速傾倒時微分項會產生一個反向力矩來抑制這個速度防止系統(tǒng)沖過頭而振蕩。這是平衡控制中不可或缺的一項沒有微分項系統(tǒng)幾乎不可能穩(wěn)定。在實際單片機編程中de/dt通常由陀螺儀輸出的角速度信號直接獲得這比用角度差分更準確、噪聲更小。4.2 PD參數整定經驗調參是個手藝活但有一些基本準則先調Kd后調Kp這是平衡車調試的黃金法則。先將Kp設為0逐漸增大Kd。你會發(fā)現車體受到擾動后會緩慢地倒下而不是“啪”地一下砸下去。這說明微分阻尼開始起作用了。加入Kp在有一個基礎Kd的前提下慢慢增加Kp。你會看到車開始嘗試“抵抗”傾倒。當Kp增加到某個臨界點車可能會開始高頻小幅振蕩“發(fā)抖”。精細調整此時需要回調Kp或微增Kd來抑制振蕩尋找一個既能快速響應又能平穩(wěn)站立的點。用手輕輕推一下車它應該能迅速回正且最多有1-2次衰減振蕩。注意飽和電機的輸出力矩是有限的PWM有上下限。如果Kp或Kd過大計算出的控制量u很容易達到飽和限幅此時系統(tǒng)會進入非線性區(qū)性能急劇下降甚至失穩(wěn)。調試時一定要在代碼中加入輸出限幅。實操心得不要指望一次就能調好。環(huán)境變化如電池電壓下降、地面摩擦系數不同、傳感器噪聲都會影響最佳參數。我們當時的做法是調出一組在實驗室光滑地板上能穩(wěn)定站立的參數后會在這組參數周圍設置幾組微調值通過撥碼開關或藍牙指令能在比賽現場快速切換以應對不同的賽道材質如瀝青、環(huán)氧地坪。4.3 PD控制的局限性PD控制器簡單有效但它存在幾個固有缺陷無法消除靜差對于平衡控制我們的期望角度是0理論上不存在靜差問題。但如果引入速度控制想讓車在平衡的同時以某個速度巡航純PD就會產生靜差。應對復雜動態(tài)能力有限PD只關注“角度”和“角速度”兩個狀態(tài)。而我們的系統(tǒng)實際上有四個狀態(tài)變量位置x、角度θ、速度x?、角速度θ?。當車需要同時進行平衡和位置控制比如停在指定點時PD控制器難以協(xié)調容易顧此失彼?？垢蓴_性依賴參數對于大幅值或持續(xù)性的外部干擾如強風、持續(xù)坡道PD控制的恢復能力完全取決于Kp和Kd的取值沒有最優(yōu)保證。正因為這些局限性在追求更高性能如更快的響應、更強的抗擾、同時實現平衡與循跡時我們就需要更高級的控制策略。5. 進階之路狀態(tài)空間與LQR控制當PD控制無法滿足你的性能要求或者你想從原理上更深刻地掌控你的車時狀態(tài)空間方法和線性二次型調節(jié)器LQR是一個優(yōu)雅而強大的工具。它直接基于我們第3節(jié)中建立的線性化狀態(tài)空間模型。5.1 狀態(tài)反饋控制核心思想是既然我們的系統(tǒng)狀態(tài)X [x, θ, x?, θ?]?包含了所有描述系統(tǒng)動態(tài)的信息那么最直接的控制方式就是根據所有這些狀態(tài)變量的組合來決定控制輸入。這被稱為狀態(tài)反饋控制律為u -K * X其中K [k1, k2, k3, k4]是一個反饋增益矩陣。k1對應位置x的權重k2對應角度θ的權重k3對應速度x?的權重k4對應角速度θ?的權重。這個負號表示負反饋。你會發(fā)現PD控制器其實是狀態(tài)反饋的一個特例它只用了θ對應k2和θ?對應k4這兩個狀態(tài)而忽略了位置x和速度x?即k10 k30。5.2 LQR如何自動找到最優(yōu)的K矩陣現在問題來了K矩陣有四個增益參數手動調試的復雜度呈指數上升。如何科學地確定一組“最優(yōu)”的增益LQR就是為了解決這個問題。LQR的思想是定義一個衡量系統(tǒng)性能“好壞”的二次型代價函數J ∫(X? * Q * X u? * R * u) dt這個積分從0到無窮大時間。其中X? * Q * X懲罰狀態(tài)偏離平衡點0點的程度。Q是一個半正定矩陣對角線元素q11, q22, q33, q44分別代表我們對位置、角度、速度、角速度的重視程度。值越大說明我們越不希望該狀態(tài)量偏離。u? * R * u懲罰控制量的大小。R是一個正數單輸入系統(tǒng)代表我們對控制能耗的重視程度。R越大說明我們越希望使用小的控制量避免電機飽和。LQR理論證明存在一個唯一的反饋增益矩陣K能夠最小化這個代價函數J。而這個K可以通過求解一個叫做Riccati的代數方程得到。幸運的是在MATLAB、PythonControl庫甚至一些在線工具中都有現成的函數如lqr(A, B, Q, R)可以一鍵求解。5.3 LQR在智能車上的應用實踐確定模型(A, B)矩陣這是最關鍵也最容易出錯的一步。你需要相對準確地測量或估算出小車的質量、長度、轉動慣量等物理參數代入第3節(jié)線性化后的公式計算出A和B矩陣。參數不準得到的K效果會大打折扣。設計Q和R矩陣這是體現你控制“偏好”的地方。例如如果你只關心平衡不關心車的位置漂移可以把q11位置權重和q33速度權重設得很小甚至為0同時給q22角度權重和q44角速度權重設較大的值。如果你希望車在平衡的同時還能穩(wěn)穩(wěn)地停在某一點位置伺服就需要給q11和q33賦予較大的權重。R的取值需要權衡。R太小計算出的K增益會很大可能導致控制量飽和R太大控制器過于“溫柔”響應慢。通常從1開始嘗試根據仿真或實際效果調整。仿真驗證在將K矩陣燒錄進小車前務必在Simulink、Python或任何仿真環(huán)境中搭建模型進行測試。觀察系統(tǒng)的階躍響應、抗干擾能力檢查控制量是否飽和。實物調試與微調由于模型誤差和現實噪聲理論計算出的最優(yōu)K在實車上不一定是最佳的。但它提供了一個極好的起點。你可以以此為基礎對K中的各個增益進行小幅度的整體縮放或微調效率遠高于盲調PD參數。注意事項LQR控制要求能獲取全部四個狀態(tài)量。x和θ可以通過編碼器和IMU慣性測量單元直接或間接測得。x?和θ?可以通過對x和θ微分得到但微分會放大噪聲。更好的方法是使用狀態(tài)觀測器如卡爾曼濾波器它能夠利用模型信息和傳感器數據最優(yōu)地估計出無法直接測量或測量噪聲大的狀態(tài)如速度這是更高階的玩法。6. 實操中的核心環(huán)節(jié)與問題排查理論再完美落地到一塊單片機和一堆傳感器上時總會遇到各種妖魔鬼怪。下面分享幾個最關鍵環(huán)節(jié)的實操要點和常見問題。6.1 傳感器數據融合姿態(tài)解算的基石平衡控制的前提是獲得準確、實時、低延遲的車體傾角θ和角速度θ?。這主要依賴IMU通常包含三軸加速度計和三軸陀螺儀。加速度計測量的是比力靜態(tài)時可反映重力方向從而計算出傾角。但對振動和運動加速度極其敏感車一動數據就“飄”了。陀螺儀測量角速度積分可得角度。動態(tài)響應好無運動加速度干擾但存在零漂和積分累積誤差時間一長角度就“飛”了。因此必須進行數據融合。最經典的方法是互補濾波其思想簡潔而有效角度估計 α * (陀螺儀積分角度) (1-α) * (加速度計計算角度)其中α是一個介于0和1之間的濾波系數通常取0.98左右。它的物理意義是在高速動態(tài)時相信陀螺儀在低速靜態(tài)時相信加速度計。在單片機中實現一個一階互補濾波器代碼量小效果顯著是初學者的首選。對于追求更高精度和穩(wěn)定性的隊伍會采用卡爾曼濾波。它將系統(tǒng)模型和傳感器噪聲統(tǒng)計特性結合起來提供最優(yōu)估計。雖然數學模型復雜但現在有很多開源庫如SimpleKalmanFilter可以簡化實現。6.2 控制周期與實時性平衡控制是一個對實時性要求極高的閉環(huán)系統(tǒng)。從讀取傳感器、解算姿態(tài)、計算控制量到輸出PWM必須在極短的時間內完成?？刂浦芷诮ㄗh至少達到5ms200Hz最好能達到2ms500Hz。周期越短系統(tǒng)對擾動的響應越快控制越“跟手”。定時器中斷務必使用定時器中斷來嚴格觸發(fā)控制循環(huán)保證周期恒定。避免使用delay()或非阻塞循環(huán)它們會引入不確定的延遲。傳感器讀取優(yōu)先選擇支持SPI或高速I2C的IMU芯片并確保在主循環(huán)中能以高于控制頻率的速度讀取數據。對于MPU6050這類常用芯片開啟DMP數字運動處理器單元可以減輕主控的運算負擔。6.3 常見問題排查速查表現象可能原因排查思路與解決方法車體高頻抖動“發(fā)抖”微分增益Kd過大傳感器噪聲大控制周期不穩(wěn)定。1. 適當減小Kd。2. 檢查并優(yōu)化姿態(tài)解算濾波算法增加濾波強度。3. 確保使用定時器中斷并檢查中斷內代碼執(zhí)行時間是否超時。車體緩慢單向傾倒比例增益Kp過小加速度計零點漂移未校準機械重心不在輪軸正上方。1. 適當增大Kp。2. 上電靜止時采集加速度計數據求平均值作為零點偏移量在程序中減去。3. 檢查機械結構盡量降低重心并保證左右、前后對稱。響應遲鈍推一下恢復慢Kp和Kd都偏小電機驅動響應慢死區(qū)大、頻率低。1. 同時增大Kp和Kd注意保持比例避免振蕩。2. 檢查電機驅動橋的響應提高PWM頻率通常8kHz-20kHz減小死區(qū)設置。啟動瞬間“彈跳”或倒下初始角度讀取錯誤控制器輸出初始值不為零。1. 系統(tǒng)上電后保持車身直立靜止1-2秒讓濾波器收斂再啟動控制循環(huán)。2. 確?？刂屏枯敵龀跏蓟禐?且從0開始平滑使能。直線行走時嚴重跑偏左右輪子直徑、摩擦力或電機響應不一致陀螺儀Z軸零漂。1. 精細校準左右編碼器脈沖當量。2. 增加一個小的轉向環(huán)PID根據航向角偏差微調左右輪速差。3. 定期如每N個控制周期更新陀螺儀零偏值。6.4 從平衡到運動速度環(huán)與轉向環(huán)的引入當車能穩(wěn)定站立后下一步就是讓它動起來。這需要引入外環(huán)控制速度環(huán)給定一個目標速度可能是0即定車也可能是前進速度將當前速度由編碼器測得與目標速度比較其誤差經過一個PI控制器注意這里需要積分項I來消除靜差輸出一個目標角度。這個目標角度再輸入給內層的平衡環(huán)PD或LQR。也就是說你想讓車加速前進實際上是讓車“故意”前傾一個角度平衡控制器為了維持這個前傾角度就會驅動輪子向前加速從而產生速度。這是一個非常巧妙且符合人體直覺的設計人想騎車前進也是先讓身體前傾。轉向環(huán)通過陀螺儀的Z軸角速度或磁力計的航向角來獲取當前轉向率或航向。給定目標轉向指令如來自攝像頭識別的路徑偏差通過一個PD控制器計算出左右輪的速度差從而實現轉向。至此一個完整的、具備平衡、速度、轉向控制能力的智能車單車組控制系統(tǒng)框架就搭建起來了。理論分析為你提供了設計這個框架的藍圖和工具而不斷的調試、觀察和思考則是將藍圖變?yōu)楝F實的關鍵。