基于行人運動模態辨識的室內外無縫導航航向算法研究

黃 欣,許建新,張 苗,熊 智

(南京航空航天大學自動化學院,南京 211100)

?

基于行人運動模態辨識的室內外無縫導航航向算法研究

黃 欣,許建新,張 苗,熊 智*

(南京航空航天大學自動化學院,南京 211100)

目前行人導航航向解算算法均基于導航傳感器在行人身體上的固定安裝模式,或者依賴其他射頻信息輔助修正陀螺航向,這極大約束了導航傳感器的適用條件。為此,利用陀螺對低頻噪聲的敏感性及加速度計低頻的穩定性,提出了解決行人手持手機穩態查看與非穩態搖擺的運動模態辨識算法和基于時域互補濾波器實現姿態的最優融合方法;研究了改進型互補濾波以消除行人的運動加速度對姿態解算的干擾誤差,提高了載體姿態的測量精度;此外,利用磁傳感器標定后的數據設計了自適應卡爾曼濾波算法,抑制了航向角的誤差發散。經實際數據測試驗證,室內外行人手持穩態與非穩態下的航向角測量精度提高了80%,同時大大提高了導航傳感器的適用性與便攜性,滿足實際工程的使用需求。

慣性傳感器;改進型互補濾波;磁異常辨識;自適應卡爾曼濾波;行人運動模態辨識

目前行人導航已經掀起國內外研究熱潮,針對行人導航技術的研究主要集中于以下兩類:一類是基于慣性傳感器,通過觀測慣性傳感器固定在行人肩部、腰部、腿部和腳部等部位的運動特性,研究零速修正、零角速修正、步態辨識等技術,實現行人的航跡推算[1-2];一類是基于無線網絡,研究利用藍牙、超聲波、WIFI、ZigBee等額外射頻設備輔助行人定位[3]。前者極大約束了傳感器的適用條件,便攜性較差;后者不僅增加了額外成本,同時其適用環境也較為狹窄。現代化社會,加速度計、陀螺儀等導航傳感器已普遍嵌入到手機中,而且據統計,除去16 h的睡眠及工作時間,手機人均每日使用時間長達3.7 h[4],已成為人們日常攜帶的必備產品,因此,利用手機自帶導航傳感器實現行人定位足以滿足日常需求。

但利用手機中的導航傳感器實現行人定位時,由于行人對手機的手持方式不同,迫切需要能夠解決行人在不同手持狀態下的航向解算算法,并盡可能提高航向角的計算精度,這也是限制手持式行人導航定位精度提高的主要因素。陀螺儀作為解算航向角的通用傳感器,受限于低成本傳感器零偏較大等因素,誤差發散較快[6],國內外針對手持式導航傳感器這一缺點,主要研究利用建筑樓向信息及射頻設備輔助陀螺修正航向[3],其適用條件受到約束;磁航向傳感器憑借對地球磁場的測量,可以長期有效地解算出當前載體的航向角,得到了廣泛的運用;但是室內環境下,鋼筋等鐵磁材料對周圍磁環境會產生強烈的干擾[6],磁異常辨識及處理不當會極大影響導航系統精度。

針對上述問題,本文以智能手機自帶的導航傳感器為硬件基礎,提出了非固連模式(手持)下高精度航向實時解算算法,主要解決行人手持穩態查看與非穩態搖擺下的室內外無縫導航航向問題。該方法研究了行人運動模態辨識算法以有效區分行人手持穩態查看與非穩態搖擺狀態;研究了改進型互補濾波器,從時域角度設計了相關參數,實現了加速度計姿態角與陀螺姿態角的最優融合,同時,消除了行人的運動加速度對姿態解算的干擾,提高了載體姿態的測量精度;此外,利用標定后的磁傳感器數據設計自適應卡爾曼濾波算法,抑制了航向角的誤差發散。本文在保證高精度的基礎上,大大提高了傳感器的適用范圍與便攜性。

1 基于行人運動模態辨識的改進型航向互補濾波器設計

將捷聯慣性導航解算算法用于行人導航解算過程中,姿態解算是其重要環節。基于陀螺對低頻噪聲的敏感性及加速度計低頻的穩定性考慮,本文研究了行人運動模態辨識算法以區分行人手持穩態查看與非穩態搖擺狀態,并在穩態下構建了互補濾波器[7-9],從時域角度研究利用最優加權平均參數估計完成姿態角的最優融合,由于行人運動會引入額外加速度,本文研究了改進型互補濾波器消除額外加速度對利用加速度計進行姿態解算的干擾,提高了載體姿態的測量精度。

1.1 行人運動模態辨識算法

