杜鑫 朱文亮＊ 文西芹 朱加豪

（1、江蘇海洋大學(xué)機(jī)械工程學(xué)院，江蘇 連云港 222005 2、鹽城工學(xué)院機(jī)械工程學(xué)院優(yōu)集學(xué)院，江蘇 鹽城 224007）

1 概述

同步定位建圖（Simultaneous Localization and Mapping,SLAM）作為機(jī)器人自主移動(dòng)的關(guān)鍵技術(shù)，已成為眾多移動(dòng)機(jī)器人研究者的關(guān)注焦點(diǎn)，該技術(shù)可以讓機(jī)器人感知周?chē)h(huán)境，并獲取其自身位置。定位精度的高低直接關(guān)乎到移動(dòng)機(jī)器人能否準(zhǔn)確行進(jìn)到目的地，因此如何提高定位精度是移動(dòng)機(jī)器人領(lǐng)域的重要研究?jī)?nèi)容。

國(guó)內(nèi)外研究者提出了許多定位與建圖方法，包括早期的聲納、超聲波定位和后來(lái)的射頻識(shí)別定位技術(shù)，以及目前主流的基于視覺(jué)傳感器與激光雷達(dá)的定位與建圖方法[1]?；谝曈X(jué)傳感器的方法在較為復(fù)雜的場(chǎng)景表現(xiàn)較好，傳感器從起初的單目視覺(jué)、雙目視覺(jué)，到RGB-D（深度圖像），有較大進(jìn)步，但視覺(jué)傳感器存在場(chǎng)景光線依賴程度高和計(jì)算資源占用大等缺點(diǎn)[2-3]。激光雷達(dá)傳感器有2D 和3D 之分，3D 激光雷達(dá)成本高，不適用于室內(nèi)移動(dòng)機(jī)器人的開(kāi)發(fā)與推廣。2D 激光雷達(dá)通過(guò)二維平面點(diǎn)的方式來(lái)采集機(jī)器人周?chē)h(huán)境信息，其精度較高，但因?yàn)橛行y(cè)量距離有限和采集信息量過(guò)少，在重定位和閉環(huán)檢測(cè)方面表現(xiàn)欠佳[4]。

超寬帶[5](Utral Wideband, UWB)是近年來(lái)新興的一種無(wú)載波通信技術(shù)，以占空比很低（幾十分之一）的超短電磁能量窄脈沖作為信息載體，因此又被稱為脈沖無(wú)線電,其具有高速數(shù)據(jù)傳輸、低功耗、保密性強(qiáng)、高處理增益、抗多徑能力強(qiáng)和定位精準(zhǔn)等特點(diǎn)，單個(gè)標(biāo)簽定位精度可達(dá)10cm[6]。但超寬帶定位具有漂移誤差，在標(biāo)簽與基站之間有遮擋時(shí)尤其明顯。

針對(duì)以上情況，本文提出一種四類(lèi)傳感器融合的室內(nèi)移動(dòng)機(jī)器人定位方法，該方法是在麥克納姆輪式移動(dòng)機(jī)器人上，使用超寬帶定位為機(jī)器人提供初始位姿和絕對(duì)坐標(biāo)，使用加速度計(jì)來(lái)消除超寬帶的漂移誤差，使用慣性測(cè)量單元(Inertial Measurement Unit, IMU)為機(jī)器人提供偏轉(zhuǎn)角度，并使用激光雷達(dá)和里程計(jì)組合在提前建好的地圖中進(jìn)行自適應(yīng)蒙特卡洛定位（Adaptive Monte Carlo Localization,AMCL），以此提高移動(dòng)機(jī)器人室內(nèi)定位精度。

2 定位算法原理

2.1 激光雷達(dá)定位算法原理

本文的激光雷達(dá)定位算法，采用自適應(yīng)蒙特卡洛定位算法,該算法基于蒙特卡洛統(tǒng)計(jì)思想，可理解為將粒子均勻地撒在已知地圖中，機(jī)器人在移動(dòng)過(guò)程中統(tǒng)計(jì)堆積粒子,并用通過(guò)多次迭代，估計(jì)出機(jī)器人自身所處的位置，同時(shí)在該過(guò)程中融入（Kullback-LeiblerDivergence, KLD）自適應(yīng)方法，解決了機(jī)器人綁架等問(wèn)題。

2.2 超寬帶定位算法原理

本文的超寬帶定位方法[7]，采用到達(dá)時(shí)間定位算法，又稱TOA（Time of Arrival）定位算法，該算法依據(jù)信號(hào)傳播時(shí)間來(lái)直接計(jì)算出被測(cè)點(diǎn)（標(biāo)簽）與參考點(diǎn)（基站）之間的距離。此算法中，標(biāo)簽發(fā)送信號(hào)到至少3 個(gè)以上的基站上，系統(tǒng)通過(guò)測(cè)量信號(hào)到達(dá)各個(gè)基站的時(shí)間，從而獲取標(biāo)簽與基站之間的距離，然后再以各個(gè)基站為圓心所得距離為半徑作圓，各圓的交點(diǎn)便是目的節(jié)點(diǎn)的參考位置。TOA 定位算法原理如圖1所示。

