肖立武，曾連蓀，朱錦順

（上海海事大學(xué) 信息工程學(xué)院，上海 201306）

在移動機器人的導(dǎo)航領(lǐng)域，軌跡研究探索不少但領(lǐng)域內(nèi)與軌跡推演相關(guān)的文獻(xiàn)較少。本文探討不依靠其它慣導(dǎo)器件（如電磁羅盤）輔助，僅使用機器人驅(qū)動電機配套的編碼器解析機器人的相對位移和航向角數(shù)據(jù)，將解算得到數(shù)據(jù)發(fā)給上位機，上位機對該數(shù)據(jù)進(jìn)行解析并依據(jù)這些解析得到的數(shù)據(jù)繪制實時軌跡，最終實現(xiàn)輪式機器人的軌跡推演。該方案增強了系統(tǒng)的緊湊度和可靠性，同時也節(jié)省了系統(tǒng)的構(gòu)建成本。本文所提算法框架在本研究組所開發(fā)的機器人平臺Carmela[3]上驗證。

1 基于Arduino的運動狀態(tài)分析

在分析Arduino的運動狀態(tài)[4]的時候，Arduino只有兩種運動狀態(tài)（如圖 1）。一種是類似如處在世界坐標(biāo)系下的a點（此時的a點是靜止時，也即Arduino的最開始狀態(tài)）坐標(biāo)（通過最初的測量可知），另一種則是Arduino運動一會到達(dá)b點的狀態(tài)，此時我們只知道b點位于以a點為原點的相對坐標(biāo)系下的坐標(biāo)（通過傳感器測量值解算可知）。當(dāng)我們解算出b點處于世界坐標(biāo)系下的坐標(biāo)時，b點此時的狀態(tài)就轉(zhuǎn)換成了a點時的狀態(tài)，也即第一種，然而當(dāng)Arduino從b點運動到另外一個點，不妨取做c點，此時c點又是相對于b的相對坐標(biāo)，b和c又轉(zhuǎn)換成了a和b時的狀態(tài)形式，Arduino的運動狀態(tài)不斷循環(huán)往復(fù)。

圖1 Arduino的運動狀態(tài)Fig.1 Arduino motion state

2 數(shù)據(jù)解析原理

根據(jù)Arduino的運動模型的分析，不難分析出求出世界坐標(biāo)系下b點的坐標(biāo)和航向角是數(shù)據(jù)解析的關(guān)鍵。將b點在相對坐標(biāo)系中進(jìn)行相應(yīng)的分解如圖 2所示，計算出b點在世界坐標(biāo)系下x軸的坐標(biāo)Xwb如式(1)所示；將b點在相對坐標(biāo)系中進(jìn)行相應(yīng)的分解如圖 2，計算出b點在世界坐標(biāo)系下y軸的坐標(biāo)Ywb如式(2)所示；將b點在相對坐標(biāo)系中進(jìn)行相應(yīng)的分解如圖 2，計算出世界坐標(biāo)系下航向角θwb如式(3)所示。

圖2 計算航向參數(shù)的分解圖Fig.2 The vehicle diagram of calculating Xwb

最終推到出數(shù)據(jù)解析的數(shù)學(xué)原理合成公式[2]如式(4)所示。

3 下位機數(shù)據(jù)的采集

Ardunio[1]作為下位機平臺它的重要職責(zé)在于：1)執(zhí)行上位機傳達(dá)的命令以及反饋命令的執(zhí)行情況；2)采集數(shù)據(jù)給上位機；當(dāng)ardunio采集數(shù)據(jù)給傳遞個上位機時，ardunio解算到b點相對a點的坐標(biāo)，是上位機軌跡推演時進(jìn)行數(shù)據(jù)解析的前提所在。

3.1 短時運動模型

微控制器中斷編程系數(shù)、編碼器的分辨率n、外部中斷辨識出的編碼器的脈沖數(shù)與輪子實際的轉(zhuǎn)數(shù)之間的關(guān)系如

機器人的運動非勻速，特別是加入PID控制時，線速度和角速度都會頻繁變化，但在極短的時間內(nèi)（例如本文所設(shè)定的時間間隔為0.2 s）或極小的位移內(nèi)，機器人的短時運動可假設(shè)為恒速，機器人運動所駛過的圓弧可近似為直線。輪式機器人的轉(zhuǎn)彎運動是其它多種運動狀態(tài)的典型表現(xiàn)，例如直行是轉(zhuǎn)彎運動的特例，由轉(zhuǎn)彎運動推導(dǎo)出的相對運動模型具有普適性，可描述所有運動狀態(tài)。記機器人左右輪距為B，同側(cè)的前后輪距為L，輪子的周長為C。本文將機器人的質(zhì)心等效為左前輪和右前輪的軸中心點，即三輪機器人模型[5]，在采樣周期足夠短，機器人運行的位移足夠小的前提下完成左轉(zhuǎn)動作，其詳細(xì)的幾何關(guān)系示于圖3。設(shè)在此采樣周期內(nèi)機器人左轉(zhuǎn)Φ，左、右輪距轉(zhuǎn)彎圓心的半徑分別為Rleft和Rright；左、右輪的位移分別為Sleft和Sright；可求得機器人左前輪和右前輪處編碼器的計數(shù)分別為Nfrontleft和Nfrontright，它們的符號表征了正轉(zhuǎn)和反轉(zhuǎn)，相應(yīng)地，左前輪和右前輪的實際轉(zhuǎn)數(shù)為Mfrontleft和 Mfrontright。 則：

將式(5)代入，可得：

圖3 幾何關(guān)系圖Fig.3 Geometric diagram

