曹華杰,呂傳龍,劉浩東

(1.西南交通大學 信息科學與技術學院,四川 成都　611756;2.西南交通大學 電氣工程學院,四川 成都　611756)

?

構建基于線性CCD的直立行走嵌入式系統(tǒng)

曹華杰1,呂傳龍2,劉浩東2

(1.西南交通大學 信息科學與技術學院,四川 成都611756;2.西南交通大學 電氣工程學院,四川 成都611756)

摘要為設計一種能在狹小和復雜電磁環(huán)境下運動的雙輪直立系統(tǒng),以飛思卡爾公司xs128嵌入式單片機作為控制和處理核心,通過線性CCD進行路況識別,并將MMA7260加速度計和ENC03陀螺儀采集的角度信號通過卡爾曼算法進行濾波,從硬件設計到軟件以及算法調試。通過實驗測試表明,本系統(tǒng)運行穩(wěn)定、可靠,可適用于該種特定場景。

關鍵詞雙輪直立;線性CCD;陀螺儀;加速度計;卡爾曼算法

隨著機器人技術的發(fā)展和成熟,其應用范圍不斷擴大。然而在一些較為復雜的環(huán)境中,如復雜電磁干擾環(huán)境中或狹小的環(huán)境中,傳統(tǒng)的多輪機器人受到沖擊而翻倒后無法繼續(xù)作業(yè)。因此,設計了一種能夠動態(tài)直立的雙輪行走系統(tǒng)[1]。本系統(tǒng)是一個輪式驅動的智能車系統(tǒng),其中涉及到傳感器技術、智能控制、驅動設計以及卡爾曼濾波算法等。這是一種適合復雜電磁環(huán)境下的雙輪直立系統(tǒng)。其對進一步研究智能機器人具有較高的價值和意義[2]。

1直立系統(tǒng)

直立自平衡系統(tǒng)電機是小車的驅動裝置,在未上電的時并不會旋轉產生轉矩。在小車偏離平衡位置,電機不進行控制時,小車是一個自然不穩(wěn)定系統(tǒng),會朝著向系統(tǒng)不穩(wěn)定的方向運動。當通過一定的控制策略控制電機轉矩時,可使小車反向于不穩(wěn)定方向運動,從而實現(xiàn)直立任務。如圖1所示,直立系統(tǒng)的3種狀態(tài)。

圖1　直立系統(tǒng)的3種狀態(tài)

對于3種狀態(tài),電機采取不同的控制策略。平衡狀態(tài):車體處于平衡位置,重心落在平衡點處,電機靜止。前傾狀態(tài):車體向前偏離平衡位置,電機驅動雙輪向前滾動。后傾狀態(tài):車體向后偏離平衡位置,電機驅動雙輪向后轉動。

2系統(tǒng)方案

整個系統(tǒng)的設計分為兩部分,硬件架構和軟件設計。整個系統(tǒng)在車?；A上進行測試,車模電機采用R380電機。線性CCD采用Sony公司的TSL1401模塊,硬件設計包括電源管理模塊、MMA7260加速度計模塊、ENC03陀螺儀模塊、電機驅動模塊、單片機電路,在實際調試過程中還使用了無線模塊。軟件設計主要將CCD采集的信息進行處理,同時還能根據陀螺儀和加速度計采集的信號準確驅動電機運動,實現(xiàn)動態(tài)平衡。整個系統(tǒng)架構如圖2所示。

圖2　系統(tǒng)架構方案

3系統(tǒng)硬件設計

硬件部分由信息采集模塊、控制模塊、電機驅動模塊、電源模塊和通信模塊5部分構成。其中,控制模塊是以MC9S12XS128嵌入式控制器為核心,各傳感器采集信息數(shù)據,傳給控制器來發(fā)出相應指令。電源模塊采用LM2940穩(wěn)壓芯片輸出5 V直流電壓供電,相關設計可參考數(shù)據手冊。

3.1信息采集模塊

信息采集模塊以陀螺儀加速度計和編碼器為主,采用的是ENC03、MMA7260和歐姆龍編碼器,其功能

