[0001] 本发明涉及移动机器人技术领域，具体涉及一种多源数据融合的室内机器人高精度定位方法、一种多源数据融合的室内机器人高精度定位系统、一种多传感器融合的室内机器人高精度重定位方法，一种计算机设备、一种计算机可读存储介质及一种计算机程序产品。

[0002] 本部分的陈述仅仅是提供了与本发明相关的背景技术，并不必然构成现有技术。

[0003] 在机器人的定位领域，多传感器融合方法已经成为主流，常见的传感器包括激光雷达、视觉传感器、惯性测量单元(IMU)和超宽带(UWB)等。这些系统通常结合多个传感器的数据来提高定位的鲁棒性与精度。例如，激光雷达可以提供高精度的距离测量，而视觉传感器则能获取丰富的环境信息，IMU则用于补偿动态场景下的位姿变化。

[0004] 现有的机器人定位及重定位方案尽管融合多个传感器数据可以提高鲁棒性，但在一些动态环境中依然面临较多挑战：(1)融合算法可能无法有效处理传感器之间的数据冲突，导致最终定位结果不可靠，现有的融合算法在处理不同传感器的估计误差时，未能有效考虑各传感器的特性与局限性，这可能导致整体定位精度的下降，尤其是在环境特征稀疏或光照变化大的情况下；(2)重定位时，现有的方法往往需要遍历较大范围的环境数据，耗时较长，尤其是在类似场景中，重定位的准确率低，导致机器人难以快速找到正确的位置；(3)现有的多传感器融合方法在数据处理和计算上可能存在延迟，影响系统的实时响应能力，这在动态环境下尤为关键，容易导致机器人在执行任务时的反应不够灵敏。

[0038] 下面结合附图与实施例对本发明作进一步说明。

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

[0040] 在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

[0041] 实施例1：

[0042] 尽管多传感器融合已经取得了一定的成功，但在复杂环境中的定位与重定位依然面临一些挑战，体现为在相似场景中容易丢失位置，重定位成功率低；当一个传感器失真后，会对整个定位系统引入较大的误差，对最终的定位结果产生较大的影响；当环境发生变化时，定位结果往往不准确；室内外适用的定位方案不统一等。有鉴于此，本实现方式提出了一种多源数据融合的室内机器人高精度定位方法，包括以下过程：

[0043] 传感器数据采集：使用双目相机、激光雷达、IMU(惯性测量单元)、轮式里程计及UWB(信号稳定时)等多种传感器采集环境信息；

[0044] 位姿估计：各传感器分别输出稳定的位姿估计；

[0045] 激光雷达在每次的有效帧中提取线特征、面特征、线与面之间的位置关系特征、线与线之间的位置关系特征、面与面之间的位置关系特征，通过上述的位置关系特征与全局地图中的点云信息进行粗略匹配，根据匹配率筛选与当前环境最相似的场景，得到粗匹配的位置。

[0046] 更具体的，激光雷达在每次的有效帧中提取线、面及线与面、线与线、面与面之间的位置关系特征，包括：

[0047] 每一帧使用例如Hough变换、RANSAC等方法，通过识别线段的连续性来检测直线提取线，根据主成分分析(PCA)提取局部点云的主要方向，提取收益同一平面的特征，将这些特征与全局点云中的点与面特征合并，构成完整的线及面特征。

[0048] 将线与周围面特征的空间几何关系作为特征描述子，线与面的关系：使用点法向量方程确定线与面是否相交，并确定交点坐标，或者线是否平行于面；

[0049] 线与线的关系：使用线的参数化方程，通过计算最小距离等方式确定两条线之间的关系，如相交、平行等；

[0050] 面与面的关系：通过面法向量的计算可以推导面与面之间得到关系，如平行、相交或有一定夹角，并计算出它们的夹角或相交线

[0051] 使用深度学习的方法提取周围环境视觉传感器返回的图像及雷达返回的点云信息中的高维度特征，如颜色和形状等信息，在粗匹配的位置处进行精确匹配(可根据图像中描述的环境的边缘、纹理等微小特征完成匹配，避免在环境相似的场景中定位失败，并且提高匹配效率)，输出雷达及视觉传感器的融合估计位姿；

