自平衡跟隨自行車控制系統設計與實現＊

張宏志，李健，祝洪宇

自平衡跟隨自行車控制系統設計與實現＊

張宏志，李健，祝洪宇

（遼寧科技大學 電子與信息工程學院，遼寧 鞍山 114051）

為解決平衡自行車跟隨行走的問題，提出了以OpenMV視覺模塊提供方向引導、慣性飛輪PID算法控制車體平衡、直流電機驅動自行車行進的系統設計方案。該方案以自行車行走過程中的姿態測量數據為反饋量，構成車身平衡雙閉環控制系統；通過OpenMV視覺模塊識別、鎖定目標物，引導自行車行走跟隨。在自行設計制作的自行車實物模型上，以STM32F103作為主控制板搭建了控制系統。軟、硬件調試結果表明，所設計的方案能夠實現自行車自平衡基礎上的行進跟隨，且控制精準度較高。

慣性飛輪；平衡車；視覺算法；PID

1 引言

自平衡自行車兼具傳統自行車靈活便捷和智能控制系統智能高效的特點，是一種全新的交通運輸和代步行走工具。與市面上流行的左輪、右輪縱向平衡車不同，自平衡自行車采用的是前輪、后輪行進的結構形式。對于這種結構形式的自行車，控制系統要解決的關鍵問題是使自行車在保持車身平衡的基礎上實現智能跟隨。為改善此類系統的控制性能，文獻[1-3]建立了無人自行車非線性動力學模型，提出了基于陀螺儀的直立姿態平衡控制方法；文獻[4]采用步進電機驅動控制技術實現自行車的相對動態平衡；考慮到自行車是多變量、強耦合的非線性系統，文獻[5-6]設計了基于LQR（Linear Quadratic Regulator）的控制算法，借以完成機器人自行車靜態平衡。上述方法各有優點和適用條件。

在借鑒前述工作的基礎上，本文提出一種基于視覺引導的自平衡跟隨自行車控制系統設計方法。該方法運用機器視覺技術對引導物圖像進行數據采集和處理，借助伺服控制技術，采用雙閉環控制結構輔助實現自行車行進過程中的動靜態平衡，在自行設計制作的自行車實物模型上進行了方法驗證。實驗結果表明，融合了機器視覺技術的控制方法可以使自平衡跟隨自行車對引導物進行精準鎖定，實現行走過程中的動態、靜態自動平衡，具備一定的抗干擾能力和智能行走功能。

2 系統構成及控制系統結構

2.1 系統構成

自行搭建的自平衡自行車物理模型如圖1所示。該模型由車體、視覺引導、車身平衡、行走控制和能源動力5個部分組成。車體部分采用Solidworks2016建立三維模型，借助3D快速成型技術進行零部件制作，然后組裝而成，主要包括車架、底盤、前后車輪和舵機，前輪為從動輪，后輪為驅動輪。視覺引導部分的核心是視覺傳感器，主要包括具有數據處理和傳輸功能的OpenMV攝像頭和配套組件。車體平衡部分主要包括慣性飛輪和附屬機械零部件，采用機械加工方式制作而成。控制部分是集成了主控制器、檢測模塊和各種驅動模塊的PCB電路板。能源動力部分包括電池和兩個直流電機，電池除了給直流電機提供動力，也給控制模塊提供工作電源；兩個直流電機中，一個帶有編碼器，用于驅動慣性飛輪，另一個不帶編碼器，通過減速裝置與后輪相聯用于驅動自行車行進。為使自行車具有比較好的動靜態控制效果，在制作過程中盡可能做到車體質量分布均勻，重心居中。

圖1 自行車車體圖

2.2 控制系統結構

根據平衡自行車跟隨行走控制要求，將控制系統設計成3個相對獨立的子系統，分別是車身平衡控制子系統、目標追蹤控制子系統和行走控制子系統，各子系統的控制結構如圖2所示。

車身平衡是平衡自行車跟隨行走的基礎，為同時兼顧穩定性和響應速度的要求，車身平衡子系統采用雙閉環串級控制結構，控制結構如圖2（a）所示。圖2（a）中，外環是速度環，內環是位置環。速度環給定信號*來自主控指令，反饋信號來自慣性飛輪直流電機編碼器，速度給定信號*與反饋信號的偏差Δ送給速度調節器ASR，經ASR處理后的輸出信號作為位置環的給定*；位置環的反饋值來自姿態檢測傳感器，它反映了自行車實際傾斜角，位置環的給定*與反饋值的偏差Δ送給位置調節器APR，調節器APR對Δ處理后，輸出具有一定占空比的PWM信號，送給電機驅動模塊，繼而調節電機轉速，完成姿態調整。

