基于零速修正的多源導(dǎo)航算法研究

姚文凱 邢礫文 黃瑩

摘要：論文針對(duì)行人行走過程中導(dǎo)航帶來的誤差，提出采用多傳感器數(shù)據(jù)融合的零速修正算法。首先，將慣性測(cè)量模塊安置于行人腿部，實(shí)時(shí)獲取行人行進(jìn)的加速度和角速度參數(shù);然后，通過采用SVM零速檢測(cè)分類方法，快速準(zhǔn)確識(shí)別出運(yùn)動(dòng)階段和靜止階段，在靜止階段速度和角速度都為零，同時(shí)利用磁強(qiáng)計(jì)確定方位，從而對(duì)行人的速度、角速度、方位等參數(shù)進(jìn)行誤差修正;最后，利用擴(kuò)展卡爾曼濾波方法對(duì)行人行走誤差進(jìn)行濾波處理，進(jìn)而對(duì)慣性導(dǎo)航解算的位置和方向信息進(jìn)行實(shí)時(shí)補(bǔ)償，實(shí)現(xiàn)對(duì)行人軌跡的跟蹤。

關(guān)鍵詞：導(dǎo)航算法;零速修正;卡爾曼濾波

中圖分類號(hào)：S762;TN713;TP212? 文獻(xiàn)標(biāo)識(shí)碼：A? 文章編號(hào)：1672-9129（2020）02-0035-02

Abstract： aiming at the errors caused by navigation during pedestrian walking， this paper proposes a zero-speed correction algorithm based on multi-sensor data fusion. First， the inertial measurement module is placed on the legs of pedestrians to obtain the acceleration and angular velocity parameters of pedestrians in real time. Then， the zero-speed detection and classification method of SVM is adopted to quickly and accurately identify the moving stage and the stationary stage. In the stationary stage， the velocity and angular velocity are both zero. At the same time， the azimuth is determined by the magnetometer， so as to correct the error of the pedestrian's velocity， angular velocity， azimuth and other parameters. Finally， the extended kalman filter method is used to filter the pedestrian walking error， and then the position and direction information of the inertial navigation solution are compensated in real time， so as to track the pedestrian trajectory.

Keywords： navigation algorithm; Zero speed correction; Kalman filtering

1 ??引言

隨著科學(xué)技術(shù)和信息化的快速發(fā)展，信息技術(shù)逐漸應(yīng)用到人們的日常生活中，為人們的日常活動(dòng)提供了更多的便利。在出行過程中，人們對(duì)于位置的需求以及路徑的導(dǎo)航規(guī)劃要求越來越高。將多元微慣性測(cè)量單元固定于移動(dòng)目標(biāo)上，通過實(shí)時(shí)測(cè)量目標(biāo)移動(dòng)過程中的加速度、角速度等數(shù)據(jù)參數(shù)，進(jìn)行目標(biāo)的位置和方向的導(dǎo)航解算，從而對(duì)目標(biāo)位置和方位進(jìn)行實(shí)時(shí)跟蹤和定位。這一技術(shù)不僅可以用于日常民用導(dǎo)航，還可以被廣泛應(yīng)用于航空航天、軍工等領(lǐng)域，具有重要的社會(huì)價(jià)值和學(xué)術(shù)意義。

2 ??SVM分類法

選取一組已知運(yùn)動(dòng)狀態(tài)的數(shù)據(jù)集作為樣本訓(xùn)練集，訓(xùn)練樣本集記為：? T={（xi，yi）}={（x1，y1），（x2，y2），...，（xn，yn）}，i=1，2，...，n? 其中，xi∈RN，為第i組提取的加速度數(shù)據(jù)和角速度數(shù)據(jù)組建的向量組;yi∈{+1，-1}，表示第i組數(shù)據(jù)的運(yùn)動(dòng)狀態(tài)。當(dāng)yi=+1時(shí)，表示行人行進(jìn)狀態(tài);當(dāng)yi=-1時(shí)，表示行人靜止?fàn)顟B(tài);為了能夠正確劃分訓(xùn)練數(shù)據(jù)集表征xi與yi的對(duì)應(yīng)關(guān)系，通過學(xué)習(xí)和訓(xùn)練在 n 維數(shù)據(jù)空間中找到一個(gè)最優(yōu)的超平面wxi+b=0，滿足yi（wxi+b）1，使幾何間隔最大。假設(shè)某樣本點(diǎn)（xi，yi）到超平面方程的幾何距離為：

λ=yi（wTxi+b）‖w‖

考慮到y(tǒng)i（wxi+b）1，則幾何間隔最大，即：

（λm=max1‖w‖）‖（λm=min12‖w‖2）

于是幾何間隔最大問題轉(zhuǎn)成對(duì)凸二次規(guī)劃求解算法改善問題。為了得到最優(yōu)化的結(jié)果，采用拉格朗日函數(shù)，并且使其滿足以下公式：

L（w，b，a）=12‖w‖2-∑nj=1ai（yi（wTxi+b）-1）

式中，ai為拉格朗日乘子。根據(jù)對(duì)偶性原理，利用求解極值的方法得到拉格朗日最優(yōu)化問題的解。首先分別對(duì)法向量w和截距b求偏導(dǎo)數(shù)，得到：w=∑naiyixi，∑naiyi

代回上式求得最優(yōu)解a=（a1，a2，...，an），然后計(jì)算得到 w 和 b 值。因此，分離超平面方程可用公式wxi+b=0來表示，相應(yīng)的分類決策函數(shù)為：f（x）=sign（wTxi+b）

3? 多源導(dǎo)航零速修正算法構(gòu)建

