基于圖像信息采集的自動尋跡平衡小車設(shè)計

崔淵 姬豐欣 陳祝洋 高倩 錢錚

摘要：針對循跡自平衡車的兩個關(guān)鍵技術(shù)難點(diǎn)——直立控制和視覺導(dǎo)航，采用STM32F407VET6單片機(jī)為控制核心。一方面，采集MPU6050六軸傳感器的加速度與角速度數(shù)據(jù)，經(jīng)卡爾曼濾波后得到精準(zhǔn)的直立傾角信息，結(jié)合速度-姿態(tài)串級PID控制器，加快電機(jī)對誤差的響應(yīng)速度，實現(xiàn)對平衡車直立的精確控制;另一方面，利用DMA機(jī)制采集OV7670攝像頭的圖像信息，并對其進(jìn)行灰度化、高斯圖像濾波、圖像二值化、路徑提取和擬合等處理，得到精確的路徑信息，實現(xiàn)平衡車的視覺導(dǎo)航。為了增強(qiáng)人機(jī)交互體驗，平衡車的關(guān)鍵數(shù)據(jù)還將實時傳輸?shù)紸ndroid 端App，并可通過App對平衡車進(jìn)行控制。

關(guān)鍵詞：STM32;自平衡車;卡爾曼濾波;循跡;串級PID;Android

中圖分類號：TM935.3文獻(xiàn)標(biāo)識碼：A文章編號：2095-7394（2021）06-0031-11

自平衡車一直是機(jī)器人領(lǐng)域的研究熱點(diǎn)，它的出現(xiàn)改變了人們的生產(chǎn)生活方式，極大地提高了生產(chǎn)效率，降低了生產(chǎn)成本和危險性，進(jìn)一步促進(jìn)了人們對智能化生活的需求。自平衡車是一個典型的倒立擺系統(tǒng)，由于倒立擺系統(tǒng)其本質(zhì)是不穩(wěn)定的，所以需對車身進(jìn)行實時調(diào)整才能保持其穩(wěn)定狀態(tài)[1]。目前，基于自平衡車這種面臨環(huán)境的不確定性，人們對其自主處理事務(wù)能力的要求越來越高，國內(nèi)外的相關(guān)研究也取得了大量重要成果，包括典型的PID控制、模糊控制以及人工神經(jīng)網(wǎng)絡(luò)控制等[2]。

相比于傳統(tǒng)的超聲波、光電對管等傳感器只能獲取單一的環(huán)境信息而言，攝像頭獲得的信息更加全面豐富，但同時也加劇了處理器的運(yùn)算負(fù)擔(dān)，所以視覺算法難以獲得廣泛應(yīng)用。近年來，由于半導(dǎo)體制造技術(shù)的快速發(fā)展以及機(jī)器視覺算法的成熟，這些瓶頸逐步得到突破，為基于視覺導(dǎo)航的自平衡車的研究奠定了重要基礎(chǔ)。目前，雖然市面上平衡車琳瑯滿目，但大多價格昂貴、功能單一，并且需要人為控制，在智能化程度方面尚不能滿足人們的需求。本文旨在結(jié)合自平衡車和視覺導(dǎo)航的優(yōu)點(diǎn)，設(shè)計和制作一款基于STM32視覺循跡的自平衡車。

1系統(tǒng)方案設(shè)計

本設(shè)計以STM32F407單片機(jī)為主控核心，通過六軸姿態(tài)傳感器和攝像頭分別采集平衡車姿態(tài)信息和路面圖像信息，實現(xiàn)兩輪小車的直立控制和循跡行駛。系統(tǒng)主控由六大模塊構(gòu)成。

1.1姿態(tài)檢測模塊

姿態(tài)檢測模塊基于六軸姿態(tài)傳感器的方式[3]。如圖1所示，通過姿態(tài)傳感器測量平衡車的加速度和角速度數(shù)據(jù)，之后利用卡爾曼濾波進(jìn)行姿態(tài)融合，得到平衡車傾角。

1.2姿態(tài)控制模塊

