改進(jìn)的Kalman融合濾波算法在無(wú)人機(jī)避障系統(tǒng)中的應(yīng)用分析

杜天華 徐金龍 劉博 章宇航 許繼弘

上海機(jī)電工程研究所 上海 201109

1 無(wú)人機(jī)避障

1.1 無(wú)人機(jī)避障方式

無(wú)人機(jī)避障方式多種多樣，主要包括機(jī)器視覺(jué)避障、激光超聲傳感器融合避障等，最近張京娟等提出了根據(jù)飛行器間測(cè)距信息對(duì)飛行器慣導(dǎo)定位進(jìn)行校正的方法[1]。機(jī)器視覺(jué)穩(wěn)定性較差不能適應(yīng)多變的飛行環(huán)境，基于成本考慮激光超聲傳感器融合避障應(yīng)用較廣。單一的傳感器存在較大的不穩(wěn)定性，無(wú)法滿足飛行階段障礙物測(cè)距的精度要求，需要采用多數(shù)據(jù)源融合技術(shù)，獲得相對(duì)精確的信息數(shù)據(jù)，得到可靠測(cè)距結(jié)果。針對(duì)無(wú)人機(jī)傳感器來(lái)說(shuō)，信息融合算法較多，包括自適應(yīng)Kalman濾波和擴(kuò)展Kalman濾波，其中，自適應(yīng)Kalman濾波較為簡(jiǎn)單有效。為了提升無(wú)人機(jī)穩(wěn)定性，需要從信息融合算法入手，對(duì)Kalman濾波算法適當(dāng)改進(jìn)。

1.2 無(wú)人機(jī)避障系統(tǒng)

現(xiàn)階段的無(wú)人機(jī)已經(jīng)不再局限單一領(lǐng)域，除了軍事領(lǐng)域外，還廣泛應(yīng)用于民用，如何在復(fù)雜環(huán)境中，較為精準(zhǔn)到達(dá)預(yù)定目標(biāo)，是業(yè)界討論的焦點(diǎn)。現(xiàn)實(shí)中，需要對(duì)飛行路徑完整優(yōu)化，借助細(xì)致算法，發(fā)揮自主避障功能，在此前提下，提升無(wú)人機(jī)應(yīng)用可靠性。研究發(fā)現(xiàn)，動(dòng)態(tài)自主避障模型作用顯著，可以將碰撞判斷問(wèn)題分解和轉(zhuǎn)化，變更為一元二次方程，合理推算出最小碰撞時(shí)間，這樣便可以給實(shí)際飛行速度提供參考，遵循盡可能指向目標(biāo)的避障原則，獲得期望的運(yùn)動(dòng)速度（無(wú)人機(jī)）[2]。需要注意的是，由于引入權(quán)重因子后，會(huì)改變自身套件，增加過(guò)載約束，減少速度判斷時(shí)間（最優(yōu)機(jī)動(dòng)速度）。

2 改進(jìn)的Kalman濾波算法

在經(jīng)濟(jì)的全方位帶動(dòng)下，飛行器技術(shù)相對(duì)成熟，飛行器品牌越來(lái)越多，在這樣的背景下，四旋翼飛行器逐漸普及，慢慢進(jìn)入大眾視野。但是該飛行器的應(yīng)用，存在較大安全隱患，撞機(jī)、墜機(jī)等事故概率較高，為了提高應(yīng)用水平，徹底改善飛行器避障系統(tǒng)問(wèn)題，可以引進(jìn)Kalman濾波改良算法，提高機(jī)身安全性。

Kalman濾波的基本理論：結(jié)合最小均方差法則，確定狀態(tài)方程和測(cè)量方程，根據(jù)觀測(cè)值和預(yù)測(cè)值完成數(shù)據(jù)實(shí)時(shí)迭代，更新模型中的參數(shù)，得出最佳估計(jì)值。

無(wú)人機(jī)距離為一維數(shù)據(jù)量，其狀態(tài)轉(zhuǎn)移方程和測(cè)量方程可以描述為：

