一种融合UWB和LiDAR的室内定位方法

摘要

本发明公开了一种融合UWB和LiDAR的室内定位方法，包括：部署UWB和LiDAR，进行相关测定；获取UWB传感器的输入，解算UWB的大致定位信息；获取LiDAR的输入，获取LiDAR的大致定位信息；计算坐标转换，将不同传感器的定位信息转换到同一坐标系下；对两者运动状态拟合求得UWB定位的朝向信息，补全UWB的定位信息；判断定位异常；在定位异常情况下进行重定位；输出实时定位结果。相对于现有技术，能够提高定位的精度，并解决机器人绑架问题，同时保证定位效率。

说明书

技术领域

本发明涉及室内定位技术领域，尤其涉及一种融合UWB(Ultra Wideband，UWB)和LiDAR(Light Detection and Ranging，LiDAR)的室内定位方法。

背景技术

近年来，随着信息时代的发展，基于位置的服务(Location Based Service，LBS)已经成为人们日常生活中不可或缺的一部分，现实生活中，人们需要在停车场、机场、商场等场所中进行定位。定位技术也是机器人技术的重要部分，各种服务型机器人只有获取了较高精度的位姿信息，才能进行准确的导航、运动和精细操作。

现有的定位方案可以分为室外定位和室内定位，室外的定位服务已经非常普及，全球导航卫星系统(Global Navigation Satellite System，GNSS)的定位精度能够达到米级别，人们可以通过手机等智能终端十分便利地获取定位服务。但是GNSS信号在室内高度衰减，导致定位的精度严重降低，因此在室内定位方面，需要寻求其他的技术手段来获取高精度的定位。目前，国内外常用的室内定位技术有红外线、蓝牙、相机、无线保真(WirelessFidelity，WiFi)、射频识别(Radio Frequency Identification，RFID)、超宽带技术(UltraWideband，UWB)以及激光探测及测距(Light Detection and Ranging，LiDAR)等技术。相比于其他技术，UWB具有定位精度高、功耗低、信号有一定穿透性等优点，而UWB容易受到非视距等因素影响，产生定位误差；同时LiDAR具有分辨率高、信息量大、抗干扰能力强等优点，但又对环境特征依赖较大，在无特征或特征单一的环境中会产生较大的累计误差。

此外，机器人绑架问题也是室内定位的一大挑战，机器人绑架问题指机器人在运行过程中受外界干扰或定位失效。在丢失先前定位信息的情况下需要进行快速准确的重定位，以解决机器人绑架问题。

对于室内定位问题，已有许多基于单一传感器的定位方法，但这些方法都受制于该特定传感器的视距及其他缺点，从而导致定位精度差或无法解决机器人绑架问题。

发明内容

本发明提供了一种融合UWB和LiDAR的室内定位方法，以解决现有的室内定位方法，受制于特定传感器的视距及其他缺点，导致定位精度差或无法解决机器人绑架的问题。

为解决上述技术问题，本发明公开了一种融合UWB和LiDAR的室内定位方法，包括如下步骤：

步骤1，在室内部署UWB基站，在待定位主体上部署LiDAR，获取所述UWB基站分别在UWB地图坐标系与LiDAR地图坐标系中的坐标信息；

步骤2，获取所述UWB基站输出的TDOA信息，采用解算方法获取所述待定位主体的第一定位信息；

步骤3，获取LiDAR的输入信息，采用AMCL获取所述待定位主体的第二定位信息；

步骤4，根据所述坐标信息计算坐标转换，将通过所述UWB基站计算的第一定位信息转换到LiDAR地图坐标系下；

步骤5，对过去一段时间内所述第一定位信息产生的运动轨迹和第二定位信息产生的运动轨迹进行拟合，获取所述UWB基站定位的朝向信息，根据所述朝向信息补全第一定位信息；

步骤6，判断对所述待定位主体的定位是否异常，并根据判断结果调整定位权重；