姿態(tài)控制模塊采用PID閉環(huán)控制[4]。如圖2所示，根據(jù)平衡車的測量傾角和期望傾角計算得到控制偏差，之后分別對偏差進(jìn)行比例、積分、微分運(yùn)算，將這3項相加得到輸出的控制量，然后輸出PWM控制電機(jī)維持車體平衡姿態(tài)。PID的輸出式為：

其中：參數(shù)P可以提高姿態(tài)控制的控制力度，但是P參數(shù)過大會造成平衡車前后劇烈搖晃，使控制效果變差;參數(shù)D可以顯著抑制平衡車的前后搖晃，但是D參數(shù)過大會使平衡車出現(xiàn)高頻顫動，可能會損壞電機(jī)驅(qū)動;在調(diào)試過程中發(fā)現(xiàn)，引入I參數(shù)后，平衡車姿態(tài)控制變得不穩(wěn)定，并且僅僅使用P、D參數(shù)控制效果已經(jīng)足夠好，故在姿態(tài)PID中，將I參數(shù)值設(shè)為0，變?yōu)镻D控制。

1.3路徑檢測模塊

如圖3所示，為最小二乘法路徑檢測框圖。

首先，利用攝像頭采集路面的灰度圖像，即單片機(jī)通過DCMI模塊對攝像頭的圖像進(jìn)行采集，為了減少CPU資源的浪費(fèi)，提高實時性，采用DMA方式進(jìn)行傳輸;與此同時，CPU可以處理其他工作。

然后，對其進(jìn)行濾波、二值化等處理，去除掉圖像的噪點(diǎn)與其他冗余信息。由于路徑與地面其他部分對比度很大，故在二值化圖像中路徑為黑色，其他部分為白色。統(tǒng)計灰度圖像的直方圖中存在的2個波峰，選取2個波峰之間的中點(diǎn)作為閾值。默認(rèn)圖像服從高斯分布，可以通過對濾波器窗口內(nèi)的像素點(diǎn)按高斯核進(jìn)行加權(quán)平均得到輸出。

在對圖像經(jīng)過處理得到二值化圖像后，根據(jù)路徑的左邊緣為白色到黑色的跳變、右邊緣為黑色到白色的跳變這兩個特點(diǎn)，即可定位路徑的邊緣。由于實際路徑可能會出現(xiàn)彎折的情況，只從行方向掃描可能無法獲得全面的路徑信息，故選擇從行、列兩個方向?qū)D像進(jìn)行掃描。同時，在水平和垂直兩個方向上檢測黑色路徑的兩個邊緣，通過計算便可以得出中點(diǎn)。

最后，根據(jù)最小二乘法[5]原理擬合出一條直線表示當(dāng)前的路徑。通過最小化誤差的平方和尋找數(shù)據(jù)的最佳函數(shù)匹配，利用最小二乘法可以簡便地求得未知數(shù)據(jù)，并使這些求得的數(shù)據(jù)與實際數(shù)據(jù)之間誤差的平方和為最小。最小二乘法擬合路徑示意圖如圖4所示。設(shè)對圖像進(jìn)行邊緣檢測后

使用最小二乘法噪聲和干擾的影響較小，并且對直線和彎折的路徑都能準(zhǔn)確表示，還能夠?qū)D像中路徑丟失的區(qū)域進(jìn)行預(yù)測。其擬合得到的路徑直線的斜率包含了方向信息，直線的函數(shù)值包含了車體偏離路徑信息，從而能夠滿足實時處理的要求。

1.4路徑控制模塊

如圖5所示，為偏差加權(quán)PID控制框圖。輸入期望路徑值，與測量路徑做偏差，再將偏差加權(quán)，輸出到PID控制器計算得到電機(jī)輸出，從而控制左右差速。

由于攝像頭采集的圖像中越靠近上邊的圖像離平衡車的距離越遠(yuǎn)，而遠(yuǎn)處的圖像對平衡車接下來位置起預(yù)測作用，故越遠(yuǎn)處的圖像信息對路徑控制越重要。將其相加可以得到路徑偏差：

