基于卡爾曼濾波的自平衡智能車(chē)控制系統(tǒng)

沈優(yōu) 張占勝 沙楠

摘要：本文設(shè)計(jì)具有循跡功能的平衡車(chē)控制系統(tǒng)，主要針對(duì)兩輪自平衡車(chē)的穩(wěn)定和運(yùn)動(dòng)過(guò)程中的控制問(wèn)題進(jìn)行研究。其中直立系統(tǒng)采用STC8G為核心處理器進(jìn)行控制，使用六軸姿態(tài)傳感器ICM20602進(jìn)行姿態(tài)估計(jì)，通過(guò)卡爾曼濾波算法進(jìn)行姿態(tài)解算，結(jié)合PID控制算法，輸出PWM信號(hào)進(jìn)行電機(jī)控制保持車(chē)體平衡;循跡部分利用攝像頭采集路徑信息，通過(guò)圖像數(shù)據(jù)處理，能夠?qū)崿F(xiàn)自主循跡。

關(guān)鍵詞：STC8;自平衡控制;運(yùn)動(dòng)過(guò)程控制;卡爾曼濾波;PID;循跡

隨著現(xiàn)代控制技術(shù)的發(fā)展，平衡車(chē)在不同領(lǐng)域的應(yīng)用愈加廣泛，所面臨的工作環(huán)境和工作任務(wù)也隨之復(fù)雜。在一些比較狹窄、需要大轉(zhuǎn)角工作場(chǎng)景下，傳統(tǒng)智能車(chē)無(wú)法滿足要求。兩輪自平衡機(jī)器人具有占地面積小、轉(zhuǎn)向半徑小等特點(diǎn)，在災(zāi)難救援，廠區(qū)巡檢、空間探索等領(lǐng)域具有廣闊的應(yīng)用前景。

兩輪自平衡智能車(chē)是現(xiàn)代汽車(chē)研究領(lǐng)域的熱點(diǎn)和難點(diǎn)，如何根據(jù)不同的道路環(huán)境選擇最佳控制方法成為一項(xiàng)新的課題。本文研究的兩輪自平衡車(chē)，通過(guò)攝像頭采集路況信息，運(yùn)用數(shù)據(jù)處理技術(shù)進(jìn)行路況判斷;結(jié)合新一代六軸傳感器獲取姿態(tài)角，用卡爾曼濾波器進(jìn)行姿態(tài)融合。能夠?qū)崿F(xiàn)運(yùn)動(dòng)靈活、自主循跡，為自平衡智能車(chē)設(shè)計(jì)提供新思路。

本設(shè)計(jì)的控制系統(tǒng)主要包括電源模塊、主控模塊、六軸姿態(tài)模塊、攝像頭模塊、電機(jī)驅(qū)動(dòng)模塊以及與上位機(jī)通信的串口模塊。整體硬件結(jié)構(gòu)如圖1所示。

電源模塊主要服務(wù)于三個(gè)部分——主控模塊3.3V，姿態(tài)模塊5V，電機(jī)驅(qū)動(dòng)模塊12V。主控模塊使用宏晶科技出品的STC8G2K系列單片機(jī)，采用51內(nèi)核，該內(nèi)核的優(yōu)點(diǎn)是資源豐富、低功耗、低成本。攝像頭模塊采用OV7725。六軸姿態(tài)模塊采用InvenSense公司推出的ICM20602傳感器，相較于傳統(tǒng)智能車(chē)采用的MPU6050等模塊，ICM20602的優(yōu)勢(shì)在于用硬件SPI進(jìn)行通信，速度快;具有更小的噪聲與溫漂，讀數(shù)穩(wěn)定，抗干擾能力強(qiáng)，模塊原理圖如圖2所示。

智能車(chē)當(dāng)前的姿態(tài)角，基于姿態(tài)模塊提供的3軸加速度、3軸陀螺儀原始數(shù)據(jù)。將原始數(shù)據(jù)的兩軸加速度計(jì)讀數(shù)，進(jìn)行反正切計(jì)算，得到當(dāng)前原始姿態(tài)角：

由于此計(jì)算方法獲取的姿態(tài)角誤差較大，不能實(shí)現(xiàn)穩(wěn)定的自平衡控制。因此，為準(zhǔn)確獲取當(dāng)前姿態(tài)，需將原始角度與陀螺儀讀數(shù)通過(guò)卡爾曼濾波器進(jìn)行波形融合，得到較為精確的姿態(tài)角?？柭鼮V波是從一組有限的，包含噪聲的，對(duì)物體位置的觀察序列（含有偏差）預(yù)測(cè)出物體的位置的去噪聲位置?？柭鼮V波利用原始姿態(tài)角的動(dòng)態(tài)信息，設(shè)法去掉噪聲的影響，得到一個(gè)關(guān)于當(dāng)前姿態(tài)角的好的估計(jì)，得到較為精確的姿態(tài)，提升自平衡系統(tǒng)魯棒性，卡爾曼濾波器工作過(guò)程如圖3所示。

