基于卡爾曼濾波算法自平衡智能車(chē)系統(tǒng)設(shè)計(jì)

孫長(zhǎng)偉 唐斌 王志強(qiáng)

摘 要：基于卡爾曼濾波算法自平衡智能車(chē)系統(tǒng)設(shè)計(jì)，以STM32F103為控制核心，采用卡爾曼濾波算法和PID算法。該設(shè)計(jì)提高了平衡車(chē)傾角的計(jì)算精度和平衡車(chē)的穩(wěn)定性。通過(guò)系統(tǒng)調(diào)試和分析，獲得了適合平衡車(chē)運(yùn)行的各項(xiàng)系統(tǒng)參數(shù)，運(yùn)行結(jié)果表明該平衡車(chē)運(yùn)動(dòng)平穩(wěn)，適應(yīng)性強(qiáng)。

關(guān)鍵詞：STM32F103;卡爾曼濾波;PID 控制算法;智能平衡車(chē);控制系統(tǒng)

引言

兩輪自平衡小車(chē)是一個(gè)綜合復(fù)雜的系統(tǒng)，集動(dòng)態(tài)決策和規(guī)劃、環(huán)境感知、行為控制和執(zhí)行等多種功能于一體[1]。STM32單片機(jī)作為控制單元，陀螺儀、加速度計(jì)、光電編碼器和電流檢測(cè)等傳感器采集相關(guān)的數(shù)據(jù)信息[2]，基于卡爾曼濾波算法和PID算法，實(shí)現(xiàn)平衡車(chē)的自動(dòng)直立與快速運(yùn)動(dòng)[3]。

1 平衡智能車(chē)原理

平衡智能車(chē)的動(dòng)力輸出來(lái)自車(chē)身底部的兩路直流電機(jī)與車(chē)輪之間運(yùn)動(dòng)，根據(jù)電機(jī)之間不同轉(zhuǎn)速引起車(chē)輪之間不同轉(zhuǎn)速控制智能車(chē)的直立不同、速度不同、方向不同。智能車(chē)車(chē)體的控制任務(wù)可分解成三個(gè)基本任務(wù)：

（1）控制車(chē)模平衡：電機(jī)的正向反向運(yùn)動(dòng)控制保持車(chē)模的直立平衡狀態(tài);

（2）控制車(chē)模速度：車(chē)模的傾角的調(diào)節(jié)控制車(chē)模速度，實(shí)質(zhì)通過(guò)控制電機(jī)的轉(zhuǎn)速來(lái)實(shí)現(xiàn)車(chē)輪速度的控制。

（3）控制車(chē)模方向：通過(guò)控制兩個(gè)電機(jī)之間的轉(zhuǎn)速差控制車(chē)模轉(zhuǎn)向 [4]。

智能車(chē)控制采用的核心算法是PID算法。平衡小車(chē)速度控制系統(tǒng)由負(fù)反饋PD 直立控制器和正反饋PI 速度控制器組成[5]。

在直立控制中，只要一產(chǎn)生角度偏差信號(hào)，在負(fù)反饋的作用下角度偏差最終被消除，小車(chē)保持直立狀態(tài);在速度控制中，一旦檢測(cè)到角度偏差信號(hào)，即明白小車(chē)要產(chǎn)生速度的變化，在正反饋的速度控制器作用下，速度會(huì)朝著期望方向上逐漸增加，以達(dá)到速度控制的目的。

2 系統(tǒng)總體方案設(shè)計(jì)

系統(tǒng)由五部分組成：中央處理部分，電源管理部分，運(yùn)動(dòng)控制部分，數(shù)據(jù)感知部分，人機(jī)交互部分。

中央處理部分，作為整個(gè)系統(tǒng)的核心部分，主要功能是數(shù)據(jù)運(yùn)算與運(yùn)動(dòng)控制，對(duì)其他部分進(jìn)行實(shí)時(shí)工作監(jiān)測(cè)，采集系統(tǒng)運(yùn)行的必要數(shù)據(jù)，反饋系統(tǒng)整個(gè)運(yùn)動(dòng)控制。電源管理部分，作為整個(gè)系統(tǒng)的能量來(lái)源部分，主要功能是提供電源與電池保護(hù)，系統(tǒng)中不同器件存在多種不同電壓要求，需要不同方式的電源輸出模塊。運(yùn)動(dòng)控制部分，主要功能為提供智能車(chē)運(yùn)動(dòng)動(dòng)力，主要有直流電機(jī)與電機(jī)驅(qū)動(dòng)模塊多個(gè)子部分組成。數(shù)據(jù)感知部分，作為系統(tǒng)的所有數(shù)據(jù)來(lái)源渠道，其提供了車(chē)體姿態(tài)的角度，運(yùn)動(dòng)過(guò)程車(chē)身速度與目標(biāo)物體空間距離等多個(gè)系統(tǒng)運(yùn)行起來(lái)不可缺少的數(shù)據(jù)。人機(jī)交互部分，由于平衡智能車(chē)設(shè)計(jì)與使用過(guò)程，存在相當(dāng)多的數(shù)據(jù)調(diào)節(jié)，利用本部分實(shí)現(xiàn)與人的良好的交互功能，本部分包含液晶顯示模塊，按鍵輸入模塊，無(wú)線通信模塊等等。

