基于光學動作捕捉的室內移動機器人軌跡跟蹤

吳文輝,唐 煒,王 勇,陳 清

(1.江蘇科技大學機械工程學院,江蘇 鎮(zhèn)江 212003;2.徐州市質量技術監(jiān)督綜合檢驗檢測中心,江蘇 徐州 221000)

0 引言

定位技術是移動機器人軌跡跟蹤控制中最基本的環(huán)節(jié)，在室內環(huán)境下，機器人的定位精度一般要求在厘米級。常見的室內定位技術包括基于編碼器和慣性傳感器的相對定位、Wi-Fi定位、無線射頻標簽(RFID)定位、超寬帶(UWB)定位和ZigBee定位等。文獻[1]基于航跡推算和無線射頻識別技術相結合的定位方式，使移動機器人的定位誤差控制在20 cm內；文獻[2]基于UWB技術用ADS-TWR測距，并采用卡爾曼濾波定位，使定位誤差控制在13 cm內；文獻[3]基于ZigBee技術對移動機器人的三邊定位進行研究，使定位誤差在5～7 cm之間，但當機器人離固定節(jié)點30 cm內無法定位。由于移動機器人的定位精度無法得到保證，導致對移動機器人軌跡跟蹤的研究大多還停留在仿真階段，沒有真正地投入應用。基于標記點的光學動作捕捉系統定位精度可達亞毫米級[4]，在要求高精度定位的室內場合下，應用廣泛，如OptiTrack通過對四軸飛行器進行實時跟蹤定位，實現了飛行器集群的協作。本文將采用基于標記點的光學動作捕捉系統對移動機器人進行定位。

針對移動機器人的軌跡跟蹤控制，常用的方法包括反演法[5]、滑模控制[6]、神經網絡法[7]和模糊控制[8]等。其中滑模控制是一種非線性控制方法，由于滑模控制律易產生抖動，使得系統不夠穩(wěn)定;神經網絡法雖然不依賴模型，并能夠消除系統的不確定性和未知擾動，但在線學習時間長，控制算法較為復雜;模糊控制無需建立精確的數學模型，但是模糊規(guī)則的選擇缺乏系統性，且無法實現在線調整。反演法因其在保證系統穩(wěn)定性的同時又具備控制器設計的靈活性，所以成為實際工程應用中最普遍的方式[9]，本文將采用反演法對移動機器人的軌跡跟蹤器進行設計。

1 光學動作捕捉原理

基于標記點的光學動作捕捉系統需要在被測場地上布置多臺紅外相機，記錄被測對象上光學標記球的反射光線，并將光學標記球信息存儲為灰度圖，通過霍夫變換確定標記球在二維圖像中的幾何中心位置，最后對多個相機中二維圖像中標記球的位置信息進行處理，計算出標記球的空間坐標。具體而言，當同一個標記球被2臺以上的攝影機同時拍攝到，則可根據相機參數和計算機視覺原理重建出標記球在三維空間中的坐標信息，具體的重建過程如圖1所示。

圖1 3D坐標點重建示意

令P1和P2為空間點P在相機C1和相機C2圖像中的點，同時2個相機都已經標定完畢，將相機C1和相機C2對應的投影矩陣記為M1和M2，于是有：

(1)

(2)

(3)

(4)

(5)

(6)

式(3)～式(6)中，只有3個未知量，故該線性方程組必然存在解，且解是唯一的。然而，由于數字圖像中數據噪聲的存在，4個等式中任何3個聯立得到的答案可能都不一樣，使用最小二乘法來求解可使誤差最小化，當相機C1中的點P1和相機C2中的點P2互相匹配，便能計算出點P的空間坐標。如果被測物體為一剛體，在被測物體上固定有至少3個不共線的光學標記球，且光學標記球不會隨著被測物體的移動而發(fā)生位置的改變，則可對這些光學標記球建立模型，基于標記點的光學動作捕捉系統通過對模型的識別以及跟蹤[10]，便能實時獲取到剛體的位姿信息。

2 軌跡跟蹤器設計