此時，需要對圖像中的路徑進(jìn)行加權(quán)處理，遠(yuǎn)處的路徑給大一些的權(quán)重，近處的路徑給小一點(diǎn)的權(quán)重，以增加平衡車的方向靈敏度。

式（3）中，road為加權(quán)后的路徑，i表示圖像的行數(shù)，i越小，離車身越近，center_i表示第i行的路徑偏差，k_i表示第i行的權(quán)重值。權(quán)重帶分區(qū)越多，比例系數(shù)越小，方向控制靈敏度越好。但是，考慮到實際計算量以及噪點(diǎn)的影響，對圖像進(jìn)行權(quán)重帶分離，同一個權(quán)重帶下路徑加權(quán)值相同，如圖6所示。在實驗條件下，光照強(qiáng)度不變，將一張圖片分為6部分，取前5部分作運(yùn)算，中間的權(quán)重更大些，底部最小，最上面的圖像由于攝像頭角度的安裝，經(jīng)常會拍到墻或者線外，故可以不作處理和運(yùn)算。再將根據(jù)式（3）計算出的擬合后的路徑偏差值作為輸出，送入PID控制器得到控制量，據(jù)此調(diào)整兩電機(jī)的轉(zhuǎn)速差，從而控制平衡車緊跟路徑行駛。

1.5速度檢測模塊

系統(tǒng)用霍爾編碼器測速[6]。如圖7所示，將霍爾編碼器安裝至兩個電機(jī)上，定時讀取兩個編碼器A相和B相輸出的脈沖數(shù)P₁和P₂，兩個電機(jī)的轉(zhuǎn)速R₁和R₂由下式計算：

將兩者求平均后乘以輪胎的周長L即可得到平衡車的速度v：

1.6速度控制模塊

速度控制模塊與姿態(tài)控制模塊結(jié)合構(gòu)成串級PID控制。由于平衡車的姿態(tài)控制和速度控制密切相關(guān)，故可以將兩者組合在一起進(jìn)行控制。如圖8所示，速度環(huán)和姿態(tài)環(huán)串聯(lián)在一起，速度環(huán)為外環(huán)，姿態(tài)環(huán)為內(nèi)環(huán)。在控制過程中，速度環(huán)根據(jù)平衡車的速度計算出當(dāng)前平衡車的最優(yōu)傾角，姿態(tài)環(huán)則負(fù)責(zé)控制平衡車達(dá)到這一傾角值，如此間接實現(xiàn)速度控制。

2軟件方案設(shè)計

2.1STM32F407軟件系統(tǒng)設(shè)計

在整個平衡車系統(tǒng)中，主控制器STM32F407VET6無疑起著關(guān)鍵的作用，平衡車的姿態(tài)控制、路徑控制、速度控制、菜單界面顯示、與Android端進(jìn)行通信以及控制電機(jī)旋轉(zhuǎn)等重要功能均由它來完成。

STM32F407軟件系統(tǒng)主程序流程如圖9所示。系統(tǒng)上電后首先進(jìn)行HAL庫、系統(tǒng)時鐘以及諸如攝像頭、MPU6050等其他各種外設(shè)的初始化;然后，判斷各個定時時間是否溢出來執(zhí)行相關(guān)軟件模塊。在5 ms的定時溢出后，系統(tǒng)進(jìn)行一次平衡車的姿態(tài)、路徑、速度等控制，并輸出PWM給電機(jī);在20 ms的定時溢出后，系統(tǒng)對菜單顯示進(jìn)行更新;在50 ms的定時溢出后，系統(tǒng)對OV7670觸發(fā)一次圖像采集，并處理與Android之間的通信任務(wù)。當(dāng)系統(tǒng)檢測到圖像采集結(jié)束時，將對圖像進(jìn)行一系列處理，以便后續(xù)使用。

2.2姿態(tài)控制軟件設(shè)計

