[发明专利]一种面向INS/WSN室内移动机器人紧组合导航的迭代扩展RTS均值滤波方法

权利要求书

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测量的未知节点的位置、速度和加速度最优估计的均值作为大周期的导航信息输出，得到最终的导航信息。