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

权利要求书

1.一种多个测距传感器的无人艇近岸实时定位与建图方法，其特征在于，包括以下步骤：

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

步骤B：激光雷达获取雷达点云数据，将雷达点云数据投影至深度图，保留深度图上的地面点与分割点，其中所述分割点为同一个聚类的激光点；

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

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

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

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

2.根据权利要求1所述的一种多个测距传感器的无人艇近岸实时定位与建图方法，其特征在于：所述步骤A的步骤具体如下：

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

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

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

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

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

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

其中的取值范围如下

所述角速度计偏置b_g，t的四阶龙格库塔预积分公式为：

b_g，t＝b_g，t-1，其中b_g，t-1为t-1时刻的角速度计偏置；

所述角加速度计偏置b_a，t的四阶龙格库塔预积分公式为：

b_a，t＝b_a，t-1，其中b_a，t-1为t-1时刻的角加速度计偏置；

是一个四元数，q_w表示实部，q_v表示虚部，的作用是把四元数归一化，^wg表示当前位置的地球加速度、q₀表示t-1时刻的姿态。

3.根据权利要求1所述的一种多个测距传感器的无人艇近岸实时定位与建图方法，其特征在于：所述步骤B的具体步骤如下：

步骤B1：激光雷达获取当前时刻t的雷达点云数据其中P代表点云，并将雷达点云数据对应投射于分辨率为1800*16深度图的像素上；

步骤B2：对深度图进行有序化、深度值划分、降维、直线拟合、地面点筛选处理，获取地面点并进行标记；

步骤B3：使用基于深度图的障碍物快速分割算法把深度图中的点云分割成一个一个的聚类；

步骤B4：剔除点数少于30的聚类，将同一个聚类的激光点标记为分割点，删除分割点与地面点以外的激光点，得到处理深度图。