基于視覺的移動機(jī)器人相對位姿優(yōu)化估計(jì)

黃雙發(fā)，周樂來，2*，李貽斌，2

（1.山東大學(xué)控制科學(xué)與工程學(xué)院，濟(jì)南 250061；2.智能無人系統(tǒng)教育部工程研究中心，濟(jì)南 250061）

1 引 言

機(jī)器人在信息化時(shí)代的大量應(yīng)用成為了當(dāng)下生產(chǎn)效率提高的重要原因，移動機(jī)器人的廣泛應(yīng)用解放了大量勞動密集型產(chǎn)業(yè)，有效提高了工農(nóng)業(yè)生產(chǎn)效率；同時(shí)其在高危、高溫、水下等環(huán)境的應(yīng)用，提高了人類面對復(fù)雜危險(xiǎn)環(huán)境的應(yīng)對能力。為了提高復(fù)雜環(huán)境下機(jī)器人的應(yīng)對能力，同時(shí)滿足越發(fā)復(fù)雜的任務(wù)需求，現(xiàn)主要有兩種解決方案，一種是提高單機(jī)器人的感知能力與執(zhí)行能力，通過加裝更多的傳感器與執(zhí)行機(jī)構(gòu)實(shí)現(xiàn)單機(jī)器人對復(fù)雜環(huán)境的適應(yīng)能力；另一種是通過多機(jī)器人協(xié)同編組，將多個(gè)低成本的機(jī)器人編組成一個(gè)機(jī)器人團(tuán)隊(duì)，分別攜帶不同的傳感器與執(zhí)行機(jī)構(gòu)，組合起來能夠執(zhí)行更加多樣化的分布式任務(wù)，并提高任務(wù)執(zhí)行效率，同時(shí)無線通信模塊配置升級為多機(jī)器人協(xié)同編組控制面對復(fù)雜環(huán)境、執(zhí)行復(fù)雜任務(wù)提供了更高的實(shí)時(shí)性與更好的魯棒性［1］。多機(jī)器人協(xié)同編組相對于單機(jī)器人面對危險(xiǎn)環(huán)境時(shí)具有更好的抗干擾、抗故障能力，其執(zhí)行效率高、執(zhí)行任務(wù)多樣且對環(huán)境的適應(yīng)性更好，可以實(shí)現(xiàn)“1+1>2”的效果。因此，多機(jī)器人協(xié)同編組控制成為了當(dāng)下機(jī)器人發(fā)展的必要一環(huán)。

移動機(jī)器人控制技術(shù)的一個(gè)重要前提就是獲取移動機(jī)器人每個(gè)時(shí)間的精確位姿，即移動機(jī)器人定位問題，移動機(jī)器人的定位方法主要有GPS衛(wèi)星定位法、慣性導(dǎo)航定位法、視覺靶標(biāo)定位法［2］。協(xié)同編組的多移動機(jī)器人的精確定位更是每個(gè)移動機(jī)器人模塊完成相關(guān)分布式任務(wù)的基礎(chǔ)，通過結(jié)合多樣的定位傳感器，即可實(shí)現(xiàn)從全局到局部的協(xié)同定位。多移動機(jī)器人編組定位方法多通過協(xié)同定位的方法，獲取部分移動機(jī)器人模塊的全局位姿以及其他機(jī)器人模塊相對其的位姿信息，進(jìn)而將各個(gè)機(jī)器人位姿展示在全局地圖中。相對于單移動機(jī)器人，多移動機(jī)器人可以搭載更加多樣的傳感器，具有更好的環(huán)境感知能力與信息處理能力，復(fù)雜環(huán)境下的多機(jī)器人協(xié)同定位具有更好的定位魯棒性。

