本发明涉及智能海上机器人技术领域，尤其是涉及一种基于视觉信息融合的无人机分阶段自主着艇方法。
背景技术：
随着科技的进步，无人系统在农业、电力、海洋等专业领域的应用越来越广泛。无人机(unmannedaerialvehicle，uav)作为无人系统中的“宠儿”，近年来，其发展速度和应用领域得到不断推进。
在海洋领域，续航能力有限但搜索范围广阔的无人机通常被装备在续航能力较强但搜索范围较小的无人艇上，形成优势互补的机艇协同编队，用来完成海上救援、环境监测、战场侦察等任务，其核心技术是舰载无人机自主航行技术。
起飞、悬停和着艇是舰载无人机自主航行技术的三个基本问题。其中，无人机自主着艇是最有挑战性的问题。在着艇过程中，无人机要面对移动和摆动的着陆平台，进行有效的地标识别和精确的位姿控制。目前国际上在无人机自主着艇技术方面研究还很少，主要的挑战体现在两个方面：
其一，无人机自主着艇的导航精度。小型无人机的gps往往只有米级精度，无法满足着艇任务精度要求；ins/gps组合导航系统只能定位无人机自身位姿，缺少无人机与着艇平台的相对位姿。虽然目前很多文献都提出了利用计算机视觉进行辅助导航，但它们的图像处理过程运算量较大，一般的机载计算机运算能力有限，由视觉导航给出的位姿往往落后于实时位姿。因此，如何在提高导航精度的同时确保位姿信息的实时性和有效性，成为无人机自主着艇过程中亟需解决的一个问题。
其二，无人机着艇末段的安全性。在着艇过程中，有些文献直接使用地标的中心点作为参考位置信号进行控制。然而这样容易使摄像头失去对地标的视野，尤其在无人艇上，地标位置并不是固定的。此外，由于翼地效应(运动物体贴近地面运行时，地面对物体产生的空气动力干扰)，无人机在贴近无人艇着陆平台时难以保持稳定，使得上述方法在应用时成功率并不高，无人机着艇末段的安全性有待提升。
技术实现要素：
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于视觉信息融合的无人机分阶段自主着艇方法。
本发明的目的可以通过以下技术方案来实现：
一种基于视觉信息融合的无人机分阶段自主着艇方法，包括以下步骤：
1)地标制作：将无人艇上相应目标着陆点作为地标，在地标上贴置apriltags标签，并调整无人机摄像头的角度，保证无人机在接近地标时能探测到地标；
2)图像处理：根据摄像头的参数信息和摄像头捕捉到的图像信息计算在无人机机载计算机中配置的apriltags视觉基准系统的参数，在发现地标时获取摄像机和地标间的相对位姿xct；
3)信息融合：将摄像机和地标间的相对位姿xct与imu的测量数据进行信息融合后，获取无人艇在无人机参考系下的实时相对位姿xvs；
4)运动控制：根据实时相对位姿xvs采用嵌套控制方式保证飞行稳定以及路径跟踪，同时采用分阶段的着艇方法进行着艇。
所述的步骤2)中apriltags视觉基准系统的参数包括宽方向上以像素为单位的焦距fw、高方向上以像素为单位的焦距fh，图像的中心位置(cw,ch)，其计算公式为：
其中，f和c分别为不同方向上的焦距和位置，lfocus和lreceptor分别为焦距和感光元件尺寸，npixel为像素数量，利用此公式分别对宽度方向和高度方向的参数进行计算即可。
所述的步骤3)具体包括以下步骤：
31)系统状态估计，包括两个阶段，第一阶段为未检测到地标阶段，第二阶段为检测到地标后；
当未检测到地标时，系统状态为imu提供的位姿信息xv，即x＝[xv]，对该系统状态进行估计，则有：
其中，
δt为采样间隔，wk-1为高斯白噪声；
自首次检测到地标开始，在系统状态中加入无人艇状态xs，即x＝[xvxs]，并更新卡尔曼滤波参数fk-1，则有：
其中，
32)获取imu和摄像头的观测结果，其中，imu的观测结果himu为：
其中，zlv为高度，φlv,θlv,ψlv分别为绕x,y,z三个运动方向的旋转角度，ulv和vlv分别为前向速度和侧向速度，其对应jacobian矩阵himu为：
所述的摄像头的观测结果htag(xkf)为：
其中，xvc为无人机参考系下摄像头的位姿，xst为无人艇参考系下标记的位姿，xlv为估计出的无人机的位姿，xls为估计出的无人艇的位姿，
其对应jacobian矩阵htag为：
33)对观测结果进行扩展卡尔曼滤波，得到实时的系统状态。
所述的步骤3)还包括以下步骤：
34)通过无人机和无人艇的历史位姿，在延迟情况下修改系统状态，获取人艇在无人机参考系下的实时相对位姿xvs。
所述的步骤4)中，嵌套控制方式具体为：
采用6个独立的闭环pid控制器进行控制，其中，内环姿态控制用于保证飞行稳定，外环位置控制用于路径跟踪。
所述的步骤4)中，分阶段的着艇方法具体包括以下步骤：
41)发现地标：首次发现地标时，初始化系统状态，并使无人机跟踪该地标；
42)慢速下降阶段：引导无人机至地标的中心处，使其保持在以地标为中心，半径为rloop，高度为hloop的圆柱体空间中，并以δz为步长垂直向下寻找下一个航迹点下降，当丢失地标视野时，则提升δz高度，直到重新发现该地标。
43)进入快速下降阶段：设置第一高度阈值hland，当无人机的高度小于hland时，则认为无需下降，直接准备着陆，若在连续n帧内，该地标都在摄像头的视野中，并满足快速下降条件时，则认为可进入快速下降阶段，直接以0作为z轴的参考信号；
44)着陆阶段：设置第二高度阈值hmin，当无人机高度低于第二高度阈值hmin时，则关闭螺旋桨，使无人机自由落体并完成着陆。
所述的步骤43)中的快速下降条件为：
z-hland≤hloop
||(x,y)||2≤rloop
其中，z为无人机相对于无人艇的高度，||(x,y)||2为无人机与无人艇的水平距离。
所述的步骤44)中，
若无人机在高于第二高度阈值hmin时，摄像头失去了对地标的视野，无论处于慢速下降阶段或快速下降阶段，则拉升无人机，直到重新发现地标。
与现有技术相比，本发明具有以下优点：
一、实时有效：在地标识别和图像处理过程中，采用了成熟开源的apriltags，可降低技术门槛；使用单目摄像头，在保证精度的前提下，简化了运算复杂度，降低了设备成本。
二、避免滞后：将摄像机得到的相对位姿与imu得到的位姿进行信息融合以提高精度；通过增加历史状态避免了图像处理时的滞后问题。
三、稳定安全：将无人机着艇嵌套控制方案和分阶段安全着艇方法进行结合，可避免在着艇过程中无人机摄像头失去对地标的视野，同时可保证无人机在贴近无人艇着陆平台时的稳定性和安全性。
附图说明
图1为本发明中无人机着艇嵌套控制方案框图。
图2为本发明中无人机分阶段安全着艇方法流程图。
图3为本发明中无人机自主着艇方法原理图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。
实施例
以下结合附图3对本发明的技术方案作具体实施描述。
为了说明方便，先做如下符号约定：
xij是指j参考系在i参考系下的位姿，这里的位姿定义为一个6维列向量[xyzφθψ]t，其中(x,y,z)是在参考系中的位置坐标，(φ,θ,ψ)分别是绕x轴、y轴、z轴旋转的角度，称为滚转角、俯仰角和偏航角。用到的参考系有：无人机参考系{v}，本地参考系{l}，无人艇参考系{s}，摄像机参考系{c}，地标参考系{t}。同时定义一些参考系变换的基本符号：若i,j,k代表三个参考系，符号
步骤1：地标制作。打印apriltags中的标签，张贴在无人艇上的相应目标着陆点作为地标。将无人机摄像头的角度调整向前并略微向下，保证无人机在接近地标时能探测到地标。
步骤2：图像处理。在无人机的机载计算机中配置apriltags视觉基准系统。利用摄像头的参数信息和摄像头捕捉到的图像信息，计算4个重要参数，即：宽、高方向上以像素为单位的焦距fw和fh，图像的中心位置(cw,ch)。计算公式如下：
其中，lfocus和lreceptor分别是焦距和感光元件尺寸，以毫米为单位。npixel是像素数量。利用这个公式分别对宽度方向和高度方向的参数进行计算即可。
将计算到的参数传给apriltags视觉基准系统，便能在发现地标时返回摄像机和标记间的相对位姿xct。
步骤3：信息融合。将摄像头和地标间的相对位姿xct与imu的测量数据进行信息融合，得到无人机和无人艇的实时位姿xlv和xls，以及相对位姿xvs。该过程分为以下4个子步骤：
(1)系统状态估计。这里的信息融合分为两个阶段，第一阶段为未检测到地标阶段，第二阶段为检测到地标后。
未检测到地标阶段，系统状态取为imu提供的位姿信息xv＝[xlvvvwv]。其中，vv是x,y,z方向的速度，而wv是绕x,y,z方向的角速度。对该系统状态进行如下估计：
其中，wk-1是高斯白噪声，这里采用匀速模型来预测系统状态，即
δt为采样间隔。
从第一次检测到地标开始，在系统状态中加入无人艇状态，即x＝[xvxs]，xv＝[xlvvvwv]，xs＝[xlsvsws]，于是，fk-1具有如下形式：
这里仅考虑无人艇在水平面上的运动，同样采用匀速模型，有
(2)获得传感器观测结果。imu和摄像头的观测模型都可表示为
z[k]＝h(x)+v[k]。
其中，v[k]～n(0,r[k])是观测噪声。h(x)表示状态x的函数。
imu对无人机的观测结果为无人机的高度、姿态和速度信息，即：
其中，zlv是高度，φlv,θlv,ψlv分别是绕x,y,z三个运动方向的旋转角度。ulv和vlv分别是前向速度和侧向速度。其对应jacobian矩阵为
摄像头对第i个地标的观测结果为
其中，htag(xkf)为摄像头的观测结果，xvc为无人机参考系下摄像头的位姿，xst为无人艇参考系下标记的位姿，
对应有
(3)扩展卡尔曼滤波。使用扩展卡尔曼滤波解决上述非线性滤波问题，由于状态量符合正态分布xkf～n(μ,∑)，其中，协方差矩阵∑表示为
扩展卡尔曼滤波按照如下公式进行更新：
经过扩展卡尔曼滤波，我们就能得到系统的实时状态x＝[xvxs]。
(4)解决状态滞后问题。由于图像计算较复杂，在k时刻，apriltag往往只能给出k-n时刻的计算结果，于是扩展卡尔曼滤波对k+1时刻的状态估计是根据k-n时刻的状态得出的，并不准确。我们通过记录无人机和无人艇的历史位姿来解决这一问题。延迟情况下将系统状态估计量修改为
其中，n是延迟状态的个数。于是系统状态估计的迭代计算公式为
相应地，imu传感器模型的jacobian矩阵修正为
由于上一步骤中的观测结果是对延迟状态进行的，对于第j个延迟状态x[j]，将摄像头的观测模型修正为
上述修正后便能用扩展卡尔曼滤波更新增广后状态xds。每个延迟的观测值被扩展卡尔曼滤波更新后，就将其忽略并移出状态向量。因此，该滤波算法只要额外存储一小段历史状态。
解决状态滞后问题得到滤波后的系统状态信息后，利用无人机和无人艇在本地参考系下的位姿xlv和xls即可求出相对位姿xvs。具体计算方法为
步骤4：运动控制。利用融合后的相对位姿对无人机进行运动控制。采用嵌套控制方案，使用6个独立的闭环pid控制器进行控制，其中内环姿态控制用于保证飞行稳定，外环位置控制用于路径跟踪。同时采用分阶段安全着艇方法。该方法分为以下4个子步骤：
(1)首次发现地标时，初始化系统状态，并让无人机跟踪这个地标。
(2)将无人机引导至地标的中心，然后保持在以地标为中心，半径为rloop，高度为hloop的圆柱体中，并以δz为步长垂直向下寻找下一个航迹点下降。一旦丢失了地标的视野，就往上提升δz，直到重新发现地标，该阶段称为慢速下降阶段。
(3)设置一个较小的高度hland，如果高度小于hland，就认为无需下降，直接准备着陆。如果在连续n帧内地标都在摄像头视野中，并满足
z-hland≤hloop，
||(x,y)||2≤rloop，
则认为可进入快速下降阶段，直接以0作为z轴的参考信号。
其中，z为无人机相对于无人艇的高度，||(x,y)||2为无人机与无人艇的水平距离。
(4)下降到一定高度时，由于近距离时摄像头无法识别地标，也就无法定位。因此，我们设置一个极小的高度hmin，当低于这个高度时，就关闭螺旋桨，让无人机自由落体着陆。但是如果在高于hmin时摄像头失去了对地标的视野，无论是处于快速下降还是慢速下降阶段，都应拉升无人机，直到重新发现地标。