基于地圖匹配輔助的多慣導(dǎo)陣列的室內(nèi)定位方法

范亞州， 管 啟， 丁德銳

（上海理工大學(xué) 光電信息與計(jì)算機(jī)工程學(xué)院， 上海200093）

0 引 言

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

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

1 慣性傳感器陣列的校正

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

1.1 慣性傳感器陣列硬件

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

圖1 嵌入式MIMU 平臺(tái)正面Fig.1 Embedded MIMU platform facade

圖2 嵌入式MIMU 平臺(tái)反面Fig.2 The reverse of the embedded MIMU platform facade

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

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

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

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

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

為了估計(jì)參數(shù)θ（j）， 輸入向量在球坐標(biāo)系下重新寫為：

其中，φn、ψn分別為第i 個(gè)IMU 的俯仰角和滾轉(zhuǎn)角。 參數(shù)的極大似然估計(jì)值以及俯仰角和滾轉(zhuǎn)角初始估計(jì)值的計(jì)算可參考文獻(xiàn)［12］。

1.3 校正方法

圖3 矯正裝置Fig.3 Rectifying device

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

2 加速度計(jì)測(cè)量、模型建立及定位

2.1 加速度計(jì)模型的建立

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

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

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

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

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

2.2 融合算法

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

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

2.3 使用加速度計(jì)信息實(shí)現(xiàn)定位

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

圖5 梯形法Fig.5 Trapezoidal method

3 地圖信息輔助定位

3.1 地圖匹配定義

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

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

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

3.2 基于位置點(diǎn)的地圖匹配

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

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

4 實(shí)驗(yàn)結(jié)果與分析

4.1 仿真實(shí)驗(yàn)參數(shù)設(shè)定

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

4.2 實(shí)驗(yàn)結(jié)果分析

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

圖8 地圖信息下多慣導(dǎo)陣列以及單個(gè)慣導(dǎo)位置解算軌跡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

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

5 結(jié)束語

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