舰船科学技术  2024, Vol. 46 Issue (16): 81-89    DOI: 10.3404/j.issn.1672-7649.2024.16.014   PDF    
基于视觉的无人机艇系统姿态感知研究
邓涛1,3, 刘双2, 魏汉迪1,3     
1. 上海交通大学 海洋工程国家重点实验室,上海 200240;
2. 中国船舶科学研究中心 上海分部,上海 200011;
3. 上海交通大学 三亚崖州湾深海科技研究院,海南 三亚 572024
摘要: 随着无人机和无人艇技术的迅速发展,无人机-无人艇协同应用逐渐成为未来海上军事任务和智慧交通的不可或缺组成部分。然而,考虑到无人艇在不同海况条件下的高动态特性,传统的雷达等手段虽然可以满足高精度和实时性的要求,但成本较高,而传统的视觉方法虽然成本低、易于实现,但稳定度较低。本文基于相机成像原理,提出一种无人机与无人艇的单目定距方法。通过对着陆区域的多个标志物进行识别定距后,对无人艇着陆区域平面进行拟合重构,从而获取无人机与无人艇的距离,标志物越多,算法失效的概率越低。搭建数值模拟器对算法进行了测试,研究了算法在无人艇大倾角摇晃下的精度和稳定性。结果显示,算法在无人艇摇晃的情况下可以得到较为准确的距离,所有工况计算出XYZ方向上的平均相对误差分别为7.47%、6.48%和8.04%。
关键词: 距离感知     单目视觉     无人机艇协同    
Research on visual-based motion sensing for unmanned drone-vessel system
DENG Tao1,3, LIU Shuang2, WEI Handi1,3     
1. State Key Laboratory of Ocean Engineering, Shanghai Jiao Tong University, Shanghai 200240, China;
2. Shanghai Branch, China Ship Scientific Research Center, Shanghai 200011, China;
3. SJTU-Sanya Yazhou Bay Institute of Deepsea Technology, Shanghai Jiao Tong University, Sanya 572024, China
Abstract: With the rapid advancement of unmanned drone and vessel technologies, the collaborative application of unmanned drones and vessel is gradually becoming an indispensable component of future maritime military missions and smart transportation. However, considering the high dynamic characteristics of unmanned vessels under different sea conditions, traditional methods such as radar, while meeting the requirements for high accuracy and real-time performance, incur high costs. On the other hand, traditional vision-based approaches, though cost-effective and easy to implement, suffer from lower stability. This paper proposes a monocular method for unmanned drone and vessel system based on the principles of pinhole camera model. By identifying and ranging multiple landmarks in the landing area, a plane fitting reconstruction of the landing area for the unmanned vessel is performed, enabling the determination of the distance between the unmanned drone and vessel. The more landmarks identified, the lower the probability of algorithm failure. A numerical simulator was constructed to test the algorithm, evaluating its accuracy and stability under the large-angle rotation of the unmanned vessel. The results demonstrate that the algorithm can obtain accurate distances, with average relative errors in the X, Y and Z directions of 7.47%, 6.48% and 8.04%, respectively.
Key words: distance perception     monocular vision     unmanned aircraft-unmanned vessel cooperation    
0 引 言

我国地域辽阔,拥有广阔的海洋疆土以及众多河流湖泊。跨足天空和海洋的跨域无人系统装备正在迅速崛起,成为颠覆性的技术,深刻地改变着未来战争规则和社会生活。无人机与无人艇协同平台作为无人系统装备的关键分支,将在未来的军事和民用领域中发挥重要作用,涵盖军事打击、侦察巡逻、远洋行动、海上救援、地理测绘以及环境监测等多个领域。

具体而言,无人机和无人艇的协同作战在军事领域中发挥着重要角色。它们可以执行精确的打击任务,对敌方目标进行监视和攻击,提高作战效率,并减少军人面临的风险。无人系统在侦察和巡逻方面具有独特的优势。它们可以迅速、灵活地监视广大区域,收集情报,提供实时的战场情报,有助于制定战略和战术决策。并且无人艇在远洋行动中发挥着重要作用。它们可以执行长时间的航行任务,监测远洋区域的海况,为无人机扩大执行任务范围提供支持。美国作为军事强国,将无人机置于优先发展的地位。在海湾战争和阿富汗战争中,美国陆军和海军纷纷引入了“先锋”无人机,以实施一系列任务,包括超视距目标定位、侦察和战损评估等。这种廉价而高效的无人机使美军实施侦察、打击和其他任务一体化成为现实。如图1所示,异构无人系统相对于同构无人系统,表现出更高效的任务执行能力和更强大的适应复杂任务的能力[1]。无人机可通过分布式方法对任务规划进行优化,使无人机与无人艇的作业效率提高[2],同时无人艇可为小型无人机提供自主起飞和降落的平台,并充当无人机的信息处理中心和通信基站[3]。如今,世界军事强国又把目光投向水上作战无人艇,我国也将无人系统群体跨域协同列为未来的战略发展方向[4]。在无人机和无人艇的协同控制研究中,主要包括协同体系结构、协同系统组成、协同任务分配、协同编队、协同路径规划、协同控制等[5]。其中,无人机与无人艇的精准协同起降控制对海面异构平台可靠执行协同任务尤为重要[6]。要研究无人机与无人艇的精准协同起降控制就绕不开无人机与无人艇的距离感知问题。

