超宽带和激光雷达融合室内定位方法及相关设备

本发明涉及一种超宽带和激光雷达融合室内定位方法及相关设备，该方法包括：获取UWB和LiDAR的定位轨迹；计算参考轨迹坐标；对于一个时间点，分别通过若干种融合算法对UWB和LiDAR的定位轨迹坐标进行融合，并计算各种融合算法的融合坐标与参考轨迹坐标的误差，以误差最小的融合坐标作为最优估计坐标；分别计算UWB、LiDAR的定位轨迹坐标和最优估计坐标与参考轨迹坐标的距离并进行比较，以最小距离对应的坐标作为当前时间点的融合轨迹坐标；对于所有时间点依次分别进行以上处理，实现分段描点融合，得到所有时间点的融合轨迹坐标；结合所有时间点的融合轨迹坐标，即得到最终的融合轨迹并输出。该方法有利于提高室内定位的精度和鲁棒性，而且计算复杂度低。