3.1導(dǎo)航參數(shù)修正。由于器件本身在工作時(shí)具有累積誤差，通過姿態(tài)解算，解算出來的姿態(tài)和速度需要進(jìn)行修正。在零速時(shí)刻，角速度誤差Δwk，速度誤差Δvk可以表示如下：? ?Δwk=[wxkwykwzk]-[000]Δvk=[vxkvykvzk]-[000]

將航向角誤差Δφk，角速度誤差Δwk，速度誤差Δvk作為觀測(cè)量，記為：Zk=[ΔφkΔwkΔvk]T

得到行人運(yùn)動(dòng)觀測(cè)方程為：Zk=HkXk+Vk

式中，Vk為測(cè)量噪聲，服從均值為零、協(xié)方差為Rk的高斯白噪聲分布;Hk是觀測(cè)轉(zhuǎn)移矩陣。

3.2擴(kuò)展卡爾曼濾波。假設(shè)在靜止的k時(shí)刻，行人運(yùn)動(dòng)特征誤差的狀態(tài)向量為：

X=E? N? U? δvE? δvN? δvU? δL? δλ? δh? εx? εy? εz?SymbolQC@x?SymbolQC@y?SymbolQC@zT

其中，E、N、U為失準(zhǔn)角;δvE、δvN、δvU分別為載體的東向、北向和天向速度誤差;δL、δλ、δh分別為緯度誤差、經(jīng)度誤差和高度誤差;εx、εy、εz、SymbolQC@x、SymbolQC@y、SymbolQC@z分別為陀螺隨機(jī)常值漂移和加速度計(jì)隨機(jī)常值零偏。（下標(biāo)E、N、U分別代表東、北、天）

基于卡爾曼濾波方程原理，可得EKF的遞推過程和步驟如下：

狀態(tài)一步預(yù)測(cè)：X^k/k-1=k，k-1X^k-1

狀態(tài)估計(jì)：X^k=X^k/k-1+Kk（Zk-HkX^k/k-1）

濾波增益：Kk=Pk/k-1HTk（HkPk/k-1HTk+Rk）-1

一步預(yù)測(cè)均方誤差：Pk/k-1=k，k-1Pk-1Tk，k-1+Qk-1

估計(jì)均方誤差：Pk=（I-KkHk）Pk/k-1

隨著采樣時(shí)刻的增加和卡爾曼濾波過程的遞推，可以得到每一個(gè)狀態(tài)的狀態(tài)向量誤差。由于在遞推過程中在靜止段對(duì)采樣和解算的加速度、角速度和方位進(jìn)行了閉環(huán)反饋修正，可高效地降低行人步進(jìn)過程中的累積誤差，改善行人導(dǎo)航和定位準(zhǔn)確度。

4? 基于零速修正的多源導(dǎo)航算法實(shí)驗(yàn)

考慮到慣性導(dǎo)航具有自主性和實(shí)時(shí)性，并能為行人慣性導(dǎo)航系統(tǒng)提供幫助的參考信息，在研究行人慣性導(dǎo)航系統(tǒng)時(shí)采用荷蘭Xsens公司提供的MEMS器件，其中慣性測(cè)量模塊由三軸加速度計(jì)、三軸陀螺儀、三軸磁力計(jì)、氣壓高度計(jì)等組成，其數(shù)據(jù)的采樣頻率可選用 50Hz、100Hz 等，另外通訊方式可采用 USB 接口或 RS232 串口通訊方式，將數(shù)據(jù)信息實(shí)時(shí)傳送并存儲(chǔ)到計(jì)算機(jī)里，進(jìn)行下一步的數(shù)據(jù)分析和解算。

對(duì)于繞圈行進(jìn)姿態(tài)變化，如圖1所示，可以清楚的看到橫滾角和俯仰角變化較小，而航向角隨著行人行進(jìn)方向持續(xù)發(fā)生變化，

按照既定路徑連續(xù)測(cè)量五次實(shí)驗(yàn)數(shù)據(jù)進(jìn)行比較，得五次的測(cè)量數(shù)據(jù)如表1所示。

行走距離（m）相對(duì)誤差（%）

表中可以看出，按圓形路徑行走，得到的軌跡平均誤差為 2%，最大誤差不超過 3%，整體導(dǎo)航效果較好。

參考文獻(xiàn)：

[1]Ladetto Q， Gabaglio V， Van Seeters J. Pedestrian navigation method and apparatus operative in a dead reckoning mode： U.S. Patent 6，826，477＼[P＼]. 2014.

[2]Hide C， Moore T， Botterill T. Low Cost Vision-Aided IMU for Pedestrian Navigation＼[J＼]. Journal of Global Positioning Systems， 2014， 10（1）： 3-10..

[3]吉訓(xùn)生， 王壽榮. MEMS陀螺儀隨機(jī)漂移誤差研究 ＼[J＼]. 宇航學(xué)報(bào)， 2016， 27（4）： 640-642.

作者簡(jiǎn)介：姚文凱（1996- ），男，漢，江蘇溧陽人，武警工程大學(xué)碩士研究生。研究方向：信息融合，導(dǎo)航制導(dǎo)與控制。? 邢礫文（1997- ），女，漢，吉林長春人，武警工程大學(xué)碩士研究生。研究方向：故障診斷。? 通訊作者：黃瑩（1978～），女，漢，博士研究生，武警工程大學(xué)信息工程學(xué)院通信裝備教研室副教授。研究方向：衛(wèi)星導(dǎo)航，衛(wèi)星通信，智能控制，數(shù)據(jù)融合。