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