CN115950432A

CN115950432A - 一种室内外移动机器人的无缝定位方法、设备及存储介质

Info

Publication number: CN115950432A
Application number: CN202310055408.8A
Authority: CN
Inventors: 何斌; 耿书城; 李刚; 陆萍; 王志鹏
Original assignee: Tongji University
Current assignee: Tongji University
Priority date: 2023-01-16
Filing date: 2023-01-16
Publication date: 2023-04-11

Abstract

本发明涉及一种室内外移动机器人的无缝定位方法、设备及存储介质，该方法包括：S1、分别在室外构建基于GNSS/IMU的组合定位子模型，在室内构建建立基于视觉/IMU的组合定位子模型；S2、输入交互；S3、模型滤波；S4、转移概率修正：采用新息向量对转移概率矩阵进行修正；S5、模型概率更新：计算组合定位子模型的似然函数，并更新各组合定位子模型的模型概率；S6、输出交互：对各组合定位子模型输出的当前时刻状态估计值和协方差进行融合，与模型概率加权后得到系统最终状态估计值和协方差，最后将更新后的模型概率传入输入至步骤S2进行下一次迭代。与现有技术相比，本发明实现了室内外导航中具备提供无缝定位服务。

Description

一种室内外移动机器人的无缝定位方法、设备及存储介质

技术领域

本发明涉及机器人定位技术领域，尤其是涉及一种室内外移动机器人的无缝定位方法、设备及存储介质。

背景技术

目前，移动机器人大多在单一环境中执行导航任务，如室内的引导任务或室外的巡检任务等，追其根本是因为定位系统仅在固定的场景中才能提供精准定位，一旦场景发生变化，定位系统会出现精度急剧下降甚至无法工作等问题，如室外GPS定位系统在室内环境中就会因卫星信号衰减，从而出现系统定位误差大的情况；在室内外环境的交界处由于存在建筑物遮挡、光照变化，覆盖区域有限等诱因，定位系统均会出现不同程度上的精度下降问题。

因此，亟需设计一种可提供室内外环境下的移动机器人的无缝定位方法。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供了一种定位精度高的室内外移动机器人的无缝定位方法、设备及存储介质，该方法采用基于新息修正的转移概率矩阵(Transition Probability Matrix，TPM)自适应多模型交互式(InteractingMultiple Model，IMM)算法，以新息作为当前子模型的发散程度判定依据，并以此对转移概率矩阵进行修正，从而实现室内外过渡区定位信息的精准提供实现了室内外导航中具备提供无缝定位服务。

本发明的目的可以通过以下技术方案来实现：

根据本发明的第一方面，提供了一种室内外移动机器人的无缝定位方法，采用多模型交互式算法进行定位，该方法包括以下步骤：

步骤S1、分别在室外构建基于GNSS/IMU的组合定位子模型，在室内构建基于视觉/IMU的组合定位子模型；

步骤S2、输入交互：采集k-1时刻各组合定位子模型的状态估计值

和方差

以k-1时刻组合定位子模型间的混合概率作为权重，计算得到k-1时刻各组合定位子模型的状态估计输入值

和方差输入值

步骤S3、模型滤波：将输入交互得到的各组合定位子模型的状态估计输入值

和方差输入值

作为各组合定位子模型的输入，计算k时刻各组合定位子模型的状态估计值和方差；

步骤S4、转移概率修正：采用新息向量对转移概率矩阵进行修正；

步骤S5、模型概率更新：计算组合定位子模型的似然函数，并更新k时刻各组合定位子模型的模型概率；

步骤S6、输出交互：对各组合定位子模型输出的当前时刻状态估计值和协方差进行融合，对步骤S3更新得到的模型概率进行加权得到系统最终状态估计值

和协方差P(k)，最后将更新后的模型概率传入输入至步骤S2中的交互阶段进行下一次迭代。

优选地，所述组合定位子模型的数学表达式为：

X(k)＝A_iX(k-1)+W_i(k)

Z(k)＝H_iX(k)+V_i(k)

式中，X(k)为k时刻的状态，A_i为状态转移矩阵，W_i(k)为过程噪声，Z(k)为k时刻的组合定位子模型的输出，H_i为观测矩阵，V_i(k)为观测噪声；下标i为组合定位子模型的编号。