方程式中： 為狀態(tài)向量； 為觀測(cè)向量；A為狀態(tài)轉(zhuǎn)移矩陣；B為控制矩陣。除此之外，H為觀測(cè)矩陣；w則表示過(guò)程激勵(lì)噪聲； 為觀測(cè)噪聲。由Kalman濾波得基本假設(shè)可知：設(shè)

式中：Q表示過(guò)程激勵(lì)協(xié)方差矩陣； R則指觀測(cè)噪聲協(xié)方差矩陣；K為卡爾曼系數(shù)。K是Kalman濾波中最重要的參數(shù)，K決定了測(cè)量值和預(yù)測(cè)值在最終估計(jì)值中的比重，R越小則證明測(cè)量值越精確，相應(yīng)的測(cè)量值在最終估計(jì)值中的比重也越大。在實(shí)際測(cè)量中，相關(guān)參數(shù)Q、R、q、r等會(huì)實(shí)時(shí)變化，不斷更新，并且想要準(zhǔn)確獲取難度較大，基于這樣的前提，建立的模型很難確保準(zhǔn)確性，與實(shí)際模型相差過(guò)大，導(dǎo)致傳感器測(cè)距效果并不理想，因此如何得到準(zhǔn)確的估計(jì)R值，同時(shí)使實(shí)際的R值盡可能小是提高測(cè)距精度的關(guān)鍵。采用改進(jìn)的Sage_Husa自適應(yīng)Kalman濾波可以降低R值不確定性帶來(lái)的影響。

式中：b為遺忘因子，kd為由此得到的加權(quán)系數(shù)。b取值范圍為0~1，可見(jiàn)隨著濾波進(jìn)行，加權(quán)系數(shù)kd越來(lái)越小，kR更多地取決于 1kR?的取值，這一過(guò)程的快慢由遺忘因子b的取值決定。事實(shí)上遺忘因子的取值受很多因素的影響，可以通過(guò)實(shí)驗(yàn)實(shí)際測(cè)量得到不同情況下的最佳取值。

為了提高測(cè)距的可靠性，需要對(duì)傳統(tǒng)算法進(jìn)行改進(jìn)，借助自適應(yīng)濾波，完成模型不斷修改、更新，從而使估計(jì)值收斂，通過(guò)算法的優(yōu)化，可以得到理想濾波結(jié)果。結(jié)合實(shí)踐經(jīng)驗(yàn)可知，常規(guī)的Kalman濾波，完成每次狀態(tài)量預(yù)估，都要花費(fèi)較多時(shí)間，參數(shù)Q、R、q、r在實(shí)際應(yīng)用中，采用的是前一環(huán)節(jié)的數(shù)值，因此四個(gè)量存在滯后性，這就會(huì)讓測(cè)距的準(zhǔn)確度降低，產(chǎn)生較大的誤差值，嚴(yán)重影響后驗(yàn)估計(jì)質(zhì)量，甚至引起發(fā)散。為了改變現(xiàn)狀，提高后驗(yàn)估計(jì)質(zhì)量，將Kalman濾波法和Sage_ Husa法進(jìn)行融合，可以根據(jù)加權(quán)系數(shù)進(jìn)行高質(zhì)量的參數(shù)更正，從而強(qiáng)化避障效果。

3 改進(jìn)的Kalman濾波器設(shè)計(jì)

隨著飛行器應(yīng)用領(lǐng)域擴(kuò)展，為了提高飛行器性能，完善其躲避障礙的功能，提高避障能力，需要基于改進(jìn)的Kalman濾波算法，對(duì)濾波器進(jìn)行優(yōu)化，確保濾波器性能穩(wěn)定，借此增強(qiáng)無(wú)人機(jī)避障能力。

以無(wú)人機(jī)測(cè)距這一場(chǎng)景為例，在傳感器內(nèi)部信息融合時(shí)，需要輸入超聲波或者是激光傳感器測(cè)距信息，二者測(cè)量值為同一量綱，想要實(shí)現(xiàn)兩類(lèi)信息的融合，就要改變其中一個(gè)的量綱。在實(shí)際應(yīng)用中，我們將狀態(tài)值設(shè)定為x，并且將激光測(cè)距采樣值的導(dǎo)數(shù)設(shè)為u，作為濾波器的輸入量，將超聲波測(cè)距采樣值設(shè)為z，可得狀態(tài)空間方程以及應(yīng)用價(jià)值較高的測(cè)量方程：為采樣頻率。除此之外， wu與 wh分別代表激光和超聲波傳感器的測(cè)量噪聲。

