[go: up one dir, main page]

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

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

Info

Publication number
CN116026316A
CN116026316A CN202310324328.8A CN202310324328A CN116026316A CN 116026316 A CN116026316 A CN 116026316A CN 202310324328 A CN202310324328 A CN 202310324328A CN 116026316 A CN116026316 A CN 116026316A
Authority
CN
China
Prior art keywords
global
visual
coordinate system
odometer
measurement
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202310324328.8A
Other languages
English (en)
Other versions
CN116026316B (zh
Inventor
罗宇
李安冉
彭东东
李文君
张�杰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shandong University of Science and Technology
Original Assignee
Shandong University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shandong University of Science and Technology filed Critical Shandong University of Science and Technology
Priority to CN202310324328.8A priority Critical patent/CN116026316B/zh
Publication of CN116026316A publication Critical patent/CN116026316A/zh
Application granted granted Critical
Publication of CN116026316B publication Critical patent/CN116026316B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

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颜色空间，转换公式如下：
Figure SMS_1
将XYZ参数转换为Lab颜色空间：
Figure SMS_2
（2）
在公式（2）中，
Figure SMS_3
表示为一种函数，设
Figure SMS_4
为自变量，其具体表述为：
Figure SMS_5
（3）
进一步的，提取其中的a分量进行特征点提取的方法为：
在a通道下，岸边水草、树木灰度值小，水面区域灰度值大，水面区域与岸上区域的灰度值有大的差异，且树木在水中的倒影区域与水面无倒影区域差别不大，使用阈值分割方法将水面部分区域分割出并剔除；处理后的图像进行特征点提取。
进一步的，滑动窗口中的状态向量表示如下：
Figure SMS_6
（4）；
Figure SMS_7
（5）；
Figure SMS_8
（6）；
其中，
Figure SMS_13
表示完整的状态向量，包括IMU状态向量
Figure SMS_15
，IMU坐标系与相机坐标系之间的坐标转换矩阵
Figure SMS_17
，初次观测的逆深度
Figure SMS_19
，
Figure SMS_23
；
Figure SMS_24
表示IMU坐标系，
Figure SMS_25
表示相机坐标系；
Figure SMS_9
与
Figure SMS_12
分别表示滑动窗口中特征点和关键帧的数量；
Figure SMS_14
是拍到第
Figure SMS_16
帧图像时的IMU状态向量；IMU初始时刻的位置被定义成局部坐标系
Figure SMS_18
的原点，后续IMU的位置
Figure SMS_20
、速度
Figure SMS_21
和姿态
Figure SMS_22
都是相对于局部坐标系
Figure SMS_10
得到的；
Figure SMS_11
分别表示的陀螺仪和加速度计的慢时变误差。
进一步的，滑动窗口中的状态的最大后验估计通过最小化所有测量残差与先验的
Figure SMS_26
范数的和获得：
Figure SMS_27
（7）
上式中，
Figure SMS_40
表示第
Figure SMS_41
个特征，
Figure SMS_42
表示
Figure SMS_43
时刻，
Figure SMS_44
表示下一个时刻，
Figure SMS_45
为过地面点的子午面与起始子午面之间的夹角，
Figure SMS_46
表示第
Figure SMS_28
张图像下过地面点的子午面与起始子午面之间的夹角，
Figure SMS_30
表示IMU坐标系，
Figure SMS_31
表示相机坐标系，
Figure SMS_32
表示位置，
Figure SMS_34
表示相邻两时刻下IMU位姿变化，
Figure SMS_35
表示为视觉测量的平方根信息矩阵；
Figure SMS_37
表示VINS系统所有测量残差，
Figure SMS_39
表示各分系统测量残差，
Figure SMS_29
和
Figure SMS_33
分别表示IMU测量残差和视觉测量残差，
Figure SMS_36
表示滑动窗口边缘化过程中获得的先验信息；
Figure SMS_38
是Huber函数，用来减小最小二乘问题中异常值的权重。
进一步的，基于紧耦合滑动窗口算法进行状态估计后，通过重投影误差方程剔除每次优化后的离群值，重投影误差方程为：
Figure SMS_47
（8）
Figure SMS_48
（9）
式中
Figure SMS_49
表示第
Figure SMS_50
个特征点的第一次观测，
Figure SMS_51
表示相机的第
Figure SMS_52
个图像，V是视觉惯性里程计模块的参考坐标系，
Figure SMS_53
是反投影函数；
Figure SMS_55
和
Figure SMS_56
分别是左目相机-IMU系统以及右目相机-IMU系统的坐标变换矩阵的旋转分量和平移分量；
Figure SMS_57
代表第
Figure SMS_58
帧左目相机图片对第
Figure SMS_59
帧左目相机图片的重投影误差的观测结果，
Figure SMS_60
代表第
Figure SMS_61
帧左目相机图片对第
Figure SMS_54
帧右目相机图片的重投影误差的观测结果。
进一步的，全局位置耦合设计模块具体工作步骤如下：
（1）将GNSS的全局坐标系与视觉惯性里程计模块的局部坐标系刚性对齐，实现全局坐标系下的多传感器耦合定位；
（2）通过局部坐标系V变换到全局坐标系G之间的坐标系变换矩阵，将视觉惯性里程计模块通过视觉测量和惯性测量得到的最新的局部测量位置转换为全局估计位置；
（3）全局估计位置作为先验信息传送给全局位置耦合设计模块进行全局位置的初始化；
（4）GNSS信号中断时，全局估计位置作为全局位置进行无人船航迹推算及自身定位；
（5）对最后输出的全局位置进行图优化，得到最优的定位效果，最后将最优全局位置更新到视觉惯性里程计模块，全局位置耦合设计模块。
进一步的，全局估计位置还能够作为局部约束利用最小二乘法进行最优位置的输出，即：
Figure SMS_62
(10)
上式中
Figure SMS_63
是所有位置的状态向量，
Figure SMS_64
是测量集合，包括局部估计位置，全局位置，
Figure SMS_65
是Mahalanobis范数，
Figure SMS_66
是对应的协方差，
Figure SMS_67
是残差函数；
第一部分局部测量残差：
Figure SMS_68
(11)
上式描述了时刻t的相对位姿残差；第一行表示的是相对位置残差，第二行表示的是相对旋转残差；旋转矩阵
Figure SMS_69
表示从局部坐标系
Figure SMS_70
变换到全局坐标系
Figure SMS_71
；
第二部分是全局测量残差：
Figure SMS_72
（12）
上式中
Figure SMS_73
是GNSS PPP定位模块得到的全局位置的测量值；
Figure SMS_74
是根据VINS定位模块得到的全局位置的估计值。
有益效果：本发明融合多种传感器，对无人船进行航迹推算和自身定位，从而为无人船自主导航提供了关键信息。本发明重点在于视觉惯性里程计设计，全局位置耦合设计。本发明提出了一种基于Lab颜色空间的视觉里程计设计，利用Lab颜色空间处理水面图像进行图像分割，剔除水面低纹理场景，提高了系统在位姿解算过程中使用特征点的准确性；本发明基于视觉惯性里程计与GNSS的半紧耦合提出了一种高精度、抗干扰的航迹推算及自身定位方法。因受恶劣天气等各种因素影响而导致GNSS信号不稳定时，视觉惯性里程计可以作为全局约束单独提供航迹推算数据，这极大的摆脱了GNSS信号的束缚，扩大了无人船的工作场景。
附图说明
图1 为本发明视觉惯性里程计模块的工作流程图。
图2 为本发明基于最小二乘的全局位置半紧耦合流程图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明公开了一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，通过视觉惯性里程计模块与全局位置耦合设计模块实现无人船航迹推算；
视觉惯性里程计模块分为视觉里程计与惯性里程计两部分；
视觉惯性里程计模块：视觉惯性里程计分为视觉里程计与惯性里程计两部分。视觉里程计采用Lab颜色空间中的a通道进行水岸线分割，使用剔除低纹理场景的图像进行特征点匹配。初始化后，初步解算无人船位姿；惯性里程计（IMU）则通过将IMU数据进行预积分，得到当前时刻的位姿、速度、旋转角；联合初始化后采用基于紧耦合滑动窗口算法进行状态估计以获得准确而鲁棒的结果。
视觉里程计首先将无人船搭载的高清相机实时获取的图像信息进行Lab颜色空间的转换，提取其中的a分量进行特征点提取，利用KLT金字塔进行光流跟踪，使用光束法平差的方法对前后两帧图像的特征匹配进行优化以获得滑动窗口中每一帧图像的最佳姿态；
惯性里程计IMU首先进行IMU数据预积分，得到当前时刻的位姿、速度、旋转角，完成惯性里程计的初始化，之后进行VINS联合初始化，采用基于紧耦合滑动窗口算法进行状态估计以获得准确而鲁棒的结果，并且作为全局位置中的局部约束；
全局位置耦合设计模块将视觉惯性里程计模块通过视觉测量和惯性测量得到的最新的局部测量位置转换为全局估计位置，全局估计位置作为全局位置进行无人船航迹推算及自身定位，对最后输出的全局位置进行图优化，得到最优的定位效果，最后将最优全局位置更新到视觉惯性里程计模块，全局位置耦合设计模块。
全局位置耦合设计模块：通过GNSS接收机天线接收GNSS信号从而根据精密单点定位模型解算出自身在大地坐标系中的经纬度，再经坐标系变换矩阵转换成地心地固坐标系中的三维位置坐标，同时将其他定位模块的预测位置坐标转换为地心地固坐标系中的三维坐标才能与无人船的位置坐标实现坐标系的统一。在全局位置节点，由视觉惯性里程计模块的测量值提供局部约束，由全局位置耦合设计模块的测量值提供的全局约束。由于复杂的天气下卫星信号的稳定性和有效性较低，视觉惯性里程计模块的定位结果将被选择性地用作全局约束，代替GNSS进行航迹推算及自身定位。对全局位置进行图优化就可以得到最优的定位结果。最后将所得的最优全局位置坐标更新到视觉惯性里程计模块中的滑动窗口模块、全局位置耦合设计模块，由此形成多源耦合高精度定位系统的耦合闭环。
视觉惯性里程计模块，具体工作步骤如下：
A1，图像预处理：将无人船搭载的高清相机实时获取的图像信息进行Lab颜色空间的转换，转换公式如下：
Figure SMS_75
将XYZ参数转换为Lab颜色空间：
Figure SMS_76
（2）
在公式（2）中，
Figure SMS_77
表示为一种函数，设
Figure SMS_78
为自变量，其具体表述为：
Figure SMS_79
（3）
在a通道下，岸边水草、树木灰度值小，水面区域灰度值大，水面区域与岸上区域的灰度值有大的差异，且树木在水中的倒影区域与水面无倒影区域差别不大，使用阈值分割方法将水面部分区域分割出并剔除。处理后的图像进行特征点提取，利用KLT金字塔进行光流跟踪，为后面仅视觉初始化求解相机位姿做准备。
A2、IMU数据预处理：将IMU数据进行预积分，得到当前时刻的位姿、速度、旋转角，同时计算在后端优化中将要用到的相邻帧间的预积分增量，及预积分的协方差矩阵和雅可比矩阵。
A3、使用光束法平差的方法对前后两帧图像的特征匹配进行优化以获得滑动窗口中每一帧图像的最佳姿态。
A4、进行视觉惯性联合初始化，在VINS初始化后，采用基于紧耦合滑动窗口算法进行状态估计以获得准确而鲁棒的结果，并且作为全局位置中的局部约束。滑动窗口中的状态向量的定义可以写成：
Figure SMS_80
（4）
Figure SMS_81
（5）
Figure SMS_82
（6）
其中，
Figure SMS_84
表示完整的状态向量，包括IMU状态向量
Figure SMS_85
，IMU坐标系与相机坐标系之间的坐标转换矩阵
Figure SMS_87
，初次观测的逆深度
Figure SMS_89
，
Figure SMS_91
；
Figure SMS_93
表示IMU坐标系，
Figure SMS_95
表示相机坐标系；
Figure SMS_83
与
Figure SMS_86
分别表示滑动窗口中特征点和关键帧的数量。
Figure SMS_88
是拍到第
Figure SMS_90
帧图像时IMU的状态向量；IMU初始时刻的位置被定义成局部坐标系V的原点，后续IMU的位置
Figure SMS_92
、速度
Figure SMS_94
和姿态
Figure SMS_96
都是相对于局部坐标系V得到的。
Figure SMS_97
分别表示的陀螺仪和加速度计的慢时变误差。
滑动窗口中的状态的最大后验估计通过最小化所有测量残差与先验的Mahalanobis范数的和获得：
Figure SMS_98
（7）
上式中，
Figure SMS_110
表示第
Figure SMS_112
个特征，
Figure SMS_113
表示
Figure SMS_114
时刻，
Figure SMS_115
表示下一个时刻，
Figure SMS_116
为过地面点的子午面与起始子午面之间的夹角，
Figure SMS_117
表示第
Figure SMS_99
张图像下过地面点的子午面与起始子午面之间的夹角，
Figure SMS_101
表示坐标系，
Figure SMS_102
表示相机坐标系，
Figure SMS_104
表示位置,
Figure SMS_105
表示相邻两时刻下位姿变化，
Figure SMS_107
表示为视觉测量的平方根信息矩阵；
Figure SMS_109
表示VINS系统所有测量残差，
Figure SMS_111
表示各分系统测量残差，
Figure SMS_100
和
Figure SMS_103
分别表示IMU测量残差和视觉测量残差，
Figure SMS_106
表示滑动窗口边缘化过程中获得的先验信息；
Figure SMS_108
是Huber函数，用来减小最小二乘问题中异常值的权重。
基于紧耦合滑动窗口算法进行状态估计后，通过重投影误差方程剔除每次优化后的离群值，重投影误差方程为：
Figure SMS_118
（8）
Figure SMS_119
（9）
式中
Figure SMS_120
表示第
Figure SMS_121
个特征点的第一次观测，
Figure SMS_122
表示相机的第
Figure SMS_123
个图像，
Figure SMS_124
是视觉惯性里程计模块的参考坐标系，
Figure SMS_125
是反投影函数。
Figure SMS_127
和
Figure SMS_128
分别是左目相机-IMU系统以及右目相机-IMU系统的坐标变换矩阵的旋转分量和平移分量；
Figure SMS_129
代表第
Figure SMS_130
帧左目相机图片对第
Figure SMS_131
帧左目相机图片的重投影误差的观测结果，
Figure SMS_132
代表第
Figure SMS_133
帧左目相机图片对第
Figure SMS_126
帧右目相机图片的重投影误差的观测结果。
全局位置耦合设计模块具体工作步骤如下：
S1，将GNSS的全局坐标系与视觉惯性里程计模块的局部坐标系刚性对齐，实现全局坐标系下的多传感器耦合定位；
S2，通过局部坐标系V变换到全局坐标系G之间的坐标系变换矩阵，将视觉惯性里程计模块通过视觉测量和惯性测量得到的最新的局部测量位置转换为全局估计位置；
S3，全局估计位置作为先验信息传送给全局位置耦合设计模块进行全局位置的初始化；还可以作为局部约束利用最小二乘法进行最优位置的输出，即：
Figure SMS_134
（10）
上式中
Figure SMS_135
是所有位置的状态向量，
Figure SMS_136
是测量集合，包括局部估计位置，全局位置，
Figure SMS_137
是
Figure SMS_138
范数，
Figure SMS_139
是对应的协方差，
Figure SMS_140
是残差函数。
第一部分局部测量残差：
Figure SMS_141
（11）
上式描述了时刻t的相对位姿残差。第一行表示的是相对位置残差，第二行表示的是相对旋转残差。旋转矩阵
Figure SMS_142
表示从局部坐标系V变换到全局坐标系G。
第二部分是全局测量残差:
Figure SMS_143
（12）
上式中
Figure SMS_144
是GNSS PPP定位模块得到的全局位置的测量值。
Figure SMS_145
是根据VINS定位模块得到的全局位置的估计值。
S4，GNSS信号中断时，全局估计位置作为全局位置进行无人船航迹推算及自身定位；
S5，对最后输出的全局位置进行图优化，得到最优的定位效果，最后将最优全局位置更新到视觉惯性里程计模块，全局位置耦合设计模块。
如图1所示，视觉惯性里程计模块具体步骤如下：
视觉里程计首先将无人船搭载的高清相机实时获取的图像信息进行Lab颜色空间的转换，提取其中的a分量进行特征点提取，利用KLT金字塔进行光流跟踪，使用光束法平差的方法对前后两帧图像的特征匹配进行优化以获得滑动窗口中每一帧图像的最佳姿态。
惯性里程计IMU首先进行数据预积分，得到当前时刻的位姿、速度、旋转角，完成惯性里程计的初始化，之后进行VINS联合初始化，采用基于紧耦合滑动窗口算法进行状态估计以获得准确而鲁棒的结果，并且作为全局位置中的局部约束。其中IMU测量残差
Figure SMS_146
和视觉测量残差
Figure SMS_147
可用观测值减去估计值的方法获得。
如图2所示，全局位置耦合模块具体步骤如下：
全局坐标系G是全局位置耦合模块的参考坐标系，它是WGS-84空间直角坐标系。局部坐标系V是，视觉惯性里程计模块的参考坐标系。通过定位系统启动前的测量可得IMU在初始时刻
Figure SMS_148
的位置和接收机天线在初始时刻
Figure SMS_149
的相位中心的相对距离和相对角度。基于接收机天线在初始时刻
Figure SMS_150
的相位中心相对全局坐标系G原点和三轴的相对距离和相对角度，通过定位系统启动前的测量数据就能得到IMU在初始时刻
Figure SMS_151
的位置相对于接收机天线在初始时刻的相位中心的在全局坐标系G下的平移向量
Figure SMS_152
。通过平移向量
Figure SMS_153
以及站心直角坐标系旋转到WGS-84空间直角坐标系的旋转矩阵
Figure SMS_154
进行全局坐标系与局部坐标系的刚性对齐。
VINS系统基于
Figure SMS_155
，
Figure SMS_156
输出全局估计位置，将全局估计位置作为全局位置耦合模块进行数据处理的初始值，得出的结果可以替代标准单点定位的结果。
若GNSS信号发生中断，全局估计位置作为全局约束提供短时航迹推算与自身定位。
全局位置（包括全局估计位置与全局测量位置）的半紧耦合基于最小二乘法完成。即：
Figure SMS_157
本发明融合多种传感器，对无人船进行航迹推算和自身定位，从而为无人船自主导航提供了关键信息。本发明重点在于视觉惯性里程计设计，全局位置耦合设计。本发明提出了一种基于Lab颜色空间的视觉里程计设计，利用Lab颜色空间处理水面图像进行图像分割，剔除水面低纹理场景，提高了系统在位姿解算过程中使用特征点的准确性；本发明基于视觉惯性里程计与GNSS的半紧耦合提出了一种高精度、抗干扰的航迹推算及自身定位方法。因受恶劣天气等各种因素影响而导致GNSS信号不稳定时，视觉惯性里程计可以作为全局约束单独提供航迹推算数据，这极大的摆脱了GNSS信号的束缚，扩大了无人船的工作场景。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (8)

1.一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，其特征在于，通过视觉惯性里程计模块与全局位置耦合设计模块实现无人船航迹推算；
视觉惯性里程计模块分为视觉里程计与惯性里程计两部分；
视觉里程计首先将无人船搭载的高清相机实时获取的图像信息进行Lab颜色空间的转换，提取其中的a分量进行特征点提取，利用KLT金字塔进行光流跟踪，使用光束法平差的方法对前后两帧图像的特征匹配进行优化以获得滑动窗口中每一帧图像的最佳姿态；
惯性里程计IMU首先进行IMU数据预积分，得到当前时刻的位姿、速度、旋转角，完成惯性里程计的初始化，之后进行VINS联合初始化，采用基于紧耦合滑动窗口算法进行状态估计以获得准确而鲁棒的结果，并且作为全局位置中的局部约束；
全局位置耦合设计模块将视觉惯性里程计模块通过视觉测量和惯性测量得到的最新的局部测量位置转换为全局估计位置，全局估计位置作为全局位置进行无人船航迹推算及自身定位，对最后输出的全局位置进行图优化，得到最优的定位效果，最后将最优全局位置更新到视觉惯性里程计模块，全局位置耦合设计模块。
2.根据权利要求1所述的一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，其特征在于，将无人船搭载的高清相机实时获取的图像信息进行Lab颜色空间的转换的方法如下：
将无人船搭载的高清相机实时获取的图像从 RGB 颜色空间转换为 Lab颜色空间，转换公式如下：
Figure QLYQS_1
，
将XYZ参数转换为Lab颜色空间：
Figure QLYQS_2
（2）
在公式（2）中，
Figure QLYQS_3
表示为一种函数，设
Figure QLYQS_4
为自变量，其具体表述为：
Figure QLYQS_5
（3）。
3.根据权利要求2所述的一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，其特征在于，提取其中的a分量进行特征点提取的方法为：
在a通道下，岸边水草、树木灰度值小，水面区域灰度值大，水面区域与岸上区域的灰度值有大的差异，且树木在水中的倒影区域与水面无倒影区域差别不大，使用阈值分割方法将水面部分区域分割出并剔除；处理后的图像进行特征点提取。
4.据权利要求1所述的一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，其特征在于，滑动窗口中的状态向量表示如下：
Figure QLYQS_6
（4）
Figure QLYQS_7
（5）
Figure QLYQS_8
（6）
其中，
Figure QLYQS_14
表示完整的状态向量，包括IMU状态向量
Figure QLYQS_16
，IMU坐标系与相机坐标系之间的坐标转换矩阵
Figure QLYQS_18
，初次观测的逆深度
Figure QLYQS_20
，
Figure QLYQS_22
；
Figure QLYQS_24
表示IMU坐标系，
Figure QLYQS_25
表示相机坐标系；
Figure QLYQS_9
与
Figure QLYQS_11
分别表示滑动窗口中特征点和关键帧的数量；
Figure QLYQS_13
是拍到第
Figure QLYQS_15
帧图像时的IMU状态向量；IMU初始时刻的位置被定义成局部坐标系
Figure QLYQS_17
的原点，后续IMU的位置
Figure QLYQS_19
、速度
Figure QLYQS_21
和姿态
Figure QLYQS_23
都是相对于局部坐标系
Figure QLYQS_10
得到的；
Figure QLYQS_12
分别表示的陀螺仪和加速度计的慢时变误差。
5.根据权利要求2所述的一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，其特征在于，滑动窗口中的状态的最大后验估计通过最小化所有测量残差与先验的Mahalanobis范数的和获得：
Figure QLYQS_26
（7）
上式中，
Figure QLYQS_37
表示第
Figure QLYQS_39
个特征，
Figure QLYQS_41
表示
Figure QLYQS_42
时刻，
Figure QLYQS_43
表示下一个时刻，
Figure QLYQS_44
为过地面点的子午面与起始子午面之间的夹角，
Figure QLYQS_45
表示第
Figure QLYQS_27
张图像下过地面点的子午面与起始子午面之间的夹角，
Figure QLYQS_29
表示IMU坐标系，
Figure QLYQS_31
表示相机坐标系，
Figure QLYQS_33
表示位置，
Figure QLYQS_35
表示相邻两时刻下IMU位姿变化，
Figure QLYQS_36
表示为视觉测量的平方根信息矩阵；
Figure QLYQS_38
表示VINS系统所有测量残差，
Figure QLYQS_40
表示各分系统测量残差，
Figure QLYQS_28
和
Figure QLYQS_30
分别表示IMU测量残差和视觉测量残差，
Figure QLYQS_32
表示滑动窗口边缘化过程中获得的先验信息；
Figure QLYQS_34
是Huber函数，用来减小最小二乘问题中异常值的权重。
6.根据权利要求5所述的一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，其特征在于，基于紧耦合滑动窗口算法进行状态估计后，通过重投影误差方程剔除每次优化后的离群值，重投影误差方程为：
Figure QLYQS_46
（8）
Figure QLYQS_47
（9）
式中
Figure QLYQS_48
表示第
Figure QLYQS_49
个特征点的第一次观测，
Figure QLYQS_50
表示相机的第
Figure QLYQS_51
个图像，
Figure QLYQS_52
是视觉惯性里程计模块的参考坐标系，
Figure QLYQS_53
是反投影函数；
Figure QLYQS_54
和
Figure QLYQS_56
分别是左目相机-IMU系统以及右目相机-IMU系统的坐标变换矩阵的旋转分量和平移分量；
Figure QLYQS_57
代表第
Figure QLYQS_58
帧左目相机图片对第
Figure QLYQS_59
帧左目相机图片的重投影误差的观测结果，
Figure QLYQS_60
代表第
Figure QLYQS_61
帧左目相机图片对第
Figure QLYQS_55
帧右目相机图片的重投影误差的观测结果。
7.根据权利要求6所述一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，其特征在于，全局位置耦合设计模块具体工作步骤如下：
（1）将GNSS的全局坐标系与视觉惯性里程计模块的局部坐标系刚性对齐，实现全局坐标系下的多传感器耦合定位；
（2）通过局部坐标系
Figure QLYQS_62
变换到全局坐标系
Figure QLYQS_63
之间的坐标系变换矩阵，将视觉惯性里程计模块通过视觉测量和惯性测量得到的最新的局部测量位置转换为全局估计位置；
（3）全局估计位置作为先验信息传送给全局位置耦合设计模块进行全局位置的初始化；
（4）GNSS信号中断时，全局估计位置作为全局位置进行无人船航迹推算及自身定位；
（5）对最后输出的全局位置进行图优化，得到最优的定位效果，最后将最优全局位置更新到视觉惯性里程计模块，全局位置耦合设计模块。
8.根据权利要求7所述一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，其特征在于，全局估计位置还能够作为局部约束利用最小二乘法进行最优位置的输出，即：
Figure QLYQS_64
（10）
上式中
Figure QLYQS_65
是所有位置的状态向量，
Figure QLYQS_66
是测量集合，包括局部估计位置，全局位置，
Figure QLYQS_67
是Mahalanobis 范数，
Figure QLYQS_68
是对应的协方差，
Figure QLYQS_69
是残差函数；
第一部分局部测量残差：
Figure QLYQS_70
（11）
上式描述了时刻t的相对位姿残差；第一行表示的是相对位置残差，第二行表示的是相对旋转残差；旋转矩阵
Figure QLYQS_71
表示从局部坐标系
Figure QLYQS_72
变换到全局坐标系
Figure QLYQS_73
；
第二部分是全局测量残差：
Figure QLYQS_74
（12）
上式中
Figure QLYQS_75
是GNSS PPP定位模块得到的全局位置的测量值；
Figure QLYQS_76
是根据VINS定位模块得到的全局位置的估计值。
CN202310324328.8A 2023-03-30 2023-03-30 一种耦合视觉惯性里程计与gnss的无人船航迹推算方法 Active CN116026316B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310324328.8A CN116026316B (zh) 2023-03-30 2023-03-30 一种耦合视觉惯性里程计与gnss的无人船航迹推算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310324328.8A CN116026316B (zh) 2023-03-30 2023-03-30 一种耦合视觉惯性里程计与gnss的无人船航迹推算方法

Publications (2)

Publication Number Publication Date
CN116026316A true CN116026316A (zh) 2023-04-28
CN116026316B CN116026316B (zh) 2023-08-29

Family

ID=86072680

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310324328.8A Active CN116026316B (zh) 2023-03-30 2023-03-30 一种耦合视觉惯性里程计与gnss的无人船航迹推算方法

Country Status (1)

Country Link
CN (1) CN116026316B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116659498A (zh) * 2023-05-18 2023-08-29 中山大学 一种紧耦合导航方法、系统、计算机设备及存储介质
CN117387609A (zh) * 2023-10-13 2024-01-12 亿航智能设备（广州）有限公司 一种多源融合冗余定位方法、设备及计算机可读存储介质

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130033574A1 (en) * 2011-08-04 2013-02-07 Apstec Systems Method and system for unveiling hidden dielectric object
US20180031387A1 (en) * 2016-07-29 2018-02-01 Carnegie Mellon University State estimation for aerial vehicles using multi-sensor fusion
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及系统
WO2018106311A2 (en) * 2016-09-22 2018-06-14 The Regents Of The University Of California Signals of opportunity aided inertial navigation
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN111780781A (zh) * 2020-06-23 2020-10-16 南京航空航天大学 基于滑动窗口优化的模板匹配视觉和惯性组合里程计
CN111968128A (zh) * 2020-07-10 2020-11-20 北京航空航天大学 一种基于图像标记的无人机视觉姿态和位置解算方法
CN113077515A (zh) * 2021-06-07 2021-07-06 之江实验室 一种水下视觉惯导压力定位的紧耦合初始化方法
CN113376669A (zh) * 2021-06-22 2021-09-10 东南大学 一种基于点线特征的单目vio-gnss融合定位算法
CN113701750A (zh) * 2021-08-23 2021-11-26 长安大学 一种井下多传感器的融合定位系统
CN114199259A (zh) * 2022-02-21 2022-03-18 南京航空航天大学 一种基于运动状态与环境感知的多源融合导航定位方法
CN114459506A (zh) * 2022-02-28 2022-05-10 清华大学深圳国际研究生院 在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法及系统

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130033574A1 (en) * 2011-08-04 2013-02-07 Apstec Systems Method and system for unveiling hidden dielectric object
US20180031387A1 (en) * 2016-07-29 2018-02-01 Carnegie Mellon University State estimation for aerial vehicles using multi-sensor fusion
WO2018106311A2 (en) * 2016-09-22 2018-06-14 The Regents Of The University Of California Signals of opportunity aided inertial navigation
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及系统
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN111780781A (zh) * 2020-06-23 2020-10-16 南京航空航天大学 基于滑动窗口优化的模板匹配视觉和惯性组合里程计
CN111968128A (zh) * 2020-07-10 2020-11-20 北京航空航天大学 一种基于图像标记的无人机视觉姿态和位置解算方法
CN113077515A (zh) * 2021-06-07 2021-07-06 之江实验室 一种水下视觉惯导压力定位的紧耦合初始化方法
CN113376669A (zh) * 2021-06-22 2021-09-10 东南大学 一种基于点线特征的单目vio-gnss融合定位算法
CN113701750A (zh) * 2021-08-23 2021-11-26 长安大学 一种井下多传感器的融合定位系统
CN114199259A (zh) * 2022-02-21 2022-03-18 南京航空航天大学 一种基于运动状态与环境感知的多源融合导航定位方法
CN114459506A (zh) * 2022-02-28 2022-05-10 清华大学深圳国际研究生院 在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法及系统

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
HAILIANG TANG ET AL.: "LE-VINS: A Robust Solid-State-LiDAR-Enhanced Visual-Inertial Navigation System for Low-Speed Robots", 《IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT 》 *
杨高朝 等: "基于抗差 ＬＭ 的视觉惯性里程计与伪卫星混合高精度室内定位", 《测绘学报》, vol. 51, no. 1 *
罗宇 等: "超短基线水声定位系统误差校准方法综述", 《工程技术》, no. 10 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116659498A (zh) * 2023-05-18 2023-08-29 中山大学 一种紧耦合导航方法、系统、计算机设备及存储介质
CN117387609A (zh) * 2023-10-13 2024-01-12 亿航智能设备（广州）有限公司 一种多源融合冗余定位方法、设备及计算机可读存储介质

Also Published As

Publication number Publication date
CN116026316B (zh) 2023-08-29

Similar Documents

Publication Publication Date Title
CN114199259B (zh) 一种基于运动状态与环境感知的多源融合导航定位方法
CN115407357B (zh) 基于大场景的低线束激光雷达-imu-rtk定位建图算法
CN110070615B (zh) 一种基于多相机协同的全景视觉slam方法
Eustice Large-area visually augmented navigation for autonomous underwater vehicles
Strasdat et al. Scale drift-aware large scale monocular SLAM
CN105865454B (zh) 一种基于实时在线地图生成的无人机导航方法
CN112146654B (zh) 基于关键约束帧的前视成像声呐水下定位与导航方法
CN113627473A (zh) 基于多模态传感器的水面无人艇环境信息融合感知方法
CN113739795B (zh) 一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法
Bucci et al. Comparison of feature detection and outlier removal strategies in a mono visual odometry algorithm for underwater navigation
CN112444245B (zh) 一种基于偏振光、光流矢量、双目视觉传感器的仿昆虫视觉组合导航方法
CN116026316B (zh) 一种耦合视觉惯性里程计与gnss的无人船航迹推算方法
CN116380079A (zh) 一种融合前视声呐与orb-slam3的水下slam方法
CN118351177B (zh) 一种融合反射率投影图像边缘特征的位姿优化方法及系统
Lu et al. A multi-sensor fusion approach for maritime autonomous surface ships berthing navigation perception
CN116659510A (zh) 水下机器人定位与避障方法、装置及存储介质
Song et al. Acoustic-VINS: Tightly coupled acoustic-visual-inertial navigation system for autonomous underwater vehicles
Chavez et al. Adaptive navigation scheme for optimal deep-sea localization using multimodal perception cues
CN118691997A (zh) 一种基于立体感知的船舶靠离泊场景地图构建方法
CN118348554A (zh) 水面无人艇绝对定位方法、设备及存储介质
Beall et al. Bundle adjustment in large-scale 3d reconstructions based on underwater robotic surveys
Zhang et al. Triangle codes and tracer lights based absolute positioning method for terminal visual docking of autonomous underwater vehicles
Hu et al. Semantic map-based localization of usv using lidar in berthing and departing scene
CN117849777B (zh) 船载无人机定位导航方法、装置、电子设备、存储介质
CN113790718A (zh) 一种基于无人船的信息共享系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant