，，，，

(中南民族大學 計算機科學學院，武漢 430074)

引 言

本文所設計的控制系統(tǒng)主要包括平衡與運動控制系統(tǒng)、位置獲取與分析系統(tǒng)兩個子系統(tǒng)。平衡與運動控制系統(tǒng)以STM32F103單片機為核心處理器，通過姿態(tài)傳感器MPU6050監(jiān)測車身所處的俯仰狀態(tài)和狀態(tài)變化率，經(jīng)過PID算法調節(jié)，保證小車在按照指令前行的情況下保持平衡；樹莓派和攝像頭組合充當平衡車的眼睛以獲取位置信息。該設計旨在實現(xiàn)雙輪小車在直立平衡的狀態(tài)下，根據(jù)樹莓派傳回的不同數(shù)據(jù)沿著黑色賽道行駛，以實現(xiàn)自主循跡的功能。在自主循跡的前提下亦可發(fā)展無人傳輸物品，以減少人力的使用。通過對該系統(tǒng)的介紹，重點講述小車循跡控制算法的實現(xiàn)。

1 雙輪平衡車國內外研究現(xiàn)狀

1986年，日本電通大學的山滕一雄(Kazuo Yamafuji)教授提出基于倒立擺模型設計自平衡模型的概念，并設計制造了一個兩輪共軸、重心在上的自平衡模型。

近十幾年來，在雙輪自平衡模型的研究上，國內外眾多專家和愛好者們取得了一系列的成果，推動了自平衡技術的發(fā)展和成熟[1]。2002年，美國Lego公司設計了一款兩輪自平衡傳感式機器人，簡稱 Legway[2]；2005年，美國Southern Methodist University的David P.Anderson教授設計了一款雙輪移動機器模型，取名nBot[3]；2007年，深圳固高科技有限公司研制了一款用于教育教學示范的自平衡模型，即GBOT1001機器人[4]；2008年，美國Tiger電子公司和日本Sega玩具公司聯(lián)合推出了AMP[5](Automated Musical Personality)兩輪自平衡機器人。

作為科學實驗儀器的自平衡模型的涌現(xiàn)，推動兩輪機器人學以及相關學科的研究和發(fā)展，同時也啟發(fā)了人們向載人運動方向發(fā)展的思路。兩輪載人自平衡車具有占地面積小、運動靈活敏捷的特點，由此提供了一種新式交通工具的設計概念。

Segway[6]是第一個推出載人自平衡車品牌并成功推入市場的公司，其由美國著名發(fā)明家Dean Kamen 創(chuàng)立，2003年首次推出第一代Segway概念型自平衡車；2005年，中國科學技術大學自動化系和力學系多位教授、博士和研究生研制了自平衡兩輪電動代步車Free Mover[7]；2008年，日本的豐田公司(TOYOTA)推出了一款兩輪個人交通工具Winglet[8]；2014年小米科技公司投資ninebot，開始致力于解決5公里或10公里內的短途交通問題；2015年4 月ninebot收購Segway公司，成為全球最大的平衡車公司，同年10月19日，ninebot迷你九號平衡車作為小米生態(tài)鏈公司的一個新領域產(chǎn)品，正式發(fā)布。

2 系統(tǒng)整體設計

所設計的雙輪小車主要由兩大部分組成：第一部分為硬件設計，第二部分為軟件設計。其中核心板模塊、姿態(tài)檢測模塊、電機驅動板模塊、Raspberry Pi高清攝像頭板模塊和Raspberry Pi3等組成了系統(tǒng)的硬件部分；姿態(tài)檢測算法、PID算法、通信方案的程序設計、小車循跡控制算法構成了系統(tǒng)的軟件部分。小車平衡與控制系統(tǒng)設計過程中涉及的直立行走任務分解、車模直立控制、車模速度控制等借鑒了參考文獻[9]。系統(tǒng)的整體框圖如圖1所示。

圖1 系統(tǒng)整體框圖

3 雙輪平衡車硬件組成

3.1 平衡車車架

平衡車車架由兩臺42步進電機(含步進電機支架、聯(lián)軸器)、橡膠輪胎、亞力克載物臺、12 V鋰電池等組成，12 V鋰電池采用DC頭作為輸出接口，方便電源的插拔。

3.2 核心板模塊

核心板模塊的主控制器芯片為STM32F103VCT6，其包括板載MCU的基本電路、晶振電路、復位電路、LDO電路、MicroUSB接口，至少支持2路硬件UART、預留8路硬件PWM、支持2路I2C、1路硬件SPI、8路模擬輸入；包含8路外部中斷；電平標準采用3.3 V。

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

姿態(tài)檢測模塊主控制器芯片為MPU6050，包含I2C接口、板載LDO電路，其工作電源電壓為3.3 V。MPU6050整合了3 軸陀螺儀、3 軸加速計，并含可藉由第二個I2C端口連接其他廠商加速器、磁力傳感器或其他傳感器的數(shù)位運動處理(Digital Motion Processor，DMP)硬件加速引擎，由主要I2C端口以單一數(shù)據(jù)流的形式向應用端輸出完整的9軸融合演算技術InvenSense 的運動處理資料庫，可處理運動感測的復雜數(shù)據(jù)。

3.4 電機驅動板

步進電機驅動模塊采用DRV8825方案，其為TI公司生產(chǎn)的一種高電壓、大電流步進電機驅動芯片，可實現(xiàn)大電流、快速響應的雙極步進電機驅動。DRV8825可以驅動一個兩相四線的步進電機，也可以驅動兩個直流有刷電機，輸入電壓為8.2～45 V，最大電流為1.7 A，可以承受2.5 A的瞬間電壓，可通過PWM輸入來驅動。

3.5 Raspberry Pi高清攝像頭板

Raspberry Pi高清 (HD) 攝像頭板可連接至 Raspberry Pi，可以創(chuàng)建高清視頻和靜止攝影。它利用 Sony 的 IMX219PQ 圖像傳感器提供高速視頻成像和高靈敏度。Raspberry Pi 攝像頭模塊可減少圖像污染，如固定模式噪聲和拖尾效應。 它還具有自動控制功能，如曝光控制、白平衡和亮度檢測。

3.6 Raspberry Pi3

Raspberry Pi型號 B 是信用卡大的計算機板，當添加了鍵盤、鼠標、顯示屏、電源和已安裝OS的 MicroSD卡時，可以啟動和運行。它是基于微型 ARM的PC，可運行許多通常需要臺式PC的應用(如電子表格、文字處理和游戲)，它還可播放高清視頻。

4 軟件設計

4.1 系統(tǒng)流程圖

圖2 程序流程圖

雙輪平衡車“跑”起來的過程如圖 2所示。首先，初始化各個外設，主函數(shù)將一直在 while(1)循環(huán)執(zhí)行。定時器中斷每 10 ms 觸發(fā)一次，從主函數(shù)跳到中斷回調相應函數(shù)，完成數(shù)據(jù)采集、處理、控制整個過程(首先獲取當前小車的姿態(tài)即傾角，然后讀取樹莓派傳回的數(shù)據(jù)，處理完后，更新PID輸出，完成電機控制)。執(zhí)行完立刻返回到上次主函數(shù)執(zhí)行的位置繼續(xù)執(zhí)行。

4.2 姿態(tài)檢測算法

在雙輪平衡車姿態(tài)檢測系統(tǒng)中，MPU6050主要用于獲取小車傾斜角和傾斜角的變化率[10]，其中加速度計可以精確計算車體靜止時的角度；陀螺儀的輸出值是旋轉角速率，通過角速率對時間積分即可得到角度值，系統(tǒng)采用STM32F103單片機循環(huán)采樣獲取陀螺儀的角速率信息，對采樣值進行累加實現(xiàn)積分功能來計算角度值，此方法存在累積誤差，為獲得可靠的車體傾角值，系統(tǒng)采用卡爾曼濾波對加速度計和陀螺儀的輸出值進行融合。

4.3 PID算法

考慮到現(xiàn)在兩輪平衡車均采用通過 MPU6050姿態(tài)傳感器讀取小車當前狀態(tài)，當雙輪平衡車的平衡狀態(tài)被破壞時，系統(tǒng)采用PID控制算法[10]，通過整合車體角度、角速度和車體速度等參數(shù)值，輸出PWM信號驅動電機，產(chǎn)生相應的力矩，使得車體保持動態(tài)平衡，其結構框圖如圖3所示。

圖3 PID算法框圖