多移動機(jī)器人協(xié)同控制技術(shù)的重點(diǎn)就是協(xié)同定位，即獲取多移動機(jī)器人模塊間的位姿，機(jī)器人定位技術(shù)可分為絕對式定位技術(shù)和相對式定位技術(shù)［3-4］。絕對式定位技術(shù)計(jì)算機(jī)器人在環(huán)境中的實(shí)時(shí)絕對位姿，不受機(jī)器人初始狀態(tài)與歷史狀態(tài)影響，常用的定位技術(shù)有無線電定位、聲波定位、環(huán)境輔助定位等方式，其中無線電定位與聲波定位的定位精度較低，不適合近距離高精度的定位，環(huán)境輔助定位精度較高但計(jì)算量大且對環(huán)境要求較高。相對式定位技術(shù)則分為依靠初始狀態(tài)與歷史狀態(tài)對當(dāng)前位姿進(jìn)行估算的本地測量，以及借助某些特征直接測定團(tuán)隊(duì)中其他機(jī)器人位姿的外感測量，常用的機(jī)器人相對定位技術(shù)有慣性導(dǎo)航定位技術(shù)（本地測量）和視覺測量定位技術(shù)（外感測量），慣性導(dǎo)航定位技術(shù)（加速度計(jì)、里程計(jì)、陀螺儀）定位精度較高但存在累計(jì)誤差和漂移［5］，視覺測量定位技術(shù)（模板匹配［6］、特征識別［7］）定位精度更高但距離較近，受光照與視角影響。多機(jī)器人協(xié)同運(yùn)作需要在靠近時(shí)獲取高精度的相對位姿，故選取相對式定位技術(shù)來監(jiān)測移動機(jī)器人的位姿信息。

如上所述，慣性導(dǎo)航定位系統(tǒng)與視覺測量定位技術(shù)存在互補(bǔ)的特性［8］，通過對兩種傳感器信息進(jìn)行融合，可以解決視覺定位的丟幀等隨機(jī)誤差影響，并避免慣性導(dǎo)航系統(tǒng)的累計(jì)誤差影響，可以獲得精度更高、魯棒性更好的相對位姿監(jiān)測方式。慣性測量定位系統(tǒng)的精確短期估計(jì)與視覺監(jiān)測系統(tǒng)的豐富環(huán)境感知是公認(rèn)的互補(bǔ)定位方式，具有很好的應(yīng)用研究前景?，F(xiàn)有許多基于慣性測量單元配合視覺監(jiān)測系統(tǒng)組成視覺慣性里程計(jì)進(jìn)行定位估算的研究：Fu等采用RGBD相機(jī)和慣性測量單元相結(jié)合的方法對運(yùn)動物體和靜態(tài)物體進(jìn)行分離，提高了SLAM系統(tǒng)在動態(tài)場景中的定位精度和適應(yīng)性［9］；Schubert等提出了一種視覺配合慣性傳感器的時(shí)間同步方法［10］；Leutenegger等提出了一種針對非線性系統(tǒng)的視覺慣性導(dǎo)航的算法［11］；同時(shí)也有針對多機(jī)器人的視覺慣性定位系統(tǒng)［12-13］。

現(xiàn)有的融合方法大多采用IMU等慣性傳感器與RGBD或者魚眼相機(jī)等視覺傳感器。然而在實(shí)際工程應(yīng)用中，IMU等慣性傳感器較容易受到環(huán)境磁場的影響，導(dǎo)致定位精度的降低，為了減少環(huán)境干擾對慣性定位的影響，本文采用車輛里程計(jì)作為慣性輸入量，誤差較易進(jìn)行估算。對于視覺傳感器來說，RGBD相機(jī)成本較高且監(jiān)測視野較小，魚眼相機(jī)則需要進(jìn)行高精度標(biāo)定才能等效理想幀相機(jī)［14］，為了保證視覺系統(tǒng)能夠?qū)崿F(xiàn)全角度的監(jiān)測，使相機(jī)能夠監(jiān)測全角度的移動機(jī)器人，同時(shí)減少計(jì)算量、提高解算速率，使用單目相機(jī)組成全角度的監(jiān)測系統(tǒng)具有搭建簡單、計(jì)算量小、成本更低的特點(diǎn)，故本實(shí)驗(yàn)選用4個(gè)單目相機(jī)組成全角度的視覺相對位姿監(jiān)測系統(tǒng)。

針對多機(jī)器人多傳感器數(shù)據(jù)融合問題，卡爾曼濾波及其衍生算法已經(jīng)得到了廣泛的研究［15-17］，通過搭建移動機(jī)器人先驗(yàn)?zāi)Ｐ蛯Χ鄠鞲衅鲾?shù)據(jù)進(jìn)行融合濾波優(yōu)化。本文利用車輛里程計(jì)的信息對視覺定位進(jìn)行優(yōu)化，針對阿克曼轉(zhuǎn)向的車輛模型［18］，搭建了阿克曼轉(zhuǎn)向機(jī)器人先驗(yàn)?zāi)Ｐ停錉顟B(tài)方程中線速度與角速度由車輛里程計(jì)反饋得到，視覺系統(tǒng)的相對位姿作為系統(tǒng)觀測值。對此非線性系統(tǒng)，采用擴(kuò)展卡爾曼濾波算法，對視覺傳感器與車輛里程計(jì)器信息進(jìn)行融合優(yōu)化，融合了兩種傳感器的互補(bǔ)特性，進(jìn)一步提高了相對位姿監(jiān)測精度，并且消除了丟幀與系統(tǒng)累計(jì)誤差，提高了系統(tǒng)定位魯棒性。

