趙小明，鄧芳瑾，楊松普，李 巍

（天津航海儀器研究所，天津 300131）

全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System, GNSS）因其可以為陸、海、空三大領(lǐng)域提供實時、全天候和全球性的導(dǎo)航服務(wù)，已廣泛應(yīng)用于軍事及民用等各個領(lǐng)域，給人們的生活提供了極大的便利。然而在室內(nèi)、高樓密集的城區(qū)道路或地下、叢林等類似的環(huán)境中，由于高樓、建筑材料的阻擋或是缺少接收器，GNSS信號將接收不到或接收很差，導(dǎo)致無法進行有效的定位與導(dǎo)航。因此，需要研發(fā)出能在室內(nèi)運行、自主、可靠的定位系統(tǒng)，即室內(nèi)定位系統(tǒng)。MIMU是一種新興的且發(fā)展非常迅速的微型慣性傳感器，具有成本低、體積小、功耗低及環(huán)境適應(yīng)能力強等優(yōu)點，被普遍應(yīng)用于室內(nèi)定位系統(tǒng)的研制。與其它慣性導(dǎo)航定位設(shè)備一樣，MIMU具有很好的隱蔽性和抗干擾性，不依賴于任何外部信息，也不向外輻射能量，能夠在空中、地面以及水下完成自主導(dǎo)航定位的功能，但同時也具有一些弊端，最明顯的就是導(dǎo)航誤差隨時間積累問題，因而長時間工作后會產(chǎn)生不同程度的積累誤差，此誤差可以通過與一個或多個輔助傳感器組合來使它達到有界，提高系統(tǒng)性能。相對來講，零速修正技術(shù)是進行誤差控制的一種簡單而且有效的手段，其利用載體停止時慣性導(dǎo)航系統(tǒng)的速度輸出作為系統(tǒng)速度誤差的觀測量，進而對其他各項誤差進行修正。零速修正算法主要分為兩個步驟，即零速檢測和零速更新。目前，零速修正技術(shù)在車載以及個人導(dǎo)航等領(lǐng)域都得到了廣泛應(yīng)用，而零速檢測算法的具體運用也不盡相同，需要根據(jù)實際載體、環(huán)境等條件選擇使用不同的檢測算法。

傳統(tǒng)的行人室內(nèi)定位系統(tǒng)如文獻[1-3]綜合MIMU中加速度計和陀螺儀的輸出信息進行零速檢測，在步行狀態(tài)下取得較好定位效果。當跑動時，單純依靠慣性信息進行零速檢測，修正效果較差，若要擴大零速檢測算法的適用性，還需引入其他運動參數(shù)。文獻[4]中利用光學檢測裝置和壓力傳感器檢測步行時不同壓力區(qū)域質(zhì)心的速度，當速度小于閾值時視為零速階段，得到較好的效果，但其在MIMU和壓力傳感器外還需要配置光學檢測裝置，增加了系統(tǒng)的復(fù)雜性和成本。本文在傳統(tǒng)捷聯(lián)慣性導(dǎo)航基礎(chǔ)上引進了零速修正技術(shù)，通過對步態(tài)特征的分析，設(shè)計了一種壓力傳感器輔助的多條件約束零速檢測方法，采用低成本的薄膜式壓力傳感器輔助MIMU對零速區(qū)間進行判斷，提高了行走時零速檢測的魯棒性。通過基于零速修正的卡爾曼濾波估計姿態(tài)誤差、速度誤差和位置誤差，校正反饋后提高了系統(tǒng)的導(dǎo)航精度。

1 系統(tǒng)組成

典型的行人室內(nèi)定位系統(tǒng)由三部分組成：微慣性測量單元、顯示控制終端和指揮控制終端。MIMU將檢測到的位置、姿態(tài)等信息發(fā)送到手持控制終端上，通過無線通訊技術(shù)將信息發(fā)送到指揮控制終端，控制端人員基于在指揮控制終端上的信息向現(xiàn)場人員發(fā)送指令。

圖1 行人室內(nèi)定位系統(tǒng)的原理方案框圖Fig.1 Principle diagram of indoor positioning system

本文中設(shè)計的行人室內(nèi)定位系統(tǒng)基于經(jīng)典捷聯(lián)慣性導(dǎo)航解算方法，對慣性器件的輸出信息進行數(shù)值積分求解導(dǎo)航參數(shù)。慣性導(dǎo)航子單元由小型化、重量輕以及低功耗的MEMS陀螺、加速度計組成，壓力檢測子單元則選用適合穿戴的薄膜式壓力傳感器。行人室內(nèi)定位系統(tǒng)原理方案框圖如圖1所示。