如圖10所示，平衡車軟件姿態(tài)控制部分的主要任務(wù)是解算平衡車傾角，并控制平衡車保持平衡姿態(tài)。平衡車的姿態(tài)控制軟件主要分為三個部分：傳感器信息采集、傾角解算、PID控制。其中：傳感器信息采集部分負(fù)責(zé)采集平衡車的加速度和角速度信息;傾角解算部分利用前面獲得的信息，通過卡爾曼濾波解算出平衡車的傾角[9];PID控制部分則負(fù)責(zé)對平衡車傾角誤差進(jìn)行控制，以維持平衡車的平衡姿態(tài)[10]。

2.3路徑控制與擬合軟件設(shè)計

如圖11所示，路徑控制主要分為三個部分：圖像采集與處理、路徑檢測與擬合、加權(quán)控制。其中：圖像采集與處理部分主要負(fù)責(zé)從OV7670攝像頭采集圖像信息，并對圖像進(jìn)行濾波、二值化等處理，以供后續(xù)路徑檢測使用[11];路徑檢測與擬合部分主要負(fù)責(zé)從二值化圖像中將路徑分離出來，獲得路徑各個點(diǎn)的坐標(biāo)，并擬合出路徑方程;加權(quán)控制部分主要負(fù)責(zé)對路徑按重要程度進(jìn)行加權(quán)，從而控制平衡車沿地面路徑行駛[12]。

路徑擬合方程的流程如圖12所示。先通過所有的路徑中點(diǎn)計算出均值和協(xié)方差矩陣，然后遍歷所有點(diǎn)，將坐標(biāo)代入下式：

得到該點(diǎn)的概率密度函數(shù)值。通過判斷該值是否大于設(shè)定的閾值來判定該點(diǎn)是否為干擾點(diǎn)，如果不是干擾點(diǎn)，則將其保留下來進(jìn)行后續(xù)處理。

至此，得到的路徑中點(diǎn)都是離散的點(diǎn)，甚至可能存在缺失，這樣不利于后續(xù)處理;因此，需要擬合出一條曲線來表示路徑?？紤]到高次多項式擬合計算量大、容易出現(xiàn)過擬合等問題，這里采用最小二乘法進(jìn)行線性擬合，即擬合出一條直線代表路徑。

設(shè)待擬合直線的方程為y=kx+b。由于直線斜率k在直線垂直于x軸時趨于無窮大，而平衡車在直線路徑上行駛時正是這種情況，這樣不便于后續(xù)處理;因此，這里以圖像的垂直方向為x軸，水平方向為y軸，當(dāng)在直線路徑上行駛時，直線的斜率為0。

2.4藍(lán)牙通信軟件設(shè)計

Android設(shè)備的藍(lán)牙由BluetoothAdapter類進(jìn)行管理[13]。藍(lán)牙通信部分的流程圖設(shè)計如圖13所示。程序為各個按鈕設(shè)置了監(jiān)聽器，用來監(jiān)聽各個按鈕的按下事件。當(dāng)寫入?yún)?shù)按鈕按下時，程序?qū)⒆x取UI界面中用戶輸入的PID參數(shù)數(shù)據(jù)，并將其按照一定的幀格式進(jìn)行打包，之后將打包好的數(shù)據(jù)幀通過藍(lán)牙輸出流進(jìn)行發(fā)送;由于發(fā)送過程會阻塞程序，因此，每次發(fā)送都將創(chuàng)建新的線程。其他按鈕的處理過程與此類似，故不再贅述。

3實驗結(jié)果與分析

3.1姿態(tài)檢測結(jié)果展示

如圖14所示，藍(lán)色的線是卡爾曼濾波之前的傾角，黃色的線是濾波之后的傾角?？梢钥吹剑涸赒值過大時，系統(tǒng)動態(tài)特性好，但是噪聲也同時增大;在R值過大時，濾波結(jié)果較為平滑，但是滯后較為嚴(yán)重。通過多次嘗試，最終確定卡爾曼濾波器的Q值為0.001，R值為0.005，此時，濾波后的角速度失真較小，滯后不明顯，波形較為平滑，符合平衡車控制的要求。整定參數(shù)后的卡爾曼濾波器效果如圖15所示。

3.2路徑檢測結(jié)果展示