目標追蹤控制子系統采用單閉環控制結構，控制結構如圖2（b）所示。在該子系統中，目標給定信號*來自主控模塊指令，反饋信號來自視覺傳感器模塊，目標給定信號*與反饋信號的偏差Δ送給舵機，用于調整自行車轉向。與目標追蹤控制子系統相似，行進控制子系統也采用單閉環控制結構，控制結構如圖2（c）所示。

圖2 控制系統結構圖

3 控制系統硬件設計

控制系統硬件電路結構如圖3所示，主要由主控制器模塊、視覺圖像處理模塊、姿態檢測模塊、直流電機驅動模塊及電源模塊5部分構成。主控制器模塊是控制系統的核心，該模塊的主要功能是接收姿態傳感器、直流電機編碼器和視覺傳感器的反饋數據，對數據進行處理及運算，最后將控制信號輸出給舵機或直流電機驅動模塊。姿態檢測模塊的功能是通過角速度和角加速度傳感器實時檢測自行車實際傾斜角，并將其轉化為主控制器模塊可以識別的信號送給主控；視覺檢測模塊的功能是利用視覺傳感技術對引導物進行位置采集，采集到的數據提供給主控制器作為控制舵機轉向的基礎。直流電機驅動模塊負責將主控制器輸出的PWM信號轉換為控制信號，驅動2個直流電機的轉速和方向。電源模塊的功能是為控制系統各組成部分提供能量來源。

圖3 控制系統硬件電路結構框圖

3.1 主控制器模塊

主控制器采用STM32F103C8T6單片機。該控制器是基于Cortex-M3內核的32 bCPU，最高工作頻率為72 MHz，在6～64 kB的SRAM存儲器片上集成了SPI、I2C和USART等通信接口，方便對外界數據進行讀取；含有7個定時器；可輸出多路PWM信號，使得電機驅動模塊獲得穩定的PWM波形。在本系統設計中，STM32F103C8T6采用串口通信方式接收視覺模塊和藍牙模塊的數據，采用IIC協議讀取姿態傳感器MPU-6050的數據，輸出PWM波形控制電機和舵機等。

3.2 視覺處理模塊

OpenMV是集成了STM32F427CPU微處理器和OV7725圖像傳感器的開源型機器視覺模塊，可方便、快捷地實現圖像檢測和跟蹤、特征提取、顏色跟蹤等功能。在OpenMV上，可以用C語言高效實現機器視覺算法；由于該模塊搭載了Micro Python解釋器，提供了Python編程接口，還可以使用Python腳本語言編程實現I/O端口控制、讀取文件系統等基礎功能[7-9]。在本設計采用視覺引導模塊工作過程中，OpenMV攝像頭實時檢測引導物的中心坐標、判斷引導物的位置和方向，最終實現以引導物為基準的跟隨運動。為精確跟蹤效果，對攝像頭采集的圖像需要每10幀處理一次，過程復雜，計算量大。為減輕STM32F103C8T6工作負擔，本設計在OpenMV視覺模塊的基礎上，使用OpenMV IDE軟件和OpenCV庫進行圖像處理。處理后的位置信息通過串口通信發送給主控模塊STM32F103C8T6，進而輸出相應的電信號去控制舵機移動，使自行車跟隨引導物。OV7725攝像頭原理電路如圖4所示。

圖4 OV7725攝像頭原理電路

3.3 姿態處理模塊

MPU-6050是美國InvenSense公司生產的整合性6軸運動處理器。該處理器整合了3軸陀螺儀和3軸加速度計，其角速度感測范圍和加速度感測范圍分別有4組參數可供選擇，且感測范圍可通過編程來控制。本設計選用MPU-6050處理器作為姿態檢測模塊，對行進中的自行車慣性飛輪角速度和角度加速度進行實時測量。MPU-6050的測量數據經過內置16位A/D轉換器轉換后，通過IIC總線接口發送給STM32F103C8T6。MPU-6050和STM32F103C8T6之間的通信使用SDA和SCL端口。

MPU-6050模塊電路如圖5所示。

圖5 MPU-6050模塊電路

3.4 電機驅動模塊

本設計采用TB6612FNG作為電機驅動模塊。TB6612FNG是一款新型驅動器件，其電路如圖6所示。該模塊具有驅動能力強、散熱好、抗干擾能力強等特點，能獨立雙向控制2臺直流電機。當圖6中的使能端ENA和ENB使能后，從AIN1、AIN2輸入的信號驅動電機1的轉速和方向，從BIN1、BIN2輸入的信號驅動電機2 的轉速和方向。

