0 引　言

自主水下机器人^[1]（Autonomous Underwater Vehicle，AUV）为集传感、通信、导航、控制、推进等技术于一体的常用海洋观测探测设备，已经广泛应用于深海、极地的科考任务。在北极地区执行任务的AUV受到海面冰层的阻隔，常用的卫星和声学导航等方法受到限制。在航行过程中惯导和DVL^[2]逐渐累积误差，不仅会导致AUV偏离任务航线，还有可能致使AUV未能到达目标冰洞进行回收。视觉定位在水质良好的极地具有良好性能，基于自然特征的视觉里程计^[3]存在累积误差不宜使用，基于人工标记的视觉定位系统可以计算AUV绝对位置用于校正导航。与一般场景不同的是，本文仅在AUV行进路线上间隔部署标记，当AUV获取到标记定位特征成像位置时，通过位姿解算校正导航，确保AUV航线准确并最终在小尺寸冰洞中安全回收。然而，冰下标记的成像亮度容易出现波动，致使标记特征的成像位置存在误差，以及在AUV偏离航线航线时标记可能出现在视野边缘，都对位姿解算方法提出挑战。

目前，对于视觉定位标记位姿解算的研究主要分为2种，一种是针对特定视觉定位标记提出的标记位姿解算方法；另一种是针对PnP问题提出的通用相机位姿解算方法。PAN G等^[4]设计了一种六定位特征的二维码，构建标记位姿和图像中6个特征点之间的线性方程，实现标记与相机相对位姿的直接解算；M. F. ASWAD等^[5]对构建的线性方程组进行参数简化，使用标记4个特征点即可解算位姿；M. UZUNOGLU等^[6]使用SVD分解求解构建的线性方程组，获取最小二乘解；OLSON E等^[7]设计了AprilTag标记，在求解线性方程后对获得的旋转矩阵进行极分解，提高标记姿态解算的精度。由于解算标记位姿是一个典型的PnP问题，因此可以使用PnP问题通用求解方法。KE T等^[8]设计了一种基于罗德里格斯变换关联特征点求解P3P问题的方法，使用牛顿法求解4次方程组，完成相机的位姿解算；S. ZHUANG等^[9]将特征点理论成像位置和实际位置之间的距离作为优化目标，使用L-BFGS算法迭代计算相机位姿；M. LOURAKIS等^[10]将特征点理论空间位置与成像投影线之间的距离作为优化目标，使用Grobner算法解算相机位姿；K. RIKU等^[11]设计了一种稀疏搜索麻雀算法迭代计算相机位姿；S. GARRIDO-JURADO等^[12]设计了Aruco标记，使用COLLINST等^[13]基于无穷小平面的相机位姿评估方法（IPPE），完成Aruco标记的位姿解算。目前，Aruco标记已被应用于多个视觉定位工程项目，然而对于校正冰下AUV导航这一应用场景，位姿解算方法需要对2个方面的问题进行改进。一方面，水下标记成像亮度波动造成标记角点存在定位误差，位姿解算需要整合多帧图像中标记的位置信息，实现更高精度的位姿解算。另一方面，AUV对Aruco标记的识别和位姿解算存在滞后，解算过程中载体持续运动，因此需要对滞后的结果进行修正。

本文针对冰下AUV导航校正的应用场景，提出了通过优化AUV视野中Aruco标记角点的成像误差，校正冰下AUV导航位置的方法CNOEM（The method to correct the navigation position of under-ice AUV by optimizing the imaging error of the Aruco marker corners recognized by the AUV camera）。首先，本文将校正周期开始时的AUV坐标系记为起始坐标系，在一个周期内基于多帧图像对起始坐标系在世界坐标系中的位姿进行优化，在周期结束时基于优化后的位姿校正导航。其次，本文设计了一种基于起始坐标系位姿，计算标记在各相机坐标系中理论位置的方法，结合标记特征点的实际成像位置，可以计算成像误差。之后，本文设计了从标记成像误差到起始坐标系位姿误差的传播方法，通过误差反向传播，可以迭代优化起始坐标系的位姿。最后，本文方法针对GPU平台进行并行优化，有效提高方法的计算效率。在仿真、水池和外场试验中通过与常用方法对比，证明方法的有效性。

1 CNOEM方法设计 1.1 方法原理

图1所示为相机成像的原理，将世界坐标系记为$ {\text{W}} $，相机坐标系记为$ {\text{O}} $，为便于表述，记AUV坐标系为$ V $与$ {\text{O}} $重合，水下标记的定位特征$ {{{P}}_i}\left( {i = {1 {\sim} 4}} \right) $在相机成像平面的成像用$ {{{p}}_i}\left( {i = {1{\sim}4}} \right) $表示。$ {{P}} $在坐标系中的3个位置分量分别记为$ X $，$ Y $和$ Z $，$ {{p}} $的3个分量记为$ x $、$ y $和$ z $。标记每组$ {{{P}}_i} $，$ {{{p}}_i} $与$ {\text{O}} $的原点共线。基于该原理得出相机的成像式(1)。

$ Z_i^{\text{O}}{\left[ {\begin{array}{*{20}{c}} {{u_i}}&{{v_i}}&1 \end{array}} \right]^{\text{T}}} = \boldsymbol {KT}_{\text{W}}^{\text{O}}\left[ {\begin{array}{*{20}{c}} {P_i^{\text{W}}} \\ 1 \end{array}} \right]。$

(1)

式中：$ Z_i^{\text{O}} $为特征点$ {{{P}}_i} $在$ {\text{O}} $中的Z轴坐标；$ {u_i} $和$ {v_i} $为$ {{{p}}_i} $在成像平面上的像素坐标；上标$ {\text{T}} $表示矩阵转置；$ \boldsymbol K $为相机的内参矩阵。