小車(chē)在偏離機(jī)械零位（平衡位置）時(shí)，需要自平衡系統(tǒng)使其恢復(fù)到平衡位置而產(chǎn)生一個(gè)回復(fù)力。小車(chē)回復(fù)力的最初來(lái)源是電機(jī)的轉(zhuǎn)矩，電機(jī)的轉(zhuǎn)矩通過(guò)齒輪傳動(dòng)到車(chē)輪，車(chē)輪與地面接觸，車(chē)輪旋轉(zhuǎn)時(shí)必將與地面有力的產(chǎn)生，作用力與反作用力，地面對(duì)車(chē)輪的摩擦力即是小車(chē)平衡系統(tǒng)回復(fù)力的來(lái)源，系統(tǒng)機(jī)械結(jié)構(gòu)如圖4所示。

在得到較為準(zhǔn)確的姿態(tài)后，利用主控模塊實(shí)現(xiàn)PID控制算法，計(jì)算輸出到電機(jī)的PWM值，實(shí)現(xiàn)自平衡控制。在PID自動(dòng)控制系統(tǒng)中，將卡爾曼濾波器的估計(jì)姿態(tài)與智能車(chē)機(jī)械零點(diǎn)進(jìn)行做差，得到當(dāng)前角度偏差值;基于此偏差利用PD控制，得到當(dāng)前PWM值，進(jìn)行反饋控制，控制程序如圖5所示。

攝像頭模塊OV7725可以實(shí)現(xiàn)模擬圖像轉(zhuǎn)化為數(shù)字圖像的功能，每幅圖像含有160*60個(gè)像素點(diǎn)，每一點(diǎn)對(duì)應(yīng)一個(gè)采樣值。圖像從近向遠(yuǎn)呈現(xiàn)梯形畸變，前端大，后端小。由于圖像以模擬量的形式采集到微處理器中，因此需要利用灰度閾值來(lái)進(jìn)行圖像黑白度區(qū)分，大于閾值為白色，小于閾值為黑色。由于主控模塊資源有限，因此利用OV7725攝像頭的行中斷引腳進(jìn)行圖像采集。圖像經(jīng)過(guò)二值化后，再利用串口模塊向上位機(jī)發(fā)送圖像，可對(duì)圖像進(jìn)行直觀的分析。

伴隨著控制理論的發(fā)展，越來(lái)越多新的控制理論和控制方法被應(yīng)用于智能汽車(chē)的自主循跡控制，兩輪自平衡車(chē)的穩(wěn)定和運(yùn)動(dòng)過(guò)程中的控制問(wèn)題，是移動(dòng)平衡車(chē)在多種環(huán)境下平穩(wěn)運(yùn)行以及自主循跡的基礎(chǔ)。本文研究的基于卡爾曼濾波的自平衡智能車(chē)控制系統(tǒng)，可以實(shí)現(xiàn)運(yùn)動(dòng)靈活、自主循跡。為低功耗、低成本且精度高的自平衡智能車(chē)設(shè)計(jì)提供新的研究方向。

參考文獻(xiàn)：

[1]王子浩，王健.基于STM32的藍(lán)牙平衡小車(chē)系統(tǒng)設(shè)計(jì)[J].電子世界，2020（09）：172-173.

[2]趙翔，杜普選，李虎.基于MEMS加速度計(jì)和陀螺儀的姿態(tài)檢測(cè)系統(tǒng)[J].鐵路計(jì)算機(jī)應(yīng)用，2012.

[3]張磊，蔣剛，肖志峰，等.MEMS陀螺儀與編碼器在機(jī)器人自主定位中的應(yīng)用[J].機(jī)械設(shè)計(jì)與制造2011（9）：

[4]高志偉，代學(xué)武.自平衡小車(chē)LQR-PID平衡與路徑跟蹤控制器設(shè)計(jì)[J].控制工程，2020，27（04）：708-714.

[5]張秦艷，林煒豪，劉春，蔡寧，林雪燕.基于兩輪自平衡小車(chē)的測(cè)控系統(tǒng)課程設(shè)計(jì)教學(xué)改革[J].教育教學(xué)論壇，2020（15）：119-121.

[6]王開(kāi)亮，王琪，秦海亭，高進(jìn)可，朱昱合.基于電磁尋跡的二輪自平衡小車(chē)的設(shè)計(jì)與實(shí)現(xiàn)[J].自動(dòng)化與儀器儀表，2020（01）：77-80.

[7]吳振磊，孫二威，李笑笑.一種基于PID算法的自平衡車(chē)控制系統(tǒng)設(shè)計(jì)[J].甘肅科技縱橫，2019，48（08）：22-24.