2 位姿監(jiān)測系統(tǒng)

相對位姿監(jiān)測系統(tǒng)的首要任務(wù)是通過視覺傳感器與車輛里程計(jì)獲取移動機(jī)器人之間的定位信息。為了能夠在全方位監(jiān)測到其他移動機(jī)器人，獲取相對的位姿信息，在移動機(jī)器人上方搭建4個(gè)方向上的4個(gè)單目相機(jī)以實(shí)現(xiàn)移動機(jī)器人的全方位位姿監(jiān)視，單目相機(jī)及其識別靶標(biāo)在移動機(jī)器人上的安裝位置如圖1所示，其中識別靶標(biāo)處于相機(jī)的正下方。

圖1 機(jī)器人傳感器安裝示意圖Fig.1 Schematic diagram of robot sensor installation

單目相機(jī)的監(jiān)測范圍與相機(jī)的視角有關(guān)，本實(shí)驗(yàn)相機(jī)采用4個(gè)方向正方形排列的視角為120°廣角的USB單目相機(jī)，移動機(jī)器人為558 mm×492 mm×420 mm阿克曼轉(zhuǎn)向機(jī)器人，單目相機(jī)安裝示意圖如圖2所示。

圖2 單目相機(jī)監(jiān)視角度示意圖Fig.2 Monocular camera surveillance angle diagram

式（1）為相機(jī)監(jiān)測范圍計(jì)算公式，考慮到車身尺寸對視覺監(jiān)測的影響，同時(shí)為了保證單目相機(jī)能夠?qū)崿F(xiàn)全角度的位姿監(jiān)測，經(jīng)計(jì)算，以車輛中心為相機(jī)安裝中心的最長安裝距離a為208 mm，本文選取單目相機(jī)距離為200 mm。

本實(shí)驗(yàn)選用二維碼靶標(biāo)作為車輛識別靶標(biāo)進(jìn)行目標(biāo)的識別，二維碼靶標(biāo)包含多個(gè)容易識別的特征。通過計(jì)算可以獲取相機(jī)與二維碼之間的相對位姿信息，并識別二維碼編碼信息，即可同時(shí)獲取移動機(jī)器人的相對位姿及編號，具體監(jiān)測步驟如下：

（1）篩選圖像中二維碼的輪廓，確定二維碼靶標(biāo)位置；

（2）監(jiān)測二維碼標(biāo)記，識別二維碼信息；

（3）結(jié)合相機(jī)內(nèi)外參數(shù)以及二維碼參數(shù)，計(jì)算相機(jī)姿態(tài)；

（4）重復(fù)監(jiān)測二維碼并計(jì)算更新相機(jī)位姿；

（5）輸出相機(jī)相對于二維碼靶標(biāo)的相對位姿。

通過對二維碼角點(diǎn)的PnP解算即可獲得移動機(jī)器人之間的相對位姿信息(x，y，z；α，β，γ)，本實(shí)驗(yàn)為室內(nèi)環(huán)境下相對位姿監(jiān)測，可以忽略Z軸方向上的位姿變化，同時(shí)提高優(yōu)化算法速度，提高系統(tǒng)實(shí)時(shí)性。簡化后的相對位姿信息可表示為(x，y， 0；0，0，γ)。此外，考慮到相機(jī)以及靶標(biāo)相對機(jī)器人中心的相對距離(△x，△y)以及誤差偏角△γ，實(shí)際相對位姿可表示為(x+△x，y+△y， 0；0，0，γ+△γ)，式（2）為機(jī)器人中心相對位姿坐標(biāo)變換方程，可實(shí)現(xiàn)機(jī)器人的協(xié)同定位。

