移動(dòng)機(jī)器人室內(nèi)外無(wú)縫切換定位方法研究

劉慧敏，李國(guó)麗，董 翔，張 鑫，汪 鵬

（安徽大學(xué)，合肥 230601）

0 引言

隨著科學(xué)技術(shù)的不斷發(fā)展和移動(dòng)機(jī)器人涉及領(lǐng)域的逐漸擴(kuò)展，對(duì)既能實(shí)現(xiàn)室內(nèi)定位也能實(shí)現(xiàn)室外定位機(jī)器人的需求日趨強(qiáng)烈[1]。為了實(shí)現(xiàn)室內(nèi)外環(huán)境下定位的連續(xù)性和平滑性，使其能夠無(wú)縫銜接，無(wú)縫定位技術(shù)應(yīng)運(yùn)而生[2]。GPS具有定位誤差較小、覆蓋范圍廣、用戶(hù)投入成本低等優(yōu)點(diǎn)，廣泛應(yīng)用于室外定位技術(shù)，但其單獨(dú)使用時(shí)會(huì)存在抗干擾能力較差以及信號(hào)易被遮擋等不足。慣性導(dǎo)航系統(tǒng)（Inertial Navigation System，INS）作為基于航位推算原理的非視距定位傳感器，抗干擾能力強(qiáng)，短時(shí)間內(nèi)的定位精度較高，但長(zhǎng)時(shí)間的定位容易產(chǎn)生累積誤差造成漂移。因此本文選擇GPS和INS組合作為室外環(huán)境定位系統(tǒng)。Wi-Fi、藍(lán)牙、UWB等都可以實(shí)現(xiàn)基本的室內(nèi)定位，但在定位精度，投入成本，復(fù)雜度上有所不同。Wi-Fi指紋庫(kù)需要時(shí)常更新維護(hù)，人工成本較大；藍(lán)牙雖然體積小，造價(jià)低，但易受多信道噪聲干擾，穩(wěn)定性差；超寬帶（ultrawide band，UWB）可靠性高，信號(hào)穿透性強(qiáng)不需要載波，其定位幀可以在多噪聲無(wú)線信道環(huán)境下進(jìn)行可靠地?cái)?shù)據(jù)傳輸，信號(hào)傳輸距離遠(yuǎn)且傳輸速度較快，不易受外界干擾，定位精度可達(dá)到厘米級(jí)、系統(tǒng)較簡(jiǎn)單。因此本文選擇UWB和INS組合作為室內(nèi)環(huán)境定位系統(tǒng)。文獻(xiàn)[3，4]提出室外采用北斗與GPS的雙模定位，室內(nèi)以及衛(wèi)星信號(hào)缺失的情況下采用基于IMU的航位推算算法，無(wú)法滿(mǎn)足長(zhǎng)時(shí)間室內(nèi)復(fù)雜環(huán)境下的定位精度要求。文獻(xiàn)[5]采用赫爾默特方差估計(jì)分類(lèi)法對(duì)兩種定位系統(tǒng)定權(quán)，沒(méi)有設(shè)置閾值來(lái)避免運(yùn)算浪費(fèi)。

本文實(shí)現(xiàn)了基于閾值機(jī)制和多傳感器定位精度的室內(nèi)外定位無(wú)縫切換，使系統(tǒng)在運(yùn)動(dòng)過(guò)程中進(jìn)行有效的定位方式切換以實(shí)現(xiàn)當(dāng)前區(qū)域下最精確的定位，同時(shí)避免傳感器間反復(fù)不必要的切換造成運(yùn)算量的浪費(fèi)。在室外環(huán)境下使用GPS/IMU組合定位來(lái)提供較精確的定位服務(wù)，在如遮擋處、室內(nèi)等無(wú)法感知GPS衛(wèi)星信號(hào)的環(huán)境下，系統(tǒng)切換至UWB/IMU組合定位，從而實(shí)現(xiàn)室內(nèi)外的無(wú)縫精準(zhǔn)定位。具體工作如下：

1）構(gòu)建機(jī)器人全局地圖；

