基于EKF/EFIR混合滤波的室内移动机器人定位方法和装置

摘要

本发明公开了一种基于EKF/EFIR混合滤波的室内移动机器人定位方法和装置，该方法包括：以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量构建UWB机器人定位模型；然后以所述UWB机器人定位模型为基础，利用UWB定位标签测量得到的某参考节点与定位标签之间的距离信息，通过EKF/EFIR混合滤波器估计机器人的位置以及参考节点的位置：首先采用EKF算法进行预估，检测EKF算法性能，若未能达到预设精度，则用EFIR滤波器对EKF算法进行补充，最终得到当前时刻目标机器人最优的位置信息。采用本发明的方法能够提高室内环境下的移动机器人的定位精度。

说明书

技术领域

本发明涉及复杂环境下组合定位技术领域，尤其涉及一种基于EKF/EFIR混合滤波的室内移动机器人定位方法。

背景技术

近年来，随着科学技术的发展和人民生活水平的提高，机器人逐步走进人们的生活。作为机器人为人类提供高质量服务的基础，面向机器人的定位正逐渐成为该领域的研究热点。

在机器人定位技术方面，为了克服全球定位系统(Global Positioning System,GPS)为代表的全球导航卫星系统(GlobalNavigation Satellite System,GNSS)在室内环境下无法克服因信号受遮挡导致定位精度下降甚至失锁的缺点，学者们提出将无线传感器网络(WirelessSensors Network,WSN)技术应用到室内机器人定位领域。例如Sweatt M.等提出基于WiFi的室内移动机器人通信和定位算法；Miah M.S.等对基于射频识别(Radiofrequency identification,RFID)技术的移动机器人定位算法进行了研究。需要指出的是，上述无线定位技术的精度为米级，不能满足室内机器人高精度导航定位的要求。为了获取更高的定位精度，学者们提出将超宽带技术(Ultra Wideband，UWB)应用于移动机器人室内导航定位中[8]。但是需要指出的是，传统的室内定位方法都需要预先获取参考节点的位置信息，这限制了这一技术的灵活性。

在数据融合方法方面，目前应用最为广泛的是卡尔曼滤波算法(Kalman filter,KF)及其相应的改进方法，如扩展卡尔曼滤波(Extended KF,EKF)、无迹卡尔曼滤波(Unscented KF,UKF)等。但需要指出的是KF及其改进算法的精度依赖于滤波模型的精度，特别是噪声特性的描述。然而在实际应用中很难获取机器人所处导航环境的精确环境噪声信息。为了克服这一问题，学者们提出了基于有限脉冲响应结构(finite impulseresponse,FIR)的数据融合算法，以提高滤波算法的鲁棒性。例如Shmaliy Y S提出了一种面向线性系统的FIR滤波器，并将其应用到机器人无线定位领域。但需要指出的是，FIR滤波器虽然具有较高的鲁棒性，但其精度略差于获取到精确噪声描述的KF及其相应改进算法。除此之外，有限脉冲响应结构也使得FIR滤波器的运行效率弱于KF。

目前需要本领域技术人员迫切解决的一个技术问题是：如何在定位过程中既保证精度又兼顾效率。

发明内容

为了解决上述问题，本发明提供了一种基于EKF/EFIR混合滤波的室内移动机器人定位方法，该方法构建UWB定位标签所获取到的距离(即参考节点与目标机器人之间的距离)信息、目标机器人的位置和速度信息之间的模型，以降低室内复杂导航环境对组合导航精度的影响，为滤波器完成对导航信息的高精度预估打下基础。

为了实现上述目的，本发明采用如下技术方案：

一种基于EKF/EFIR混合滤波的室内移动机器人定位方法，包括以下步骤：

(1)以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量构建UWB机器人定位模型；

(2)以所述UWB机器人定位模型为基础，利用UWB定位标签测量得到的某参考节点与定位标签之间的距离信息，通过EKF/EFIR混合滤波器估计机器人的位置以及参考节点的位置：首先采用EKF算法进行预估，检测EKF算法性能，若未能达到预设精度，则用EFIR滤波器对EKF算法进行补充，最终得到当前时刻目标机器人最优的位置信息。

作为状态量的目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置的初值根据实际需要自行设定。

所述EKF/EFIR混合滤波器的状态方程为：

其中，(x_k，y_k)分别为移动机器人在k时刻的x和y方向的位置；(Vx_k，Vy_k)分别为移动机器人在k时刻的x和y方向的速度；T为采样周期；为k时刻参考节点的位置向量，表示参考节点在k时刻在x和y方向的位置，其中，l为参考节点的数目；ω_k为满足均值为零正态分布的系统噪声，其协方差矩阵为Q。