對于阿克曼轉(zhuǎn)向車輛，通過對前進(jìn)電機(jī)與轉(zhuǎn)向電機(jī)前進(jìn)里程以及轉(zhuǎn)角信息進(jìn)行解算，即可得到車輛每個(gè)輪子的角速度與線速度，通過CAN總線與車載工控機(jī)連接起來，即可得到阿克曼轉(zhuǎn)向車輛里程計(jì)的系統(tǒng)輸入信息。車輛里程計(jì)信息有4個(gè)輪子線速度(vf1，vf2，vr1，vr2)與4個(gè)轉(zhuǎn)向電機(jī)的轉(zhuǎn)向角度(φf1，φf2，φr1，φr2)，經(jīng)過解算作為系統(tǒng)狀態(tài)方程的控制量。由于車輛編碼器受車輛滑動等的影響，需要結(jié)合視覺監(jiān)測系統(tǒng)進(jìn)行融合優(yōu)化。

3 模型搭建與優(yōu)化

單目視覺監(jiān)測移動靶標(biāo)的相對位姿存在著環(huán)境干擾，且視覺方法容易受到環(huán)境光線等因素干擾；車輛里程計(jì)常常也受路面環(huán)境影響存在計(jì)算誤差，其誤差可以認(rèn)為服從高斯分布，為了消除環(huán)境干擾對系統(tǒng)相對位姿監(jiān)測的影響，本文提出一種基于擴(kuò)展卡爾曼濾波的方法對視覺定位系統(tǒng)以及車輛里程計(jì)定位系統(tǒng)進(jìn)行優(yōu)化處理，得到更加準(zhǔn)確的且魯棒性更高的相對位姿估算方法。

3.1 阿克曼轉(zhuǎn)向模型

對于阿克曼轉(zhuǎn)向模型的車輛，不考慮車輛在Z軸方向的運(yùn)動時(shí)，可以簡化為平面模型，其線速度與轉(zhuǎn)角信息可以通過車輛里程計(jì)反饋得到，并對四輪的線速度與角速度進(jìn)行簡化處理，阿克曼簡化轉(zhuǎn)向模型如圖3所示。

圖3 阿克曼簡化轉(zhuǎn)向模型Fig.3 Ackerman simplified steering model

其中(x，y)表示車輛中心相對于慣性坐標(biāo)系的位置信息；γ表示車輛相對于慣性參考系X軸的夾角，即車輛的航向角；φf為前輪轉(zhuǎn)角，φr為后輪轉(zhuǎn)角，vf為前輪轉(zhuǎn)速，vr為后輪轉(zhuǎn)速；O為車輛轉(zhuǎn)彎圓心，R表示車輛的轉(zhuǎn)彎半徑；v表示車輛線速度，β為車輛滑移角，本實(shí)驗(yàn)使用簡化模型，低速運(yùn)動狀態(tài)下默認(rèn)車輛滑移為0。

阿克曼轉(zhuǎn)向模型的車輛轉(zhuǎn)彎半徑是重要的參數(shù)，車輛的轉(zhuǎn)彎半徑與前后輪的轉(zhuǎn)角以及車身前后輪軸之間的距離le有關(guān)，le=2lr，式（3）給出了車輛轉(zhuǎn)彎半徑的計(jì)算方法，Rf表示前輪轉(zhuǎn)彎半徑。

車輛速度較低時(shí)，可以假設(shè)車輛的方向變化率等于車輛的角速度，故車輛的角速度可表示為

至此，獲得了車輛的角速度與線速度反饋信息，可以給出車輛相對于慣性坐標(biāo)系的線速度與角速度公式

式中，v表示車輛質(zhì)心處的線速度，vx，vy分別表示車輛相對于慣性坐標(biāo)系的橫向和縱向線速度，ω=ωf為車輛相對慣性坐標(biāo)系的角速度，vf、vr、φf、φr為通過CAN總線獲取并解算的實(shí)時(shí)車輛狀態(tài)信息，作為車輛的輸入量，搭建出阿克曼轉(zhuǎn)向車輛的數(shù)學(xué)模型。

由于本文關(guān)注多移動機(jī)器人相對位姿監(jiān)測，需要關(guān)注機(jī)器人相對位姿，故將單一機(jī)器人位姿[xyθ]T，即平面位移與角度作為狀態(tài)方程狀態(tài)量，將式（5）中的慣性坐標(biāo)系中速度信息轉(zhuǎn)換到相對于慣性坐標(biāo)系的位姿信息，并將位姿信息方程離散化，如式（6）所示，此即為對于本實(shí)驗(yàn)所用的阿克曼轉(zhuǎn)向模型的狀態(tài)方程。

