近岸复杂水域的强一致性里程计及高精度建图方法和系统技术方案

本发明专利技术涉及水域定位技术领域，尤其涉及一种近岸复杂水域的强一致性里程计及高精度建图方法和系统。本发明专利技术采用自适应可观测性分析算法，保障了里程计输出的一致性和相对精度；其次，基于滑窗优化器综合里程计约束、预积分约束、全球卫星导航约束和先验地图约束，确保水面移动载体在复杂近岸水域仍能实现厘米级的高精度全局定位；利用两步两次全局位姿图优化的手段，剔除全球卫星导航约束和闭环约束中的离群值，有效提升了三维点云地图的全局一致性和精度，相对于现有技术，能够提升水面移动载体在复杂近岸水域定位效率和一致性，以及建图的精度和平滑度。图的精度和平滑度。图的精度和平滑度。

【技术实现步骤摘要】
近岸复杂水域的强一致性里程计及高精度建图方法和系统


[0001]本专利技术涉及水域定位
，尤其涉及一种近岸复杂水域的强一致性里程计及高精度建图方法和系统。

技术介绍

[0002]目前市面上的水面智能无人移动载体大多采用组合导航或者加入激光雷达、摄像头等进行融合定位。基于组合导航的定位成本高，在近岸水域这种多遮挡物地区会产生多径效应，导致定位结果有较大的漂移。传统基于激光雷达、摄像头和组合导航的融合定位与建图技术在组合导航信号间断或观测退化时，会出现里程计跳变、前后估计不一致，建图模糊等问题。
[0003]如何在降低定位和建图成本的同时，克服传感器在某些场景失效，保障水面移动载体在近岸复杂水域能够获得鲁棒强、一致性高的里程计信息和构建精确的三维点云地图，是目前水面测绘建图和自主导航规划的核心研究热点。

技术实现思路

