速度自適應約束卡爾曼濾波方法*

周承松，彭　競，劉文祥，王飛雪

（國防科學技術大學電子科學與工程學院，長沙410073）

速度自適應約束卡爾曼濾波方法*

周承松，彭競，劉文祥，王飛雪*

（國防科學技術大學電子科學與工程學院，長沙410073）

為解決動態目標定位過程中使用傳統卡爾曼濾波算法不能利用先驗信息提高定位精度、而使用速度約束卡爾曼濾波算法易發散等問題，論文提出一種基于速度先驗信息自適應約束卡爾曼濾波方法。該方法以速度觀測值和定速誤差統計特性建立變異系數，實時判斷速度觀測值是否滿足約束濾波的要求，從而自適應地在速度約束濾波和傳統卡爾曼濾波間選擇。實驗結果表明，與傳統卡爾曼濾波和速度約束濾波相比，本文算法的定位精度提高了cm級至m級，既克服了速度約束卡爾曼濾波易受定速誤差影響的缺點，也能利用合理的速度觀測值來提高定位精度。

卡爾曼濾波；速度自適應約束；動態定位；變異系數；先驗信息

動態目標定位過程中，利用卡爾曼濾波KF （Kalman Filtering）對目標進行狀態估計，會受到觀測誤差和動態模型誤差影響導致狀態估計精度較差［1-3］，而利用先驗信息能夠提高狀態估計精度［4-8］。

文獻［9］提出以道路先驗信息作為約束條件的卡爾曼濾波算法，該方法必須實時獲取道路信息，因此道路約束卡爾曼濾波算法存在一定局限性［10］。另外，文獻［10-12］提出速度約束卡爾曼濾波算法，實驗證明，該方法的定位精度易受定速誤差的影響。

本論文提出一種速度自適應約束卡爾曼濾波方法，該方法以定速標準差和速度觀測值建立變異系數，即自適應因子，在傳統卡爾曼濾波和速度約束卡爾曼濾波方法間進行自適應選擇。而文獻［13］的自適應卡爾曼濾波依據道路信息自適應卡爾曼濾波方法是自適應調整動態模型。實驗表明，與傳統卡爾曼濾波相比，本文算法能有效提高定位精度，同時避免了道路約束濾波需實時獲取道路信息的缺陷，也克服了速度約束濾波易受定速誤差影響的不足。

1　卡爾曼濾波

卡爾曼濾波由狀態方程和觀測方程組成，即

式（1）中，Xk為k時刻的狀態向量，φk，k-1為狀態轉移矩陣，Xk-1為k-1時刻的狀態向量，Wk為狀態模型噪聲；式（2）中，Lk為觀測向量，Ak為觀測矩陣，ek為觀測噪聲。

卡爾曼濾波算法的具體流程為：依次計算預測狀態向量、預測狀態協方差、新息及其協方差、增益矩陣，最終計算得到狀態估值和狀態估計協方差。

2　速度自適應約束卡爾曼濾波

速度自適應約束卡爾曼濾波，就是在速度約束濾波的基礎上，加入了自適應選擇傳統卡爾曼濾波的過程。因此，首先闡述先驗信息約束卡爾曼濾波的原理、速度約束卡爾曼濾波的過程，再介紹自適應因子等內容，進而對本文算法更詳盡的描述。

2.1先驗信息約束卡爾曼濾波原理

將先驗信息約束融入卡爾曼濾波中的方法主要有偽測量法［14］、降維法［15］和投影法［16］等，論文以投影法為例。投影法將卡爾曼無約束狀態估計向量通過約束條件函數直接投影到約束面，得到約束狀態估計向量X′。為求得X′，需根據先驗信息建立狀態參數間的關系式，即狀態約束方程：

其中，B為狀態參數的系數矩陣，D為常數矩陣。再以投影法原則建立目標函數：

其中，W為任意對稱的正定權矩陣。

為了同時滿足狀態約束方程和投影法目標函數，根據式（3）和式（4）構造拉格朗日條件式，即式（5），式中λ為拉格朗日乘數向量。