图 1 海上异构无人装备一体化协同作战架构[7] Fig. 1 Integrated collaborative combat architecture for heterogeneous unmanned maritime systems[7]

因此,对无人机[8-9]和无人艇的距离感知研究对无人系统的协同控制具有关键意义。距离感知研究中一般采用定距方法,无人机和无人艇的定距方法包括雷达、光电定距方法,视觉定距方法以及卫星Align[10]或载波相位(Real-Time Kinematic, RTK)定距方法。在各种定距方法中,视觉定距方法以其高灵活性、轻量化集成、成本效益高等优势日益受到关注。

基于单目视觉的无人机定距方法一般有2个步骤:特征检测和位姿估计。在特征检测方面,Leutenegger等[11]提出了二进制鲁棒不变可扩展关键点(Binary Robust Invariant Scalable Keypoints,BRISK),BRISK是一种二进制特征描述子,在计算速度和存储效率上表现优越,适用于嵌入式系统和实时应用。Alcantarilla等[12]在2012年提出了Z顺序椭圆关键点分析(Keypoint Analysis for Z-ordered Ellipses,KAZE),KAZE 是一种用于特征检测和描述的算法,它倾向于提取具有高阶特性的特征点,如边缘和角点。在2013年Alcantarilla等[13]对KAZE进行了改进,提出了Z顺序椭圆加速关键点分析(Accelerated-Keypoint Analysis for Z-ordered Ellipses,AKAZE),相较于KAZE,AKAZE在速度和稳定性方面进行了优化,适用于实时应用。

在位姿估计方面,Triggs [14]在1999年提出了束调整(Bundle Adjustment,BA),BA是一种用于优化相机姿态和三维点位置的技术,通过最小化代价函数,同时优化相机参数和三维点坐标,以提高相机和三维点的精确度。Nistér等[15]在2004年提出了视觉里程计(Visual Odometry,VO),VO是一种用于估计相机的运动的技术,通过匹配连续图像帧中的特征点来计算相机的位姿变化, 这通常通过光流(OF)分析来完成[16]。之后有很多研究团队和学者将VO和惯性测量单元(IMU)结合提出了视觉惯性里程计(Visual-Inertial Odometry,VIO),相较于VO,VIO提高了运动估计的鲁棒性和精确度,Gui等[17]从过滤和优化角度回顾了VIO。随着卷积神经网络(CNN)[18]的出现,计算机视觉领域发生了翻天覆地的变化,Goforth和Lucey[19]提出了一种在优化框架内结合CNN和VO的方法,该方法允许学习用于直接图像对齐的深度特征表示。

1 基于标志物阵列的无人机艇单目视觉定距算法

为实现无人机相对于无人艇的视觉定距,本文提出一种基于标志物阵列的视觉定距算法。算法的核心是通过已知图像中的点(通常是特征点)与物体三维点之间的对应关系,来计算相机的位姿,并通过标志物阵列来降低目标丢失概率,提高定位精度。在本节中,先说明坐标系定义,再介绍基于标志物阵列的改进视觉定距算法。

1.1 坐标系定义

图2所示,常用的与无人机视觉有关的坐标系有4种:大地坐标系(Earth Coordinate System)$ {O}_{w}{X}_{w}{Y}_{w}{Z}_{w} $、无人机坐标系(Aircraft Coordinate System)$ {O}_{d}{X}_{d}{Y}_{d}{Z}_{d} $、相机坐标系(Camera Coordinate System)$ {O}_{c}{X}_{c}{Y}_{c}{Z}_{c} $和随体坐标系(Body Coordinate System)$ {O}_{b}{X}_{b}{Y}_{b}{Z}_{b} $。其中,大地坐标系的坐标原点为初始时刻静止不动的着陆平面中心在大地上的投影点,X轴正方向为东方,Y轴正方向为北方,Z轴正方向垂直地面向上。无人机坐标系的坐标原点为无人机质心,X轴正方向指向飞行器的前方,Y轴正方向指向飞行器的右侧,Z轴正方向指向飞行器的底部(垂直向下)。相机坐标系的坐标原点为相机光心,Z轴正方向和光轴正方向一致(垂直相机镜片向外),X轴正方向指向相机的右侧,Y轴正方向指向相机的下方,相机仅做纵摇和首摇2个自由度的运动。随体坐标系的坐标原点为初始时刻静止不动的着陆平面中心,X轴正方向指向着陆平面的前方,Y轴正方向指向着陆平面的右方,Z轴正方向为垂直着陆平面向上。着陆平面位于无人艇甲板上表面 ,上面布置了标志物阵列作为视觉标记。受制于无人机的电池容量和有效载荷,本文选用ArUco码作为视觉标记,减小了特征检测的工作量。

图 2 坐标系定义 Fig. 2 Coordinate system definition
1.2 基于标志物阵列的无人机定距算法

图3所示,改进的基于视觉的无人机定距算法的流程可分为4个部分:数据输入、特征提取和匹配、运动估计、距离估计。

图 3 基于标志物阵列的改进视觉定距算法流程图 Fig. 3 Flowchart of improved visual distance estimation algorithm based on marker array