$ \boldsymbol K = \left[ {\begin{array}{*{20}{c}} {{k_1}}&0&{{u_0}}&0 \\ 0&{{k_2}}&{{v_0}}&0 \\ 0&0&1&0 \end{array}} \right]。$

(2)

式中：$ {k_1} $、$ {k_2} $、$ {u_0} $和$ {v_0} $为相机的内参；$ P_i^{\text{W}} $为特征点在$ {\text{W}} $中的位置；$ {\boldsymbol{T}}_{\text{W}}^{\text{O}} $为世界坐标系向相机坐标系的变换矩阵，由$ {\boldsymbol{T}}_{\text{V}}^{\text{W}} $求逆获得。向量$ n_{\text{V}}^{\text{W}} $为AUV坐标系向世界坐标系变换的六维向量，通过导航直接读取。

$ n_{\text{V}}^{\text{W}} = \left[ {\begin{array}{*{20}{c}} \alpha &\beta &\gamma &X&Y&Z \end{array}} \right]。$

(3)

式中：$ \alpha $、$ \beta $和$ \gamma $分别为绕$ {{X}} $、$ {{Y}} $和$ {{Z}} $轴旋转的欧拉角度，旋转顺序为$ {{Z}} - {{X}} - {{Y}} $、$ X $、$ Y $和$ Z $为平移量。$ {\boldsymbol{T}}_{\text{V}}^{\text{W}} $见式(4)：

$ {{\boldsymbol{T}}_{\text{V}}^{\text{W}} = {\text{J}}\left( {n_{\text{V}}^{\text{W}}} \right) = \left[ {\begin{array}{*{20}{c}} {{\rm{c}}\gamma *{\rm{c}}\beta - {\rm{s}}\gamma *{\rm{s}}\alpha *{\rm{s}}\beta }&{ - {\rm{s}}\gamma *{\rm{c}}\alpha }&{{\rm{c}}\gamma *{\rm{s}}\beta + {\rm{s}}\gamma *{\rm{s}}\alpha *{\rm{c}}\beta }&X \\ {{\rm{s}}\gamma *{\rm{c}}\beta + {\rm{c}}\gamma *{\rm{s}}\alpha *{\rm{s}}\beta }&{{\rm{c}}\gamma *{\rm{c}}\alpha }&{{\rm{s}}\gamma *{\rm{s}}\beta - {\rm{c}}\gamma *{\rm{s}}\alpha *{\rm{c}}\beta }&Y \\ { - {\rm{c}}\alpha *{\rm{s}}\beta }&{{\rm{s}}\alpha }&{{\rm{c}}\alpha *{\rm{c}}\beta }&Z \\ 0&0&0&1 \end{array}} \right]}{。} $

(4)

式中：$ * $为标量相乘；$ {\rm{s}} $为函数$ \sin $；$ {\rm{c}} $为函数$ \cos $。

当AUV导航$ n_{\text{V}}^{\text{W}} $不存在误差时，式(1)等式成立。当AUV导航出现误差后，特征共线关系不再成立如图2(a)所示，$ {\boldsymbol{T}}_{\text{W}}^{\text{O}} $存在误差，$ {{{P}}_i} $与$ {{{p}}_i} $以及$ {\text{O}} $原点不共线，点$ {{{P}}_i} $到$ {{{p}}_i} $与原点所在直线间的距离由$ {L_i} $表示，距离的垂足为$ {{{P}}_i}' $。记$ {E = \displaystyle\sum\nolimits_i {{{\left( {{L_i}} \right)}^2}} } $为标记特征点的成像误差。由于$ E $与$ {\boldsymbol{T}}_{\text{W}}^{\text{O}} $存在函数关系，以$ E = 0 $为目标对$ {\boldsymbol{T}}_{\text{W}}^{\text{O}} $进行优化，消除$ {\boldsymbol{T}}_{\text{W}}^{\text{O}} $的误差后得到图2(b)所示的结果。

将校正的$ {\boldsymbol{T}}_{\text{W}}^{\text{O}} $求逆获得AUV的准确位姿$ {\boldsymbol{T}}_{\text{V}}^{\text{W}} $，再计算准确的导航值$ n_{\text{V}}^{\text{W}} $，见式(5)，用求得的$ n_{\text{V}}^{\text{W}} $对导航进行更新。

$ n_{\text{V}}^{\text{W}} = {{\text{J}}^{ - 1}}\left( {{\boldsymbol{T}}_{\text{V}}^{\text{W}}} \right) = {\left[ {\begin{array}{*{20}{c}} {{\text{arcsin}}\left( {{\boldsymbol{T}}_{\text{V}}^{\text{W}}\left( {3,2} \right)} \right)} \\ {{\text{arctan}}\left( {{\boldsymbol{T}}_{\text{V}}^{\text{W}}\left( {3,1} \right),{\boldsymbol{T}}_{\text{V}}^{\text{W}}\left( {3,3} \right)} \right)} \\ {{\text{arctan}}\left( {{{ - }}{\boldsymbol{T}}_{\text{V}}^{\text{W}}\left( {1,2} \right),{\boldsymbol{T}}_{\text{V}}^{\text{W}}\left( {2,2} \right)} \right)} \\ {{\boldsymbol{T}}_{\text{V}}^{\text{W}}\left( {1,4} \right)} \\ {{\boldsymbol{T}}_{\text{V}}^{\text{W}}\left( {2,4} \right)} \\ {{\boldsymbol{T}}_{\text{V}}^{\text{W}}\left( {3,4} \right)} \end{array}} \right]^{\text{{{T}}}}}。$

(5)

式中：$ {\text{arcsin}} $为函数反正弦；$ {\text{arctan}} $为函数反正切；$ {\boldsymbol{T}}_{\text{V}}^{\text{W}}\left( {p,q} \right) $为索引矩阵$ {\boldsymbol{T}}_{\text{V}}^{\text{W}} $第$ p $行$ q $列的元素。