步骤7，若对所述待定位主体的定位异常，根据所述第一定位信息对AMCL进行初始化，对所述待定位主体重新定位；

步骤8，根据所述第一定位信息、第二定位信息及定位权重计算并输出待定位主体的实时定位结果。

进一步地，在一种实现方式中，所述步骤1包括：

步骤1-1，在室内部署至少3台位于同一平面的UWB基站，在所述平面内选取任意位置作为原点，在所述平面内选取任意方向作为坐标轴，建立二维UWB地图坐标系，测定每个所述UWB基站在UWB地图坐标系中的坐标信息；

步骤1-2，在所述待定位主体上部署UWB标签及LiDAR，使得所述待定位主体运动时，所述待定位主体与UWB标签之间的相对位置，以及所述待定位主体与LiDAR之间的相对位置均保持不变；

步骤1-3，绘制所述待定位主体所在场所的地图，在所述地图中选取任意位置作为原点，在所述地图中选取任意方向作为坐标轴，建立二维LiDAR地图坐标系，在所述地图中标定每个UWB基站在LiDAR地图坐标系中的位置，即获取所述UWB基站在UWB地图坐标系中的坐标信息。

进一步地，在一种实现方式中，所述步骤2包括：

步骤2-1，获取所述UWB基站输出的TDOA信息，基于所述TDOA信息在室内区域内建立TDOA指纹地图，使用k最近邻算法(k-Nearest Neighbor，knn)在所述TDOA指纹地图上匹配接收到的TDOA信息，将k最近邻匹配结果

步骤2-2，进行taylor迭代，使所述大致定位结果更精确，利用TDOA数据构造对待测位置的非线性方程：