行人手持載體在復雜運動環境下主要分為兩種模態,一種模態為行人行走時人體手臂擺動下的非穩態狀態,如圖1(a)所示;一種模態為行人行走時的穩態查看狀態,如圖1(b)所示。

圖1 行人運動模態示意圖

為有效區分上述的兩種模態,通過觀測z軸陀螺每秒角速度絕對值積分對兩種模態進行判別,僅在穩態下對姿態進行改進型互補濾波估計。模式判別約束如下所示:

(1)

式中:ω為z軸方向陀螺儀輸出的角速度,Δt為時間間隔。對陀螺儀輸出的角速度絕對值進行積分可以避免人體擺動周期性相消。為避免行人急速轉彎時產生誤判,則同時針對z軸方向加速度計進行判別約束,當z軸方向加速度計同時大于某值時,判為穩態,否則為非穩態。

因此,當滿足

(2)

此時判為手持穩態查看狀態,否則為非穩態狀態,其中ε、gt為靜止穩態下計算出來的參數值,gz為z軸方向加速度計輸出,如式(3)所示,其中U代表大小為N的數組,g為當地重力加速度。

(3)

1.2 基于行人運動模態辨識的改進型互補濾波器設計

姿態解算利用陀螺儀與加速度計所測得的數據進行互補濾波獲得。加速度計主要用來測量載體坐標系下的線加速度,通過比較加速度計各軸輸出與重力加速度的比值可以解算出姿態角,如式(4)所示,其測量誤差不會隨時間發散,但會受到測量誤差和運動狀態的影響。

(4)

陀螺儀主要用來測量載體相對于參考坐標系下的角速度,利用陀螺儀輸出解算出姿態角目前主要包括歐拉角法、方向余弦法和四元素法,其中歐拉角法每個方程均包含三角函數運算,該姿態算法復雜且不能全姿態工作;方向余弦法微分方程組維數太高且需進行正交化處理;四元素法微分方程只有四維且可以全姿態工作,因此本文選擇四元素法解算姿態角[10],如式(5)所示:

(5)

1.2.1 互補濾波器模型設計

鑒于加速度計在低頻下的測量穩定性以及陀螺對低頻噪聲的敏感性,本文從時域角度分析,構建互補濾波器在低頻穩態下對兩者姿態進行數據融合,提高姿態解算精度,如圖2所示。

圖2 互補濾波器原理

假設陀螺儀的權重為k1,加速度計的權重為k2,則對真值的估計為:

(6)

且

k1+k2=1

(7)

則估計值的方差為

(8)

對其求偏導,可以求得最小值

(9)

此時,最優解參數為

(10)

則經互補濾波后的最優姿態角為:

η=k1ηw+k2ηf

(11)

式中:η代表橫滾角與俯仰角,又因為:

ηwn=ηwn-1+ωT

(12)

則

ηk=k1(ηwk-1+ωT)+k2ηfk

(13)

式中:T為采樣周期,則式(13)即為k時刻的姿態角解算方程。

1.2.2 改進的互補濾波器模型設計

由于行人運動環境下,會引入額外加速度,必須對其加以補償,否則會造成加速度計姿態角解算不準確的問題。本文在常規的互補濾波器的基礎上,研究了改進型互補濾波器,消除行人自帶加速度對利用加速度計進行姿態解算的干擾,其原理如圖3所示。

圖3 改進型互補濾波器原理

(14)

(15)

2 基于自適應卡爾曼濾波的慣性/地磁航向最優融合

第1節設計了行人運動模態下的高精度水平姿態解算算法(橫滾、俯仰),解決了行人不同運動模態下的地磁投影問題,下面將以此為基礎,設計航向和慣性的最優組合算法。

2.1 卡爾曼濾波器狀態建模

卡爾曼濾波器作為一種最小方差估計,從概率統計最優的角度估計出系統誤差并將其消除。本文選用“東北天”地理坐標系,構建18階模型,如式(16)所示。其中 包括:3個平臺角誤差、3個速度誤差、3個位置誤差;3軸陀螺隨機常數、3軸陀螺一階馬爾可夫過程與3軸加速度計一階馬爾可夫過程[11-13]。在扣除加速度計、陀螺儀零偏之后,利用卡爾曼濾波器在穩態下修正陀螺漂移、加速度計漂移及航向角誤差;在非穩態下修正陀螺漂移、加速度計漂移、姿態角誤差及航向角誤差。

(16)

2.2 卡爾曼濾波器量測建模