1.2 方法计算流程

在水下环境中，受光照影响标记的成像亮度存在波动，使得算法识别的标记角点位置存在误差。为减小该误差对位姿解算精度造成的影响，本文设计了基于多帧图像的AUV导航校正方法。同时，方法还有效解决了标记识别造成的解算结果滞后性问题。在本文的AUV系统中，将各特征点和坐标系做以下命名。将多帧图像中识别的全部标记按识别时间顺序编号$ j $，标记$ j $所在图像成像时，成像相机的坐标系记为$ {\text{O}}j $；AUV坐标系记为$ {\text{V}}j $；标记的第$ i $个特征点记为$ {{P}}_i^j $；在相机坐标系中的位置记为$ P_i^{{\text{O}}j} $；在世界坐标系中的位置记为$ P_i^{{\text{W}}j} $；特征点的成像点记为$ {\text{p}}_i^j $；其像素横纵坐标记为$ u_i^j $和$ v_i^j $；在相机坐标系中的坐标记为$ p_i^{{\text{O}}j} $；特征满足相机成像公式和特征共线关系。

$ Z_i^{{\text{O}}j}{\left[ {\begin{array}{*{20}{c}} {u_i^j}&{v_i^j}&1 \end{array}} \right]^{\text{{{T}}}}} = {{\boldsymbol{K}}^j}\left[ {\begin{array}{*{20}{c}} {P_i^{{\text{O}}j}} \\ 1 \end{array}} \right]，$

(6)

$ \begin{array}{*{20}{c}} {z_i^{{\text{O}}j} = {f^j}}&{X_i^{{\text{O}}j} = Z_i^{{\text{O}}j}\dfrac{{x_i^{{\text{O}}j}}}{{z_i^{{\text{O}}j}}}}&{Y_i^{{\text{O}}j} = Z_i^{{\text{O}}j}\dfrac{{y_i^{{\text{O}}j}}}{{z_i^{{\text{O}}j}}}} 。\end{array} $

(7)

式中：$ {{\boldsymbol{K}}^j} $和$ {f^j} $分别为第$ j $个标记对应相机的内参矩阵和焦距。将式(7)代入式(6)消元求得$ p_i^{{\text{O}}j} $的计算公式为：

$ p_i^{{\text{O}}j} = \left[ {\begin{array}{*{20}{c}} {x_i^{{\text{O}}j}} \\ {y_i^{{\text{O}}j}} \\ {z_i^{{\text{O}}j}} \end{array}} \right] = {\left[ {\begin{array}{*{20}{c}} {\dfrac{{u_i^j - u_0^j}}{{k_1^j}}{f_j}}&{\dfrac{{v_i^j - v_0^j}}{{k_2^j}}{f_j}}&{{f_j}} \end{array}} \right]^{\text{{{T}}}}}。$

(8)

起始坐标系记为$ {\text{F}} $，即校正周期开始时AUV的坐标系。末端坐标系记为$ {\text{G}} $，即校正周期结束时AUV的坐标系。

方法在一个校正周期内校正导航值$ n_{\text{G}}^{\text{W}} $的过程主要包括2个部分信息获取和校正计算这2个部分。周期开始时，创建$ \mathrm{marks} $用于暂存标记信息，$ j = 1 $用于记录标记识别次序。信息获取的第1步中，算法从惯导读取位姿信息$ n $作为$ n_{\text{F}}^{\text{W}} $并计算${{\boldsymbol{T}}_W^F = {{\left( {{\text{J}}\left( {n_F^W} \right)} \right)}^{ - 1}}} $和$ n_{\text{W}}^{\text{F}} = {{\text{J}}^{ - 1}}\left( {{\boldsymbol{T}}_{\text{W}}^{\text{F}}} \right) $。第2步，从相机读取一帧新图像$ img $的同时，读取新惯导位姿$ n $，识别$ \mathrm{img} $中的所有标记并将$ u_i^j $、$ v_i^j $，标记编号和相机编号暂存入$ \mathrm{marks} $。第3步，从$ \mathrm{marks} $中取出一个标记的信息为其赋予识别次序$ j $，根据标记编号和相机编号在数据库中读取$ P_i^{{\text{W}}j} $和$ {\boldsymbol{T}}_{{\text{O}}j}^{{\text{V}}j} $。第4步，将$ n $作为$ n_{{\text{V}}j}^{\text{W}} $计算$ {{\boldsymbol{T}}_{\text{F}}^{{\text{O}}j} = {{\left( {{\boldsymbol{T}}_{\text{W}}^{\text{F}}{\text{J}}\left( {n_{{\text{V}}j}^{\text{W}}} \right){\boldsymbol{T}}_{{\text{O}}j}^{{\text{V}}j}} \right)}^{ - 1}}}$并将$ j $的值加一。第5步，重复第3和4步直到$ \mathrm{marks} $中的信息被全部取出。最后，重复第2～5步直到AUV的移动距离达到阈值。校正计算的第1步，基于当前的$ n_{\text{W}}^{\text{F}} $值以及$ j $组$ P_i^{{\text{W}}j} $，$ {\boldsymbol{T}}_{\text{F}}^{{\text{O}}j} $，$ u_i^j $和$ v_i^j $分别计算$ {E^j} $。第2步，根据$ j $个成像误差$ {E^j} $向$ n_{\text{W}}^{\text{F}} $反向传播误差$ e{n^j} $，并对$ n_{\text{W}}^{\text{F}} $进行更新。第3步，重复第1步和第2步直到迭代次数达到阈值。第4步，读取新的$ n $作为$ n_{\text{G}}^{\text{W}} $，计算$ {{\boldsymbol{T}}_{\text{G}}^{\text{F}} = {{\left( {{\boldsymbol{T}}_{\text{F}}^{\text{W}}} \right)}^{ - 1}}{\text{J}}\left( {n_{\text{G}}^{\text{W}}} \right)}$。第5步，根据更新后的$ n_{\text{W}}^{\text{F}} $计算新的$ {n_{\text{G}}^{\text{W}} = {{\text{J}}^{ - {\text{1}}}}\left( {{{\left( {{\text{J}}\left( {n_{\text{W}}^{\text{F}}} \right)} \right)}^{ - 1}}{\boldsymbol{T}}_{\text{G}}^{\text{F}}} \right)} $，并对导航进行更新。

