移動機器人室內外無縫切換定位方法研究

劉慧敏，李國麗，董 翔，張 鑫，汪 鵬

（安徽大學，合肥 230601）

0 引言

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

本文實現了基于閾值機制和多傳感器定位精度的室內外定位無縫切換，使系統在運動過程中進行有效的定位方式切換以實現當前區域下最精確的定位，同時避免傳感器間反復不必要的切換造成運算量的浪費。在室外環境下使用GPS/IMU組合定位來提供較精確的定位服務，在如遮擋處、室內等無法感知GPS衛星信號的環境下，系統切換至UWB/IMU組合定位，從而實現室內外的無縫精準定位。具體工作如下：

1）構建機器人全局地圖；

2）使用容積卡爾曼（CKF）實現室外GPS/IMU以及室外UWB/IMU融合定位；

3）利用基于閾值機制和多傳感器定位精度的室內外定位的無縫切換算法實現室內外定位的無縫切換。通過實驗平臺驗證了該算法提高了室內外切換的中間區域定位的精度，使切換更加平滑穩定，優于傳統的算法。

1 全局地圖的構建

為了方便組合定位，需要將各個定位系統輸出的移動機器人位置坐標等信息轉換至統一的坐標系中，這是實現室內外無縫定位的基礎和前提。地理坐標系又叫做東-北-天坐標系，該坐標系以載體中心為坐標原點，x軸、y軸、z軸分別指向東、北、天方向。本文選擇該坐標系作為全局坐標系（n系）。

GPS測得的經緯度坐標需要先轉換至地心地固坐標系下再轉換至移動機器人全局坐標系（n系）。選取機器人定位區域內的某一點為全局坐標系原點，并測得原點的經緯度坐標（lon0，lat0，alt0）。地心地固坐標系（ECEF系）下的某一點經過旋轉矩陣Cn

e可以得到全局坐標系下的坐標。

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

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

2 室內外定位方法

2.1 室外環境下的GPS定位

在定位區域內設定一點為導航坐標系（enu坐標系）的原點P0，經緯度坐標為(lon0，lat0，alt0)，P0點在ECEF系下對應坐標為(x0，y0，z0)。機器人運動過程中某一點Pi(loni，lati，alti)，可以通過如下過程轉換至導航坐標系下。其中，(xi，yi，zi)為該點在ECEF系中對應的坐標，(ei，ni，ui)為該點在導航坐標系下的坐標。

其中，e為橢球體偏心率，N為基準橢球體曲率半徑。

圖1 地圖坐標系示意圖

2.2 室內環境下的UWB定位

目前的室內定位方案主要有基于RSSI（接收信號強度）的藍牙定位技術、基于反射式測距法的超聲波定位技術、基于TOF（飛行時間）的UWB定位技術、基于指紋定位的WIFI定位技術等。本文選用UWB作為室內定位傳感器，采用TWR測距。

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

圖2 雙邊測距原理圖

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

圖3 雙曲線定位原理圖

其中：

根據余弦定理，可得：

整理可得：

同理可得：

可得移動機器人的位置坐標。

2.3 慣性導航系統定位

慣性導航系統（Inertial Navigation System，INS）由加速度計（Accelerometer）、陀螺儀（Gyro）、磁力計（magnetometer）構成。通過磁力計的指北功能可以得到慣性導航模塊相對于地理坐標系的偏轉角度。通過對線加速度和角加速度的量測，進行積分運算從而推導出載體的位置、姿態和速度信息。

加速度計可以測出三軸的比力，將所測比力信息通過坐標轉換，去除地球自轉角速度以及重力加速度的影響后得到速度微分進而得到三軸的速度信息。同樣，已知載體上一時刻的位置坐標及慣性導航系統的采樣周期為T，將已經解算出的速度進行周期性的積分即可得到這一時刻的位置坐標。

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

當系統模型是非線性的，需要非線性濾波方法來進行數據的融合，容積卡爾曼濾波器通過對非線性系統的概率分布進行近似輸出，這要比通過復雜的雅可比矩陣求解將非線性系統近似為線性系統要容易且精度更高。CKF利用一組等權值的容積點來進行狀態和協方差的更新[8]。

圖4 容積卡爾曼融合定位系統框架

慣導系統的誤差值作為狀態變量，維數為15，ΔR為位置誤差，ΔV為速度誤差，Δ?為姿態角誤差，Δ為加速度計零偏誤差，ε為陀螺儀零偏誤差[9]。

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

由IMU運動學方程可得系統的非線性狀態方程。

由GPS量測數據和UWB量測數據可得系統的非線性觀測方程。