式中： ui為激光測(cè)距導(dǎo)數(shù)值速度；hi為超聲波測(cè)距值；f

為便于該方程式的實(shí)現(xiàn),現(xiàn)取 x = [h,b]T，h代表距離量，b為常數(shù)，也就是在實(shí)際測(cè)量過(guò)程中，激光測(cè)距產(chǎn)生的測(cè)量偏差。實(shí)際應(yīng)用中由于天氣或者周?chē)瓷涮匦缘牟煌す鉁y(cè)距和超聲波測(cè)距的誤差會(huì)隨之改變。通過(guò)調(diào)節(jié)遺忘因子即可巧妙地改變兩種濾波器在融合后的測(cè)量噪聲R中的比例，激光測(cè)距誤差小時(shí)，b較小，使得測(cè)量噪聲R主要反應(yīng)激光測(cè)距的噪聲；反之超聲波測(cè)距效果好時(shí)，b一般偏大，這樣就可以通過(guò)融合激光和超聲波測(cè)距兩種方式來(lái)提高測(cè)量噪聲R的準(zhǔn)確性，從而降低Kalman濾波發(fā)散的可能性，提升測(cè)距精度。

從無(wú)人機(jī)融合超聲波和激光測(cè)距來(lái)提升測(cè)距精度優(yōu)化濾波輸出，我們可以得到啟發(fā)，在測(cè)量同一物理量時(shí)，都可以考慮應(yīng)用這一方法將多個(gè)測(cè)量手段綜合起來(lái)以提高整體測(cè)量的精度，在實(shí)際應(yīng)用中可以通過(guò)實(shí)驗(yàn)得到不同場(chǎng)景下合適的遺忘因子取值，這樣在遇到特定場(chǎng)景時(shí)即可靈活切換，自適應(yīng)改變Kalman參數(shù)，優(yōu)化濾波器性能。

在Kalman濾波器設(shè)計(jì)中，需要對(duì)原Kalman濾波器進(jìn)行功能上的完善與性能上的優(yōu)化，由于原Kalman濾波器使用受到限制，經(jīng)常狀態(tài)估計(jì)不準(zhǔn)，甚至在實(shí)際應(yīng)用中，會(huì)出現(xiàn)發(fā)散現(xiàn)象。為了避免此類(lèi)問(wèn)題,需要持續(xù)優(yōu)化原有的Kalman濾波器，在原有性能結(jié)構(gòu)基礎(chǔ)上進(jìn)行改良，實(shí)際操作中，借助簡(jiǎn)化的Sage_ Husa法，完成對(duì)原濾波器的性能修正[3]。實(shí)踐證明，改進(jìn)的Sage_ Husa濾波器性能穩(wěn)定，在原濾波器中引入了加權(quán)系數(shù)d（遺忘因子的），通過(guò)這樣的改進(jìn)，可以提升濾波器穩(wěn)定性，使其隨迭代變化，在測(cè)距方面更加精準(zhǔn)，從而強(qiáng)化濾波器的應(yīng)用效果，為避障系統(tǒng)功能發(fā)揮提供保障。

4 實(shí)例應(yīng)用分析