二值化過程用于去除灰度圖像中的冗余信息，將之轉(zhuǎn)換為使用黑白兩種顏色表示的二值化圖像，以突出路徑的特征。平衡車的二值化效果如圖16所示。其中：左側(cè)為灰度圖像;右側(cè)為二值化處理的結(jié)果。

如圖17所示，在實際運(yùn)行中，由于平衡車所處的環(huán)境復(fù)雜，上述步驟提取的路徑中可能存在干擾點(diǎn)需要去除，以防止對后續(xù)的處理造成影響。

在平衡車行駛的過程中，還有可能由于環(huán)境的原因，導(dǎo)致提取到的路徑不連續(xù)，甚至出現(xiàn)缺失的現(xiàn)象;因此，直接通過提取路徑控制平衡車的行進(jìn)是不合理的，可使用最小二乘法對路徑進(jìn)行擬合。對于不同類型的路徑，擬合的效果如圖18所示。

3.3藍(lán)牙發(fā)送結(jié)果展示

平衡車與Android手機(jī)之間通過藍(lán)牙進(jìn)行數(shù)據(jù)交換。為了保證通信過程不發(fā)生錯亂，雙方通過約定的數(shù)據(jù)格式進(jìn)行傳輸。如圖19所示，平衡車正確地向Android手機(jī)發(fā)送了傳感器、參數(shù)以及圖像等信息。

3.4整車測試效果及PID參數(shù)確定

針對平衡車的聯(lián)合調(diào)試，制作模擬路徑，并進(jìn)行整車的實際運(yùn)行測試。平衡車行駛時的情況如圖20所示。平衡車的PID參數(shù)確定主要分為速度-姿態(tài)串級PID參數(shù)的整定以及路徑PID參數(shù)的整定兩個部分。其中：PID控制器中的P參數(shù)是比例系數(shù)，表示系統(tǒng)調(diào)節(jié)的力度;I參數(shù)是積分系數(shù)，用來消除控制系統(tǒng)的靜差;D參數(shù)是微分系數(shù)，用來抑制系統(tǒng)的超調(diào)，對控制量的變化起預(yù)測作用。

（1）測試中將速度環(huán)的所有參數(shù)和姿態(tài)環(huán)的D參數(shù)置為0，逐漸加大姿態(tài)環(huán)的P參數(shù)，觀察平衡車的姿態(tài)控制效果。在不同P參數(shù)下，平衡車的姿態(tài)控制效果分析如表1所示。

由表1可見，加大參數(shù)P可以提高姿態(tài)控制的控制力度，但是，P參數(shù)過大又會造成平衡車前后劇烈搖晃，使控制效果變差;因此，以下調(diào)試將P參數(shù)取值為-90。

（2）在整定完姿態(tài)環(huán)的P參數(shù)后，整定D參數(shù)，即逐漸加大D參數(shù)，觀察平衡車的姿態(tài)控制效果。在不同D參數(shù)下，平衡車的姿態(tài)控制效果分析如表2所示。

由表2可見，加大參數(shù)D可以顯著抑制平衡車的前后搖晃，但是，D參數(shù)過大會造成平衡車出現(xiàn)高頻顫動，并損壞電機(jī)驅(qū)動;因此，需要合理確定D參數(shù)的取值，以下調(diào)試將D參數(shù)取值為-1.10。

（3）在整定完姿態(tài)環(huán)參數(shù)后，平衡車已經(jīng)能夠維持短時間的平衡姿態(tài)了;但是，由于缺乏速度控制，在經(jīng)過短暫的平衡后，它會加速向一個方向前進(jìn)。當(dāng)速度達(dá)到電機(jī)輸出極限時，平衡車將無法繼續(xù)控制姿態(tài)，進(jìn)而倒下。因此，為了控制住平衡車在平衡狀態(tài)下的速度，需要對速度環(huán)進(jìn)行調(diào)參。速度環(huán)與路徑PID參數(shù)整定大致步驟同上，這里不再贅述。

按照上述過程，對平衡車姿態(tài)環(huán)、速度環(huán)和路徑環(huán)的參數(shù)進(jìn)行了反復(fù)多次的調(diào)節(jié)，最終確定的參數(shù)如表3所示。

