基于地圖匹配輔助的多慣導陣列的室內定位方法

范亞州， 管 啟， 丁德銳

（上海理工大學 光電信息與計算機工程學院， 上海200093）

0 引 言

隨著互聯網的快速發(fā)展，移動機器人在服務行業(yè)的應用得到了廣泛關注。 例如，利用位置信息提供導航、跟蹤等。 移動機器人的應用服務研究主要包含兩個方面：基于室外的位置信息應用服務和基于室內的位置信息應用服務。 其中基于室外的定位技術已經日趨成熟，諸如美國的GPS 定位系統、伽利略定位系統、中國的北斗定位系統等衛(wèi)星定位導航系統。 然而在室內環(huán)境中，由于室內的復雜環(huán)境，使得衛(wèi)星定位導航系統的信號衰減嚴重，無法保證與室內的相關定位設備直接通信。 因此，以GPS 系統為代表的衛(wèi)星定位導航系統并不適用于室內的定位。 近年來，對于室內定位問題的研究主要集中在基于基礎輔助設施的定位系統和全自主定位系統。文獻［1］介紹了一種基于Wifi－Fingerprint 的室內定位方法，這種方法不僅需要定位環(huán)境中有充足的AP，而且還需要人工采集大量的訓練數據，并需要建立指紋數據庫。 此外，還有些室內定位方法采用的是基于藍牙、超聲波、紅外線、UWB、ZigBee 等技術［2－4］。 上述的定位手段主要是借助于一些基礎設施，其優(yōu)點是定位精度較高，缺點是不僅成本較高，而且可移植性非常低。 如果定位環(huán)境缺乏相關基礎設施，則無法實現定位。 所以這些并不是最佳的移動機器人室內定位措施。

微電子機械系統（MEMS）技術的快速發(fā)展，使得完全依據自身定位設備的全自主定位系統成為可能。 全自主定位系統最常用的方法是使用慣性測量單元進行自主的導航定位。 基于慣性導航定位的技術原理是：通過一個起始點，移動機器人的前進方向、速度等信息，進而推算出當前的位置信息［5］。文獻［6］中介紹了基于慣性測量單元的定位技術，并比較了多種可用于慣性測量單元的傳感器節(jié)點的優(yōu)缺點。 文獻［7］介紹了GPS 定位和慣性測量單元相結合定位系統。 雖然基于慣性設備的定位方法不需要安裝基礎設施，但是這類定位方法具有累積誤差和漂移誤差的缺點，而且依靠單一慣性傳感器定位的方法易受信號強度影響。 文獻［8－10］以集成式的方式描述了低成本多慣性傳感器系統平臺，該系統平臺提高了單個慣性傳感器的測量精度，增大了慣性傳感器的測量范圍，并且分析了多慣性傳感器陣列相比單個慣性傳感器的優(yōu)點。 根據上述分析，本文在慣性傳感器陣列的數據融合室內定位算法的基礎上，考慮到室內的地圖信息，利用解算的Multi－IMUs－Array信息和地圖信息約束，來提高定位精度。

1 慣性傳感器陣列的校正

理想情況下，慣性測量單元（IMU）的三個靈敏軸是正交的。 但由于IMU 在制造過程中結構的不精確，導致加速度計和陀螺儀的三個靈敏軸往往是非正交的。 因此，在融合來自IMU 陣列的信息之前，需要對慣性傳感器陣列進行校正。 此外，在校正的過程中不僅要考慮單慣性傳感器的比例因子誤差、靈敏度軸的非正交性、偏置和噪聲，還要考慮在集成制造過程中，每個慣性測量單元之間由旋轉偏置造成的對齊誤差。

1.1 慣性傳感器陣列硬件

利用單片機和多個低成本的IMU，構建一個IMU 陣 列。 該 設 備 包 括 32 個 IMU 和 一 個AVR32UC3C2512Μc 芯片，如圖1、2 所示。

圖1 嵌入式MIMU 平臺正面Fig.1 Embedded MIMU platform facade

圖2 嵌入式MIMU 平臺反面Fig.2 The reverse of the embedded MIMU platform facade

其中，正面安裝16 個IMU，背面安裝16 個。 每一個IMU 包含三個正交加速度計、三個陀螺儀和三個磁力計。

1.2 慣性傳感器陣列的校正理論

由于陣列中IMU 安裝的不完善，不同IMU 的坐標軸不會完全對齊。 為了對這些偏差進行建模，可通過參考單個慣性傳感器的加速度計的校正模型，將其校正方法引入到多慣導陣列中的校正中。 假設，慣性傳感器陣列中第j 個IMU 加速度計三元組在方向n 處，輸出可由加速度校正模型［11］得到：

