，，，，

(中南民族大學(xué) 計(jì)算機(jī)科學(xué)學(xué)院，武漢 430074)

引 言

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

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

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

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

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

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

2 系統(tǒng)整體設(shè)計(jì)

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

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

3 雙輪平衡車硬件組成

3.1 平衡車車架

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

3.2 核心板模塊

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

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

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

3.4 電機(jī)驅(qū)動(dòng)板

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

3.5 Raspberry Pi高清攝像頭板

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

3.6 Raspberry Pi3

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

4 軟件設(shè)計(jì)

4.1 系統(tǒng)流程圖

圖2 程序流程圖

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

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

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

4.3 PID算法

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

圖3 PID算法框圖

4.4 通信方案的實(shí)現(xiàn)與程序設(shè)計(jì)

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

4.5 小車循跡控制

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

圖4 小車行駛圖

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

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

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

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

(1)

(2)

圖6 樹莓派返回?cái)?shù)據(jù)生成的表

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

turn=k1×theta+k2×dist

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

小車循跡程序設(shè)計(jì)：

'''隔行掃描'''

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)

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

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)

結(jié) 語(yǔ)