田 健，劉 宇，路永樂*，吳 英

(1.重慶郵電大學光電工程學院，重慶 400065；2.重慶科技學院智能技術與工程學院，重慶 400065)

1 引言

精度是對隨機誤差的描述，是統(tǒng)計變異性的度量。準確度用來表示系統(tǒng)誤差的大小，是對統(tǒng)計偏差的一種度量[1]。而大多數傳感器數據融合中存在著無法獲得真實值的問題。因此，融合濾波器嘗試最小化估計方差并將估計值收斂到精確結果作為真實值的參考值?？柭鼮V波器(Kalman Filter，KF)便是該類型的濾波器之一。在經典的控制學中，著名的比例積分控制器(Proportional Integral Controller)可以在系統(tǒng)中實現穩(wěn)態(tài)精度，簡單來說，PI控制器主要有以下兩種作用：比例調節(jié)作用：按比例反應系統(tǒng)的偏差，系統(tǒng)一旦出現了偏差，比例調節(jié)立即產生調節(jié)作用用以減少偏差。比例作用大，可以加快調節(jié)，減少誤差。積分調節(jié)作用：使系統(tǒng)消除穩(wěn)態(tài)誤差，提高無誤差度。因為有誤差，積分調節(jié)就至無差，積分作用常與另兩種調節(jié)規(guī)律結合，組成PI調節(jié)器或PID調節(jié)器[2]。Bas等人提出的基于比例調節(jié)作用的Kalman濾波器，該濾波器中通過黎卡提方程算出比例積分和卡爾曼增益，從而求出最小誤差方差[3]。June提出了一種基于線性矩陣不等式(LMI)的LI-KF，該算法常用于邊界不確定的隨機線性系統(tǒng)[4]。

由于傳統(tǒng)的觀測器與估計器只有一個估計誤差的比例環(huán)不同，PI狀態(tài)估計器具有額外的積分反饋回路，該回路以與PI控制方案類似的方式提高了穩(wěn)態(tài)估計的準確性，并提高了針對干擾，未知輸入或建模錯誤的估計魯棒性。這得益于設計估計器時附加的積分自由度。因此，該設計方法可簡單概括為嘗試增加主系統(tǒng)的積分狀態(tài)，并通過以減少誤差協(xié)方差為重點來計算估計器新的增益。該思想中，估計器嘗試優(yōu)化協(xié)方差而不是穩(wěn)態(tài)誤差，這種方法通常被稱為自適應增量卡爾曼濾波器(AIKF)[5]。

本文提出了一種基于該思想改進的AIKF算法，稱為PI卡爾曼濾波器(PIKF)。KF是一種估計最小方差的估計器，AIKF有類似PI控制的積分項，但在AIKF中，該積分項只能計算增廣系統(tǒng)的卡爾曼增益，因此KF和AIKF算法的基本思想都可概括為協(xié)方差最小化。而PIKF也擴展了積分項，且積分項和主系統(tǒng)獨立工作。在PIKF中，用獨立的經典控制設計方法計算了無積分主系統(tǒng)的最優(yōu)Kalman增益和增廣系統(tǒng)的比例和積分增益，有效的提升了系統(tǒng)的精度和準確度。通過KF和PIKF之間的仿真結果比較顯示了PIKF能夠如何有效地減少穩(wěn)態(tài)誤差以及在時域和頻域中的誤差。

2 穩(wěn)態(tài)誤差模型

穩(wěn)態(tài)誤差是系統(tǒng)從一個穩(wěn)態(tài)過渡到新的穩(wěn)態(tài)，或系統(tǒng)受擾動作用又重新平衡后，系統(tǒng)出現的偏差。穩(wěn)態(tài)誤差記作(Steady-State Errors)。通常影響的因素有這三種，系統(tǒng)自身的結構參數，外作用的類型如控制量、擾動量及作用點，以及外作用的形式(階躍、斜坡或加速度等)[6]。其計算如式(1)所示

(3)

靜態(tài)位置誤差系數

(4)

靜態(tài)速度誤差系數

(5)

靜態(tài)加速度誤差系數

(6)

3 卡爾曼濾波器基本模型

觀測模型

(7)

初值：x0和誤差協(xié)方差P0；

預測

(9)

更新

(12)

其中xk為n×1維狀態(tài)矢量，wk是n×1維過程噪聲矢量，yk是n×1維觀測矢量，vk是n×1維測量噪聲矢量，Fk-1是矩陣A的離散形式，Hk是m×1維的觀測矢量形式，Qk為n×n維噪聲協(xié)方差矩陣，Rk為m×m維測量噪聲協(xié)方差矩陣。

(13)

δvPIKFk=kpδvpk+δvik-1

(14)

(15)

δvik=δvik-1+kiTsδvpk

(16)

其中vpk為測量殘差，vik是積分項，vPIFKk是新的PIKF測量殘差，Ts是離散采樣時間，kp和ki是PI增益。

