一种多个测距传感器的无人艇近岸实时定位与建图方法技术

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

【技术实现步骤摘要】
一种多个测距传感器的无人艇近岸实时定位与建图方法


[0001]本专利技术涉及无人艇
，特别是一种多个测距传感器的无人艇近岸实时定位与建图方法。

技术介绍

[0002]目前市面上的测绘无人艇大多采用惯导系统定位加激光雷达建图或惯导系统定位加声呐建图的方案。基于惯导系统定位的无人艇在经过桥洞或一些多遮挡物地区时，由于惯导系统信号不稳定，会造成无人艇位姿估计产生误差从而造成测绘建图的不精确。
[0003]此外，目前多传感器融合定位仍有很多未解决的问题。如何在无人艇上使用多个激光雷达进行厘米级的定位和多个声呐构建高精度的三维点云地图，是无人艇多传感器融合定位的热门话题。

技术实现思路

[0004]针对上述缺陷，本专利技术的目的在于提出一种多个测距传感器的无人艇近岸实时定位与建图方法。
[0005]为达此目的，本专利技术采用以下技术方案：
[0006]一种多个测距传感器的无人艇近岸实时定位与建图方法包括以下步骤：
[0007]步骤A：惯性测量单元获取无人艇的预测位姿数据；
[0008]步骤B：激光雷达获取雷达点云数据，将雷达点云数据投影至深度图，保留深度图上的地面点与分割点。
[0009]步骤C：将深度图分割成六个子深度图，通过每个激光点的曲率得到特征点集，并将所有激光雷达的特征点集转化至无人艇的坐标；
[0010]步骤D：通过相邻两帧的雷达点云数据得到当前无人艇的相对位姿变换矩阵；
[0011]步骤E：获取激光雷达里程计因子、惯性测量单元因子、多普勒速度计因子、北斗卫星定位因子和闭环因子并通过因子图的形式优化当前无人艇位姿；
[0012]步骤F：通过各个激光雷达点云数据与各个声呐点云数据以及无人艇位姿构建出三维云点图。
[0013]在一个实施例中，所述步骤A的具体如下：
[0014]所惯性测量单元根据当前时刻的角速度与角加速度通过四阶龙格库塔法预积分得到无人艇的预测位姿，其中所述无人艇的预测位姿包括当前帧与上一帧的相对旋转相对位移速度V
t
、角速度计偏置b
g,t
和角加速度计偏置b
a,t
；
[0015]其中相对旋转的四阶龙格库塔预积分公式为：
[0016][0017]其中的取值范围如下
[0018]角速度是一个三维的向量，和分别表示三个方向轴的数据；
[0019][0020]所述速度V
t
的四阶龙格库塔预积分公式为：
[0021][0022][0023][0024][0025]其中，V
t
为当前t时刻的速度，V
t
‑1为t
‑
1时刻的速度，C(q)函数是从四元数转换到旋转矩阵R的函数；
[0026]所述相对位移的四阶龙格库塔预积分公式为：
[0027][0028]其中的取值范围如下
[0029]所述角速度计偏置b
g,t
的四阶龙格库塔预积分公式为：
[0030]b
g,t
＝b
g,t
‑1，其中b
g,t
‑1为t
‑
1时刻的角速度计偏置；
[0031]所述角加速度计偏置b
a,t
的四阶龙格库塔预积分公式为：
[0032]b
a,t
＝b
a,t
‑1，其中b
a,t
‑1为t
‑
1时刻的角加速度计偏置；
[0033]是一个四元数，q
w
表示实部，q
v
表示虚部，的作用是把四元数归一化，
w
g表示当前位置的地球加速度、q0表示t
‑
1时刻的姿态。
[0034]在一个实施例中，所述步骤B的具体步骤如下：
[0035]步骤B1：激光雷达获取当前时刻t的雷达点云数据其中P代表点云，并将雷达点云数据对应投射于分辨率为1800*16深度图的像素上；
[0036]步骤B2：对深度图进行有序化、深度值划分、降维、直线拟合、地面点筛选处理，获取地面点并进行标记；
[0037]步骤B3：使用基于深度图的障碍物快速分割算法把深度图中的点云分割成一个一个的聚类；
[0038]步骤B4：剔除点数少于30的聚类，将同一个聚类的激光点标记为分割点，删除分割点与地面点以外的激光点，得到处理深度图。
[0039]在一个实施例中，所述步骤C的具体步骤如下：
[0040]步骤C1：将处理深度图分割成六个分辨率为300*16的子深度图，获取子深度图的激光点
Li
p
j
的曲率，其中激光点
Li
p
j
的曲率获取公式如下：
[0041]r
k
和r
j
表示为子深度图中像素点的深度值，k、j为子深度图中的索引值，S是同一行连续十个激光点的索引的集合；
[0042]步骤C2：根据激光点
Li
p
j
的曲率，得到出边缘点与平面点；
[0043]选取每个子深度图中每一行中两个分割点组成边缘特征点集，其中组成边缘特征点集的分割点为子深度图每一行中拥有最大曲率的两个边缘点；
[0044]选取每个子深度图中每一行中最小曲率c的四个平面点组成平面特征点集；
[0045]步骤C3：所有激光雷达的平面特征点集与缘特征点集通过外参将激光雷达的坐标转换到无人艇机体坐标系，并组成当前时刻的特征点集
b
F
t
，所述特征点集
b
F
t
包括缘特
征点集与平面特征点集。
[0046]在一个实施例中，所述步骤D的具体步骤如下：
[0047]步骤D1：将当前时刻相邻的二十五个关键帧中的特征点集
b
F
t
进行位姿变换得到局部特征点云图
w
M
t
，其中局部特征点云图
w
M
t
包括局部边缘特征点云图和局部平面特征点云图
[0048]w
F
ts
为局部边缘特征点云图的特征点集；
[0049]w
F
tp
为局部平面特征点云图的特征点集；
[0050]步骤D2：使用四阶龙格库塔法预积分获取局部特征点云图的位姿变换矩阵
[0051]步骤D3：获取局部特征点云图
w
M
t
的所述特征点集
b
F
t1
，并将
b
F
t1
从无人艇机体坐标系转换到导航坐标系；
[0052]步骤D4：在局部边缘特征点云图选择三个激光点
w
p
as
、
w...

