孫森震，李廣云，馮其強(qiáng)，王 力

(中國(guó)人民解放軍戰(zhàn)略支援部隊(duì)信息工程大學(xué) 地理空間信息學(xué)院，河南 鄭州 450001)

1 引 言

隨著中國(guó)制造2025、工業(yè)4.0產(chǎn)業(yè)政策的制定及人工智能技術(shù)的快速發(fā)展，制造業(yè)、服務(wù)業(yè)對(duì)室內(nèi)移動(dòng)機(jī)器人的需求日益增長(zhǎng)。定位技術(shù)的性能決定了機(jī)器人的應(yīng)用場(chǎng)景及服務(wù)能力。精度高、可靠性強(qiáng)、延時(shí)小、成本低的定位技術(shù)一直是研究的熱點(diǎn)。在移動(dòng)機(jī)器人定位領(lǐng)域，磁導(dǎo)軌定位技術(shù)成功應(yīng)用到自動(dòng)化工廠車間，但是磁導(dǎo)軌需要預(yù)先埋設(shè)，線路固定，維護(hù)成本較高?；赪i-Fi、藍(lán)牙的室內(nèi)定位精度無(wú)法滿足移動(dòng)機(jī)器人的定位需要，基于超寬帶的高精度定位方案布設(shè)成本較高，容易受到電磁干擾，規(guī)?；瘧?yīng)用復(fù)雜度高。二維碼掃描定位方案，僅需要在地面粘貼二維碼導(dǎo)航標(biāo)簽，適用于自動(dòng)化車間，主要應(yīng)用在固定的自動(dòng)化流水線上，具有高精度、高可靠性的特點(diǎn)，但定位范圍有限。室內(nèi)移動(dòng)機(jī)器人SLAM導(dǎo)航方法在人員移動(dòng)環(huán)境中容易受到干擾[1]，因此多種定位技術(shù)融合是移動(dòng)機(jī)器人定位導(dǎo)航技術(shù)的主要手段[2]。

隨著LED照明系統(tǒng)的廣泛應(yīng)用，基于可見光通信(Visible Light Communication, VLC)的室內(nèi)定位技術(shù)引起了業(yè)界的廣泛重視。國(guó)內(nèi)外研究人員紛紛提出基于可見光通信的室內(nèi)定位方法，這些方法可以分為成像和非成像兩種方式[3]。非成像方式主要利用光電探測(cè)器(Photo-Diode, PD)感知接收可見光信號(hào)，通過(guò)信號(hào)強(qiáng)度和編碼把定位問(wèn)題轉(zhuǎn)化為三角測(cè)量或場(chǎng)景分析問(wèn)題[3-4]。此類方法需要接收多個(gè)LED光源信息，對(duì)光源信號(hào)調(diào)制及分布要求較高，容易受到環(huán)境的影響[5]。針對(duì)這一問(wèn)題，文獻(xiàn)[6-8]利用智能算法提升定位精度并進(jìn)行了仿真，文獻(xiàn)[9]使用人工神經(jīng)網(wǎng)絡(luò)對(duì)信道參數(shù)進(jìn)行擬合，在1 m×1 m×1.2 m的實(shí)驗(yàn)測(cè)試中獲得了較高的精度。智能算法可以提升可見光非成像定位方式的定位精度，但算法復(fù)雜度較高，工程部署難度大。成像方式主要利用LED光源顯著的計(jì)算機(jī)視覺特征和通信能力，通過(guò)攝影測(cè)量方法實(shí)現(xiàn)高精度定位?？梢姽獬上駵y(cè)量的精度主要受所使用成像傳感器的分辨率、成像距離及光源中心提取精度的影響[10]。使用單個(gè)成像傳感器通常需要同時(shí)對(duì)3個(gè)以上的光源進(jìn)行成像，且成像姿態(tài)受限，應(yīng)用難度較大。針對(duì)這一問(wèn)題，文獻(xiàn)[11]提出使用雙圖像傳感器的成像定位方法，利用兩個(gè)LED光源實(shí)現(xiàn)了厘米級(jí)定位，并能夠提供航向角糾正，但該方法假定雙圖像傳感器無(wú)傾斜姿態(tài)，且假設(shè)光源已經(jīng)識(shí)別。文獻(xiàn)[12-14]采用單個(gè)圖像傳感器與姿態(tài)傳感器(IMU)結(jié)合的方式，利用成像卷簾快門效應(yīng)實(shí)現(xiàn)對(duì)LED標(biāo)簽信息(LED-ID)的識(shí)別，受成像通信距離的限制，成像傳感器與LED距離較近，且對(duì)成像傳感器的幀率和圖像處理速率有較高要求。文獻(xiàn)[15]利用單個(gè)成像傳感器對(duì)兩個(gè)LED進(jìn)行成像通信定位，在0.8 m×0.8 m×2 m的實(shí)驗(yàn)環(huán)境中實(shí)現(xiàn)了厘米級(jí)定位，并解決了光源識(shí)別問(wèn)題，但成像通信對(duì)成像姿態(tài)角度要求較高，且不適合移動(dòng)定位。室內(nèi)移動(dòng)機(jī)器人多集成雙目圖像和慣性測(cè)量單元(Inertial Measurement Unit,IMU)傳感器，可見光成像定位方式可以充分利用已有光源設(shè)施，但是要滿足精度高、延時(shí)小的室內(nèi)移動(dòng)定位仍然存在一定的技術(shù)難度，需要解決傳感器集成標(biāo)定、光源探測(cè)識(shí)別跟蹤[16]、連續(xù)圖像匹配[17]和快速動(dòng)態(tài)位置計(jì)算等難題。

