[发明专利]一种多个测距传感器的无人艇近岸实时定位与建图方法

说明书

一种多个测距传感器的无人艇近岸实时定位与建图方法，包括以下步骤：步骤A：惯性测量单元获取无人艇的预测位姿数据；步骤B：激光雷达获取雷达点云数据，将雷达点云数据投影至深度图，保留深度图上的地面点与分割点。步骤C：将深度图分割成六个子深度图，通过每个激光点的曲率得到特征点集，并将所有激光雷达的特征点集转化至无人艇的坐标；步骤D：通过相邻两帧的雷达点云数据得到当前无人艇的相对位姿变换矩阵；步骤E：获取激光雷达里程计因子、惯性测量单元因子、多普勒速度计因子、北斗卫星定位因子和闭环因子并通过因子图的形式优化当前无人艇位姿；步骤F：通过各个激光雷达点云数据与各个声呐点云数据以及无人艇位姿构建出三维云点图。

技术领域

本发明涉及无人艇技术领域，特别是一种多个测距传感器的无人艇近岸实时定位与建图方法。

背景技术

目前市面上的测绘无人艇大多采用惯导系统定位加激光雷达建图或惯导系统定位加声呐建图的方案。基于惯导系统定位的无人艇在经过桥洞或一些多遮挡物地区时，由于惯导系统信号不稳定，会造成无人艇位姿估计产生误差从而造成测绘建图的不精确。

此外，目前多传感器融合定位仍有很多未解决的问题。如何在无人艇上使用多个激光雷达进行厘米级的定位和多个声呐构建高精度的三维点云地图，是无人艇多传感器融合定位的热门话题。

发明内容

针对上述缺陷，本发明的目的在于提出一种多个测距传感器的无人艇近岸实时定位与建图方法。

为达此目的，本发明采用以下技术方案：

一种多个测距传感器的无人艇近岸实时定位与建图方法包括以下步骤：

步骤A：惯性测量单元获取无人艇的预测位姿数据；

步骤B：激光雷达获取雷达点云数据，将雷达点云数据投影至深度图，保留深度图上的地面点与分割点。

步骤C：将深度图分割成六个子深度图，通过每个激光点的曲率得到特征点集，并将所有激光雷达的特征点集转化至无人艇的坐标；

步骤D：通过相邻两帧的雷达点云数据得到当前无人艇的相对位姿变换矩阵；

步骤E：获取激光雷达里程计因子、惯性测量单元因子、多普勒速度计因子、北斗卫星定位因子和闭环因子并通过因子图的形式优化当前无人艇位姿；

步骤F：通过各个激光雷达点云数据与各个声呐点云数据以及无人艇位姿构建出三维点云图。

在一个实施例中，所述步骤A的具体如下：

所惯性测量单元根据当前时刻的角速度与角加速度通过四阶龙格库塔法预积分得到无人艇的预测位姿，其中所述无人艇的预测位姿包括当前帧与上一帧的相对旋转相对位移速度V_t、角速度计偏置b_g，t和角加速度计偏置b_a，t；

其中相对旋转的四阶龙格库塔预积分公式为：

其中的取值范围如下角速度是一个三维的向量，和分别表示三个方向轴的数据；

所述速度V_t的四阶龙格库塔预积分公式为：

其中，V_t为当前t时刻的速度，V_t-1为t-1时刻的速度，C(q)函数是从四元数转换到旋转矩阵R的函数；

所述相对位移的四阶龙格库塔预积分公式为：