為求得當Ω最小時的X′，令Ω對X′和λ的一階偏導數等于零，即式（6）和式（7），再解得約束狀態估計量X′，即式（8）。

2.2速度約束卡爾曼濾波

2.2.1速度約束卡爾曼濾波模型

車載接收機的測速精度約為0.2 m/s，而測距碼的動態單點定位精度約為10 m［17］。因此，利用速度觀測值間的某種關系提高定位精度，在理論上是可行的。由于車輛行駛速度、行駛范圍受到車輛性能、交通法規和道路寬度等的限制，假設車輛在較短時間間隔Δt內的行駛軌跡是直線，平均速度為接收機在Δt內始末時刻速度觀測值的平均值［13］?；谠摷僭O，在Δt內車輛的位置值滿足空間直線關系，如式（9），速度值滿足式（10）。

式（9）和式（10）中，（u，v，w）為直線方向向量，（xk，yk，zk）為k時刻的位置值，）為k時刻和k-1時刻的速度均值。

根據xyz方向間的狀態約束方程式（11）和式（12），可構建形如式（3）的狀態約束方程，再參照式（8）就可求得速度約束濾波的結果。

2.2.2速度約束卡爾曼濾波條件

速度約束卡爾曼濾波利用xyz方向速度觀測值兩兩間的狀態約束方程進行濾波。分析式（11）和式（12）可得，要保證狀態約束方程的準確性，速度觀測值間的比值必須要盡可能準確，以接近真實速度間的比值，這就是速度約束卡爾曼濾波能提高濾波精度的前提條件。

由于存在定速誤差，各歷元的速度觀測值間比值不一定都接近真實速度間的比值。當不滿足速度約束濾波條件時，速度約束濾波結果將出現不可估量的偏差。因此，速度約束濾波存在易受定速誤差影響的缺點。

2.3速度自適應約束卡爾曼濾波

速度自適應約束卡爾曼濾波是在速度約束濾波的基礎上，通過速度比值的變異系數確定狀態約束方程的準確程度，在約束方程不準確時選擇傳統卡爾曼濾波，在約束方程準確時選擇速度約束濾波。實現這一思想的核心是構造一個自適應因子，在傳統卡爾曼濾波和速度約束濾波間進行自適應選擇。

2.3.1自適應因子

式（13）中，0為速度誤差均值，σ為速度分量誤差的標準差。在實際動態定位中，多普勒定速誤差約為0.2 m/s，則速度分量的標準差σ約為0.115 5 m/s［17］。

要使得定速誤差對速度分量間的比值影響較小以便保證狀態約束方程的正確性，速度觀測值間的比值則要盡可能接近真實速度間的比值，以xy方向為例，即要滿足

在尋求滿足式（14）的條件時，由于無法獲知定速誤差的真實值（εvxk，εvyk），則考慮從定速誤差的統計特性著手，即式（13）。首先，利用速度分量誤差的標準差σ構建集合U：

然后分析集合U，若集合內五個值的離散程度越大，說明定速誤差對速度分量間的比值影響越大；若離散程度小，則說明定速誤差對其速度分量間比值影響小。例如，當xy方向的真實速度值均為20 m/s，比值為1時，考慮定速誤差的影響后，的比值是接近1的，即滿足式（14），另外，集合中的五個值也都接近于1，此時這五個值的離散程度較小，定速誤差對速度分量間比值影響較??；但當xy方向的真實速度值均為0.1 m/s，比值為1時，考慮定速誤差的影響后，的比值是難以保證接近1的，即不能保證滿足式（14），此時，集合中的5個值也都不一定接近于1，離散程度較大，定速誤差對速度分量間比值影響也較大。故以集合中五個值的離散程度來衡量定速誤差對速度分量間比值的影響是可行的。

衡量離散程度的指標主要有極差、平均差、標準差、方差和變異系數等，而標準差最為常用。但集合U中的元素Ui在各歷元處的平均值是不一樣的，若利用標準差衡量離散程度顯得不合理，而采用標準差與平均值的比值（變異系數）來表示離散程度則更為合理［18］。以xy方向為例，其變異系數cvxy的求取公式為：