由于本文通過單目視覺監(jiān)視車輛靶標(biāo)的方式測量車輛的相對位姿，其觀測量選取為車輛的相對位姿，故系統(tǒng)的觀測方程可以表示為式（7），其中(x′k，y′k，θ′k)表示為前車k時(shí)刻在慣性坐標(biāo)系的位姿，(x″k，y″k，θ″k)表示為后車k時(shí)刻在慣性坐標(biāo)系的位姿。

同時(shí)由于依靠車輛編碼器獲取的車輪運(yùn)動轉(zhuǎn)角信息受道路環(huán)境影響存在系統(tǒng)噪聲，表示為wk，該噪聲服從高斯分布P(w)～N(0，Q)；同時(shí)環(huán)境干擾也會對視覺觀測的車輛定位造成影響，其隨機(jī)誤差干擾可以表示為vk，同樣服從高斯分布P(v)～N(0，R)，故在考慮系統(tǒng)噪聲的情況下，實(shí)際狀態(tài)方程可以表示為式（8）。其中，系統(tǒng)控制量表示為Xk=[xk′yk′θk′x″ky″kθ″k]T，即為車輛狀態(tài)傳感器估算的前后車的位姿信息，系統(tǒng)觀測量表示為Zk=[zxkzykzθk]，即為單目視覺測量的前后車輛相對位姿。

3.2 濾波優(yōu)化

針對上節(jié)提到的相對位姿監(jiān)測系統(tǒng)中存在的相對誤差，由于該誤差服從高斯分布，故可以通過卡爾曼濾波的方式對系統(tǒng)觀測值進(jìn)行優(yōu)化估算。針對式（8）的非線性系統(tǒng)，本實(shí)驗(yàn)采用擴(kuò)展卡爾曼濾波的方式對系統(tǒng)進(jìn)行濾波優(yōu)化，以減少環(huán)境變化對視覺監(jiān)測的消極影響，提高系統(tǒng)的監(jiān)測精度以及對環(huán)境的適應(yīng)能力。為此將式（8）的非線性模型在處，即k-1時(shí)刻的狀態(tài)估計(jì)值處進(jìn)行線性化處理，得到線性化的狀態(tài)方程為式（9）所示，其中

式中，A′k、A″k分別為k時(shí)刻每輛車的狀態(tài)轉(zhuǎn)移矩陣在處的雅克比矩陣，具體如式（10）所示。對于擴(kuò)展卡爾曼算法，其系統(tǒng)矩陣與上一時(shí)刻的狀態(tài)估計(jì)量X?k-1有關(guān)，需要在每次迭代過后重新計(jì)算。Hk=[I-I]為k時(shí)刻每輛車的系統(tǒng)觀測矩陣。

擴(kuò)展卡爾曼濾波算法是一種遞歸算法，需要給出系統(tǒng)迭代所需的初始值，本文關(guān)注目標(biāo)為前后車輛的相對位姿，同時(shí)為了簡化計(jì)算，選取尾車的初始狀態(tài)X″0為坐標(biāo)原點(diǎn)，頭車的初始狀態(tài)X′0為單目視覺的初始監(jiān)測相對位姿，系統(tǒng)誤差方差矩陣為Q=0.09I6，R=[0.08 0.08 0.03]。選取卡爾曼濾波算法中系統(tǒng)的狀態(tài)誤差協(xié)方差矩陣初始值P0=I6。

擴(kuò)展卡爾曼濾波可以分為預(yù)測與更新兩個(gè)步驟，首先進(jìn)行系統(tǒng)狀態(tài)預(yù)測如式（11）所示，先假設(shè)不存在系統(tǒng)噪聲，代入上一時(shí)刻的后驗(yàn)估計(jì)值，即可得到當(dāng)前時(shí)刻的系統(tǒng)先驗(yàn)估計(jì)值

獲取擴(kuò)展卡爾曼濾波系統(tǒng)的預(yù)測結(jié)果后，需繼續(xù)對預(yù)測結(jié)果進(jìn)行更新，進(jìn)而獲取下一時(shí)刻的濾波優(yōu)化的結(jié)果。首先對擴(kuò)展卡爾曼濾波的卡爾曼增益進(jìn)行更新，

獲取系統(tǒng)當(dāng)前時(shí)刻的卡爾曼增益后，即可對狀態(tài)量與觀測量進(jìn)行數(shù)據(jù)融合，獲得系統(tǒng)的下一時(shí)刻的狀態(tài)估計(jì)后驗(yàn)值

