[發明專利]基于Chebyshev正交多項式擴展RTS Kalman平滑方法

有效

• 申請號: 201810309568.X
• 申請日: 2018-04-09
• 公開（公告）號: CN108681621B
• 公開（公告）日: 2021-11-19
• 發明（設計）人: 丁國強;張煥龍;婁泰山;楊存祥;張鐸;王曉雷;方潔
• 申請（專利權）人: 鄭州輕工業學院
• 主分類號: G06F30/20
• 分類號: G06F30/20;G06F111/10
• 代理公司: 鄭州優盾知識產權代理有限公司 41125
• 代理人: 孫詩雨;栗改

本發明提出了一種基于Chebyshev正交多項式擴展RTS Kalman平滑方法，用以解決傳統的平滑算法無法對非線性系統狀態變量開展濾波后平滑操作的問題。本發明建立SLAM系統的非線性的狀態模型；對非線性SLAM系統的狀態變量參數Kalman濾波；基于Chebyshev多項式擬合逼近SLAM系統實施Chebyshev多項式逼近計算操作，計算平滑算法的預測均值、預測方差矩陣和協方差矩陣；獲取非線性系統方程的Chebyshev多項式擬合逼近計算的平滑均值及平滑方差矩陣；根據估計數據開展Chebyshev多項式RTS平滑計算。本發明利用Chebyshev多項式擬合SLAM系統的模型方程，實現狀態向量的濾波平滑計算，具有較好的計算優勢和計算效能。

技術領域

本發明涉及導航制導與控制中的航空航天系統信息處理的技術領域，尤其涉及一種基于Chebyshev正交多項式擴展RTS(Rauch-Tung-Striebel)Kalman平滑方法，對運動載體的即時定位與地圖構建(Simultaneous Localization And Mapping，SLAM)SLAM問題進行平滑處理。

背景技術

根據估計時刻用到的觀測量信息情況，最優估計理論分為預測、濾波和平滑三種類型，其中濾波計算是利用當前時刻以及以前時刻的所有觀測信息對當前系統狀態變量進行估計計算，而平滑方法除了利用濾波計算用到的觀測信息，還要用到當前時刻以后的部分或者所有觀測信息。因此，理論上平滑方法是一種離線處理方法，在濾波計算基礎上對系統狀態變量進一步做出改善，從而獲得更加精確的計算數據結果。

平滑算法和濾波算法一樣，其理論基礎也是基于Bayesians最優濾波理論，假設系統狀態變量滿足高斯分布。平滑估計算法一般可分為固定點平滑、固定滯后平滑和固定區間平滑等，其中固定區間平滑是利用某一時間區間內的所有觀測信息對所有狀態變量進行估計計算的一種方法，其應用范圍最為廣泛，而Rauch-Tung-Striebel平滑算法就是一種固定區間平滑計算方法，但是傳統的平滑算法都是針對線性系統狀態變量開展濾波后的平滑操作，但是近年來也有很多種非線性平滑算法，如其中應用最廣泛的基于Taylor級數擴展表達式的一階或者二階RTS非線性平滑算法，被稱為擴展RTS平滑算法，以及基于Sigma點非線性RTS平滑算法、中心差分RTS平滑算法、Gauss-Hermite數值積分逼近的RTS平滑算法和容積RTS平滑算法等。

發明內容

針對傳統的平滑算法都是針對線性系統狀態變量開展濾波后平滑操作的技術問題，本發明提出一種基于Chebyshev正交多項式擴展RTS Kalman平滑方法，將Chebyshev正交多項式的優異性質應用于SLAM問題系統設計中，實現SLAM系統非線性模型狀態參數最優濾波平滑計算，具有較好的計算優勢和計算效能。

為了達到上述目的，本發明的技術方案是這樣實現的：一種基于Chebyshev正交多項式擴展RTS Kalman平滑方法，其步驟如下：

步驟一：建立SLAM系統的非線性的狀態模型，包括狀態方程和非線性觀測方程；

步驟二：對非線性SLAM系統狀態模型的狀態變量參數的Kalman濾波計算，經由k＝T步迭代計算獲得第T步的最優濾波結果，并存儲各個時刻的估計數據；

步驟三：從k＝T開始，對步驟二獲取的各個時刻濾波的估計數據開展逆向平滑操作，基于Chebyshev多項式擬合逼近SLAM系統的非線性狀態方程，實施Chebyshev多項式逼近計算操作，根據第T步濾波數據，計算平滑算法的預測均值、預測方差矩陣和協方差矩陣；

步驟四：計算Chebyshev多項式擬合逼近計算的平滑增益，獲取非線性系統方程的Chebyshev多項式擬合逼近計算的平滑均值及平滑方差矩陣；