針對(duì)室內(nèi)移動(dòng)機(jī)器人定位導(dǎo)航的應(yīng)用需求，本文提出一種基于可見光通信與雙目視覺測(cè)量的室內(nèi)定位方法，并制作了雙目視覺定位模塊驗(yàn)證了該方法的適用性。

2 定位方法

2.1 可見光室內(nèi)定位方法設(shè)計(jì)與場(chǎng)景分析

基于LED-ID的VLC定位方法[4]能夠滿足室內(nèi)定位米級(jí)定位需要，易于部署，接收解碼簡(jiǎn)單，對(duì)硬件性能要求較低，定位速度較快。為適用典型室內(nèi)應(yīng)用場(chǎng)景，提高定位的穩(wěn)定性與可靠性，如圖1所示，將雙目視覺傳感器、IMU、PD、微處理器及電源集成為定位模塊，當(dāng)定位模塊處于光源P下方時(shí)，通過(guò)PD接收光源LED-ID信息并解析，實(shí)現(xiàn)光源世界坐標(biāo)的獲取。然后使用雙目圖像傳感器對(duì)光源進(jìn)行成像，在雙目視覺坐標(biāo)系中計(jì)算光源P相對(duì)于定位模塊中心d的坐標(biāo)向量l；在成像同時(shí)使用IMU記錄成像的三維角度，根據(jù)光源P的世界坐標(biāo)及成像姿態(tài)角將雙目視覺坐標(biāo)系與世界坐標(biāo)系進(jìn)行轉(zhuǎn)換，得到定位模塊相對(duì)光源P的世界坐標(biāo)，實(shí)現(xiàn)三維定位。

圖1 定位方法示意圖Fig.1 Schematic diagram of positioning method

圖2 室內(nèi)LED光源布局情況Fig.2 Layout of indoor LED light source

目前，室內(nèi)LED光源主要為圓形射燈，直徑為7 cm，單個(gè)光源功率一般為5～7 W。圖2為某建筑區(qū)域內(nèi)部光源布局所示，多為兩個(gè)相距0.6 m的光源為一組，相鄰組光源間距約2 m。廊道光源之間相距2.5 m，光源距離地面高度為3.1 m。針對(duì)圖2場(chǎng)景，需要解決單燈與雙燈光源識(shí)別、光源中心提取、雙目視覺坐標(biāo)系與IMU坐標(biāo)系標(biāo)定以及雙目成像測(cè)量快速解算等問(wèn)題。

