基于光學(xué)動(dòng)作捕捉的室內(nèi)移動(dòng)機(jī)器人軌跡跟蹤

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

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

0 引言

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

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

1 光學(xué)動(dòng)作捕捉原理

基于標(biāo)記點(diǎn)的光學(xué)動(dòng)作捕捉系統(tǒng)需要在被測(cè)場(chǎng)地上布置多臺(tái)紅外相機(jī)，記錄被測(cè)對(duì)象上光學(xué)標(biāo)記球的反射光線，并將光學(xué)標(biāo)記球信息存儲(chǔ)為灰度圖，通過霍夫變換確定標(biāo)記球在二維圖像中的幾何中心位置，最后對(duì)多個(gè)相機(jī)中二維圖像中標(biāo)記球的位置信息進(jìn)行處理，計(jì)算出標(biāo)記球的空間坐標(biāo)。具體而言，當(dāng)同一個(gè)標(biāo)記球被2臺(tái)以上的攝影機(jī)同時(shí)拍攝到，則可根據(jù)相機(jī)參數(shù)和計(jì)算機(jī)視覺原理重建出標(biāo)記球在三維空間中的坐標(biāo)信息，具體的重建過程如圖1所示。

圖1 3D坐標(biāo)點(diǎn)重建示意

令P1和P2為空間點(diǎn)P在相機(jī)C1和相機(jī)C2圖像中的點(diǎn)，同時(shí)2個(gè)相機(jī)都已經(jīng)標(biāo)定完畢，將相機(jī)C1和相機(jī)C2對(duì)應(yīng)的投影矩陣記為M1和M2，于是有：

(1)

(2)

(3)

(4)

(5)

(6)

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

2 軌跡跟蹤器設(shè)計(jì)

2.1 運(yùn)動(dòng)模型建立

本文研究對(duì)象為一款基于麥克納姆輪的全向移動(dòng)機(jī)器人，能夠?qū)崿F(xiàn)全方向的移動(dòng)，具有3個(gè)自由度，即沿x軸和y軸的平動(dòng)以及繞z軸的轉(zhuǎn)動(dòng)，其中輪轂軸線與輥?zhàn)虞S線的夾角為45°，整體的布局方式如圖2所示，并在其幾何中心建立局部參考坐標(biāo)系xoy。

圖2 全向移動(dòng)機(jī)器人示意

假設(shè)麥克納姆輪與地面接觸充分且無(wú)打滑現(xiàn)象，且移動(dòng)機(jī)器人的平臺(tái)重心與其幾何中心重合。通過對(duì)移動(dòng)機(jī)器人車體速度與車輪轉(zhuǎn)速之間的關(guān)系進(jìn)行分析[11]，可得到全向移動(dòng)機(jī)器人的正向運(yùn)動(dòng)學(xué)方程為

(7)

因?yàn)檠趴吮染仃嘕的秩rank(J)=3，故全向移動(dòng)機(jī)器人不存在奇異位形，根據(jù)廣義逆運(yùn)算可得到移動(dòng)機(jī)器人的逆向運(yùn)動(dòng)學(xué)方程為

(8)

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

2.2 位姿誤差模型建立

圖3 位姿描述示意

根據(jù)坐標(biāo)變化公式可以得到機(jī)器人全局位姿誤差與局部位姿誤差之間的關(guān)系為

(9)

進(jìn)一步化簡(jiǎn)可得

(10)

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

對(duì)式(10)求微分，可得

(11)

2.3 軌跡跟蹤系統(tǒng)設(shè)計(jì)

移動(dòng)機(jī)器人的軌跡跟蹤控制系統(tǒng)結(jié)構(gòu)如圖4所示，該控制系統(tǒng)的輸入變量為參考位姿Pr、實(shí)際位姿P和參考速度ur，其中參考速度由參考線速度vr和參考角速度ωr構(gòu)成?？刂破鞲鶕?jù)參考位姿Pr與實(shí)際位姿P進(jìn)行比較，得到全局位姿誤差E(t)，通過坐標(biāo)轉(zhuǎn)換得到局部位姿誤差e(t)，其次將系統(tǒng)位姿誤差e(t)和參考速度ur作為軌跡跟蹤器的輸入得到機(jī)器人平臺(tái)的期望速度u，根據(jù)運(yùn)動(dòng)學(xué)模型得到每個(gè)車輪的期望轉(zhuǎn)速，通過速度控制器便能實(shí)現(xiàn)整個(gè)平臺(tái)的運(yùn)動(dòng)，最后光學(xué)動(dòng)作捕捉系統(tǒng)對(duì)移動(dòng)機(jī)器人的位姿P進(jìn)行實(shí)時(shí)采集并反饋給移動(dòng)機(jī)器人，根據(jù)此流程反復(fù)對(duì)軌跡跟蹤控制器進(jìn)行調(diào)節(jié)，從而使移動(dòng)機(jī)器人的位姿誤差趨于零。

圖4 軌跡跟蹤系統(tǒng)結(jié)構(gòu)

為了實(shí)現(xiàn)軌跡跟蹤的目的，將xe作為虛擬控制輸入：

(12)

(13)

對(duì)式(13)求微分可得

(14)

將xe代入式(14)可得

(15)

(16)

對(duì)式(16)求微分可得

(17)

(18)

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

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

(19)

(20)

k1、k2和k3為任意的正常數(shù)，k1對(duì)xe進(jìn)行調(diào)節(jié)，k2、k3分別對(duì)ye和θe進(jìn)行調(diào)節(jié)。

3 測(cè)試平臺(tái)搭建

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

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

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

圖6 硬件模塊連接示意

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

4 仿真與實(shí)驗(yàn)

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

4.1 直線跟蹤實(shí)驗(yàn)

圖7 位姿誤差時(shí)間響應(yīng)曲線

圖8 實(shí)際直線軌跡跟蹤效果

4.2 圓周跟蹤實(shí)驗(yàn)

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

圖9 位姿誤差時(shí)間響應(yīng)曲線

圖10 實(shí)際圓周軌跡跟蹤效果

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

5 結(jié)束語(yǔ)

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