是采集到車體運動時實際的重力加速度分量、旋轉角速度的模擬電壓量及車體運動速度。直立自平衡小車在穩(wěn)運行狀態(tài)下,頻率在0.3～300 Hz之間,需要設定特定電路截取此區(qū)間的頻率。陀螺儀是系統(tǒng)角速度的測量器件,針對ENC03芯片設計了帶通濾波器。ENC03及帶通濾波器電路如圖3所示。

圖3　陀螺儀濾波電路

3.2電機驅動模塊

電機驅動模塊是以驅動電路為主,PWM是主要控制方式。電機驅動電路有兩種方案,可使用英飛凌公司的集成芯片BTS7971,兩片BTS7971可構成一個全H橋電路,通過單片機產生PWM控制電機的正向和反向轉動。也可使用分立的MOS管進行設計全H橋電路。如圖4所示,設計使用分立的MOS管搭建電機驅動電路,這樣控制的靈敏度更高。

圖4　MOS管電機驅動電路

無線模塊采用藍牙模塊實現(xiàn)電腦與自平衡系統(tǒng)的數(shù)據傳輸,主要在調試中使用。

4系統(tǒng)軟件及算法設計

4.1程序執(zhí)行流程

采用Netroworks公司Codewarrior IDE進行程序編譯以及實現(xiàn)在線調試等功能。程序先初始化各模塊參數(shù),然后可在人機交互界面進行參數(shù)設置。調試程序時,在液晶上顯示相關數(shù)據,并可通過按鍵進行調節(jié)。對于加速度計和陀螺儀采集的信號通過卡爾曼算法進行融合,對于電機控制使用位置式PID算法進行調節(jié),軟件系統(tǒng)的執(zhí)行結構如圖5所示。

圖5　軟件執(zhí)行結構

4.2卡爾曼算法

小車的傾角信息處理是雙輪直立平衡系統(tǒng)的重要部分。陀螺儀和加速度計分別為不同的角度傳感器,在系統(tǒng)中采用雙傳感器進行角度測量。然而,加速度計易受到自身器件的噪聲、電機噪聲以及陀螺儀混疊的影響,而且在高速運動體系下具有嚴重的滯后特性;陀螺儀噪聲小,但存在著溫度漂移現(xiàn)象,靜態(tài)性能不足以達到要求。為保留加速度計的靜態(tài)性能,陀螺儀的動態(tài)性能,使兩者優(yōu)勢可相互結合,采用離散卡爾曼算法對陀螺儀和加速度計數(shù)據進行融合[3-4]。

卡爾曼濾波算法利用反饋控制來實現(xiàn)對狀態(tài)的估計,其根據前一時刻的系統(tǒng)狀態(tài),估計出當前的系統(tǒng)狀態(tài),然后根據當前時刻的實際觀測值作為反饋,來修正估計狀態(tài)。因此,卡爾曼濾波算法可分為狀態(tài)預測和測量修正兩個步驟[5]。狀態(tài)預測方程由前一刻系統(tǒng)狀態(tài)和噪聲方差預測出當前的系統(tǒng)狀態(tài);而測量修正方程則負責修正,在狀態(tài)預測方程中得到的預測估計狀態(tài)中加入當前實際傳感器的信號,最終得到修正后的系統(tǒng)狀態(tài)。

卡爾曼算法由狀態(tài)預測方程和測量修正方程組成,其中狀態(tài)預測方程由式(1)和式(2)組成。其中式(1)為向前推算狀態(tài)變量,式(2)為向前推算誤差協(xié)方差

X(k)=AX(k-1)+BU(k)

(1)

P(k)=AP(k-1)AT+Q

(2)

測量修正方程由式(3)～式(5)組成。其中,式(3)為計算卡爾曼增益;式(4)是根據測量值進行修正估計;式(5)為更新協(xié)方差計算

K(k)=P(k)HT(HP(k)HT+R)-1

(3)

X(k)=X(k)+K(k)(Z(k)-HX(k))