[0052] 由于机器人lidar及视觉传感器可感知一定范围内的特征。因此将环境地图以可调节距离进行大体素分割，如几米分割为一个场景，将当前帧中的特征描述子在地图分割的几个子区域内进行快速粗略匹配；

[0053] 计算匹配率：通过欧氏距离、余弦相似度等方法对提取到的描述符进行匹配，距离较小或相似度较高的描述符对被认为是成功匹配，通过成功匹配的描述符对的数量与当前帧中提取的总描述符数量之比确定匹配比例，最终得到匹配率相似的几个场景，并确定在这几个场景中的大约位置，缩小精匹配的范围。

[0054] 本实现方式中，精匹配过程为在粗匹配得到的几个粗匹配位置中进行精确匹配，最终得到一个准其额度高的位姿估计，使用深度学习的方法提取视觉传感器返回的图像信息及雷达返回的点云信息，并从中提取高维度特征，完成高纬度特征匹配，过程如下：

[0055] 将视觉图像的特征信息(如颜色、纹理、边缘等)与激光雷达点云的几何结构在一个联合空间内编码成高维特征向量，视觉特征和几何特征并行输入，构成“多维度多模态联合特征向量”；

[0056] 在以上特征向量的基础上，使用“多层次特征关联网络”进行学习，提取更加鲁棒的特征，其中包含三个模块：(1)局部特征模块：采用局部卷积提取视觉特征和几何特征的低级关联模式；(2)全局特征模块：基于图神经网络(GNN)构建视觉特征与几何特征的全局空间关系图，捕获视觉和点云中可能的长程空间关系；(3)跨模态交互模块：利用跨模态注意力机制，在视觉和几何特征间建立关联映射，提取共同的环境结构信息(例如，视觉中边缘与点云中曲面的交互)，并强化匹配过程中的特征表达能力。

[0057] 在特征匹配阶段，引入“语义一致性度量”，衡量视觉特征和点云特征的相似性，考虑二者在全局空间关系中的一致性。例如，计算“关键视觉边缘与相邻点云法向量”的一致性，使得匹配更加精准，减小噪声和数据偏差带来的影响。

[0058] 针对噪声和动态环境，设计自适应权重优化模块，该模块在匹配过程中自动调整视觉和点云特征的贡献比例，具体而言，利用图神经网络中的注意力机制，依据视觉和几何特征的稳定性实时调整其权重，减少环境复杂性带来的影响。

[0059] 本实现方式中，IMU基于内部的加速度计和陀螺仪传感器测量加速度和角速度数据，通过积分运算输出第一估计位姿；轮式里程计通过检测轮子旋转角度和速度信息进行积分计算，输出第二估计位姿；UWB使用信号稳定时的第三估计位姿；

[0060] 位姿差异度量：激光及视觉传感器输出频率较低，IMU及轮式里程计频率较高，根据相同时间段内传感器输出的机器人位姿之间的差异，采用欧氏距离、旋转差异、加权差异等方法筛查数据的正确性，忽略差距过大的位姿，该方法可有效过滤传感器数据失效对机器人位姿估计的影响。

[0061] 例如，轮式里程计打滑会输出错误的机器人位移距离，而此时根据激光雷达及双目视觉得到的融合位姿则相对正确；又如，在长走廊、机房等场景相似度较高的环境中，激光传感器定位出错概率会显著上升，此时应更加置信轮式里程计及IMU的输出数据。

[0062] 更具体的，位姿差异度量的方法，包括：

[0063] 依据各传感器的当前测量状态给出权重分配，权重越高，估计位姿可靠性越高，例如，UWB信号稳定时赋予其较高权重，而信号稳定性低时则忽略其定位信息，在高振动环境中优先考虑IMU位姿；采用共识筛选机制，利用位姿残差的欧氏距离或马氏距离衡量每个估计位姿与其他位姿之间的偏差；计算IMU、轮式里程计、UWB、雷达及视觉融合后里程计位姿的差异度，若两个或多个位姿之间的差异小于设定的阈值，可以视为彼此接近的估计位姿，表示较高的可靠性，否则认为至少有一个传感器为失真状态，通过比较位姿相似度，确定并忽略失真位姿；若有多个接近的估计位姿，通过图优化(如g2o)，在结合环境约束和误差度量的条件下进行最终位姿优化。