同理，求出yz方向的變異系數cvyz。以變異系數（cvxy，cvyz）作為自適應因子，當cvxy、cvyz都小于閾值d時，則認為定速誤差對速度分量間的比值影響較小，滿足速度約束濾波條件，選擇速度約束濾波；反之，則選擇傳統卡爾曼濾波。

2.3.2閾值的確定

變異系數用來衡量序列離散程度，變異系數不超過某閾值則認為該數據穩定合格，這在工業制造、醫學等行業有廣泛應用。在白細胞過氧化物傳感器研制中，制備固定細胞膜的重現性變異系數為5.5%［19］；在機器發動機使用壽命可靠性研究中設為30%～40%［20］；在學生考試成績的穩定評估中設為20%［21］；在GBT 27404—2008實驗室質量控制規范中，根據成分濃度的不同其變異系數門限變動在1.3%～43%區間。綜上所述，本文實驗變異系數的閾值d取0～90%內不同值，分析不同閾值對實驗結果的影響。

3　實驗及分析

3.1實驗數據

實驗中，跑車作起步、勻速、加減速、轉彎等復雜的運動，共6個路段，真實軌跡如圖1所示。

圖1　車輛真實軌跡

各觀測歷元的參考坐標和參考速度已知，行駛時間為920 s，車載接收機采樣間隔為1 s。在參考坐標和參考速度上加入服從正態分布的隨機誤差，模擬得到接收機的坐標觀測值和速度觀測值。

3.2實驗方案

采用勻速運動模型，狀態向量為Xk，以接收機輸出坐標和速度為觀測向量，建立狀態方程和觀測方程。

實驗中，分別在不同定速誤差的情況下，分析比較卡爾曼濾波算法、速度約束卡爾曼濾波算法和本文算法的差異。本文以位置偏差的標準差SD（Standard Deviation）和均方根RMS（Root Mean Square）作為濾波精度指標，SD越大則濾波結果越離散，RMS越大則濾波精度越差。

位置偏差的求解過程為：先求出各歷元濾波結果與參考坐標的偏差（dxk，dyk，dzk）；再求出位置偏差dk，即

位置偏差的標準差SD的求取公式為：

位置偏差的均方根RMS的求取公式為：

3.3實驗及分析

分析不同的定速誤差、不同的閾值對卡爾曼濾波算法、速度約束卡爾曼濾波算法和本文算法的影響及差異。

3.3.1定速誤差

定速誤差即為加入參考速度上的隨機誤差，其服從高斯分布。在閾值為20%的前提下，實驗選取均值為0，速度分量誤差標準差σ分別為0.015 5 m/s至1.115 5 m/s等間隔的12個值，共12組定速誤差序列。

為簡要說明，給出當 σ1=0.115 5 m/s、σ2= 0.615 5 m/s時3種算法的位置偏差圖，如圖2，其位置偏差標準差SD和均方根值RMS如表1。

圖2　位置偏差圖

表1　位置偏差的SD和RMS　單位：m

另外，在12個不同σ的情況下，統計3種算法的位置偏差標準差SD和均方根值RMS，結果如圖3、圖4所示。

圖3　位置偏差標準差（SD）

圖4　位置偏差均方根（RMS）

結合表1、圖2，重點分析圖3和圖4，整體上觀察，隨著定速誤差的增大，三種算法的濾波位置偏差標準差和均方根也出現增大的現象，即定位精度呈下降趨勢，其中，速度約束濾波受定速誤差的影響最大，而本文算法基本不受影響，傳統卡爾曼濾波受的影響最小。

當速度分量誤差標準差σ在0～0.15 m/s時，此時的速度觀測值相對準確，能夠滿足速度約束濾波的要求，故速度約束濾波的精度最高。而本文算法以變異系數不超過閾值的原則對速度觀測值進行自適應篩選，而該原則不可能完全準確，排除了部分原本能用來約束濾波的速度觀測值，因此其濾波精度比速度約束濾波法要差；但與傳統卡爾曼濾波相比，其濾波精度要優。