3 系統(tǒng)硬件結(jié)構(gòu)設(shè)計(jì)

3.1 單片機(jī)最小系統(tǒng)

控制系統(tǒng)選用 STM32單片機(jī)為控制器，該控制器是具備并行運(yùn)算功能，適用于交復(fù)雜環(huán)境下的系統(tǒng)控制[6]。

3.2姿態(tài)采集模塊

姿態(tài)采集模塊包含加速度傳感器和角速度傳感器兩部分。加速度傳感器MMA7260，其外圍電路如圖1所示。MMA7260是一款基于重力分量換算原理的低功耗高靈敏度的加速度傳感器，同時(shí)輸出x，y，z三個(gè)方向的加速度值，用于測(cè)量平衡車(chē)的運(yùn)動(dòng)姿態(tài)和方向。角速度傳感器，采用陀螺儀ENC-03，其輸出的模擬電壓信號(hào)與角速度成正比，將該角速度對(duì)時(shí)間積分便得到用于測(cè)量平衡車(chē)相對(duì)于靈敏軸的傾角。

3.3電機(jī)驅(qū)動(dòng)模塊

電機(jī)驅(qū)動(dòng)電路為H橋式電機(jī)驅(qū)動(dòng)電路，該電路包括4個(gè)三極管和一個(gè)電機(jī)。增加總線驅(qū)動(dòng)芯片74LVC245來(lái)提高信號(hào)驅(qū)動(dòng)能力，隔離三極管和單片機(jī)，保護(hù)單片機(jī)芯片。電機(jī)驅(qū)動(dòng)隔離電路電路如圖2所示。防止電機(jī)瞬間換向產(chǎn)生峰值電壓進(jìn)而損壞其它芯片，在電機(jī)輸出端增加壓敏電阻，考慮到死區(qū)時(shí)間與高邊導(dǎo)通電壓使用IR2104作為全橋驅(qū)動(dòng)芯片。HH橋式電機(jī)驅(qū)動(dòng)電路如圖3所示。

4 系統(tǒng)軟件結(jié)構(gòu)設(shè)計(jì)

4.1 數(shù)據(jù)卡爾曼濾波

數(shù)據(jù)濾波是一種去除噪聲的非線性方法，該方法可以還原真實(shí)檢測(cè)值的數(shù)據(jù)?？柭鼮V波技術(shù)是數(shù)字濾波技術(shù)的一種，在測(cè)量方差己知的前提下從存在噪聲的數(shù)據(jù)中估算系統(tǒng)的狀態(tài)。在智能車(chē)姿態(tài)檢測(cè)系統(tǒng)中，加速度傳感器測(cè)量車(chē)體靜態(tài)時(shí)的角度，但該傳感器對(duì)震動(dòng)較敏感，并受動(dòng)態(tài)加速度的影響較大[7];角速度傳感器用于檢測(cè)車(chē)體傾斜角的變化，通過(guò)對(duì)測(cè)量的角速率累加計(jì)算傾斜角，但存在累積漂移誤差。通過(guò)卡爾曼濾波算法對(duì)加速度傳感器和角速度傳感器的輸出值進(jìn)行有效的融合，使加速傳感器與角速度傳感器得到的值更接近真實(shí)值。

4.2 雙閉環(huán)PID控制算法

加速度計(jì)用來(lái)檢測(cè)小車(chē)是否傾斜和傾斜程度，一旦傾斜，由陀螺儀觸發(fā)控制電機(jī)啟動(dòng)，再由加速度計(jì)控制小車(chē)是否回正，并停止電機(jī)驅(qū)動(dòng)[8-10]。

通過(guò)PID控制算法對(duì)車(chē)體角速度、角度、車(chē)體速度和車(chē)體位置等參數(shù)值進(jìn)行整合，利用輸出的PWM信號(hào)驅(qū)動(dòng)電機(jī)，產(chǎn)生相應(yīng)的力矩，以此保持車(chē)體的動(dòng)態(tài)平衡。采用雙閉環(huán) PID控制算法對(duì)平衡車(chē)進(jìn)行控制，其原理如下：

（1）位置閉環(huán)控制實(shí)現(xiàn)靜態(tài)自平衡。

車(chē)體的位姿信息通過(guò)兩個(gè)姿態(tài)傳感器獲得，將融合后的位姿信息發(fā)送給主控芯片從而實(shí)現(xiàn)機(jī)器人位置閉環(huán)控制。位置閉環(huán)可以實(shí)現(xiàn)車(chē)體在平衡點(diǎn)處無(wú)外界干擾時(shí)的靜態(tài)自平衡。

（2）速度閉環(huán)控制實(shí)現(xiàn)動(dòng)態(tài)自平衡。