算法中的输入部分一般为图片数据或视频流,对输入的图片数据或视频流做了以下预处理工作,一是灰度转换,将获取的彩色图像转换为灰度图像,以减少处理复杂度;二是图像去噪,使用高斯滤波[20]来减少图像中的噪声,以提高ArUco码检测的准确性。特征提取和匹配部分主要是要检测和识别ArUco码的角点和ID,为确定其位置提供参考。

运动估计则有3个内容,先计算相机与ArUco码的旋转角度和位移距离:使用检测到的ArUco码的角点和相机内参,计算相机相对于ArUco码的旋转矩阵和平移向量,得到相机在ArUco码坐标系下的位姿。其次是着陆平面拟合,使用相机检测到的ArUco码阵列中各个ArUco码的位姿信息,拟合得到一个着陆平面,该平面用于无人机降落时的参考。着陆平面拟合的数学推导如下:

空间中任一平面可以由该平面的法向量和平面上的一个点描述,该平面的数学表示可以写为:

$ \begin{array}{c}A\left(x-{x}^{{'}}\right)+B\left(y-{y}^{{'}}\right)+C\left(z-{z}^{{'}}\right)=0\end{array}。$ (1)

式中:A、 B、 C为法向量;$ {x}^{{'}}{\text{、}}{y}^{{'}}{\text{、}}{z}^{{'}} $为平面上任一点坐标。

着陆平台上有ArUco码阵列,通过之前的特征提取和匹配,可以得到每个ArUco码在相机坐标系中的坐标及平移向量,而每个ArUco码与着陆平面中心的相对距离可以直接测得,于是每个ArUco码与着陆平面中心形成的向量可以写为:

$ \begin{array}{c}{\boldsymbol{p}}_{i}=\left[\begin{array}{c}{x}_{i}-{x}_{0}\\ {y}_{i}-{y}_{0}\\ {z}_{i}-{z}_{0}\end{array}\right]\end{array}。$ (2)

式中:$ {x}_{0},{y}_{0},{z}_{0} $为着陆平面中心在随体坐标系下坐标;$ {X}_{i},{Y}_{i},{Z}_{i} $为ArUco码在随体坐标系下坐标;i为ArUco码ID。

将式(2)中向量拼装起来得到一个新向量:

$ \begin{array}{c}{\boldsymbol{P}}=\left[\begin{array}{c}{P}_{1}\\ {P}_{2}\\ \begin{array}{c}\cdots \\ {P}_{n}\end{array}\end{array}\right]\end{array}。$ (3)

式中:n为着陆平面上ArUco码的数量。

当所测得ArUco码的数量在3个以上时,将所有测得的ArUco码进行拟合,着陆平面的法向量为:

$ \begin{array}{c}{\boldsymbol{N}}=\displaystyle {{\displaystyle\sum_j}{k\varepsilon L,j\ne k}\left|{p}_{j}\times {p}_{k}\right|}/{{A}_{l}^{3}}\end{array}。$ (4)

式中:L为相机测得ArUco码ID的集合;l为相机测得ArUco码的数量。

最后计算相机与着陆平面的旋转角度和位移距离,通过计算相机在着陆平面坐标系下的位姿,估计相机相对于着陆平面的旋转角度和位移距离,用于无人机降落控制。

对相机坐标系来说,相机所拍摄物体的坐标与相机坐标系中坐标的转换关系为:

$ \begin{array}{c}\left[\begin{array}{c}{x}_{c}\\ {y}_{c}\\ {z}_{c}\end{array}\right]=\left[\begin{array}{cc}{\boldsymbol{R}}_{3 \times 3}& {{T}}_{3\times1}\\ {0}_{1\times3}& 1\end{array}\right]\left[\begin{array}{c}{x}_{b}\\ {y}_{b}\\ {z}_{b}\end{array}\right]\end{array}。$ (5)

式中:R为拍摄物体与相机的旋转矩阵;T为拍摄物体与相机的位移向量。

相机每识别到一个ArUco码,便会得到一个旋转向量和一个位移向量,那么ArUco码阵列所在的着陆平面相对相机坐标系的旋转向量为:

$ \begin{array}{c}{\boldsymbol{r}}_{\mathit{m}}=\displaystyle{\displaystyle\sum _{i=1}^{n}{\boldsymbol{r}}_{i}}/{n}\end{array}。$ (6)

式中:r为ArUco码相对相机的旋转向量。

考虑到坐标转换中使用到的是旋转矩阵而不是旋转向量,可以使用罗德里格斯公式将式(6)转换为相应的旋转矩阵:

$ \begin{array}{c} \boldsymbol{I}_{\boldsymbol{r}}=\displaystyle \frac{{r}_{m}}{\left|{r}_{m}\right|}\end{array},$ (7)
$ \begin{array}{c}{\boldsymbol{R}}_{\boldsymbol{b2}\boldsymbol{c}}=\boldsymbol{I}+\mathit{{\mathrm{sin}}}{\theta }^{{'}}\cdot \boldsymbol{K}+\left(1-\mathit{{\mathrm{cos}}}{\theta }^{{'}}\right)\cdot {\boldsymbol{K}}^{2}\end{array}。$ (8)

式中:I为单位矩阵;$ {\theta }^{{'}} $为绕单位向量$ {\boldsymbol{I}}_{\boldsymbol{r}} $旋转的角度;K表示单位向量$ {\boldsymbol{I}}_{\boldsymbol{r}} $的斜对称矩阵。

由迭代最近点算法可得着陆平台中心相对于相机坐标的位移向量为:

$ \begin{array}{c}f\left({\boldsymbol{R}}_{\boldsymbol{c2}\boldsymbol{b}},{\boldsymbol{T}}_{\boldsymbol{c2}\boldsymbol{b}}\right) = \displaystyle \frac{1}{k} \sum_{i=1}^{k}{\text{‖}}{q}_{i} - \left({\boldsymbol{R}}_{\boldsymbol{c2}\boldsymbol{b}}\cdot {p}_{i}+{\boldsymbol{T}}_{\boldsymbol{c2}\boldsymbol{b}}\right){\text{‖}}^{2} = {\mathrm{min}}\end{array}。$ (9)

式中:$ {q}_{i} $$ {p}_{i} $分别为平台上和图像坐标中对应ArUco码的坐标。

算法的距离估计通过相机与无人机的坐标转换,将相机在着陆平面坐标系下的位姿转换为无人机在着陆平面坐标系下的位姿。

空间中旋转矩阵可以表示为:

$ \begin{array}{c}{\boldsymbol{R}}_{\boldsymbol{Z}\boldsymbol{Y}\boldsymbol{X}} = {\boldsymbol{R}}_{\boldsymbol{x}}\left(\alpha \right)\cdot {\boldsymbol{R}}_{\boldsymbol{y}}\left(\beta \right)\cdot {\boldsymbol{R}}_{\boldsymbol{z}}\left(\gamma \right) = \left[ \begin{array}{ccc}\mathit{{\mathrm{cos}}}\beta \cdot \mathit{{\mathrm{cos}}}\gamma & -\mathit{{\mathrm{cos}}}\beta \cdot \mathit{{\mathrm{sin}}}\gamma & \mathit{{\mathrm{sin}}}\beta \\ \mathit{{\mathrm{sin}}}\alpha \cdot \mathit{{\mathrm{sin}}}\beta \cdot \mathit{{\mathrm{cos}}}\gamma +\mathit{{\mathrm{cos}}}\alpha \cdot \mathit{{\mathrm{sin}}}\gamma & -\mathit{{\mathrm{sin}}}\alpha \cdot \mathit{{\mathrm{sin}}}\beta \cdot \mathit{{\mathrm{sin}}}\gamma +\mathit{{\mathrm{cos}}}\alpha \cdot \mathit{{\mathrm{cos}}}\gamma & -\mathit{{\mathrm{sin}}}\alpha \cdot \mathit{{\mathrm{cos}}}\beta \\ -\mathit{{\mathrm{cos}}}\alpha \cdot \mathit{{\mathrm{sin}}}\beta \cdot \mathit{{\mathrm{cos}}}\gamma +\mathit{{\mathrm{sin}}}\alpha \cdot \mathit{{\mathrm{sin}}}\gamma & \mathit{{\mathrm{cos}}}\alpha \cdot \mathit{{\mathrm{sin}}}\beta \cdot \mathit{{\mathrm{sin}}}\gamma +\mathit{{\mathrm{sin}}}\alpha \cdot \mathit{{\mathrm{cos}}}\gamma & \mathit{{\mathrm{cos}}}\alpha \cdot \mathit{{\mathrm{cos}}}\gamma \end{array} \right]\end{array}。$ (10)

式中:$ \alpha \mathrm{、}\beta \mathrm{、}\gamma $分别为绕XYZ轴的旋转角度。

相机坐标系和无人机坐标系的初始旋转矩阵可认为是相机坐标系先绕$ {Z}_{c} $轴旋转$\displaystyle-\frac{{\text π} }{2}$,再绕$ {Y}_{c} $轴旋转$ \displaystyle-\frac{{\text π} }{2} $得到,将其代入式(10)可得:

$ \begin{array}{c}{\boldsymbol{R}}_{1}={\boldsymbol{R}}_{\boldsymbol{x}}\left(0\right)\cdot {\boldsymbol{R}}_{\boldsymbol{y}}\left(\displaystyle-\frac{{\text π} }{2}\right)\cdot {\boldsymbol{R}}_{\boldsymbol{z}}\left(\displaystyle-\frac{{\text π} }{2}\right)=\left[\begin{array}{ccc}0& 0& 1\\ 1& 0& 0\\ 0& 1& 0\end{array}\right]\end{array} 。$ (11)

对于相机的摇荡运动,相机全程只有纵摇和艏摇运动,相机的艏摇运动为绕相机坐标系的$ {Y}_{c} $轴旋转,相机的纵摇运动为绕相机坐标系相机坐标系的$ {X}_{c} $轴旋转,则相机运动后恢复的旋转矩阵可以写为:

$ \begin{aligned}[b] {\boldsymbol{R}}_{\boldsymbol{c}}= &{\boldsymbol{R}}_{\boldsymbol{z}}\left(0\right)\cdot {\boldsymbol{R}}_{\boldsymbol{x}}\left({\beta }_{c}\right)\cdot {\boldsymbol{R}}_{\boldsymbol{y}}\left({\gamma }_{c}\right)=\\ & \left[\begin{array}{ccc}\mathit{{\mathrm{cos}}}{\beta }_{c}& 0& {\mathit{{\mathrm{sin}}}\gamma }_{c}\\ \mathit{{\mathrm{sin}}}{\beta }_{c}\cdot \mathit{{\mathrm{sin}}}{\gamma }_{c}& \mathit{{\mathrm{cos}}}{\gamma }_{c}& -\mathit{{\mathrm{sin}}}{\beta }_{c}\cdot \mathit{{\mathrm{cos}}}{\gamma }_{c}\\ -\mathit{{\mathrm{cos}}}{\beta }_{c}\cdot \mathit{{\mathrm{sin}}}{\gamma }_{c}& \mathit{{\mathrm{sin}}}{\beta }_{c}& \mathit{{\mathrm{cos}}}{\beta }_{c}\cdot \mathit{{\mathrm{cos}}}{\gamma }_{c}\end{array}\right]。\end{aligned} $ (12)

式中:$ {\beta }_{c} $${\gamma }_{c} $分别为相机绕$ {X}_{c} $轴和$ {Y}_{c} $轴旋转的角度。

由式(11)、式(12)相乘可以得到相机坐标系到无人机坐标系的旋转矩阵:

$ \begin{aligned} {\boldsymbol{R}}_{\boldsymbol{c2}\boldsymbol{d}}=& {\boldsymbol{R_{1}}}\cdot {\boldsymbol{R}}_{\boldsymbol{c}}= \\ & \left[\begin{array}{ccc}-\mathit{{\mathrm{cos}}}{\beta }_{c}\cdot \mathit{{\mathrm{sin}}}{\gamma }_{c}& \mathit{{\mathrm{sin}}}{\beta }_{c}& \mathit{{\mathrm{cos}}}{\beta }_{c}\cdot \mathit{{\mathrm{cos}}}{\gamma }_{c}\\ \mathit{{\mathrm{cos}}}{\beta }_{c}& 0& {\mathit{{\mathrm{sin}}}\gamma }_{c}\\ \mathit{{\mathrm{sin}}}{\beta }_{c}\cdot \mathit{{\mathrm{sin}}}{\gamma }_{c}& \mathit{{\mathrm{cos}}}{\gamma }_{c}& -\mathit{{\mathrm{sin}}}{\beta }_{c}\cdot \mathit{{\mathrm{cos}}}{\gamma }_{c}\end{array}\right]。\end{aligned} $ (13)

同理可以得到无人机坐标系到大地坐标系的旋转矩阵:

$ \begin{aligned} &{\boldsymbol{R}}_{\boldsymbol{d2}\boldsymbol{w}}= {\boldsymbol{R_{2}}}\cdot {\boldsymbol{R}}_{\boldsymbol{d}}= \\ & \left[ \begin{array}{ccc}{\alpha }_{d} \cdot {\beta }_{d} \cdot \mathrm{cos}{\gamma }_{d} + \mathrm{sin}{\gamma }_{d} & {-\alpha }_{d} \cdot {\beta }_{d} \cdot \mathrm{sin}{\gamma }_{d} + \mathrm{cos}{\gamma }_{d} & -{\alpha }_{d}\\ \mathrm{cos}{\gamma }_{d} & - \mathrm{sin}{\gamma }_{d} & {\beta }_{d}\\ {\beta }_{d} \cdot \mathrm{cos}{\gamma }_{d}-{\alpha }_{d} \cdot \mathrm{sin}{\gamma }_{d} & -{\beta }_{d}\cdot \mathrm{sin}{\gamma }_{d} - {\alpha }_{d} \cdot \mathrm{cos}{\gamma }_{d} & -1\end{array} \right]。\end{aligned} $ (14)

式中:$ {\alpha }_{d} $${\beta }_{d} $${\gamma }_{d} $分别为相机绕$ {X}_{d} $轴、$ {Y}_{d} $轴和$ {Z}_{d} $旋转的角度。

由式(8)、式(13)可以得到着陆平面相对于无人机的旋转矩阵:

$ \begin{array}{c}{\boldsymbol{R}}_{\boldsymbol{b2}\boldsymbol{d}}={{\boldsymbol{R}}_{\boldsymbol{c2}\boldsymbol{d}}\cdot \boldsymbol{R}}_{\boldsymbol{b2}\boldsymbol{c}}\end{array}。$ (15)

由式(4)、式(5)、式(15)可以得到无人机坐标系下着陆平面的法向量为:

$ \begin{array}{c}{\boldsymbol{N}}^{\mathit{\text{'}}}={\boldsymbol{R}}_{\boldsymbol{b2}\boldsymbol{d}}\cdot {\boldsymbol{N}}\end{array}。$ (16)

由式(5)、式(9)、式(13)可以得到无人机坐标系下着陆平面中心与无人机坐标系的位移向量:

$ \begin{array}{c} {\boldsymbol{T}}_{\boldsymbol{d2}\boldsymbol{b}}={\boldsymbol{R}}_{\boldsymbol{c2}\boldsymbol{d}}\cdot \left[{\boldsymbol{R}}_{\boldsymbol{b2}\boldsymbol{c}}\cdot \left[\begin{array}{c}{x}_{0}\\ {y}_{0}\\ {z}_{0}\end{array}\right]+{\boldsymbol{T}}_{\boldsymbol{c2}\boldsymbol{b}}\right]+{\boldsymbol{T}}_{\boldsymbol{d2}\boldsymbol{c}} \end{array}。$ (17)

式中:$ {\boldsymbol{T}}_{\boldsymbol{d2}\boldsymbol{c}} $为相机坐标系相对于无人机坐标系的位移位移向量。

由于着陆平面中心$ {O}_{b} $坐标均为0,式(17)可化简为:

$ \begin{array}{c}{\boldsymbol{T}}_{\boldsymbol{d2}\boldsymbol{b}}={\boldsymbol{R}}_{\boldsymbol{c2}\boldsymbol{d}}{\cdot \boldsymbol{T}}_{\boldsymbol{c2}\boldsymbol{b}}+{\boldsymbol{T}}_{\boldsymbol{d2}\boldsymbol{c}} \end{array} 。$ (18)

由式(16),式(18)可以得到无人机坐标系到着陆平面的距离与到着陆平面中心的距离分别为:

$ \begin{array}{c}D=\displaystyle \frac{\left|{\boldsymbol{N}}'\cdot {\boldsymbol{T}}_{\boldsymbol{d}2\boldsymbol{b}}\right|}{\left|{\boldsymbol{N}}'\right|}\end{array},$ (19)
$ \begin{array}{c}d=\left|{\boldsymbol{T}}_{\boldsymbol{d2}\boldsymbol{b}}\right|\end{array}。$ (20)
2 实验验证方案 2.1 数值仿真环境

基于PX4 Drone Autopilot平台对无人机的动力学响应进行模拟,基于波浪谱使用六自由度运动平台对无人艇的动力学响应进行模拟,基于机器人操作系统(Robot Operation System, ROS)对无人机的数据采集系统和控制系统进行模拟,并在Gazebo中进行模拟。上述算法通过Python编写ROS节点文件进行实时处理与计算。其中,无人机采集Gazebo虚拟环境中的图像信息,通过本文视觉算法对着陆平台相对于无人机的位置进行测算,作为测量值。同时采集无人机和着陆平台的实时位置坐标,计算着陆平台相对于无人机随体坐标系的位置,作为目标值,进行对比。

目标值的计算公式为:

$ \begin{array}{c}\left[\begin{array}{c}{x}_{t}\\ {y}_{t}\\ {z}_{t}\end{array}\right]={\boldsymbol{R}}_{\boldsymbol{d2}\boldsymbol{w}}^{-1}\cdot \left[\begin{array}{c}{x}_{p}-{x}_{d}\\ {y}_{p}-{y}_{d}\\ {z}_{p}-{z}_{d}\end{array}\right]\end{array}。$ (21)

式中:$ {x}_{p}{\text{、}}{y}_{p}{\text{、}}{z}_{p} $$ {x}_{d}{\text{、}}{y}_{d}{\text{、}}{z}_{d} $分别为着陆平台和无人机的实时位置坐标;$ \boldsymbol{R}_{\boldsymbol{d2w}} $为从无人机坐标系到大地坐标系的旋转矩阵。

Gazebo虚拟仿真环境如图4所示,在数值仿真环境中对算法进行测试,每个工况中,在无人机起飞后,基于PID算法控制其环绕着陆平台进行圆周运动,无人机运动轨迹如图5所示,同时控制无人机相机云台,使镜头始终朝向着陆平台[21]

图 4 无人机与停靠平台仿真环境 Fig. 4 Simulation environment of unmanned aircraft and docking platform

图 5 无人机3D轨迹图 Fig. 5 UAV 3D trajectory chart
2.2 相机标定

在进行实验前需要对无人机的相机进行标定。基于张正友标定法[22]对无人机相机的内部参数进行标定。相机标定的过程包括拍摄不同角度下的网格纸图像,并利用图像中的角点信息来计算相机的焦距、主点坐标和畸变系数等内部参数。这些参数用于将图像中的像素坐标转换为实际世界中的物理坐标。

2.3 实验工况

表1所示,实验工况包含平台单自由度运动和平台多自由度运动2个部分,旨在评估在不同运动状态下的视觉定距算法的精度。平台单自由度运动工况包括垂荡、横摇和纵摇这3种运动,每种运动模式下有8、12、16 s这3个运动周期。具体而言,垂荡运动的振幅范围从0.1 ~0.5 m,而横摇和纵摇运动的幅度在0.2 ~0.28 rad之间变化。平台多自由度运动工况将垂荡、横摇和纵摇这3种运动结合在一起,也包含8、12、16 s这3个运动周期,平台运动中垂荡运动固定为0.3 m,横摇和纵摇运动的幅度均为0.24 rad。

表 1 实验工况 Tab.1 Experimental conditions
3 结果与分析 3.1 无人艇单自由度运动下的视觉定距结果分析

本节给出垂荡运动、横摇运动和纵摇运动下测量值与目标值的时历曲线,以时历的峰值和谷值的平均差值作为判断标准,计算相对误差。

3.1.1 垂荡运动

图6(a)~图6(c)所示,在运动周期为8、12、16 s时,目标值与测量值的横坐标趋势相似且有相近的波峰和波谷。同样地,目标值与测量值的纵坐标也表现出相似的趋势,伴随着相近的波峰和波谷,同时目标值与测量值的横纵坐标在−2 ~2 m之间周期性波动,符合无人机的运动轨迹。

图 6 运动周期为8、12、16 s时垂荡运动测量值与目标值时历曲线 Fig. 6 Time history curves of heave motion measurements and target values at motion cycles of 8 s, 12 s and 16 s

表2所示,目标值与测量值的横坐标平均峰谷差值相对误差较小,15个工况中有4个工况的平均峰谷差值相对误差超过10%,最大误差26.82%,最小误差0.10%,平均相对误差为8.31%。目标值与测量值的纵坐标平均峰谷差值相对误差也较小,15个工况中有5个工况的平均峰谷差值相对误差超过10%,最大误差20.69%,最小误差0.11%,平均误差为8.74%。目标值与测量值的高度坐标平均峰谷差值相对误差也较小,15个工况中有6个工况的平均峰谷差值相对误差超过10%,最大误差16.34%,最小误差2.49%,平均误差为9.58%

表 2 垂荡运动下目标值与测量值平均峰谷差值相对误差 Tab.2 Relative error of average peak-to-valley difference between target values and measurement values under heave motion
3.1.2 横摇运动

图7(a)~图7(c)所示,在运动周期为8、12、16 s时,目标值与测量值的横坐标与纵坐标趋势相似,且有相近的波峰、波谷和周期,且目标值与测量值的横坐标与纵坐标范围在−2~2 m,符合无人机运动轨迹。测量值高度坐标的波动范围小于目标值高度坐标的波动范围。

图 7 运动周期为8 s、12 s、16 s时横摇运动测量值与目标值时历曲线 Fig. 7 Time history curves of roll motion measurements and target values at motion cycles of 8 s, 12 s, and 16 s

表3所示,目标值与测量值的横坐标平均峰谷差值相对误差较小,15个工况中有3个工况的平均峰谷差值相对误差超过10%,最大误差17.32%,最小误差0.31%,平均相对误差为5.08%。目标值与测量值的纵坐标平均峰谷差值相对误差也较小,15个工况中有3个工况的平均峰谷差值相对误差超过10%,最大误差24.71%,最小误差0.63%,平均相对误差为6.49%。目标值与测量值的高度坐标平均峰谷差值相对误差也较小,15个工况中仅有1个工况的平均峰谷差值相对误差超过10%,最大误差31.62%,最小误差0.24%,平均相对误差为5.23%。

表 3 横摇运动下目标值与测量值平均峰谷差值相对误差 Tab.3 Relative error of average peak-to-valley difference between target values and measurement values under roll motion
3.1.3 纵摇运动

图8所示,在运动周期为8、12、16 s时,目标值与测量值的横纵坐标趋势相近,且波峰、波谷和周期较为相似,目标值与测量值的横纵坐标范围在−2~2 m,符合无人机运动轨迹。测量值高度坐标的波动范围小于目标值高度坐标的高度范围。

图 8 运动周期为8 s、12 s、16 s时纵摇运动测量值与目标值时历曲线 Fig. 8 Time history curves of pitch motion measurements and target values at motion cycles of 8 s, 12 s, and 16 s

表4所示,目标值与测量值的横坐标平均峰谷差值相对误差较小,15个工况中有4个工况的平均峰谷差值相对误差超过10%,最大误差18.67%,最小误差0.32%,平均相对误差为7.23%。目标值与测量值的纵坐标平均峰谷差值相对误差也较小,15个工况中有2个工况的平均峰谷差值相对误差超过10%,最大误差11.52%,最小误差0.65%,平均相对误差为5.05%。目标值与测量值的高度坐标平均峰谷差值相对误差也较小,15个工况中有4个工况的平均峰谷差值相对误差超过10%,最大误差29.53%,最小误差0.77%,平均相对误差为8.06%。

表 4 纵摇运动下目标值与测量值平均峰谷差值相对误差 Tab.4 Relative error of average peak-to-valley difference between target values and measurement values under pitch motion
3.2 无人艇多自由度运动下的视觉定距结果分析

图9所示,在运动周期为8、12、16 s时,目标值和测量值横纵坐标的趋势基本一致。此外,它们的峰值和谷值也表现出相似性,与无人机的运动轨迹相符。

图 9 运动周期为8s、12s、16s时多自由运动测量值与目标值时历曲线 Fig. 9 Time history curves of multi-DOF motion measurements and target values at motion cycles of 8s, 12s, and 16s

表5所示,目标值与测量值的横坐标平均峰谷差值相对误差较小,3个工况中有1个工况的平均峰谷差值相对误差超过10%,最大误差13.49%,最小误差5.79%,平均相对误差为9.24%。目标值与测量值的纵坐标平均峰谷差值相对误差也较小,3个工况中有1个工况的平均峰谷差值相对误差超过10%,最大误差10.64%,最小误差2.69%,平均相对误差为5.62%。目标值与测量值的高度坐标平均峰谷差值相对误差也较小,3个工况中有1个工况的平均峰谷差值相对误差超过10%,最大误差14.99%,最小误差5.02%,平均相对误差为9.29%。

表 5 多自由运动下目标值与测量值平均峰谷差值相对误差 Tab.5 Relative error of average peak-to-valley difference between target values and measurement values under multi-DOF motion
4 结 语

本文提出了一种用于无人机与无人艇之间的单目距离测量方法,该方法在通过标志物阵列对无人艇甲板进行平面拟合,能够在无人艇大倾角摇晃的情况下准确测量无人机与无人艇之间的距离。研究结果表明,该方法的距离测量结果表现良好,相对误差始终保持在10%以内。总的来说,本研究为无人机与无人艇之间的距离测量提供了一种新方法,特别是在无人艇摇晃等复杂环境中,这一方法有望在军事任务中提升战术感知和无人系统的协同作战能力,对于提高整体作战效能具有潜在价值。

参考文献
[1]
徐博, 王朝阳. 基于无人艇跨域异构编队协同导航研究进展与未来趋势[J]. 中国舰船研究, 2022, 17(4): 1-11,56.
[2]
吴蔚楠, 崔乃刚, 郭继峰, 等. 多异构无人机任务规划的分布式一体化求解方法[J]. 吉林大学学报 (工学版), 2018, 48(6): 1827-1837.
[3]
MURPHY R R, STEIMLe E, GRIFFIN C, et al. Cooperative use of unmanned sea surface and micro aerial vehicles at Hurricane Wilma[J]. Journal of Field Robotics, 2008, 25(3): 164-180. DOI:10.1002/rob.20235
[4]
周思全, 化永朝, 董希旺, 等. 面向空地协同作战的无人机-无人车异构时变编队跟踪控制[J]. 航空兵器, 2019, 26(4): 54-59. DOI:10.12132/ISSN.1673-5048.2019.0149
[5]
GOMES J, URBANO P, CHRISTENSEN A L. Evolution of swarm robotics systems with novelty search[J]. Swarm Intelligence, 2013, 7: 115-144. DOI:10.1007/s11721-013-0081-z
[6]
徐小斌, 段海滨, 曾志刚, 等. 无人机/无人艇协同控制研究进展[J]. 航空兵器, 2021, 27(6): 1-6. DOI:10.12132/ISSN.1673-5048.2021.0145
[7]
李凌昊, 张晓晨, 王浩, 等. 海上异构无人装备一体化协同作战架构[J]. 舰船科学技术, 2019, 41(12): 50-53
[8]
KANELLAKIS C, NIKOLAKOPOULOS G. Survey on computer vision for UAVs: Current developments and trends[J]. Journal of Intelligent & Robotic Systems, 2017, 87: 141-168.
[9]
AL-KAFF A, MARTIN D, GARCIA F, et al. Survey of computer vision algorithms and applications for unmanned aerial vehicles[J]. Expert Systems with Applications, 2018, 92: 447-463. DOI:10.1016/j.eswa.2017.09.033
[10]
HOFMANN-W B, LICHTENEGGER H, W E. GNSS–global navigation satellite systems: GPS, GLONASS, Galileo, and more[M]. Springer Science & Business Media, 2007.
[11]
LEUTENEGGER S, CHLI M, SIEGWART R Y. BRISK: Binary robust invariant scalable keypoints[C]//2011 International conference on computer vision. Ieee, 2011: 2548-2555.
[12]
ALCANTARILLA P F, BARTOLI A, DAVISON A J. KAZE features[C]//Computer Vision–ECCV 2012: 12th European Conference on Computer Vision, Florence, Italy, October 7-13, 2012, Proceedings, Part VI 12. Springer Berlin Heidelberg, 2012: 214-227.
[13]
ALCANTARILLA P F, SOLUTIONS T. Fast explicit diffusion for accelerated features in nonlinear scale spaces[J]. IEEE Trans. Patt. Anal. Mach. Intell, 2011, 34(7): 1281-1298.
[14]
TRIGGS B, MCLAUCHLAN P F, HARTLEY R I, et al. Bundle adjustment—a modern synthesis[C]//Vision Algorithms: Theory and Practice: International Workshop on Vision Algorithms Corfu, Greece, September 21–22, 1999 Proceedings. Springer Berlin Heidelberg, 2000: 298-372.
[15]
NISTÉR D, NARODITSKY O, BERGEN J. Visual odometry[C]//Proceedings of the 2004 IEEE Computer Society Conference on Computer Vision and Pattern Recognition, 2004. CVPR 2004. IEEE, 2004.
[16]
FORSTER C, PIZZOLI M, SCARAMUZZA D. SVO: Fast semi-direct monocular visual odometry[C]//2014 IEEE international conference on robotics and automation (ICRA). IEEE, 2014: 15-22.
[17]
GUI J, GU D, WANG S, et al. A review of visual inertial odometry from filtering and optimisation perspectives[J]. Advanced Robotics, 2015, 29(20): 1289-1301. DOI:10.1080/01691864.2015.1057616
[18]
KRIZHEVSKY A, SUTSKEVER I, HINTON G E. ImageNet classification with deep convolutional neural networks[J]. Communications of the ACM, 2017, 60(6): 84-90. DOI:10.1145/3065386
[19]
GOFORTH H, LUCEY S. GPS-denied UAV localization using pre-existing satellite imagery[C]//2019 International conference on robotics and automation (ICRA). IEEE, 2019: 2974−2980.
[20]
D'HAEYER J P F. Gaussian filtering of images: A regularization approach[J]. Signal processing, 1989, 18(2): 169-181. DOI:10.1016/0165-1684(89)90048-0
[21]
LIM J. Mavros_Controllers—Aggressive Trajectory Tracking Using Mavros for PX4 Enabled Vehicles[J]. 2019.
[22]
ZHANG Z. Flexible camera calibration by viewing a plane from unknown orientations[C]//Proceedings of the seventh IEEE international conference on computer vision. IEEE, 1999, 1: 666−673.