最后獲取系統(tǒng)下一時(shí)刻的后驗(yàn)狀態(tài)估計(jì)誤差協(xié)方差矩陣，并進(jìn)入下一時(shí)刻的卡爾曼濾波遞歸。

本文所采用的相對位姿監(jiān)測系統(tǒng)的系統(tǒng)框架如圖4所示。相對位姿監(jiān)測系統(tǒng)將視覺監(jiān)測結(jié)果與車輛狀態(tài)信息通過擴(kuò)展卡爾曼進(jìn)行融合濾波優(yōu)化，將每個(gè)時(shí)刻的后驗(yàn)估計(jì)作為系統(tǒng)狀態(tài)輸出值，并依此獲取車輛的相對位姿信息。

圖4 系統(tǒng)框架圖Fig.4 System framework diagram

至此實(shí)現(xiàn)了對視覺相對位姿監(jiān)測信息以及車輛里程信息的數(shù)據(jù)融合，通過擴(kuò)展卡爾曼濾波，對數(shù)據(jù)信息進(jìn)行了優(yōu)化分析，并對視覺監(jiān)測系統(tǒng)中的系統(tǒng)誤差進(jìn)行了優(yōu)化。

4 實(shí)驗(yàn)與數(shù)據(jù)分析

上節(jié)介紹了本實(shí)驗(yàn)中阿克曼轉(zhuǎn)向模型相對位姿監(jiān)測系統(tǒng)的視覺與車輛狀態(tài)監(jiān)測系統(tǒng)，以及針對視覺監(jiān)測及里程計(jì)信息存在的系統(tǒng)誤差進(jìn)行融合優(yōu)化處理的擴(kuò)展卡爾曼濾波算法。本節(jié)將針對該算法進(jìn)行實(shí)物實(shí)驗(yàn)，搭建實(shí)物平臺并對實(shí)驗(yàn)數(shù)據(jù)進(jìn)行分析。

針對上文對阿克曼轉(zhuǎn)向模型車輛相對位姿監(jiān)測系統(tǒng)的分析，搭建阿克曼轉(zhuǎn)向?qū)嵨锬Ｐ腿鐖D5所示，在車輛上方架設(shè)固定4個(gè)方向的單目相機(jī)，并將二維碼靶標(biāo)設(shè)置在單目相機(jī)下方作為目標(biāo)監(jiān)測特征，監(jiān)測目標(biāo)靶標(biāo)獲取視覺的相對位姿，并對二維碼信息進(jìn)行識別，進(jìn)而實(shí)現(xiàn)車輛識別。通過CAN總線實(shí)現(xiàn)工控機(jī)與車身控制器的連接，將車輛角度與線速度反饋信息進(jìn)行解算，并獲得相對位姿監(jiān)測系統(tǒng)的輸入信息。

圖5 單機(jī)器人系統(tǒng)實(shí)物圖（紅色為單目相機(jī)位置，藍(lán)色為定位靶標(biāo)位置）Fig.5 Ackerman robot and the camera mounted on（monocular camera is marked in red and the positioning target is marked in blue）

通過監(jiān)視其他移動機(jī)器人上的二維碼靶標(biāo)，獲得相對其他機(jī)器人的相對位置與偏角作為系統(tǒng)觀測值，將視覺監(jiān)測數(shù)據(jù)與車輛狀態(tài)傳感數(shù)據(jù)對齊時(shí)間后進(jìn)入擴(kuò)展卡爾曼濾波優(yōu)化系統(tǒng)進(jìn)行融合優(yōu)化分析，即可得到最終的相對位姿估算值，并將其坐標(biāo)變換信息可視化如圖6所示，其中car2_location表示通過視覺監(jiān)測得到的相對位姿，car2表示經(jīng)過融合濾波優(yōu)化后的相對位姿。

圖6 可視化系統(tǒng)相對位姿Fig.6 Visualization of the relative posture

為了進(jìn)一步判斷相對位姿監(jiān)測系統(tǒng)效果，本文設(shè)計(jì)了基于全局視野下的位姿判定。在室內(nèi)環(huán)境下搭建全局相機(jī)視野如圖7所示，通過使用全局相機(jī)監(jiān)測移動平臺頂部二維碼靶標(biāo)，計(jì)算移動機(jī)器人位姿信息；將其作為系統(tǒng)的位姿變化實(shí)際值，對濾波優(yōu)化后的相對位姿監(jiān)測效果進(jìn)行分析評估。