所述EKF/EFIR混合滤波器的观测方程为：

其中，y_k＝[d₁>2…d_l]^T为k时刻通过UWB定位系统测量的到的参考节点与未知节点之间的距离；v_k为系统的观测噪声矩阵，其协方差矩阵为R，表示参考节点k时刻在x和y方向的位置，l为参考节点的数目。

所述EKF/EFIR滤波器中EKF算法的迭代过程为：

其中，表示EKF在k时刻预估的状态向量，表示EKF由k-1时刻到k时刻预估的状态向量，F_k表示表示k时刻的系统矩阵，表示EKF由k-1时刻到k时刻的最小预测均方误差矩阵；表示EKF k时刻的最小预测均方误差矩阵；K_k表示EKF在k时刻的误差增益矩阵；I表示单位阵。

所述EKF/EFIR滤波器中EKF算法的性能采用信道质量评价指标D_k来评价：

判断D_k与门限door的关系，如果D_k<door，则信道质量正常，反之，则信道质量差，进入EFIR滤波器。

所述EKF/EFIR滤波器中EFIR算法的迭代过程为：

首先，若当前采样时刻大于EFIR预先设置的窗口N，定义m＝k-N+1，s＝m+M-1，G_s＝I；

然后，从s+1时刻到k时刻，执行以下迭代过程：

其中，表示EFIR在j时刻预估的状态向量，表示EFIR由j-1时刻到j时刻预估的状态向量，F_j表示表示j时刻的系统矩阵，表示EFIR由j-1时刻到j时刻的最小预测均方误差矩阵；表示EFIR j时刻的最小预测均方误差矩阵；K_j表示EKF在j时刻的误差增益矩阵；I表示单位阵；

最后，

所述EKF/EFIR混合滤波器的迭代过程为：

首先采用EKF滤波器进行一步预估：

若D_k＜door，则继续进行EKF滤波器的更新：

P_k＝(I-K_kH_k)P_k|k-1

反之，则信道质量差，进入EFIR滤波器；

若当前采样时刻大于EFIR预先设置的窗口N，定义m＝k-N+1，s＝m+M-1，G_s＝1，

从s+1时刻到k时刻，执行以下迭代过程：

其中，表示EFIR在j时刻预估的状态向量，表示EFIR由j-1时刻到j时刻预估的状态向量，F_j表示表示j时刻的系统矩阵，表示EFIR由j-1时刻到j时刻的最小预测均方误差矩阵；表示EFIR j时刻的最小预测均方误差矩阵；K_j表示EKF在j时刻的误差增益矩阵；I表示单位阵。

最后，

本发明还提供了一种基于EKF/EFIR混合滤波的室内移动机器人定位装置，包括：

UWB机器人定位模型构建模块，以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量构建UWB机器人定位模型；

机器人定位模块，以所述UWB机器人定位模型为基础，利用UWB定位标签测量得到的某参考节点与定位标签之间的距离信息，通过EKF/EFIR混合滤波器估计机器人的位置以及参考节点的位置：首先采用EKF算法进行预估，检测EKF算法性能，若未能达到预设精度，则用EFIR滤波器对EKF算法进行补充，最终得到当前时刻目标机器人最优的位置信息。

本发明的有益效果：

1、移动机器人定位方法采用改进的UWB定位模型，该模型以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量构建UWB机器人定位模型；在此基础上，通过EKF/EFIR滤波将UWB获取的参考节点到未知节点之间的距离信息进行数据融合，最终得到当前时刻最优的目标机器人位置信息和对参考节点位置信息的预估。提高了数据融合滤波器的精度和鲁棒性。

2、可用于室内环境下的移动机器人中高精度定位。

附图说明

图1为一种基于EKF/EFIR混合滤波的室内移动机器人定位方法的示意图。

具体实施例

下面结合附图对本发明进行详细说明：

实施例1：

本实施例提供了一种基于EKF/EFIR混合滤波的室内移动机器人定位方法，示意图如图1，该方法包括：

(1)以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量构建UWB机器人定位模型；

其中，作为状态量的目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置的初值根据实际需要自行设定。

参考节点位置可以任意选定，本实施例中要求参考节点静止。

(2)以所述UWB机器人定位模型为基础，利用UWB定位标签测量得到的某参考节点与定位标签之间的距离信息，通过EKF/EFIR混合滤波器估计机器人的位置以及参考节点的位置：首先采用EKF算法进行预估，检测EKF算法性能，若未能达到预设精度，则用EFIR滤波器对EKF算法进行补充，最终得到当前时刻目标机器人最优的位置信息。

其中，所述EKF/EFIR混合滤波器的状态方程为：