2.2 定位模塊硬件參數(shù)及定位區(qū)域分析

使用LED-ID方法對(duì)應(yīng)用場(chǎng)景進(jìn)行光源改造，場(chǎng)景中光源散射角度為60°，在3.1 m高的空間范圍內(nèi)相鄰光源照射范圍有一定重疊。LED-ID方法通常使用光強(qiáng)型PD，PD通過(guò)感知場(chǎng)景光強(qiáng)變化進(jìn)行信號(hào)接收，在光源重疊區(qū)域存在信號(hào)干擾問(wèn)題。根據(jù)場(chǎng)景中的光源分布特點(diǎn)，定位模塊設(shè)計(jì)參數(shù)如表1所示。

表1 定位模塊參數(shù)

圖3 有效定位空間范圍分析Fig.3 Analysis of effective positioning space range

當(dāng)PD位于光源正下方2 m×2 m區(qū)域內(nèi)時(shí)，相互干擾影響較小，因此定位模塊選用視場(chǎng)角為70°的雙目圖像傳感器。如圖3所示，當(dāng)定位模塊處于光通信靈敏接收區(qū)域時(shí)接收光源對(duì)應(yīng)的空間坐標(biāo)信息，同時(shí)光通信靈敏區(qū)域中具有合適的成像范圍，能夠?qū)ι戏降腖ED光源進(jìn)行雙目視覺測(cè)量，并以光源為參考確定自身位置。

2.3 圓形LED光源中心坐標(biāo)提取

定位模塊用于移動(dòng)機(jī)器人視覺，使用普通的CMOS圖像傳感器，在成像過(guò)程中保持曝光值不變，如圖4(a)所示,對(duì)LED光源的成像具有一定暈光效應(yīng)，直接使用橢圓法擬合光斑中心方法[18]容易受成像角度影響，光斑中心提取不穩(wěn)定，影響雙目視覺的測(cè)量精度[19]。對(duì)光源圖像進(jìn)行二值化如圖4(b)所示，其光斑為不規(guī)則多邊形，但基本保持圓形特征。參考圖像處理輪廓度量概念[20]，以光斑圓度為定權(quán)因子使用光斑輪廓質(zhì)心與光斑橢圓擬合中心的加權(quán)平均值作為光源的中心,計(jì)算方法為：

(1)

其中α=4πSc/Lc2，為光斑輪廓的圓度，α∈(0.4,1.0)；Sc為光斑輪廓的面積，Lc為輪廓的周長(zhǎng)，O1為光斑輪廓區(qū)域的質(zhì)心，O2為擬合光斑的外接橢圓的中心。圓度加權(quán)擬合法可以提高光源中心提取的穩(wěn)定性，引入光斑輪廓圓度計(jì)算方法可以作為光源識(shí)別的依據(jù)，提高光源識(shí)別的準(zhǔn)確度，光源中心提取效果如圖4(c)所示。

圖4 光源成像與中心提取效果Fig.4 Imaging and center extraction of light source

2.4 定位計(jì)算過(guò)程

使用張氏標(biāo)定法對(duì)定位模塊進(jìn)行雙目視覺標(biāo)定后[21]，可知左右相機(jī)的內(nèi)參數(shù)及外參數(shù)矩陣。如圖1所示，設(shè)光源中心P在左右相片上的中心坐標(biāo)為pl(ul,vl)，pr(ur,vr),根據(jù)雙目視覺幾何關(guān)系采用加權(quán)最小二乘算法計(jì)算P點(diǎn)在雙目視覺坐標(biāo)系中的空間坐標(biāo)[22]。以雙目基線中間點(diǎn)d作為雙目視覺坐標(biāo)系的原點(diǎn)，則光源中心P相對(duì)坐標(biāo)原點(diǎn)d的坐標(biāo)向量為l(x,y,z)。設(shè)成像空間姿態(tài)角(α，β，θ)，P點(diǎn)的世界坐標(biāo)為(X,Y,Z)，定位模塊中心的世界坐標(biāo)D的計(jì)算方法為：