以所述k最近邻匹配结果(x

f

其中，i表示UWB基站的编号，

通过上述方程算得所述k最近邻匹配结果(x

本发明采用TDOA信号，使用TDOA信号能降低不同传感器之间时间同步的要求，在实际场景中需要部署至少3台基站。

进一步地，在一种实现方式中，所述步骤3包括：

步骤3-1，获取LiDAR的输入信息，若未进行过粒子初始化，则进行粒子初始化，在空间中以随机位姿散布若干粒子，所述粒子总数为M；若进行过粒子初始化，则更新所有粒子位姿，根据第i个粒子上一时刻，即所述第i个粒子在t-1时刻的后验位姿

步骤3-2，根据似然域模型更新所述每个粒子的权重

步骤3-3，根据所述每个粒子的权重

w

w

其中，α

具体的，α

步骤3-4，对于每个所述粒子，以

进一步地，在一种实现方式中，所述步骤4包括计算坐标转换的旋转量和平移量，所述坐标转换用于将第一定位信息转换至第二定位信息所在的LiDAR坐标系，具体的，由于人工标定UWB基站坐标时会产生误差，为了减小该误差对UWB地图坐标系和LiDAR地图坐标系间坐标转换的影响，需要进行多次计算：

步骤4-1，每次选取两个不同的UWB基站作为基站对，所述基站对包括第一UWB基站和第二UWB基站；所述第一UWB基站在UWB地图坐标系中的坐标为p

对于室内2D定位，只计算yaw角，pitch角和roll角均为0，并简化计算公式为

步骤4-2，选取不同的基站对多次执行所述步骤4-1的操作，并对获得的不同旋转量取平均作为坐标转换的旋转量；具体的，本实施例中执行次数选取了UWB基站数目次。

步骤4-3，选取一个标定基站，所述标定基站在UWB地图坐标系为p

步骤4-4，选取不同的标定基站多次执行所述步骤4-3的操作，并对获得的不同平移量取平均作为坐标转换的平移量。

进一步地，在一种实现方式中，由于采用低成本的UWB定位方案，无法通过解算方法得出朝向信息，因此通过UWB和LiDAR输出的运动状态拟合求取UWB定位的朝向信息，所述步骤5包括：

步骤5-1，记录当前时刻及过去n个历史时刻UWB和LiDAR获取的待定位主体的定位坐标；设当前时刻UWB输出的定位坐标为p

步骤5-2，获取一个旋转矩阵

步骤5-3，根据所述旋转矩阵R将当前时刻LiDAR输出的位姿进行转换，取转换结果的朝向部分作为当前时刻UWB定位的朝向信息，并根据所述朝向信息补全步骤2中的第一定位信息。

进一步地，在一种实现方式中，所述步骤6包括：

步骤6-1，通过UWB和LiDAR两者定位的位姿差判断待定位主体的定位是否异常；

步骤6-2，若判断结果为定位异常，则将LiDAR定位权重w

进一步地，在一种实现方式中，所述步骤6-1包括：

步骤6-1-1，若AMCL中的粒子未收敛，则计算第一定位信息转换至LiDAR坐标系后的结果和第二定位信息的欧氏距离作为位姿差；

若AMCL中的粒子已收敛，则计算所述步骤5中得到的旋转矩阵R对应欧拉角(θ

所述旋转矩阵R与欧拉角的转换关系如下：

设

则θ

步骤6-1-2，若所述位姿差大于人工设定的阈值diff_threshold，则认为定位产生异常，需要进行重定位。

进一步地，在一种实现方式中，所述步骤7包括：利用所述第一定位信息转换至LiDAR坐标系后的结果初始化AMCL中的粒子，随着所述待定位主体的运动收集LiDAR的输入，使AMCL中的粒子不断收敛。具体的，重定位时按高维高斯分布初始化AMCL中的所有粒子，该分布以当前UWB输出的位姿为均值，在各坐标上以0.5为方差，在各朝向上以π/12为方差定位主体运动，LiDAR接收到新的输入后，粒子逐渐收敛并能在步骤3中输出精度较高的定位结果。

进一步地，在一种实现方式中，所述步骤8包括：随着定位主体的运动，根据UWB和LiDAR的定位信息及定位权重计算并输出实时定位结果，所述实时定位结果表示为pose：

pose＝(1-w

其中，pose

由以上技术方案可知，本发明实施例提供一种融合UWB和LiDAR的室内定位方法，包括：在室内部署UWB基站，在待定位主体上部署LiDAR，获取UWB基站分别在UWB地图坐标系与LiDAR地图坐标系中的坐标信息；获取UWB基站输出的TDOA信息，采用解算方法获取待定位主体的第一定位信息；获取LiDAR的输入信息，采用AMCL获取待定位主体的第二定位信息；根据坐标信息计算坐标转换，将通过UWB基站计算的第一定位信息转换到LiDAR地图坐标系下；对过去一段时间内第一定位信息产生的运动轨迹和第二定位信息产生的运动轨迹进行拟合，获取UWB基站定位的朝向信息，根据朝向信息补全第一定位信息；判断对待定位主体的定位是否异常，并根据判断结果调整定位权重；若对待定位主体的定位异常，根据第一定位信息对AMCL进行初始化，对待定位主体重新定位；最终根据第一定位信息、第二定位信息及定位权重计算并输出待定位主体的实时定位结果。

现有技术中，受制于特定传感器的视距及其他缺点，导致定位精度差或无法解决机器人绑架的问题。而采用前述方法，通过融合两款低成本的传感器UWB和LiDAR进行定位，在成本较低、性能较差的嵌入式设备上进行实时的室内定位，利用UWB和LiDAR定位的优势进行互补，克服其各自的缺点，相对于现有技术，能够提高定位的精度，并解决机器人绑架问题，同时保证定位效率。

附图说明

为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例部分提供的一种融合UWB和LiDAR的室内定位方法的工作流程示意图；

图2是本发明实施例部分提供的一种融合UWB和LiDAR的室内定位方法中采用自适应蒙特卡洛定位方法处理输入的LiDAR信息的工作流程示意图；

图3a是本发明实施例部分提供的一种融合UWB和LiDAR的室内定位方法中实验场景示意图；

图3b是本发明实施例部分提供的一种融合UWB和LiDAR的室内定位方法中实验场景平面示意图；

图4a是本发明实施例部分提供的一种融合UWB和LiDAR的室内定位方法在一般运动场景下的预设路径示意图；

图4b是本发明实施例部分提供的一种融合UWB和LiDAR的室内定位方法在一般运动场景下的第一定位信息及实时定位结果示意图；

图5a是本发明实施例部分提供的一种融合UWB和LiDAR的室内定位方法在机器人绑架问题模拟场景下的预设机器人运动及人工搬运路径示意图；

图5b是本发明实施例部分提供的一种融合UWB和LiDAR的室内定位方法在机器人绑架问题模拟场景下绑架前的第一定位信息及实时定位结果示意图；

图5c是本发明实施例部分提供的一种融合UWB和LiDAR的室内定位方法在机器人绑架问题模拟场景下绑架结束时的第一定位信息及实时定位结果示意图；

图5d是本发明实施例部分提供的一种融合UWB和LiDAR的室内定位方法在机器人绑架问题模拟场景下绑架结束后的第一定位信息及实时定位结果示意图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

本发明实施例公开一种融合UWB和LiDAR的室内定位方法，本方法应用于复杂场景下稳定的室内定位，此类环境中UWB基站与标签间的障碍往往会使UWB信号产生NLOS误差，例如折射或反射导致信号测量时间不准；而长期持续的定位过程中，传感器的暂时性功能故障及定位主体的快速移动都会导致机器人绑架问题的产生，进而使一般的定位方法失效。

如图1所示，本实施例所述的一种融合UWB和LiDAR的室内定位方法，包括如下步骤：

步骤1，在室内部署UWB基站，在待定位主体上部署LiDAR，获取所述UWB基站分别在UWB地图坐标系与LiDAR地图坐标系中的坐标信息；本实施例中，需要保证所述UWB基站与待定位主体的相对位置，以及LiDAR与待定位主体的相对位置均不会随着定位主体的运动而产生较大的变化。

步骤2，获取所述UWB基站输出的TDOA信息，采用解算方法获取所述待定位主体的第一定位信息；

本实施例中，UWB基站提供多种测距信号，所述测距信号包括到达时间差(TimeDifference of Arrival，TDOA)信号和到达时间(Time of Arrival，TOA)信号，本发明采用TDOA信号，使用TDOA信号能降低不同传感器之间时间同步的要求，具体的，在实际场景中需要部署至少3台基站。

步骤3，获取LiDAR的输入信息，采用AMCL获取所述待定位主体的第二定位信息；

步骤4，根据所述坐标信息计算坐标转换，将通过所述UWB基站计算的第一定位信息转换到LiDAR地图坐标系下；

步骤5，对过去一段时间内所述第一定位信息产生的运动轨迹和第二定位信息产生的运动轨迹进行拟合，获取所述UWB基站定位的朝向信息，根据所述朝向信息补全第一定位信息；

步骤6，判断对所述待定位主体的定位是否异常，并根据判断结果调整定位权重；

步骤7，若对所述待定位主体的定位异常，根据所述第一定位信息对AMCL进行初始化，对所述待定位主体重新定位；

步骤8，根据所述第一定位信息、第二定位信息及定位权重计算并输出待定位主体的实时定位结果。

本实施例所述的一种融合UWB和LiDAR的室内定位方法中，所述步骤1包括：

步骤1-1，在室内部署至少3台位于同一平面的UWB基站，在所述平面内选取任意位置作为原点，在所述平面内选取任意方向作为坐标轴，建立二维UWB地图坐标系，测定每个所述UWB基站在UWB地图坐标系中的坐标信息；

步骤1-2，在所述待定位主体上部署UWB标签及LiDAR，使得所述待定位主体运动时，所述待定位主体与UWB标签之间的相对位置，以及所述待定位主体与LiDAR之间的相对位置均保持不变；

步骤1-3，绘制所述待定位主体所在场所的地图，在所述地图中选取任意位置作为原点，在所述地图中选取任意方向作为坐标轴，建立二维LiDAR地图坐标系，在所述地图中标定每个UWB基站在LiDAR地图坐标系中的位置，即获取所述UWB基站在UWB地图坐标系中的坐标信息。

本实施例所述的一种融合UWB和LiDAR的室内定位方法中，所述步骤2包括：

步骤2-1，获取所述UWB基站输出的TDOA信息，基于所述TDOA信息在室内区域内建立TDOA指纹地图，使用k最近邻算法(k-Nearest Neighbor，knn)在所述TDOA指纹地图上匹配接收到的TDOA信息，将k最近邻匹配结果

步骤2-2，进行taylor迭代，使所述大致定位结果更精确，利用TDOA数据构造对待测位置的非线性方程：

以所述k最近邻匹配结果(x

f

其中，i表示UWB基站的编号，

通过上述方程算得所述k最近邻匹配结果(x

如图2所示，本实施例所述的一种融合UWB和LiDAR的室内定位方法中，所述步骤3包括：

步骤3-1，获取LiDAR的输入信息，若未进行过粒子初始化，则进行粒子初始化，在空间中以随机位姿散布若干粒子，所述粒子总数为M；若进行过粒子初始化，则更新所有粒子位姿，根据第i个粒子上一时刻，即所述第i个粒子在t-1时刻的后验位姿

步骤3-2，根据似然域模型更新所述每个粒子的权重

步骤3-3，根据所述每个粒子的权重

w

w

其中，α

具体的，α

步骤3-4，对于每个所述粒子，以

由于人工标定UWB基站坐标时会产生误差，为了减小该误差对UWB地图坐标系和LiDAR地图坐标系间坐标转换的影响，需要进行多次计算。因此，本实施例所述的一种融合UWB和LiDAR的室内定位方法中，所述步骤4包括计算坐标转换的旋转量和平移量，所述坐标转换用于将第一定位信息转换至第二定位信息所在的LiDAR坐标系：

步骤4-1，每次选取两个不同的UWB基站作为基站对，所述基站对包括第一UWB基站和第二UWB基站；所述第一UWB基站在UWB地图坐标系中的坐标为p

对于室内2D定位，只计算yaw角，pitch角和roll角均为0，并简化计算公式为

步骤4-2，选取不同的基站对多次执行所述步骤4-1的操作，并对获得的不同旋转量取平均作为坐标转换的旋转量；具体的，本实施例中选取了UWB基站数目次。

步骤4-3，选取一个标定基站，所述标定基站在UWB地图坐标系为p

步骤4-4，选取不同的标定基站多次执行所述步骤4-3的操作，并对获得的不同平移量取平均作为坐标转换的平移量。

由于采用低成本的UWB定位方案，无法通过解算方法得出朝向信息，通过UWB和LiDAR输出的运动状态拟合求取UWB定位的朝向信息。因此，本实施例所述的一种融合UWB和LiDAR的室内定位方法中，所述步骤5包括：

步骤5-1，记录当前时刻及过去n个历史时刻UWB和LiDAR获取的待定位主体的定位坐标；设当前时刻UWB输出的定位坐标为p

步骤5-2，获取一个旋转矩阵

步骤5-3，根据所述旋转矩阵R将当前时刻LiDAR输出的位姿进行转换，取转换结果的朝向部分作为当前时刻UWB定位的朝向信息，并根据所述朝向信息补全步骤2中的第一定位信息。

本实施例所述的一种融合UWB和LiDAR的室内定位方法中，所述步骤6包括：

步骤6-1，通过UWB和LiDAR两者定位的位姿差判断待定位主体的定位是否异常；

步骤6-2，若判断结果为定位异常，则将LiDAR定位权重w

本实施例所述的一种融合UWB和LiDAR的室内定位方法中，所述步骤6-1包括：

步骤6-1-1，若AMCL中的粒子未收敛，则计算第一定位信息转换至LiDAR坐标系后的结果和第二定位信息的欧氏距离作为位姿差；

若AMCL中的粒子已收敛，则计算所述步骤5中得到的旋转矩阵R对应欧拉角(θ

所述旋转矩阵R与欧拉角的转换关系如下：

设

则θ

步骤6-1-2，若所述位姿差大于人工设定的阈值diff_threshold，则认为定位产生异常，需要进行重定位。

具体的，本实施例中，AMCL中的粒子未收敛时，取diff_threshold＝1；AMCL中的粒子已收敛时，取diff_threshold＝π/6。

本实施例所述的一种融合UWB和LiDAR的室内定位方法中，所述步骤7包括：利用所述第一定位信息转换至LiDAR坐标系后的结果初始化AMCL中的粒子，随着所述待定位主体的运动收集LiDAR的输入，使AMCL中的粒子不断收敛。具体的，本实施例中，重定位时按高维高斯分布初始化AMCL中的所有粒子，该分布以当前UWB输出的位姿为均值，在各坐标上以0.5为方差，在各朝向上以π/12为方差。定位主体运动，LiDAR接收到新的输入后，粒子逐渐收敛并能在步骤3中输出精度较高的定位结果。

本实施例所述的一种融合UWB和LiDAR的室内定位方法中，所述步骤8包括：随着定位主体的运动，根据UWB和LiDAR的定位信息及定位权重计算并输出实时定位结果，所述实时定位结果表示为pose：

pose＝(1-w

其中，pose

本实施例中，在硬件接受的频率下不断执行上述步骤2到步骤8，并输出实时定位结果，所述硬件包括UWB基站、LiDAR及计算设备等。

实施例

为了验证本实施例所述的一种融合UWB和LiDAR的室内定位方法的有效性，在实际场景中进行实例验证，实验场景设置为长6米，宽4米的房间，该房间中布置了一些障碍物，实验场景如图3a与图3b所示，图3a为实验场景的布置及网格标注，其中每个网格长宽均为1米，图3b为实验场景的平面示意图，以黑色块标示障碍物。在场景中部署移动机器人，按照以下步骤进行室内定位：

步骤1，本实施例中，在场景的四角部署UWB基站，在待定位主体上，即移动机器人上安装LiDAR，对UWB基站所在的位置进行测定，并在事先绘制的场景二维地图中标示出各UWB基站的坐标，即获取UWB基站分别在UWB地图坐标系与LiDAR地图坐标系中的坐标信息，通过对移动机器人发送运动指令，使其按照图4a中的预设路径行走；

步骤2，获取UWB输出的TDOA信息，采用解算方法获取移动机器人的第一定位信息；

步骤3，获取LiDAR的输入信息，利用自适应蒙特卡洛定位方法(Adaptive MonteCarlo Localization，AMCL)获取移动机器人的第二定位信息；

步骤4，根据UWB地图坐标系CS_uwb和LiDAR地图坐标系CS_lidar计算坐标转换，将不同传感器的定位信息转换到同一坐标系下，即将通过所述UWB基站计算的第一定位信息转换到LiDAR地图坐标系下；

步骤5，将UWB和LiDAR的历史定位信息分别汇总，得到UWB和LiDAR各自的运动状态信息，对两者运动状态拟合求得UWB定位的朝向信息，即对过去一段时间内第一定位信息产生的运动轨迹和第二定位信息产生的运动轨迹进行拟合，根据朝向信息补全第一定位信息；

步骤6，通过UWB和LiDAR两者定位的位姿差判断对移动机器人的定位是否异常，如果对移动机器人的定位异常，则减小LiDAR定位权重w

步骤7，如果对移动机器人的定位异常，根据第一定位信息对AMCL进行初始化，对移动机器人重新定位，即初始化AMCL中的粒子，随着移动机器人的运动收集LiDAR输入，使AMCL中的粒子不断收敛；

步骤8，根据第一定位信息、第二定位信息及定位权重及定位权重计算当前位姿，随着移动机器人的运动，即输出移动机器人的实时定位结果。

将移动机器人行走过程中融合定位的结果相连，绘制出其行走路线，如图4b中的平滑线条所示。图4b中偏差和抖动较大的线条为仅使用UWB信号解算得到的原始定位路线，经过手工测量，融合定位的结果误差小于10厘米，上述结果可见，本发明在精度上得到了令人满意的结果。

同时，本发明针对机器人绑架问题的解决进行了实例验证，具体流程为：在上述实例验证过程中，当移动机器人运动至如图5a所示的“搬走”位置时，发送指令停止移动机器人的运动，并手工将其搬运至如图5a所示的“放置”位置，再次进入定位流程，待步骤7中的重定位过程执行完毕后，继续按照既定路线使机器人运动。手工搬运模拟了机器人定位失效的情况，在短暂的失效后，本发明重新获得了较精确的定位。

在机器人绑架问题的实例验证中，搬走前的定位结果如图5b所示，搬走至机器人重定位完成前的定位结果如图5c所示，重定位完成至运行结束的定位结果如图5d所示，各图中平滑线条表示融合定位结果，重定位过程在5秒内完成。由结果可见，本发明在移动机器人搬运至新的位置后快速准确地进行了重定位，能够有效地解决机器人绑架问题。

由以上技术方案可知，本发明实施例提供一种融合UWB和LiDAR的室内定位方法，包括：在室内部署UWB基站，在待定位主体上部署LiDAR，获取UWB基站分别在UWB地图坐标系与LiDAR地图坐标系中的坐标信息；获取UWB基站输出的TDOA信息，采用解算方法获取待定位主体的第一定位信息；获取LiDAR的输入信息，采用AMCL获取待定位主体的第二定位信息；根据坐标信息计算坐标转换，将通过UWB基站计算的第一定位信息转换到LiDAR地图坐标系下；对过去一段时间内第一定位信息产生的运动轨迹和第二定位信息产生的运动轨迹进行拟合，获取UWB基站定位的朝向信息，根据朝向信息补全第一定位信息；判断对待定位主体的定位是否异常，并根据判断结果调整定位权重；若对待定位主体的定位异常，根据第一定位信息对AMCL进行初始化，对待定位主体重新定位；最终根据第一定位信息、第二定位信息及定位权重计算并输出待定位主体的实时定位结果。

现有技术中，受制于特定传感器的视距及其他缺点，导致定位精度差或无法解决机器人绑架的问题。而采用前述方法，通过融合两款低成本的传感器UWB和LiDAR进行定位，在成本较低、性能较差的嵌入式设备上进行实时的室内定位，利用UWB和LiDAR定位的优势进行互补，克服其各自的缺点，相对于现有技术，能够提高定位的精度，并解决机器人绑架问题，同时保证定位效率。

具体实现中，本发明还提供一种计算机存储介质，其中，该计算机存储介质可存储有程序，该程序执行时可包括本发明提供的一种融合UWB和LiDAR的室内定位方法的各实施例中的部分或全部步骤。所述的存储介质可为磁碟、光盘、只读存储记忆体(英文：read-only memory，简称：ROM)或随机存储记忆体(英文：random access memory，简称：RAM)等。

本领域的技术人员可以清楚地了解到本发明实施例中的技术可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解，本发明实施例中的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例或者实施例的某些部分所述的方法。

本说明书中各个实施例之间相同相似的部分互相参见即可。以上所述的本发明实施方式并不构成对本发明保护范围的限定。