面向輪式機(jī)器人導(dǎo)航的軌跡推演

肖立武，曾連蓀，朱錦順

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

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

1 基于Arduino的運(yùn)動(dòng)狀態(tài)分析

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

圖1 Arduino的運(yùn)動(dòng)狀態(tài)Fig.1 Arduino motion state

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

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

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

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

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

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

3.1 短時(shí)運(yùn)動(dòng)模型

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

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

將式(5)代入，可得：

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

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

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

化簡(jiǎn)可得：

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

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

參見(jiàn)圖 3，又由三角形相似可得：

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

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

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

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

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

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

4 上位機(jī)軌跡推演原理

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

5 結(jié)論

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

[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的開(kāi)源實(shí)驗(yàn)機(jī)器人平臺(tái)設(shè)計(jì)[J].上海海事大學(xué)學(xué)報(bào),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]王仲民,岳宏,李充寧,等.輪式移動(dòng)機(jī)器人軌跡跟蹤的最優(yōu)控制[J].機(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]劉磊,向平,王永驥,等.非完整約束下的輪式移動(dòng)機(jī)器人軌跡跟蹤[J].清華大學(xué)學(xué)報(bào):自然科學(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/.