(2)

旋轉(zhuǎn)矩陣為：

其中：α為俯仰角，β為翻滾角，θ為航向角。

2.5 雙目相機(jī)與IMU結(jié)構(gòu)參數(shù)的標(biāo)定與檢驗(yàn)

雙目傳感器與IMU在安裝時(shí)不能保證嚴(yán)格的水平一致，導(dǎo)致IMU測(cè)量雙目傳感器的俯仰角和傾斜角與真實(shí)角度存在一個(gè)固定差值，需要確定雙目相機(jī)與IMU的外參數(shù)。使用Kalibr工具進(jìn)行標(biāo)定[23]，根據(jù)標(biāo)定結(jié)果對(duì)IMU測(cè)量的傾斜姿態(tài)角進(jìn)行補(bǔ)償及定位檢驗(yàn)。檢驗(yàn)方法如圖5(a)所示，將定位模塊放置在轉(zhuǎn)臺(tái)上，雙目圖像分辨率為1 280×720，以正上方距離1.8 m的視覺點(diǎn)標(biāo)志為參考進(jìn)行旋轉(zhuǎn)定位，查看定位點(diǎn)的軌跡分布。模塊在轉(zhuǎn)臺(tái)上旋轉(zhuǎn)一周的定位軌跡趨近為一個(gè)點(diǎn)，如圖5(b)所示，平面定位結(jié)果之差不大于1 cm，表明雙目傳感器與IMU的參數(shù)標(biāo)定結(jié)果滿足傾斜姿態(tài)補(bǔ)償精度要求。

圖5 姿態(tài)校準(zhǔn)檢測(cè)Fig.5 Attitude calibration detection

2.6 雙燈識(shí)別與定向糾正

兩個(gè)光源與LED-ID提供的世界坐標(biāo)對(duì)應(yīng)關(guān)系有兩種情況，分別根據(jù)兩種情況計(jì)算定位模塊的航向角，將計(jì)算結(jié)果與定位模塊IMU提供的航向角比較，以差值作為匹配關(guān)系，具體流程如圖6所示。其中C值取50，通過(guò)該流程能夠?qū)崿F(xiàn)雙燈的識(shí)別與坐標(biāo)匹配，同時(shí)對(duì)IMU的航向角進(jìn)行了定向糾正。

圖6 光源識(shí)別定向流程Fig.6 Light source identification and orientation process

在圖2場(chǎng)景中，當(dāng)定位模塊位于相距0.6 m的雙燈下方時(shí)，雙目圖像中存在兩個(gè)光源，雙燈匹配效果如圖7所示。

圖7 雙燈識(shí)別效果Fig.7 Images of dual-lamp light source recognition

3 實(shí) 驗(yàn)

3.1 單燈定位測(cè)試

以單個(gè)光源作為定位參考，使用全站儀作為輔助測(cè)量手段建立空間坐標(biāo)系。設(shè)光源中心坐標(biāo)為(0,0,310 cm)，定位模塊放置在平面坐標(biāo)為(50 cm，50 cm)高度不同的平臺(tái)上進(jìn)行旋轉(zhuǎn)定位測(cè)試，并記錄三維方向上的最大誤差，最大誤差的變化趨勢(shì)如圖8所示。

圖8 單燈定位模塊定位誤差測(cè)試Fig.8 Positioning errors of positioning module with single LED source

在2 m×2 m范圍內(nèi)設(shè)置距離光源高度為260 cm的5×5的網(wǎng)格點(diǎn)上進(jìn)行旋轉(zhuǎn)定位測(cè)試，定位點(diǎn)的軌跡分布如圖9所示。網(wǎng)格點(diǎn)(100 cm,100 cm,50 cm)處定位軌跡中心坐標(biāo)為(97.6 cm,102.1 cm，51.1 cm)，三維方向上的定位誤差隨航向角誤差的變化趨勢(shì)如圖10所示，其中平面定位誤差隨航向角誤差的變化趨勢(shì)一致，航向角誤差的波動(dòng)范圍為7.9°。