[0064] 本实现方式中，因子图优化处理，包括：将通过筛选后的正确位姿数据传递给图优化模块，使用g2o算法进行位姿优化，从而得到更加精确的位姿估计。

[0065] 本实现方式，还提供了一种多传感器融合的室内机器人高精度重定位方法，具体的，包括：激光雷达在每次的有效帧中提取线特征、面特征、线与面之间的位置关系特征、线与线之间的位置关系特征、面与面之间的位置关系特征，通过上述的位置关系特征与全局地图中的点云信息进行粗略匹配，根据匹配率筛选与当前环境最相似的场景，得到粗匹配的位置；使用深度学习的方法提取周围环境视觉传感器返回的图像及雷达返回的点云信息中的高维度特征，如颜色和形状等信息，在粗匹配的位置处进行精确匹配，输出雷达及视觉传感器的融合估计位姿，以融合估计位姿作为最终的重定位结果。

[0066] 实施例2：

[0067] 如图2所示，本实现方式提供了一种多源数据融合的室内机器人高精度定位系统，包括：

[0068] 粗匹配单元，被配置为：在激光雷达帧中提取位置关系特征，将所述位置关系特征与全局地图中的点云信息进行匹配，根据匹配率筛选与当前环境最相似的场景，得到粗匹配位置；

[0069] 融合位姿估计单元，被配置为：提取视觉图像信息及激光雷达的点云信息中的高维度特征，在粗匹配出的位置处进行精确匹配，得到融合估计位姿；

[0070] 位姿估计单元，被配置为：采用惯性测量单元的测量数据，得到第一估计位姿，采用轮式里程计的测量数据，得到第二估计位姿，采用无线载波通信的测量数据，得到第三估计位姿；

[0071] 位姿优化单元，被配置为：对所述融合估计位姿、第一估计位姿、第二估计位姿和第三估计位姿，进行位姿差异度量，筛选得到最优估计位姿，根据所述最优估计位姿进行位姿优化后得到最终位姿。

[0072] 可以理解的，上述各个单元可以分别或全部合并为一个或若干个另外的单元来构成，或者其中的某个(些)单元还可以再拆分为功能上更小的多个单元来构成，这可以实现同样的操作，而不影响本申请的实施例的技术效果的实现。上述单元是基于逻辑功能划分的，在实际应用中，一个单元的功能也可以由多个单元来实现，或者多个单元的功能由一个单元实现。在本申请的其它实施例中，该系统也可以包括其它单元，在实际应用中，这些功能也可以由其它单元协助实现，并且可以由多个单元协作实现。

[0073] 根据本申请的另一个实施例，可以通过在包括中央处理单元(Central Processing Unit，CPU)、存取存储介质(Random Access Memory，RAM)、只读存储介质(Read Only Memory，ROM)等处理元件和存储元件的例如计算机的通用计算设备上运行能够执行实施例1所述的相应方法所涉及的各步骤的计算机程序(包括程序代码)，来构造本实施例所述的系统，以及来实现本申请实施例1的方法，计算机程序可以记载于例如计算机可读记录介质上，并通过计算机可读记录介质装载于上述计算设备中，并在其中运行。

[0074] 实施例3：

[0075] 如图3所示，本实现方式提供了一种电子设备，该电子设备包括处理器1001、通信接口1002以及计算机可读存储介质1003。其中，处理器1001、通信接口1002以及计算机可读存储介质1003可通过总线或者其它方式连接。

[0076] 其中，通信接口1002用于接收和发送数据，计算机可读存储介质1003可以存储在电子设备的存储器中，计算机可读存储介质1003用于存储计算机程序，计算机程序包括程序指令，处理器1001用于执行计算机可读存储介质1003存储的程序指令。

[0077] 处理器1001(或称CPU(Central Processing Unit，中央处理器))是电子设备的计算核心以及控制核心，其适于实现一条或多条指令，具体适于加载并执行一条或多条指令从而实现相应方法流程或相应功能。

[0078] 所述处理器1001被配置为执行如下过程：

[0079] 在激光雷达帧中提取位置关系特征，将所述位置关系特征与全局地图中的点云信息进行匹配，根据匹配率筛选与当前环境最相似的场景，得到粗匹配位置；