2 零速修正

圖2所示為行走時的步伐周期。步伐周期從中間站立階段開始，然后進入腳尖離地階段，接著進入擺動階段，最后進入腳跟觸地階段，完成了一個步伐周期的行為運動。在腳跟觸地到腳尖離地的過程中，只有中間站立階段是我們要檢測的零速時間段。

圖2 行走時的步伐周期Fig.2 Stance phase in human bipedal locomotion

2.1 IMU檢測條件

步行時，固定在腳部的慣性元件敏感行走過程中加速度和角速度的變化，靜止時比力模值穩(wěn)定在重力附近，比力模值變化與變化幅度相較運動時明顯較小，人腳的角速度變化趨于零。由此，我們可以根據(jù)加速度計和陀螺的輸出來判斷零速區(qū)間。通過第i時刻加速度計輸出的比力信息陀螺儀輸出的角速度信息進行零速檢測。檢測算法包括三個條件：加速度模值、加速度滑動方差和角速度模值，分別記為Q1、Q2、Q3。各條件實現(xiàn)方式如下：

2）比力滑動方差檢測，即當滑動窗口大小為N時比力方差小于等于給定閾值時，條件Q2(i)=1。

IMU檢測法為多條件復(fù)合檢測方法，即同時滿足以上三個條件時為零速區(qū)間：

2.2 壓力輔助檢測條件

當穿戴者跑動時，中間站立階段十分短暫，簡單對角速度或加速度設(shè)定閾值的方法很難將其識別出來。所以，我們引入了足底壓力作為零速檢測的另一個重要依據(jù)。

2.2.1 壓力傳感器的固定位置

壓力傳感器選擇電阻式薄膜壓力傳感器（Force Sensitive Resistor, FSR），該傳感器由高分子聚合物薄膜構(gòu)成，可以靜態(tài)和動態(tài)地測試兩個表面之間的力，具有纖薄、輕柔、小巧、靈敏度較高等特點，適合足底壓力的檢測。正常人平均動態(tài)足底峰值壓力為（2.96±0.66）kg/cm2，系統(tǒng)中采用的FSR圓形有效區(qū)域的直徑為12.7 mm，壓力量程為10 kg。隨著表面壓力的增加，F(xiàn)SR的電阻值減小。

跑動時腳接觸地面的時間一般只有幾十到幾百毫秒，中間站立階段則更短，僅僅依靠陀螺儀和加速度計的輸出判斷零速階段效果并不理想。但只要腳和地面接觸，就會受到壓力，我們檢測腳觸地階段腳底壓力模值對零速階段進行判斷，即當檢測區(qū)壓力達到預(yù)定閾值時視為零速階段。行走時前腳掌和后足跟處壓力信息特征較為明顯，且重復(fù)性與穩(wěn)定性較好，然而要將壓力傳感器判斷的零速區(qū)間和MIMU判斷的零速區(qū)間大致吻合需要將其放在同一位置，本系統(tǒng)將FSR固定在足底A處，MIMU固定在A處對應(yīng)的腳面上，如圖3所示。

圖3 FSR實物及安裝位置示意圖Fig.3 Force sensitive resistor and its location

2.2.2 壓力模值檢測

FSR的電阻值隨施加在其上的壓力變化而變化，并根據(jù)電阻材料的不同存在特定的非線性關(guān)系。本文采用的FSR傳感器感應(yīng)面受到外界施加的壓力時，電阻減小。壓力檢測模塊原理圖如圖 4所示，其中 R2為滑動變阻器，U1為比較器。比較器的功能是將模擬電壓信號與一個基準電壓相比較，其兩個輸入端為模擬信號，輸出則為二進制信號?；瑒幼冏杵?R5可分為兩部分Ra和Rb，當R1與RFSR的比值大于Ra和Rb的比值，即：

圖4 FSR零速檢測模塊原理圖Fig.4 Zero-velocity detector of pressure signal

時，比較器輸出端輸出一個低電平，此時視為零速階段。調(diào)節(jié)滑動變阻器阻值即可改變閾值。通過多次試驗測得零速檢測效果最佳時滑動變阻器阻值。

綜合壓力傳感器輔助慣性組件進行零速區(qū)間的檢測，將慣組檢測條件與壓力檢測條件進行―或‖運算：

