基于MEMS IMU和運動約束的共享單車定位算法

牛小驥, 丁龍陽, 曠 儉, 吳宜斌

(1. 武漢大學 衛星導航定位技術研究中心，武漢 430079；2. 武漢大學 測繪學院，武漢 430079）

共享單車作為一種新型綠色環保的出行工具，近些年已經深入到人們的日常生活中。高精度高可靠性的單車定位系統對于提升用戶體驗，減少運營成本至關重要。目前共享單車所依靠的主流定位手段是全球衛星導航系統（Global Navigation Satellite System, GNSS），但是GNSS 在城市峽谷、隧道等場景中，因為信號遮擋和多路徑等因素的影響，定位精度會迅速下降甚至完全失效。因此亟需一種價格低廉、連續可靠、易于推廣的單車定位方案用于彌補當前GNSS 定位的不足。

微機電系統（Micro-Electro-Mechanical System, MEMS）慣性測量單元（Inertial Measurement Unit, IMU）因為尺寸小巧、成本低廉且不受外界環境干擾，被廣泛應用在各種要求連續可靠的定位場景中。但是基于MEMS IMU 的慣性導航系統（Inertial Navigation System, INS）因為傳感器噪聲和零偏誤差等影響，定位誤差會隨著時間迅速積累[1]，很快就超出共享單車定位誤差的容許范圍，因此需要尋求輔助信息來抑制其誤差發散。

對于地面輪式載體，里程計（Odometer, ODO）是一類重要的輔助傳感器[2]。車載導航中常采用里程計提供的速度或距離信息與INS 組合（ODO/INS）作為GNSS 定位的補充，在GNSS 失效的環境下提供穩定連續的定位結果[3-5]。但ODO/INS 組合方案需要增加額外的車輪編碼器，增加了成本和系統復雜度，無法滿足目前共享單車定位系統對于低成本以及可推廣性的迫切需求。

地面輪式載體一般滿足非完整性約束（Non-Holonomic Constraints, NHC），即在不打滑、不騰空的情況下，載體橫向和垂向的速度為零[2]。該運動約束作為地面輪式載體的固有特性，也常用作輔助信息抑制INS 誤差的累積[6]。文獻[7]將NHC 和里程計結合使用用于車載導航，有效抑制了INS 誤差的發散，提高了系統穩定性。文獻[8][9]將IMU 安裝在車輪中心與車輪一起旋轉，通過陀螺測量值與車輪半徑得到載體運動速度，并結合NHC 構造三維速度信息。

上述有關非完整性約束用于導航定位的研究主要集中在車載導航或輪式機器人導航，用于自行車這類兩輪載體的較少，主要是顧慮于騎行過程中的左右晃動對NHC 的破壞。文獻[10]基于騎行者的運動模型設計了一套用在騎行者所攜帶設備上的航位推算（Cycling Dead Reckoning, CDR）算法，但也無法解決目前共享單車定位存在的問題。針對上述問題，本文設計并實現了一套融合INS 和非完整性約束的單車定位算法（NHC/INS），并著重探討了安裝角誤差和桿臂補償對NHC 使用效果的影響。

1 單車定位算法

1.1 算法框架

圖1 展示了本文設計的共享單車定位算法流程：首先通過慣導機械編排獲得IMU 的位置、速度和姿態。然后將GNSS 解算位置、INS 推算結果以及NHC約束信息通過擴展卡爾曼濾波（Extended Kalman Filter, EKF）進行數據融合得到修正后的載體位姿。在GNSS 信號中斷時，僅利用NHC 修正INS 誤差（即NHC/INS 方案）。

圖1 單車定位算法框架Fig.1 Architecture of the proposed bicycle positioning algorithm

1.2 IMU 安裝方式與坐標系定義

本文算法涉及到的坐標系包括導航坐標系（n系），IMU 坐標系（b 系）和載體坐標系（v 系）。具體地，本文采用北-東-地（NED）軸線定義的當地水平坐標系作為導航坐標系，其原點選在載體上的某一參考點，其z 軸與該點大地參考橢球的法線重合并指向地心，x 軸與子午線重合并指向北極，三軸構成右手坐標系。IMU 坐標系的原點位于IMU 的測量中心，各個坐標軸分別平行于IMU 的三個敏感軸，構成右手坐標系。載體坐標系的坐標原點選在自行車后輪與地面的接觸點，x 軸沿后輪前進方向，y 軸垂直后輪平面指向載體右側，z 軸垂直向下，與x、y 軸共同構成右手坐標系（如圖2 所示）。

圖2 IMU 安裝方式與相關坐標系定義Fig.2 Installation scheme of the MEMS IMU and the definitions of the vehicle frame(v), IMU body frame(b) and navigation frame(n)

本文將IMU 安裝在自行車后座上。慣導機械編排推算得到IMU 自己的導航狀態，而NHC 約束的是載體坐標系下的載體速度，因此融合NHC 和INS 需要已知兩個坐標系（IMU 坐標系和載體坐標系）之間的相對關系。而在實際系統中，不可避免地會存在安裝誤差，包括安裝角和桿臂。安裝角指的是IMU 坐標系（b 系）與載體坐標系（v 系）因軸系不平行所引起的差異；桿臂在這里指的是IMU 測量中心與載體速度投影中心之間的相對位置關系。值得注意的是，自行車不同于四輪載體，在騎行過程中為保持平衡而難免發生左右晃動，因此NHC 在自行車車身處并不嚴格成立，但在車輪觸地點卻是成立的。因此本文選擇后車輪觸地點作為載體速度投影中心來融合NHC 和INS，并定義桿臂為IMU 測量中心到后車輪觸地點的向量。IMU 安裝位置以及安裝角和觸地點桿臂的定義如圖2所示。

1.3 系統誤差狀態模型

帶有輔助信息的慣性導航常采用誤差狀態卡爾曼濾波（間接卡爾曼濾波）進行組合導航解算，以解決系統的非線性問題[11]。本算法采用21 維誤差狀態向量，包括導航狀態向量與IMU 誤差向量，即：

陀螺和加速度計零偏以及比例因子均建模為一階高斯-馬爾可夫過程[2]：

其中 Tgb、 Tab、 Tgs、 Tas分別為陀螺零偏、加速度計零偏、陀螺比例因子以及加速度計比例因子的相關時間，wgb、wab、wgs、was為對應的驅動白噪聲。

1.4 車輪觸地點零速約束模型

對于一般輪式載體，如汽車或者輪式機器人，當其正常行駛時（不發生側滑和跳躍）只有沿前進方向的速度，側向和垂向速度為零，這一基本假設被稱為非完整性約束（NHC）。但不同于汽車或輪式機器人，自行車騎行過程中為了保持平衡而不得不發生或大或小的左右晃動，因此NHC 在自行車車身處并不總是成立。但對于車輪著地點而言，只要車輪不發生側滑或垂向跳躍，該點處的非完整性約束是嚴格成立的。因此本文將IMU 測量中心的速度通過安裝角和桿臂投影到后輪著地點處，利用其側向和垂向速度為零進行約束。在載體坐標系下，當非完整性約束成立時，速度觀測向量可以表示為：

vv表示載體沿前進方向的速度； ev表示觀測噪聲，其方差參數可以根據載體的運動狀態初步給定，并通過殘差進行調整。由IMU 推算得到的速度投影到自行車后輪觸地點可以表示為：

本算法只對側向速度和垂向速度進行約束，因此上述觀測方程只取側向與垂向分量。

2 實驗描述

為驗證本文所設計的NHC/INS 組合導航算法的定位精度，并與傳統INS 方案進行對比，本文在開闊的馬路上進行了實測驗證。實驗設備及安裝情況如圖3 所示。本文采用了武漢大學衛星導航定位技術研究中心自主研發的GNSS/INS 組合導航模塊INS-Probe，并安裝了HX-CSX601A 測量型天線用于收集GNSS數據。INS-Probe內置的MEMS IMU型號為InvenSense公司的ICM20602，主要技術參數如表1 所示。INS-Probe 被安裝在車輪后座并使其盡可能與車輪平行。車輪觸地點桿臂通過卷尺多次測量取平均值得到，安裝角采用文獻[12]提出的方法估計得到。實驗數據的處理方式均為實時采集模式下的事后處理。

表1 被測MEMS IMU(ICM20602)技術參數Tab.1 Technical parameters of the MEMS IMU (ICM20602) used in the tests

圖3 實驗數據采集設備Fig.3 Experimental platform.

為比較不同騎行動態下安裝角誤差和桿臂補償給NHC/INS 定位性能帶來的影響，本文進行了兩組實驗。一組實驗為正常騎行，自行車左右晃動幅度較小；另一組實驗為“S 形”騎行，自行車左右晃動幅度較大。兩組實驗的自行車傾角變化如圖4 所示。實驗1 自行車平均速度為2.2 m/s，實驗2 自行車平均速度為2.9 m/s，兩組實驗騎行時間均在30 min 左右。

圖4 自行車傾角變化Fig.4 Lean angle of the bicycle in the two tests

本文實驗的位置參考真值采用事后動態差分（Post Processed Kinematic, PPK）與INS 松組合的結果，其中PPK 全部為固定解，參考真值精度可達厘米級。為了評估本文設計的NHC/INS 共享單車定位算法的性能，本文采用模擬GNSS 中斷的方式進行實驗分析。通過融合GNSS 定位信息，在MEMS IMU 誤差充分收斂后，周期性地模擬GNSS 數據中斷60 s，每次中斷之間恢復GNSS 數據60 s 使慣導誤差重新收斂。每組實驗共中斷10 次，并統計每次中斷定位誤差最大值的RMS 值作為統計結果。對比實驗結果分析具體分為以下三個方面：

1) 對比純慣導推算與加入NHC 后的位置誤差漂移；

