大視場室內(nèi)移動機器人高精度動態(tài)定位方法

田明銳 楊 皓 胡永彪

1.長安大學道路施工技術(shù)與裝備教育部重點實驗室，西安，7100642.廣西柳工機械股份有限公司，柳州，545006

0 引言

室內(nèi)移動機器人是運行在室內(nèi)環(huán)境的自主移動機器人，如工業(yè)生產(chǎn)機器人、服務(wù)機器人、醫(yī)療機器人和倉儲機器人等[1-3]。目前，關(guān)于室內(nèi)移動機器人領(lǐng)域的研究主要集中在室內(nèi)定位、自主導航、運動控制和協(xié)同作業(yè)幾個方面，其中室內(nèi)定位技術(shù)是移動機器人完成其他任務(wù)的基礎(chǔ)與前提。相對于室外定位技術(shù)大都依賴衛(wèi)星定位的情況，室內(nèi)定位技術(shù)受到室內(nèi)環(huán)境復雜性和動態(tài)性的影響，一直是機器人定位技術(shù)研究領(lǐng)域的難點與熱點。

借助于不同的傳感器技術(shù)，室內(nèi)定位方法種類繁多，包括紅外定位[4]、超聲波定位[5]、無線網(wǎng)絡(luò)定位[6]、藍牙定位[7]、激光雷達定位[8]和視覺定位[9]等。機器視覺傳感器對復雜環(huán)境的強感知性和圖像處理技術(shù)獲得的高精度定位使視覺定位方法受到了廣泛的關(guān)注[10]。

目前，基于視覺的室內(nèi)定位方法主要分為相對定位方法和絕對定位方法兩類。

相對定位方法利用固定在移動機器人上的相機傳感器采集周圍環(huán)境圖像并處理，通過檢測環(huán)境中的人工標識來進行機器人的定位。謝家浩等[11]研究了一種基于視覺同步定位與建圖(SLAM)的相對定位方法，采用ORB(oriented FAST and rotated BRIEF)算法對特征點進行提取和描述，實現(xiàn)了對目標物的實時定位。楊鹿情[12]設(shè)計了一種將顏色特征與形狀特征相結(jié)合的人工信標，利用串聯(lián)式組合誤匹配剔除算法實現(xiàn)室內(nèi)移動機器人的定位，減小了單目視覺SLAM系統(tǒng)前端存在的較多誤匹配對位姿估計的影響。相對定位方法定位迅速，精度較高，但需安裝大量機載設(shè)備，成本較高，同時該方法算法復雜，易受環(huán)境不確定因素的影響。

相比之下，絕對定位方法通過置于環(huán)境中的相機傳感器對目標物及周圍環(huán)境進行圖像采集，通過圖像處理進行移動機器人的定位，可有效降低機器人機載設(shè)備的復雜性，同時具有成本低、適應(yīng)性強和算法簡單等優(yōu)點。黎安慶等[13]提出了一種基于分布式視覺技術(shù)的機器人絕對定位方法，在3 m×5 m的區(qū)域內(nèi)該方法的平均定位精度為8.1 cm，方位角平均精度為8.05°。林劍冰等[14]采用定距雙球作為標志物，設(shè)計了一種室內(nèi)自動運輸車的絕對定位系統(tǒng)，可同時實現(xiàn)其位置與方向的定位，相對誤差一般情況下保持在1.5%以內(nèi)，在距離系統(tǒng)3 m范圍的短距離定位的絕對誤差小于1 cm。李鵬等[15]提出了一種基于標定板絕對定位方案，利用高斯混合模型背景減除法檢測機器人運動區(qū)域，縮小了標定板角點的提取范圍，提高了定位的實時性，當機器人速度為0.3 m/s時，平均定位精度為6.86 cm。

上述絕對定位方法研究為保證定位精度，視場范圍設(shè)置相對較小，在機器人低速移動時可滿足較高的實時定位要求。目前，關(guān)于大視場范圍與高速移動目標的絕對定位方法研究較少。本文對大視場范圍移動機器人的高速動態(tài)定位方法進行了研究，將相機多參量非線性標定方法與基于光信標高速定位算法相結(jié)合，提高高速移動機器人的定位精度。

