Sage-Husa自適應濾波在運動障礙物軌跡預測中的應用

孫立新，侯 陽

（河北工業大學 機械工程學院，天津 300130）

0 引言

隨著機器人技術不斷發展，人們對機器人與環境的交互能力的要求也不斷提高。機器人在復雜的工作環境下，需要對接收到的障礙物的運動信息作出分析，從而安全準確地避開障礙物完成工作，因此對運動障礙物的軌跡預測是機器人智能化的關鍵技術。運動障礙物軌跡預測包含狀態獲取、多步迭代、矯正優化等機制[1]，卡爾曼濾波（KF）作為一種線性最優的濾波方法[2]，在目標預測方面具有計算量小、便于實時處理、預測效果較好等優點，近年來被廣泛應用于各個領域[3～5]。

然而該算法沒有考慮到實際情況下系統噪聲的實時變化，因此隨著研究的深入，出現了許多自適應卡爾曼濾波算法（AKF），如新息自適應估計卡爾曼濾波算法、模糊自適應濾波法算法、貝葉斯估計法、極大似然法等[6]。旨在濾波過程中對噪聲的變化作出預估與更新，從而對預測值作出更加精確的修正，提高濾波精度。本文采用Sage-Husa自適應濾波方法實現對運動障礙物的軌跡預測，該方法計算簡便、步驟清晰、更加滿足實際需要，其實驗結果具有普遍意義。

1 狀態空間模型建立

建立被測物體離散時刻下線性隨機系統狀態空間模型，它由狀態方程和觀測方程組成。狀態方程描述了物體在兩個時刻間的狀態變化，觀測方程描述了實際觀測量與狀態變量之間的關系：

式中：k—離散時間；Xk—被測物體在時刻的狀態向量；A—狀態轉移矩陣；B—控制增益矩陣；Uk被測物體在時刻的控制輸入；Zk—被測物體在時刻的觀測值；H—觀測矩陣；Wk、Vk分別表示系統的狀態噪聲和觀測噪聲，它們都是互相獨立的高斯白噪聲，設噪聲均值分別為qk、rk，方差為Qk、Rk，則可表示為：

2 Sage-Husa自適應濾波

如果噪聲均值為qk、rk為0，且噪聲方差Qk、Rk均為已知，則卡爾曼濾波公式如下所示，它是一種基于遞推線性最小方差估計準則的最佳估計，其過程分為預測過程和修正過程，預測過程是根據上一時刻的被測物體的狀態與協方差進行推測，以此預測出這一時刻的狀態，得到被測物體狀態的先驗估計；修正過程以觀測數據作為反饋，對預測值進行修正，獲得被測物體的后驗估計。

預測過程：

修正過程：

然而在實際情況下，噪聲均值qk、rk并不一定為0，且qk、rk、Qk、Rk部分或者全部未知并隨時間不斷變化，這對于卡爾曼濾波的預測結果產生了干擾。Sage-Husa自適應濾波在卡爾曼濾波預測-修正過程的基礎上，增加了實時運用觀測更新的數據對狀態噪聲和觀測噪聲進行動態的統計估計[7]，更新噪聲均值與方差qk、rk、Qk、Rk，從而減少噪聲的變化對預測結果帶來的影響。其基本算法如下所示：

使用Sage-Husa自適應濾波預測物體運動軌跡的基本步驟如下：首先對被測物體建立狀態空間模型，根據運動學方程確定狀態轉移矩陣A和觀測矩陣H，根據控制輸入變量確定控制增益矩陣B；其次經過自適應濾波的預測過程，根據上一時刻的狀態向量Xk-1和協方差Pk-1預測這一時刻的狀態向量Xk|k-1和協方差Pk|k-1；再加入被測物體的觀測數據Zk，將預測出的Xk|k-1，Pk|k-1修正為Xk，Pk；最后將狀態噪聲和觀測噪聲的均值與方差qk-1、rk-1、Qk-1、Rk-1，更新為qk，rk，Qk，Rk，為下一次迭代做準備。完成上述一個周期的迭代，可得到被測物體在時刻位置的單步預測。只要將所得到的Xk，Pk，qk，rk，Qk，Rk，作為下一時刻的初始值，進行多次迭代，最終可以得到被測物體運動的預測軌跡。

3 運動障礙物軌跡預測

3.1 運動障礙物狀態模型建立

模擬障礙物在XOY平面內運動，運動中的位移，速度，加速度記為p，v，a，它們隨時間的變化不斷變化。在任意一個時刻內可將障礙物的運動分解為沿X方向和沿Y方向的直線運動，其位移記為px，py；速度記為vx，vy；加速度記為ax，ay，其運動學分解如圖1所示。

圖1 障礙物運動學分解