圖6 電機驅動電路

3.5 電源轉換模塊

電源模塊包括12 V航模鋰電池和穩壓電路兩部分。航模電池供電電壓穩定，可對電機驅動模塊直接供電；STM32F103C8T6和其他外設模塊的供電要求在3～6 V之間，所以采用穩壓電路產生3.3 V和5 V兩個電壓，其中 3.3 V給STM32F103C8T6和MPU6050姿態傳感器供電，5 V給OLED顯示屏等外圍設備供電。電源穩壓電路如圖7所示。

圖7 電源穩壓電路

4 控制系統軟件設計

4.1 軟件總體流程

主程序主要包括初始化子程序、中斷響應子程序、姿態處理模塊數據采集子程序、視覺處理模塊數據采集子程序、PID控制子程序、電機PWM控制子程序六個部分。主體流程如圖8所示。

系統啟動后，首先對所有設備進行初始化，分配資源，然后執行中斷程序，判斷5 ms時間是否到。若時間到，則讀取姿態檢測模塊MPU-6050采集到的偏移角數據，利用內置濾波算法進行數據融合得到姿態估計值；若該估計值不滿足平衡要求，則執行增量式PID控制算法對慣性飛輪的角度和角速度進行調節，改變PWM占空比，進而驅動慣性飛輪調整自行車偏移角，直到自行車能夠在靜止狀態下收斂于平衡狀態。之后，視覺檢測模塊的OV7725攝像頭開始不斷掃描周圍環境，一旦引導物出現在檢測范圍內，主控模塊STM32F103C8T6就會根據視覺模塊傳回的引導物中心坐標判斷其位置。若判斷出引導物出現在自行車的左側，則繼續判斷自行車與引導物的距離是否大于設定值閾值。如果大于該閾值，則舵機向左調整，自行車左轉向后繼續前進；否則舵機向右調整，自行車右轉向、后退，一直退到自行車與引導物的距離大于設定值閾值時自行車再繼續跟隨引導物前進。若判斷出引導物出現在自行車的右側，則重復與上述類似的調整過程。這樣，在自行車行進過程中，OV7725攝像頭實時測量自行車與目標引導物的距離、MPU-6050實時采集偏移角數據，從而引導主控模塊適時調整舵機轉向、車身姿態和行進速度，使自行車始終與引導物保持一定距離穩定跟隨。

4.2 系統初始化

系統初始化主要包括系統時鐘初始化、NVIC（Nested Vectored Interrupt Controller）中斷初始化、定時器外部中斷初始化、通信（IIC）初始化、OpenMV視覺模塊初始化、MPU-6050初始化、驅動模塊TB6612FNG初始化等。STM32F103C8T6有72 MHz和36 MHz兩種工作頻率可供選擇，本系統對時鐘頻率的要求高，為減小系統誤差，在時鐘初始化程序中將時鐘頻率設置為72 MHz。

通信（IIC）初始化主要工作是設置串口波特率、中斷允許標志位和模塊工作模式。本系統將主控制器模塊STM32F103C8T6的串口1波特率設置為115 200 b/s，用于與OpenMV進行通信；將串口3波特率設置為9 600 b/s，用于與藍牙進行通信。

圖8 系統主程序流程圖

4.3 控制算法

控制算法是自行車平衡和跟隨控制的核心[10-11]。為了提高自行車系統整體反應速度，便于物理實現和實際操作，本設計對不同的控制子系統采用了不同的控制算法。為降低自行車突然加減速對車體的沖擊，行進控制系統采用帶有中間區的雙位控制算法；目標追蹤控制系統采用增量式PI控制算法，以兼顧舵機反應速度和攝像頭跟蹤準確度的要求；對前后輪結構形式的自行車來說，車體平衡控制是自行車穩定行走的前提，控制精度要求高，所以在串級結構基礎上主控制器ASR采用增量式PI算法，副控制器APR采用增量式PD算法。

帶中間區的雙位控制器輸出與輸入偏差之間的關系如下：

根據式（1），當攝像頭與引導物之間的距離大于給定值某一數值后（即偏差大于某一閾值），主控模塊STM32F103C8T6輸出具有一定占空比的信號max，電機加速，正向前進，靠近引導物；反之，當攝像頭與引導物之間的距離小于給定值某一數值后（即偏差小于某一閾值），STM32F103C8T6輸出具有一定占空比的信號min，電機減速，反向后退，離開引導物。當偏差在中間區變化時，主控模塊STM32F103C8T6輸出值不變。

