CN116026316B

CN116026316B - 一种耦合视觉惯性里程计与gnss的无人船航迹推算方法

Info

Publication number: CN116026316B
Application number: CN202310324328.8A
Authority: CN
Inventors: 罗宇; 李安冉; 彭东东; 李文君; 张�杰
Original assignee: Shandong University of Science and Technology
Current assignee: Shandong University of Science and Technology
Priority date: 2023-03-30
Filing date: 2023-03-30
Publication date: 2023-08-29
Anticipated expiration: 2043-03-30
Also published as: CN116026316A

Abstract

本发明公开了一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，视觉里程计使用光束法平差的方法对前后两帧图像的特征匹配进行优化以获得滑动窗口中每一帧图像的最佳姿态；惯性里程计IMU采用基于紧耦合滑动窗口算法进行状态估计以获得准确而鲁棒的结果，作为全局位置中的局部约束；全局位置耦合设计模块将视觉惯性里程计模块通过视觉测量和惯性测量得到的最新的局部测量位置转换为全局估计位置，全局估计位置作为全局位置进行无人船航迹推算及自身定位，将最优全局位置更新到视觉惯性里程计模块，全局位置耦合设计模块。本发明提出了一种高精度、抗干扰的航迹推算及自身定位方法。

Description

一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法

技术领域

本发明涉及无人船技术领域，尤其涉及一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法。

背景技术

科技创新是实现海洋强国的重要举措以及唯一途径。在人工智能迅速发展的今天，无人船作为新型的海上智能体平台，拥有自主运动控制、复杂环境适应性强等特点，与其他智能平台一样，无人船是一种能够自主路径规划自主航行可以采用人工干预的方式或自主信息采集的方式对水面复杂环境完成环境感知、目标识别与探测、目标跟踪等多种任务。

无人船作为新型智能化设备有着非常广泛的应用。例如科学研究：深度测量研究、多船协作、控制策略研究；环境研究：海洋环境检测、抽样和评估、台风海洋预警；军事应用：港口侦查与巡逻、搜寻与救援、反恐保护；海洋资源探索：海底探测、海上石油天然气探索、海面平台建设与维护等。在复杂的水环境中，无人船能否准确的进行航迹推算与定位是无人船安全作业的关键。基于GNSS的精密单点定位（Precise Point Positioning，PPP）技术是进行高精度绝对定位的有效工具，而视觉惯性导航系统（Visual-Inertial NavigationSystem,VINS）也被证明能够实现短时的高精度相对定位。GNSS与VINS具有较强的互补性，因此，如何提供一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法成为了本领域技术人员急需解决的技术问题。

发明内容

本发明目的是提供了一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，基于视觉惯性里程计与GNSS的半紧耦合提出了一种高精度、抗干扰的航迹推算及自身定位方法。

本发明解决技术问题采用如下技术方案：

一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，通过视觉惯性里程计模块与全局位置耦合设计模块实现无人船航迹推算；

视觉惯性里程计模块分为视觉里程计与惯性里程计两部分；

视觉里程计首先将无人船搭载的高清相机实时获取的图像信息进行Lab颜色空间的转换，提取其中的a分量进行特征点提取，利用KLT金字塔进行光流跟踪，使用光束法平差的方法对前后两帧图像的特征匹配进行优化以获得滑动窗口中每一帧图像的最佳姿态；

惯性里程计IMU首先进行IMU数据预积分，得到当前时刻的位姿、速度、旋转角，完成惯性里程计的初始化，之后进行VINS联合初始化，采用基于紧耦合滑动窗口算法进行状态估计以获得准确而鲁棒的结果，并且作为全局位置中的局部约束；

全局位置耦合设计模块将视觉惯性里程计模块通过视觉测量和惯性测量得到的最新的局部测量位置转换为全局估计位置，全局估计位置作为全局位置进行无人船航迹推算及自身定位，对最后输出的全局位置进行图优化，得到最优的定位效果，最后将最优全局位置更新到视觉惯性里程计模块，全局位置耦合设计模块。

进一步的，将无人船搭载的高清相机实时获取的图像信息进行Lab颜色空间的转换的方法如下：

将无人船搭载的高清相机实时获取的图像从 RGB 颜色空间转换为 Lab颜色空间，转换公式如下：

将XYZ参数转换为Lab颜色空间：

（2）

在公式（2）中，表示为一种函数，设/>为自变量，其具体表述为：

（3）

进一步的，提取其中的a分量进行特征点提取的方法为：