2 CNOEM标记成像误差计算与坐标参数更新 2.1 误差计算与参数更新流程

图3所示为误差计算过程中各参数的关系图。

$ eP_i^{{\text{O}}j} $、$ e{\boldsymbol{T}}_{\text{W}}^{{\text{O}}j} $、$ e{\boldsymbol{T}}_{\text{W}}^{{\text{F}}j} $和$ e{n^j} $分别为反向传播到变量$ P_i^{{\text{O}}j} $、$ {\boldsymbol{T}}_{\text{W}}^{{\text{O}}j} $、$ {\boldsymbol{T}}_{\text{W}}^{\text{F}} $和$ n_{\text{W}}^{\text{F}} $的误差。误差正向计算过程中，根据已知的$ n_{\text{W}}^{\text{F}} $、$ {\boldsymbol{T}}_{\text{F}}^{{\text{O}}j} $、$ P_i^{{\text{W}}j} $、$ u_i^j $和$ v_i^j $依次使用式(9)～式(12)计算$ {E^j} $。

$ {\boldsymbol{T}}_{\text{W}}^{\text{F}} = {\text{J}}\left( {n_{\text{W}}^{\text{F}}} \right)，$

(9)

$ {\boldsymbol{T}}_{\text{W}}^{{\text{O}}j} = {\boldsymbol{T}}_{\text{F}}^{{\text{O}}j}{\boldsymbol{T}}_{\text{W}}^{\text{F}}，$

(10)

$ P_i^{{\text{O}}j} = {\boldsymbol{T}}_{\text{W}}^{{\text{O}}j}P_i^{{\text{W}}j}，$

(11)

$ {E^j} = \sum\limits_i {{{\left( {L_i^j} \right)}^2}} = \sum\limits_i {\frac{{{{\left| {p_i^{{\text{O}}j} \times ( - P_i^{{\text{O}}j})} \right|}^2}}}{{{{\left| {p_i^{{\text{O}}j}} \right|}^2}}}}。$

(12)

式(12)中的$ “\times” $为向量叉乘，其展开形式为：

$ {{{E^j} = \displaystyle\sum\limits_i {\dfrac{{{{\left( {Y_i^{{\text{O}}j}z_i^{{\text{O}}j} - Z_i^{{\text{O}}j}y_i^{{\text{O}}j}} \right)}^2} + {{\left( {X_i^{{\text{O}}j}z_i^{{\text{O}}j} - Z_i^{{\text{O}}j}x_i^{{\text{O}}j}} \right)}^2} + {{\left( {X_i^{{\text{O}}j}y_i^{{\text{O}}j} - Y_i^{{\text{O}}j}x_i^{{\text{O}}j}} \right)}^2}}}{{{{\left( {x_i^{{\text{O}}j}} \right)}^2} + {{\left( {y_i^{{\text{O}}j}} \right)}^2} + {{\left( {z_i^{{\text{O}}j}} \right)}^2}}}}}}。$

(13)

该公式求解多组三维点到直线距离的平方和，将定位特征理论位置$ P_i^{{\text{O}}j} $到成像特征$ {{p}}_i^j $与$ {\text{O}}j $原点所在直线的垂直距离$ L_i^j $作为误差，对每个标记分别求特征误差距离的平方和$ {E^j} $作为成像误差。

误差反向传播过程中，首先对式(13)中的$ X_i^{{\text{O}}j} $，$ Y_i^{{\text{O}}j} $和$ Z_i^{{\text{O}}j} $分别求偏导，计算$ {E^j} $向特征点位置$ P_i^{{\text{O}}j} $的误差传播梯度，将$ {E^j} $与误差传播梯度相乘求得特征点位置误差$ eP_i^{{\text{O}}j} $，

$ {{eP_i^{{\text{O}}j} = \left[ {\begin{array}{*{20}{c}} {eX_i^{{\text{O}}j}} \\ {eY_i^{{\text{O}}j}} \\ {eZ_i^{{\text{O}}j}} \end{array}} \right] = {E^j}\left[ {\begin{array}{*{20}{c}} {2\dfrac{{z_i^{{\text{O}}j}\left( {X_i^{{\text{O}}j}z_i^{{\text{O}}j} - Z_i^{{\text{O}}j}x_i^{{\text{O}}j}} \right) + y_i^{{\text{O}}j}(X_i^{{\text{O}}j}y_i^{{\text{O}}j} - Y_i^{{\text{O}}j}x_i^{{\text{O}}j})}}{{{{\left( {x_i^{{\text{O}}j}} \right)}^2} + {{\left( {y_i^{{\text{O}}j}} \right)}^2} + {{\left( {z_i^{{\text{O}}j}} \right)}^2}}}} \\ {2\dfrac{{z_i^{{\text{O}}j}\left( {Y_i^{{\text{O}}j}z_i^{{\text{O}}j} - Z_i^{{\text{O}}j}y_i^{{\text{O}}j}} \right) - x_i^{{\text{O}}j}(X_i^{{\text{O}}j}y_i^{{\text{O}}j} - Y_i^{{\text{O}}j}x_i^{{\text{O}}j})}}{{{{\left( {x_i^{{\text{O}}j}} \right)}^2} + {{\left( {y_i^{{\text{O}}j}} \right)}^2} + {{\left( {z_i^{{\text{O}}j}} \right)}^2}}}} \\ {2\dfrac{{ - y_i^{{\text{O}}j}\left( {Y_i^{{\text{O}}j}z_i^{{\text{O}}j} - Z_i^{{\text{O}}j}y_i^{{\text{O}}j}} \right) - x_i^{{\text{O}}j}(X_i^{{\text{O}}j}z_i^{{\text{O}}j} - Z_i^{{\text{O}}j}x_i^{{\text{O}}j})}}{{{{\left( {x_i^{{\text{O}}j}} \right)}^2} + {{\left( {y_i^{{\text{O}}j}} \right)}^2} + {{\left( {z_i^{{\text{O}}j}} \right)}^2}}}} \end{array}} \right]}}。$