目標追蹤控制系統采用增量式PI控制算法。根據式（2）所示的增量式離散PID公式，得到采用增量式PI控制算法表達式，如式（3）所示。

Δ=p[（）－（－1）]+i（）+

d[（）－2（－1）+（－2）] （2）

Δ舵機=p舵機[（）－（－1）]+i舵機（）（3）

式（2）（3）中：（）為當前時刻偏差；（－1）、（）、（－2）分別代表相鄰三個時刻偏差；Δ舵機為主控模塊STM32F103C8T6根據PI算法計算出的PWM信號占空比的增量值。

于是，STM32F103C8T6輸出的、用于控制舵機旋轉的PWM信號為：

舵機=（）+ Δ舵機（4）

車體平衡控制系統采用了串級控制結構。根據經驗可知，慣性飛輪的旋轉速度和自行車的傾斜角有關，當傾斜角增大后，在副控制器APR的作用下，產生一個加速度使飛輪旋轉速度加快；當傾斜角減小后，APR仍然會產生一個加速度使飛輪旋轉速度減慢。為了快速克服擾動和編碼器噪聲對調節精度的影響，APR采用增量式PD控制算法，主控制器ASR采用增量式PI控制算法。

ASR增量式PI控制算法和APR增量式PD控制算法表達式分別如式（5）（6）所示。

ΔASR=pASR[（）－（－1）]+iASR（）（5）

ΔAPR=pAPR[（）－（－1）]+dAPR[（）－

2（－1）+（－2）] （6）

4.4 電機控制策略

本設計中，慣性飛輪和后輪驅動使用的都是12 V直流電機。電機轉速由施加給電機驅動使能端ENA和ENB的PWM信號決定，電機正、反轉通過改變電機驅動IN1和IN2正負極性來實現，電機的加減速通過增大或減小PWM的方法來實現。通過PID控制算法后的PWM信號由主控制器STM32F103C8T6直接發送給直流電機驅動模塊，從而控制直流電機的速度和方向。自行車轉彎是通過主控制器控制舵機旋轉帶動扶手來實現的。

5 系統調試

5.1 OpenMV目標追蹤

目標追蹤調試包括閾值設定和測距兩個內容。在軟件代碼中，主要通過運行OpenMV視覺模塊中的OpenMV IDE軟件和調用OpenCV圖像處理數據庫實現引導物的識別和測距定位。

閾值設定基本步驟如下：①運行基礎程序hello word.py，在IDE中運行framebuffer，顯示圖案；②運行Mechine Vision，選擇Threshold Edito；③點擊Frame Buffer獲取IDE中的圖像，選擇閾值；④通過find_blobs函數找到引導物圖像；⑤通過uart.write函數將max_blob.cx（引導物中心橫坐標）和max_blob.cy（引導物中心縱坐標）發送給主控。

OpenMV實現測距定位的步驟如下：①設定引導物與攝像頭距離=10 cm；②通過find_blobs計算引導物圖像像素點M；③根據步驟①和②中的數據計算常數K=×M；④將引導物放在攝像頭檢測范圍內的任意位置；⑤通過find_blobs函數找到引導物圖像，計算像素點；⑥根據公式K=×M計算引導物與攝像頭之間的距離，通過uart.write將該數據發送回主控。

5.2 舵機PI控制參數整定

追蹤系統是單閉環控制系統，其PI控制器參數采用經驗試湊法進行整定。舵機的控制一般需要20 ms左右的時鐘脈沖，該脈沖的高電平部分一般為0.5～2.5 ms范圍內的角度控制脈沖部分，分別對應0°～180°。因為PWM太大會使舵機位移太大，使系統失去平衡，所以對PWM限幅為1 000（舵機最大偏移量為45°）。經實際調試，控制周期為5 ms，p舵機值為﹣0.3，i舵機值為﹣0.005時，舵機能較平穩的向著目標物體靠近。

5.3 車身平衡系統PID控制參數整定

