CN116026316B - 一种耦合视觉惯性里程计与gnss的无人船航迹推算方法 - Google Patents
一种耦合视觉惯性里程计与gnss的无人船航迹推算方法 Download PDFInfo
- Publication number
- CN116026316B CN116026316B CN202310324328.8A CN202310324328A CN116026316B CN 116026316 B CN116026316 B CN 116026316B CN 202310324328 A CN202310324328 A CN 202310324328A CN 116026316 B CN116026316 B CN 116026316B
- Authority
- CN
- China
- Prior art keywords
- global
- visual
- coordinate system
- imu
- 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.)
- Active
Links
- 230000000007 visual effect Effects 0.000 title claims abstract description 77
- 238000000034 method Methods 0.000 title claims abstract description 46
- 230000008878 coupling Effects 0.000 title claims abstract description 45
- 238000010168 coupling process Methods 0.000 title claims abstract description 45
- 238000005859 coupling reaction Methods 0.000 title claims abstract description 45
- 238000005259 measurement Methods 0.000 claims abstract description 53
- 238000013461 design Methods 0.000 claims abstract description 27
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 22
- 239000011159 matrix material Substances 0.000 claims description 18
- 239000013598 vector Substances 0.000 claims description 16
- 230000009466 transformation Effects 0.000 claims description 11
- 238000005457 optimization Methods 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 7
- 230000010354 integration Effects 0.000 claims description 6
- 230000003287 optical effect Effects 0.000 claims description 5
- 230000000694 effects Effects 0.000 claims description 4
- 238000013519 translation Methods 0.000 claims description 4
- 241001113556 Elodea Species 0.000 claims description 3
- 230000008859 change Effects 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 claims description 3
- 238000011160 research Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 2
- 238000003709 image segmentation Methods 0.000 description 2
- 238000007781 pre-processing Methods 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，视觉里程计使用光束法平差的方法对前后两帧图像的特征匹配进行优化以获得滑动窗口中每一帧图像的最佳姿态；惯性里程计IMU采用基于紧耦合滑动窗口算法进行状态估计以获得准确而鲁棒的结果，作为全局位置中的局部约束；全局位置耦合设计模块将视觉惯性里程计模块通过视觉测量和惯性测量得到的最新的局部测量位置转换为全局估计位置，全局估计位置作为全局位置进行无人船航迹推算及自身定位，将最优全局位置更新到视觉惯性里程计模块，全局位置耦合设计模块。本发明提出了一种高精度、抗干扰的航迹推算及自身定位方法。
Description
技术领域
本发明涉及无人船技术领域，尤其涉及一种耦合视觉惯性里程计与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数据进行预积分，得到当前时刻的位姿、速度、旋转角；联合初始化后采用基于紧耦合滑动窗口算法进行状态估计以获得准确而鲁棒的结果。
视觉里程计首先将无人船搭载的高清相机实时获取的图像信息进行Lab颜色空间的转换，提取其中的a分量进行特征点提取，利用KLT金字塔进行光流跟踪，使用光束法平差的方法对前后两帧图像的特征匹配进行优化以获得滑动窗口中每一帧图像的最佳姿态；
惯性里程计IMU首先进行IMU数据预积分，得到当前时刻的位姿、速度、旋转角，完成惯性里程计的初始化，之后进行VINS联合初始化，采用基于紧耦合滑动窗口算法进行状态估计以获得准确而鲁棒的结果，并且作为全局位置中的局部约束；
全局位置耦合设计模块将视觉惯性里程计模块通过视觉测量和惯性测量得到的最新的局部测量位置转换为全局估计位置，全局估计位置作为全局位置进行无人船航迹推算及自身定位，对最后输出的全局位置进行图优化，得到最优的定位效果，最后将最优全局位置更新到视觉惯性里程计模块，全局位置耦合设计模块。
全局位置耦合设计模块：通过GNSS接收机天线接收GNSS信号从而根据精密单点定位模型解算出自身在大地坐标系中的经纬度，再经坐标系变换矩阵转换成地心地固坐标系中的三维位置坐标，同时将其他定位模块的预测位置坐标转换为地心地固坐标系中的三维坐标才能与无人船的位置坐标实现坐标系的统一。在全局位置节点，由视觉惯性里程计模块的测量值提供局部约束，由全局位置耦合设计模块的测量值提供的全局约束。由于复杂的天气下卫星信号的稳定性和有效性较低，视觉惯性里程计模块的定位结果将被选择性地用作全局约束，代替GNSS进行航迹推算及自身定位。对全局位置进行图优化就可以得到最优的定位结果。最后将所得的最优全局位置坐标更新到视觉惯性里程计模块中的滑动窗口模块、全局位置耦合设计模块，由此形成多源耦合高精度定位系统的耦合闭环。
视觉惯性里程计模块，具体工作步骤如下：
A1，图像预处理：将无人船搭载的高清相机实时获取的图像信息进行Lab颜色空间的转换，转换公式如下：
将XYZ参数转换为Lab颜色空间：
（2）
在公式（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）
式中表示第/>个特征点的第一次观测，/>表示相机的第/>个图像，/>是视觉惯性里程计模块的参考坐标系，/>是反投影函数。
和/>分别是左目相机-IMU系统以及右目相机-IMU系统的坐标变换矩阵的旋转分量和平移分量；/>代表第/>帧左目相机图片对第/>帧左目相机图片的重投影误差的观测结果，/>代表第/>帧左目相机图片对第/>帧右目相机图片的重投影误差的观测结果。
全局位置耦合设计模块具体工作步骤如下：
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 (2)
1.一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，其特征在于，通过视觉惯性里程计模块与全局位置耦合设计模块实现无人船航迹推算；
视觉惯性里程计模块分为视觉里程计与惯性里程计两部分；
视觉里程计首先将无人船搭载的高清相机实时获取的图像信息进行Lab颜色空间的转换，提取其中的a分量进行特征点提取，利用KLT金字塔进行光流跟踪，使用光束法平差的方法对前后两帧图像的特征匹配进行优化以获得滑动窗口中每一帧图像的最佳姿态；
惯性里程计IMU首先进行IMU数据预积分，得到当前时刻的位姿、速度、旋转角，完成惯性里程计的初始化，之后进行VINS联合初始化，采用基于紧耦合滑动窗口算法进行状态估计以获得准确而鲁棒的结果，并且作为全局位置中的局部约束；
全局位置耦合设计模块将视觉惯性里程计模块通过视觉测量和惯性测量得到的最新的局部测量位置转换为全局估计位置，全局估计位置作为全局位置进行无人船航迹推算及自身定位，对最后输出的全局位置进行图优化，得到最优的定位效果，最后将最优全局位置更新到视觉惯性里程计模块，全局位置耦合设计模块；
将无人船搭载的高清相机实时获取的图像信息进行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通道下，岸边水草、树木灰度值小，水面区域灰度值大，水面区域与岸上区域的灰度值有大的差异，且树木在水中的倒影区域与水面无倒影区域差别不大，使用阈值分割方法将水面部分区域分割出并剔除；处理后的图像进行特征点提取；
滑动窗口中的状态向量表示如下：
其中，XV表示完整的状态向量，包括IMU状态向量xk，IMU坐标系与相机坐标系之间的坐标转换矩阵初次观测的逆深度γl，l∈[0,m]；B表示IMU坐标系，C表示相机坐标系；m与n分别表示滑动窗口中特征点和关键帧的数量；xk是拍到第k帧图像时的IMU状态向量；IMU初始时刻的位置被定义成局部坐标系V的原点，后续IMU的位置/>速度/>和姿态/>都是相对于局部坐标系V得到的；bGy,k,bAc,k分别表示的陀螺仪和加速度计的慢时变误差；
滑动窗口中的状态的最大后验估计通过最小化所有测量残差与先验的Mahalanobis范数的和获得：
上式中，l表示第l个特征，k表示拍摄的第k帧，k+1表示拍摄的下一帧，L为过地面点的子午面与起始子午面之间的夹角，Lj表示第j张图像下过地面点的子午面与起始子午面之间的夹角，B表示IMU坐标系，C表示相机坐标系，P表示位置,表示相邻两时刻下IMU位姿变化，/>表示为视觉测量的平方根信息矩阵；r表示各分系统测量残差，/>和分别表示IMU测量残差和视觉测量残差，(rp-HpXV)表示滑动窗口边缘化过程中获得的先验信息；ρ(·)是Huber函数，用来减小最小二乘问题中异常值的权重；
基于紧耦合滑动窗口算法进行状态估计后，通过重投影误差方程剔除每次优化后的离群值，重投影误差方程为：
式中表示第l个特征点的第一次观测，Ci,j表示相机的第i个图像，V是视觉惯性里程计模块的参考坐标系，/>是反投影函数；
和/>分别是左目相机-IMU系统以及右目相机-IMU系统的坐标变换矩阵的旋转分量和平移分量；/>代表第i帧左目相机图片对第j帧左目相机图片的重投影误差的观测结果，/>代表第i帧左目相机图片对第j帧右目相机图片的重投影误差的观测结果；
全局位置耦合设计模块具体工作步骤如下：
(1)将GNSS的全局坐标系与视觉惯性里程计模块的局部坐标系刚性对齐，实现全局坐标系下的多传感器耦合定位；
(2)通过局部坐标系V变换到全局坐标系G之间的坐标系变换矩阵，将视觉惯性里程计模块通过视觉测量和惯性测量得到的最新的局部测量位置转换为全局估计位置；
(3)全局估计位置作为先验信息传送给全局位置耦合设计模块进行全局位置的初始化；
(4)GNSS信号中断时，全局估计位置作为全局位置进行无人船航迹推算及自身定位；
(5)对最后输出的全局位置进行图优化，得到最优的定位效果，最后将最优全局位置更新到视觉惯性里程计模块，全局位置耦合设计模块。
2.根据权利要求1所述一种耦合视觉惯性里程计与GNSS的无人船航迹推算方法，其特征在于，全局估计位置还能够作为局部约束利用最小二乘法进行最优位置的输出，即：
上式中X＝[x0，x1，...，xn]是所有位置的状态向量，S是测量集合，包括局部估计位置，全局位置，是Mahalanobis范数，T是对应的协方差，/>是残差函数；
第一部分局部测量残差：
上式描述了时刻t的相对位姿残差；
第二部分是全局测量残差：
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 CN116026316A (zh) | 2023-04-28 |
| CN116026316B true 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) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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 | 清华大学深圳国际研究生院 | 在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法及系统 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9304190B2 (en) * | 2011-08-04 | 2016-04-05 | Apstec Systems Ltd | Method and system for unveiling hidden dielectric object |
| US10295365B2 (en) * | 2016-07-29 | 2019-05-21 | Carnegie Mellon University | State estimation for aerial vehicles using multi-sensor fusion |
-
2023
- 2023-03-30 CN CN202310324328.8A patent/CN116026316B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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 (1)
| Title |
|---|
| LE-VINS: A Robust Solid-State-LiDAR-Enhanced Visual-Inertial Navigation System for Low-Speed Robots;Hailiang Tang et al.;《IEEE Transactions on Instrumentation and Measurement 》;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116026316A (zh) | 2023-04-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110070615B (zh) | 一种基于多相机协同的全景视觉slam方法 | |
| CN108534782B (zh) | 一种基于双目视觉系统的地标地图车辆即时定位方法 | |
| CN114199259B (zh) | 一种基于运动状态与环境感知的多源融合导航定位方法 | |
| CN105865454B (zh) | 一种基于实时在线地图生成的无人机导航方法 | |
| CN107451593B (zh) | 一种基于图像特征点的高精度gps定位方法 | |
| CN101598556B (zh) | 一种未知环境下无人机视觉/惯性组合导航方法 | |
| CN110044354A (zh) | 一种双目视觉室内定位与建图方法及装置 | |
| CN109239709B (zh) | 一种无人船的局部环境地图自主构建方法 | |
| CN113223045B (zh) | 基于动态物体语义分割的视觉与imu传感器融合定位系统 | |
| CN113739795B (zh) | 一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法 | |
| CN110187375A (zh) | 一种基于slam定位结果提高定位精度的方法及装置 | |
| CN114234967B (zh) | 一种基于多传感器融合的六足机器人定位方法 | |
| CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| WO2023198090A1 (en) | 3d vision aided gnss real-time kinematic positioning for autonomous systems in urban canyons | |
| CN116448100A (zh) | 一种多传感器融合的近岸无人船slam方法 | |
| CN116380079A (zh) | 一种融合前视声呐与orb-slam3的水下slam方法 | |
| CN114677531B (zh) | 一种融合多模态信息的水面无人艇目标检测与定位方法 | |
| CN117470259A (zh) | 一种子母式空地协同多传感器融合三维建图系统 | |
| CN116026316B (zh) | 一种耦合视觉惯性里程计与gnss的无人船航迹推算方法 | |
| CN115344033B (zh) | 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法 | |
| CN115930948A (zh) | 一种果园机器人融合定位方法 | |
| CN114459474B (zh) | 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法 | |
| CN114025320A (zh) | 一种基于5g信号的室内定位方法 | |
| Mirisola et al. | Trajectory recovery and 3d mapping from rotation-compensated imagery for an airship |
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 |