[發明專利]一種面向INS/WSN室內移動機器人緊組合導航的迭代擴展RTS均值濾波方法

有效

• 申請號: 201310447155.5
• 申請日: 2013-09-26
• 公開（公告）號: CN103471595A
• 公開（公告）日: 2013-12-25
• 發明（設計）人: 陳熙源;李慶華;徐元;高金鵬
• 申請（專利權）人: 東南大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 南京瑞弘專利商標事務所(普通合伙) 32249
• 代理人: 楊曉玲

1.一種面向INS／WSN室內移動機器人緊組合導航的迭代擴展RTS均值濾波方法，其特征在于，包括下列步驟：

步驟1)，首先通過迭代擴展卡爾曼濾波將INS、WSN在本地相對坐標系中獲取的導航信息進行數據融合：

構建迭代擴展卡爾曼濾波器的系統方程，所述迭代擴展卡爾曼濾波的系統方程以INS每一時刻在本地相對坐標系中東向和北向的位置、速度和加速度誤差作為狀態變量，系統方程如式(1)所示：

其中，δP_E，k為k時刻的東向位置誤差，δP_N，k為k時刻的北向位置誤差，δV_E，k為k時刻的東向速度誤差，δV_N，k為k時刻的北向速度誤差，δAcc_E，k為k時刻的東向加速度誤差，δAcc_N，k為k時刻的北向加速度誤差，T為采樣周期，ω為系統噪聲；

所述迭代擴展卡爾曼濾波的觀測方程以每一時刻INS和WSN各自測量的未知節點與參考節點之間的距離的平方差和速度差作為觀測量，觀測方程如式(2)所示：

其中，Δvx為INS和WSN測量得到的東向速度之差；Δvy為INS和WSN測量得到的北向速度之差；Δρi2=(ρiins)2-(ρiwsn)2,ρiins=(xINS-xRi)2+(yINS-yRi)2]]>為INS測量的未知節點與參考節點之間的距離，(x_Ri，y_Ri)為已知第i個節點在相對坐標系中的位置，(x^INS，y^INS)為當前時刻INS自身測量得到的未知節點的位置，為WSN通過測量TOA得到的未知節點與第i個參考節點之間的距離；其中，i=1，2，…，N，N為參考節點數目；v為系統的觀測噪聲；

步驟2)，將所述迭代擴展卡爾曼濾波器工作的濾波周期定義為小周期，根據所述步驟1)經迭代擴展卡爾曼濾波后的數據，將小周期內的未知節點的位置、速度、加速度數據進行迭代擴展卡爾曼RTS平滑處理，提高導航信息的輸出周期，得到INS測量誤差的最優估計值，步驟如下：

步驟2.1)，采用迭代擴展卡爾曼濾波的遞歸過程，作為迭代擴展卡爾曼RTS平滑的前向預測部分，得到的未知節點狀態預估，其步驟為：

X_k+1|k=AX_k|k???(3)

其中，A為系統矩陣，X_k|k表示k時刻的系統狀態，X_k+1|k表示在k時刻基礎上預估的下一時刻的系統狀態；

步驟2.2)，在所述步驟2.1)的基礎上進行n次迭代：

Xk|kn+1=Xk|kn+Kkn[zk-hn(Xk|kn)-Hn(Xk|kn)×(Xk|k-1-Xk|kn)]---(4)]]>

Kk+1n=Pk|k-1Hn(Xk|kn)T[Hn(Xk|kn)Pk|k-1Hn(Xk|kn)T+R]-1---(5)]]>

Pk+1n=[I-Kk+1nHn(Xk|kn)Pk+1|k]---(6)]]>

其中，n=1，2，...，j；為第k時刻進行第n步迭代時的濾波器增益，z_k為第k時刻系統的觀測向量，為第k時刻進行第n步迭代時的濾波器觀測矩陣，為第k時刻進行第n步迭代時的濾波器觀測矩陣的雅克比矩陣，為第k時刻進行第n步迭代時的濾波器的概率矩陣；R為過噪聲和測量噪聲協方差矩陣；

系統狀態的最終狀態預估如下：

Xk+1|k+1=Xk|kj---(7)]]>

Pk+1|k+1=Pk+1j---(8)]]>

步驟2.3)，將導航解的輸出周期定義為大周期，在大周期內對所述式(7)和式(8)得到的最終狀態預估做逆向平滑，得到INS測量誤差的最優估計，其步驟為：

KkS=Pk+1|k+1APk+1|k-1---(9)]]>

XkS=Xk+1|k+1+KkS(Xk+1S-Xk+1|k)---(10)]]>

PkS=Pk+KkS(Pk+1S-Pk+1|k)(KkS)T---(11)]]>

其中，為逆向平滑過程中的狀態變量，為逆向平滑過程中的濾波器增益，為逆向平滑過程中的概率；

步驟3)，將同一時刻的INS測量得到的未知節點的位置、速度、加速度，與所述步驟2)迭代擴展卡爾曼RTS平滑得到的INS測量誤差的最優估計作差，得到小周期內INS測量的未知節點的位置、速度和加速度最優估計；

步驟4)，將所述步驟3)得到的小周期內INS測量的未知節點的位置、速度和加速度最優估計的均值作為大周期的導航信息輸出，得到最終的導航信息。