3.1 基于李雅普諾夫穩(wěn)定性的PI控制器設計

修改式13，14，15，16以及式8，有

(17)

解上式得

(18)

(19)

盡管上式可以確保系統(tǒng)的穩(wěn)定性，但其收斂性卻無法進行驗證。為了確保收斂性，可將收斂邊界條件修改成-2σP[7]，則式19可修改為

(20)

由文獻[8]中的結論可證明，用式17計算的增益矩陣可使得xk以高于σ的速率收斂到零。由于變量矩陣Kf和正定的實對稱矩陣P相乘，所以式19為矩陣線性不等式(LMI)。

(21)

在上式左右兩側乘以diag(In，P)得

(22)

令N=PKf可得上式的LMI

(23)

1) 初始化初值x0，P0，Q0，R0；

2) 當迭代值k小于最終的收斂值，且GPS有觀測值：

2.3) 設σ=0.01，解式23求得PI控制器增益kp和ki；

2.4) 構造PIKF算法的殘差項，如式11所示以及計算如式10所示的狀態(tài)更新量xk；

2.7) 修正傳感器誤差。

此算法的步驟2.4和2.5是PIKF所特有的，顯示了PIKF如何修改了傳統(tǒng)的KF狀態(tài)更新過程，且PI控制器則保證了估計的穩(wěn)定性和收斂性，這種對KF的優(yōu)化有效減小了穩(wěn)態(tài)誤差。

4 仿真與實驗

圖1 位置誤差估計

圖2 速度誤差估計值

圖1與圖2所示為在上述所設環(huán)境下，PIKF濾波器的濾波誤差圖，并與未進行濾波的情形作對比。圖2為算法濾波后的位置誤差估計值，圖2為速度誤差估計值，不難發(fā)現，無論是位置估計還是速度估計，未經濾波處理得組合會一定時間后會出現發(fā)散。而PIKF濾波器，其觀測量的噪聲統(tǒng)計特性未知，且隨時間發(fā)生變化，但卻有著較好的濾波精度，誤差值較小，依然可以進行較好的估計。

參考文獻[12]提出的方法將噪聲設置為：v～(1-ρ)N(0，∑n)+ρf，噪聲野值的分布為f～N(0，10∑n)，ρ=0.2表示野值出現的概率，圖3與圖4展示了噪聲存在野值的情況下，KF與PIKF算法的位置誤差及速度誤差情況。如圖所示，在加入噪聲以后，由于卡爾曼濾波器自身的局限性，在量測噪聲統(tǒng)計特性非恒定的情況下，誤差顯著提升。而基于PI控制器的PIKF濾波器在穩(wěn)態(tài)誤差以及瞬時誤差上都具有較好的濾波特性，其速度與位置上的漂移小于KF。

圖3 噪聲存在野值時位置誤差估計值

圖4 噪聲存在野值時速度誤差估計值

兩種算法穩(wěn)態(tài)誤差情況對比如下表所示，可看出PIKF可有效減小穩(wěn)態(tài)誤差。

表1 KF PIKF穩(wěn)態(tài)誤差對比

基于上述理論，為進一步驗證本算法的實用性，搭建如圖5所示的組合導航系統(tǒng)硬件平臺。

圖5 硬件平臺 圖6 實驗場地

其中，GPS模塊采用瑞士u-blox公司的NEO-6M-0-001模塊，并將其集成于重慶郵電大學單兵導航定位系統(tǒng)中來進行實驗測試。實驗場地設置在如圖6所示的重慶郵電大學風華操場，人員行走路徑為A-B-C-D-E-A。為驗證算法的有效性，總共設置三組實驗，第一組為純慣導組；第二組為基于經典KF融合定位算法組；第三組為本文提出的組合導航定位算法組。每組實驗前載入相應的定位算法。

圖7為上述情況下三種運動軌跡復現結果，可以看出，純慣導定位方式誤差較大，在實際行走過程中已偏離了實際坐標，且隨著行走距離的增加，誤差逐漸增加；而基于KF算法的定位系統(tǒng)雖相較于純慣導組有了較好的改善，但仍存在一定范圍的誤差波動，且在回到原點時坐標誤差較大；而改進后的PIKF定位算法，實際平均定位精度相較于經典KF定位平均精度提升約27%，相較于純慣導定位平均精度提升45%。

圖7 不同算法下的行走軌跡復現結果

5 結語

本文針對組合導航系統(tǒng)中估計與觀測受到野值和變化的觀測噪聲影響等問題，利用經典控制學中的PI控制器來替代KF濾波器中的殘差項，并基于李雅普諾夫穩(wěn)定性和收斂性思想來優(yōu)化PI增益的計算過程。通過大量的仿真證明了PIKF算法的效能，驗證了該算法不僅嘗試保留了KF最小方差估計特性，還可有效減少穩(wěn)態(tài)誤差，改進組合系統(tǒng)的位置和速度估計，有效的提高了導航精度，在工程上較有應用價值。