用于驅(qū)動(dòng)電機(jī)的PWM信號(hào)，通過(guò)軟件算法的處理，獲得車(chē)輪的速度和位置信息，實(shí)現(xiàn)速度閉環(huán)控制。此環(huán)可在沒(méi)有編碼盤(pán)等傳感器提供硬件支持的前提下，通過(guò)軟件編碼算法獲取機(jī)器人的速度信息，從而實(shí)現(xiàn)閉環(huán)控制。速度閉環(huán)主要功能在于實(shí)現(xiàn)機(jī)器人在外界干擾狀態(tài)時(shí)的動(dòng)態(tài)自平衡。

4.3 總體程序設(shè)計(jì)

本系統(tǒng)軟件設(shè)計(jì)首先對(duì)各個(gè)模塊進(jìn)行初始化設(shè)置，對(duì)人機(jī)交互界面進(jìn)行初始化設(shè)計(jì)。然后獲取各傳感器數(shù)值，其中加速度計(jì)傳感器信號(hào)轉(zhuǎn)換為對(duì)應(yīng)的角度值;接著將角度傳感器采集的數(shù)值進(jìn)行優(yōu)化，將兩個(gè)數(shù)值調(diào)整到同樣規(guī)格，通過(guò)卡爾曼濾波算法將信號(hào)進(jìn)行濾波及角度融合。通過(guò)PID算法后的PWM波控制電機(jī)的輸出，從而保持車(chē)體平衡。

5 結(jié)語(yǔ)

本文基于卡爾曼濾波算法和PID 算法實(shí)現(xiàn)兩輪平衡車(chē)控制系統(tǒng)的設(shè)計(jì)。系統(tǒng)設(shè)計(jì)采用模塊化設(shè)計(jì)，簡(jiǎn)化了系統(tǒng)設(shè)計(jì)，統(tǒng)使用模塊化的設(shè)計(jì)，簡(jiǎn)化了系統(tǒng)的設(shè)計(jì)，各模塊之間既可獨(dú)立又可協(xié)調(diào)運(yùn)行，既減少了系統(tǒng)的響應(yīng)時(shí)間又增加了系統(tǒng)的可靠性。該設(shè)計(jì)具有穩(wěn)定性能良好，對(duì)地形適應(yīng)能力強(qiáng)，可以在復(fù)雜環(huán)境工作等優(yōu)點(diǎn)。本設(shè)計(jì)采用的技術(shù)在避障小車(chē)、機(jī)器人和無(wú)人駕駛汽車(chē)等領(lǐng)域中，具有較高的應(yīng)用價(jià)值。

參考文獻(xiàn)

[1] 何雅靜，屈勝利，孟祥忠等.兩輪不平衡小車(chē)變結(jié)構(gòu)控制抖振與魯棒性研究[J].電子技術(shù)應(yīng)用，2011（5）：130-133.

[2] 周牡丹，康愷，蔡普郎等.兩輪自平衡車(chē)控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[J].自動(dòng)化技術(shù)與應(yīng)用，2014（10）：4-8.

[3] 高正中，龔群英，宋森森.基于STM32的智能平衡車(chē)控制系統(tǒng)設(shè)計(jì)[J].現(xiàn)代電子技術(shù)，2016（14）：46-48.

[4] 王軍年，王慶年，宋傳學(xué)等.四輪驅(qū)動(dòng)電動(dòng)汽車(chē)差動(dòng)助力轉(zhuǎn)向系統(tǒng)聯(lián)合仿真與試驗(yàn)[J].農(nóng)業(yè)機(jī)械學(xué)報(bào)，2010（6）：7-14，30.

[5] 李亞文，常亮.一種基于ARM-STM32的兩輪自平衡小車(chē)的設(shè)計(jì)與實(shí)現(xiàn)[J].計(jì)算機(jī)與數(shù)字工程，2017（7）：1426-1429，1436.

[6] 張亞宜，米琦，高倩.基于FPGA的FFT處理器設(shè)計(jì)[J].中國(guó)民航大學(xué)學(xué)報(bào)，2007（2）：12-15.

[7] 賴義漢，王凱.基于MPU6050的雙輪平衡車(chē)控制系統(tǒng)設(shè)計(jì)[J].河南工程學(xué)院學(xué)報(bào)（自然科學(xué)版）2014（1）：53-57.

[8] 楊程引，程菊花，陳友榮等.兩輪直立自平衡車(chē)的設(shè)計(jì)[J].浙江樹(shù)人大學(xué)學(xué)報(bào)（自然科學(xué)版），2013（04）：1-7.

[9] 朱恩亮.PID控制算法在智能滅火機(jī)器人中的應(yīng)用[J].鹽城工學(xué)院學(xué)報(bào)（自然科學(xué)版），2012（03）：73-78.

[10] 石浩然.兩輪智能平衡車(chē)控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[D].長(zhǎng)安大學(xué)，2017.

作者簡(jiǎn)介：孫長(zhǎng)偉，女，（1982-），碩士研究生。主要研究方向?yàn)樾盘?hào)處理。