目前,國內外針對磁補償主要包括二維橢圓標定算法和三維橢球標定算法,二維橢圓標定算法利用水平旋轉一圈水平磁矢量大小不變的特性進行磁標定,三維橢球標定算法利用三維空間磁矢量大小不變的特性對磁進行補償[14-16]。鑒于兩者標定精度相當及二維標定的便捷性,本文采用二維橢圓標定算法,對磁傳感器數據進行誤差補償,消除硬磁、軟磁以及自身的非正交安裝誤差帶來的干擾。通過對磁傳感器輸出數據實時坐標投影,以獲得水平方向磁傳感器數據,如圖4所示。

圖4 磁航向示意圖

則磁航向角可由如下公式獲得:

(17)

本文構建一維量測模型,量測周期為1s,量測方程如下式所示:Zv(t)=[φw-φm]=[δφ+Mφ]=Hv(t)X(t)+Vv(t)

(18)

式中:φw為陀螺解算出的航向角,φm為磁航向角,δφ為姿態誤差角。姿態誤差角與平臺誤差角存在如下關系[17]:

(19)

式中:θ為俯仰角,φx、φy、φy為3個平臺誤差角。因此:

(20)

2.3 基于實時統計信息的磁異常辨識及量測噪聲模 型更新方法

針對2.2節量測噪聲陣的選取,本文首先采集了室內外環境下相關磁信息,基于統計學相關理論對數據進行了分析與處理,如圖5所示,其中圖5(a)、圖5(b)分別為室外與室內一段路線總磁場強度的變化曲線,圖5(c)、圖5(d)分別為室外與室內一段路線總磁場強度方差的變化曲線,可以發現室外磁場強度維持在5 μT左右變化,方差大小均在1.4以下,而室內磁場幅度變化較大,利用此相關信息可以有助于區分磁異常情況。

圖5 室內外磁傳感器信息對比

本文研究利用磁傳感器實時統計信息進行磁異常辨識,判別約束條件如下所示:

(21)

(22)

式中:λ為設定的經驗值,η為事先人為設定的增益常量,ε3與ε4為通過統計學方法獲得的參數值。

3 仿真驗證與實驗結果分析

本次實驗儀器采用華為Mate 8 型號手機,采集頻率50 Hz,分別對行人步行穩態查看場景、行人步行穩態與非穩態順序切換場景進行了室內外測試,室外以GPS航向角作為參考(初始靜止狀態下,GPS誤差較大,不進行誤差對比),室內以人為規劃路徑航向角作為行人行走航向角參考,其中實驗航向角每0.02 s輸出一次,GPS航向角與標準航向角均為1 s輸出一次,僅以整秒時刻量化驗證航向角計算精度。

3.1 行人室外步行穩態查看場景

圖6 室內外磁二維橢圓標定對比

行人步行穩態查看場景下,手持載體可以輕微抖動,但是不存在大姿態例如手臂搖擺情況。鑒于室外磁異常情況較少,如圖6(a)所示,因此室外環境僅用來分析純陀螺解算航向角與經自適應卡爾曼濾波融合航向角的差異性。純陀螺解算航向角與GPS航向角對比曲線及手持模式判別曲線如圖7所示,組合航向角與GPS航向角對比曲線及手持模式判別曲線如圖8所示。

圖7 純陀螺航向與GPS航向對比、模式判別曲線

圖8 組合航向與GPS航向對比、模式判別曲線

3.2 行人室外步行穩態與非穩態切換場景

行人步行穩態查看場景下,手持載體可以輕微抖動,但是不存在手臂搖擺情況;行人步行非穩態場景下,手臂搖擺,兩者循環交替出現。純陀螺解算航向角與GPS航向角對比曲線及手持模式判別曲線如圖9所示,組合航向角與GPS航向角對比曲線及手持模式判別曲線如圖10所示,其中圖中綠色為行人運動模態辨識曲線,91 s～128 s、220 s～272 s為非穩態狀態,模態置0,其余為穩態,模態置1。

圖10 組合航向與GPS航向對比、模式判別曲線

圖9 純陀螺航向與GPS航向對比、模式判別曲線

本文將室外場景下的航向角誤差均值做了對比,由于手持擺動情況下,航向角反應的是手持載體的航向角,GPS反應的是行人行走航向角,因此主要觀測擺動過后,航向角能否迅速跟蹤GPS航向角,如表1所示,可以發現組合航向角精度相對于純捷聯航向角有了很大的提高。

表1 室外不同組合下航向角誤差均值

