本发明属于auv水下导航技术领域，具体涉及一种rgi-lidar/sins紧耦合auv水下导航定位方法与系统。
背景技术：
在当今日益竞争的国际形势下，水下导航定位无论是对于打捞搜救、海底管道铺设和检查维护、沉船遗址科考、珊瑚礁及海洋生物保护等民事应用，还是潜艇战略巡航、水下排雷、航道封锁和通信中继等军事用途，都具有很高的研宄价值及意义，目前已成为国内外研究的热点与难点。
在现有的定位技术中，由于捷联式惯性导航系统可以长时间稳定的提供载体的方位信息和距离信息，既不依赖于外界设备又不受外界干扰，因此现有的auv自主导航方法都是围绕惯性导航为中心。但是惯性器件具有误差积累的性质，需要其他导航定位方法辅助，定时修正惯导的导航参数误差，才能实现对载体长时间的精确定位。
传统的惯导辅助导航方法包括地磁导航、重力导航、水声定位导航、gps位置信息校正等。重力导航定位技术和地磁导航技术需要重力或地磁数据库等先验图作为辅助导航定位参考，但是在绝大多数的海域都没有这类先验图，所以该两种方法不能满足更广泛的水下导航定位需求；水声定位导航作用范围广，但是需要在海底布设应答器阵列，这种方法限制了auv的任务区域，即必须在声学信标预置的范围内执行任务；并且声学信标的放置和维护成本极高，花费巨大；gps位置信息校正需要auv在执行任务过程中中断任务，上浮到水面进行gps定位，不利于任务的执行。传统的辅助导航方法不能满足广泛、长时间、远距离的水下导航定位的需求。
技术实现要素：
本发明的目的在于，提供一种rgi-lidar/sins紧耦合auv水下导航定位方法及系统，其切片成像技术大大的抑制了水体的后向散射，提高了图像的对比度，和传统光学成像相比，大大提升了光学图像的对比度，能满足广泛、长时间、远距离的水下导航定位的需求。
为解决上述技术问题，本发明的技术方案为：一种rgi-lidar/sins紧耦合auv水下导航定位方法，包括以下步骤：
通过捷联解算得到相应时刻的auv位置信息、姿态信息的导航信息；
获取水下远距离、高清晰度的选通图像；
对imu输出信息进行预积分，获取auv在不同时刻的imu位姿约束；
对当前帧的选通图像进行特征提取，并将上一帧的选通图像作为参考帧与之进行特征匹配，获取相邻图像之间的位姿关系，即视觉里程计约束；
对当前帧的选通图像进行回环检测，若检测到符合条件的回环帧图像，则将当前帧的选通图像与相应的回环帧图像进行特征匹配，获取回环检测约束；
将imu位姿约束、视觉里程计约束以及回环检测约束作为约束关系加入到位姿图中进行联合优化，获取auv位姿；
输出auv位姿；
将auv位姿作为惯性数据输入。
进一步地，对imu进行预积分的具体方法为先捷联解算得到auv在不同时刻速度、位置和姿态，再根据以下公式进行预积分处理，获取auv在不同时刻之间的位姿约束因子，获取auv在不同时刻的imu位姿约束，
式中，
进一步地，特征提取的方法为orb特征提取方法，即利用fast角点检测子提取关键点的位置，并为fast角点增加尺度和旋转的描述，采用brief描述子来描述关键点的属性。
进一步地，特征匹配的方法为在相邻帧图像提取的特征点中，找到相对应的特征点，即特征点的描述子特征向量之间的距离越小，表示两个特征点越匹配。
进一步地，回环检测为基于欧氏距离的回环检测方法，即根据捷联惯性导航系统推算出当前时刻的位姿xi，并对之前时刻的位姿进行搜索，找到欧氏距离最近的位姿xj，用d来表示最近欧氏距离；若d大于阈值d0，则认为当前帧的选通图像无相应的回环帧图像；若d不大于阈值d0，则认为当前帧的选通图像有相应的回环帧图像，求出两者的位姿关系，即回环约束；其中阈值d0是根据实际应用中auv的移动速度和成像系统的帧频来设置的。
进一步地，位姿图根据下式表示：
g＝(x，f)
式中，x是位姿图中的节点，表示的是与选通图像相对应时刻auv的位姿；f是位姿图中的边，表示的是节点之间或者单个节点的约束条件。
一种用于实现上述的rgi-lidar/sins紧耦合auv水下导航定位方法的系统，其特征在于，包括rgi-lidar，sins和数据处理单元；其中，
rgi-lidar，用于获取水下远距离、高清晰度的选通图像，并将选通图像发送至数据处理单元；
sins，用于通过捷联惯导算法计算得到相应时刻的auv位置信息、姿态信息的导航信息并发送至数据处理单元；
数据处理单元，用于根据auv位置信息，对imu进行预积分，获取auv在不同时刻的imu位姿约束；对当前帧的选通图像进行特征提取，并将上一帧的选通图像作为参考帧与之进行特征匹配，获取相邻图像之间的位姿关系，即视觉里程计约束；对当前帧的选通图像进行回环检测，若检测到符合条件的回环帧图像，则将当前帧的选通图像与相应的回环帧图像进行特征匹配，获取回环检测约束；将imu位姿约束、视觉里程计约束以及回环检测约束作为约束关系加入到位姿图中进行联合优化，获取并输出auv位姿；将auv位姿发送至sins作为惯性数据输入。
进一步地，对imu进行预积分的具体方法为先捷联解算得到auv在不同时刻速度、位置和姿态，再根据以下公式进行预积分处理，获取auv在不同时刻之间的位姿约束因子，获取auv在不同时刻的imu位姿约束，
式中，
进一步地，回环检测为基于欧氏距离的回环检测方法，即根据捷联惯性导航系统推算出当前时刻的位姿xi，并对之前时刻的位姿进行搜索，找到欧氏距离最近的位姿xj，用d来表示最近欧氏距离；若d大于阈值d0，则认为当前帧的选通图像无相应的回环帧图像；若d不大于阈值d0，则认为当前帧的选通图像有相应的回环帧图像，求出它们之间的位姿关系，即回环约束；其中阈值d0是根据实际应用中auv的移动速度和成像系统的帧频来设置的。
进一步地，该系统中还包括传感器组，用于采集多组观测信息加入到位姿图中进行融合。
与现有技术相比，本发明的有益效果为：
1、首次将距离选通三维激光成像雷达rgi-lidar引入到水下auv导航定位中，其切片成像技术大大的抑制了水体的后向散射，提高了图像的对比度，和传统光学成像相比，大大提升了光学图像的对比度；
2、配置了rgi-lidar，填补了水下slam技术在近距离、高精度导航定位这一应用中的空白；
3、提出了一种基于位姿图的紧耦合slam框架，适用于多传感器信息的融合，不仅可以融合sins、rgi-lidar的观测信息，还可以将成像声呐、gps、多普勒测速仪(dvl)、测高仪等多种传感器的观测信息加入到位姿图中进行融合，为更广泛、更长时间、更远距离、更精确的水下auv导航定位提供更多有效的信息冗余。
附图说明
图1为本发明实施例的流程框图；
图2为本发明实施例的位姿图示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。
在此公开本发明结构实施例和方法的描述。可以了解的是并不意图将本发明限制在特定公开的实施例中，而是本发明可以通过使用其它特征，元件方法和实施例来加以实施。不同实施例中的相似元件通常会标示相似的号码。
本发明由安装在由安装在auv上的捷联惯性导航系统sins、距离选通三维激光成像雷达rgi-lidar和数据处理单元三大部分组成，其中所述的sins包括惯性测量单元imu和捷联解算模块，所述的数据处理单元包括imu预积分模块，图优化模块，特征提取及匹配模块，以及回环检测模块，如图1所示，所述方法通过以下步骤实现：
步骤1、捷联惯性导航系统sins中的imu元件可以输出系统的加速度和角加速度等惯性数据，捷联解算模块通过对惯性数据进行捷联解算得到相应时刻的auv位置信息、姿态信息等导航信息；距离选通三维激光成像雷达获取水下远距离、高清晰度的选通图像，并全都输出给数据处理单元；
所述的距离选通三维激光成像雷达采用脉冲激光器作为照明光源，以选通成像器件iccd作为探测器，以时序控制器实现照明光源和探测器之间的同步；工作中，脉冲激光器发射激光脉冲，当激光脉冲传至水体悬浮颗粒、目标以及背景时，被反射或者散射，会分别形成向后的水体后向散射、目标回波信号以及背景噪声信号；只有当目标的回波信号传播到选通成像器件时，选通门才开启，接收信号并完成图像的采集和生成，其他时候选通门都关闭，因此可以抑制水体后向散射和背景噪声，提高图像的对比度和系统的作用距离。
步骤2、数据处理单元中的imu预积分模块对imu输出进行预积分，从而获取auv在不同时刻之间的imu位姿约束；特征提取及匹配模块对当前帧选通图像提取特征，并将上一帧作为参考帧与之进行特征匹配，利用从而获取相邻图像之间的位姿关系，即视觉里程计约束；回环检测模块对当前帧图像进行回环检测，若检测到符合条件的回环帧，则将当前帧图像与相应的回环帧进行特征提取与匹配，从而获取回环检测约束；
所述的imu预积分可以计算出速度、位置和姿态在不同时刻i和j之间的相对值，分别用δvij,δpij,和δrij来表示，它们的计算方式如下：
其中，
所述的对当前帧选通图像提取特征，指的是从图像中提取一些有代表性的特征点来代表整个图像信息，得到特征点和描述特征点信息的特征向量。本发明所选用的特征为orb(orientedfastandrotatedbrief)特征，即利用fast角点检测子提取关键点的位置，并为fast角点增加尺度和旋转的描述，采用brief描述子来描述关键点的属性。相比较于常用的sift特征和surf特征，orb特征提取方法速度上有一个数量级的优势，适用于auv的实时导航定位。需要说明的是，本发明所提供水下auv导航定位方法适用但不仅限于orb特征。
所述的特征匹配指的是在相邻帧图像提取的特征点中，找到相对应的特征点，即特征点的描述子特征向量之间的距离越小，表示两个特征点越匹配。通过特征匹配，可以得到一组匹配点，以及它们的像素坐标。利用这些匹配关系，用对极几何约束即求解两帧之间的位姿关系，即视觉里程计约束。
所述的回环检测，采用的是基于欧氏距离的回环检测方法：当新的图像帧到来时，根据捷联惯性导航系统推算出当前时刻的位姿xi，并对之前时刻的位姿进行搜索，找到欧氏距离最近的位姿xj，用d来表示最近欧氏距离；若d大于阈值d0，则认为当前帧没有找到回环；若d不大于阈值d0，则认为当前帧找到了回环，将当前帧与最近位姿对应的选通图像进行特征提取并匹配，进而求出它们之间的位姿关系，即回环约束；其中阈值d0是根据实际应用中auv的移动速度和成像系统的帧频来设置的。
步骤3、图优化模块将imu位姿约束、视觉里程计约束以及回环检测约束作为约束关系加入到位姿图中进行联合优化，从而获取更准确的auv在不同时刻的位姿。图优化模块输出的更准确的auv在不同时刻的位姿不仅可以作为导航信息输出，还可以反馈给捷联解算系统sins，为下一次迭代提供更准确的惯性数据输入。
所述的位姿图如图2所示，位姿图由下式表示：
g＝(x，f)
式中，x是位姿图中的节点，表示的是与选通图像相对应时刻auv的位姿；f是位姿图中的边，表示的是节点之间或者单个节点的约束条件。本发明中的约束条件包含imu位姿约束、视觉里程计约束和回环约束；可选的，gps约束以及声呐、深度传感器、压力传感器等其他传感器的测量值均可以作为约束条件加入到位姿图中，来进一步提高auv位姿估计的精度。
所述位姿图的优化求解，即在众多约束条件的联合约束下求解auv的位姿，可以等效为最大似然估计问题，进一步的，在高斯噪声的假设下，可以等效为非线性最小二乘的求解问题。工程中，可以应用ceressolver、g2o等非线性库很方便的求解此类问题。
以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
1.一种rgi-lidar/sins紧耦合auv水下导航定位方法，其特征在于，包括以下步骤：
通过捷联解算得到相应时刻包含auv位置信息、速度、姿态信息的导航信息；
获取水下远距离、高清晰度的选通图像；
对imu输出信息进行预积分，获取auv在不同时刻的imu位姿约束；
对当前帧的选通图像进行特征提取，并将上一帧的选通图像作为参考帧与之进行特征匹配，获取相邻图像之间的位姿关系，即视觉里程计约束；
对当前帧的选通图像进行回环检测，若检测到符合条件的回环帧图像，则将当前帧的选通图像与相应的回环帧图像进行特征匹配，获取回环检测约束；
将imu位姿约束、视觉里程计约束以及回环检测约束作为约束关系加入到位姿图中进行联合优化，获取优化后的auv位姿；
输出auv位姿；
将auv位姿作为惯性数据输入。
2.根据权利要求1所述的方法，其特征在于，对imu进行预积分的具体方法为先捷联解算得到auv在不同时刻速度、位置和姿态，再根据以下公式进行预积分处理，获取auv在不同时刻之间的位姿约束因子，获取auv在不同时刻的imu位姿约束，
式中，
3.根据权利要求1所述的方法，其特征在于，特征提取的方法为orb特征提取方法，即利用fast角点检测子提取关键点的位置，并为fast角点增加尺度和旋转的描述，采用brief描述子来描述关键点的属性。
4.根据权利要求1所述的方法，其特征在于，特征匹配的方法为在相邻帧图像提取的特征点中，找到相对应的特征点，即特征点的描述子特征向量之间的距离越小，表示两个特征点越匹配。
5.根据权利要求1所述的方法，其特征在于，回环检测为基于欧氏距离的回环检测方法，即根据捷联惯性导航系统推算出当前时刻的位姿xi，并对之前时刻的位姿进行搜索，找到欧氏距离最近的位姿xj，用d来表示最近欧氏距离；若d大于阈值d0，则认为当前帧的选通图像无相应的回环帧图像；若d不大于阈值d0，则认为当前帧的选通图像有相应的回环帧图像，求出两者的位姿关系，即回环约束；其中阈值d0是根据实际应用中auv的移动速度和成像系统的帧频来设置的。
6.根据权利要求1所述的方法，其特征在于，位姿图根据下式表示：
g＝(x，f)
式中，x是位姿图中的节点，表示的是与选通图像相对应时刻auv的位姿；f是位姿图中的边，表示的是节点之间或者单个节点的约束条件。
7.一种用于实现权利要求1所述的rgi-lidar/sins紧耦合auv水下导航定位方法的系统，其特征在于，包括rgi-lidar，sins和数据处理单元；其中，
rgi-lidar，用于获取水下远距离、高清晰度的选通图像，并将选通图像发送至数据处理单元；
sins，用于通过捷联惯导算法计算得到相应时刻的auv位置信息、姿态信息的导航信息并发送至数据处理单元；
数据处理单元，用于根据auv位置信息，对imu进行预积分，获取auv在不同时刻的imu位姿约束；对当前帧的选通图像进行特征提取，并将上一帧的选通图像作为参考帧与之进行特征匹配，获取相邻图像之间的位姿关系，即视觉里程计约束；对当前帧的选通图像进行回环检测，若检测到符合条件的回环帧图像，则将当前帧的选通图像与相应的回环帧图像进行特征匹配，获取回环检测约束；将imu位姿约束、视觉里程计约束以及回环检测约束作为约束关系加入到位姿图中进行联合优化，获取并输出优化后的auv位姿；将auv位姿发送至sins作为惯性数据输入。
8.根据权利要求7所述的系统，其特征在于，对imu进行预积分的具体方法为先计算出auv在不同时刻速度、位置和姿态的相对值，再根据以下公式进行预积分处理，获取auv在不同时刻之间的位姿约束因子，获取auv在不同时刻的imu位姿约束，
式中，
9.根据权利要求7所述的系统，其特征在于，回环检测为基于欧氏距离的回环检测方法，即根据捷联惯性导航系统推算出当前时刻的位姿xi，并对之前时刻的位姿进行搜索，找到欧氏距离最近的位姿xj，用d来表示最近欧氏距离；若d大于阈值d0，则认为当前帧的选通图像无相应的回环帧图像；若d不大于阈值d0，则认为当前帧的选通图像有相应的回环帧图像，求出它们之间的位姿关系，即回环约束；其中阈值d0是根据实际应用中auv的移动速度和成像系统的帧频来设置的。
10.根据权利要求7所述的系统，其特征在于，还包括传感器组，用于采集多组观测信息加入到位姿图中进行融合。
技术总结