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

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

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

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

1 系統(tǒng)組成

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

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

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

2 零速修正

圖2所示為行走時(shí)的步伐周期。步伐周期從中間站立階段開始，然后進(jìn)入腳尖離地階段，接著進(jìn)入擺動(dòng)階段，最后進(jìn)入腳跟觸地階段，完成了一個(gè)步伐周期的行為運(yùn)動(dòng)。在腳跟觸地到腳尖離地的過程中，只有中間站立階段是我們要檢測(cè)的零速時(shí)間段。

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

2.1 IMU檢測(cè)條件

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

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

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

2.2 壓力輔助檢測(cè)條件

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

2.2.1 壓力傳感器的固定位置

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

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

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

2.2.2 壓力模值檢測(cè)

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

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

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

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

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

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

2.3 零速更新

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

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

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

2.4 半實(shí)物仿真驗(yàn)證

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

圖6 前向速度Fig.6 Forward velocity

3 試驗(yàn)驗(yàn)證

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

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

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

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

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

表2 跑動(dòng)試驗(yàn)結(jié)果Tab.2 Results of running experiment

在已知平面圖的室內(nèi)環(huán)境下進(jìn)行閉合曲線行走試驗(yàn)，并在上位機(jī)實(shí)時(shí)顯示行走軌跡，如圖12所示。起點(diǎn)和終點(diǎn)幾乎重合，行走軌跡符合實(shí)際情況，水平定位精度優(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 跑動(dòng)下無壓力輔助軌跡圖Fig.10 Trajectory of running without pressure information aiding

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

圖12 實(shí)時(shí)行走軌跡Fig.12 Real-time trajectory

4 結(jié) 論

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

參考文獻(xiàn)（References）：

[1] 田曉春, 陳家斌, 韓勇強(qiáng), 等. 多條件約束的行人導(dǎo)航零速區(qū)間檢測(cè)算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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] 費(fèi)程羽, 蘇中, 李擎. 行人慣性導(dǎo)航零速檢測(cè)算法[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ù)學(xué)報(bào), 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.