基于雷达、超宽带和惯性导航的采掘装备定位系统及方法

一种基于雷达、超宽带和惯性导航的采掘装备定位系统及方法，系统：多源传感器单元、数据同步与采集模块和预处理模块依次连接，状态融合模块分别与预处理模块、UWB测距误差补偿模块、雷达运动估计模块和位姿输出模块连接，上位机与位姿输出模块连接；方法：采集多源数据；进行时间戳对齐和格式转换处理，并对点云数据进行特征和速度约束信息提取；采用加权最小二乘法对测距数据进行拟合修正，通过误差加权平均算法得到位置信息；进行特征匹配，并结合KISS‑ICP点云配准方法，获取速度估计结果；进行数据融合，并采用扩展卡尔曼滤波算法获得位姿估计结果；输出装备位置和姿态信息。本发明能实现采掘装备姿态与位置信息的高精度实时估计。