2) 對比不同騎行動態下安裝角誤差對本文NHC/INS 算法性能的影響；

3) 對比不同騎行動態下桿臂補償對本文NHC/INS算法性能的影響。

3 實驗結果與分析

3.1 INS 方案與NHC/INS 方案對比

圖5 為實驗1（正常騎行）在10 次模擬中斷GNSS信號60 s 條件下得到的兩種方案的定位軌跡對比圖。統計正常騎行時（實驗1）每次中斷誤差漂移最大值的RMS 值作為位置誤差，結果如表2 所示。此時對IMU 安裝角誤差和桿臂都進行了補償。

表2 正常騎行時GNSS 中斷60 s 位置誤差統計（RMS 值）Tab.2 Position error statistics (RMS) during 60 s GNSS outages in test 1

圖5 模擬10 次GNSS 中斷60s 條件下定位軌跡對比。其中紅色表示參考真值軌跡，藍色表示INS 方案推算軌跡，綠色表示本文NHC/INS 方案推算軌跡。Fig.5 Estimated trajectories of different schemes in the 10 sets of 60s GNSS outage experiments. The red represents the ground truth; the blue represents the trajectory estimated by INS; the green represents the trajectory estimated by the NHC/INS scheme proposed in this paper

結果顯示，純慣導推算在GNSS 中斷條件下，由于沒有任何的約束信息，誤差發散很快，在1 min 后其水平方向和垂直方向的位置漂移分別達到了20 m和4 m 左右。當正確加入NHC 約束修正后，三個方向的定位精度分別提升了91%、95%和83%。此外，在NHC/INS 方案中，北向位置誤差發散相較于東向和垂向更大，這是因為NHC 無法對前向速度進行約束，而本文實驗軌跡的主要前進方向為南北方向。