优选地，所述步骤S2中k时刻各组合定位子模型j的状态估计值，表达式为：

式中，

为k-1时刻各组合定位子模型i的状态估计值，μ_ij(k-1)为k-1时刻修正后组合定位子模型i与组合定位子模型j之间的模型概率；n为组合定位子模型的数量。

优选地，所述k-1时刻更新的组合定位子模型i跳转至组合定位子模型j的模型概率μ_ij(k-1)，表达式为：

式中，c为设定的常系数，Λ_j(k)为组合定位子模型j的似然函数，π_ij(k-1)为k-1时刻的组合定位子模型i与组合定位子模型j之间的转移概率，μ_i(k-1)为k-1时刻组合定位子模型i的模型概率，n为组合定位子模型的数量。

优选地，所述步骤S3中的组合定位子模型采用扩展卡尔曼滤波算法进行滤波操作。

优选地，所述步骤S4包括以下子步骤：

步骤S41、根据组合定位子模型的新息向量，计算得到新息平方和：

v_s(k)＝v^T(k)v(k)

式中，v(k)为组合定位子模型的新息向量；上标T为转置；

步骤S42、对新息平方和进行归一化，计算新息平方和归一值之差：

式中，V_sj(k)为组合定位子模型j的新息平方和归一值，n为组合定位子模型的数量；v_j(k)为组合定位子模型j的新息向量；

步骤S43、计算k时刻与k-1时刻的新息平方和归一值之差：

ΔV_j(k)＝V_sj(k)-V_sj(k-1)

式中，ΔV_j(k)为组合定位子模型j的新息平方和归一值之差；V_sj(k)、V_sj(k-1)分别为k-1、k时刻组合定位子模型j的新息平方和归一值；

步骤S44、选取y＝e^x作为修正函数，结合新息平方和归一值之差ΔV_j(k)，得到最终的修正因子：

步骤S45、利用修正因子α对组合定位子模型i与组合定位子模型j之间的转移概率进行修正：

式中，

为k时刻修正后组合定位子模型i与组合定位子模型j之间的转移概率，π_ij(k-1)为k-1时刻组合定位子模型i与组合定位子模型j之间的转移概率的归一化值，α_j为组合定位子模型j的修正因子；

步骤S46、对修正后的转移概率进行归一化处理：

式中，π_ij(k)为k时刻修正后组合定位子模型i与组合定位子模型j之间的转移概率的归一化值，n为组合定位子模型的数量。

优选地，所述步骤S5中组合定位子模型的似然函数，表达式为：

式中，下标j为组合定位子模型的编号，S_j(k)为协方差矩阵，v_j为残差。

优选地，所述步骤S6中的系统最终状态估计值

和协方差P(k)，表达式分别为：

式中，

为k时刻组合定位子模型i的状态估计值，μ_i(k)为k时刻组合定位子模型i的模型概率，

为k时刻组合定位子模型i的协方差估计值，n为组合定位子模型的数量。

根据本发明的第二方面，提供了一种电子设备，包括存储器和处理器，所述存储器上存储有计算机程序，所述处理器执行所述程序时实现任一项所述的方法。

根据本发明的第三方面，提供了一种计算机可读存储介质，其上存储有计算机程序，所述程序被处理器执行时实现任一项所述的方法。

与现有技术相比，本发明具有以下优点：

本发明以多模型交互式算法IMM为基础，针对转移概率矩阵中元素无法动态调整的问题，采用基于新息修正的TPM自适应多模型交互式IMM算法，以新息作为当前子模型的发散程度判定依据，并以此对转移概率矩阵进行修正，提高移动机器人在室内外过渡区中的定位精度及定位模型切换延时，即在室内外导航中具备提供无缝定位服务的能力。

附图说明

图1为多模型交互式IMM算法流程图；

图2为本发明的基于新息修正的TPM自适应多模型交互式IMM算法的总体框架。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。

实施例

本实施例给出了一种室内外移动机器人的无缝定位方法，采用多模型交互式算法进行定位，该方法包括以下步骤：

步骤S1、分别在室外构建基于GNSS/IMU的组合定位子模型，在室内构建基于视觉/IMU的组合定位子模型；其中，组合定位子模型的数学表达式为：

X(k)＝A_iX(k-1)+W_i(k)

Z(k)＝H_iX(k)+V_i(k)