圖7 全局位姿監(jiān)測Fig.7 Global posture monitoring

將車輛的狀態(tài)傳感器信息、視覺監(jiān)測系統(tǒng)融合優(yōu)化后的位姿信息作為基于視覺的相對位姿監(jiān)測系統(tǒng)的輸出值，全局視野下的車輛位姿信息作為參照值，將兩個(gè)數(shù)據(jù)通過MATLAB展示出來。圖8展示了雙車系統(tǒng)在監(jiān)測中的位置變化過程，圖9、圖10展示了雙車系統(tǒng)角度變化，即系統(tǒng)估算值與全局相機(jī)位姿參照值比較。

圖8 機(jī)器人位置變化Fig.8 Robot position change diagram

圖9 前置機(jī)器人角度變化Fig.9 Front robot angle change diagram

圖10 后置機(jī)器人角度變化Fig.10 Rear robot angle change diagram

將雙車系統(tǒng)中每輛車的位姿代入觀測方程中，即可得到雙車的相對位姿，雙車相對位置變化如圖11所示，雙車相對角度變化如圖12所示。

圖11 機(jī)器人相對位置變化Fig.11 Diagram of relative position change of robot

圖12 機(jī)器人相對角度變化Fig.12 Diagram of relative angle change of robot

通過在MATLAB中對相對位姿監(jiān)測系統(tǒng)的雙機(jī)器人位姿信息進(jìn)行比較分析，經(jīng)擴(kuò)展卡爾曼濾波算法融合兩類傳感器數(shù)據(jù)后的相對位姿對比全局相機(jī)觀測到的相對位姿參考值，其位置誤差標(biāo)準(zhǔn)差為6.02 cm，角度誤差標(biāo)準(zhǔn)差為8.42°。經(jīng)擴(kuò)展卡爾曼融合濾波后的檢測精度相對直接測量綜合提升了20%～25%；同時(shí)系統(tǒng)輸出值穩(wěn)定平滑，有效減少了環(huán)境誤差的隨機(jī)影響，提高了系統(tǒng)檢測魯棒性。

5 結(jié) 論

本文提出了一種針對多移動機(jī)器人的相對位姿定位方法，通過融合視覺監(jiān)測與車輛狀態(tài)反饋的互補(bǔ)特性，彌補(bǔ)了視覺監(jiān)測定位易受環(huán)境影響導(dǎo)致隨機(jī)誤差以及車輛狀態(tài)反饋定位存在累計(jì)誤差的不足。將兩類傳感器信息作為觀測量與輸入量通過擴(kuò)展卡爾曼濾波進(jìn)行優(yōu)化，基于阿克曼模型對系統(tǒng)位姿做出后驗(yàn)估計(jì)，減少傳感測量的系統(tǒng)誤差。

本實(shí)驗(yàn)搭建實(shí)物樣機(jī)進(jìn)行實(shí)驗(yàn)并搭建全局的視覺定位監(jiān)測，通過對實(shí)驗(yàn)數(shù)據(jù)進(jìn)行分析，驗(yàn)證了本文提出的基于視覺的相對位姿監(jiān)測系統(tǒng)相比于單一的傳感器定位，定位精度更高且定位魯棒性更好，受環(huán)境影響更小。

本文提出的基于視覺的機(jī)器人相對定位系統(tǒng)，通過結(jié)合多種傳感器實(shí)現(xiàn)了更加精確的相對定位效果，針對多機(jī)器人協(xié)同定位，還面臨著一些挑戰(zhàn)：

（1）對于多機(jī)器人的協(xié)同定位，如何將現(xiàn)有的雙車系統(tǒng)擴(kuò)展為多車系統(tǒng)，并在直接視野丟失時(shí)通過多機(jī)器人間的相對定位對整體相對位姿做出整合估算還需進(jìn)一步研究。

（2）本實(shí)驗(yàn)傳感器誤差的選取為固定的高斯誤差，在機(jī)器人定位時(shí)視覺與車輛狀態(tài)誤差常與環(huán)境光線與路面狀況有關(guān)，如何根據(jù)系統(tǒng)環(huán)境變化自適應(yīng)設(shè)置系統(tǒng)誤差對機(jī)器人協(xié)同定位具有重要研究價(jià)值。