在a通道下，岸边水草、树木灰度值小，水面区域灰度值大，水面区域与岸上区域的灰度值有大的差异，且树木在水中的倒影区域与水面无倒影区域差别不大，使用阈值分割方法将水面部分区域分割出并剔除；处理后的图像进行特征点提取。

进一步的，滑动窗口中的状态向量表示如下：

（4）；

（5）；

（6）；

其中，表示完整的状态向量，包括IMU状态向量/>，IMU坐标系与相机坐标系之间的坐标转换矩阵/>，初次观测的逆深度/>，/>；/>表示IMU坐标系，/>表示相机坐标系；/>与/>分别表示滑动窗口中特征点和关键帧的数量；/>是拍到第/>帧图像时的IMU状态向量；IMU初始时刻的位置被定义成局部坐标系/>的原点，后续IMU的位置/>、速度/>和姿态/>都是相对于局部坐标系/>得到的；/>分别表示的陀螺仪和加速度计的慢时变误差。

进一步的，滑动窗口中的状态的最大后验估计通过最小化所有测量残差与先验的范数的和获得：

（7）

上式中，表示第/>个特征，/>表示/>时刻，/>表示下一个时刻，/>为过地面点的子午面与起始子午面之间的夹角，/>表示第/>张图像下过地面点的子午面与起始子午面之间的夹角，/>表示IMU坐标系，/>表示相机坐标系，/>表示位置，/>表示相邻两时刻下IMU位姿变化，/>表示为视觉测量的平方根信息矩阵；/>表示VINS系统所有测量残差，/>表示各分系统测量残差，/>和/>分别表示IMU测量残差和视觉测量残差，表示滑动窗口边缘化过程中获得的先验信息；/>是Huber函数，用来减小最小二乘问题中异常值的权重。

进一步的，基于紧耦合滑动窗口算法进行状态估计后，通过重投影误差方程剔除每次优化后的离群值，重投影误差方程为：

（8）

（9）

式中表示第/>个特征点的第一次观测，/>表示相机的第/>个图像，V是视觉惯性里程计模块的参考坐标系，/>是反投影函数；

和/>分别是左目相机-IMU系统以及右目相机-IMU系统的坐标变换矩阵的旋转分量和平移分量；/>代表第/>帧左目相机图片对第/>帧左目相机图片的重投影误差的观测结果，/>代表第/>帧左目相机图片对第/>帧右目相机图片的重投影误差的观测结果。

进一步的，全局位置耦合设计模块具体工作步骤如下：

（1）将GNSS的全局坐标系与视觉惯性里程计模块的局部坐标系刚性对齐，实现全局坐标系下的多传感器耦合定位；

（2）通过局部坐标系V变换到全局坐标系G之间的坐标系变换矩阵，将视觉惯性里程计模块通过视觉测量和惯性测量得到的最新的局部测量位置转换为全局估计位置；

（3）全局估计位置作为先验信息传送给全局位置耦合设计模块进行全局位置的初始化；

（4）GNSS信号中断时，全局估计位置作为全局位置进行无人船航迹推算及自身定位；

（5）对最后输出的全局位置进行图优化，得到最优的定位效果，最后将最优全局位置更新到视觉惯性里程计模块，全局位置耦合设计模块。

进一步的，全局估计位置还能够作为局部约束利用最小二乘法进行最优位置的输出，即：

(10)

上式中是所有位置的状态向量，/>是测量集合，包括局部估计位置，全局位置，/>是Mahalanobis范数，/>是对应的协方差，/>是残差函数；

第一部分局部测量残差：

(11)

上式描述了时刻t的相对位姿残差；第一行表示的是相对位置残差，第二行表示的是相对旋转残差；旋转矩阵表示从局部坐标系/>变换到全局坐标系/>；

第二部分是全局测量残差：

（12）

上式中是GNSS PPP定位模块得到的全局位置的测量值；/>是根据VINS定位模块得到的全局位置的估计值。

有益效果：本发明融合多种传感器，对无人船进行航迹推算和自身定位，从而为无人船自主导航提供了关键信息。本发明重点在于视觉惯性里程计设计，全局位置耦合设计。本发明提出了一种基于Lab颜色空间的视觉里程计设计，利用Lab颜色空间处理水面图像进行图像分割，剔除水面低纹理场景，提高了系统在位姿解算过程中使用特征点的准确性；本发明基于视觉惯性里程计与GNSS的半紧耦合提出了一种高精度、抗干扰的航迹推算及自身定位方法。因受恶劣天气等各种因素影响而导致GNSS信号不稳定时，视觉惯性里程计可以作为全局约束单独提供航迹推算数据，这极大的摆脱了GNSS信号的束缚，扩大了无人船的工作场景。