如图2所示，对于视觉/IMU组合定位子模型，其系统状态变量一般由IMU确定，本实施例选取位置、速度、四元素、加速度计零偏和陀螺仪零偏作为IMU的系统状态变量，即，

其中，G代表世界坐标系，I代表IMU坐标系，^Gp_I为IMU在世界坐标系中的表示，^Iq_G为世界坐标系到IMU坐标系的单位四元数，^Gv为IMU在世界系下的速度，b_a为IMU的加速度计零偏，b_g为IMU的陀螺仪零偏，^Gp′_I为前一关键帧相机在世界系中的位置，^Iq′_G为前一关键帧世界坐标系到IMU坐标系的四元数。

和方差

和方差输入值

其中，k时刻各组合定位子模型j的状态估计值

表达式为：

式中，n为组合定位子模型的数量；

为k-1时刻各组合定位子模型i的状态估计值，μ_ij(k-1)为k-1时刻更新的组合定位子模型i与组合定位子模型j之间的模型概率，表达式为：

和方差输入值

作为各组合定位子模型的输入，计算k时刻各组合定位子模型的状态估计值和方差；本实施例中组合定位子模型采用扩展卡尔曼滤波算法进行滤波操作。

步骤S4、转移概率修正：采用新息向量对转移概率矩阵进行修正，具体包括以下子步骤：

v_s(k)＝v^T(k)ν(k)

式中，v(k)为组合定位子模型的新息向量；上标T为转置；

步骤S43、计算k时刻与k-1时刻的新息平方和归一值之差：

ΔV_j(k)＝V_sj(k)-V_sj(k-1)

步骤S44、由于状态概率模型TPM中各元素的真实物理含义为模型间的跳转概率，具有非负性，通过修正因子修正后的状态概率模型TPM也必须为非负，同时当ΔV_j(k)＝0时，对应模型未发生跳变的情况，此时修正因子应为1，本实施例中选取y＝e^x作为修正函数，结合新息平方和归一值之差ΔV_j(k)，得到最终的修正因子：

式中，

为k时刻修正后组合定位子模型i与组合定位子模型j之间的转移概率，π_ij(k-1)为k-1时刻组合定位子模型i与组合定位子模型j之间的转移概率，α_j为组合定位子模型j的修正因子；

步骤S46、在马尔科夫过程中，所有组合定位子模型向某一组合定位子模型跳转的概率之和为一，即转移概率具有归一性，因此对修正后的转移概率进行归一化处理：

步骤S5、模型概率更新：计算组合定位子模型的似然函数，并更新k时刻各组合定位子模型的模型概率；其中，组合定位子模型的似然函数，表达式为：

其中，系统最终状态估计值

和协方差P(k)，表达式分别为：

式中，

本发明电子设备包括中央处理单元(CPU)，其可以根据存储在只读存储器(ROM)中的计算机程序指令或者从存储单元加载到随机访问存储器(RAM)中的计算机程序指令，来执行各种适当的动作和处理。在RAM中，还可以存储设备操作所需的各种程序和数据。CPU、ROM以及RAM通过总线彼此相连。输入/输出(I/O)接口也连接至总线。

设备中的多个部件连接至I/O接口，包括：输入单元，例如键盘、鼠标等；输出单元，例如各种类型的显示器、扬声器等；存储单元，例如磁盘、光盘等；以及通信单元，例如网卡、调制解调器、无线通信收发机等。通信单元允许设备通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换TP信息/数据。

处理单元执行上文所描述的各个方法和处理，例如方法S1～S6。例如，在一些实施例中，方法S1～S6可被实现为计算机软件程序，其被有形地包含于机器可读介质，例如存储单元。在一些实施例中，计算机程序的部分或者全部可以经由ROM和/或通信单元而被载入和/或安装到设备上。当计算机程序加载到RAM并由CPU执行时，可以执行上文描述的方法S1～S6的一个或多个步骤。备选地，在其他实施例中，CPU可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行方法S1～S6。

本文中以上描述的功能可以至少部分地由一个或多个硬件逻辑部件来执行。例如，非限制性地，可以使用的示范类型的硬件逻辑部件包括：场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上系统的系统(SOC)、负载可编程逻辑设备(CPLD)等等。

用于实施本发明的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器，使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。

在本发明的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。