一种基于融合多传感器的因子图室内定位方法技术

本发明专利技术公开了一种基于融合多传感器的因子图室内定位方法，包括步骤：根据多传感器的测量模型制定传感器因子，将传感器因子输入至预设的因子动态适应函数作为因子节点、将机器人导航状态作为变量节点构建因子图框架；对获取的多传感器的测量数据进行非线性融合，得到机器人导航状态的最优状态增量估计；结合增量平滑算法计算机器人所有导航状态下的全导航解。本发明专利技术可有效融合多传感器的测量信息，提高实时性和定位精度。高实时性和定位精度。高实时性和定位精度。

【技术实现步骤摘要】
一种基于融合多传感器的因子图室内定位方法


[0001]本专利技术涉及机器人定位
，尤其是一种基于融合多传感器的因子图室内定位方法。

技术介绍

[0002]自主移动机器人已应用于工业生产、卫生、交通及军事等各个领域，而精准的位置服务是机器人控制的关键环节之一，近年来已成为研究热点并已成为一个重要课题。相比于室外定位，室内由于存在物体的遮挡、人员的走动及物品摆放繁杂、存在许多不确定的障碍物等复杂环境，导致实现高精准定位难度更大。与早期以惯性测量单元(Inertial Measurement Unit,IMU)为主要传感器的定位技术相比，目前室内机器人定位会配备多种传感器，以获得更精确、更可靠的导航解决方案。虽然采用不同传感器组合可以提高定位精度，但传感器测量的异步性和非线性使来自多传感器的测量信息难以有效融合，定位精度不够高且计算复杂度高。

技术实现思路