附图说明

图1 为本发明视觉惯性里程计模块的工作流程图。

图2 为本发明基于最小二乘的全局位置半紧耦合流程图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明公开了一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，通过视觉惯性里程计模块与全局位置耦合设计模块实现无人船航迹推算；

视觉惯性里程计模块分为视觉里程计与惯性里程计两部分；

视觉惯性里程计模块：视觉惯性里程计分为视觉里程计与惯性里程计两部分。视觉里程计采用Lab颜色空间中的a通道进行水岸线分割，使用剔除低纹理场景的图像进行特征点匹配。初始化后，初步解算无人船位姿；惯性里程计（IMU）则通过将IMU数据进行预积分，得到当前时刻的位姿、速度、旋转角；联合初始化后采用基于紧耦合滑动窗口算法进行状态估计以获得准确而鲁棒的结果。

全局位置耦合设计模块：通过GNSS接收机天线接收GNSS信号从而根据精密单点定位模型解算出自身在大地坐标系中的经纬度，再经坐标系变换矩阵转换成地心地固坐标系中的三维位置坐标，同时将其他定位模块的预测位置坐标转换为地心地固坐标系中的三维坐标才能与无人船的位置坐标实现坐标系的统一。在全局位置节点，由视觉惯性里程计模块的测量值提供局部约束，由全局位置耦合设计模块的测量值提供的全局约束。由于复杂的天气下卫星信号的稳定性和有效性较低，视觉惯性里程计模块的定位结果将被选择性地用作全局约束，代替GNSS进行航迹推算及自身定位。对全局位置进行图优化就可以得到最优的定位结果。最后将所得的最优全局位置坐标更新到视觉惯性里程计模块中的滑动窗口模块、全局位置耦合设计模块，由此形成多源耦合高精度定位系统的耦合闭环。

视觉惯性里程计模块，具体工作步骤如下：

A1，图像预处理：将无人船搭载的高清相机实时获取的图像信息进行Lab颜色空间的转换，转换公式如下：

将XYZ参数转换为Lab颜色空间：

（2）

（3）

在a通道下，岸边水草、树木灰度值小，水面区域灰度值大，水面区域与岸上区域的灰度值有大的差异，且树木在水中的倒影区域与水面无倒影区域差别不大，使用阈值分割方法将水面部分区域分割出并剔除。处理后的图像进行特征点提取，利用KLT金字塔进行光流跟踪，为后面仅视觉初始化求解相机位姿做准备。

A2、IMU数据预处理：将IMU数据进行预积分，得到当前时刻的位姿、速度、旋转角，同时计算在后端优化中将要用到的相邻帧间的预积分增量，及预积分的协方差矩阵和雅可比矩阵。

A3、使用光束法平差的方法对前后两帧图像的特征匹配进行优化以获得滑动窗口中每一帧图像的最佳姿态。

A4、进行视觉惯性联合初始化，在VINS初始化后，采用基于紧耦合滑动窗口算法进行状态估计以获得准确而鲁棒的结果，并且作为全局位置中的局部约束。滑动窗口中的状态向量的定义可以写成：

（4）

（5）

（6）

其中，表示完整的状态向量，包括IMU状态向量/>，IMU坐标系与相机坐标系之间的坐标转换矩阵/>，初次观测的逆深度/>，/>；/>表示IMU坐标系，/>表示相机坐标系；/>与/>分别表示滑动窗口中特征点和关键帧的数量。/>是拍到第/>帧图像时IMU的状态向量；IMU初始时刻的位置被定义成局部坐标系V的原点，后续IMU的位置/>、速度/>和姿态/>都是相对于局部坐标系V得到的。/>分别表示的陀螺仪和加速度计的慢时变误差。

滑动窗口中的状态的最大后验估计通过最小化所有测量残差与先验的Mahalanobis范数的和获得：

（7）

上式中，表示第/>个特征，/>表示/>时刻，/>表示下一个时刻，/>为过地面点的子午面与起始子午面之间的夹角，/>表示第/>张图像下过地面点的子午面与起始子午面之间的夹角，/>表示坐标系，/>表示相机坐标系，/>表示位置, />表示相邻两时刻下位姿变化，/>表示为视觉测量的平方根信息矩阵；/>表示VINS系统所有测量残差，/>表示各分系统测量残差，/>和/>分别表示IMU测量残差和视觉测量残差，/>表示滑动窗口边缘化过程中获得的先验信息；/>是Huber函数，用来减小最小二乘问题中异常值的权重。