1 基于多相機的大視場傳感器系統(tǒng)

1.1 定位系統(tǒng)硬件配置與軟件構(gòu)架

如圖1所示，定位系統(tǒng)硬件由一組具有外部同步觸發(fā)器的相機、一臺工控機和LED光信標組成，定位面積即相機總視場可達43 m2。系統(tǒng)選用MER-131-75GX-P工業(yè)相機進行圖像采集，相機內(nèi)部采用CMOS感光芯片，采集圖像分辨率為1280×1024。每臺相機通過GigE數(shù)據(jù)傳輸線連接到處理節(jié)點，允許以每秒75幀的速度進行圖像采集。處理節(jié)點為一個通用的多核PC平臺，具有控制和處理來自多個相機信息的能力。實驗中使用的所有機器人平臺采用Traxxas-83024-4輪式機器人底盤，采用全輪驅(qū)動、前輪轉(zhuǎn)向的方式運動，最高速度可達48.3 km/h。機器人上部安裝220×20 mm的LED光信標，其方向與機器人幾何中心線平行，光信標的幾何中心與機器人的質(zhì)心重合。

圖1 定位系統(tǒng)方案示意圖Fig.1 Schematic diagram of positioning system hardware

定位系統(tǒng)軟件使用C/C++語言實現(xiàn)程序框架和定位功能算法，利用MATLAB相機標定工具完成視覺系統(tǒng)標定，其構(gòu)架如圖2所示。該軟件系統(tǒng)主要由相機標定、移動機器人定位、動態(tài)誤差補償三方面構(gòu)成。

圖2 定位系統(tǒng)軟件構(gòu)架Fig.2 Positioning system software architecture

1.2 大視場多相機非線性標定

1.2.1多參量非線性標定方法

相機標定旨在獲得圖像坐標(u,v)與地面坐標(xw,yw)的映射關(guān)系，是后續(xù)定位系統(tǒng)實現(xiàn)的前提。傳統(tǒng)相機的線性標定方法不考慮圖像的徑向、切向的畸變，標定后的精度較低，且對噪聲敏感。相機的非線性標定方法引入了相機的畸變參數(shù)，成像模型更加精確，可有效提高標定精度。

傳統(tǒng)的相機非線性標定[16]借助一組靶標數(shù)據(jù)對相機參數(shù)進行標定，當視場范圍增大時，該標定方法精度較低。為提高大視場環(huán)境下的相機標定精度，排除偶然誤差的影響，本文選用多組靶標對相機進行非線性標定，靶標組數(shù)的選取與相機視場大小相關(guān)。

第k(k=1,2,…,n)組靶標中第i個靶標點的圖像坐標與地面坐標滿足如下關(guān)系：

(1)

式中，m′k11、m′k12、m′k14、m′k21、m′k22、m′k24、m′k31、m′k32為標定參量。

式(1)的矩陣方程形式為

Akm′k=Bk

(2)

Ak=

m′k=

利用k個單應(yīng)矩陣計算平均單應(yīng)矩陣：

(3)

將該平均單應(yīng)矩陣作為最終將圖像坐標映射至地面坐標的單應(yīng)矩陣。

根據(jù)式(1)和式(3)可計算相機圖像坐標(ui,vi)映射至地面的坐標(x′wi,y′wi)。相機的標定誤差即定位系統(tǒng)的靜態(tài)誤差在地面坐標系xw、yw方向上分別為Ex=|xwi-x′wi|、Ey=|ywi-y′wi|。該標定方法和僅選用一組靶標點的標定方法相比可有效提高定位精度。

非線性標定實驗利用MATLAB中的Camera Calibrator相機標定工具，對相機內(nèi)部參數(shù)進行標定，結(jié)果如表1所示。

表1 相機內(nèi)部參數(shù)

根據(jù)相機內(nèi)參數(shù)據(jù)，對相機所采集的圖像進行去畸變校正。由于單個相機視場約為25 m2，故在去畸變后的圖像中共選取4組靶標點作為單應(yīng)矩陣的標定點，即k=4。根據(jù)式(1)～式(3)計算左右兩相機的平均單應(yīng)矩陣：

(4)

(5)