[0003]针对现有技术中的缺陷，本专利技术提供了一种基于融合多传感器的因子图室内定位方法，可有效融合多传感器的测量信息，提高定位精度。
[0004]本专利技术提供了一种基于融合多传感器的因子图室内定位方法，包括步骤：
[0005]根据多传感器的测量模型制定传感器因子，将所述传感器因子输入至预设的因子动态适应函数作为因子节点、将机器人导航状态作为变量节点构建因子图框架；
[0006]对获取的多传感器的测量数据进行非线性融合，得到所述机器人导航状态的最优状态增量估计；
[0007]结合增量平滑算法计算机器人所有导航状态下的全导航解。
[0008]优选地，所述多传感器包括IMU、里程计和激光雷达。
[0009]优选地，所述根据多传感器的测量模型制定传感器因子具体包括：
[0010]根据IMU测量模型制定IMU因子；
[0011]根据里程计测量模型制定里程计因子；
[0012]根据激光雷达测量模型制定激光雷达二元因子和外部因子。
[0013]优选地，所述对获取的多传感器的测量数据进行非线性融合包括：
[0014]利用高斯牛顿方法对获取的多传感器的测量数据进行非线性融合。
[0015]优选地，所述对获取的多传感器的测量数据进行非线性融合包括：
[0016]利用LM方法对获取的多传感器的测量数据进行非线性融合。
[0017]优选地，还包括：定义机器人最优导航解为对机器人导航状态的最大后验估计。
[0018]优选地，还包括：将所述机器人导航状态构建成贝叶斯树。
[0019]优选地，所述增量平滑算法的步骤包括：
[0020]当初始因子图框架加入新因子节点时，根据所述新因子节点包含的状态量确定贝
叶斯树中的受影响部分及对应的受影响状态量、未受影响部分及对应的未受影响状态量；
[0021]将所述受影响状态量与所述新因子节点包含的状态量进行非线性融合，得到新增状态量；
[0022]将所述新增状态量与所述未受影响状态量相接，得到新因子图框架。
[0023]本专利技术的有益效果为：
[0024]将IMU、里程计(Odom)和激光雷达(Lidar)构成的预积分因子、配准因子和闭环因子插入全局因子图中，并采用基于贝叶斯树的因子图优化算法实现数据的融合和对变量节点进行优化估计。将导航状态作为变量节点、传感器模型作为因子节点构建因子图模型，使用智能优化算法解决非线性问题得到每个时刻的最优状态估计，并结合增量平滑技术有效地计算所有状态下的全导航解。增量方法优化了图中的部分节点，使本专利技术适用于实时性要求比较高的场景。同时，IMU测量数据生成一个简单的因子链，可以非常高效地在该拓扑上运行，能保证全局位姿轨迹和所在环境的全局一致性。本专利技术提升了定位实时性、鲁棒性和准确性，对室内机器人的自主导航定位具有重大的应用研究意义。
附图说明
[0025]为了更清楚地说明本专利技术具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍。在所有附图中，类似的元件或部分一般由类似的附图标记标识。附图中，各元件或部分并不一定按照实际的比例绘制。
[0026]图1为本专利技术实施例提供的流程示意图；
[0027]图2a为本专利技术实施例提供的具有常规IMU和偏差因子的因子图；
[0028]图2b为本专利技术实施例提供的使用等效IMU因子的因子图；
[0029]图3为本专利技术实施例提供的里程计和IMU传感器测量的因子图；
[0030]图4为本专利技术实施例提供的激光雷达、里程计和IMU传感器测量的因子图；
[0031]图5为本专利技术实施例提供的初始因子图；
[0032]图6为本专利技术实施例提供的雅可比矩阵；
[0033]图7为本专利技术实施例提供的加入新节点后的因子图；
[0034]图8为本专利技术实施例提供的加入新节点后的雅可比矩阵。
具体实施方式
[0035]下面将结合附图对本专利技术技术方案的实施例进行详细的描述。以下实施例仅用于更加清楚地说明本专利技术的技术方案，因此只作为示例，而不能以此来限制本专利技术的保护范围。
[0036]需要注意的是，除非另有说明，本申请使用的技术术语或者科学术语应当为本专利技术所属领域技术人员所理解的通常意义。
[0037]下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本专利技术一部分实施例，而不是全部的实施例。基于本专利技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本专利技术保护的范围。
[0038]应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和“包含”指示
所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
[0039]还应当理解，在此本专利技术说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本专利技术。如在本专利技术说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。
[0040]还应当进一步理解，在本专利技术说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
[0041]将主动移动机器人应用在搬运货物，以及在没电返回进行无线充电时需要非常精准的定位。将多传感器融合的因子图模型应用到基于slam的自主导航机器人中，在机器人建图时通过三种传感器构建周围环境的地图，并且获得先验信息和路标。机器人开始导航时，在搬动货物和交接货物时，需要非常精准的定位，控制机器人移动到指定的位置。机器人在完成任务或者电量不足时，返回进行初始位置补充电量，只有定位足够精准才行完成充电。
[0042]基于上述描述，本专利技术实施例提供了一种基于融合多传感器的因子图室内定位方法，参见图1本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于融合多传感器的因子图室内定位方法，其特征在于，包括步骤：根据多传感器的测量模型制定传感器因子，将所述传感器因子输入至预设的因子动态适应函数作为因子节点、将机器人导航状态作为变量节点构建因子图框架；对获取的多传感器的测量数据进行非线性融合，得到所述机器人导航状态的最优状态增量估计；结合增量平滑算法计算机器人所有导航状态下的全导航解。2.根据权利要求1所述的一种基于融合多传感器的因子图室内定位方法，其特征在于，所述多传感器包括IMU、里程计和激光雷达。3.根据权利要求2所述的一种基于融合多传感器的因子图室内定位方法，其特征在于，所述根据多传感器的测量模型制定传感器因子具体包括：根据IMU测量模型制定IMU因子；根据里程计测量模型制定里程计因子；根据激光雷达测量模型制定激光雷达二元因子和外部因子。4.根据权利要求1所述的一种基于融合多传感器的因子图室内定位方法，其特征在于，所述对获取的多传感器的测量数据进行非线性融合包括：利用高斯牛顿方法对获取的多传感器的...

【专利技术属性】
技术研发人员：潘雷，武星宇，张立阳，高瑞，
申请(专利权)人：天津城建大学，
类型：发明
国别省市：