0 引　言

UUV的水下对接回收技术一直是国内外学者关注的难点和热点，特别是在对接回收过程的末段，对定位数据的准确度和稳定性提出了相当高的要求，当前多采用基于水下单、双目相机的视觉定位方法。然而不同于空气环境中，视觉定位技术的水下应用还面临诸多挑战，如由于水的折射率大于空气，相机的有效视场角无法与空气中相提并论。此外，海洋、湖泊等真实水下环境中有各种漂浮物，容易遮挡相机视线。

国内外许多学者对水下视觉定位方法进行了深入研究。蔡迎波等^[1]提出一种基于单目视觉的四自由度水下定位方法，相对精度达到10 cm以内。严卫生等^[2]使用旋转定位法提高AUV自主回收过程中单目视觉定位的精度。龙泽升等^[3]引入Cascade RCNN神经网络模型优化了AUV水下对接过程中视觉导引的定位精度和计算时间。张伟等^[4]使用非线性卡尔曼滤波器实现了一种基于非对称光阵列和单目视觉的UUV回收方法。李光乐等^[5]利用两异面直线公垂线中点作为目标点，设计了一种高精度双目视觉定位方法。徐筱龙等^[6]通过建立非线性畸变模型对单目视觉定位方法进行非线性修正，获得了较好的水下定位精度。

本文从提高视觉定位稳定性的角度出发，基于P3P（Perspective-Three-Point, P3P）算法和ICP（Iterative Closest Point, ICP）算法设计了一种冗余视觉定位系统。该系统使用3条线程以不同的视觉位姿估计算法解算回收装置相对于UUV的位姿，3条线程的输出互相独立、互为备份，相较于单一功能的单目或双目视觉定位系统具有更高的可靠性。

1 基础理论

视觉位姿估计方法的基本原理是：通过光学相机分别获取目标特征点在回收装置坐标系和相机坐标系下的空间坐标。结合相机内外参数求解2个坐标系之间的旋转平移矩阵，经变换得回收装置和UUV之间的相对位姿关系。

其中，单目视觉定位采用二维码标识的4个角点作为目标特征点，使用P3P算法解算特征点在相机坐标系下的空间坐标，再使用ICP算法进行位姿估计；双目视觉采用3个绿光LED灯构成的导引灯阵作为目标特征点，通过视差图解算特征点在相机坐标系下的坐标，同样使用ICP算法解算位姿。

1.1 P3P算法

P3P算法是求解PnP（perspective-n-point, PnP）问题的经典算法，以3对目标特征点为例进行说明（4个目标特征点可获得唯一解^[7]），图1为目标特征点的成像示意图。

图中， ${O_c}$ 为相机光心, ${P}_{1}、{P}_{2}、{P}_{3}$ 为世界坐标系下的目标特征点， ${p}_{1}、{p}_{2}、{p}_{3}$ 分别为 ${P}_{1}、{P}_{2}、{P}_{3}$ 在相机成像平面上的投影。由几何相似关系可得：

$\begin{aligned} & \Delta {O_{\text{c}}}{p_1}{p_2} \sim \Delta {O_{\text{c}}}{P_1}{P_2} ，\\ & \Delta {O_{\text{c}}}{p_1}{p_3} \sim \Delta {O_{\text{c}}}{P_1}{P_3} ，\\ & \Delta {O_{\text{c}}}{p_2}{p_3} \sim \Delta {O_{\text{c}}}{P_2}{P_3} 。\end{aligned}$

(1)

在 $\Delta {O}_{\text{c}}{P}_{1}{P}_{2}、\Delta {O}_{\text{c}}{P}_{1}{P}_{3}、\Delta {O}_{\text{c}}{P}_{2}{P}_{3}$ 中，由余弦定理可得：

$\begin{aligned} & {P_1}{P_2}^2 = {O_c}{P_1}^2 + {O_c}{P_2}^2 - 2{O_c}{P_1} \cdot {O_c}{P_2} \cdot\\ & \cos < {O_c}{P_1},{O_c}{P_2} > , \\ & {P_1}{P_3}^2 = {O_c}{P_1}^2 + {O_c}{P_3}^2 - 2{O_c}{P_1} \cdot {O_c}{P_3} \cdot\\ & \cos < {O_c}{P_1},{O_c}{P_3} > , \\ & {P_2}{P_3}^2 = {O_c}{P_2}^2 + {O_c}{P_3}^2 - 2{O_c}{P_1} \cdot {O_c}{P_3} \cdot\\ & \cos < {O_c}{P_2},{O_c}{P_3} > 。\end{aligned}$

(2)

