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

摘要

本发明公开了一种考虑可变有色测量噪声的移动机器人定位方法及系统，包括：以移动机器人k时刻在东向和北向的位置和速度作为状态量，将UWB测量得到的移动机器人与UWB参考节点之间的距离作为系统观测量，构建滤波模型；在扩展有限脉冲响应滤波器的基础上，根据在离线阶段选择的不同局部滤波窗口，构建m个不同的考虑有色测量噪声的子滤波器；通过IMM方式对构建的子滤波器的输出进行融合，得到当前时刻的移动机器人最优的位置预估，实现移动机器人的定位。本发明构建不同的有色测量噪声下的子EFIR滤波器，通过交互多模型算法，将各子滤波器的输出进行融合，最终得到最优的UWB测量的移动机器人最优位置预估。

说明书

技术领域

本发明涉及复杂环境下组合定位技术领域，尤其涉及一种考虑可变有色测量噪声的移动机器人定位方法及系统。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

移动机器人导航与定位作为家庭服务机器人为人类提供高质量服务的基础，目前面临很多挑战。特别是在室内环境下，建筑物室内布局、建筑材料、甚至空间尺寸都会对导航信号产生影响，严重影响定位精度。与此同时，面向室内环境的家庭服务机器人相对较小的平台也对导航仪器的体积、精度和成本提出了苛刻的要求。在室内环境下，如何利用所获取的有限信息消除室内复杂导航环境对服务机器人导航信息获取的准确性和实时性的影响，保证服务机器人在室内环境下导航精度的持续稳定，具有重要的科学理论意义和实际应用价值。