為進一步評估NHC/INS 方案在極端情況下的定位性能，對實驗1（正常騎行）模擬5 次GNSS 中斷5 min 的場景，每次中斷相互獨立，統計5 次中斷下誤差漂移最大值的RMS 值，繪制誤差漂移曲線如圖6(b)所示。圖6(a)為純INS 方案對應的位置誤差漂移曲線。

圖6 位置誤差漂移曲線Fig.6 Position drift errors of the two navigation schemes.

根據圖6 可以看到在中斷GNSS 信號5 min 的苛刻條件下，INS 方案的水平位置誤差統計值會達到千米量級；而當正確加入NHC 后，北向和東向定位誤差統計值只有4.0 m 和2.5 m。此外，NHC/INS 方案的誤差曲線中北向和東向誤差并不是一直發散，而是發散與持平相間出現。這是因為本文實驗軌跡中存在周期性的掉頭，而慣導位置誤差的一些影響因素（如航向誤差）是與此相關的，這樣會導致水平方向的誤差漂移方向被逆轉，誤差不再沿一個方向持續累積。同時，曲線統計的是GNSS 中斷期間誤差漂移最大值的RMS 值，故會出現一段局部持平的狀態。此外，由于高程方向的這種軌跡方向變化帶來誤差漂移方向明顯逆轉的作用不強，故而呈現出持續發散的趨勢。

在實際GNSS 信號遮擋嚴重的環境中，只依賴MEMS IMU推算所帶來的幾百米甚至超過1公里的定位誤差對于共享單車的管理而言顯然是難以接受的。而當正確加入NHC 后（即有效補償IMU 安裝角和桿臂影響后），不需要增加任何額外的硬件成本便可以保證較高的定位精度，從而為共享單車在復雜場景下的定位難題提供了一套可行的解決方案。下文通過實驗數據的對比處理來探討在不同騎行動態下IMU 安裝角誤差與桿臂補償對NHC 修正效果的影響。

3.2 安裝角誤差對NHC/INS 定位性能影響

本文實驗采用文獻[12]提出的方法估計得到的航向安裝角為-0.649 °，俯仰安裝角為-1.449 °（橫滾安裝角不可估，對于NHC 也不需要）。表3 展示了兩組實驗中，補償與不補償安裝角的條件下NHC/INS 定位算法在模擬中斷GNSS 信號60 s 條件下的位置漂移誤差的統計結果。圖7(a)-(d)為兩組實驗在補償安裝角前后，車輪觸地點在載體坐標系下的速度投影圖。