2）使用容積卡爾曼（CKF）實(shí)現(xiàn)室外GPS/IMU以及室外UWB/IMU融合定位；

3）利用基于閾值機(jī)制和多傳感器定位精度的室內(nèi)外定位的無(wú)縫切換算法實(shí)現(xiàn)室內(nèi)外定位的無(wú)縫切換。通過(guò)實(shí)驗(yàn)平臺(tái)驗(yàn)證了該算法提高了室內(nèi)外切換的中間區(qū)域定位的精度，使切換更加平滑穩(wěn)定，優(yōu)于傳統(tǒng)的算法。

1 全局地圖的構(gòu)建

為了方便組合定位，需要將各個(gè)定位系統(tǒng)輸出的移動(dòng)機(jī)器人位置坐標(biāo)等信息轉(zhuǎn)換至統(tǒng)一的坐標(biāo)系中，這是實(shí)現(xiàn)室內(nèi)外無(wú)縫定位的基礎(chǔ)和前提。地理坐標(biāo)系又叫做東-北-天坐標(biāo)系，該坐標(biāo)系以載體中心為坐標(biāo)原點(diǎn)，x軸、y軸、z軸分別指向東、北、天方向。本文選擇該坐標(biāo)系作為全局坐標(biāo)系（n系）。

GPS測(cè)得的經(jīng)緯度坐標(biāo)需要先轉(zhuǎn)換至地心地固坐標(biāo)系下再轉(zhuǎn)換至移動(dòng)機(jī)器人全局坐標(biāo)系（n系）。選取機(jī)器人定位區(qū)域內(nèi)的某一點(diǎn)為全局坐標(biāo)系原點(diǎn)，并測(cè)得原點(diǎn)的經(jīng)緯度坐標(biāo)（lon0，lat0，alt0）。地心地固坐標(biāo)系（ECEF系）下的某一點(diǎn)經(jīng)過(guò)旋轉(zhuǎn)矩陣Cn

e可以得到全局坐標(biāo)系下的坐標(biāo)。

機(jī)器人采用的是捷聯(lián)式慣性導(dǎo)航系統(tǒng)（SINS，Strapdown Inertial Navigation System），陀螺儀、加速度計(jì)以及磁力計(jì)直接固定在載體上。在運(yùn)動(dòng)過(guò)程中，磁力計(jì)可以測(cè)得載體相對(duì)于真北方向的角度，陀螺儀可以測(cè)得載體相對(duì)于慣性參考系的角速度，從而得到載體運(yùn)動(dòng)過(guò)程中的橫滾角、俯仰角、偏航角。慣性導(dǎo)航輸出的載體坐標(biāo)系（b系）下的坐標(biāo)通過(guò)旋轉(zhuǎn)矩陣轉(zhuǎn)換至移動(dòng)機(jī)器人全局坐標(biāo)系（n系）下，旋轉(zhuǎn)矩陣為Cnb。將加速度計(jì)測(cè)量得到的載體坐標(biāo)系下的加速度以及陀螺儀測(cè)量得到的載體坐標(biāo)系下的角速度經(jīng)過(guò)旋轉(zhuǎn)矩陣變換至移動(dòng)機(jī)器人全局坐標(biāo)系下是慣導(dǎo)解算的前提。

其中c代表三角函數(shù)cos，s代表三角函數(shù)sin，γ、θ、ψ分別為載體橫滾角、俯仰角、偏航角。

2 室內(nèi)外定位方法

2.1 室外環(huán)境下的GPS定位

在定位區(qū)域內(nèi)設(shè)定一點(diǎn)為導(dǎo)航坐標(biāo)系（enu坐標(biāo)系）的原點(diǎn)P0，經(jīng)緯度坐標(biāo)為(lon0，lat0，alt0)，P0點(diǎn)在ECEF系下對(duì)應(yīng)坐標(biāo)為(x0，y0，z0)。機(jī)器人運(yùn)動(dòng)過(guò)程中某一點(diǎn)Pi(loni，lati，alti)，可以通過(guò)如下過(guò)程轉(zhuǎn)換至導(dǎo)航坐標(biāo)系下。其中，(xi，yi，zi)為該點(diǎn)在ECEF系中對(duì)應(yīng)的坐標(biāo)，(ei，ni，ui)為該點(diǎn)在導(dǎo)航坐標(biāo)系下的坐標(biāo)。

