基于積分反饋控制的單兵組合導(dǎo)航定位算法

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

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

1 引言

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

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

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

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

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

(3)

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

(4)

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

(5)

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

(6)

3 卡爾曼濾波器基本模型

觀測模型

(7)

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

預(yù)測

(9)

更新

(12)

其中xk為n×1維狀態(tài)矢量，wk是n×1維過程噪聲矢量，yk是n×1維觀測矢量，vk是n×1維測量噪聲矢量，F(xiàn)k-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是積分項(xiàng)，vPIFKk是新的PIKF測量殘差，Ts是離散采樣時(shí)間，kp和ki是PI增益。

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

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

(17)

解上式得

(18)

(19)

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

(20)

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

(21)

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

(22)

令N=PKf可得上式的LMI

(23)

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

2) 當(dāng)?shù)祂小于最終的收斂值，且GPS有觀測值：

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

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

2.7) 修正傳感器誤差。

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

4 仿真與實(shí)驗(yàn)

圖1 位置誤差估計(jì)

圖2 速度誤差估計(jì)值

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

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

圖3 噪聲存在野值時(shí)位置誤差估計(jì)值

圖4 噪聲存在野值時(shí)速度誤差估計(jì)值

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

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

基于上述理論，為進(jìn)一步驗(yàn)證本算法的實(shí)用性，搭建如圖5所示的組合導(dǎo)航系統(tǒng)硬件平臺(tái)。

圖5 硬件平臺(tái) 圖6 實(shí)驗(yàn)場地

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

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

圖7 不同算法下的行走軌跡復(fù)現(xiàn)結(jié)果

5 結(jié)語

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