隨著無(wú)人機(jī)用途的增多，應(yīng)用領(lǐng)域不斷擴(kuò)張，無(wú)人機(jī)可以輔助完成高難度的信息收集以及監(jiān)測(cè)工作，運(yùn)行效率較高。現(xiàn)實(shí)應(yīng)用中，想要完善無(wú)人機(jī)的功能，就必須從避障系統(tǒng)優(yōu)化入手，通過(guò)增強(qiáng)避障功能，確保無(wú)人機(jī)性能穩(wěn)定。超聲波測(cè)距避障系統(tǒng)在現(xiàn)實(shí)應(yīng)用中，分為三個(gè)步驟：①借助模塊測(cè)量距離（空間陣列超聲波模塊）對(duì)前方的障礙物精準(zhǔn)判斷。其原理是首先利用電調(diào)，將其作為最為關(guān)鍵的供電模塊，在此前提下，使用芯片驅(qū)動(dòng)超聲波完成想要的距離探測(cè)，這是最為關(guān)鍵的步驟，不容忽視。模塊上電前，需要確保模塊處于理想的串口觸發(fā)模式，只有這樣，才能讓輸入信號(hào)立即發(fā)揮作用，發(fā)出超聲波脈沖，基礎(chǔ)搭建完成后，便可以進(jìn)行回波信號(hào)檢測(cè)。在整個(gè)過(guò)程中，會(huì)對(duì)溫度值進(jìn)行測(cè)量，結(jié)合實(shí)際溫度需求進(jìn)行校正，最終保證輸出距離結(jié)果的精準(zhǔn)。②飛控中的芯片作用顯著，根據(jù)接收數(shù)據(jù)以及科學(xué)設(shè)定的距離，可以提出最佳決策方案。③最后一步是根據(jù)操作指令（現(xiàn)實(shí)中操作員發(fā)出的）以及相關(guān)報(bào)警信號(hào)，作出合理的綜合決策。研究發(fā)現(xiàn)，在實(shí)際操作中，人員的操作指令不容忽視，往往會(huì)和報(bào)警信號(hào)一起，穿越組合的傳輸門(mén)，完成最后的決策，實(shí)際操作中，需要結(jié)合現(xiàn)實(shí)需求，針對(duì)不同安全距離，制定合理性較高的控制策略，在此基礎(chǔ)上，發(fā)揮智能的綜合避障作用。

通過(guò)上述分析了解到，無(wú)人機(jī)避障系統(tǒng)功能強(qiáng)大，涉及內(nèi)容較多，想要實(shí)現(xiàn)避障系統(tǒng)的優(yōu)化，必須進(jìn)行精準(zhǔn)的測(cè)量，對(duì)原有的算法進(jìn)行完善，將Kalman濾波法和Sage_ Husa法進(jìn)行融合，實(shí)踐表明，該算法的融合意義非凡，可以根據(jù)加權(quán)系數(shù)完成優(yōu)質(zhì)的參數(shù)更正，從而增強(qiáng)避障系統(tǒng)功能，強(qiáng)化避障效果，消除無(wú)人機(jī)飛行的隱患。現(xiàn)實(shí)中的主要方法是：在算法中合理引進(jìn)遺忘因子，在此基礎(chǔ)上進(jìn)行校正，將其和Kalman濾波算法融合，得出最佳的計(jì)算方案，完成對(duì)Kalman的簡(jiǎn)化，借助算法的改進(jìn)，增強(qiáng)避障系統(tǒng)的適應(yīng)性，為無(wú)人機(jī)應(yīng)用效果改善助力，提高其操作水平。

5 結(jié)束語(yǔ)

綜上所述，在現(xiàn)實(shí)應(yīng)用中，無(wú)論是激光傳感器還是較為常見(jiàn)的超聲波傳感器，當(dāng)進(jìn)行不同障礙物的測(cè)距時(shí)，都會(huì)出現(xiàn)一定測(cè)量誤差，并且測(cè)距結(jié)果非常不穩(wěn)定，為了改善這方面的問(wèn)題，可以將先進(jìn)的Sage_Husa自適應(yīng)法與目前使用的Kalman濾波器進(jìn)行融合，完成相關(guān)參數(shù)的優(yōu)化。現(xiàn)實(shí)表明，兩者的數(shù)據(jù)融合具有一定前瞻性，可以有效改善傳統(tǒng)數(shù)據(jù)采集不穩(wěn)定的問(wèn)題，提高數(shù)據(jù)采集精度，保持穩(wěn)定測(cè)量結(jié)果，在此基礎(chǔ)上降低測(cè)量誤差，為無(wú)人機(jī)性能提升奠定基礎(chǔ)。