其中，e為橢球體偏心率，N為基準(zhǔn)橢球體曲率半徑。

圖1 地圖坐標(biāo)系示意圖

2.2 室內(nèi)環(huán)境下的UWB定位

目前的室內(nèi)定位方案主要有基于RSSI（接收信號(hào)強(qiáng)度）的藍(lán)牙定位技術(shù)、基于反射式測(cè)距法的超聲波定位技術(shù)、基于TOF（飛行時(shí)間）的UWB定位技術(shù)、基于指紋定位的WIFI定位技術(shù)等。本文選用UWB作為室內(nèi)定位傳感器，采用TWR測(cè)距。

UWB的定位幀中的的測(cè)距信息由定位請(qǐng)求信息、應(yīng)答信息以及最終信息組成。以測(cè)得標(biāo)簽與基站1之間定位信息的飛行時(shí)間TOF為例，需測(cè)得標(biāo)簽接收到基站1應(yīng)答信息的時(shí)間TRR與標(biāo)簽發(fā)送定位請(qǐng)求信息時(shí)間TTP之差、基站1接收到標(biāo)簽發(fā)送的最終信息時(shí)間TBF與基站1發(fā)送應(yīng)答信息時(shí)間TTR之差、標(biāo)簽發(fā)送最終信息時(shí)間TTF與標(biāo)簽接收到基站1應(yīng)答消息時(shí)間TRR之差、基站1發(fā)送應(yīng)答消息時(shí)間TTR與基站1接收到標(biāo)簽發(fā)送定位請(qǐng)求信息時(shí)間TRP之差。標(biāo)簽與各個(gè)基站的距離d可以用TOF與光速c的乘積得到。

圖2 雙邊測(cè)距原理圖

實(shí)現(xiàn)測(cè)距后，首先確定各個(gè)UWB基站在導(dǎo)航坐標(biāo)系中的位置坐標(biāo)，基站坐標(biāo)的精確度直接影響定位的精度。在得到移動(dòng)機(jī)器人距離基站的距離后，可對(duì)移動(dòng)節(jié)點(diǎn)進(jìn)行定位[6]。已知距離機(jī)器人最近的三個(gè)基站及坐標(biāo)為B1(x1，y1)、B2(x2，y2)、B3(x3，y3)。R(x，y)為待求的機(jī)器人坐標(biāo)，標(biāo)簽距三個(gè)基站的距離分別為d1、d2、d3。B1與B2之間距離為l12，B1與B2之間距離為l13。以B1、B2為焦點(diǎn)，作過(guò)(x，y)的雙曲線，再以B1、B3為焦點(diǎn)，作過(guò)(x，y)的雙曲線，如圖3所示。

圖3 雙曲線定位原理圖

其中：

根據(jù)余弦定理，可得：

整理可得：

同理可得：

可得移動(dòng)機(jī)器人的位置坐標(biāo)。

2.3 慣性導(dǎo)航系統(tǒng)定位

慣性導(dǎo)航系統(tǒng)（Inertial Navigation System，INS）由加速度計(jì)（Accelerometer）、陀螺儀（Gyro）、磁力計(jì)（magnetometer）構(gòu)成。通過(guò)磁力計(jì)的指北功能可以得到慣性導(dǎo)航模塊相對(duì)于地理坐標(biāo)系的偏轉(zhuǎn)角度。通過(guò)對(duì)線加速度和角加速度的量測(cè)，進(jìn)行積分運(yùn)算從而推導(dǎo)出載體的位置、姿態(tài)和速度信息。

加速度計(jì)可以測(cè)出三軸的比力，將所測(cè)比力信息通過(guò)坐標(biāo)轉(zhuǎn)換，去除地球自轉(zhuǎn)角速度以及重力加速度的影響后得到速度微分進(jìn)而得到三軸的速度信息。同樣，已知載體上一時(shí)刻的位置坐標(biāo)及慣性導(dǎo)航系統(tǒng)的采樣周期為T(mén)，將已經(jīng)解算出的速度進(jìn)行周期性的積分即可得到這一時(shí)刻的位置坐標(biāo)。

