[发明专利]一种考虑可变有色测量噪声的移动机器人定位方法及系统

权利要求书

1.一种考虑可变有色测量噪声的移动机器人定位方法，其特征在于，包括：

以移动机器人k时刻在东向和北向的位置和速度作为状态量，将UWB测量得到的移动机器人与UWB参考节点之间的距离作为系统观测量，构建滤波模型；

在扩展有限脉冲响应滤波器的基础上，根据在离线阶段选择的不同局部滤波窗口，构建m个不同的考虑有色测量噪声的子滤波器，m为设定值；

通过IMM方式对构建的子滤波器的输出进行融合，得到当前时刻的移动机器人最优的位置预估，实现移动机器人的定位；

其中，所述通过IMM方式对构建的子滤波器的输出进行融合，具体过程包括：

构建马尔可夫的转移概率矩阵，在此基础上，引入权重矩阵；

计算各子滤波器的权重；具体过程包括：

其中，为由k-1时刻得到的k时刻的权重值；i为子滤波器的个数；为当前时刻的权重值、C_k＝H_k-γⁱH_k-1F^-1，H_k为j时刻的观测方程，γⁱ为第i个子滤波器中的CMN的比例，为由k-1时刻得到的k时刻的误差矩阵，R观测噪声协方差，φ_k＝Q_kγⁱH_k-1F^-1，Q_k为k时刻系统噪声的协方差，F为系统矩阵；

根据各子滤波器的权重以及子滤波器的状态向量预估，得到数据融合滤波器的总输出。

2.如权利要求1所述的一种考虑可变有色测量噪声的移动机器人定位方法，其特征在于，各子滤波器的状态方程具体为：

其中，(x_k,y_k)、(x_k-1,y_k-1)分别为k和k-1时刻移动机器人的东向和北向的位置；分别为k和k-1时刻移动机器人的东向和北向的速度；τ为采样周期；w_k为k时刻的系统噪声。

3.如权利要求1所述的一种考虑可变有色测量噪声的移动机器人定位方法，其特征在于，各子滤波器的观测方程具体为：

其中，(x_i,y_i),i∈[1,4]为第i个UWB参考节点的位置；v_k为系统k时刻的观测噪声；为k时UWB测量得到的移动机器人与UWB参考节点之间的距离观测量。

4.如权利要求1所述的一种考虑可变有色测量噪声的移动机器人定位方法，其特征在于，在离线阶段选择的不同局部滤波窗口的数量根据对算法性能的需要进行选择。

5.如权利要求1所述的一种考虑可变有色测量噪声的移动机器人定位方法，其特征在于，得到数据融合滤波器的总输出，具体为：

其中，为k时刻主滤波器的输出、为k时刻子滤波器的状态预估。