按照車身平衡采用串級控制系統的情況，其控制器參數整定采用兩步整定法，即先整定副控制器APR，后整定主控制器ASR，最終整定出能使自行車實現自平衡的最佳參數。在本設計中，主控模塊PWM輸出設置值是7 200，對應占空比為100%。根據這一設定，如果pAPR值為720，則自行車在傾斜角度為±10°的時候飛輪就會滿轉。考慮到自行車傾斜過程中飛輪反應速度對車體平衡的影響，設定車體傾斜角度在±4°時，飛輪滿轉。據此估算出pAPR的取值范圍為[0，1 800]。當pAPR增大到1 500時，可以看到自行車在發生傾斜時，飛輪產生的反作用慣性力矩使車體能夠暫近收斂于直立的靜止平衡狀態，但存在低頻抖動。此時加入微分控制，估算dAPR值應該在0～8之間，設定dAPR值為7.2。

在速度環控制器參數整定中，首先通過STM32定時器的編碼器接口模式對編碼器模式進行四倍頻，并使用M法測速得到飛輪的速度信息。此時，編碼器最大值在200左右。考慮到系統的反應時間，假定當速度偏差達到最大速度50%的時候，電機速度最快，根據PWM輸出設置值與占空比之間的對應關系，得到pASR最大值為72。所以，iASR值應設定為pASR/50。經過實際測試，當pASR=50，iASR=1時，自行車可以保持趨近平衡狀態。

5.4 系統的動態調試

在小車自平衡過程中，若給小車一個外力來破壞小車的平衡，則小車能迅速響應這個變化，立即運動來減小傾角的變化，建立穩定平衡狀態。開始調試時，能響應，但波動較大，建立平衡的時間較長，通過修正PID參數，最終能使小車快速達到平衡狀態。

5.5 調試結果

大量實驗和調試結果表明，系統通電后，自行車完全實現靜止自平衡的時間為3 s左右；在普通水磨石地面上，平衡時傾角范圍約在±10°。給予一定外力干擾時，小車能夠立刻自動調節達到自平衡狀態；跟隨目標在1 m范圍內跟隨效果最優。

6 結論

實驗結果表明，所提出的設計方案和實現方法可使自平衡跟隨自行車在不需要人為控制的情況下保持平衡跟隨。該控制系統把機器視覺技術、嵌入式技術、運動控制技術融合在一起，有效解決了自平衡自行車行進過程中的姿態控制和目標跟隨問題，符合當前各個領域智能和自動化產品的趨勢。經過實驗室進一步完善改進后，在貨物輸送、智能交通等領域會有很好的發展與應用前景。

［1］郭磊，黃用華，廖啟征，等.自平衡自行車機器人的運動學分析［J］.北京郵電大學學報，2011，34（6）：99-102.

［2］郭磊，廖啟征，魏世民.用速率陀螺儀實現基于單片機的角度隨動系統研究——在自行車機器人的平衡控制中的應用［J］.機電產品開發與創新，2005，18（2）：1-3.

［3］呂寬州，陳素霞，黃金振.基于陀螺儀的無人自行車平衡控制方法［J］.計算機測量與控制，2015，23（7）：2344-2346.

［4］黃用華，廖啟征，魏世民，等.前輪驅動自行車機器人直線運動的控制實現［J］.北京郵電大學學報，2012，35（3）：65-69.

［5］古文生，王峰，周全，等.基于LQR控制的機器人自行車靜態平衡研究［J］.電腦知識與技術，2020，16（4）：192-194.

［6］楊凌霄，李曉陽.基于GA的兩輪自平衡小車LQR最優控制器設計［J］.河南理工大學學報（自然科學版），2015，34（1）：85-90.

［7］田渠，羅淦，尹海濤.基于OpenMV的智能跟蹤小車設計［J］.計算機測量與控制，2019， 27（8）：168-169.

［8］莊瓊云.基于OpenMV的智能尋跡小車設計與實現［D］.泉州：黎明職業大學，2018.

［9］謝彥昆.基于單目視覺的自平衡車編隊行進控制設計與實現［D］.重慶：重慶大學，2018.

［10］黨新安，唐鵬，楊立軍.單片機控制PID調節的自平衡自行車［J］.機械設計與制造，2019（10）：115-118.

［11］王素青，熊維堂.基于STM32的兩輪自平衡小車系統設計［J］.實驗室研究與探索，2016，35（5）：146-150.

TH17

A

10.15913/j.cnki.kjycx.2021.01.017

2095－6835（2021）01－0051－05

遼寧省大學生創新創業訓練計劃項目（編號：201910146027）

張宏志（1999—），男，吉林蛟河人，在讀本科生，自動化專業。李健（1998—），男，山西朔州人，在讀本科生，自動化專業。

祝洪宇（1971—），男，遼寧海城人，博士，副教授，研究方向為電機狀態監測和故障診斷。

〔編輯：嚴麗琴〕