(14)

其次，对式(11)中$ {\boldsymbol{T}}_{\text{W}}^{{\text{O}}j} $的变量分别求偏导，计算$ P_i^{{\text{O}}j} $向$ {\boldsymbol{T}}_{\text{W}}^{{\text{O}}j} $的误差传播梯度，由于$ {\boldsymbol{T}}_{\text{W}}^{{\text{O}}j} $为坐标变换矩阵，其前3行4列是变量，所以误差传播梯度包含12个分量。将传播梯度的分量与对应$ eP_i^{{\text{O}}j} $分量相乘的结果可以整理成向量相乘的形式。由于标记$ j $各特征点的误差都向矩阵$ {\boldsymbol{T}}_{\text{W}}^{{\text{O}}j} $传播，因此再将向量乘积对$ i $求和即可求得$ e{\boldsymbol{T}}_{\text{W}}^{{\text{O}}j} $。

$ e{\boldsymbol{T}}_{\text{W}}^{{\text{O}}j} = \sum\limits_i {\left[ {\begin{array}{*{20}{c}} {eX_i^{{\text{O}}j}} \\ {eY_i^{{\text{O}}j}} \\ {eZ_i^{{\text{O}}j}} \end{array}} \right]{{\left[ {\begin{array}{*{20}{c}} {X_i^{{\text{W}}j}} \\ {Y_i^{{\text{W}}j}} \\ {Z_i^{{\text{W}}j}} \\ 1 \end{array}} \right]}^{\text{{{T}}}}}}。$

(15)

然后，对式(10)中$ {\boldsymbol{T}}_{\text{W}}^{\text{F}} $的变量分别求偏导，计算$ {\boldsymbol{T}}_{\text{W}}^{{\text{O}}j} $向$ {\boldsymbol{T}}_{\text{W}}^{\text{F}} $的误差传播梯度，将传播梯度的分量与对应$ e{\boldsymbol{T}}_{\text{W}}^{{\text{O}}j} $元素相乘的结果整理成矩阵相乘的形式，即可求得标记$ j $向$ {\boldsymbol{T}}_{\text{W}}^{\text{F}} $传播的误差$ e{\boldsymbol{T}}_{\text{W}}^{{\text{F}}j} $。

$ e{\boldsymbol{T}}_{\text{W}}^{{\text{F}}j} = {\left( {{\boldsymbol{T}}_{{\text{O}}j}^{\text{F}}} \right)^{\text{s}}}{\text{ }}e{\boldsymbol{T}}_{\text{W}}^{{\text{O}}j}。$

(16)

式中：上标$ {\text{s}} $为取矩阵的前3行前3列。

随后，对式(9)中$ n_{\text{W}}^{\text{F}} $的欧拉角$ \alpha $、$ \beta $和$ \gamma $分别求偏导，计算$ {\boldsymbol{T}}_{\text{W}}^{\text{F}} $向$ \alpha $、$ \beta $和$ \gamma $的误差传播梯度，将其与对应的$ e{\boldsymbol{T}}_{\text{W}}^{{\text{F}}j} $的分量相乘，将结果整理成矩阵对位相乘再矩阵求和的形式，即可求得标记$ j $向欧拉角传播的误差$ e{\alpha ^j} $、$ e{\beta ^j} $和$ e{\gamma ^j} $。