綜合上述室外結果可以發現,利用改進型互補濾波器及卡爾曼濾波器實現航向角的最優融合有助于提高其精度。鑒于行人速度下,GPS航向角精度難以保證,同時室內缺乏GPS信息,因此室內環境下,人為規劃方形標準路徑,如圖11所示。路徑初始航向58°,每經過2號位、3號位、4號位時刻,航向偏移90°,并利用計時器精準記錄當前時刻以保證后續精度的對比。觀測圖6(b),室內磁異常較為嚴重,在驗證磁異常檢測及自適應濾波的同時,對比組合航向角與標準航向角誤差。

圖11 室內路徑示意圖

3.3 行人室內步行穩態查看場景

行人步行穩態查看場景下,手持載體可以輕微抖動,但是不存在手臂搖擺情況。未加自適應濾波的組合航向角與標準航向角對比曲線及手持模式判別曲線如圖12所示,加入自適應濾波的組合航向角與標準航向角對比曲線及手持模式判別曲線如圖13所示。

3.4 行人室內步行穩態與非穩態切換場景

行人步行穩態查看場景下,手持載體可以輕微抖動,但是不存在手臂搖擺情況;行人步行非穩態場景下,手臂搖擺,兩者循環交替出現,未加自適應濾波的組合航向角與標準航向角對比曲線及手持模式判別曲線如圖14所示,加入自適應濾波的組合航向角與標準航向角對比曲線及手持模式判別曲線如圖15所示,其中圖中綠色為行人運動模態辨識曲線,56 s～81 s、125 s～154 s為非穩態狀態,模態置0,其余為穩態,模態置1。

圖12 航向對比、模式判別曲線

圖13 航向對比、模式判別曲線

圖14 航向對比、模式判別曲線

圖15 航向對比、模式判別曲線

由于標準路徑航向角僅與人體運動方向相對應,手持搖擺情況下,由于左右的擺動幅度角,曲線組合航向角反應的僅是手機的航向,并非行人行走的航向,因此僅以穩態對應路段下行人行走的航向角作對比,其結果如表2所示。

表2 室內不同情景下航向角誤差均值

觀測上述數據,本文提出的基于實時統計信息進行磁異常判斷及量測噪聲陣修正具有明顯的效果,有效的剔除了磁異常航向角,同時,當行人從非穩態進入穩態狀態下,航向角能精準而又迅捷的跟蹤行人行駛航向角,算法精度較高。

綜述上述實驗,本文提出的基于行人運動模態辨識的室內外無縫導航航向算法,無論是步行穩態查看狀態,還是步行穩態與非穩態切換狀態;無論是室外磁正常環境,亦或是室內磁異常環境,其均有較高的航向精度,算法適應性較高。

4 仿真驗證與實驗結果分析

本文以智能手機自帶導航傳感器為硬件基礎,結合行人復雜運動環境下不同模態特性,設計了一種非固連模式(手持)下高精度航向實時解算算法。該方法研究了運動模態辨識算法有效區分出行人手持穩態查看與非穩態搖擺狀態。同時,基于互補濾波器,從時域角度設計了相關參數,實現了加速度計姿態角與陀螺姿態角的最優融合,在此基礎上研究了改進型互補濾波器,消除了行人運動加速度對利用加速度計進行姿態解算的干擾誤差,提高了載體姿態的測量精度。此外,本文還研究了低精度導航傳感器誤差的實時最優估計算法并研究了自適應卡爾曼濾波器實現陀螺航向角與磁航向角的最優融合,利用實時統計信息實現磁異常辨識及組合濾波量測噪聲陣的自適應調整。本算法能夠保證室內外行人手持傳感器復雜運動環境下的航向角精度,提高了傳感器的適用性,具有足夠的工程應用價值。

[1] 錢偉行,曾慶化,萬駿煒,等. 基于人體下肢運動學機理的行人導航方法[J]. 中國慣性技術學報,2015(1):24-28.

[2] Stirling R G. Development of a Pedestrian Navigation System Using Shoe Mounted Sensors[J]. 2005.

[3] Antonio Ramón Jiménez Ruiz,Fernando Seco Granja,José Carlos Prieto Honorato,et al. Accurate Pedestrian Indoor Navigation by Tightly Coupling Foot-Mounted IMU and RFID Measurements.[J]. IEEE T Instrumentation and Measurement,2011,61:178-189.

[4] 賀艷. 試析移動互聯網背景下新媒體劇的敘事變化與價值指向[J]. 中國電視,2015(4):65-69.

[5] Afzal M H,Renaudin V,Lachapelle G. Magnetic Field Based Heading Estimation for Pedestrian Navigation Environments[C]//International Conference on Indoor Positioning and Indoor Navigation. IEEE,2011:1-10.

[6] 王昌剛,劉強,韓瑜. 姿態航向測量與誤差補償方法研究[J]. 傳感器與微系統,2014,33(11):37-40.