2.1 運動模型建立

本文研究對象為一款基于麥克納姆輪的全向移動機器人，能夠實現全方向的移動，具有3個自由度，即沿x軸和y軸的平動以及繞z軸的轉動，其中輪轂軸線與輥子軸線的夾角為45°，整體的布局方式如圖2所示，并在其幾何中心建立局部參考坐標系xoy。

圖2 全向移動機器人示意

假設麥克納姆輪與地面接觸充分且無打滑現象，且移動機器人的平臺重心與其幾何中心重合。通過對移動機器人車體速度與車輪轉速之間的關系進行分析[11]，可得到全向移動機器人的正向運動學方程為

(7)

因為雅克比矩陣J的秩rank(J)=3，故全向移動機器人不存在奇異位形，根據廣義逆運算可得到移動機器人的逆向運動學方程為

(8)

J+為雅克比矩陣J的廣義逆矩陣。

2.2 位姿誤差模型建立

圖3 位姿描述示意

根據坐標變化公式可以得到機器人全局位姿誤差與局部位姿誤差之間的關系為

(9)

進一步化簡可得

(10)

設定移動機器人在局部坐標系中的速度vx為全局坐標系中的速度v，即vx=v,vy=0。為了便于描述，將沿X軸方向的速度vX與沿Y軸方向的速度vY合成為v，其中vX=vcosθ，vY=vsinθ。

對式(10)求微分，可得

(11)

2.3 軌跡跟蹤系統設計

移動機器人的軌跡跟蹤控制系統結構如圖4所示，該控制系統的輸入變量為參考位姿Pr、實際位姿P和參考速度ur，其中參考速度由參考線速度vr和參考角速度ωr構成。控制器根據參考位姿Pr與實際位姿P進行比較，得到全局位姿誤差E(t)，通過坐標轉換得到局部位姿誤差e(t)，其次將系統位姿誤差e(t)和參考速度ur作為軌跡跟蹤器的輸入得到機器人平臺的期望速度u，根據運動學模型得到每個車輪的期望轉速，通過速度控制器便能實現整個平臺的運動，最后光學動作捕捉系統對移動機器人的位姿P進行實時采集并反饋給移動機器人，根據此流程反復對軌跡跟蹤控制器進行調節(jié)，從而使移動機器人的位姿誤差趨于零。

圖4 軌跡跟蹤系統結構

為了實現軌跡跟蹤的目的，將xe作為虛擬控制輸入：

(12)

(13)

對式(13)求微分可得

(14)

將xe代入式(14)可得

(15)

(16)

對式(16)求微分可得

(17)

(18)

將式(18)代入式(17),可得

k1sin(arctanω)(-ωxe+vrsinθe)-v+vrcosθe]-

(19)

(20)

k1、k2和k3為任意的正常數，k1對xe進行調節(jié)，k2、k3分別對ye和θe進行調節(jié)。

3 測試平臺搭建

本文采用的光學動作捕捉系統包括6臺光學動作捕捉相機，具體型號為OptiTrack Prime13、運行Motive軟件的上位機PC和支持POE功能的千兆交換機，其中上位機PC與光學動作捕捉相機通過六類網線連接至千兆交換機，并將多個光學標記球通過支架固定在移動機器人上，整個測試環(huán)境如圖5所示。在上位機軟件Motive中，將固定在機器人底盤的光學標記球選中使之成為RigidBody，將其命名為robot，便能實時獲取到所選光學標記球幾何中心的位姿信息，由于移動機器人的底盤為剛體，且光學標記球與移動機器人底盤為剛性連接，所以獲取的數據信息可當作移動機器人整體的位姿信息，同時在Motive中將上位機配置為VRPN[13](virtual-reality peripheral network)服務端。

圖5 測試環(huán)境示意

全向移動機器人的每個車輪都由單獨的電機獨立控制，其硬件模塊主要由樹莓派、基于STM32的主控板、4個電機驅動器和4個直流電機組成，硬件模塊連接情況如圖6所示。安裝在機器人上的樹莓派是室內移動機器人的控制核心，是一款運行Linux操作系統的開源卡片電腦，將其與上位機PC連接在同一個局域網下，基于ROS[14](機器人操作系統)安裝相關的功能包將其配置為VRPN客戶端，通過VRPN協議便能實現上位機PC與樹莓派的數據交互。