其中：K（i）＝diag（k（i）） 表示第i 個IMU 的刻度系數矩陣；L（i）表示從第i 個IMU 所在實際平臺坐標系，轉換到理想平臺坐標系的旋轉矩陣；表示第j 個IMU 在方向n 處的輸出測量值；參數k（i）＝?3，分別表示傳感器增益、靈敏度軸的非正交性、偏置和噪聲；∈?3為方向n 處施加于第i 個加速度計三元組上的真實比例；∈SO（3） 表示第i個IMU 的理想平臺坐標系旋轉到第j 個IMU 理想平臺坐標系的旋轉矩陣； ξ（j）表示第j 個IMU 所處實際平臺坐標系與理想平臺坐標系的對齊誤差， 且很?。唬郐危╦）］×表示對齊誤差的反對稱矩陣。 進而，分別表 示IMU 三個 軸的 刻 度 系數，分別表示第i 個IMU 的安裝誤差角，i ＝1，2，…M， j ＝1，2，…M， n ＝1，2，…N。 N 和M 分別為方向數和IMU 的編號數。

在公式（1）中，θ（j）為需要校正的參數，所包含的參數項表示形式如下：

為了估計參數θ（j）， 輸入向量在球坐標系下重新寫為：

其中，φn、ψn分別為第i 個IMU 的俯仰角和滾轉角。 參數的極大似然估計值以及俯仰角和滾轉角初始估計值的計算可參考文獻［12］。

1.3 校正方法

圖3 矯正裝置Fig.3 Rectifying device

圖4 矯正效果圖Fig.4 A rendering of the correction

2 加速度計測量、模型建立及定位

2.1 加速度計模型的建立

慣性傳感器的輸出通常會受到測量噪聲、確定性偏差和偏差漂移等確定性和隨機性誤差的影響。一般加速度模型可以定義為［13］：

其中，y 是測量加速度值， a 是真正的加速度值，bd是確定性偏置誤差，bv是隨機偏差漂移，v 是導致速率隨機游走（VRW）誤差的測量白噪聲。 速率隨機游走誤差是由傳感器測得的速率信號經積分后，在短時間內檢測到的高頻噪聲。 偏置漂移項也是傳感器工作時的主要誤差。 本文考慮在較短的時間內可以忽略偏置漂移誤差。 因此，式（7）可以簡化為：

進而，通過考慮傳感器的動態(tài)行為，隨機游走過程是加速度計的一個典型模型，如下所示：

根據式（7）－（9），可得單個加速度計狀態(tài)空間模型表達式為：

其中wk為過程噪聲，vk為測量噪聲。 測量和過程噪聲協方差矩陣分別具有以下特征：

2.2 融合算法

由于使用的是多個性能相同的慣性傳感器，因此在實際應用，忽略傳感器數據傳送過程中時間戳的影響。 本文采用最小均方誤差（MMSE）融合，利用最小均方誤差準則將測量向量、測量矩陣和測量噪聲協方差矩陣結合起來。 狀態(tài)空間模型中的測量矩陣包含12 個向量，通過使用最小均方誤差，可以將測量矩陣的維數降低4 倍。 融合后的結果通過數據加權，使得所有傳感器得到的測量向量和測量矩陣組合如下［14］：

其中，Rt、Zt、Ht分別為t 時刻的等效測量噪聲協方差矩陣、測量向量矩陣和測量矩陣。 Ns為慣性傳感器測量單元的個數， yi為第i 個慣性傳感器測量單元的輸出值，Ri為第i 個慣性傳感器測量單元的測量噪聲協方差矩陣。 通過公式（12）的狀態(tài)空間參數，三軸加速度估計值變化如下：

2.3 使用加速度計信息實現定位

通過對慣性傳感器陣列加速度數據的采集，并利用對加速度進行二重積分獲得移動機器人的位置信息［15］。 為了減少積分誤差及損失，采用梯形法得到采樣信號的一階近似。 積分區(qū)域可由兩塊小的區(qū)域Area1 和Area2 組合而成，如圖5 及式（14）所示。為了獲得連貫值，采樣時間必須相同。 采樣時間由代表這塊區(qū)域的寬來表示，同時區(qū)域的高代表采樣得到的值。 為了消除帶有分數的乘法，假定時間為一個單位。 另外，實驗數據的仿真是在線下進行的。