3 基于容積卡爾曼濾波器的融合定位

當(dāng)系統(tǒng)模型是非線性的，需要非線性濾波方法來(lái)進(jìn)行數(shù)據(jù)的融合，容積卡爾曼濾波器通過(guò)對(duì)非線性系統(tǒng)的概率分布進(jìn)行近似輸出，這要比通過(guò)復(fù)雜的雅可比矩陣求解將非線性系統(tǒng)近似為線性系統(tǒng)要容易且精度更高。CKF利用一組等權(quán)值的容積點(diǎn)來(lái)進(jìn)行狀態(tài)和協(xié)方差的更新[8]。

圖4 容積卡爾曼融合定位系統(tǒng)框架

慣導(dǎo)系統(tǒng)的誤差值作為狀態(tài)變量，維數(shù)為15，ΔR為位置誤差，ΔV為速度誤差，Δ?為姿態(tài)角誤差，Δ為加速度計(jì)零偏誤差，ε為陀螺儀零偏誤差[9]。

將INS和GPS/UWB解算得到的位置、速度信息作差作為濾波器的觀測(cè)量Z，維數(shù)為6，RINS和VINS為n系下INS解算出的位置和速度，RGPS/UWB和VGPS/UWB為n系下GPS/UWB解算出的位置和速度。

由IMU運(yùn)動(dòng)學(xué)方程可得系統(tǒng)的非線性狀態(tài)方程。

由GPS量測(cè)數(shù)據(jù)和UWB量測(cè)數(shù)據(jù)可得系統(tǒng)的非線性觀測(cè)方程。

其中αk和βk分別為系統(tǒng)過(guò)程噪聲和系統(tǒng)測(cè)量噪聲。容積卡爾曼濾波器通過(guò)對(duì)后驗(yàn)概率密度進(jìn)行高斯近似來(lái)實(shí)現(xiàn)系統(tǒng)的狀態(tài)估計(jì)，對(duì)一組等權(quán)值的cubature采樣點(diǎn)的加權(quán)計(jì)算來(lái)近似求解積分，得到狀態(tài)變量的均值和方差。Fk為估計(jì)協(xié)方差的一個(gè)平方根，。cubature采樣點(diǎn)的個(gè)數(shù)為2n，n為狀態(tài)變量的維數(shù)。η為cubature采樣點(diǎn)的點(diǎn)集，點(diǎn)集的第i個(gè)元素為ηi，該點(diǎn)對(duì)應(yīng)的權(quán)值為εi[10]。

其中[1]代表將n維單位向量e=[1，0，...，0]T的元素進(jìn)行全排列并改變符號(hào)產(chǎn)生的完整全對(duì)稱(chēng)點(diǎn)集的第i個(gè)元素。

1）時(shí)間更新

非線性狀態(tài)方程變換后的采樣點(diǎn)：

2）測(cè)量更新

非線性量測(cè)方程變換后的采樣點(diǎn)：

4 室內(nèi)外無(wú)縫切換定位方法

無(wú)縫切換定位技術(shù)是指機(jī)器人在復(fù)雜環(huán)境內(nèi)運(yùn)動(dòng)過(guò)程中可以根據(jù)室內(nèi)外環(huán)境的特征，選擇最佳的定位方式以保證定位的精準(zhǔn)性，同時(shí)在進(jìn)行室內(nèi)外定位方式切換時(shí)需要滿(mǎn)足定位的無(wú)縫銜接、平滑穩(wěn)定，最終實(shí)現(xiàn)機(jī)器人在室內(nèi)室外以及室內(nèi)外中間區(qū)域全覆蓋高精度的定位。GPS的定位精度不僅僅與可視衛(wèi)星數(shù)量有關(guān)，如果只根據(jù)GPS搜索衛(wèi)星數(shù)量是否大于4來(lái)作為定位切換的條件，無(wú)法達(dá)到無(wú)縫切換的要求。本文提出一種基于閾值機(jī)制和多傳感器定位精度的室內(nèi)外定位切換算法，可以通過(guò)傳感器實(shí)時(shí)定位精度來(lái)決定各傳感器系統(tǒng)的置信度在切換區(qū)域內(nèi)實(shí)現(xiàn)室內(nèi)外兩種定位系統(tǒng)的充分融合，避免出現(xiàn)定位跳變和由于在臨界區(qū)域內(nèi)反復(fù)無(wú)效的定位切換產(chǎn)生乒乓效應(yīng)導(dǎo)致的大量運(yùn)算浪費(fèi)。