應(yīng)用以上提出的零速檢測算法對采集的數(shù)據(jù)進行處理，零速區(qū)間檢測結(jié)果如圖5所示，其中：紅色實線為IMU檢測條件判斷的結(jié)果，藍色實線為壓力檢測條件判斷的結(jié)果，黑色實線為壓力輔助IMU檢測條件判斷的結(jié)果。從圖5可看出，IMU檢測條件判斷的零速區(qū)間與壓力檢測條件相比較小，這是由于閾值設(shè)定的不同所致。但根據(jù)實踐經(jīng)驗，通過改變閾值，在改變閾值加大IMU判斷的零速區(qū)間的同時也引入了更多的野值。壓力輔助IMU的檢測結(jié)果剔除了前兩種條件判斷的野值，準確性最高。

圖5 零速區(qū)間檢測結(jié)果Fig.5 Detection results of zero velocity intervals

2.3 零速更新

由于單兵定位系統(tǒng)是非線性的，利用擴展卡爾曼濾波對非線性系統(tǒng)中的待估參數(shù)X進行線性化，得到線性化的模型，然后再利用標準Kalman濾波進行估計。選取地理坐標系中姿態(tài)誤差角、速度誤差、位置誤差、陀螺常值漂移和加速度計常值漂移為狀態(tài)量取系統(tǒng)速度誤差δv為觀測量，建立15維Kalman濾波器：

式中：F是根據(jù)誤差模型和狀態(tài)矢量構(gòu)成的15×15維系統(tǒng)矩陣；W為15維系統(tǒng)隨機過程噪聲序列；V為三維系統(tǒng)隨機觀測噪聲序列；是3× 15維觀測矩陣。

在零速階段人腳的速度為零，因此可以將零速作為外參考速度，應(yīng)用Kalman濾波器對系統(tǒng)狀態(tài)進行估計。

2.4 半實物仿真驗證

為了驗證零速修正算法的有效性，通過Matlab對實測數(shù)據(jù)進行半實物仿真，觀察步行者行進方向的速度，即前向速度。圖6中顯示的為未經(jīng)修正和分別通過兩種方法修正后的前向速度。未經(jīng)零速修正時，速度發(fā)散，在行走45 s到50 s后就發(fā)散到-20 m/s；用IMU條件檢測零速區(qū)間并進行零速更新后，速度誤差大大減小，但峰值均超過4.5 m/s；改用壓力輔助IMU條件檢測零速區(qū)間并進行零速更新后，速度峰值下降到 4.5m/s以下，誤差進一步減小。未經(jīng)零速修正的前向速度發(fā)散，經(jīng)過零速修正后，速度誤差明顯減小，前向速度收斂，驗證了零速修正方法的有效性。

圖6 前向速度Fig.6 Forward velocity

3 試驗驗證

圖7為試驗中所用行人室內(nèi)定位導(dǎo)航系統(tǒng)實物圖，由手持終端和導(dǎo)航定位單元組成。導(dǎo)航定位單元又包含慣性導(dǎo)航子單元和壓力檢測子單元，其中：慣性導(dǎo)航子單元包含一個三軸陀螺儀和一個三軸加速度計，陀螺儀的零偏穩(wěn)定性為14.5 (°)/h，加速度計的零偏穩(wěn)定性為0.25 mg；壓力檢測子單元包含了一個單點式薄膜壓力傳感器。為了驗證零速修正算法在行人定位系統(tǒng)中的應(yīng)用效果，進行了矩形閉合曲線行走試驗，通過Matlab對實驗數(shù)據(jù)進行仿真，將分別用兩種檢測方法得到的結(jié)果進行對比，驗證壓力輔助MIMU零速修正方法的效果，最后在室內(nèi)環(huán)境下進行閉合曲線行走試驗，并在上位機實時顯示行走軌跡。

共進行兩組矩形行走試驗，行進距離為176 m。試驗1為正常步行試驗，試驗2為跑動試驗。每組試驗對MIMU和壓力輔助 MIMU兩種零速修正方法進行仿真，行進軌跡如圖8～11所示。

圖7 系統(tǒng)實物圖Fig.7 Picture of the system

從表1和表2的試驗結(jié)果可以看出，融合壓力信息的零速檢測算法適用范圍更廣，定位精度有顯著提升，試驗1（正常步行）的水平定位精度從0.56%D提升到0.19%D，試驗2（跑動）的水平定位精度從2.17%D提升到0.84%D。