圖5 梯形法Fig.5 Trapezoidal method

3 地圖信息輔助定位

3.1 地圖匹配定義

地圖匹配技術（Map－Matching）也稱地圖輔助技術。 是一種通過算法利用地圖信息限制定位誤差，將計算得到的點坐標進行信息修正，并且呈現在數字地圖上的方法。 采用地圖匹配技術不僅提供了移動裝置位置信息在地圖上實時顯示的方法，而且可以提高移動裝置的定位精度。

大多數情況下，采用傳感器獲得的移動位置信息并沒有落在道路上，所以還需要由一定的地圖輔助算法修正計算，得到定位點在道路上的匹配點，用該匹配點來代替不合適的定位點作為最終移動設備的估計位置。 定位點以及軌跡修正示意如圖6 所示。

圖6 地圖信息輔助以及位置修正示意圖Fig.6 Map information assistance and location correction diagram

3.2 基于位置點的地圖匹配

基于位置點的地圖匹配算法的基本原理是：將解算的位置信息坐標點向附近道路作投影，投影后的坐標點作為最終修正后的位置信息。 本文以地圖路徑的中線為參考路徑，采用垂直投影的修正定位方法來修正定位信息。 定位修正流程如圖7 所示。

圖7 定位以及定位修正流程圖Fig.7 Location and location correction flow chart

4 實驗結果與分析

4.1 仿真實驗參數設定

為了驗證本文提出的基于Multi－IMUs－Array與地圖信息融合的室內定位算法，評估該算法在驗證累積誤差上的有效性，采用Matlab 軟件進行仿真實驗，來分析算法性能的優(yōu)劣。 本實驗相關仿真環(huán)境及參數設置如下：假設移動機器人的運行軌跡為30 m×30 m 的回字型環(huán)境，起點和終點坐標為（5，5），采樣時間設為0.2 s，共分四個階段（對應軌跡的四個邊），每個階段分為勻加速，勻速和勻減速三個階段。 本文采用平均定位誤差作為定位性能的測量標準。 定位誤差是指實際位置和估計位置之間的歐幾里得距離。 為了說明該算法的有效性，從平均誤差的角度比較了地圖信息輔助下的Multi－IMUs 位置信息、Multi－IMUs 位置信息以及Single－IMU 位置信息。 實驗平均定位誤差計算公式如下：

4.2 實驗結果分析

實驗分別構建了不同傳感器信息下的軌跡信息。 在Single－IMU、Multi－IMUs－Array 以及地圖信息約束下，Multi－IMUs－Array 的軌跡以及平均定位誤差如圖8 和表1 所示。

圖8 地圖信息下多慣導陣列以及單個慣導位置解算軌跡Fig.8 The trajectory of multiple inertial navigation array and single inertial navigation position is calculated under map information

表1 本文方法和其他方法的平均定位誤差比較Tab.1 Comparison of average positioning error between the method in this paper and other methods

通過仿真結果可以看出，多慣導傳感器陣列結合地圖信息的軌跡更加接近真實軌跡，其它兩種方法均會產生較大的漂移誤差。 而單純使用慣性傳感器定位時，產生的漂移誤差更大。 從圖6 可以看出，單純使用慣性傳感器定位算法所產生的最大漂移誤差達到了13.57 m 左右；多慣導傳感器陣列進行定位時所產生的最大漂移誤差達到了6.75 m 左右。因此可以看出，通過多慣導傳感器陣列的數據融合，在一定程度上減小了定位誤差。 此外，結合所提出的室內地圖信息輔助多慣導傳感器陣列的室內定位算法，將定位的平均誤差降低到了1 m 左右，且修正后的軌跡得到了較為滿意的結果。 實驗的結果表明，所提出的基于地圖信息輔助的慣導陣列的多傳感器數據融合的定位算法，一方面可以平滑軌跡，另一方面，可以降低累積誤差，有效地提高了整體定位精度。

5 結束語

針對單個慣性傳感器定位方法存在的漂移和累積誤差問題，本文提出了一種基于地圖匹配方法輔助，以陣列形式排列的慣性傳感器數據融合的室內定位算法。 仿真實驗結果表明，與單一傳感器的定位算法相比較，本文提出的方法具有較高的定位精度。 并且在一定范圍內改善了單個慣性傳感器定位的漂移效應，修正了移動軌跡，且有較強的魯棒性，提高了整個定位系統的有效性。 實驗表明，多種數據信息和多傳感器之間的信息融合將成為今后的研究方向和重點。