建立障礙物的運動狀態變化：狀態向量Xt-1從t-1時刻到t時刻間的時段Δt內沿X，Y方向分別以加速度ax(t-1)、ay(t-1)轉變到下一個狀態向量Xt，則運動障礙物在Δt內運動的狀態方程可描述為：

因最終要實現軌跡預測，應選取X，Y方向的位移作為觀測值，所以觀測矩陣H為：

狀態噪聲Wt與觀測噪聲Vt為實際預測過程中引入的噪聲，其均值qt、rt和方差Qt、Rt根據實際情況確定。

3.2 運動障礙物軌跡預測仿真

設運動障礙物起始位置為（0，0），初始速度vx=3m/s，vy=6m/s，傳感器采樣時間間隔為Δt=0.5s，采樣次數為120次，運動總時長T=0.5×120=60s。則初始狀態向量為：

該模型的控制輸入為ax，ay。為檢驗在障礙物的運動狀態不斷變化過程中Sage-Husa自適應濾波的預測效果，取ax，ay，為振幅、頻率互不相同的弦函數輸入，其圖像如圖2、圖3所示，橫坐標代表采樣次數：

圖2 X方向加速度輸入

圖3 Y方向加速度輸入

仿真給出了障礙物運動過程的理想軌跡，傳感器傳回的觀測軌跡，卡爾曼濾波預測軌跡和Sage-Husa自適應濾波的預測軌跡。綜合以上四條軌跡的比較與分析，即可得到對Sage-Husa自適應濾波預測效果的評價，取遺忘因子b=0.99。

圖4中顯示了觀測軌跡和兩種算法預測軌跡都能以理想軌跡為中心進行波動。從圖上可以看出，不加入濾波算法的情況下，僅靠傳感器傳回的數據擬合運動軌跡受噪聲影響較大，軌跡呈不規則鋸齒狀，預測精度很低。而卡爾曼濾波算法和Sage-Husa自適應濾波算法對障礙物運動狀態的變化都具有很高的預測精度，其預測軌跡更加光滑、平整。證明了兩種算法不僅在理論上能很好的對運動障礙物進行軌跡預測，而且具有實際的可行性。圖5展示了運動軌跡曲線的局部放大圖，對比卡爾曼濾波算法和Sage-Husa自適應濾波算法預測軌跡可知，Sage-Husa自適應濾波預測軌跡更加貼合理想軌跡，其預測效果更好。

圖4 運動障礙物軌跡預測

圖5 局部放大圖

3.3 誤差分析

為定量分析Sage-Husa自適應濾波的預測精度，引入同一時刻下觀測軌跡點、卡爾曼濾波預測軌跡點、Sage-Husa自適應濾波預測軌跡點到理想軌跡點的幾何誤差[9]，對誤差進行比較，得到并分析誤差分布的統計特性。該誤差采用歐式距離計算公式求出：

其中(x，y)為t時刻理想軌跡點的坐標；(x′，y′)為t時刻觀測軌跡點的坐標；(x"，y")為t時刻卡爾曼濾波預測軌跡點的坐標；(x'''，y''')為t時刻Sage-Husa自適應濾波預測軌跡點的坐標；RMS1表示觀測誤差；RMS2表示卡爾曼濾波預測誤差；RMS3表示Sage-Husa自適應濾波預測誤差，其結果如圖6所示，橫坐標代表采樣次數：

圖6 誤差分析

三組誤差的統計特性如表1所示：

表1 誤差統計特性

從表1可以得到，卡爾曼濾波預測誤差與Sage-Husa自適應濾波預測誤差最大值分別為3.1371、2.3921，相比觀測誤差最大值減少了66.40%、74.38%；誤差均值分別為1.7928、1.0372，相比觀測誤差均值減少了45.22%、68.31%，說明Sage-Husa自適應濾波比卡爾曼濾波在降低噪聲對預測過程帶來的誤差中有更顯著的作用，其軌跡預測精度更高。而Sage-Husa自適應濾波預測誤差方差為0.3260，小于卡爾曼濾波預測誤差方差0.4608，說明Sage-Husa自適應濾波相比卡爾曼濾波預測過程更加穩定，不易產生突變，預測軌跡更加平滑。

4 結語

本文針對機器人在復雜工作環境下的避障和傳感器的觀測值存在噪聲等問題，提出使用具有時變性能的噪聲估計器的Sage-Husa自適應濾波對運動障礙物的運動軌跡進行預測。從理論分析、運動狀態建模、仿真實驗、誤差分析等方面完整闡述了該方法的可行性。相比傳統卡爾曼濾波，Sage-Husa自適應濾波具有噪聲處理更好，預測精度更高，預測軌跡更平滑等特點。且對于障礙物運動狀態的不斷變化也具有很強的適應性，為機器人實現自主避障功能提供了可靠信息。但該方法仍有其局限性，例如該方法不適用于障礙物運動狀態為非線性的系統等，這些問題有待進一步的研究論證。