[7] 萬曉鳳,康利平,余運俊,等. 互補濾波算法在四旋翼飛行器姿態解算中的應用[J]. 測控技術,2015(2):8-11.

[8] Calvin Coopmans,Austin M Jensen,Yangquan Chen. Fractional-Order Complementary Filters for Small Unmanned Aerial System Navigation[J]. Journal of Intelligent and amp;Robotic Systems,2014,731-4:.

[9] Tian Y,Wei H,Tan J. An Adaptive-Gain Complementary Filter for Real-Time Human Motion Tracking With MARG Sensors in Free-Living Environments.[J]. IEEE Transactions on Neural Systems and Rehabilitation Engineering,2013,21(2):254-264.

[10] 丁君,趙忠華. AHRS航姿解算中的兩種濾波方法的比較研究[J]. 計算機仿真,2013(9):60-65.

[11] 錢偉行,彭晨,田恩剛,等. 基于導航信息雙向融合的行人/移動機器人協同導航方法[J]. 中國慣性技術學報,2014(1):74-78.

[12] 代金華,張麗杰. 多準則MEMS陀螺隨機誤差在線建模與實時濾波[J]. 傳感技術學報,2016,29(1):75-79.

[13] 吳濤,白茹,朱禮堯,等. 基于卡爾曼濾波的航姿參考系統設計[J]. 傳感技術學報,2016,29(4):531-535.

[14] 李艷,管斌,王成賓,等. 電子磁羅盤航向角誤差推導及分析[J]. 儀器儀表學報,2013(7):127-133.

[15] 袁智榮. 磁航向傳感器使用中的誤差補償[J]. 測控技術,2001(1):58-59.

[16] 張樨,李杰,蔣竅,等. 基于橢球擬合的三軸磁傳感器快速標定補償方法[J]. 機械工程與自動化,2012(4):163-165.

[17] 丁濤,趙忠. 捷聯慣導系統的誤差角關系與推導[J]. 兵工自動化,2007,26(12):66-67.

Research on Indoor and Outdoor Seamless Navigation Heading Algorithm Based on Pedestrian Modal Identification

HUANG Xin,XU Jianxin,ZHANG Miao,XIONG Zhi*

(College of Automation Engineering,Nanjing University of Aeronautics and Astronautics,Nanjing 211100,China)

Nowadays,heading algorithm in pedestrian navigation is mostly based on the fixed mode of navigation sensor on the pedestrian body,or relies on other auxiliary radio frequency information,which greatly reduces the portability of sensors. Hence,based on the sensitivity of the gyroscope to the low frequency noise and the stability of the accelerometer,this paper puts forward one solution to determine the steady-state and non-steady state of the hand held mobile based on the modal identification and proposes complementary filter to achieve optimal attitude based on time domain. This paper uses modified complementary filter to weaken the interference caused by pedestrian movement and improves the accuracy of the carrier attitude measurement. In addition,the adaptive kalman filter algorithm is designed by using the magnetic sensor calibration data to restrain the divergence of heading angle error. The test results show that this algorithm can ensure that the accuracy of heading angle measurement in indoor and outdoor pedestrians can be improved by 80% and greatly improve the adaptability and portability of sensors simultaneously,which meets the demand of practical engineering.

inertial sensors;modified complementary filter;identification of magnetic anomaly;adaptive Kalman filter;model identification of pedestrian movement

黃 欣(1993-),男,漢族,江蘇省常州市人,2015年獲南京航空航天大學自動化專業學士學位,同年進入南航導航研究中心讀研,研究方向是慣性技術及組合導航研究,huangxin@nuaa.edu.cn;

熊 智(1976-),男,漢族,四川人,研究員,博士研究生導師,主要研究領域為慣性技術、景象匹配輔助導航、天文導航及慣性組合導航等領域。

項目來源:國家自然科學基金項目(61533008,61673208,61533009,61374115);江蘇省六大人才高峰項目(2013-JY-013);江蘇高校優勢學科建設工程項目和江蘇省“物聯網與控制技術”重點實驗室基金項目;中央高校基本科研業務費專項資金項目(NZ2016104,NS2017016);江蘇省“333工程”科研立項項目(BRA2016405);留學人員擇優項目(2016年);航空科學基金項目(20165552043);江蘇省普通高校研究生科研創新計劃項目(KYLX15_0264);江蘇省自然科學基金項目(BK20141453)

2016-11-11 修改日期:2017-01-15

V249.32+8

A

1004-1699(2017)06-0878-08

C:7230

10.3969/j.issn.1004-1699.2017.06.013