$\left\{ \begin{array}{l}e{\alpha }^{j}=\text{sum}\left(e{{\boldsymbol{T}}}_{\text{W}}^{\text{F}j}\bigodot\mathop {{\boldsymbol{T}}_{\text{W}}^{\text{F}}}\limits^ \bullet\left(\alpha \right)\right)，\\ e{\beta }^{j}=\text{sum}\left(e{{\boldsymbol{T}}}_{\text{W}}^{\text{F}j}\bigodot\mathop {{\boldsymbol{T}}_{\text{W}}^{\text{F}}}\limits^ \bullet\left(\beta \right)\right)，\\ e{\gamma }^{j}=\text{sum}\left(e{{\boldsymbol{T}}}_{\text{W}}^{\text{F}j}\bigodot\mathop {{\boldsymbol{T}}_{\text{W}}^{\text{F}}}\limits^ \bullet\left(\gamma \right)\right)。\end{array} \right.$

(17)

式中：$ {\text{sum}} $为对矩阵全部元素求和；$ \odot $为矩阵元素对位相乘；$ {\boldsymbol{T}}_{\text{W}}^{\text{F}} $为各元素对变量求偏导，计算式为：

$ \left\{\begin{split} &{ \mathop {{\boldsymbol{T}}_{\text{W}}^{\text{F}}}\limits^ \bullet \left( \alpha \right) = \left[ \begin{array}{*{20}{c}} { -{\text{s}}\beta \cdot {\text{s}}\gamma \cdot {\text{c}}\alpha }&{ {\text{s}}\alpha \cdot {\text{s}}\gamma }&{ {\text{s}}\gamma \cdot {\text{c}}\alpha \cdot {\text{c}}\beta }&0 \\ { {\text{s}}\beta \cdot {\text{c}}\alpha \cdot {\text{c}}\gamma }&{ - {\text{s}}\alpha \cdot {\text{c}}\gamma }&{ - {\text{c}}\alpha \cdot {\text{c}}\beta \cdot {\text{c}}\gamma }&0 \\ { {\text{s}}\alpha \cdot {\text{s}}\beta }&{ {\text{c}}\alpha }&{ - {\text{s}}\alpha \cdot {\text{c}}\beta }&0 \end{array} \right]}，\\ & {\mathop {{\boldsymbol{T}}_{\text{W}}^{\text{F}}}\limits^ \bullet \left( \beta \right) = \left[ \begin{array}{*{20}{c}} { - {\text{s}}\beta \cdot {\text{c}}\gamma - {\text{s}}\alpha \cdot {\text{s}}\gamma \cdot {\text{c}}\beta }&0&{{\text{c}}\beta \cdot{\text{c}}\gamma - {\text{s}}\alpha \cdot {\text{s}}\beta \cdot {\text{s}}\gamma }&0 \\ { - {\text{s}}\beta \cdot {\text{s}}\gamma + {\text{s}}\alpha \cdot {\text{c}}\beta \cdot {\text{c}}\gamma }&0&{{\text{s}}\gamma \cdot {\text{c}}\beta + {\text{s}}\alpha \cdot {\mathrm{s}}\beta \cdot {\text{c}}\gamma }&0 \\ { - {\text{c}}\alpha \cdot {\text{c}}\beta }&0&{ -{\text{s}}\beta \cdot {\text{c}}\alpha }&0 \end{array} \right]}，\\ & {\mathop {{\boldsymbol{T}}_{\text{W}}^{\text{F}}}\limits^ \bullet \left( \gamma \right) = \left[ \begin{array}{*{20}{c}} { - {\text{s}}\gamma \cdot {\text{c}}\beta - {\text{s}}\alpha \cdot {\text{s}}\beta \cdot {\text{c}}\gamma }&{ - {\text{c}}\alpha \cdot {\text{c}}\gamma }&{ - {\text{s}}\beta \cdot {\text{s}}\gamma + {\text{s}}\alpha \cdot {\text{c}}\beta \cdot {\text{c}}\gamma }&0 \\ {{\text{c}}\beta \cdot {\text{c}}\gamma - {\text{s}}\alpha \cdot {\text{s}}\beta \cdot {\text{s}}\gamma }&{ - {\text{s}}\gamma \cdot {\text{c}}\alpha }&{{\text{s}}\beta \cdot \text{{\text{c}}}\gamma + {\text{s}}\alpha \cdot {\text{s}}\gamma \cdot {\text{c}}\beta }&0 \\ 0&0&0&0 \end{array} \right]}。\end{split}\right. $

(18)

相应的，对式(9)中$ n_{\text{W}}^{\text{F}} $的位移量$ X $、$ Y $和$ Z $分别求偏导，计算标记$ j $向位移量传播的误差$ e{X^j} $、$ e{Y^j} $和$ e{Z^j} $。

$ \left\{\begin{gathered} e{X^j} = e{\boldsymbol{T}}_{\text{W}}^{{\text{F}}j}\left( {1,4} \right)，\\ e{Y^j} = e{\boldsymbol{T}}_{\text{W}}^{{\text{F}}j}\left( {2,4} \right)，\\ e{Z^j} = e{\boldsymbol{T}}_{\text{W}}^{{\text{F}}j}\left( {3,4} \right)。\\ \end{gathered}\right. $

(19)

之后，更新误差$ e{n^j} $的参数。

$ e{n^j} = \left[ {\begin{array}{*{20}{c}} {e{\alpha ^j}}&{e{\beta ^j}}&{e{\gamma ^j}}&{e{X^j}}&{e{Y^j}}&{e{Z^j}} \end{array}} \right]。$

(20)

最后，根据$ e{n^j} $和更新步长$ {s^j} $将$ n_{\text{W}}^{\text{F}} $的数值更新为$ {n}_{\text{W}}^{\text{F}}-{\displaystyle {\sum }_{j}{s}^{j}\odot e{n}^{j}} $，即可进行下一轮迭代。

2.2 并行计算加速

为提高计算速度，本文方法针对CUDA平台进行了并行计算优化。由于单个标记计算过程中使用的线程数一般不超过16，GPU最小线程并行数为32，因此将GPU线程块的尺寸设为32，每个块负责2个标记的计算，各线程块的计算相互独立，所有标记同时计算。对于计算中的矩阵相乘，均采用一个GPU线程求解结果矩阵一个元素的策略。对于计算中结构较为复杂的矩阵特别设计了用于计算矩阵元素的核函数，即在GPU上运行的代码。

使用向量$ n_{\text{W}}^{\text{F}} $计算矩阵$ {\boldsymbol{T}}_{\text{W}}^{\text{F}} $时，由于$ {\boldsymbol{T}}_{\text{W}}^{\text{F}} $各元素的结构较为复杂，特别设计了并行优化方法。GPU临近的32个线程存在同步约束，需要各线程执行相同的程序计算不同的矩阵元素，本文使用了两级索引方法。为避免三角函数的重复计算和GPU线程束的同步约束，创建式(21)所示的数组，其中$ para $为变量数组，其前9个参数在每次迭代中根据$ n_{\text{W}}^{\text{F}} $计算，$ para\left( 0 \right) $为索引数组中第1个数。数组$ Inde{x_1} $和$ Inde{x_2} $均为常量数组，各线程使用常量数组的不同字段做为$ para $的索引数，将索引结果拼接，即可实现各线程并行执行相同运算指令，计算不同结构的函数。创建初始值为1的12维数组$ {{\boldsymbol{T}}^{\text{A}}} $用于暂存计算结果。

$\left\{ {\begin{gathered} para = {\left[ {s\alpha }\ \ {s\beta }\ \ {s\gamma }\ \ {c\alpha }\ \ {c\beta }\ \ {c\gamma }\ \ X\ \ Y\ \ Z\ \ 0\ \ 1\ \ { - 1} \right]_{1\times 12}} ，\\ Inde{x_1} = {\left[ {10,4,5,11,2,3,...} \right]_{1\times 36}}，\\ Inde{x_2} = {\left[ {{\text{0,11,2,0,1,2,10,2,0,4,4,10,5,0,1,6,11,5,0,4}}} \right]_{1\times 20}}。\\[-1pt] \end{gathered}} \right. $

(21)

计算$ {\boldsymbol{T}}_{\text{W}}^{\text{F}} $元素时首先启动12个线程线程编号为$ \left[ {0:12} \right) $。对于其中一个线程，使用$ t $作为线程编号，算法在$ i = \left[ {3t}{3t + 3} \right) $的循环中执行$ {{\boldsymbol{T}}^{\text{A}}}\left( t \right) = {{\boldsymbol{T}}^{\text{A}}}\left( t \right)* para\left( {Inde{x_{\text{1}}}\left( i \right)} \right) $。以编号为0和1的线程为例，其对应的$ Inde{x_1} $字段分别为$ \left[ {10,4,5} \right] $和$ \left[ {11,2,3} \right] $，对应的$ para $索引结果分别为$ \left[ {1,c\beta ,c\gamma } \right] $和$ \left[ { - 1,s\gamma ,c\alpha } \right] $，计算的结果分别为$ {{\boldsymbol{T}}^A}\left( 0 \right) = 1\times c\beta \times c\gamma $和$ {{\boldsymbol{T}}^A}\left( 1 \right) = - 1\times s\gamma \times c\alpha $，对应$ {\boldsymbol{T}}_{\text{W}}^{\text{F}} $中第一行前2个元素。其中，$ {{\boldsymbol{T}}^{\text{A}}}\left( 1 \right) $已完成计算。再次启动4个线程分别创建结果暂存变量$ temp = 1 $，算法在$ i = \left[ {\begin{array}{*{20}{c}} {5t + 1\;}&{5t + 5} \end{array}} \right) $的循环中执行$ temp = temp\cdot para\left( {Inde{x_{\text{2}}}\left( i \right)} \right) $，再将结果累加到$ {{\boldsymbol{T}}^{\text{A}}} $，$ {{\boldsymbol{T}}^{\text{A}}}\left( {{\text{ }}Inde{x_{\text{2}}}\left( {5t} \right)} \right) = {{\boldsymbol{T}}^{\text{A}}}\left( {{\text{ }}Inde{x_{\text{2}}}\left( {5t} \right)} \right) + temp $。以编号为0和1的线程为例，其对应的求解结果分别为$ {{\boldsymbol{T}}^{\text{A}}}\left( 0 \right) = \left( {1\times c\gamma \times c\beta } \right) + \left( { - 1 \times s\gamma \times s\alpha \times s\beta } \right) $和$ {{\boldsymbol{T}}^{\text{A}}}\left( 2 \right) = \left( {1\times c\gamma \times s\beta } \right) + \left( {1\times s\gamma \times s\alpha \times c\beta } \right) $。各GPU线程根据该编号确定分配的子任务，并行的执行任务实现加速。$ {{\boldsymbol{T}}^{\text{A}}} $完成计算后将其元素按行顺序写入到$ {\boldsymbol{T}}_{\text{W}}^{\text{F}} $的前3行中。CNOEM在CPU平台上运行时，计算66个标记的误差传播完成一次迭代需要12.9 ms，经过并行优化，在GPU平台上使用5.26TFLOPS算力完成一次迭代仅需0.11 ms。

3 试验与分析 3.1 仿真实验

为测试CNOEM的导航校正精度，设计了仿真试验。试验时AUV匀速前进，每次试验在世界坐标系中选取不同位置，生成标记物的角点及其在成像平面的像素坐标，并对坐标分量施加高斯噪声，同时生成基于导航位置计算的标记角点理论位置，如图4所示。

其中，圆点为标记特征在视野中的实际成像位置；方形为初始化$ n_{\text{W}}^{\text{F}} $对应的理论特征成像位置。迭代过程中$ n_{\text{W}}^{\text{F}} $的数值被逐步修正，相应的方形特征点逐渐向圆点靠拢。每次试验生成一组数据，模拟AUV在一定距离上向标记运动的过程，使用CNOEM和IPPE分别计算导航起始坐标系在世界坐标系的位姿$ {\boldsymbol{T}}_{\text{F}}^{\text{W}} $，将其中的三维坐标参数与仿真数据对比，计算2种方法求解结果与准确位置的距离误差，相机参数如表1所示。

导航校正仿真试验中，对2个相机视野中标记角点成像的横纵坐标分别施加高斯噪声$ N(u,\sigma^{\text{2}}) $，其中均值$ {{u}} $为0，标准差$ \sigma $取值包括0.5、1和1.5。标记被置于多种位置，相对于起始坐标系的位置包括5、10和15 m这3种不同距离L，水平方向偏移X为距离L的0%、20%和40%，垂直方向偏移Y为距离L的0%、10%和20%，水平和垂直偏移模拟AUV从多种位置靠近标记的过程。对于CNOEM，模拟AUV分别以0.5、1、1.5 m/s的速度V匀速靠近标记。$ n_{\text{W}}^{\text{F}} $的姿态初始化时，对$ n_{\text{W}}^{\text{F}}\left[ {0:3} \right) $施加$ {{N}}( {0{\text{,}}{{0.0174}^2}} ) $作为姿态误差，对$ n_{\text{W}}^{\text{F}}\left[ {3:6} \right) $施加$ {{N}}( {0{\text{,}}1{{\text{0}}^2}}) $作为位置误差，仿真试验主要参数如表2所示。

将噪声标准差$ \sigma $、标记距离L、水平偏移量X、垂直偏移量Y以及AUV运动速度5种条件的不同取值进行组合，每种组合生成1000个数据点，使用2种方法分别解算起始坐标系的位置，并计算求解结果与实际位置的平均距离，试验结果如表3所示。

导航校正仿真试验结果表明，CNOEM的解算误差主要受成像噪声、AUV速度和标记距离3个条件的影响。方法位姿解算的误差随标记距离的增加、成像造成的增强以及AUV速度的增加而逐渐增大。在所有条件下CNOEM的误差均小于IPPE，平均误差仅为IPPE的21.7%。在标记距离较远且处于视野边缘的情况下，CNOEM的误差减小了一个数量级，例如在L=15 m、X=0.4L和Y=0.2L的条件下，CNOEM的平均误差仅为IPPE的10.3%。

对仿真试验中CNOEM的收敛时间进行测试，将$ n_{\text{W}}^{\text{F}}\left[ {3:6} \right) $的初始化分为准确值添加$ {{N}}( {0{\text{,}}1{{\text{0}}^2}} ) $噪声初始化和基于IPPE解算结果初始化，试验结果如表4所示。

导航校正仿真试验结果表明，本文算法的收敛速度主要受$ n_{\text{W}}^{\text{F}} $初始化、标记距离和水平方向偏移量X的3个条件影响。$ n_{\text{W}}^{\text{F}} $基于误差初始化时，方法平均收敛时间为0.058 s，标记距离越远或水平偏移量越大，收敛越慢。$ n_{\text{W}}^{\text{F}} $基于IPPE结果初始化时，平均收敛时间为0.018 s。

在AUV导航$ n $无法读取的情况下，AUV小速度运动，CNOEM也可基于临近的数帧图像解算AUV位姿。设计位姿解算仿真试验，$ \sigma=\text{1} $、$ {{L}} = \left[ {2:12} \right) $、V=0.1 m/s，$ {\boldsymbol{T}}_{\text{F}}^{{\text{O}}j} $取单位矩阵，一个校正周期使用AUV左侧相机的5帧图像进行位姿解算，解算结果如图5所示，CNOEM表现出了更好的性能，平均误差为IPPE的50.9%。

3.2 水池实验

为测试CNOEM在2～12 m距离上的位姿解算精度，设计了水池试验。图6(a)所示是设计的水池试验，将AUV投放到水池中，拖动AUV在标记前以约0.1 m/s的速度往复运动，使用AUV的左侧相机收集视频数据。图6(b)为收集的图像之一，将世界坐标系定在标记的中心处如图1中$ {\text{W}} $所示，使用算法识别图像中标记4个角点的横纵坐标，如图6(b)中的圆点所示，再使用CNOEM和IPPE分别计算AUV在世界坐标系中的位置。目前，AUV试验平台的导航系统与搭载的视觉定位计算机还未进行适配。因此，水池试验中算法仅基于图像进行位姿解算，本文算法基于临近的5帧图像计算AUV位置，$ {\boldsymbol{T}}_{\text{F}}^{{\text{O}}j} $取单位矩阵，5帧图像在相同的位置上成像位置。

水池试验的主要参数如表5所示。

根据AUV往复运动中的一次逼近和一次远离数据分别计算相机坐标系在世界标系中的位置，对于解算结果，计算临近20个求解结果的标准差，在2～12 m的距离上，每隔0.25 m划分一个距离区间，将计算的标准差根据距离分配到各区间上，再对每个区间内的标准差求平均，将其作为误差制成折线图，逼近试验和远离试验的误差如图7所示。在2次试验中，CNOEM求解的结果表现出了更好的稳定性，求解结果的平均误差为IPPE的66%，对于9 m以上的标记平均误差为IPPE的48%。

图8所示为位姿解算仿真试验与逼近试验的结果，可以看到2种方法的水池试验和仿真试验结果都具有较好一致性，可以认为本文仿真试验中使用的噪声模型和试验条件，能够较好模拟水下AUV靠近标记的过程。因此使用该噪声模型和仿真条件的导航校正仿真试验，其试验结果可较为真实的反应CNOEM的实际性能。

3.3 外场实验

为测试CNOEM在冰下环境中的位姿解算精度，设计了外场试验。如图9所示将AUV投放到冰下，控制AUV靠近标记，识别标记角点获取试验数据。

试验中受湖水浑浊度的限制，识别算法仅在2.5 m距离内可以稳定识别标记，且标记角点位置相比水池试验存在更大误差。试验中，将标准差划分区间的宽度设为0.1 m，2种方法的试验结果如图10所示，在更强噪声条件下，CNOEM表现出了更高的稳定性，位姿解算误差为IPPE的36.6%。

4 结　语

本文提出的一种基于Aruco标记冰下AUV的导航校正方法CNOEM，通过迭代减小视野中标记角点理论空间位置与实际成像投影线之间的距离，实现AUV导航位置的校正。该方法可以同时处理多帧图像中标记的位置信息，对于水下标记成像亮度不稳定，标记角点成像位置存在的误差，具有良好的适应性。在导航校正仿真试验中，其平均误差仅为常用算法IPPE的21.7%，当标记标记距离较远且处于视野边缘的情况下，CNOEM相较于IPPE的精度提升了一个数量级。在位姿解算仿真、水池和外场试验中，CNOEM的平均误差分别为IPPE的50.9%、66%和36.6%。试验结果表明，CNOEM相较于常用算法表现出了更好的精度。仿真和水池试验结果的一致性有效证明了本文仿真中噪声模型和试验条件的合理性。此外，CNOEM针对GPU平台进行并行优化，单次迭代的运行时间仅为CPU平台的0.8%，平均收敛时间为0.018 s，具有良好的实时性。