表3 IMU 安裝角補償對自行車NHC/INS 定位性能影響Tab.3 Effect of mounting angles compensation on the performance of NHC/INS

結果顯示兩組實驗在補償安裝角前后其平面方向的位置精度差異不大，這是因為在安裝角中影響水平位置精度的主要是航向軸，而本實驗中航向安裝角較小(-0.649°)，所以從統計結果來看，補償安裝角前后對水平方向的定位精度影響不大，補償前后在同一個數量級。但是，經安裝角補償后，高程方向精度提升明顯，其中實驗1 的垂向誤差從3.92 m 減小至0.73 m，實驗2 的垂向誤差從5.07 m 減小至0.81 m。這是因為在將IMU 速度從b 系投影到v 系的過程中，俯仰安裝角主要影響的是垂直方向的速度。如圖7 所示，補償安裝角之后，兩組實驗在載體坐標系（v 系）中的垂向速度投影均明顯減小，更嚴格地符合NHC 約束。

圖7 安裝角補償前后的速度投影Fig.7 Vehicle velocity in the vehicle frame before and after mounting angles compensation

具體可解釋為，在只考慮俯仰安裝角時，從b 系轉換到v 系的方向余弦矩陣Cvb可以表示為：

由于只有前向速度 vx較大，故俯仰安裝角對補償前后的垂向速度影響更大。

3.3 桿臂補償對NHC/INS 定位性能影響

本文實驗對IMU 桿臂進行多次測量得到的平均值為（-0.053 m，0.001 m，0.730 m）。表4 展示了兩組實驗（正常騎行和“S”形騎行）中，補償與不補償桿臂的條件下NHC/INS 定位算法在中斷GNSS 信號60s條件下的位置誤差統計結果。圖8(c)與圖8(d)分別為兩組實驗在不進行桿臂補償時，車輪觸地點在載體坐標系下的速度投影。

表4 桿臂補償對自行車NHC 性能的影響Tab.4 Effect of lever-arm compensation on the performance of NHC/INS

從表格數據可以看出，補償桿臂前后垂向誤差變化不大；東向位置精度在補償桿臂后提升較大；而北向位置精度提升非常顯著。同時從結果可以看出，桿臂補償對于實驗2 的影響要明顯大于實驗1。這是因為不進行桿臂補償時，NHC 約束的是車輪后座IMU處的速度，而實驗2 自行車的左右晃動幅度更大，此時桿臂對速度投影的精度和穩定性影響更大（如圖8(d)）。從圖8(c)-(d)中可以看到，實驗2 不補償桿臂時，側向速度投影明顯增大。因此這種情況（自行車動態較大）下，桿臂補償帶來的性能提升更為明顯。

圖8 桿臂補償前后的速度投影Fig.8 Vehicle velocity in the vehicle frame before and after lever-arm compensation

此外，從統計結果來看，南北方向作為自行車軌跡的主要前進方向，其受桿臂補償的影響要明顯大于東向和垂向，這與文獻[13]的實驗結果是一致的。這是因為當不進行桿臂補償的時候，NHC 約束的是自行車后座的速度，該處的垂向和側向速度實際上并不為零，然而NHC 卻以較高的置信度認為該點垂向和側向速度為零，那么算法就會將這一矛盾不恰當地歸咎到前向上，從而造成前向位置誤差增大，在本文實驗中則主要表現為南北方向的位置誤差。

4 結 論

本文針對當前共享單車在城市復雜場景下難以實現連續精準定位的問題，設計并實現了一套基于MEMS IMU 與車輪觸地點非完整性約束（NHC）的定位算法。相比于傳統純慣導推算（INS 方案），本文所設計的NHC/INS 融合定位方案在GNSS 中斷60s 條件下，“北-東-地”三個方向的定位精度分別提升了91%、95%和83%。在模擬GNSS 中斷5 min 的極端情況下，NHC/INS 方案在“北-東-地”三個方向的定位誤差最大值的RMS 統計值也僅為4.0 m、2.5 m 和4.0 m，可以滿足共享單車日常管理對定位精度的需求。此外，本文還重點探討了IMU 安裝角誤差與桿臂補償對自行車NHC 修正性能的影響。定量對比結果表明，忽略安裝角或桿臂補償都會嚴重影響NHC 約束作用的發揮，顯著降低定位精度。

本算法未對自行車前向速度進行約束，因此后續可以考慮以下研究方向：一是根據自行車本身的平衡特性構建約束信息，進一步抑制前向定位誤差的發散；二是考慮將IMU 安裝在車輪上與之一同旋轉，起到里程計的作用，從而約束前向速度誤差，進一步提高定位精度和可靠性。