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

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

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

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

中圖分類號：TM935.3文獻標識碼：A文章編號：2095-7394（2021）06-0031-11

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

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

1系統方案設計

本設計以STM32F407單片機為主控核心，通過六軸姿態傳感器和攝像頭分別采集平衡車姿態信息和路面圖像信息，實現兩輪小車的直立控制和循跡行駛。系統主控由六大模塊構成。

1.1姿態檢測模塊

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

1.2姿態控制模塊

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

其中：參數P可以提高姿態控制的控制力度，但是P參數過大會造成平衡車前后劇烈搖晃，使控制效果變差;參數D可以顯著抑制平衡車的前后搖晃，但是D參數過大會使平衡車出現高頻顫動，可能會損壞電機驅動;在調試過程中發現，引入I參數后，平衡車姿態控制變得不穩定，并且僅僅使用P、D參數控制效果已經足夠好，故在姿態PID中，將I參數值設為0，變為PD控制。

1.3路徑檢測模塊

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

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

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

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

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

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

1.4路徑控制模塊

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

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

此時，需要對圖像中的路徑進行加權處理，遠處的路徑給大一些的權重，近處的路徑給小一點的權重，以增加平衡車的方向靈敏度。

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

1.5速度檢測模塊

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

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

1.6速度控制模塊

速度控制模塊與姿態控制模塊結合構成串級PID控制。由于平衡車的姿態控制和速度控制密切相關，故可以將兩者組合在一起進行控制。如圖8所示，速度環和姿態環串聯在一起，速度環為外環，姿態環為內環。在控制過程中，速度環根據平衡車的速度計算出當前平衡車的最優傾角，姿態環則負責控制平衡車達到這一傾角值，如此間接實現速度控制。

2軟件方案設計

2.1STM32F407軟件系統設計

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

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

2.2姿態控制軟件設計

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

2.3路徑控制與擬合軟件設計

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

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

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

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

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

2.4藍牙通信軟件設計

Android設備的藍牙由BluetoothAdapter類進行管理[13]。藍牙通信部分的流程圖設計如圖13所示。程序為各個按鈕設置了監聽器，用來監聽各個按鈕的按下事件。當寫入參數按鈕按下時，程序將讀取UI界面中用戶輸入的PID參數數據，并將其按照一定的幀格式進行打包，之后將打包好的數據幀通過藍牙輸出流進行發送;由于發送過程會阻塞程序，因此，每次發送都將創建新的線程。其他按鈕的處理過程與此類似，故不再贅述。

3實驗結果與分析

3.1姿態檢測結果展示

如圖14所示，藍色的線是卡爾曼濾波之前的傾角，黃色的線是濾波之后的傾角。可以看到：在Q值過大時，系統動態特性好，但是噪聲也同時增大;在R值過大時，濾波結果較為平滑，但是滯后較為嚴重。通過多次嘗試，最終確定卡爾曼濾波器的Q值為0.001，R值為0.005，此時，濾波后的角速度失真較小，滯后不明顯，波形較為平滑，符合平衡車控制的要求。整定參數后的卡爾曼濾波器效果如圖15所示。

3.2路徑檢測結果展示

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

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

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

3.3藍牙發送結果展示

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

3.4整車測試效果及PID參數確定

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

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

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

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

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

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

按照上述過程，對平衡車姿態環、速度環和路徑環的參數進行了反復多次的調節，最終確定的參數如表3所示。

4結語

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

參考文獻：

[1]孫傳開.基于視覺自主導航的自平衡車設計與實現[D].廣州：華南理工大學，2015.

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

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

[4]白志剛.自動調節系統解析與PID整定[M].北京：化學工業出版社，2012.

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

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

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

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

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

[10]王迪.基于卡爾曼濾波的慣性導航技術[D].北京：北方工業大學，2018.

[11]王海菊，譚常玉，王坤林，等.自適應高斯濾波圖像去噪算法[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的藍牙通信系統設計與實現[D].武漢：湖北大學，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