[0080] 提取视觉图像信息及激光雷达的点云信息中的高维度特征，在粗匹配出的位置处进行精确匹配，得到融合估计位姿；

[0081] 采用惯性测量单元的测量数据，得到第一估计位姿，采用轮式里程计的测量数据，得到第二估计位姿，采用超宽带的测量数据，得到第三估计位姿；

[0082] 对所述融合估计位姿、第一估计位姿、第二估计位姿和第三估计位姿，进行位姿差异度量，筛选得到最优估计位姿，根据所述最优估计位姿进行位姿优化后得到最终位姿[0083] 实施例4：

[0084] 本实现方式提供了一种计算机可读存储介质(Memory)，计算机可读存储介质是电子设备中的记忆设备，用于存放程序和数据。可以理解的是，此处的计算机可读存储介质既可以包括电子设备中的内置存储介质，当然也可以包括电子设备所支持的扩展存储介质。计算机可读存储介质提供存储空间，该存储空间存储了电子设备的处理系统。

[0085] 并且，在该存储空间中还存放了适于被处理器加载并执行的一条或多条的指令，这些指令可以是一个或多个的计算机程序(包括程序代码)。需要说明的是，此处的计算机可读存储介质可以是高速RAM存储器，也可以是非不稳定的存储器，例如至少一个磁盘存储器；可选的，还可以是至少一个位于远离前述处理器的计算机可读存储介质。

[0086] 在一个实施例中，该计算机可读存储介质中存储有一条或多条指令；由处理器加载并执行计算机可读存储介质中存放的一条或多条指令，以实现如下过程：

[0087] 在激光雷达帧中提取位置关系特征，将所述位置关系特征与全局地图中的点云信息进行匹配，根据匹配率筛选与当前环境最相似的场景，得到粗匹配位置；

[0088] 提取视觉图像信息及激光雷达的点云信息中的高维度特征，在粗匹配出的位置处进行精确匹配，得到融合估计位姿；

[0089] 采用惯性测量单元的测量数据，得到第一估计位姿，采用轮式里程计的测量数据，得到第二估计位姿，采用超宽带的测量数据，得到第三估计位姿；

[0090] 对所述融合估计位姿、第一估计位姿、第二估计位姿和第三估计位姿，进行位姿差异度量，筛选得到最优估计位姿，根据所述最优估计位姿进行位姿优化后得到最终位姿[0091] 实施例5：

[0092] 本实现方式提供一种计算机程序产品或计算机程序，该计算机程序产品或计算机程序包括计算机指令，该计算机指令存储在计算机可读存储介质中。电子设备的处理器从计算机可读存储介质读取该计算机指令，处理器执行该计算机指令，使得该电子设备执行如下过程：

[0093] 在激光雷达帧中提取位置关系特征，将所述位置关系特征与全局地图中的点云信息进行匹配，根据匹配率筛选与当前环境最相似的场景，得到粗匹配位置；

[0094] 提取视觉图像信息及激光雷达的点云信息中的高维度特征，在粗匹配出的位置处进行精确匹配，得到融合估计位姿；

[0095] 采用惯性测量单元的测量数据，得到第一估计位姿，采用轮式里程计的测量数据，得到第二估计位姿，采用超宽带的测量数据，得到第三估计位姿；

[0096] 对所述融合估计位姿、第一估计位姿、第二估计位姿和第三估计位姿，进行位姿差异度量，筛选得到最优估计位姿，根据所述最优估计位姿进行位姿优化后得到最终位姿[0097] 本领域普通技术对象可以意识到，结合本申请中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术对象可以对每个特定的应用，使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

[0098] 在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行计算机程序指令时，全部或部分地产生按照本申请实施例的流程或功能。计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程设备。计算机指令可以存储在计算机可读存储介质中，或者通过计算机可读存储介质进行传输。计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如，同轴电缆、光纤、数字线(DSL))或无线(例如，红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据处理设备。可用介质可以是磁性介质(例如，软盘、硬盘、磁带)、光介质(例如，DVD)、或者半导体介质(例如，固态硬盘(Solid State Disk，SSD))等。

[0099] 以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。