面向輪式機器人導航的軌跡推演

肖立武，曾連蓀，朱錦順

（上海海事大學 信息工程學院，上海 201306）

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

1 基于Arduino的運動狀態分析

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

圖1 Arduino的運動狀態Fig.1 Arduino motion state

2 數據解析原理

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

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

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

3 下位機數據的采集

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

3.1 短時運動模型

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

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

將式(5)代入，可得：

圖3 幾何關系圖Fig.3 Geometric diagram

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

則根據三角形的相似性可得：

化簡可得：

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

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

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

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

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

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

3.2 b相對a點的轉角和位移的合成

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

類似地，位移可相應地表示成：

4 上位機軌跡推演原理

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

5 結論

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

[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的開源實驗機器人平臺設計[J].上海海事大學學報,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]王仲民,岳宏,李充寧,等.輪式移動機器人軌跡跟蹤的最優控制[J].機械科學與技術,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].清華大學學報:自然科學版,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/.