[0004]本专利技术提供一种面向近岸复杂水域的强一致性里程计及高精度建图方法和系统，旨在解决现有技术中在近岸复杂水域环境下的定位技术所具有的定位时延高、一致性低，建图精度差、平滑度差等问题。
[0005]为解决上述问题，第一方面，本专利技术实施例提供一种近岸复杂水域的强一致性里程计及高精度建图的方法，所述方法包括以下步骤：
[0006]S1、基于迭代误差卡尔曼滤波器构建激光
‑
惯性里程计，并通过所述激光
‑
惯性里程计采集近岸复杂水域的局部里程计数据，同时，获取近岸复杂水域的惯性测量单元数据、全球卫星导航观测数据以及先验地图数据；
[0007]S2、通过对预设时间段内的所述局部里程计数据、所述惯性测量单元数据、所述全球卫星导航观测数据以及所述先验地图数据进行综合处理，构建批量非线性最小二乘问题，并根据所述非线性最小二乘问题计算输出得到全局定位信息；
[0008]S3、通过对所有所述局部里程计数据、所述惯性测量单元数据、所述全球卫星导航观测数据进行综合处理，构建关于所述全局定位信息的全局位姿图优化问题，并根据所述全局位姿图优化问题计算输出得到三维点云地图，并进一步生成概率体素地图，之后，融合所述全局定位信息和所述概率体素地图的数据实现近岸复杂水域的定位。
[0009]更进一步地，步骤S1中，所述迭代误差卡尔曼滤波器满足以下关系式：
[0010][0011]u
k
＝[ω
m，k
，a
m，k
][0012]w
k
＝[n
g，k
，n
a，k
，n
bg，k
，n
ba，k
]；
[0013]其中，x
k
为状态量，u
k
为输入量，w
k
为噪声量；为k时刻机体系相对于LIO系的旋
转；为k时刻机体系相对于LIO系的平移；为k时刻机体系相对于LIO系的速度；b
g，k
为k时刻陀螺仪的零偏；b
a，k
为k时刻加速度计的零偏；分别为激光雷达相对于惯性测量单元的安装外参；ω
m，k
为k时刻陀螺仪测量的角速度；a
m，k
为k时刻加速度计测量的加速度；n
g，k
为k时刻陀螺仪的测量噪声；n
a，k
为k时刻加速度计的测量噪声；n
bg，k
为k时刻陀螺仪的零偏噪声；n
ba，k
为k时刻加速度计的零偏噪声。
[0014]更进一步地，步骤S1还包括：
[0015]对所述激光
‑
惯性里程计进行可观测度分析，对于所述误差状态卡尔曼滤波器中每一个状态向量的分量x
k(j)
(j＝1，2，...，n)在0时刻协方差与k时刻协方差的比值：
[0016][0017]其中，P
0(j,j)
表示为0时刻协方差矩阵中第j行第j列的元素；P
k(j,j)
表示为k时刻协方差矩阵中第j行第j列的元素；
[0018]当ρ
k(j)
小于预设阈值时，将所述激光
‑
惯性里程计的输出变换为：
[0019][0020][0021][0022]其中，为ENU系相对于LIO系的外参变换矩阵，为所述先验地图数据。
[0023]更进一步地，步骤S2中，对于所述批量非线性最小二乘问题，若ρ
k(j)
大于所述预设阈值时，构建里程计约束残差并表示为：
[0024][0025]其中，Log[
·
]为李群到李代数的对数映射运算符；分别为第i时刻，第j时刻机体系相对于LIO系的变换矩阵；分别为第i时刻，第j时刻机体系相对于ENU系的变换矩阵，是待优化值；
[0026]若当前位姿在概率体素地图的范围内，构建点到概率分布约束，并表示为：
[0027][0028][0029][0030]其中，p＝[p
x
,p
y
,p
z
]T
为激光点，idx
map
为所述激光点的区块索引，分别为原始点云地图x，y轴方向的最小值；floor[
·
]为向下取整函数；取值为取值为为原始点云地图x轴方向的最大值，r表示每隔多少米进行区块
分割；
[0031]若所述区块索引未超出最大索引值，则在对应的区块中计算体素索引值：
[0032]h＝h
x
+h
y
*D
x
+hz*D
x
*D
y
[0033]h
x
＝floor[(p
x
‑
x
min
)/n][0034]h
y
＝floor[(p
y
‑
ymin)/n][0035]h
z
＝floor[(p
z
‑
z
min
)/n]；
[0036]其中，D
x
，D
y
分别为该区块中x，y轴的体素维度；x
min
，y
min
，z
min
分别为该区块原始点云的x，y，z轴最小值；n为该区块体素分辨率；
[0037]对应的，根据所述体素索引值查询指定体素中的均值和协方差，构建点到概率分布残差并表示为：
[0038][0039]其中，为ENU系相对于MAP系的变换矩阵；为第i帧相对于ENU系的变换矩阵；p
i
为某个激光点；q
j
为第j个体素中的均值；Σ
j
为第j个体素中的协方差；
[0040]构建预积分约束残差并表示为：
[0041][0042][0043][0044][0045]其中，分别为第i时刻，第j时刻机体系相对于EN本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种近岸复杂水域的强一致性里程计及高精度建图的方法，其特征在于，所述方法包括以下步骤：S1、基于迭代误差卡尔曼滤波器构建激光
‑
惯性里程计，并通过所述激光
‑
惯性里程计采集近岸复杂水域的局部里程计数据，同时，获取近岸复杂水域的惯性测量单元数据、全球卫星导航观测数据以及先验地图数据；S2、通过对预设时间段内的所述局部里程计数据、所述惯性测量单元数据、所述全球卫星导航观测数据以及所述先验地图数据进行综合处理，构建批量非线性最小二乘问题，并根据所述非线性最小二乘问题计算输出得到全局定位信息；S3、通过对所有所述局部里程计数据、所述惯性测量单元数据、所述全球卫星导航观测数据进行综合处理，构建关于所述全局定位信息的全局位姿图优化问题，并根据所述全局位姿图优化问题计算输出得到三维点云地图，并进一步生成概率体素地图，之后，融合所述全局定位信息和所述概率体素地图的数据实现近岸复杂水域的定位。2.如权利要求1所述的近岸复杂水域的强一致性里程计及高精度建图的方法，其特征在于，步骤S1中，所述迭代误差卡尔曼滤波器满足以下关系式：u
k
＝[ω
m,k
,a
m,k
]w
k
＝[n
g,k
,n
a,k
,n
bg,k
,n
ba,k
]；其中，x
k
为状态量，u
k
为输入量，w
k
为噪声量；为k时刻机体系相对于LIO系的旋转；为k时刻机体系相对于LIO系的平移；为k时刻机体系相对于LIO系的速度；b
g,k
为k时刻陀螺仪的零偏；b
a,k
为k时刻加速度计的零偏；分别为激光雷达相对于惯性测量单元的安装外参；ω
m,k
为k时刻陀螺仪测量的角速度；a
m,k
为k时刻加速度计测量的加速度；n
g,k
为k时刻陀螺仪的测量噪声；n
a,k
为k时刻加速度计的测量噪声；n
bg,k
为k时刻陀螺仪的零偏噪声；n
ba,k
为k时刻加速度计的零偏噪声。3.如权利要求2所述的近岸复杂水域的强一致性里程计及高精度建图的方法，其特征在于，步骤S1还包括：对所述激光
‑
惯性里程计进行可观测度分析，对于所述误差状态卡尔曼滤波器中每一个状态向量的分量x
k(j)
(j＝1,2,
…
,n)在0时刻协方差与k时刻协方差的比值：其中，P
0(j,j)
表示为0时刻协方差矩阵中第j行第j列的元素；P
k(j,j)
表示为k时刻协方差矩阵中第j行第j列的元素；当ρ
k(j)
小于预设阈值时，将所述激光
‑
惯性里程计的输出变换为：惯性里程计的输出变换为：惯性里程计的输出变换为：
其中，为ENU系相对于LIO系的外参变换矩阵，为所述先验地图数据。4.如权利要求3所述的近岸复杂水域的强一致性里程计及高精度建图的方法，其特征在于，步骤S2中，对于所述批量非线性最小二乘问题，若ρ
k(j)
大于所述预设阈值时，构建里程计约束残差并表示为：其中，Log[
·
]为李群到李代数的对数映射运算符；分别为第i时刻，第j时刻机体系相对于LIO系的变换矩阵；分别为第i时刻，第j时刻机体系相对于ENU系的变换矩阵，是待优化值；若当前位姿在概率体素地图的范围内，构建点到概率分布约束，并表示为：若当前位姿在概率体素地图的范围内，构建点到概率分布约束，并表示为：若当前位姿在概率体素地图的范围内，构建点到概率分布约束，并表示为：其中，为激光点，idx
map
为所述激光点的区块索引，分别为原始点云地图x，y轴方向的最小值；floor[
·
]为向下取整函数；取值为取值为为原始点云地图x轴方向的最大值，r表示每隔多少米进行区块分割；若所述区块索引未超出最大索引值，则在对应的区块中计算体素索引值：h＝h
x
+h
y
*D
x
+h
z
*D
x
*D
y
h
x
＝floor[(p
x
‑
x
min
)/n]h
y
＝floor[(p
y
‑
y
min
)/n]h
z
＝floor[(p
z
‑
z
min
)/n]；其中，D
x
,D
y
分别为该区块中x，y轴的体素维度；x
min
,y

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