4結(jié)語

綜上所述，基于圖像信息采集的自動尋跡平衡小車設(shè)計充分發(fā)揮了STM32單片機(jī)的性能優(yōu)勢，使平衡車能夠跟隨地面上的單根黑線路徑行駛;同時，針對實際運(yùn)用中可能遇到的雙線或者引導(dǎo)線不連續(xù)的情況，設(shè)計進(jìn)行了算法優(yōu)化。在后續(xù)的研究中，將嘗試更換分辨率更高、角度更廣的攝像頭，同時搭配速度更快的嵌入式處理器、更優(yōu)化的上位機(jī)控制軟件和更完善的視覺導(dǎo)航算法，以進(jìn)一步改進(jìn)和完善該平衡車設(shè)計，使其可以真正投入市場，并廣泛適用于實地勘察、目標(biāo)引導(dǎo)、自動駕駛等現(xiàn)實應(yīng)用場景。

參考文獻(xiàn)：

[1]孫傳開.基于視覺自主導(dǎo)航的自平衡車設(shè)計與實現(xiàn)[D].廣州：華南理工大學(xué)，2015.

[2]孫俊杰.自主雙輪平衡車智能控制研究[D].廣州：華南理工大學(xué)，2016.

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

[4]白志剛.自動調(diào)節(jié)系統(tǒng)解析與PID整定[M].北京：化學(xué)工業(yè)出版社，2012.

[5]田垅，劉宗田.最小二乘法分段直線擬合[J].計算機(jī)科學(xué)，2012，39（S1）：482-484.

[6]時為.STM8S103F3P單片機(jī)正交編碼器接口的使用方法[J].揚(yáng)州職業(yè)大學(xué)學(xué)報，2017，21（1）：47-49.

[7]葉俊明，馬海琴.基于MPU6050的陀螺儀測控系統(tǒng)設(shè)計[J].中國高新技術(shù)企業(yè)，2015（27）：26-27.

[8]李斐然.基于霍爾原理的絕對式磁編碼器的研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2015.

[9]秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].3版.西安：西北工業(yè)大學(xué)出版社，2015.

[10]王迪.基于卡爾曼濾波的慣性導(dǎo)航技術(shù)[D].北京：北方工業(yè)大學(xué)，2018.

[11]王海菊，譚常玉，王坤林，等.自適應(yīng)高斯濾波圖像去噪算法[J].福建電腦，2017（11）：5-6.

[12] GANG W，CHOI S，LEE J. Balance control of a 12-DOF mobilemanipulator based on two- wheel inverted pendulum robot[J].Journal of Measurement Science and Instrumentation ，2013，4（1）：52-57.

[13]鄭昊.基于Arduino/Android的藍(lán)牙通信系統(tǒng)設(shè)計與實現(xiàn)[D].武漢：湖北大學(xué)，2012.

Design of Automatic Tracing Balance Car Based on Image Information Acquisition

CUI Yuan，JI Fengxin，CHEN Zhuyang，GAO Qian，QIAN Zheng

（School of Electricity and Information Engineering，Jiangsu University of Technology，Changzhou 213001，China）

Abstract：Aiming at the two key technical difficulties of tracking self-balancing vehicle，namely upright control and visual navigation，STM32F407VET6 single-chip microcomputer is used as the control core in this paper. On the one hand，the accurate vertical angle information is obtained by Kalman filter using the collected acceleration and angular velocity data of MPU6050 six-axis sensor. Combined with the speed attitude cascade PID controller，the response speed of the motor to the error is accelerated，and the accurate control of the upright of the balance vehicle is realized. On the other hand，the image information of ov7670 camera is collected by DMA mechanism，and processed by graying，Gaussian image filtering，image binarization，path extraction and fitting，so as to obtain accurate path information and realize the visual navigation of balance vehicle. In order to enhance the humancomputer interaction experience，the key data of the balancing car will also be transmitted to the Android App in real time，and the balancing car can be controlled through the App.

Key words：STM32;self-balancing vehicle;Kalman filtering;tracking;cascade PID;Android