圖6 硬件模塊連接示意

當樹莓派與上位機成功建立連接后，ROS中會產生額外的Topic-/vrpn_client_node/robot/pose，該話題的主要內容為移動機器人的位置信息與姿態(tài)信息。在ROS中新建節(jié)點設計軌跡跟蹤器，通過訂閱該話題獲取機器人當前的位姿信息作為軌跡跟蹤器的輸入，輸出為移動機器人的期望速度，并使用高速串口發(fā)送給主控板。主控板接收到數據后，通過運動學方程將其轉化為4個電機的期望轉速，并由CAN總線發(fā)送給電機控制器，電機控制器運行增量式PID控制算法對直流電機進行實時調速，最后電機驅動麥克納姆輪從而實現整個移動機器人的運動。

4 仿真與實驗

為了驗證所研究控制算法的可行性，通過在MATLAB環(huán)境下對移動機器人的直線軌跡和圓周軌跡的跟蹤過程進行仿真，同時將仿真的參數設置到試驗樣機中，在光學動作捕捉系統的測試環(huán)境內，利用其對移動機器人進行實時定位，并令移動機器人按照給定的軌跡進行移動，同時記錄機器人在實際移動過程中的位姿信息。

4.1 直線跟蹤實驗

圖7 位姿誤差時間響應曲線

圖8 實際直線軌跡跟蹤效果

4.2 圓周跟蹤實驗

設定移動機器人的參考運動軌跡為Xr=cos(0.2t)，Yr=sin(0.2t)，θr=π/2+0.2t，即給定軌跡為半徑1 m圓周；令參考輸入為vr=0.25 m/s，ωr=0.2 rad/s；在全局坐標系下初始參考位姿為xr(0)=1 m，yr(0)=0，θr(0)=π/2 rad；同時移動機器人在全局坐標系下的初始位姿為x(0)=1.25 m，y(0)=-0.2 m，θ(0)=2π/3 rad；取k1=5，k2=5.2，k3=2.2。仿真環(huán)境下的位姿誤差時間響應曲線如圖9所示，真實環(huán)境下的圓周軌跡跟蹤效果如圖10所示。

圖9 位姿誤差時間響應曲線

圖10 實際圓周軌跡跟蹤效果

由圖7和圖9可看出，在仿真環(huán)境下，移動機器人最終收斂于參考軌跡，其中直線軌跡為3.8 s左右，圓周軌跡的收斂時間為3.2 s左右，同時發(fā)現不同的參考軌跡、參考速度以及機器人的初始位姿都會對參數k1、k2、k3產生影響；由圖8和圖10可以看出，通過在仿真中選取合適的參數，實際運行過程中，雖然全向移動機器人存在車輪打滑的情況，但在本文設計的跟蹤控制系統作用下，其實際軌跡一直趨于參考軌跡，且偏差較小。仿真結果表明，使用基于標記點的光學動作捕捉系統采集機器人的實時位姿，并將其位姿信息加入控制回路中，可以令全向移動機器人擁有良好的軌跡跟蹤效果。

5 結束語

自主搭建了1臺基于麥克納姆輪的全向移動機器人，利用基于標記點的光學動作捕捉系統對其進行實時追蹤定位，并將光學動作捕捉系統獲取的位姿信息作為移動機器人控制系統的輸入。利用反演法構造出一種全局漸進穩(wěn)定的軌跡跟蹤控制器，通過MATLAB仿真證實了該控制方法的有效性，并在自主搭建的試驗樣機進行驗證。結果表明：通過對軌跡跟蹤器選取合適的參數，可使移動機器人獲取良好的軌跡跟蹤性能，同時利用基于標記點的光學動作捕捉系統精確定位的特性，能夠為移動機器人的室內導航以及運動檢測提供一種新的解決辦法。