(4)

P(k)=(1-K(k)H)P(k)

(5)

其中,X(k-1)和P(k-1)為初始估計值。

卡爾曼濾波算法,即利用線性系統(tǒng)狀態(tài)方程,通過系統(tǒng)輸入輸出觀測數(shù)據,對系統(tǒng)狀態(tài)進行最優(yōu)估計的算法。狀態(tài)預測方程如式(6)所示,修正方程如式(7)所示

(6)

(7)

當狀態(tài)預測方程和測量修正方程循環(huán)運算時,就能使輸出信號越來越接近真值。使用卡爾曼濾波算法能有效過濾信號中的噪聲。

根據卡爾曼濾波原理。設計濾波算法如下:以小車的車身傾角φ,陀螺儀的漂移溫差b,作為狀態(tài)向量,相應的狀態(tài)方程如下

(8)

式中,ωgro為陀螺儀的實際輸出角速度;φocce為加速度計的實際輸出;ω1為加速度計過程噪聲;ω2為陀螺儀過程噪聲;v為加速度觀測噪聲。

卡爾曼濾波過程的協(xié)方差,測量協(xié)方差如式(9)所示

(9)

qacce和qgro速度計和陀螺儀測量的協(xié)方差,協(xié)方差取值的物理意義是對傳感器信號品質的一種評估。例如,若認為加速度計測量的數(shù)據更可靠,便可將qacce值設置的較小,從而在濾波過程中體現(xiàn)這種可靠性。R矩陣代表測量誤差的協(xié)方差。同理,R取值的大小,就代表測量噪聲的大小。Q和R矩陣的取值,通過大量實驗獲取比較合理的參數(shù)值[6]。

卡爾曼算法容易通過單片機編程實現(xiàn),而且不需要轉換到頻域進行高通或低通濾波,且只需保存上一狀態(tài)估計值,可對現(xiàn)場數(shù)值實時進行更新,濾波效果能達到控制要求。經過卡爾曼濾波器得到的角度波形示意圖,如圖6所示,加速度計具有較大的噪聲,單獨使用加速度計無法使車保持直立狀態(tài),通過卡爾曼濾波器之后輸入的角度平滑,且跟隨性良好。

圖6　卡爾曼濾波結果

4.3PID算法

PID 控制器是最實用的一種控制器,其是一種滯后-超前校正裝置,PID控制器同時兼具PI和PD兩種調節(jié)器的作用,前者用于提高系統(tǒng)的穩(wěn)態(tài)精度。后者用于改善系統(tǒng)的動態(tài)性能。兩者相輔相成,使校正后的系統(tǒng)具有優(yōu)良的性能[7-8]。PID算法根據實際使用有兩種,分別為位置式PID算法和增量式PID算法。本系統(tǒng)中使用位置式PID算法。

對于傳統(tǒng)的PID控制結構,將比例、積分、微分項中的積分項,對導數(shù)項用后向差分逼近[9],得到數(shù)字PID控制器的基本位置算式如式(10)所示

(10)

其中,T是采樣時間;kp、Ti、Td為3個待調參數(shù),在用實際代碼實現(xiàn)算法時,處理成式(11)形式

PreU=Kp×error+Ki×Integral+Kd×derror

(11)

經過大量的調試之后,3個閉環(huán)均選擇了PD控制器。3個閉環(huán)按照線性疊加的關系進行運算。其中直立PID運算周期為6ms,速度PID運算周期為120ms,方向PID周期為6ms。系統(tǒng)PID的控制策略如下:(1)直立閉環(huán)采用負反饋控制,使用PD控制器,控制量為車身傾角;(2)速度閉環(huán)采用負反饋控制,使用PI控制器,控制量為車體速度;(3)方向控制為負反饋控制,使用PD控制器,控制量為轉向偏差量。

4.4CCD圖像處理