4.4 通信方案的實現(xiàn)與程序設計

利用Raspberry Pi3處理攝像頭采集信息，并將處理后的信息以字節(jié)形式通過串口傳輸?shù)絊TM32F103VCT6。STM32F103VCT6將接收的信息分解成兩部分，將字節(jié)的最高位作為方向，1表示右，0表示左，低七位表示為速度；停止信息分別為0xff和0x7f，運行期間Raspberry Pi3返回的低七位構成的數(shù)值大小范圍為0～100，小車初始速度+數(shù)值大小×系數(shù)等于其中一個輪子的速度，另一個輪子的速度等于小車初始速度-數(shù)值大小×系數(shù)。

4.5 小車循跡控制

循跡是指小車在白色的紙(地板等)上循黑線行走，本文的循跡是采用攝像頭采集圖像并分析實線，即用攝像頭拍攝白色紙上的黑色引導線，樹莓派利用最小二乘法擬合路徑[11]得到路徑方向和位置，將轉彎信息通過串口的方式傳送給STM32主控制器，STM32F103VCT6將接收到的數(shù)據(jù)用于轉換為輪子速度和方向的控制。以保證小車能正常沿著黑色引導線行駛。圖4為行駛圖，黑色為引導線，白色線條為切線，車會沿著切線方向行走如圖5所示。

圖4 小車行駛圖

圖5 攝像頭拍攝的圖片并作出切線

樹莓派運行的是基于Debian的樹莓派定制系統(tǒng)Raspbian OS，圖像處理程序使用Python3.5編寫，圖像處理邏輯依賴OpenCV2、Numpy、Pyserial實現(xiàn)。

程序首先從攝像頭獲取圖像，由于樹莓派性能有限，但又要有較為精確的識別，最后采用160×90的分辨率，最后得到的幀率接近60 fps，保證了小車具有較高的反應速度。

程序再對圖像進行隔行掃描，獲取每行的路徑邊緣，取左右邊界中心得到小車軌跡中心。最后以水平向右方向為y軸，豎直向下為x軸，建立坐標系，對得到的離散點使用最小二分法進行直線擬合，如圖6所示。最小二分法擬合函數(shù)參數(shù)k、b的計算公式如下：

(1)

(2)

圖6 樹莓派返回數(shù)據(jù)生成的表

最后得到直線斜率k，再轉換為弧度theta即為小車角度偏移，作為小車方向控制的主要參數(shù)。同時，取擬合的直線中點，計算到圖像中心的距離dist，作為小車的距離偏移，于是得到小車的轉彎結果為：

turn=k1×theta+k2×dist

turn的絕對值為轉彎幅度，turn的正負代表轉彎方向。最后將turn轉化為8位，通過串口傳輸?shù)絊TM32主控制器，然后進行相應的轉彎控制。

小車循跡程序設計：

'''隔行掃描'''

def getBorder(self, th, row, find_step=2, confidence=2):

size =th.shape

left = 0

right = size[1]

black_count = 0

white_count = 0

black_overed = False

for i in range(0, size[1], find_step):

pixel =th[row][i]

if pixel == 0:

if notblack_overed:

left = i

black_overed = True

elif black_overed:

white_count += 1

if white_count >= confidence:

right =i-confidence*find_step

break

return (left, right)

'''計算最小二分法擬合函數(shù)參數(shù)k、b，判斷軌跡起始、結束點 '''

def minCost(self, th, img):

sum_x2 = 0

sum_x = 0

sum_y = 0

sum_xy = 0

count = 0

size =th.shape

start_point = 0

end_point = size[0]

Xi = []

Yi = []

for i in range(start_point, end_point, 5):

border =self.getBorder(th, i)

l, r = border

if (l == 0 and r == size[1]) or (l == 0 and r == 0):

continue

x =i

y =int((l+r)/2)

Xi.append(x)

Yi.append(y)

Xi =np.array(Xi)

Yi =np.array(Yi)

Para =leastsq(leatsq_error, [1,20], args=(Xi,Yi))

k, b = Para[0]

center_x = int(sum(Xi)/len(Xi))

center_y = int(sum(Yi)/len(Yi))

center = (center_y, center_x)

theta = -math.atan(k)

distance = (center_y - size[1]/2)*200/size[1]

return (theta, distance)

結 語