當速度分量誤差標準差大于0.15 m/s時，速度約束濾波的精度急劇變差；傳統卡爾曼濾波的精度緩慢變差，但最為穩定。而本文算法的濾波精度明顯優于速度約束濾波；與傳統卡爾曼濾波相比，其SD值要優，即濾波結果的離散程度較小，但在速度分量標準差為0.20 m/s～0.40 m/s、0.72 m/s～1.11 m/s區間時，其RMS略差。

綜合比較，本文算法既有傳統卡爾曼濾波的穩健性，又有速度約束濾波的優點，雖然在速度分量誤差標準差在0.2 m/s以上時，其濾波精度偶爾會比傳統卡爾曼濾波略差，但實際中的速度分量誤差標準差一般低于0.2 m/s，因此，實際定位過程中采用本文算法較為合適。

3.3.2閾值

在速度分量誤差標準差為0.115 5 m/s時，分析閾值d對本文算法的影響。閾值是變異系數的門限，在該門限以內則進行速度約束濾波，否則進行傳統卡爾曼濾波。在不同閾值的情況下，本文算法的位置誤差標準差SD和均方根RMS如圖5所示。

圖5　閾值對濾波精度影響圖

分析圖5，本文算法的濾波精度隨閾值的增加有先好再差的現象，呈凹形趨勢，其原因是：閾值越小，對速度觀測值的要求越嚴格，過于嚴格則會剔除原本能用來約束濾波的速度觀測值，導致濾波精度變差，閾值為0時本文算法就相當于傳統卡爾曼濾波；而閾值越大，對速度觀測值要求越松，當要求過松時，會導致原本不能用來約束濾波的速度觀測值卻能滿足要求，故濾波精度也會變差，閾值無窮大時本文算法相當于速度約束濾波。因此，選取合適的閾值是關鍵，本實驗中合適的閾值區間是20%～60%。

4　總結

本文算法是基于速度觀測值的自適應約束濾波，根據速度觀測值和定速誤差的統計特性建立變異系數，以變異系數與閾值作比較，實時判斷速度觀測值是否滿足約束濾波條件，從而在傳統卡爾曼濾波和速度約束濾波中進行自適應選擇，達到合理利用速度先驗信息提高定位精度的目的。

實驗證明，本文算法不僅克服了速度約束濾波易受定速誤差影響的缺點，也比傳統卡爾曼濾波提高了濾波精度，既有傳統卡爾曼濾波的穩健性，又有速度約束濾波的優點，是兩者的有效結合，相比之下其定位精度提高的范圍在cm～m。

［1］Rousseeuw P J，Driessen K V.A Fast Algo Rithm for the Minimum Covariance Deter Minant Estimator［J］.Technometrics，1999，41（3）：212-223.

［2］楊元喜，任夏，許艷.自適應抗差濾波理論及應用的主要進展［J］.導航定位學報，2013，1（1）：9-15.

［3］楊元喜，曾安敏，景一帆.函數模型和隨機模型雙約束的GNSS數據融合及其性質［J］.武漢大學學報（信息科學版），2014，39（2）：127-128.

［4］嚴浙平，郝悅，王千一，等.基于變維卡爾曼濾波的UUV目標運動優化估計方法［J］.傳感技術學報，2014，27（12）：1637-1642.

［5］蔡斌，梁甸農，董臻，等.基于先驗信息的分布式星載單基線SAR/ATI方法［J］.國防科學技術大學學報，2008，30（4）：76-81.

［6］丁海強，齊光快，莊華亮，等.基于負約束條件下最大似然估計的無線傳感網絡定位算法［J］.傳感技術學報，2014，27（11）：1545-1550.

［7］任曉棟，馮祖仁，蘇承平.利用姿態約束的并聯機器人運動學標定方法［J］.西安交通大學學報，2008，42（12）：1445-1449.

［8］苑艷華，李四海，南江.基于卡爾曼濾波器的航姿系統測姿算法研究［J］.傳感技術學報，2011，24（12）：1718-1722.