根據(jù)式(1)、式(4)、式(5)計算地面所有靶標點的世界坐標，并利用MATLAB中Curve Fitting工具擬合出所有像素點靜態(tài)誤差，如圖3所示，定位系統(tǒng)的平均靜態(tài)定位精度為2.26 mm。若只選取單組靶標點，即k=1，則經(jīng)計算得定位系統(tǒng)的平均靜態(tài)定位精度為4.77 mm。基于多組靶標點的相機非線性標定方法使相機標定精度提高約52.6%。

1.2.2多相機視場融合

在大視場移動機器人定位之前，需將各相機成像平面坐標統(tǒng)一化，即多相機視場融合，作為后續(xù)同名目標檢測的計算基礎(chǔ)。同一平面目標點在不同成像平面上的映射關(guān)系可通過單應(yīng)矩陣變換表達，因此本文基于不同視場圖像坐標系的單應(yīng)矩陣標定來實現(xiàn)多相機視場融合。

選取同時出現(xiàn)在相機p、相機q視場中的4個靶標點，組成兩組圖像坐標(upi,vpi)、(uqi,vqi)，i=1,2,3,4，代入下式：

(6)

根據(jù)1.2.1節(jié)所述方法，計算相機q映射至相機p的單應(yīng)矩陣m′q-p，可得由右相機標定至左相機的單應(yīng)矩陣：

(a)左相機Ex

(7)

2 動態(tài)定位系統(tǒng)算法設(shè)計

2.1 基于光信標的高速目標定位算法

模板匹配、光流法和幀差法等算法是基于圖像的運動目標檢測及定位的常用方法。模板匹配法能夠適應(yīng)較為復雜的背景，定位精度較高；光流法與幀差法可直接獲得目標物的運動信息，易于進行實時處理。當定位目標增多，且目標物高速運動時，上述定位算法會非常復雜，圖像處理時間長，造成定位系統(tǒng)實時性較差、動態(tài)定位誤差大等問題。為解決高速多目標定位算法的上述問題，本文提出基于光信標的高速定位算法。該算法定位目標具有較高的移動速度，并涉及跨視場多目標的定位問題，算法流程圖見圖4。

圖4 基于光信標的高速定位算法流程圖Fig.4 Flow chart of high-speed positioning algorithmbased on optical beacon

使用光信標能夠簡化圖像運動目標檢測算法，有利于提高定位算法的運行速度。該定位算法設(shè)置較短的相機曝光時間，借助固定閾值方法快速提取圖像中安裝光信標的移動檢測物體，得到無其他干擾的目標物二值圖像。設(shè)原圖像像素點灰度級為f(u,v)，二值化后圖像像素點灰度級為g(u,v)，固定閾值為T0，則二值化計算公式為

(8)

針對二值化處理后的圖像，利用連通域分析與循環(huán)遍歷算法，獲得連通域外接矩形的長h、寬w、質(zhì)心坐標(uc,vc)和左上角點坐標(u0,v0)等參數(shù)。由于光信標的中心與機器人質(zhì)心重合，連通域外接矩形的圖像質(zhì)心坐標(uc,vc)為機器人質(zhì)心的圖像坐標，故可根據(jù)式(1)、式(4)和式(5)計算機器人質(zhì)心的位置坐標(xc,yc)。

