,,,,
(中南民族大學 計算機科學學院,武漢 430074)
本文所設計的控制系統主要包括平衡與運動控制系統、位置獲取與分析系統兩個子系統。平衡與運動控制系統以STM32F103單片機為核心處理器,通過姿態傳感器MPU6050監測車身所處的俯仰狀態和狀態變化率,經過PID算法調節,保證小車在按照指令前行的情況下保持平衡;樹莓派和攝像頭組合充當平衡車的眼睛以獲取位置信息。該設計旨在實現雙輪小車在直立平衡的狀態下,根據樹莓派傳回的不同數據沿著黑色賽道行駛,以實現自主循跡的功能。在自主循跡的前提下亦可發展無人傳輸物品,以減少人力的使用。通過對該系統的介紹,重點講述小車循跡控制算法的實現。
1986年,日本電通大學的山滕一雄(Kazuo Yamafuji)教授提出基于倒立擺模型設計自平衡模型的概念,并設計制造了一個兩輪共軸、重心在上的自平衡模型。
近十幾年來,在雙輪自平衡模型的研究上,國內外眾多專家和愛好者們取得了一系列的成果,推動了自平衡技術的發展和成熟[1]。2002年,美國Lego公司設計了一款兩輪自平衡傳感式機器人,簡稱 Legway[2];2005年,美國Southern Methodist University的David P.Anderson教授設計了一款雙輪移動機器模型,取名nBot[3];2007年,深圳固高科技有限公司研制了一款用于教育教學示范的自平衡模型,即GBOT1001機器人[4];2008年,美國Tiger電子公司和日本Sega玩具公司聯合推出了AMP[5](Automated Musical Personality)兩輪自平衡機器人。
作為科學實驗儀器的自平衡模型的涌現,推動兩輪機器人學以及相關學科的研究和發展,同時也啟發了人們向載人運動方向發展的思路。兩輪載人自平衡車具有占地面積小、運動靈活敏捷的特點,由此提供了一種新式交通工具的設計概念。
Segway[6]是第一個推出載人自平衡車品牌并成功推入市場的公司,其由美國著名發明家Dean Kamen 創立,2003年首次推出第一代Segway概念型自平衡車;2005年,中國科學技術大學自動化系和力學系多位教授、博士和研究生研制了自平衡兩輪電動代步車Free Mover[7];2008年,日本的豐田公司(TOYOTA)推出了一款兩輪個人交通工具Winglet[8];2014年小米科技公司投資ninebot,開始致力于解決5公里或10公里內的短途交通問題;2015年4 月ninebot收購Segway公司,成為全球最大的平衡車公司,同年10月19日,ninebot迷你九號平衡車作為小米生態鏈公司的一個新領域產品,正式發布。
所設計的雙輪小車主要由兩大部分組成:第一部分為硬件設計,第二部分為軟件設計。其中核心板模塊、姿態檢測模塊、電機驅動板模塊、Raspberry Pi高清攝像頭板模塊和Raspberry Pi3等組成了系統的硬件部分;姿態檢測算法、PID算法、通信方案的程序設計、小車循跡控制算法構成了系統的軟件部分。小車平衡與控制系統設計過程中涉及的直立行走任務分解、車模直立控制、車模速度控制等借鑒了參考文獻[9]。系統的整體框圖如圖1所示。
圖1 系統整體框圖
平衡車車架由兩臺42步進電機(含步進電機支架、聯軸器)、橡膠輪胎、亞力克載物臺、12 V鋰電池等組成,12 V鋰電池采用DC頭作為輸出接口,方便電源的插拔。
核心板模塊的主控制器芯片為STM32F103VCT6,其包括板載MCU的基本電路、晶振電路、復位電路、LDO電路、MicroUSB接口,至少支持2路硬件UART、預留8路硬件PWM、支持2路I2C、1路硬件SPI、8路模擬輸入;包含8路外部中斷;電平標準采用3.3 V。
姿態檢測模塊主控制器芯片為MPU6050,包含I2C接口、板載LDO電路,其工作電源電壓為3.3 V。MPU6050整合了3 軸陀螺儀、3 軸加速計,并含可藉由第二個I2C端口連接其他廠商加速器、磁力傳感器或其他傳感器的數位運動處理(Digital Motion Processor,DMP)硬件加速引擎,由主要I2C端口以單一數據流的形式向應用端輸出完整的9軸融合演算技術InvenSense 的運動處理資料庫,可處理運動感測的復雜數據。
步進電機驅動模塊采用DRV8825方案,其為TI公司生產的一種高電壓、大電流步進電機驅動芯片,可實現大電流、快速響應的雙極步進電機驅動。DRV8825可以驅動一個兩相四線的步進電機,也可以驅動兩個直流有刷電機,輸入電壓為8.2~45 V,最大電流為1.7 A,可以承受2.5 A的瞬間電壓,可通過PWM輸入來驅動。
Raspberry Pi高清 (HD) 攝像頭板可連接至 Raspberry Pi,可以創建高清視頻和靜止攝影。它利用 Sony 的 IMX219PQ 圖像傳感器提供高速視頻成像和高靈敏度。Raspberry Pi 攝像頭模塊可減少圖像污染,如固定模式噪聲和拖尾效應。 它還具有自動控制功能,如曝光控制、白平衡和亮度檢測。
Raspberry Pi型號 B 是信用卡大的計算機板,當添加了鍵盤、鼠標、顯示屏、電源和已安裝OS的 MicroSD卡時,可以啟動和運行。它是基于微型 ARM的PC,可運行許多通常需要臺式PC的應用(如電子表格、文字處理和游戲),它還可播放高清視頻。
圖2 程序流程圖
雙輪平衡車“跑”起來的過程如圖 2所示。首先,初始化各個外設,主函數將一直在 while(1)循環執行。定時器中斷每 10 ms 觸發一次,從主函數跳到中斷回調相應函數,完成數據采集、處理、控制整個過程(首先獲取當前小車的姿態即傾角,然后讀取樹莓派傳回的數據,處理完后,更新PID輸出,完成電機控制)。執行完立刻返回到上次主函數執行的位置繼續執行。
在雙輪平衡車姿態檢測系統中,MPU6050主要用于獲取小車傾斜角和傾斜角的變化率[10],其中加速度計可以精確計算車體靜止時的角度;陀螺儀的輸出值是旋轉角速率,通過角速率對時間積分即可得到角度值,系統采用STM32F103單片機循環采樣獲取陀螺儀的角速率信息,對采樣值進行累加實現積分功能來計算角度值,此方法存在累積誤差,為獲得可靠的車體傾角值,系統采用卡爾曼濾波對加速度計和陀螺儀的輸出值進行融合。
考慮到現在兩輪平衡車均采用通過 MPU6050姿態傳感器讀取小車當前狀態,當雙輪平衡車的平衡狀態被破壞時,系統采用PID控制算法[10],通過整合車體角度、角速度和車體速度等參數值,輸出PWM信號驅動電機,產生相應的力矩,使得車體保持動態平衡,其結構框圖如圖3所示。
圖3 PID算法框圖
利用Raspberry Pi3處理攝像頭采集信息,并將處理后的信息以字節形式通過串口傳輸到STM32F103VCT6。STM32F103VCT6將接收的信息分解成兩部分,將字節的最高位作為方向,1表示右,0表示左,低七位表示為速度;停止信息分別為0xff和0x7f,運行期間Raspberry Pi3返回的低七位構成的數值大小范圍為0~100,小車初始速度+數值大小×系數等于其中一個輪子的速度,另一個輪子的速度等于小車初始速度-數值大小×系數。
循跡是指小車在白色的紙(地板等)上循黑線行走,本文的循跡是采用攝像頭采集圖像并分析實線,即用攝像頭拍攝白色紙上的黑色引導線,樹莓派利用最小二乘法擬合路徑[11]得到路徑方向和位置,將轉彎信息通過串口的方式傳送給STM32主控制器,STM32F103VCT6將接收到的數據用于轉換為輪子速度和方向的控制。以保證小車能正常沿著黑色引導線行駛。圖4為行駛圖,黑色為引導線,白色線條為切線,車會沿著切線方向行走如圖5所示。
圖4 小車行駛圖
圖5 攝像頭拍攝的圖片并作出切線
樹莓派運行的是基于Debian的樹莓派定制系統Raspbian OS,圖像處理程序使用Python3.5編寫,圖像處理邏輯依賴OpenCV2、Numpy、Pyserial實現。
程序首先從攝像頭獲取圖像,由于樹莓派性能有限,但又要有較為精確的識別,最后采用160×90的分辨率,最后得到的幀率接近60 fps,保證了小車具有較高的反應速度。
程序再對圖像進行隔行掃描,獲取每行的路徑邊緣,取左右邊界中心得到小車軌跡中心。最后以水平向右方向為y軸,豎直向下為x軸,建立坐標系,對得到的離散點使用最小二分法進行直線擬合,如圖6所示。最小二分法擬合函數參數k、b的計算公式如下:
(1)
(2)
圖6 樹莓派返回數據生成的表
最后得到直線斜率k,再轉換為弧度theta即為小車角度偏移,作為小車方向控制的主要參數。同時,取擬合的直線中點,計算到圖像中心的距離dist,作為小車的距離偏移,于是得到小車的轉彎結果為:
turn=k1×theta+k2×dist
turn的絕對值為轉彎幅度,turn的正負代表轉彎方向。最后將turn轉化為8位,通過串口傳輸到STM32主控制器,然后進行相應的轉彎控制。
小車循跡程序設計:
'''隔行掃描'''
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)
'''計算最小二分法擬合函數參數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)