其中， $< {O_c}{P_1},{O_c}{P_2} >$ 为三角形 $\Delta {O_{\text{c}}}{P_1}{P_2}$ 的2条边 ${O_c}{P_1}$ 和 ${O_c}{P_2}$ 的夹角。设 $x = {O_c}{P_1}/{O_c}{P_3}$ ， $y = {O_c}{P_2}/ {O_c}{P_3}$ ， $u = {P_1}{P_2}^2/{O_c}{P_3}^2$ ， $v = {P_1}{P_3}^2/{P_1}{P_2}^2$ ， $w = {P_2}{P_3}^2/ {P_1}{P_2}^2$ ，则式(2)化简为：

$\begin{aligned} & u={x^2}+{y^2}-2x\cdot y \cdot \cos < {O_c}{P_1},{O_c}{P_2} > ，\\ & uv = {x^2} + 1 - 2x \cdot \cos < {O_c}{P_1},{O_c}{P_3} > ，\\ & uw = {y^2} + 1 - 2y \cdot \cos < {O_c}{P_2},{O_c}{P_3} > 。\end{aligned}$

(3)

进一步化简得：

$\begin{aligned} & (1 - v){x^2} - v{y^2} + 2v \cdot x \cdot y \cdot \\ & \cos < {O_c}{P_1},{O_c}{P_2} > - 2x \cdot \\ &\cos < {O_c}{P_1},{O_c}{P_3} > + 1 = 0 ，\\ & (1 - w){x^2} - w{y^2} + 2w \cdot x \cdot y \cdot \\ & \cos < {O_c}{P_1},{O_c}{P_2} > - 2y \cdot \\ & \cos < {O_c}{P_2},{O_c}{P_3} > + 1 = 0 。\end{aligned}$

(4)

由已知的相机内参数和 ${p}_{1}、{p}_{2}、{p}_{3}$ 可得 $< {O_c}{P_1},{O_c}{P_2} >$ 、 $< {O_c}{P_1},{O_c}{P_3} >$ 、 $< {O_c}{P_2},{O_c}{P_3} >$ 和 $u$ 的值，由世界坐标系中特征点的相对关系可得 $v$ 和 $w$ ，使用吴消元法进行求解式(4)中未知参数 $x$ 和 $y$ ，进而可得 ${O}_{c}{P}_{1}、{O}_{c}{P}_{2}、{O}_{c}{P}_{3}$ ，再根据三角形相似原理求得特征点 ${P}_{1}、{P}_{2}、{P}_{3}$ 在相机坐标系下的坐标 ${P}_{1}{}_{c}、{P}_{2c}、{P}_{3\text{c}}$ 。获得 ${P}_{1}{}_{c}、{P}_{2c}、{P}_{3\text{c}}$ 后即可使用ICP算法求解目标和相机之间的相对位姿关系。

1.2 ICP算法

使用ICP算法求解目的是求解变换矩阵 ${\boldsymbol{R},{\boldsymbol{t}}}$ ，使得相机坐标系下的目标特征点经过旋转和平移矩阵后与回收装置坐标系下对应点之间的距离最短^[8]。设共有 $n$ 个目标特征点，其在回收装置坐标系中坐标及在相机坐标系中的坐标分别为 $P$ 和 $P'$ ：

$\begin{split} & P = \left\{ {{p_1},{p_2}, \cdots ,{p_n}} \right\} , \\ & P'= \left\{ {{{p'}_1},{{p'}_2}, \cdots ,{{p'}_n}} \right\} 。\end{split}$

(5)

定义第 $i$ 对匹配点的误差项为：

