Underwater Positioning Method Based on Vision-Inertia-Pressure Fusion
-
摘要: 水下机器人在水下非结构化环境作业时, 难以依赖外部基站进行定位, 多传感器融合自主定位在此类环境具有重要应用价值。文中针对水下多传感器融合定位中视觉定位稳定性差、惯性导航存在较大漂移等问题, 提出一种紧组合视觉、惯性及压力传感器的多传感器融合定位方法。通过图优化方法进行多传感器融合, 基于深度信息对视觉惯性数据进行误差辨识以提升融合数据质量; 针对融合定位过程中出现的漂移和定位丢失问题, 采用深度传感器进行权重分配以提供更加细致的系统初始化, 同时引入闭环检测和重定位方法, 有效改善漂移和定位丢失的问题。通过试验验证发现, 所提出的融合定位算法相较于视觉惯性融合定位方法精度提升48.4%, 具有更好的精度和鲁棒性, 实际工况定位精度可达厘米级。Abstract: In underwater unstructured environments, robots face difficulties in relying on external base stations for localization. Therefore, autonomous localization using multi-sensor fusion has significant application value in such settings. This paper aimed to address issues such as poor stability in visual localization and substantial drift in inertial navigation within underwater multi-sensor fusion localization and proposed a tightly integrated multi-sensor fusion localization method that combined visual, inertial, and pressure sensors. By utilizing graph optimization techniques for multi-sensor fusion and identifying errors in visual-inertial data based on depth information, the quality of the fused data was enhanced. To address drift and localization loss during the fusion localization process, a depth sensor was employed for weight allocation to provide more detailed system initialization. Additionally, closed-loop detection and relocalization methods were introduced to effectively mitigate drift and localization loss. Experimental validation demonstrates that the proposed fusion localization algorithm improves accuracy by 48.4% compared to visual-inertial fusion localization methods, achieving superior precision and robustness. The actual positioning accuracy can reach the centimetre level.
-
0. 引言
水下机器人在执行任务过程中, 高精度的定位对保证其安全和作业效率至关重要。水下环境通常高度非结构化, 常见的基于声学的定位方法, 如多普勒计程仪(Doppler velocity log, DVL)、超短基线(ultra-short baseline, USBL)定位系统和声呐[1], 因价格昂贵、部署复杂且不适用于高度非结构化环境, 限制了其在水下机器人定位领域的应用和普及。近年来, 基于多传感器融合的水下实时定位广受关注, 取得了大量的研究成果。
Wang等[1]针对水下单机自主定位难的问题, 首次提出一种联合视觉惯性的水下定位系统, 通过惯性测量单元(inertial measurement unit, IMU)和视觉对水下机器人位姿分别进行估计, 建立联合优化模型, 相较于单IMU航位推算, 引入传感器联合约束后的定位结果有效解决了IMU的累积漂移和在低速状态下的不稳定。Smithanik等[2]在上述联合视觉惯性水下定位系统的基础上, 提出了一种针对水下非结构化环境的融合定位与探测系统, 基于视觉建立起针对非结构化环境的视觉定位约束, 由于对环境进行了特征提取与识别, 故融合定位效果更加准确; Cheng等[3]通过结合声呐、双目摄像头和IMU信息, 使得到的融合定位算法更适合于水下环境; Shen等[4]考虑到水下环境较为昏暗, 对水下光路成像模型进行构建, 并基于此模型建立了水下图像增强算法, 显著提升了水下定位的精度; Raveendran[5]和Li[6]等都对水下图像处理进行了研究, Raveendran的研究集中在增强水下图像, 以便获得更多的特征点; Li则专注于测试不同类型的特征点在水下环境中的效果。二者研究结果均有效提升了水下视觉位姿估计的精度; Shaukat等[7]提出一种基于水下目标物的传感器融合定位方法, 通过图优化方法融合IMU与视觉这2种具有互补性质的传感器, 同时基于视觉对目标物进行特征提取与识别, 进一步增强对目标物的高精度定位能力; Wang等[8]提出一种基于拓展卡尔曼滤波的水底环境实时三维重建算法, 该算法采用双目摄像头作为主要传感器, 通过特征点检测重建海底三维环境, 可以实时计算机器人位姿, 同时建立海底地形地图。
虽然水下多传感器融合定位研究成果丰硕, 但多为强依赖于视觉和声呐的定位方法。水下典型非结构化环境如图1所示。声学定位在残骸和洞穴等水下非结构化环境中无法使用, 视觉由于水下悬浮颗粒、模糊度以及光线和颜色衰减, 特征不像水上环境那样能清晰定义, 易导致定位不准确甚至完全丢失。同时, 现有多传感器融合定位多为基于卡尔曼滤波的松耦合方式, 无法有效利用各个传感器的观测数据, 融合定位效果不佳。针对此, 文中提出一种专门针对水下环境的新型定位系统, 尤其适合于残骸和水下洞穴等声呐无法发挥作用的非结构化环境。该算法利用图优化框架融合视觉、惯性和深度传感器数据, 通过深度传感器提供视觉特征丢失及IMU低速漂移状态下的误差辨识以提升数据质量。针对融合定位过程中存在的累积误差, 采用紧耦合方式, 通过深度传感器对闭环优化和定位系统初始化过程添加约束, 提高机器人多传感器融合定位的鲁棒性和精度。
1. 视觉-惯性-压力融合定位模型
图优化作为一种非线性优化方法, 可以直观地融合多种约束, 进而建立优化目标函数, 通过构建因子图, 快速搭建优化模型。在多传感器融合定位领域, 因子图的边代表不同传感器对机器人位姿的观测, 作为损失函数的约束; 因子图的节点代表不同的优化变量, 作为损失函数的待优化变量。
1.1 融合定位框架描述
针对声学定位无法用于封闭非结构化环境、视觉定位稳定性差以及IMU存在较大漂移问题, 通过引入深度传感器提供稳定的水下观测来优化融合定位的精度。完整的传感器系统包含以下4个坐标系: 相机、IMU、深度传感器和世界坐标系, 分别表示为C、I、D和W。任意2个坐标系X和Y之间的转换用齐次变换矩阵表示为
${_X}\_{{\boldsymbol{T}}_Y} = \left[ {{{\left. {{_X}\_{{\boldsymbol{R}}_Y}} \right|}_X}{{\boldsymbol{p}}_Y}} \right]$ , 其中${_X}\_{{{\boldsymbol{R}}}_Y}$ 是对应四元数${_X}\_{{{\boldsymbol{q}}}_Y}$ 的旋转矩阵;${_X}\_{{\boldsymbol{p}}_Y}$ 是位置向量。定义系统正在估计的机器人R的状态为
$$ {{\boldsymbol{x}}_R} = {\left[ {{_W}\_{\boldsymbol{p}}_I^{\text{T}}, {}{_W}\_{\boldsymbol{q}}_I^{\text{T}}, {}{_W}\_{\boldsymbol{v}}_I^{\text{T}}, {\boldsymbol{b}}_g^{\text{T}}, {\boldsymbol{b}}_a^{\text{T}}} \right]^{\text{T}}} $$ (1) 其中包含机器人位置
$ {_W}\_{\boldsymbol{p}}_I^{} $ , 由四元数表示的机器人姿态${}{_W}\_{\boldsymbol{q}}_I^{}$ 以及机器人线速度${}{_W}\_{\boldsymbol{v}}_I^{}$ , 所有状态量定义在机器人IMU坐标系下; 此外, 状态向量还包含陀螺仪和加速度计的偏差${{\boldsymbol{b}}_g}$ 和${{\boldsymbol{b}}_a}$ 。采用图优化方式对上述传感器进行紧耦合优化, 变量作为图优化节点, 3种传感器的观测作为图优化的边, 建立图优化模型并进行增量方程求解, 整体流程如图2所示。
由于融合定位模型所引入的深度传感器不依赖于外部基站, 仅通过水压即可得到高精度的深度信息, 在系统IMU漂移或者视觉跟踪失效的情况下, 仍然能够保证稳定的机器人位姿观测。故使用深度误差
${\boldsymbol{e_u}}$ 对视觉重投影观测误差${{\boldsymbol{e}}_r}$ 和IMU观测误差${{\boldsymbol{e}}_s}$ 进行异常值筛选, 有效降低由于IMU和视觉传感器失效带来的定位误差。然后建立图优化模型, 以多传感器的观测作为约束进行非线性优化, 联合计算机器人位姿。非线性优化为紧耦合方法, 优化模型协同权衡多传感器观测数据而不是单一传感器的组合, 可进一步提高融合定位结果的精度。1.2 基于视觉-惯性-深度测量的紧耦合非线性优化
依据上文阐述的多传感器融合框架和融合定位精度提高机理, 给出具体的紧耦合优化模型。首先构造如下损失函数
${\boldsymbol{J}}({{x}})$ , 作为紧耦合优化模型的目标函数$$ \begin{align} {\boldsymbol{J}}({{x}}) =& \sum\limits_{i = 1}^2 {\sum\limits_{k = 1}^K {\sum\limits_{j \in \mathcal{J}(i, k)} {{\left({\boldsymbol{e}}_r^{i, j,k} \right)}^{\text{T}}} } } {\boldsymbol{P}}_r^k{\boldsymbol{e}}_r^{i, j, k} + \\ &\sum\limits_{k = 1}^{K - 1} {\left({\boldsymbol{e}}_s^k\right)^{\text{T}}} {\boldsymbol{P}}_s^k{\boldsymbol{e}}_s^k + \sum\limits_{k = 1}^{K - 1} {\left({{\boldsymbol{e}}_u^k}\right)^{\text{T}}} {\boldsymbol{P}}_u^k{\boldsymbol{e}}_u^k \end{align} $$ (2) 式中: i为相机索引, 分别表示立体相机中的左侧相机(i=1)和右侧相机(i=2); j为特征点索引; k为视觉图像帧索引;
${\boldsymbol{P}}_r^k$ ,${\boldsymbol{P}}_s^k$ 和${\boldsymbol{P}}_u^k$ 分别为第k帧对应的视觉特征点、IMU和深度传感器的信息矩阵。为完整起见, 简要讨论每个误差项, 更多细节参考文献[9]和文献[10]。重投影误差描述了在相机坐标系C中的特征点与根据立体投影模型的相应特征点投影之间的差异。IMU误差项通过IMU预积分[10] 计算相邻相机帧间所有的加速度计和陀螺仪积分, 表示先前状态和当前状态预测之间的机器人姿态和速度误差。
文中引入的压力传感器基于水压可以提供准确的深度测量。深度值沿着与世界坐标系W的z轴对齐的重力方向提取, z轴方向已知是由于系统紧耦合了IMU, 可以提供z轴方向的绝对观测。第k帧对应的深度数据为
$$ {_W}\_{p_{{{\textit{z}}\_D}}^k} = {d^k} - {d^0} $$ (3) 有了深度测量
${\textit{z}}_u^k$ , 深度误差可以通过机器人沿着 z 轴方向的位置与深度数据之间的差异得到, 将误差项代入优化方程以纠正机器人的位置。误差项可以定义为$$ e_u^k\left( {{_W}\_p_{{{\textit{z}}\_I}}^k, \;{\textit{z}}_u^k} \right) = \left. {} \right| { {_W}\_}p_{{{\textit{z}}\_I}}^k - {}{_W}\_p_{{{\textit{z}}\_D}}^k\left. {} \right| $$ (4) 式中:
$ {_W}\_p_{{{\textit{z}}\_I}}^k $ 为机器人z轴位置, 是机器人待优化变量;$ {}{_W}\_p_{{{\textit{z}}\_D}}^k $ 为深度传感器的观测量。这2个量建立起水下机器人z轴方向位置变量约束。因深度传感器在水下具有极强的鲁棒性, 可认为该约束绝对准确。通过深度约束对其余2个传感器进行异常值筛选, 当IMU或视觉在z轴方向误差与深度误差的差值超过给定阈值, 认为数据失效并剔除失效的误差项。通过上述融合定位方法和鲁棒筛选机制, 可提高机器人在水下多传感器融合定位的精度。将所有误差项都添加到 Ceres Solver 非线性优化框架中[7], 构建式(2)紧耦合非线性优化模型, 估计式(1)机器人状态。
2. 定位模型初始化与闭环优化
2.1 融合定位系统初始化
紧耦合非线性融合定位系统需要稳健而准确的初始化, 以建立机器人初始坐标, 为后续定位提供参考。现有水下多传感器融合定位初始化多依赖于相机和IMU。初始化过程中, 视觉容易被遮挡且特征稀少; IMU适用于高速但初始化通常要求机器人在较低速度下运动的情况, 这导致基于视觉和IMU的融合定位系统初始化误差较大, 成功率较低, 极大地限制了融合定位方法在水下的普及。根据对视觉惯性状态估计系统的研究可知[8], 在水下多传感器融合定位过程中, 大多数最先进的系统存在无法初始化或进行了错误的初始化导致最终定位发散的问题。因此, 文中提出一种基于立体相机、IMU和深度传感器的水下多传感器稳健初始化方法。
如图3所示, 由于初始化过程时间短, 采集的多传感器数据少, 不能直接删除错误的视觉和IMU测量, 故基于深度传感器测量引入权重因子, 以修正视觉和IMU观测。
深度传感器提供准确的深度测量, 用于修正初始化参数, 设修正因子为
${s_1}$ , 相机C和深度传感器D之间的转换可以表示为$$ {_W}\_{\boldsymbol{p}}_{\textit{z}\_D}=s_{1{{W}\_}}{\boldsymbol{p}}_{\textit{z}\_C}+{_W}\_\boldsymbol{R}_{\textit{z}C\cdot C}\boldsymbol{p}_D $$ (5) 对于视觉关键帧k, 机器人初始位置
${_W}\_{{\boldsymbol{p}}_{r1C}}$ 的第1次修正为$$ {{_W}\_}{{\boldsymbol{p}}_{r1C}} = {s_1}{{_W}\_}{{\boldsymbol{p}}_C} $$ (6) 引入
${s_2}$ 进一步修正机器人位姿为$$ {_W}\_{{\boldsymbol{p}}_I} = {s_2} {}{_W}\_{{\boldsymbol{p}}_{r1C}}{ + {_W}\_}{{\boldsymbol{R}}_{C }}_{C}\_{{\boldsymbol{p}}_I} $$ (7) 将式(7)得到的相机位姿估计与 IMU 预积分值进行对齐。建立相机C和IMU之间的转换, 引入修正因子
${s_2}$ , 对IMU信息进行修正。除上述引入修正因子之外, 采用近似初始速度和重力矢量[10], 通过IMU预积分得到测量的机器人初始化状态为
$$\left\{ {\begin{array}{*{20}{l}} {{_W}\_\hat {\boldsymbol{p}}_I^{i + 1}{ = {_W}\_}{\boldsymbol{p}}_I^i{ + {_W}\_}{\boldsymbol{v}}_I^i\Delta {t_i} + {{\dfrac{1}{2}}{_W}\_}{\boldsymbol{g}}\Delta t_{i}^2{_W}\_{\boldsymbol{R}}_I^i{\boldsymbol{\alpha }}_{{I\_i}}^{i + 1}}\\ {{_W}\_\hat {\boldsymbol{v}}_I^{i + 1}{ = {_W}\_}{\boldsymbol{v}}_{IW}^i{_W}\_{\boldsymbol{g}}\Delta {t_i}{ + {_W}\_}{\boldsymbol{R}}_I^i{\boldsymbol{\beta }}_{{I\_i}}^{i + 1}}\\ {{_W}\_\hat {\boldsymbol{q}}_I^{i + 1} = \gamma _{{I\_i}}^{i + 1}} \end{array}} \right. $$ (8) 式中,
${\boldsymbol{ \alpha }}_{{I\_i}}^{i + 1} $ ,${\boldsymbol{ \beta}} _{{I\_i}}^{i + 1} $ ,$ {\boldsymbol{\gamma }}_{{I\_i}}^{i + 1} $ 为定义在2个关键帧之间, 时间$ {{\Delta }}{t_i} $ 范围内的IMU 预积分结果, 其值仅取决于IMU的测量值, 简化上式为相对于${\boldsymbol{ \alpha}} _{{I\_i}}^{i + 1} $ ,${\boldsymbol{ \beta}} _{{I\_i}}^{i + 1} $ 的形式为$$ \left\{ \begin{gathered} {\boldsymbol{\alpha}} _{{I\_i}}^{i + 1}{ = _I}\_{\boldsymbol{R}}_W^i\left( {{_W}\_\hat {\boldsymbol{p}}_I^{i + 1}{ - {_W}\_}{\boldsymbol{p}}_I^i{ - {_W}\_}{\boldsymbol{v}}_I^i{{\Delta }}{t_i} - \frac{1}{2}{}{_W}\_{{\boldsymbol{g}}^2}} \right) \\ {\boldsymbol{\beta}} _{{I\_i}}^{i + 1}{ = _I}\_{\boldsymbol{R}}_W^i\left( {{_W}\_\hat {\boldsymbol{v}}_I^{i + 1}{ - {_W}\_}{\boldsymbol{v}}_I^i - {}{_W}\_{\boldsymbol{g}}{{\Delta }}{t_i}} \right) \\ \end{gathered} \right. $$ (9) 建立最小二乘优化目标为
$$ \underset{{\chi }_{S}}{\mathrm{min}}\sum _{i\in K}{\hat{{\textit{z}}}}_{{S}_{i}}^{i+1}-{{\boldsymbol{H}}}_{{S}_{i}}^{i+1}{\chi }_{S}{}^{2} $$ (10) 式中:
$\hat {\textit{z}}_{{S\_i}}^{i + 1} = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol{\alpha}} _{{I\_i}}^{i + 1}{ - _I}\_{\boldsymbol{R}}{{_{W }^i}{_W}\_}{\boldsymbol{R}}{{_C^{i + 1}}_{{C}\_}}{\boldsymbol{p}}_I^{i + 1}{ +_{{I}\_}}{\boldsymbol{R}}_{C {{C}\_}}^i{\boldsymbol{p}}_I^i} \\ {{\boldsymbol{\beta}} _{{I\_i}}^{i + 1}} \end{array}} \right]$ ;$ H_{{S\_i}}^{i + 1} = $ ${\left[ {\begin{array}{*{20}{c}} { - {\boldsymbol{I}}\Delta {t_i}} & {\text{0}} & { - {{\dfrac{1}{2}}_{}}{}{{_I}\_}{\boldsymbol{R}}_W^i\Delta {t_i}^2} & {{{_I}\_}{\boldsymbol{R}}_W^i \left( {{_W}\_{{\boldsymbol{p}}_{r1C}^{i + 1}}{ - {_W}\_}{{\boldsymbol{p}}_{r1C}^i}} \right)} \\ { - {\boldsymbol{I}}} & {{{_I}\_}{\boldsymbol{R}}_{W }^i{ {_W}\_}{\boldsymbol{R}}_I^{i + 1}} & {{ - {{_I}\_}}{\boldsymbol{R}} _W^i\Delta {t_i}} & {\text{0}} \end{array}} \right]} $ 。根据优化变量g建立机器人定位参考坐标系, 将初始化速度v作为紧耦合定位初值, 便于系统快速收敛。
将式(8)代入式(10)即可通过对比估计值与预测值, 求解最小二乘问题来估计系统初始化的关键未知量
${\chi _S} = \left[ {{\boldsymbol{v}}_I^i, {\boldsymbol{v}}_I^{i + 1}{, {_W}\_}{\boldsymbol{g}}} \right]$ 。2.2 回环检测和重定位
对融合定位算法在测试中出现的累积误差问题进行分析, 可知在图优化过程中, 为了控制计算规模, 采用了基于滑动窗口的局部优化方法, 导致姿态估计上的漂移随时间累积。为了消除这种漂移并实现全局一致性, 采用全局优化和重定位方法。文中采用了一种基于二进制词袋, 可对机器人在水下运动中重复场景进行识别的视觉词袋法[11]。若检测到重复场景, 说明机器人曾经过该地点, 则当前姿态与词袋数据库中保存的先前位姿理论上一致, 基于此在式(2)的基础上加入新的重复场景约束, 称为回环检测[12]。
检测到环路后, 全局重定位模块通过将当前姿态相较于先前姿态的漂移返回到视觉-惯性-深度优化线程, 计算回环检测引入的重投影误差
$$ J'({\boldsymbol{x}}) = \mathop \sum \limits_{i = 1}^2 \mathop \sum \limits_{k = 1}^K \mathop \sum \limits_{j \in {\mathrm{Loop}}(i, k)} \left({\boldsymbol{e}}_r^{i, j,k}\right)^{\text{T}} {\boldsymbol{P}}_r^k{\boldsymbol{e}}_r^{i, j, k} $$ (11) 在环路检测完成后, 进行6自由度(位置
${_X}\_{{\boldsymbol{p}}_{Y}}$ 和旋转${_X}\_{{\boldsymbol{q}}_{Y}}$ )姿态图优化, 以消除累积误差。机器人当前位姿${{\boldsymbol{T}}_i}$ 相对于回环检测位姿${{\boldsymbol{T}}_j}$ 的相对变换$\Delta {{\boldsymbol{T}}_i}_j = {{\boldsymbol{T}}_j}{{\boldsymbol{T}}_i}^{ - 1}$ 。关键帧 i 和 j 之间的误差项${\boldsymbol{e}}_{{{\boldsymbol{x}}\_p}, {{\boldsymbol{x}}\_q}}^{i, j}$ 在切空间[13]中以最小形式表示为$$ {\boldsymbol{e}}_{{{\boldsymbol{x}}\_p}, {{\boldsymbol{x}}\_q}}^{i, j} = {{\Delta }}{{\boldsymbol{T}}_{ij}}{\hat {\boldsymbol{T}}_i}\hat {\boldsymbol{T}}_j^{ - 1} $$ (12) 式中: (
${\hat {\;}}$ )表示从局部视觉-惯性-深度优化中获得的估计值。优化式(2)得到要最小化的成本函数为
$$ \begin{align} J\left({{\boldsymbol{x}}}_{p}, {{\boldsymbol{x}}}_{q}\right)=&\displaystyle \sum _{i, j}{{\boldsymbol{e}}}_{{{\boldsymbol{x}}}\_{p}, {{\boldsymbol{x}}}\_{q}}^{i, j}{}^{\text{T}}{{\boldsymbol{P}}}_{{{\boldsymbol{x}}}\_{p}, {{\boldsymbol{x}}}\_{q}}^{i, j}{{\boldsymbol{e}}}_{{{\boldsymbol{x}}}\_{p}, {{\boldsymbol{x}}}\_{q}}^{i, j}+\\ &\displaystyle \sum _{(i, j)\in Loop}\rho \left({{\boldsymbol{e}}}_{{{\boldsymbol{x}}}\_{p}, {{\boldsymbol{x}}}\_{q}}^{i, j}{}^{\text{T}}{{\boldsymbol{P}}}_{{{\boldsymbol{x}}}\_{p}, {{\boldsymbol{x}}}\_{q}}^{i, j}{{\boldsymbol{e}}}_{{{\boldsymbol{x}}}\_{p}, {{\boldsymbol{x}}}\_{q}}^{i, j}\right) \end{align} $$ (13) 式中:
$ {\boldsymbol{P}}_{{{\boldsymbol{x}}\_p}, {{\boldsymbol{x}}\_q}}^{i, j} $ 是设置为单位矩阵的信息矩阵[14];$ \rho $ 为Huber损失函数[15], 可以有效降低错误环路权重, 避免影响整个系统优化效果。完成系统初始化和回环检测后, 搭建最终视觉-惯性-压力融合定位算法如图4所示。3. 试验验证与分析
首先对文中提出的多传感器融合状态估计系统开展基于水下数据集的复杂场景试验, 与常见的IMU定位和视觉-惯性融合定位进行对比, 以验证算法的有效性。然后, 在高精度试验环境下进行定量验证, 测试算法精度并分析精度提高原因。
3.1 复杂水下环境测试
文中提出的传感器融合状态估计系统面向水下环境, 在试验部分展示了来自2个不同水下环境数据集的结果。其一是位于水下100 m的考古遗址, 数据由潜水员使用定制的水下传感器套件收集[16], 图像受到水雾和低能见度的影响。另一水下环境来自于海底沉船, 属于典型密闭非结构化环境, 基于外部基站的定位方法(如USBL)在此种环境下无法工作。
试验采用多传感器数据融合进行协同定位, 立体摄像头以30帧/s捕获图像, IMU以200 Hz进行数据采集, 深度传感器型号为MS5837。传感器参数如表1所示。
表 1 传感器参数表Table 1. Sensor parameters相机 型号 ZED2 分辨率 3 840×1 080 帧率/(帧/s) 30 视场角/(°) 94.5 IMU 型号 MEMS-MPU-9250 陀螺仪频率/Hz 200 加速度计频率/Hz 200 磁力计频率/Hz 200 实验通过对比文中所提多传感器融合定位方法、单目视觉惯性导航系统(monocular visual-inertial system, VINS-Mono)[17]和开源关键帧视觉惯性同步定位与地图构建(simultaneous localization and mapping, SLAM)方法和OKVIS(open keyframe-based visual-inertial SLAM)方法, 定性评估算法性能。图5和图6显示了文中多传感器融合定位方法、VINS-Mono和OKVIS在上述数据集中的轨迹。
图 5 考古遗址中不同定位方法定位精度对比Figure 5. Comparison of positioning accuracy among different positioning methods in archaeological sites
图 6 水下沉船环境下不同定位方法定位精度对比Figure 6. Comparison of positioning accuracy among different positioning methods in underwater shipwreck environments分析可知, 基于惯性器件IMU的定位在2组试验中漂移很大, 完全无法定位; 视觉-惯性融合定位方法在水下沉船环境表现稳定, 定位效果较好, 但在水下考古遗址由于缺乏优质视觉特征, 视觉定位误差增大, 定位失效; 文中方法通过深度传感器对错误数据进行筛选, 在2种环境下均有稳定变现, 验证了算法的有效性。
3.2 试验环境测试
前文基于水下数据集试验完成了算法的定性分析, 验证了算法相对于现有定位算法的优势。下文中将针对算法提升机理和具体定位精度, 进行试验环境下的定量分析。
图7展示了融合定位试验系统及5 m×4 m×1.5 m的试验水池。试验系统包括水下机器人及其上位机操作系统。水下机器人嵌入式处理单元为NVIDIA TX2, 机器人搭载相机(ZED2)、IMU(MEMS-MPU-9250)和深度传感器(MS5837), 具体参数同表2。安装于机器人顶部的超宽带(ultra wide band, UWB)定位标签(NoopLoop: LinkTrack)和分布于试验水池四角的UWB定位基站提供机器人的定位基准, 试验过程中, 为避免UWB定位标签受水下信号衰减影响精度下降, 机器人搭载的UWB定位标签试验全程处于水面以上。
表 2 融合算法精度测试结果Table 2. Results of fusion algorithm accuracy test运动形式 最大误差/m 直航 0.064 转向 0.028 快速机动 0.127 试验所测试多传感器融合定位算法运行于NVIDIA TX2上, 基于前述理论进行多传感器融合, 以30 Hz每秒输出机器人融合定位结果。UWB移动终端通过与UWB基站的拓扑定位, 引入GN-ULS 和GN-SDP 估计器对定位结果进一步优化[18], 试验环境无电磁干扰, 机器人以0.2 m/s速度行驶, 定位精度达0.02 m, 以提供机器人的高精度定位。通过串口读取UWB高精度定位结果, UWB输出频率为100 Hz, 作为真值参与算法验证。基于NVIDIA TX2时钟对UWB测量数据进行插值, 将UWB定位数据与融合定位结果进行时间同步。同步后的数据导入evo工具进行定量分析, 测试融合定位算法的精度和鲁棒性。
相较于现有融合定位方法, 文中定位方法主要改进点为引入深度传感器对多传感器数据进行筛选, 提升数据质量。同时引入了重定位与回环检测以消除长时间定位过程中的累积误差。文中针对由此带来的精度提升进行了验证, 结果如图8和图9所示。
图 8 无重定位和回环检测模块下定位精度Figure 8. Positioning accuracy without relocation and loopback detection module图8中, 在试验环境下操纵水下机器人进行复杂运动, 记录机器人多传感器融合定位信息。机器人分别运行无重定位与回环检测模块的定位算法和完整的定位算法。对比图8(a)与图9(a)可知, 无重定位与回环检测模块的定位算法随着时间的增加, 定位误差显著增加, 最终导致定位失败, 而完整系统全程保持较好的定位精度。进一步对比图8(b)与图9(b), 无重定位与回环检测系统最大定位误差8.084 m, 完整系统最大定位误差0.016 m, 进一步证明引入重定位与回环检测方法可显著增加系统定位精度。
图10中, 不加深度传感器, 仅采用视觉-惯性进行融合定位, 分析定位效果可知, 在没有深度传感器进行误差辨识和筛选的情况下, 融合定位系统精度显著下降, 最大定位误差可达0.031 m。由图11可以看出, 完整定位系统最大定位误差为0.016 m, 验证了通过引入深度传感器, 可提高融合系统定位精度, 相较于视觉-惯性融合定位系统精度提升48.4%。
为进一步验证融合定位系统在不同工况下的定位精度, 试验对水下机器人直航、转向和快速机动等3种运动模式进行测试。由图11可知, 直航运动试验结果定位误差最大为0.040 m, 转向运动定位结果最大误差0.028 m, 快速机动运动最大定位误差0.038 m, 各常用工况定位精度均稳定在厘米级。
通过试验环境下定量分析, 得到融合定位算法的精度值如表2所示, 各运动模式下最大误差不超过0.040 m, 具有较好的定位精度。
4. 结束语
文中针对现有视觉-IMU融合定位算法鲁棒性差, 有累积误差的问题, 基于深度信息对视觉惯性数据进行误差辨识以提升融合数据质量; 采用深度传感器进行权重分配以提供更加细致的系统初始化; 同时引入闭环检测和重定位方法, 有效改善漂移和定位丢失的问题。文中算法在高精度试验环境和不同水下数据集上的试验结果都表现出了优异的性能, 测试精度可以达到厘米级。可为复杂非结构化环境下机器人低成本自主定位提供参考。
-
图 5 考古遗址中不同定位方法定位精度对比
Figure 5. Comparison of positioning accuracy among different positioning methods in archaeological sites
图 6 水下沉船环境下不同定位方法定位精度对比
Figure 6. Comparison of positioning accuracy among different positioning methods in underwater shipwreck environments
图 8 无重定位和回环检测模块下定位精度
Figure 8. Positioning accuracy without relocation and loopback detection module
表 1 传感器参数表
Table 1. Sensor parameters
相机 型号 ZED2 分辨率 3 840×1 080 帧率/(帧/s) 30 视场角/(°) 94.5 IMU 型号 MEMS-MPU-9250 陀螺仪频率/Hz 200 加速度计频率/Hz 200 磁力计频率/Hz 200 
下载: 导出CSV
表 2 融合算法精度测试结果
Table 2. Results of fusion algorithm accuracy test
运动形式 最大误差/m 直航 0.064 转向 0.028 快速机动 0.127
下载: 导出CSV
-
[1] WANG Y, MA X, WANG J, et al. Pseudo-3D vision-inertia based underwater self-localization for AUVs[J]. IEEE Transactions on Vehicular Technology, 2020, 69(7): 7895-7907. doi: 10.1109/TVT.2020.2993715 [2] SMITHANIK J R, ATKINS E M, SANNER R M. Visual positioning system for an underwater space simulation environment[J]. Journal of Guidance, Control, and Dynamics, 2016, 29(4): 858-869. [3] CHENG C, WANG C, YANG D, et al. Underwater localization and mapping based on multi-beam forward looking sonar[J]. Frontiers in Neurorobotics, 2022, 15: 801956. doi: 10.3389/fnbot.2021.801956 [4] SHEN Y, ZHAO C, LIU Y, et al. Underwater optical imaging: Key technologies and applications review[J]. IEEE Access, 2021, 9: 85500-85514. doi: 10.1109/ACCESS.2021.3086820 [5] RAVEENDRAN S, PATIL M D, BIRAJDAR G K. Underwater image enhancement: A comprehensive review, recent trends, challenges and applications[J]. Artificial Intelligence Review, 2021, 54: 5413-5467. doi: 10.1007/s10462-021-10025-z [6] LI C, GUO C, REN W, et al. An underwater image enhancement benchmark dataset and beyond[J]. IEEE Transactions on Image Processing, 2019, 29: 4376-4389. [7] SHAUKAT N, LI A Q, REKLEITIS I. Svin2: An underwater slam system using sonar, visual, inertial, and depth sensor[C]//2019 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS). Macau: IEEE, 2019: 1861-1868. [8] WANG X, FAN X, SHI P, et al. An overview of key SLAM technologies for underwater scenes[J]. Remote Sensing, 2023, 15(10): 2496. doi: 10.3390/rs15102496 [9] HU K, WANG T, SHEN C, et al. Overview of underwater 3D reconstruction technology based on optical images[J]. Journal of Marine Science and Engineering, 2023, 11(5): 949. doi: 10.3390/jmse11050949 [10] LEUTENEGGER S, FURGALE P, RABAUD V, et al. Keyframe-based visual-inertial slam using nonlinear optimization[J]. Proceedings of Robotis Science and Systems, 2023, 51: 5213-5667. [11] QIN T, LI P, SHEN S. VINS-MOMO: A robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020. doi: 10.1109/TRO.2018.2853729 [12] MOURIKIS A I, ROUMELIOTIS S I. A multi-state constraint Kalman filter for vision-aided inertial navigation[C]//Proceedings 2007 IEEE International Conference on Robotics and Automation. Roma, Italy: IEEE, 2017. [13] SIBLEY G, MATTHIES L, SUKHATME G. Sliding window filter with application to planetary landing[J]. J. Field Robot, 2019, 27(5): 587-608. [14] 王霞, 左一凡. 视觉SLAM研究进展[J]. 智能系统学报, 2020, 15(5): 825-834. doi: 10.11992/tis.202004023WANG X, ZUO Y F. Advances in visual SLAM research[J]. CAAI Transactions on Intelligent Systems, 2020, 15(5): 825-834. doi: 10.11992/tis.202004023 [15] MEI C, RIVES P. Single view point omnidirectional camera calibration from planar grids[C]//Proceedings 2007 IEEE International Conference on Robotics and Automation. Roma, Italy: IEEE, 2007. [16] FERRERA M, CREUZE V, MORAS J, et al. AQUALOC: An underwater dataset for visual-inertial-pressure localization[J]. The International Journal of Robotics Research, 2019, 38(14): 1549-1559. doi: 10.1177/0278364919883346 [17] LIN Y, GAO F, QIN T, et al. Autonomous aerial navigation using monocular visual inertial fusion[J]. Journal of Field Robot, 2017, 35: 23-51. [18] JIANG H, WANG W, SHEN Y, et al. Efficient planar pose estimation via UWB measurements[C]//2023 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2023: 1954-1960. -
下载:
点击查看大图
计量
- 文章访问数: 155
- HTML全文浏览量: 54
- PDF下载量: 41
- 被引次数: 0