圖9 單燈旋轉(zhuǎn)定位軌跡分布Fig.9 Distribution of rotation positioning track using a LED

圖10 單燈定位誤差隨航向角的變化情況Fig.10 Variation of single light positioning error with heading angle

在定位模塊雙目基線固定的情況下，圖像分辨率越高定位的精度越高。選用不同的成像分辨在距離光源不同距離時(shí)，成像測(cè)距的2倍中誤差變化如表2所示。選用1 280×720的分辨率時(shí)，通過(guò)連續(xù)跟蹤方法縮小光源搜索范圍，提高定位解算速度，定位模塊的解算頻率達(dá)到5 Hz以上。

表2 不同成像分辨率下的測(cè)距誤差

Tab.2 Performance of positioning module at different resolutions

成像分辨率距離光源的距離/cm100200300400定位頻率/Hz800×6001.25.813.825.210～141 280×7201.03.28.217.85～81 920×1 0800.62.85.211.22～4

3.2 雙燈定位測(cè)試

以相距60 cm的兩個(gè)光源作為定位參考，同樣使用全站儀作為輔助測(cè)量手段建立坐標(biāo)系，在2 m×2 m內(nèi)設(shè)置距離光源高度為260 cm的5×5的網(wǎng)格點(diǎn)上，以雙燈定位模式進(jìn)行旋轉(zhuǎn)定位測(cè)試，定位結(jié)果分布如圖11所示。網(wǎng)格點(diǎn)(100 cm,100 cm,50 cm)處的定位軌跡中心坐標(biāo)為(98.8 cm,99.2 cm,51.8 cm)，定位誤差隨航向角誤差的變換趨勢(shì)如圖12所示。三維方向上的定位誤差均小于5 cm，平面定位誤差隨航向角誤差的變化趨勢(shì)仍具有一致性，航向角誤差的波動(dòng)范圍為3.1°。

圖11 雙燈旋轉(zhuǎn)定位軌跡分布Fig.11 Distribution of rotation positioning track using dual LED

圖12 雙燈定位誤差隨航向角的變化情況Fig.12 Variation of dual light positioning error with heading angle

3.3 單燈雙燈動(dòng)態(tài)定位比較

在同一組光源下高度為90 cm平臺(tái)上放置定位模塊，使定位模塊沿著參考點(diǎn)連線做矩形運(yùn)動(dòng)，同時(shí)使用單燈定位和雙燈定位模式進(jìn)行定位計(jì)算，其定位軌跡如圖13所示。兩種定位模式的定位軌跡基本一致，參考點(diǎn)處的定位誤差均小于4 cm。

圖13 單燈和雙燈模式的定位軌跡Fig.13 Positioning tracks of single and dual light positioning methods

當(dāng)定位實(shí)驗(yàn)平臺(tái)下方放置磁性物體時(shí)，同樣使定位模塊沿著另一組參考點(diǎn)連線做矩形運(yùn)動(dòng)，同時(shí)使用單燈定位和雙燈定位模式進(jìn)行定位計(jì)算，其定位軌跡如圖14所示。雙燈定位模式的定位軌跡與參考點(diǎn)連線基本一致,參考點(diǎn)處的最大誤差為3.5 cm，單燈定位模式的定位軌跡明顯偏離了參考路線，參考點(diǎn)處的最大定位誤差為8.5 cm。

在有磁場(chǎng)干擾的定位測(cè)試中兩種模式的航向角變化如圖15所示。單燈模式的航向角波動(dòng)范圍為11.8°，雙燈模式的航向角波動(dòng)范圍為3.5°。顯然，單燈定位模式航向角受到周圍磁場(chǎng)變化的干擾，而雙燈定位模式則沒(méi)有受到該磁場(chǎng)變化的影響。雙燈定位模式實(shí)現(xiàn)了定向糾正，提高了定位的魯棒性。

圖15 有磁場(chǎng)干擾時(shí)單燈和雙燈模式的航向角變化Fig.15 Heading angle change in single and dual light positioning methods with magnetic interference