如圖3所示，并由圓心角的定義可知：

則根據(jù)三角形的相似性可得：

化簡可得：

將式(12)代入式(10)得:

將式(8)和式(9)代入式(13)得:

參見圖 3，又由三角形相似可得：

將式(12)代入式(15)，并化簡得：

將式(8)和式(9)代入式(16)可得：

則式(14)和式(17)構(gòu)成了從編碼器脈沖計數(shù)解析兩個采樣時刻間相對運動的短時運動模型，當(dāng)將此左轉(zhuǎn)模型推廣到一般運動時，轉(zhuǎn)角Φ的符號表征了機器人的運動狀態(tài)：當(dāng)Φ大于零時為左轉(zhuǎn)；當(dāng)Φ小于零時為右轉(zhuǎn)；當(dāng)Φ等于零時直行；當(dāng)S大于零時為前進(jìn)；當(dāng)S小于零時為后退；當(dāng)S等于零時機器人停止。

3.2 b相對a點的轉(zhuǎn)角和位移的合成

機器人在一個較長的時間內(nèi)的動作可分解成若干短時運動周期，機器人作業(yè)時的任意動作都可基于短時運動模型合成，一個長時運動的轉(zhuǎn)角可表示為：

類似地，位移可相應(yīng)地表示成：

4 上位機軌跡推演原理

上位機的軌跡推演原理由兩部分構(gòu)成：數(shù)據(jù)解析原理和視覺暫留現(xiàn)象。根據(jù)上述數(shù)據(jù)解析原理設(shè)計出了核心的C++數(shù)據(jù)解析代碼compound函數(shù)。視覺暫留現(xiàn)象（Visual staying phenomenon，duration of vision）又稱“余暉效應(yīng)”，1824年由英國倫敦大學(xué)教授皮特‘馬克’羅葛特在他的研究報告《移動物體的視覺暫留現(xiàn)象》中最先提出。人眼在觀察景物時，光信號傳入大腦神經(jīng)，需經(jīng)過一段短暫的時間，光的作用結(jié)束后，視覺形象并不立即消失，這種殘留的視覺稱“后像”，視覺的這一現(xiàn)象則被稱為“視覺暫留”。軌跡的繪制部分就必須借助視覺暫留現(xiàn)象：由于電腦的主頻很高，人會產(chǎn)生眼視覺暫留，根本無法分辨一次次重新繪制的靜態(tài)圖，而在人眼中呈現(xiàn)出來的就是一幅動態(tài)的圖像。使用Qt的開源版本[6]中自帶的QPainterPath類，使用解析出來的數(shù)據(jù)進(jìn)行路徑繪制，那么下一次的繪制則是在上一次的路徑的基礎(chǔ)之上lineTo到剛剛獲得的點上。從而我們就可以實現(xiàn)推演從輪式機器人的運動軌跡。

5 結(jié)論

使用課題組搭建的低成本四驅(qū)輪式機器人平臺Carmela[3]驗證本文提及的算法。該平臺的3個關(guān)鍵參數(shù)m,n和ω的取值分別為43.7，16和2。實驗前，通過角度測量在地面上標(biāo)記了不同角度（45度、90度和180度）所對應(yīng)的線。實驗中，手工遙控機器人轉(zhuǎn)動并逼近不同角度所對應(yīng)的基準(zhǔn)線，通過使用基于第3節(jié)介紹的算法，機器人驅(qū)動電機配套的編碼器解算機器人的相對位移和航向角數(shù)據(jù)，并通過RS232發(fā)送到上位機，上位機依據(jù)第2節(jié)介紹的算法實時解析這些上傳的數(shù)據(jù)，并將解析得到的數(shù)據(jù)繪制實時軌跡，實地實驗驗證了輪式機器人相對位移解算以及對上報數(shù)據(jù)進(jìn)行解析算法的性能。本算法為輪式機器人的軌跡估算推演提供了新思路，實驗結(jié)果證明了該算法的正確性與實用性，可以很好的解決輪式機器人導(dǎo)航中軌跡推演的問題。

[1]Arduino.An open-source electronics prototyping platform[EB/OL].[2012-12-18]http://www.arduino.cc/.

[2]Smith R,Self M,Cheeseman P.Estimating uncertain spatial relationships in robotics[J].Autonomous Robot Vehicles,1990:167-193.

[3]孫作雷,張波,曾連蓀.基于Arduino和Qt的開源實驗機器人平臺設(shè)計[J].上海海事大學(xué)學(xué)報,2013,34(2)：80-83.SUN Zuo-lei,ZHANG Bo,ZENG Lian-sun.Low-cost opensource experimental robot platform design based on Arduino and QT[J].Journal of Shanghai Maritme Univ,2013,34(2)：80-83.

[4]王仲民,岳宏,李充寧,等.輪式移動機器人軌跡跟蹤的最優(yōu)控制[J].機械科學(xué)與技術(shù),2006,25(1):21-23.WANG Zhong-min,YUE Hong,LI Chong-ning,et al.Optional Control of Wheeled Mobile Robot Trajectory Tracking[J].Mechanical Science and Technology,2006,25(1):21-23.

[5]劉磊,向平,王永驥,等.非完整約束下的輪式移動機器人軌跡跟蹤[J].清華大學(xué)學(xué)報:自然科學(xué)版,2007,42(S2):1884-1889.LIU Lei,XIANG Ping,WANG Yong-ji,et al.Trajectory tracking of a nonholonomic wheeled mobile robot[J].J Tsinghua Uni:Sci&Tech,2007,42(S2):1884-1889.

[6]Digia Plc.Qt open-source project[EB/OL].[2013-01-18]http://qt-project.org/.