其中αk和βk分別為系統過程噪聲和系統測量噪聲。容積卡爾曼濾波器通過對后驗概率密度進行高斯近似來實現系統的狀態估計，對一組等權值的cubature采樣點的加權計算來近似求解積分，得到狀態變量的均值和方差。Fk為估計協方差的一個平方根，。cubature采樣點的個數為2n，n為狀態變量的維數。η為cubature采樣點的點集，點集的第i個元素為ηi，該點對應的權值為εi[10]。

其中[1]代表將n維單位向量e=[1，0，...，0]T的元素進行全排列并改變符號產生的完整全對稱點集的第i個元素。

1）時間更新

非線性狀態方程變換后的采樣點：

2）測量更新

非線性量測方程變換后的采樣點：

4 室內外無縫切換定位方法

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

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

由于UWB定位誤差與信標和基站之間的幾何分布關系、信號強度的大小、標簽當前處于靜止狀態還是運動狀態以及磁場干擾等因素有關，所以機器人在實驗場景的不同區域的UWB定位精度各不相同。對實驗場景中的室內外中間區域的UWB的定位數據進行多次采樣，并計算出該區域內標簽與基站不同距離對應的UWB定位精度PUWB。

在室內與室外區域間有一個中間區域，在這個區域內兩套定位傳感器的定位信息都能接收到，當機器人從其中一個定位系統服務區域運行到中間區域時，接收到了另一個定位系統的信號，如果此時直接采取定位切換，最終的定位結果就會與無縫銜接、平滑穩定的需求相違背。考慮到兩套定位系統的定位精度不同以及定位切換算法不能過于繁瑣，可以對兩個定位系統的定位結果進行加權融合。

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

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

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

由此可確定符合融合目的的權值范圍：

圖5 室內外無縫切換算法流程圖

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

5 實驗及分析

為了對本文的定位方法進行驗證，在安徽大學校磬苑校區內使用機器人小車搭載的模塊進行數據采集，由于機器人實驗平臺的車輪對地面光滑程度要求較高，所以選擇由室外馬路至一樓車庫作為實驗場地。實驗軌跡如圖6所示，實驗平臺實物和結構簡圖分別如圖7(a)、圖7(b)所示。

圖6 實驗軌跡示意圖

圖7 移動機器人平臺

實驗采用SIM868模塊進行GPS數據的采集，MPU9250進行角度、角速度、線加速度的采集，DWM1000實現UWB信標和基站之間距離的采集。實驗使用的GPS模塊數據采集頻率為1Hz，UWB模塊的數據采集頻率為3Hz，使用插值的方法實現室內外數據在時間上的對應。通過UWB定位精度測量發現機器人在運行到UWB基站附近時和運行到室內外中間區域時，定位誤差較機器人處于基站連線以內的區域時高。

圖8中，虛線為實測軌跡，點為實測定位點，實線為濾波后并使用不同切換算法的定位軌跡，左側區域為室外區域，中間淺色區域為室內外切換的中間區域，右側深色區域為室內區域。可以看出在UWB基站分布良好且沒有遮擋物的情況下UWB的定位精度比GPS定位精度高很多，機器人在UWB基站旁轉向，此時的定位誤差較大，精度受到影響。圖8(a)中標注軌跡為單基于UWB定位精度進行室內外定位切換的軌跡，當機器人從室外向室內運行時，定位系統只在UWB的定位誤差小于閾值α1時切換到UWB進行定位。圖8(b)中標注軌跡為單基于GPS定位精度進行室內外傳感器切換的定位軌跡，當移動機器人從室外向室內運動時，定位傳感器只在GPS的GDOP大于7時切換到UWB進行定位。圖8(c)中標注軌跡是使用了基于計數閾值和雙定位精度的無縫切換算法進行切換的定位軌跡。由實驗結果分析得到使用無縫切換算法可以實現室內外定位切換的平滑穩定。

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

圖9中，區域Ⅰ為室外區域，區域Ⅱ為室內外切換的中間區域，區域Ⅲ為室內區域。

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

表1為在使用CKF的基礎上使用各種切換算法在中間區域的誤差對比。由圖9和表1得到，使用無縫切換算法可以有效減小室內外切換中間區域的定位誤差。

表1 各切換算法在中間區域的誤差

圖10中，由于遮擋導致GPS的幾何精度因子會發生跳變，使用未采用計數閾值的切換算法會導致定位系統在中間區域時反復切換造成不必要的運算浪費。

圖10 采用計數閾值和未采用計數閾值切換對比

6 結語

本文開發設計了一種基于閾值機制和多傳感器定位精度的室內外無縫定位切換算法，通過室內和室外兩種定位系統實時精度來分配置信度從而提高室內外中間區域定位精度并保證定位軌跡平滑，同時采用計數閾值的方法避免反復切換造成乒乓效應，增加了系統穩定性。搭建實驗平臺完成對該方法的驗證，實現了室內外定位的無縫切換，完成了同一坐標系下室內外的全局定位。室內外無縫切換定位在巡檢機器人、消防機器人上都有廣闊的應用前景。