其中，(x_k，y_k)分别为移动机器人在k时刻的x和y方向的位置；(Vx_k，Vy_k)分别为移动机器人在k时刻的x和y方向的速度；T为采样周期；为k时刻参考节点的位置向量，表示参考节点在k时刻在x和y方向的位置，其中，l为参考节点的数目；ω_k为满足均值为零正态分布的系统噪声，其协方差矩阵为Q。

所述EKF/EFIR混合滤波器的观测方程为：

其中，y_k＝[d₁>2…d_l]^T为k时刻通过UWB定位系统测量的到的参考节点与未知节点之间的距离；v_k为系统的观测噪声矩阵，其协方差矩阵为R，表示参考节点k时刻在x和y方向的位置，l为参考节点的数目。

所述EKF/EFIR滤波器中EKF算法的迭代过程为：

其中，表示EKF在k时刻预估的状态向量，表示EKF由k-1时刻到k时刻预估的状态向量，F_k表示表示k时刻的系统矩阵，表示EKF由k-1时刻到k时刻的最小预测均方误差矩阵；表示EKF k时刻的最小预测均方误差矩阵；K_k表示EKF在k时刻的误差增益矩阵；I表示单位阵。

所述EKF/EFIR滤波器中EKF算法的性能采用信道质量评价指标D_k来评价：

判断D_k与门限door的关系，如果D_k<door，则信道质量正常，反之，则信道质量差，进入EFIR滤波器。

所述无线通信信道中EKF/EFIR滤波器中EFIR算法的迭代过程为：

首先，若当前采样时刻大于EFIR预先设置的窗口N，定义m＝k-N+1，，，m。M-1·Gs，1；

然后，从s+1时刻到k时刻，执行以下迭代过程：

其中，表示EFIR在j时刻预估的状态向量，表示EFIR由j-1时刻到j时刻预估的状态向量，F_j表示表示j时刻的系统矩阵，表示EFIR由j-1时刻到j时刻的最小预测均方误差矩阵；表示EFIR j时刻的最小预测均方误差矩阵；K_j表示EKF在j时刻的误差增益矩阵；I表示单位阵；

最后，

所述EKF/EFIR混合滤波器的迭代过程为：

首先采用EKF滤波器进行一步预估：

若D_k＜door，则继续进行EKF滤波器的更新：

P_i＝(l-K_kH_k)P_k|k-1

反之，则信道质量差，进入EFIR滤波器；

若当前采样时刻大于EFIR预先设置的窗口N，定义m＝k-N+1，s＝m+M-1，G_s＝1，

从s+1时刻到k时刻，执行以下迭代过程：

其中，表示EFIR在j时刻预估的状态向量，表示EFIR由j-1时刻到j时刻预估的状态向量，F_j表示表示j时刻的系统矩阵，表示EFIR由j-1时刻到j时刻的最小预测均方误差矩阵；表示EFIR j时刻的最小预测均方误差矩阵；K_j表示EKF在j时刻的误差增益矩阵；I表示单位阵。

最后，

为了更清楚地表达EKF/EFIR滤波器的算法运行过程，将运行过程进行如下表示。

EKF/EFIR滤波器中EKF算法的迭代过程为：

其中，

EKF/EFIR滤波器中EFIR算法的迭代过程为：

EKF/EFIR混合滤波器的迭代过程为：

实施例2：

基于实施例1所述的方法，本发明还提供了一种基于EKF/EFIR混合滤波的室内移动机器人定位装置，包括：

UWB机器人定位模型构建模块，以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量构建UWB机器人定位模型；

其中，作为状态量的目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置的初值根据实际需要自行设定。

参考节点位置可以任意选定，本实施例中要求参考节点静止。

机器人定位模块，以所述UWB机器人定位模型为基础，利用UWB定位标签测量得到的某参考节点与定位标签之间的距离信息，通过EKF/EFIR混合滤波器估计机器人的位置以及参考节点的位置：首先采用EKF算法进行预估，检测EKF算法性能，若未能达到预设精度，则用EFIR滤波器对EKF算法进行补充，最终得到当前时刻目标机器人最优的位置信息。

本装置基于实施例1中所述的方法，EKF/EFIR混合滤波器具体的算法流程可参见实施例1。

本发明对UWB定位模型进行了改进，以目标机器人x向和y向的位置和速度以及各UWB参考节点的x向和y向位置作为状态量，以UWB定位标签测量得到的参考节点与定位标签之间的距离作为系统观测量，提高了数据融合滤波器的精度和鲁棒性；采用本发明的方法，提高了室内环境下的移动机器人的定位精度。

本领域技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算机装置来实现，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。本发明不限制于任何特定的硬件和软件的结合。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。