移動機器人的姿態(tài)角計算示意圖見圖5。首先，根據(jù)外接矩形長寬比的不同(圖5a為w>h，圖5b為w

(9)

(a)w>h(b)h>w圖5 機器人姿態(tài)角計算示意圖Fig.5 Schematic diagram of robot attitudeangle calculation

式中，ucl、uc(l-1)分別為第l與第l-1時刻機器人質(zhì)心的圖像橫坐標值。

移動機器人的位姿信息包含其質(zhì)心坐標與姿態(tài)角，向量形式表示為(xc,yc,θ)T。

2.2 跨視場同名目標檢測算法

考慮相機布置中存在一定的視場重合區(qū)域，當物體位于重合區(qū)域時將同時出現(xiàn)在兩個相機的圖像中，如何區(qū)分不同視場中的同名目標物是多相機定位方法中的一個關(guān)鍵。在目標定位計算前，可對相鄰相機視場圖像中的同名目標物進行判別與剔除，避免同名目標的重復定位計算。

跨視場同名目標檢測算法將同一時刻相鄰相機采集的圖像進行處理，分別獲得左相機、右相機圖像中所有連通域目標的質(zhì)心參數(shù)(uLcj,vLcj)和(uRci,vRci)，結(jié)合1.2.2節(jié)中得到的單應(yīng)矩陣，將右相機圖像中所有質(zhì)心點(uRci,vRci)映射至左相機圖像中，得到(u′Rci、v′Rci)。計算映射后各質(zhì)心點坐標(u′Rci、v′Rci)與左相機圖像中原質(zhì)心點坐標(uLcj,vLcj)間的距離：

(10)

將其與距離閾值Te比較，進而判定右相機中對應(yīng)目標物是否為同名目標物，并將其排除，Te的選取與相機參數(shù)、相機的布置以及移動機器人幾何尺寸參數(shù)有關(guān)，可通過實驗方法確定。設(shè)命題Q(i)為右相機中第i個目標物為同名目標物，當Q(i)=TRUE時該目標為同名目標物，當Q(i)=FALSE時該目標為非同名目標物，其判別公式如下：

(11)

該算法通過比對左右相機中每個連通域目標質(zhì)心坐標，利用式(11)實現(xiàn)同名目標的判別和剔除，其流程如圖6所示。

圖6 同名目標檢測算法流程圖Fig.6 Flow chart of same-named targetdetection algorithm

2.3 動態(tài)定位誤差補償算法

盡管上述高速定位算法程序運行周期較短，但系統(tǒng)仍會產(chǎn)生一定的時延，尤其目標物處于高速移動時，系統(tǒng)輸出的位姿信息滯后較為嚴重，即定位系統(tǒng)存在一定的動態(tài)誤差，因此，需進行動態(tài)定位誤差的補償以提高系統(tǒng)定位精度。

由于程序運行周期較短，故假設(shè)機器人行駛速度v在同一運行周期內(nèi)保持不變。選取機器人質(zhì)心(x,y)為第一特征點，建立該特征點的運動學方程[17]：

(12)

(13)

式中，下標m表示時刻；T為程序運行周期；vx、vy為第一特征點在圖像空間中x、y方向上的速度。

同理，選取機器人姿態(tài)角θ為第二特征點，建立該特征點的運動學方程：

θm+1=θm+vθT

(14)

vθ=(θm-θm-1)/T

(15)

式中，vθ為第二特征點的變化率，即角速度。

動態(tài)定位誤差補償算法基于上述運動學模型，在C/C++語言編程環(huán)境中開發(fā)，分析計算機器人的移動軌跡、速度和方向等信息，預測定位系統(tǒng)存在時延時機器人的真實位姿，完成誤差補償。該算法流程圖見圖7。

圖7 動態(tài)定位誤差補償算法流程圖Fig.7 Flow chart of dynamic positioning errorcompensation algorithm

3 定位系統(tǒng)實驗

3.1 移動機器人位姿定位實驗

根據(jù)2.1節(jié)提出的定位算法，以左相機為例，設(shè)置合適的曝光參數(shù)，使圖像能夠清晰完整地顯示移動機器人上安裝的光信標，程序運行結(jié)果如圖8所示。由輸出結(jié)果可知，共檢測到三臺移動機器人，分別為Car_0、Car_1和Car_2，它們對應(yīng)的位姿向量分別為(579.28 mm,385.67 mm,36.87°)T、(583.28 mm,482.33 mm,-36.87°)、(692.84 mm,569.38 mm,124.13°)T。

圖8 移動機器人位姿計算程序運行結(jié)果Fig.8 Running results of mobile robot position andattitude angle calculation program

3.2 跨視場同名目標物檢測實驗

首先進行距離閾值Te的設(shè)定。選取三組位于相機視場重疊區(qū)域的地面坐標點(xLcj,yLcj)、(xRci,yRci)，i,j=1,2,3(即每組包含3對坐標點)，分別獲得它們位于左圖、右圖中的圖像坐標(uLcj,vLcj)、(uRci,vRci)。同時根據(jù)1.2.2得到的單應(yīng)矩陣m′R-L，將位于右圖中的三組坐標點映射至左圖并統(tǒng)計映射誤差eui=|u′Rci-uLcj|、evi=|v′Rci-vLcj|，結(jié)果如表2所示。

表2 坐標點映射誤差

根據(jù)表2中數(shù)據(jù)，結(jié)合移動機器人幾何尺寸，將距離閾值Te設(shè)定為25 pixel。而后，根據(jù)2.2節(jié)中算法，對視場范圍中圖9所示的移動機器人分布進行同名目標的檢測與排除，該視場范圍內(nèi)共有四臺移動機器人，其中2號、3號為出現(xiàn)在視場重疊區(qū)域的同一移動機器人。

圖9 兩視場中移動機器人分布Fig.9 Distribution of mobile robots in twofields of view

該算法的輸出結(jié)果如圖10所示，成功檢測出3號機器人為同名目標物并對其進行了剔除，最終獲得三個有效目標，分別用Car_0、Car_1與Car_2表示。由結(jié)果可知，該算法能夠準確地判斷有效目標并去除重復同名目標，為定位系統(tǒng)檢測多目標的準確性提供保障。

圖10 同名目標檢測程序運行結(jié)果Fig.10 Running results of same-named targetdetection algorithm

3.3 移動機器人定位動態(tài)誤差補償實驗

在獲得移動機器人靜態(tài)位姿且進行了同名目標檢測與排除的前提下，通過實驗驗證機器人在0.3 m/s、1.2 m/s、2.3 m/s、3.2 m/s四組不同平均速度下動態(tài)誤差補償算法的有效性。當機器人處于最高平均速度3.2m/s時，其質(zhì)心坐標實際值與估計值對比圖、姿態(tài)角實際值與估計值對比圖見圖11和圖12。

圖11 平均速度值為3.2 m/s時機器人質(zhì)心軌跡實際值與估計值對比Fig.11 Comparison of the actual and estimated valuesof the robot’s center of mass position with the averagespeed is 3.2 m/s

圖12 平均速度值為3.2 m/s時機器人姿態(tài)角實際值與估計值對比Fig.12 Comparison of the actual and estimated valuesof the robot’s attitude angle with the averagespeed is 3.2 m/s

根據(jù)不同速度下的實際值與估計值對比圖得質(zhì)心坐標誤差補償平均精度與姿態(tài)角誤差補償平均精度，如表3所示。

表3 動態(tài)誤差補償后定位系統(tǒng)精度

由表3可得，在移動機器人最高平均速度可達3.2 m/s時，系統(tǒng)位置平均動態(tài)定位精度為5 cm、姿態(tài)角平均動態(tài)定位精度為0.6°。

4 結(jié)論

本文針對室內(nèi)高速移動機器人實時定位問題，提出了一種基于視覺的高精度絕對定位方法。該方法利用多相機視場融合，實現(xiàn)了43 m2的大視場絕對定位，結(jié)合同名目標物排除算法，解決了多相機視場重疊區(qū)域機器人定位問題；利用多參量非線性標定方法，提高平均靜態(tài)定位精度至2.26 mm；采用光信標簡化圖像處理算法提高定位系統(tǒng)運行速度，并借助線性誤差補償算法，使機器人在平均速度為3.2 m/s時，定位系統(tǒng)位置平均動態(tài)定位精度達5 cm，姿態(tài)角平均動態(tài)定位精度達0.6°。實驗結(jié)果證明，該定位系統(tǒng)可實現(xiàn)室內(nèi)大范圍下高速移動機器人的實時定位，定位系統(tǒng)的靜態(tài)與動態(tài)定位精度滿足要求。本文方法的局限性主要在于，大視場平面靶標點布置過程導致單應(yīng)矩陣標定工作量較大，例如當移動機器人光信標到地面的垂直距離發(fā)生變化時，需要重新標定與地面平行的另一平面的單應(yīng)矩陣，此時需要在目標平面內(nèi)布置(x,y)坐標已知的若干靶標點，當精度要求較高時，靶標點布置難度和工作量較大。這也限定了各移動機器人光信標到地面的垂直距離最好相等。因此，后續(xù)工作還需研究大視場平面的快速標定方法。