圖1 TOA 模式

已知三個(gè)基站坐標(biāo)為A0(x0,y0)、A1(x1,y1)和A2(x2,y2)，激光雷達(dá)的紅外線信號(hào)傳播速度V 為光速，t0、t1和t2為信號(hào)從標(biāo)簽TAG 到各個(gè)基站的飛行時(shí)間。標(biāo)簽的坐標(biāo)設(shè)為T(mén)0（X,Y），可由以下式（1）求得。

2.3 里程計(jì)定位算法原理

里程計(jì)定位算法選用慣性測(cè)量單元與輪式里程計(jì)組合定位，慣性測(cè)量單元采用三軸慣性測(cè)量單元來(lái)獲取機(jī)器人的姿態(tài)角和位移量，輪式里程計(jì)數(shù)據(jù)由編碼器從直流電機(jī)上采集。由麥克納姆輪運(yùn)動(dòng)方程與輪子半徑，可估計(jì)當(dāng)前位置與偏轉(zhuǎn)角大小：

式（2）中，V1、V2、V3和V4是麥克納姆輪的線速度，Vx和Vy代表移動(dòng)機(jī)器人在自身X 和Y 方向上的分速度，ω 代表移動(dòng)機(jī)器人繞豎直方向的旋轉(zhuǎn)角速度，L 和W 代表麥克納姆輪與地面的四個(gè)觸點(diǎn)構(gòu)成的矩形長(zhǎng)和寬，由此可求得Vx、Vy和ω，繼而求出一定時(shí)間內(nèi)移動(dòng)機(jī)器人的位姿變化量。

3 激光雷達(dá)、里程計(jì)和超寬帶定位融合

該方法首先對(duì)超寬帶定位數(shù)據(jù)進(jìn)行擴(kuò)展卡爾曼濾波處理，將濾波后的超寬帶定位數(shù)據(jù)用作移動(dòng)機(jī)器人的初始絕對(duì)坐標(biāo)，此舉可以提高移動(dòng)機(jī)器人初始定位速度。然后在機(jī)器人移動(dòng)過(guò)程中，每隔0.5s 將AMCL 定位輸出坐標(biāo)與超寬帶定位坐標(biāo)進(jìn)行對(duì)比，在邏輯判斷中，如果二者間出現(xiàn)5%以上的差值，則用超寬帶定位坐標(biāo)對(duì)AMCL 定位進(jìn)行修正，以此來(lái)降低里程計(jì)組合定位造成的累積誤差，該方法通過(guò)程序在移動(dòng)機(jī)器人上實(shí)現(xiàn)。融合算法流程圖如圖2 所示。

圖2 融合定位流程圖

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

本實(shí)驗(yàn)采用對(duì)比方式，令移動(dòng)機(jī)器人在室內(nèi)邊長(zhǎng)3 米的正方形路線進(jìn)行順時(shí)針閉環(huán)移動(dòng)。隨機(jī)截取端點(diǎn)實(shí)驗(yàn)數(shù)據(jù)如表1 所示。

表1 三種定位方法的定位實(shí)驗(yàn)數(shù)據(jù)

超寬帶定位效果如圖3 所示，其定位軌跡存在明顯的漂移誤差。

圖3 單一超寬帶定位軌跡圖

單一里程計(jì)的定位軌跡如圖4 所示，其定位軌跡存在較大的累積誤差。

圖4 單一里程計(jì)定位軌跡圖

激光雷達(dá)、里程計(jì)與UWB 融合的定位軌跡如圖5 所示，累積誤差較里程計(jì)定位有所減少，且能基本實(shí)現(xiàn)軌跡閉環(huán)。

圖5 激光雷達(dá)、里程計(jì)與超寬帶融合軌跡圖

5 結(jié)論

5.1 傳統(tǒng)AMCL 定位方法是在移動(dòng)機(jī)器人行進(jìn)過(guò)程中，不斷迭代處理粒子來(lái)實(shí)現(xiàn)定位，而利用機(jī)器人自主初始化位姿的方法，時(shí)間更是長(zhǎng)達(dá)90s 左右。本研究中引入了超寬帶定位方法，在10s 內(nèi)即可完成初始定位，顯著提高了室內(nèi)移動(dòng)機(jī)器人初始定位速度。

5.2 在融合定位方法中，超寬帶定位為移動(dòng)機(jī)器人提供了絕對(duì)坐標(biāo)，消除了里程計(jì)與慣性測(cè)量單元在定位上的累積誤差，融合后的定位軌跡能基本實(shí)現(xiàn)閉合，較里程計(jì)與慣性測(cè)量單元的組合定位，閉環(huán)效果更好。

通過(guò)實(shí)驗(yàn)數(shù)據(jù)分析，該定位算法較AMCL 定位方法加快了位姿初始化的速度，時(shí)間節(jié)省了80s 左右；其全局定位精度較單一超寬帶定位方法或激光雷達(dá)、里程計(jì)組合定位方法，都有不同程度的提高，尤其相于AMCL 定位方法，定位精度可提高8%。該方法對(duì)室內(nèi)移動(dòng)機(jī)器人定位精度的提高具有一定的參考價(jià)值。