定位系統(tǒng)間切換時(shí)基于對(duì)兩套定位傳感器的精度的判斷，采用多個(gè)門(mén)限閾值同時(shí)也會(huì)降低切換出錯(cuò)的概率。在GPS定位中，幾何精度因子（Geometric Dilution Precision，GDOP）是衡量定位質(zhì)量的好壞的重要參數(shù)，這意味著各種延遲和噪聲造成的距離誤差會(huì)被幾何放大。GDOP的大小受所能觀測(cè)到的衛(wèi)星在空間中幾何分布的影響，若當(dāng)前可見(jiàn)衛(wèi)星數(shù)量不少于4且在空間中分布較為分散定位結(jié)果可以獲得更高的準(zhǔn)確率。GDOP的數(shù)值越大，當(dāng)前時(shí)刻的定位精度越差。用戶(hù)等效測(cè)距誤差（User Equivalent Range Error，UERE）包含用戶(hù)測(cè)距誤差（User Range Error，URE）和用戶(hù)設(shè)備誤差（User Equipment Error，UEE）[11]。URE主要取決于衛(wèi)星鐘差、衛(wèi)星的位置和星歷誤差，和機(jī)器人平臺(tái)的位置無(wú)關(guān)。UEE與機(jī)器人所處環(huán)境有關(guān)。通過(guò)GDOP和UERE可以評(píng)估GPS定位的精度PGPS。

由于UWB定位誤差與信標(biāo)和基站之間的幾何分布關(guān)系、信號(hào)強(qiáng)度的大小、標(biāo)簽當(dāng)前處于靜止?fàn)顟B(tài)還是運(yùn)動(dòng)狀態(tài)以及磁場(chǎng)干擾等因素有關(guān)，所以機(jī)器人在實(shí)驗(yàn)場(chǎng)景的不同區(qū)域的UWB定位精度各不相同。對(duì)實(shí)驗(yàn)場(chǎng)景中的室內(nèi)外中間區(qū)域的UWB的定位數(shù)據(jù)進(jìn)行多次采樣，并計(jì)算出該區(qū)域內(nèi)標(biāo)簽與基站不同距離對(duì)應(yīng)的UWB定位精度PUWB。

在室內(nèi)與室外區(qū)域間有一個(gè)中間區(qū)域，在這個(gè)區(qū)域內(nèi)兩套定位傳感器的定位信息都能接收到，當(dāng)機(jī)器人從其中一個(gè)定位系統(tǒng)服務(wù)區(qū)域運(yùn)行到中間區(qū)域時(shí)，接收到了另一個(gè)定位系統(tǒng)的信號(hào)，如果此時(shí)直接采取定位切換，最終的定位結(jié)果就會(huì)與無(wú)縫銜接、平滑穩(wěn)定的需求相違背?？紤]到兩套定位系統(tǒng)的定位精度不同以及定位切換算法不能過(guò)于繁瑣，可以對(duì)兩個(gè)定位系統(tǒng)的定位結(jié)果進(jìn)行加權(quán)融合。

式(32)中，(x，y)為加權(quán)融合后的定位坐標(biāo)，(xGPS，yGPS)為GPS定位坐標(biāo)，(xUWB，yUWB)為UWB定位坐標(biāo)，k1為GPS融合權(quán)重，k2為UWB融合權(quán)重。

GPS系統(tǒng)的定位誤差（δxGPS,δyGPS）在二維空間中服從二維正態(tài)分布。UWB系統(tǒng)的定位誤差（δxUWB,δyUWB）同理。兩個(gè)系統(tǒng)在室內(nèi)外中間區(qū)域融合后的誤差為（δx,δy）。?1、?2分別為GPS、UWB定位誤差均方差。