【技术保护点】

【技术特征摘要】
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
表示虚部，的作用是把四元数归一化，
w
g表示当前位置的地球加速度、q0表示t
‑
1时刻的姿态。3.根据权利要求1所述的一种多个测距传感器的无人艇近岸实时定位与建图方法，其
特征在于：所述步骤B的具体步骤如下：步骤B1：激光雷达获取当前时刻t的雷达点云数据其中P代表点云，并将雷达点云数据对应投射于分辨率为1800*16深度图的像素上；步骤B2：对深度图进行有序化、深度值划分、降维、直线拟合、地面点筛选处理，获取地面点并进行标记；步骤B3：使用基于深度图的障碍物快速分割算法把深度图中的点云分割成一个一个的聚类；步骤B4：剔除点数少于30的聚类，将同一个聚类的激光点标记为分割点，删除分割点与地面点以外的激光点，得到处理深度图。4.根据权利要求3所述的一种多个测距传感器的无人艇近岸实时定位与建图方法，其特征在于：所述步骤C的具体步骤如下：步骤C1：将处理深度图分割成六个分辨率为300*16的子深度图，获取子深度图的激光点的曲率，其中激光点的曲率获取公式如下：r
k
和r
j
表示为子深度图中像素点的深度值，k、j为子深度图中的索引值，S是同一行连续十个激光点的索引的集合；步骤C2：根据激光点的曲率，得到出边缘点与平面点；选取每个子深度图中每一行中两个分割点组成边缘特征点集，其中组成边缘特征点集的分割点为子深度图每一行中拥有最大曲率的两个边缘点；选取每个子深度图中每一行中最小曲率c的四个平面点组成平面特征点集；步骤C3：所有激光雷达的平面特征点集与缘特征点集通过外参将激光雷达的坐标转换到无人艇机体坐标系，并组成当前时刻的特征点集
b
F
t
，所述特征点集
b
F
t
包括缘特征点集与平面特征点集。5.根据权利要求4所述的一种多个测距传感器的无人艇近岸实时定位与建图方法，其特征在于：所述步骤D的具体步骤如下：步骤D1：将当前时刻相邻的二十五个关键帧中的特征点集
b
F
t
进行位姿变换得到局部特征点云图
w
M
t
，其中局部特征点云图
w
M
t
包括局部边缘特征点云图和局部平面特征点云图图
w
F
ts
为局部边缘特征点云图的特征点集；
w
F
tp
为局部平面特征点云图的特征点集；步骤D2：使用四阶龙格库塔法预积分获取局部特征点云图的位姿变换矩阵步骤D3：获取局部特征点云图
w
M
t
的所述特征点集
b
F
t1
，并将
b
F
t1
从无人艇机体坐标系转
换到导航坐标系；步骤D4...

【专利技术属性】
技术研发人员：鲁仁全，陈梓杰，徐雍，饶红霞，林明，
申请(专利权)人：广东工业大学，
类型：发明
国别省市：