在现有的定位方式中，全球卫星导航系统(Global Navigation SatelliteSystem,GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息，但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。

近年来，UWB(Ultra Wideband)以其在复杂环境下定位精度高的特点在短距离局部定位领域表现出很大的潜力。学者们提出将基于UWB的目标跟踪应用于GNSS失效环境下的行人导航。这种方式虽然能够实现室内定位，但是由于室内环境复杂多变，UWB信号十分容易受到干扰而导致定位精度下降甚至失锁；与此同时，由于UWB采用的通信技术通常为短距离无线通信技术，因此若想完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于UWB的目标跟踪在室内导航领域仍旧面临很多挑战，特别是在移动机器人转向的过程中，有色测量噪声(Colored Measurement Noise，CMN)会对移动机器人的位置预估精度造成影响，但是现有技术无法对有色测量噪声进行准确预测。

发明内容

为了解决上述问题，本发明提出了一种考虑可变有色测量噪声的移动机器人定位方法及系统，对传统的扩展有限脉冲响应(EFIR)滤波器进行改进，以在离线阶段选择的不同有色噪声比例构建不同的有色测量噪声下的子EFIR(扩展有限脉冲响应)滤波器，并通过交互多模型(IMM)方法对构建的子EFIR滤波器的输出进行融合，最终得到当前时刻最优的移动机器人位置预估。

在一些实施方式中，采用如下技术方案：

一种考虑可变有色测量噪声的移动机器人定位方法，包括：

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

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

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

作为进一步地改进，各子滤波器的状态方程具体为：

其中，(x

作为进一步地改进，各子滤波器的观测方程具体为：

其中，(x

作为进一步地改进，在离线阶段选择的不同局部滤波窗口的数量根据对算法性能的需要进行选择。

作为进一步地改进，通过IMM方式对构建的子滤波器的输出进行融合，具体过程包括：

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

计算各子滤波器的权重；

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

作为进一步地改进，计算各子滤波器的权重，具体过程包括：

其中，

作为进一步地改进，得到数据融合滤波器的总输出，具体为：

其中，

在另一些实施方式中，采用如下技术方案：

一种考虑可变有色测量噪声的移动机器人定位系统，包括：

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

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

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

在另一些实施方式中，采用如下技术方案：

一种终端设备，其包括处理器和存储器，处理器用于实现各指令；存储器用于存储多条指令，所述指令适于由处理器加载并执行上述的考虑可变有色测量噪声的移动机器人定位方法。

在另一些实施方式中，采用如下技术方案：

一种计算机可读存储介质，其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行上述的考虑可变有色测量噪声的移动机器人定位方法。

与现有技术相比，本发明的有益效果是：

本发明在离线阶段通过独立的数据，获取子EFIR滤波器的有色测量噪声的比例，构建不同的有色测量噪声下的子EFIR滤波器，在此基础上，通过交互多模型(IMM)算法，将各子滤波器的输出进行融合，最终得到最优的UWB测量的移动机器人最优位置预估；可用于实现室内环境下的中高精度定位。

本发明的其他特征和附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本方面的实践了解到。

附图说明

图1为本发明实施例中考虑可变有色测量噪声的移动机器人定位系统结构示意图；

图2为本发明实施例中考虑可变有色测量噪声的移动机器人定位方法流程图。

具体实施方式

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本发明使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

实施例一

在一个或多个实施方式中，公开了一种考虑可变有色测量噪声的移动机器人定位方法，该方法基于图1所示的系统实现，参照图1，UWB包括UWB参考节点和UWB定位标签，UWB参考节点预先固定在已知坐标上，UWB定位标签固定在移动机器任上；INS主要由固定在目标行人足部的IMU组成。UWB获取的是当前时刻目标行人与参考节点之间的距离信息；INS获取的是当前时刻目标行人与参考节点之间的距离信息。

参照图2，考虑可变有色测量噪声的移动机器人定位方法，具体包括：

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

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

其中，在离线阶段选择的不同局部滤波窗口的数量根据对算法性能的需求自行选择。

本实施例中，所述子滤波器为有色测量噪声下的子EFIR滤波器(子cEFIR滤波器)。

子cEFIR滤波器的输出为子滤波器对移动机器人当前时刻位置的预估，子cEFIR滤波器的输入为各个时刻的观测向量。

子cEFIR滤波器的状态方程为：

其中，(x

子cEFIR滤波器的观测方程为：

其中，(x

子cEFIR滤波器的迭代算法为：

设定中间变量m和s：

m

其中，N

当采样时刻大于N

z

C

在此基础上，

其中，F为系统矩阵；

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

通过IMM方式对构建的子滤波器的输出进行融合的过程具体为：

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

2)计算各子cEFIR滤波器的权重；具体计算方法如下：

其中，

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

数据融合滤波器的总输出具体为：

其中，

实施例二

在一个或多个实施方式中，公开了一种考虑可变有色测量噪声的移动机器人定位系统，具体包括：

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

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

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

需要说明的是，上述各模块的具体实现方式已经在实施例一中进行说明，不再赘述。

实施例三

在一个或多个实施方式中，公开了一种终端设备，包括服务器，所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现实施例一中的考虑可变有色测量噪声的移动机器人定位方法。为了简洁，在此不再赘述。

应理解，本实施例中，处理器可以是中央处理单元CPU，处理器还可以是其他通用处理器、数字信号处理器DSP、专用集成电路ASIC，现成可编程门阵列FPGA或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

存储器可以包括只读存储器和随机存取存储器，并向处理器提供指令和数据、存储器的一部分还可以包括非易失性随机存储器。例如，存储器还可以存储设备类型的信息。

在实现过程中，上述方法的各步骤可以通过处理器中的硬件的集成逻辑电路或者软件形式的指令完成。

实施例一中的考虑可变有色测量噪声的移动机器人定位方法可以直接体现为硬件处理器执行完成，或者用处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器、闪存、只读存储器、可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器，处理器读取存储器中的信息，结合其硬件完成上述方法的步骤。为避免重复，这里不再详细描述。

本领域普通技术人员可以意识到，结合本实施例描述的各示例的单元即算法步骤，能够以电子硬件或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

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