融合的目的是為了讓定位誤差最小化，所以需要滿(mǎn)足下式：

由此可確定符合融合目的的權(quán)值范圍：

圖5 室內(nèi)外無(wú)縫切換算法流程圖

無(wú)縫切換算法中引入計(jì)數(shù)值n、計(jì)數(shù)閾值N、減小率R（0＜R＜1，R的大小需要根據(jù)傳感器頻率設(shè)定）、UWB定位精度的兩個(gè)門(mén)限閾值α1和α2（其中α1＜α2）。啟動(dòng)定位后，首先對(duì)GDOP的大小進(jìn)行判斷，若GDOP小于3，說(shuō)明此時(shí)GPS模塊可以提供較為精準(zhǔn)的定位，當(dāng)前可觀測(cè)衛(wèi)星空間分布狀況良好，機(jī)器人處于無(wú)遮擋的室外環(huán)境，選擇使用GPS/IMU組合定位；若3＜GDOP＜7，且UWB的定位精度滿(mǎn)足條件α1＜PUWB＜α2，說(shuō)明此時(shí)機(jī)器人可能處在室內(nèi)外切換的中間區(qū)域，滿(mǎn)足觸發(fā)切換的條件，此時(shí)計(jì)數(shù)值加1，當(dāng)大于計(jì)數(shù)閾值N時(shí)，進(jìn)行切換判決階段，確定機(jī)器人處于中間區(qū)域，此時(shí)采用基于定位精度的加權(quán)平均的算法融合GPS和UWB的定位結(jié)果，計(jì)數(shù)閾值的使用可以有效避免乒乓效應(yīng)的產(chǎn)生；若GDOP＞7且滿(mǎn)足PUWB＜α1，機(jī)器人可能已經(jīng)運(yùn)行到了室內(nèi)，此時(shí)GPS基本為無(wú)效定位，如果繼續(xù)采用GPS進(jìn)行定位，會(huì)使定位結(jié)果產(chǎn)生漂移，這時(shí)將計(jì)數(shù)值乘上一個(gè)減小率R，如果定位精度一直滿(mǎn)足當(dāng)前的條件，直到小于計(jì)數(shù)閾值N時(shí)，即可確定移動(dòng)機(jī)器人處于室內(nèi)，系統(tǒng)切換到UWB/IMU組合方式進(jìn)行定位。

5 實(shí)驗(yàn)及分析

為了對(duì)本文的定位方法進(jìn)行驗(yàn)證，在安徽大學(xué)校磬苑校區(qū)內(nèi)使用機(jī)器人小車(chē)搭載的模塊進(jìn)行數(shù)據(jù)采集，由于機(jī)器人實(shí)驗(yàn)平臺(tái)的車(chē)輪對(duì)地面光滑程度要求較高，所以選擇由室外馬路至一樓車(chē)庫(kù)作為實(shí)驗(yàn)場(chǎng)地。實(shí)驗(yàn)軌跡如圖6所示，實(shí)驗(yàn)平臺(tái)實(shí)物和結(jié)構(gòu)簡(jiǎn)圖分別如圖7(a)、圖7(b)所示。

圖6 實(shí)驗(yàn)軌跡示意圖

圖7 移動(dòng)機(jī)器人平臺(tái)

實(shí)驗(yàn)采用SIM868模塊進(jìn)行GPS數(shù)據(jù)的采集，MPU9250進(jìn)行角度、角速度、線加速度的采集，DWM1000實(shí)現(xiàn)UWB信標(biāo)和基站之間距離的采集。實(shí)驗(yàn)使用的GPS模塊數(shù)據(jù)采集頻率為1Hz，UWB模塊的數(shù)據(jù)采集頻率為3Hz，使用插值的方法實(shí)現(xiàn)室內(nèi)外數(shù)據(jù)在時(shí)間上的對(duì)應(yīng)。通過(guò)UWB定位精度測(cè)量發(fā)現(xiàn)機(jī)器人在運(yùn)行到UWB基站附近時(shí)和運(yùn)行到室內(nèi)外中間區(qū)域時(shí)，定位誤差較機(jī)器人處于基站連線以?xún)?nèi)的區(qū)域時(shí)高。