經過CCD采集得到的圖像是一幅一維陣列,陣列元素成為灰度值或者強度值。圖像在經過數(shù)字單片機采集之前是一系列連續(xù)強度函數(shù)的集合,場景信息包含在這些強度之中。圖像通常被劃分為256個不同的灰度值,在對某些應用中,也常有32、64、128或512個灰度級的情況。對于傳感器來說,圖像灰度級越高,圖像質量越好,但所需的內存越大。為保證得到足夠準確的圖像信息,在本系統(tǒng)中將圖像劃分為4 096個灰度級,相應的每個像素點占用內存為2Byte,整幅圖像占用內存為256Byte。

本課題中,在保證圖像成像質量的基礎上盡可能減少每幅圖像的處理時間,改進后的圖像處理周期為6ms,即可達到166幅/s。本設計選用閾值分割法,即設定靜態(tài)閾值,大于閾值的像素點置1,反之置0。

5結束語

雙輪直立系統(tǒng)是一個復雜的控制系統(tǒng),其中涉及到傳感器技術、智能控制、驅動設計以及卡爾曼濾波算法等。本文先給出了系統(tǒng)的設計框架,分別從硬件和軟件上分別設計了該系統(tǒng)。在硬件設計部分主要介紹了陀螺儀濾波電路和電機驅動電路。軟件設計部分,主要說明了卡爾曼算法以及控制的PID算法。使用液晶調節(jié)卡爾曼參數(shù)和PD參數(shù),如圖7所示。實物效果如圖8所示,整個系統(tǒng)經過測試,能較好地滿足動態(tài)直立的要求。

圖7　液晶調試界面

圖8　實物效果圖

參考文獻

[1]繆壽洪,曹其新,孔祥宣.自行直立雙輪移動機器人設計與實現(xiàn)[J].上海交通大學學報,2006,40(11):1832-1835,1841.

[2]郭玉龍,于永會,何志琴.基于MC9S12XS128的直立輪式智能小車設計[J].現(xiàn)代機械,2013(5):31-34.

[3]崔景泰.卡爾曼濾波及實時應用[M].北京:清華大學出版社,2013.

[4]王仲生.智能檢測與控制技術[M].西安:西北工業(yè)大學出版社,2010.

[5]馬磊,史習智.多智能體系統(tǒng)中一致性卡爾曼濾波的研究進展[J].西南交通大學學報,2011,46(2):287-293.

[6]王振宇.兩輪移動機器人平衡控制系統(tǒng)的研究[D].成都:西南交通大學,2010.

[7]李凡紅.兩輪自平衡小車系統(tǒng)[D].北京:北京交通大學,2010.

[8]張陽.MC9S12XS單片機原理及嵌入式開發(fā)[M].北京:電子工業(yè)出版社,2011.

[9]肖文健,李永科.基于增量式PID控制算法的智能車設計[J].信息技術,2012(10):125-127.

A Vertical Working Embedded System Based on Linear CCD

CAO Huajie1,Lü Chuanlong2,LIU Haodong2

(1.School of Information Technology,Southwest Jioatong University,Chengdu 611756,China;

2.School of Electrical Engineering,Southwest Jioatong University,Chengdu 611756,China)

AbstractA two wheeled upright systems for narrow and complex electromagnetic environment is designed with a Freescale xs128 embedded micro-controller as the controlling and processing core.The linear CCD and MMA7260 accelerometer and ENC03 gyroscope are used for road condition detection and Kalman algorithm for filtering.Hardware and software design is performed and the algorithm debugged.The experimental tests show that the system is stable and reliable,and can be applied to this particular scene.

Keywordsvertical working;linear CCD;gyroscope;accelerometer;Kalman

中圖分類號TP316.2

文獻標識碼A

文章編號1007-7820(2016)02-058-05

doi:10.16180/j.cnki.issn1007-7820.2016.02.015

作者簡介:曹華杰(1989—),男,碩士研究生。研究方向:嵌入式系統(tǒng)應用等。

基金項目:全國大學生創(chuàng)新基金資助項目(201310292023Z);江蘇省大學生創(chuàng)新基金資助項目(201310292039Y)

收稿日期:2015- 06- 16