基于紧耦合滑动窗口算法进行状态估计后，通过重投影误差方程剔除每次优化后的离群值，重投影误差方程为：

（8）

（9）

式中表示第/>个特征点的第一次观测，/>表示相机的第/>个图像，/>是视觉惯性里程计模块的参考坐标系，/>是反投影函数。

全局位置耦合设计模块具体工作步骤如下：

S1，将GNSS的全局坐标系与视觉惯性里程计模块的局部坐标系刚性对齐，实现全局坐标系下的多传感器耦合定位；

S2，通过局部坐标系V变换到全局坐标系G之间的坐标系变换矩阵，将视觉惯性里程计模块通过视觉测量和惯性测量得到的最新的局部测量位置转换为全局估计位置；

S3，全局估计位置作为先验信息传送给全局位置耦合设计模块进行全局位置的初始化；还可以作为局部约束利用最小二乘法进行最优位置的输出，即：

（10）

上式中是所有位置的状态向量，/>是测量集合，包括局部估计位置，全局位置，/>是/>范数，/>是对应的协方差，/>是残差函数。

第一部分局部测量残差：

（11）

上式描述了时刻t的相对位姿残差。第一行表示的是相对位置残差，第二行表示的是相对旋转残差。旋转矩阵表示从局部坐标系V变换到全局坐标系G。

第二部分是全局测量残差:

（12）

上式中是GNSS PPP定位模块得到的全局位置的测量值。/>是根据VINS定位模块得到的全局位置的估计值。

S4，GNSS信号中断时，全局估计位置作为全局位置进行无人船航迹推算及自身定位；

S5，对最后输出的全局位置进行图优化，得到最优的定位效果，最后将最优全局位置更新到视觉惯性里程计模块，全局位置耦合设计模块。

如图1所示，视觉惯性里程计模块具体步骤如下：

视觉里程计首先将无人船搭载的高清相机实时获取的图像信息进行Lab颜色空间的转换，提取其中的a分量进行特征点提取，利用KLT金字塔进行光流跟踪，使用光束法平差的方法对前后两帧图像的特征匹配进行优化以获得滑动窗口中每一帧图像的最佳姿态。

惯性里程计IMU首先进行数据预积分，得到当前时刻的位姿、速度、旋转角，完成惯性里程计的初始化，之后进行VINS联合初始化，采用基于紧耦合滑动窗口算法进行状态估计以获得准确而鲁棒的结果，并且作为全局位置中的局部约束。其中IMU测量残差和视觉测量残差/>可用观测值减去估计值的方法获得。

如图2所示，全局位置耦合模块具体步骤如下：

全局坐标系G是全局位置耦合模块的参考坐标系，它是WGS-84空间直角坐标系。局部坐标系V是，视觉惯性里程计模块的参考坐标系。通过定位系统启动前的测量可得IMU在初始时刻的位置和接收机天线在初始时刻/>的相位中心的相对距离和相对角度。基于接收机天线在初始时刻/>的相位中心相对全局坐标系G原点和三轴的相对距离和相对角度，通过定位系统启动前的测量数据就能得到IMU在初始时刻/>的位置相对于接收机天线在初始时刻的相位中心的在全局坐标系G下的平移向量/>。通过平移向量/>以及站心直角坐标系旋转到WGS-84空间直角坐标系的旋转矩阵/>进行全局坐标系与局部坐标系的刚性对齐。

VINS系统基于，/>输出全局估计位置，将全局估计位置作为全局位置耦合模块进行数据处理的初始值，得出的结果可以替代标准单点定位的结果。

若GNSS信号发生中断，全局估计位置作为全局约束提供短时航迹推算与自身定位。

全局位置（包括全局估计位置与全局测量位置）的半紧耦合基于最小二乘法完成。即：

本发明融合多种传感器，对无人船进行航迹推算和自身定位，从而为无人船自主导航提供了关键信息。本发明重点在于视觉惯性里程计设计，全局位置耦合设计。本发明提出了一种基于Lab颜色空间的视觉里程计设计，利用Lab颜色空间处理水面图像进行图像分割，剔除水面低纹理场景，提高了系统在位姿解算过程中使用特征点的准确性；本发明基于视觉惯性里程计与GNSS的半紧耦合提出了一种高精度、抗干扰的航迹推算及自身定位方法。因受恶劣天气等各种因素影响而导致GNSS信号不稳定时，视觉惯性里程计可以作为全局约束单独提供航迹推算数据，这极大的摆脱了GNSS信号的束缚，扩大了无人船的工作场景。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims

