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

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

1 引 言

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

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

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

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

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

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

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

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

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

單目相機(jī)的監(jiān)測(cè)范圍與相機(jī)的視角有關(guān)，本實(shí)驗(yàn)相機(jī)采用4個(gè)方向正方形排列的視角為120°廣角的USB單目相機(jī)，移動(dòng)機(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)測(cè)范圍計(jì)算公式，考慮到車(chē)身尺寸對(duì)視覺(jué)監(jiān)測(cè)的影響，同時(shí)為了保證單目相機(jī)能夠?qū)崿F(xiàn)全角度的位姿監(jiān)測(cè)，經(jīng)計(jì)算，以車(chē)輛中心為相機(jī)安裝中心的最長(zhǎng)安裝距離a為208 mm，本文選取單目相機(jī)距離為200 mm。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.2 濾波優(yōu)化

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

將車(chē)輛的狀態(tài)傳感器信息、視覺(jué)監(jiān)測(cè)系統(tǒng)融合優(yōu)化后的位姿信息作為基于視覺(jué)的相對(duì)位姿監(jiān)測(cè)系統(tǒng)的輸出值，全局視野下的車(chē)輛位姿信息作為參照值，將兩個(gè)數(shù)據(jù)通過(guò)MATLAB展示出來(lái)。圖8展示了雙車(chē)系統(tǒng)在監(jiān)測(cè)中的位置變化過(guò)程，圖9、圖10展示了雙車(chē)系統(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

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

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

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

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

5 結(jié) 論

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

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

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

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

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