表1 正常行走試驗結(jié)果Tab.1 Results of natural walking experiment

表2 跑動試驗結(jié)果Tab.2 Results of running experiment

在已知平面圖的室內(nèi)環(huán)境下進行閉合曲線行走試驗，并在上位機實時顯示行走軌跡，如圖12所示。起點和終點幾乎重合，行走軌跡符合實際情況，水平定位精度優(yōu)于1%D。

圖8 正常行走無壓力輔助軌跡圖Fig.8 Trajectory of natural walking without pressure information aiding

圖9 正常行走加壓力輔助軌跡圖Fig.9 Trajectory of natural walking with pressure information aiding

圖10 跑動下無壓力輔助軌跡圖Fig.10 Trajectory of running without pressure information aiding

圖11 跑動下加壓力輔助軌跡圖Fig.11 Trajectory of running with pressure information aiding

圖12 實時行走軌跡Fig.12 Real-time trajectory

4 結(jié) 論

本文提出了一種基于MIMU和壓力傳感器的多條件約束零速修正方法。通過比力模值、比力滑動方差、角速度模值和足底壓力模值，綜合檢測零速區(qū)間。該方法易于實現(xiàn)并且可以有效減小MEMS慣性元件的漂移誤差，相較單純依靠加速度和角速度的零速修正方法，其水平定位精度有顯著提高，正常步行和跑動下水平定位精度優(yōu)于1%D。由于條件所限本方法未在跳躍或非平坦路面等運動方式下進行試驗比較，定位精度還需進一步試驗驗證，以實現(xiàn)多種步態(tài)的導(dǎo)航定位。

參考文獻（References）：

[1] 田曉春, 陳家斌, 韓勇強, 等. 多條件約束的行人導(dǎo)航零速區(qū)間檢測算法[J]. 中國慣性技術(shù)學報, 2016, 24(1): 1-5.Tian X C, Chen J B, Han Y Q, et al. Zero-velocity interval detection algorithm with multi-condition constraint for pedestrian navigation[J]. Journal of Chinese Inertial Technology, 2016, 24(1): 1-5.

[2] 費程羽, 蘇中, 李擎. 行人慣性導(dǎo)航零速檢測算法[J]. 傳感器與微系統(tǒng), 2016, 35(3): 147-153.Fei C Y, Su Z, Li Q. Zero velocity detection algorithm for pedestrian inertial navigation[J]. Transducer and Microsystem Technologies,2016, 35(3): 147-153.

[3]Ganesh A, Tech M, Ganeshkumar K, et al. Multi sensor for reliable soldier and indoor navigation system[J]. International Journal of Advancements in Research & Technology, 2014, 3(2): 74-80.

[4]Nilsson J O, Skog I, H?ndel P.Foot-mounted INS for everybody– an open-source embedded implementation[C]//IEEE/ION Position Location and Navigation Symposium. 2012: 140-145.

[5]Nilsson J O, Gupta A K, H?ndel P.Foot-mounted inertial navigation made easy[C]//International Conference on Indoor Positioning and Indoor Navigation, Busan, Korea,2014.

[6]Wang Z L, Zhao H Y, Qiu S.Stance-phase detection for ZUPT-aided foot-mounted pedestrian navigation system[J]. ASME Transactions on Mechatronics, 2015, 20(6):3170-3181.

[7]Abirami S, Daniel S J. Navigation system for multi floor positioning using MEMS[J]. International Journal of Modern Trends in Engineering and Research, 2015, 2(1):293- 300.

[8]Harle R, Taherian S, Pias M. Towards real-time profiling of sprints using wearable pressure sensors[J]. Computer Communications,2012, 35: 650-660.

[9]李超, 蘇中, 朱嘉林. 可穿戴式自主定位技術(shù)的零速觸發(fā)算法研究[J]. 傳感技術(shù)學報, 2014,27(5): 627-632.Li C, Su Z, Zhu J L. A research about zero velocity trigger algorithm used in the wearable autonomous positioning technology[J].Chinese Journal of Sensors and Actuators, 2014, 27(5): 627-632.

[10]Fourati H. Heterogeneous data fusion algorithm for pedestrian navigation via foot-mounted inertial measurement unit and complementary filter[J]. Transactions on Instrumentation and Measurement,2015, 64(1): 221-229.

[11]Xu Y, Chen X Y, Wang Y M. Zero velocity detection algorithm for pedestrian inertial navigation[J].Journal of Chinese Inertial Technology, 2016, 24(3): 325-329.