1.一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，其特征在于，通过视觉惯性里程计模块与全局位置耦合设计模块实现无人船航迹推算；

视觉惯性里程计模块分为视觉里程计与惯性里程计两部分；

全局位置耦合设计模块将视觉惯性里程计模块通过视觉测量和惯性测量得到的最新的局部测量位置转换为全局估计位置，全局估计位置作为全局位置进行无人船航迹推算及自身定位，对最后输出的全局位置进行图优化，得到最优的定位效果，最后将最优全局位置更新到视觉惯性里程计模块，全局位置耦合设计模块；

将无人船搭载的高清相机实时获取的图像信息进行Lab颜色空间的转换的方法如下：

将无人船搭载的高清相机实时获取的图像从RGB颜色空间转换为Lab颜色空间，转换公式如下：

X＝0.412453·R+0.357580·G+0.180423·B

Y＝0.212671·R+0.715160·G+0.072169·B

Z＝0.019334·R+0.119193·G+0.950227·B (1)

将XYZ参数转换为Lab颜色空间：

在公式(1)中，f表示为一种函数，其具体表述为：

提取其中的a分量进行特征点提取的方法为：

在a通道下，岸边水草、树木灰度值小，水面区域灰度值大，水面区域与岸上区域的灰度值有大的差异，且树木在水中的倒影区域与水面无倒影区域差别不大，使用阈值分割方法将水面部分区域分割出并剔除；处理后的图像进行特征点提取；

滑动窗口中的状态向量表示如下：

其中，X^V表示完整的状态向量，包括IMU状态向量x_k，IMU坐标系与相机坐标系之间的坐标转换矩阵初次观测的逆深度γ_l，l∈[0,m]；B表示IMU坐标系，C表示相机坐标系；m与n分别表示滑动窗口中特征点和关键帧的数量；x_k是拍到第k帧图像时的IMU状态向量；IMU初始时刻的位置被定义成局部坐标系V的原点，后续IMU的位置/>速度/>和姿态/>都是相对于局部坐标系V得到的；b_Gy,k,b_Ac,k分别表示的陀螺仪和加速度计的慢时变误差；

上式中，l表示第l个特征，k表示拍摄的第k帧，k+1表示拍摄的下一帧，L为过地面点的子午面与起始子午面之间的夹角，L_j表示第j张图像下过地面点的子午面与起始子午面之间的夹角，B表示IMU坐标系，C表示相机坐标系，P表示位置,表示相邻两时刻下IMU位姿变化，/>表示为视觉测量的平方根信息矩阵；r表示各分系统测量残差，/>和分别表示IMU测量残差和视觉测量残差，(r_p-H_pX^V)表示滑动窗口边缘化过程中获得的先验信息；ρ(·)是Huber函数，用来减小最小二乘问题中异常值的权重；

式中表示第l个特征点的第一次观测，C_i,j表示相机的第i个图像，V是视觉惯性里程计模块的参考坐标系，/>是反投影函数；

和/>分别是左目相机-IMU系统以及右目相机-IMU系统的坐标变换矩阵的旋转分量和平移分量；/>代表第i帧左目相机图片对第j帧左目相机图片的重投影误差的观测结果，/>代表第i帧左目相机图片对第j帧右目相机图片的重投影误差的观测结果；

全局位置耦合设计模块具体工作步骤如下：

(1)将GNSS的全局坐标系与视觉惯性里程计模块的局部坐标系刚性对齐，实现全局坐标系下的多传感器耦合定位；

(2)通过局部坐标系V变换到全局坐标系G之间的坐标系变换矩阵，将视觉惯性里程计模块通过视觉测量和惯性测量得到的最新的局部测量位置转换为全局估计位置；

(3)全局估计位置作为先验信息传送给全局位置耦合设计模块进行全局位置的初始化；

(4)GNSS信号中断时，全局估计位置作为全局位置进行无人船航迹推算及自身定位；

(5)对最后输出的全局位置进行图优化，得到最优的定位效果，最后将最优全局位置更新到视觉惯性里程计模块，全局位置耦合设计模块。

2.根据权利要求1所述一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，其特征在于，全局估计位置还能够作为局部约束利用最小二乘法进行最优位置的输出，即：

上式中X＝[x₀，x₁，...，x_n]是所有位置的状态向量，S是测量集合，包括局部估计位置，全局位置，是Mahalanobis范数，T是对应的协方差，/>是残差函数；

第一部分局部测量残差：

上式描述了时刻t的相对位姿残差；

第二部分是全局测量残差：