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