圖8中，虛線為實(shí)測(cè)軌跡，點(diǎn)為實(shí)測(cè)定位點(diǎn)，實(shí)線為濾波后并使用不同切換算法的定位軌跡，左側(cè)區(qū)域?yàn)槭彝鈪^(qū)域，中間淺色區(qū)域?yàn)槭覂?nèi)外切換的中間區(qū)域，右側(cè)深色區(qū)域?yàn)槭覂?nèi)區(qū)域。可以看出在UWB基站分布良好且沒(méi)有遮擋物的情況下UWB的定位精度比GPS定位精度高很多，機(jī)器人在UWB基站旁轉(zhuǎn)向，此時(shí)的定位誤差較大，精度受到影響。圖8(a)中標(biāo)注軌跡為單基于UWB定位精度進(jìn)行室內(nèi)外定位切換的軌跡，當(dāng)機(jī)器人從室外向室內(nèi)運(yùn)行時(shí)，定位系統(tǒng)只在UWB的定位誤差小于閾值α1時(shí)切換到UWB進(jìn)行定位。圖8(b)中標(biāo)注軌跡為單基于GPS定位精度進(jìn)行室內(nèi)外傳感器切換的定位軌跡，當(dāng)移動(dòng)機(jī)器人從室外向室內(nèi)運(yùn)動(dòng)時(shí)，定位傳感器只在GPS的GDOP大于7時(shí)切換到UWB進(jìn)行定位。圖8(c)中標(biāo)注軌跡是使用了基于計(jì)數(shù)閾值和雙定位精度的無(wú)縫切換算法進(jìn)行切換的定位軌跡。由實(shí)驗(yàn)結(jié)果分析得到使用無(wú)縫切換算法可以實(shí)現(xiàn)室內(nèi)外定位切換的平滑穩(wěn)定。

圖8 不同切換算法下的定位軌跡圖

圖9中，區(qū)域Ⅰ為室外區(qū)域，區(qū)域Ⅱ?yàn)槭覂?nèi)外切換的中間區(qū)域，區(qū)域Ⅲ為室內(nèi)區(qū)域。

圖9 不同切換算法下的定位誤差

表1為在使用CKF的基礎(chǔ)上使用各種切換算法在中間區(qū)域的誤差對(duì)比。由圖9和表1得到，使用無(wú)縫切換算法可以有效減小室內(nèi)外切換中間區(qū)域的定位誤差。

表1 各切換算法在中間區(qū)域的誤差

圖10中，由于遮擋導(dǎo)致GPS的幾何精度因子會(huì)發(fā)生跳變，使用未采用計(jì)數(shù)閾值的切換算法會(huì)導(dǎo)致定位系統(tǒng)在中間區(qū)域時(shí)反復(fù)切換造成不必要的運(yùn)算浪費(fèi)。

圖10 采用計(jì)數(shù)閾值和未采用計(jì)數(shù)閾值切換對(duì)比

6 結(jié)語(yǔ)

本文開(kāi)發(fā)設(shè)計(jì)了一種基于閾值機(jī)制和多傳感器定位精度的室內(nèi)外無(wú)縫定位切換算法，通過(guò)室內(nèi)和室外兩種定位系統(tǒng)實(shí)時(shí)精度來(lái)分配置信度從而提高室內(nèi)外中間區(qū)域定位精度并保證定位軌跡平滑，同時(shí)采用計(jì)數(shù)閾值的方法避免反復(fù)切換造成乒乓效應(yīng)，增加了系統(tǒng)穩(wěn)定性。搭建實(shí)驗(yàn)平臺(tái)完成對(duì)該方法的驗(yàn)證，實(shí)現(xiàn)了室內(nèi)外定位的無(wú)縫切換，完成了同一坐標(biāo)系下室內(nèi)外的全局定位。室內(nèi)外無(wú)縫切換定位在巡檢機(jī)器人、消防機(jī)器人上都有廣闊的應(yīng)用前景。