[发明专利]一种矿井下多传感器融合的外部参数标定及精准定位方法

说明书

本发明公开了一种矿井下多传感器融合的外部参数标定及精准定位方法，外部参数标定的步骤：获取井巷内各UWB锚节点在世界坐标系下的绝对位置，获取各个传感器的观测信息；确定激光雷达传感器与惯性测量单元之间的旋转外参，并自动判断旋转标定过程是否完成；构建基于滑动窗口的线性估计器，估计相关状态量，并自动判断线性估计器是否收敛；利用估计出的UWB锚节点在局部坐标系下的位置坐标，与UWB锚节点在世界坐标系下的绝对位置坐标进行对齐，确定局部坐标系与世界坐标系之间的外参变换；输出所有估计的参数值。本发明能够实现在线实时或间歇性的外部参数自动标定，并精准构建地图及定位。

技术领域

本发明属于矿井探测领域，特别涉及了一种矿井下的传感器外部参数标定方法。

背景技术

特种车辆或机器人，尤其是在封闭的煤矿井下工作的具备自主行走能力的智能化矿用机器人，需要基于多传感器融合的结果实现可靠的全场景无缝定位以及地图构建。对于井下作业的机器人，使用激光雷达(LiDAR)/惯性测量单元(IMU)/超宽带模组的多传感器融合方法是有效的方案。激光雷达具有测量精度高、扫描范围广、强信噪比等特点，缺点是在结构相似的巷道结构退化场景下，利用激光点云进行扫描匹配和特征提取都很困难，导致定位精度低。惯性测量单元短期测量精度高，适合颠簸、快速旋转等场合下为其他传感器提供短期可靠的运动状态估计，但是直接使用惯性导航在恶劣条件下累积误差很大。超宽带的测距精度高，相较于其他无线定位方式具有更好的可靠性，对于多径效应和信号丢失具有更强的抑制能力，缺点是直接进行二维和三维定位时需要使用大量固定基站，基础设施部署代价较大，但是可以作为辅助传感器在巷道方向进行精确的一维定位以降低成本。利用激光雷达/惯性测量单元/超宽带模组的多传感器融合方法可以实现矿用机器人在封闭、恶劣的复杂场景下精确定位和高精度环境建模，有效应对场景少结构时退化、颠簸路面振动剧烈、无线信号丢失等可能工况，同时减少UWB固定基站的使用数量，降低成本。

多传感器融合的前提是精确已知传感器之间的外参变换关系(包括传感器之间的相对平移和相对旋转)，从而实现统一的状态估计。由于长时间工作在井下颠簸、振动剧烈、频繁撞击的环境作业条件下，传感器之间的外参不可避免会发生变化，需要具备在线估计的方法来实时或间歇性进行外参标定与自动校准。目前无论是学术界还是工业界，外参标定主要集中在激光雷达/相机，相机/惯性测量单元上，没有专门的方法可以同时在线标定激光雷达/惯性测量单元/超宽带模组。

因此，为了满足井下机器人及其他工作在封闭场景下使用三维激光雷达/惯性导航单元/超宽带模组进行定位、建图、感知等任务的实际需求，亟待设计出可以自动在线标定其外部参数(以下简称外参)的方法。

发明内容

为了解决上述背景技术提到的技术问题，本发明提出了一种矿井下多传感器融合的外部参数标定及精准定位方法。

为了实现上述技术目的，本发明的技术方案为：

一种矿井下多传感器融合的外部参数标定方法，所述多传感器包括激光雷达传感器、惯性测量单元和超宽带模组，所述超宽带模组包括UWB移动节点和若干UWB锚节点，所述激光雷达传感器、惯性测量单元和UWB移动节点分别设置在井下移动设备上，所述若干UWB锚节点分布于井巷内不同位置；该外部参数标定方法如下：

(1)获取井巷内各UWB锚节点在世界坐标系下的绝对位置，获取各个传感器的观测信息；

(2)控制井下移动设备进行包含一定旋转的运动，确定激光雷达传感器与惯性测量单元之间的旋转外参，并自动判断旋转标定过程是否完成；

(3)构建基于滑动窗口的线性估计器，估计重力的方向、各UWB锚节点在局部坐标系下的位置、激光雷达传感器与惯性测量单元之间的平移外参、UWB移动节点与惯性测量单元之间的平移外参、惯性测量单元在局部坐标系下的位置和速度，并自动判断线性估计器是否收敛；