4 結(jié)果分析

4.1 單燈定位誤差分析

根據(jù)誤差傳播率由式(2)可得定位誤差計(jì)算公式(3)。根據(jù)定位原理，模塊的定位誤差主要由三部分組成，一是光源中心的世界坐標(biāo)誤差，二是雙目視覺測(cè)量誤差，三是IMU傳感器的角度測(cè)量誤差。在單燈定位模式下，定位模塊的航向角以地磁方位角為基準(zhǔn)。由于室內(nèi)環(huán)境磁場(chǎng)不均勻，磁傳感器提供的方位角誤差約為1～4°，如果存在局部強(qiáng)磁干擾，則誤差更大，這給單燈定位模式帶來(lái)了較大誤差。當(dāng)定位模塊水平放置時(shí)，設(shè)α=0,β=0，式(3)可簡(jiǎn)化為式(4)。由式(4)分析,當(dāng)定位模塊傾斜姿態(tài)角較小時(shí)，平面定位誤差主要與航向角及定位模塊相對(duì)光源的位置有關(guān)，Z軸方向的誤差主要與視覺測(cè)量精度及模塊平面位置有關(guān)。

(3)

(4)

4.2 雙燈定位誤差分析

雙燈定位模式使用兩個(gè)坐標(biāo)已知的光源作為定向參考。航向角精度與兩個(gè)光源的世界坐標(biāo)測(cè)量精度及視覺測(cè)量精度有關(guān)，航向角精度的計(jì)算方法為：

(5)

其中：ω1為兩個(gè)光源世界坐標(biāo)系的平面相對(duì)坐標(biāo)測(cè)量誤差，ω2為兩個(gè)光源中心在雙目相機(jī)坐標(biāo)系中的平面相對(duì)坐標(biāo)計(jì)算誤差，L為兩個(gè)光源之間的距離。當(dāng)定位模塊水平放置距光源3 m處的定位區(qū)域時(shí)，根據(jù)成像幾何關(guān)系，光斑中心的提取誤差小于7 mm，則兩個(gè)光源在雙目相機(jī)坐標(biāo)系中的平面相對(duì)坐標(biāo)計(jì)算誤差ω2小于14 mm。兩個(gè)光源之間的距離L為60 cm，設(shè)ω1為4 mm，根據(jù)式(5)計(jì)算雙燈定位模式提供的航向角誤差小于1.4°，該精度相對(duì)IMU提供的航向角精度提升了一倍以上，且能夠抵抗磁場(chǎng)干擾。雙燈模式為視覺定位計(jì)算提供了更有利的觀測(cè)條件，有利于提高定位精度和可靠性。

5 結(jié) 論

本文提出一種基于可見光通信與雙目視覺的室內(nèi)定位方法，并制作了定位模塊進(jìn)行了系統(tǒng)測(cè)試。結(jié)果表明，在2 m×2 m×3 m的定位區(qū)域內(nèi)，使用單個(gè)LED光源實(shí)現(xiàn)了厘米級(jí)室內(nèi)定位，利用兩個(gè)相距60 cm的LED在定位區(qū)域內(nèi)三維方向上的定位誤差均小于5 cm，定位頻率大于5 Hz。系統(tǒng)研究了IMU與雙目成像傳感器集成檢校，利用光斑圓度定權(quán)法減弱光源成像暈光效應(yīng)對(duì)光源中心的提取誤差，在單個(gè)LED定位的基礎(chǔ)上設(shè)計(jì)了雙LED定位方法，解決了兩個(gè)光源識(shí)別的問(wèn)題并實(shí)現(xiàn)了航向角的糾正，能夠抵抗磁場(chǎng)干擾。從工程應(yīng)用角度出發(fā)，基于可見光通信與雙目視覺測(cè)量融合方式的定位方法，可以利用少量LED光源實(shí)現(xiàn)室內(nèi)高精度移動(dòng)定位，能夠滿足室內(nèi)機(jī)器人厘米級(jí)移動(dòng)定位的導(dǎo)航需求。