${e_i} = {p_i} - \left( {R{{p'}_i} + t} \right),$

(6)

其最小二乘问题如下式：

$\mathop {\min }\limits_{R,t} J = \frac{1}{2}\sum\limits_{i = 1}^n {\left\| {{p_i} - \left( {R{{p'}_i} + t} \right)} \right\|_2^2}。$

(7)

展开误差项函数并简化得：

$\begin{split} & \frac{1}{2}\sum\limits_{i = 1}^n {\left\| {{p_i} - \left( {R{{p'}_i} + t} \right)} \right\|_2^2} {\text{ = }}\frac{1}{2}\sum\limits_{i = 1}^n +{\left\| {\left( {{p_i} - p - R\left( {{{p'}_i} - p'} \right)} \right)} \right\|_2^2} +\\ & \left\| {\left( {p - Rp' - t} \right)} \right\|_2^2。\\[-15pt]\end{split}$

(8)

计算2组点的质心 $p$ 和 $p'$ ，去质心坐标如下式：

$\begin{gathered} {q_i} = {p_i} - p ，\\ {{q'}_i} = {{p'}_i} - p'。\\ \end{gathered}$

(9)

求解优化问题：

${R^ * }{\text{ = }}\arg \mathop {\min }\limits_R J = \frac{1}{2}\sum\limits_{i = 1}^n {\left\| {{q_i} - R{{q'}_i}} \right\|_2^2}，$

(10)

展开后提取与 $R$ 相关部分，可简化为：

$\sum\limits_{i = 1}^n { - {q_i}^{\rm{T}}R{{q'}_i}}= \sum\limits_{i = 1}^n { - tr\left( {R{{q'}_i}{q_i}^{\rm{T}}} \right)} = tr\left( {R\sum\limits_{i = 1}^n {{{q'}_i}{q_i}^{\rm{T}}} } \right)。$

(11)

定义矩阵 ${\boldsymbol{W}}{\text{ = }}\displaystyle\sum\limits_{i = 1}^n {{q_i}{q_i}{{^\prime }^{\rm{T}}}}$ ， $\boldsymbol{W}$ 为一个 $3 \times 3$ 的矩阵，对其进行SVD分解得 ${\boldsymbol{W}} = U\displaystyle\sum {V^{\rm{T}}}$ 。当矩阵 ${\boldsymbol{W}}$ 满秩时解得：

$\begin{split} & R = U{V^{\rm{T}}}，\\ & t = p - R{p^i} 。\end{split}$

(12)

2 系统设计 2.1 整体结构

本文的冗余视觉定位系统由2部分组成，一部分由二维码和导引灯阵构成的目标特征标识，设置于回收装置上；另一部分由双目相机和图像处理板卡构成的视觉检测系统，设置于UUV上，双目相机采用Stereolabs公司的ZED 2i双目相机，图像处理板卡则采用Nvidia公司的Xavier NX六核高性能处理板，支持CUDA图形加速^[9]。当UUV接近回收装置时，双目相机捕捉到二维码和导引灯阵，经图像处理板卡解算后得到回收装置相对于UUV的位置和姿态，UUV主控制器根据位姿信息进行相应的控制调节完成整个回收过程。冗余视觉定位系统硬件结构示意图如图2所示。

2.2 软件设计

视觉检测系统通过ZED相机采集图像数据，并使用Xavier NX板卡进行数据并行处理。系统主程序使用3条独立线程并行执行位姿估计任务，分别命名为左目线程、右目线程和双目线程。其中左、右目线程分别基于左、右目图像通过P3P算法和ICP算法解算位姿，双目线程基于双目视差图通过ICP算法解算位姿。通过特殊设计的通道选择器对3条线程的输出进行判断，选择合理的输出作为最终的位姿解算结果。3条线程互为备份构成冗余系统，只要有一条线程能够正常工作便可保证系统的运行，在一定程度上避免由于遮挡、噪声和目标丢失等偶然因素导致的系统异常，提高了视觉定位的可靠性。系统程序框图如图3所示。

3 水池试验

进行水池试验对冗余视觉定位系统的位置解算效果进行评估，具体试验步骤如下：1）将二维码标识和由2个LED灯组成的灯阵固定在六自由度平台上的同一水平面；2）在二维码和灯阵所在的平面上方1.5 m处设置视觉检测系统，使二维码和灯阵均不超出相机视野范围；3）设置六自由度平台在y轴方向做幅度为150 mm、频率为1 Hz的余弦运动，通过视觉检测系统获取六自由度平台的位置信息；4）在试验过程中随机遮挡某一目相机，分别观察3条线程的输出结果。

试验1：在试验开始1.0 s时对右目进行遮挡（通过软件失能右目相机实现），在3.7 s时移除遮挡（通过软件失能右目相机实现），试验结果如图4所示。

试验2：在试验开始0.3 s时对左目进行遮挡，在1.6 s时移除遮挡；在2.5 s时对右目进行遮挡，在3.4 s时移除遮挡，试验结果如图5所示。

结果表明：1）左目、右目线程输出位置的中误差不超过3 mm，最大误差不超过15 mm，双目线程输出位置的中误差不超过2.5 mm，最大误差不超过10 mm；2）单目和双目线程的图像处理速度均不低于30 FPS；3）双目相机中某一目被遮挡时该目对应的线程和双目线程输出均失效，但剩余一目对应的线程仍可正常输出，且去除遮挡后2条失效线程均能恢复正常输出。

4 结　语

为满足UUV进行水下回收时对定位准确性和可靠性的高要求，本文将双目视觉定位和单目视觉定位结合，设计了一种水下冗余视觉定位系统。该系统采用1台双目相机同时部署单目视觉和双目视觉位姿估计算法，即使双目相机中的某一目因遮挡或视野限制失效，剩余一目仍能完成对目标位姿的检测。经水池试验测试，该系统对运动目标位置的检测速度和精度均满足实际使用需求，与单一的单目或双目视觉定位系统相比具有更高的可靠性，能够为UUV回收提供准确可靠的定位信息。