［9］左峻疆，鮑遠律，劉艷.一種帶約束條件的GPS動態濾波新方法［J］.計算機仿真，2004，21（12）：80-83.

［10］左峻疆.帶約束條件的GPS定位濾波方法研究［D］.合肥：中國科學技術大學，2005：41-42.

［11］付強文，秦永元，李四海.速度約束輔助車載捷聯慣導系統零速校正算法［J］.系統工程與電子技術，2013，35（8）：1723-1728.

［12］伍小潔，房建成.一種帶速度觀測量的GPS動態定位自適應卡爾曼濾波新算法［J］.中國慣性技術學報，2000，8（3）：28-32.

［13］劉友文，劉經南，朱敦堯.附有道路信息約束的自適應卡爾曼濾波在車載導航中的應用［J］.武漢大學學報（信息科學版），2008，33（8）：828-830.

［14］Alouani A T，Blair W D.Use of a Kinematic Constraint in Track-ing Constant Speed，Ma Neuvering Targets［J］.IEEE Transactions on Automatic Control，1993，38（7）：1107-1111.

［15］Wen W，Durrant H F.Model Based Multi Sensor Data Fusion［C］// IEEE International Conference on Robotics and Automation，1992：1720-1726.

［16］Simon D，Chia T L.Kalman Filtering with State Equality Constraints［J］.IEEE Transactions on Aerospace and Electronic Systems，2002，38（1）：128-136.

［17］謝鋼.GPS原理與接收機設計［M］.北京：電子工業出版社，2009：120-121.

［18］董滿才，芮筱亭.射彈落點變異系數研究［J］.彈道學報，2005，17（4）：27-31.

［19］那曉琳，張宏緒.白細胞過氧化物傳感器的研制［J］.傳感技術學報，1995，8（2）：39-43.

［20］高連華，孫偉.變異系數在可靠性中的應用［J］.裝甲兵工程學院學報，2004，18（4）：5-8.

［21］吳媚，顧賽賽.變異系數的統計推斷及其應用［J］.銅仁學院學報，2010，12（1）：139-141.

周承松（1991-），男，江西吉安人，武漢大學本科畢業，現為國防科大研究生，從事衛星導航、控制理論、濾波相關工作，1025158324@qq.com；

王飛雪（1971-），男，福建長汀人，IEEE高級會員，教授，博士生導師，擔任某國防項目的副總工程師，第9屆全國青聯委員，主要從事衛星導航領域的研究，whunudter@whu.edu.cn。

A Kalman Filtering Method with Adaptive Constraints of Velocity Information*

ZHOU Chengsong，PENG Jing，LIU Wenxiang，WANG Feixue*
（Electronic Science and Engineering School，National University of Defense Technology，Changsha 410073，China）

In dynamic positioning，the positioning accuracy could be improved through the constraints of priori information，but the Kalman filtering with constraints of priori velocity information diverges easily owing to velocity measure errors.Thus this paper puts forward a new Kalman filtering method based on adaptive constraints of priori velocity information.It takes the velocity observations and the statistical characteristics of velocity measure errors to build the coefficient of variation，and adaptively determines whether the current velocity observations reach the requirements of restraining the filtering or not，then chooses the appropriate algorithm from the traditional Kalman filtering without constraints and the Kalman filtering with constraints of priori velocity information.The experiment results show that，the positioning accuracy of the new method is centimeter to meter better than the traditional Kalman filtering and the Kalman filtering with constraints of priori velocity information.It not only overcomes the shortcoming of the Kalman filtering with constraints of priori velocity information which is affected easily by velocity measure errors，but also improves the positioning accuracy by the appropriate velocity observations.

Kalman filtering；adaptive velocity constraints；dynamic positioning；coefficient of variation；priori information

P228.42

A

1004-1699（2016）08-1180-06

EEACC：2575；7230；763010.3969/j.issn.1004-1699.2016.08.010

項目來源：國家自然科學基金項目（61403413）

2016-04-11修改日期：2016-06-05