基于零速修正的多源導航算法研究

姚文凱 邢礫文 黃瑩

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

關鍵詞：導航算法;零速修正;卡爾曼濾波

中圖分類號：S762;TN713;TP212? 文獻標識碼：A? 文章編號：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 ??引言

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

2 ??SVM分類法

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

λ=yi（wTxi+b）‖w‖

考慮到yi（wxi+b）1，則幾何間隔最大，即：

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

于是幾何間隔最大問題轉成對凸二次規劃求解算法改善問題。為了得到最優化的結果，采用拉格朗日函數，并且使其滿足以下公式：

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

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

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

3? 多源導航零速修正算法構建

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

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

得到行人運動觀測方程為：Zk=HkXk+Vk

式中，Vk為測量噪聲，服從均值為零、協方差為Rk的高斯白噪聲分布;Hk是觀測轉移矩陣。

3.2擴展卡爾曼濾波。假設在靜止的k時刻，行人運動特征誤差的狀態向量為：

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

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

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

狀態一步預測：X^k/k-1=k，k-1X^k-1

狀態估計：X^k=X^k/k-1+Kk（Zk-HkX^k/k-1）

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

一步預測均方誤差：Pk/k-1=k，k-1Pk-1Tk，k-1+Qk-1

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

隨著采樣時刻的增加和卡爾曼濾波過程的遞推，可以得到每一個狀態的狀態向量誤差。由于在遞推過程中在靜止段對采樣和解算的加速度、角速度和方位進行了閉環反饋修正，可高效地降低行人步進過程中的累積誤差，改善行人導航和定位準確度。

4? 基于零速修正的多源導航算法實驗

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

對于繞圈行進姿態變化，如圖1所示，可以清楚的看到橫滾角和俯仰角變化較小，而航向角隨著行人行進方向持續發生變化，

按照既定路徑連續測量五次實驗數據進行比較，得五次的測量數據如表1所示。

行走距離（m）相對誤差（%）

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

參考文獻：

[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]吉訓生， 王壽榮. MEMS陀螺儀隨機漂移誤差研究 ＼[J＼]. 宇航學報， 2016， 27（4）： 640-642.

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