一种旋翼式无人机的视觉着陆位姿估计方法

摘要

本发明提供一种旋翼式无人机的视觉着陆位姿估计方法，将Tsai方法与快速四点方法融合起来计算位姿参数，该融合的方式既可以避免Tsai方法中迭代求解所消耗的时间，也能够提高Tsai方法和快速四点方法中位姿参数计算的精度，并基于该方法设计了一种分级合作目标，该合作目标能适应不同降落高度下的位姿估计，最后进行了大量仿真实验进行方法验证。

说明书

技术领域

本发明属于无人机控制领域，涉及一种旋翼式无人机的视觉着陆位姿估计方法。

背景技术

旋翼式无人机能够实现垂直起降、自由悬停以及低空低速飞行等多种飞行方式，并且具有成本低、功耗小、反应灵活等优点。这些优点使其在复杂环境中的服务和救援等领域都受到了广泛的应用。在无人机的起飞、巡航、悬停、着陆四个阶段中，着陆是其中最为重要的环节。目前，旋翼式无人机的自主着陆导航系统主要有GPS/INS导航系统、视觉导航系统、以及多传感器融合导航系统。其中，利用GPS/INS等信号进行定位导航着陆精度不高，且GPS信号在室内等场景中无法使用，多传感器融合导航成本高，且存在累积误差，而视觉导航具有成本低、精度高、抗干扰性强等优点。

旋翼式无人机的视觉导航大多以在着陆平台放置合作目标的方式进行自主着陆，随着计算机视觉的发展，国内外已有很多利用视觉估计无人机自主着陆位姿的方法。位姿包括无人机相对于着陆点三维距离的位置参数和无人机的俯仰角、滚转角和偏航角的姿态参数。不同的位姿估计方法在时间和精度上都存在着不足，如文献利用Tsai提出的基于RAC的相机标定方法计算无人机的位姿参数，该方法需以迭代过程求解最优解，且通常是求取局部最优解，因此位置参数估计精度与时间效率不高；文献设计了分级策略的图标，利用快速四点方法实现3D位姿估计，该方法能够实现对位姿参数的快速求解，且高度计算精度较高，所需特征较少，但对于姿态角度的计算精度不足。

发明内容

本发明的目的在于解决上述现有技术存在的缺陷，提供一种旋翼式无人机的视觉着陆位姿估计方法，提高了Tsai和快速四点方法估计位姿的精度，能够满足自主降落对无人机位姿参数获取准确性和实时性的要求。

为实现上述目的采用如下技术方案：

一种旋翼式无人机的视觉着陆位姿估计方法，包括以下步骤：

(1)设计合作目标；

(2)特征提取与标记：

通过机载视觉设备采集合作目标图像，图像采集后进行合作目标的特征提取，特征提取包括图像预处理、目标域划分、特征点提取；

(2)位姿估计方法

采用Tsai方法和快速四点方法两种位姿估计方法的融合算法，根据合作目标的图像坐标与世界坐标信息，利用Tsai方法计算无人机相对于合作目标坐标系的姿态角、快速四点方法计算无人机相对位置参数。

进一步，所述的合作目标，考虑以下3点：

(1)合作目标易区别于其它物体和环境，特征明显，易于提取与识别；

(2)合作目标中至少需包含5个特征点。，且每三个互不共线，以及其中四点能形成矩形；

(3)合作目标的尺寸能适应无人机降落高度以及舰船等运动无人机着陆平台上的要求，在降落距离范围内能够使用的目标图像占整幅图像的10％-50％。

进一步，图像预处理、目标域划分、特征点提取具体流程如下：

(1)图像预处理：使用最大类间方差(Otsu)算法求取最佳阈值对图像进行自动阈值分割，获取二值图像；对二值图像进行连通区域提取，去掉面积较小和长宽比相差过大的连通域，若无连通域，则认定图像中无合作目标，调整无人机姿态重新获取图像检索目标；

(2)目标域划分：利用每级六边形合作目标质心接近的特点，计算剩余连通区域的质心距离D_i,j

D_i,j＝[(x_i-x_j)²+(y_i-y_j)²]^1/2>

若其质心距离D_i,j小于设定阈值，则认定该连通区域为合作目标；进行合作目标二次筛选后，利用合作目标固定的面积比确定合作目标等级label；

(3)特征点提取：确定着陆目标等级后，先对目标进行LSD直线提取[8]，利用图像的梯度幅值和方向检测直线l；若该级六边形目标提取的直线数量N_l＜6，则选取label+1等级目标重新提取直线；若直线数量N_l≥6，再通过颜色信息对提取的直线进行标记；对标记的直线求交点并按顺序排列p(u_i,v_i),i＝1,2,...6，作为计算位姿参数的特征点。

进一步，Tsai姿态估计具体方法为：

根据摄像机坐标系与世界坐标系关系式得到

$>\left(\begin{array}{c}x=r_1{x}_w+r_2{y}_w+r_3{z}_w+T_x\\ y=r_4{x}_w+r_5{y}_w+r_6{z}_w+T_y\\ z=r_7{x}_w+r_8{y}_w+r_9{z}_w+T_z\end{array}\right)---\left(3\right)$>

由径向约束关系(RAC)得

$>\frac{x}{y}=\frac{x_c}{y_c}=\frac{r_1{x}_w+r_2{y}_w+r_3{z}_w+T_x}{r_4{x}_w+r_5{y}_w+r_6{z}_w+T_y}---\left(4\right)$>

设定合作目标所在平面z_w＝0，则上式可整理得

$>\left[\begin{array}{ccccc}{x}_w{y}_c& y_w{y}_c& y_c& -x_w{x}_c& -y_w{x}_c\end{array}\right]\left(\begin{array}{c}{r}_1/T_y\\ r_2/T_y\\ T_x/T_y\\ r_4/T_y\\ r_5/T_y\end{array}\right)=x_c---\left(5\right)$>

每个特征点都可列出对应式(5)的方程，则用最小二乘法解超定方程组，N≥5得到未知参数，利用旋转矩阵的性质可求得旋转矩阵R_CW：

进而求得俯仰角θ＝arcsin(-r₃)、偏航角ψ＝arcsin(r₂/cosθ)、翻滚角

进一步，快速四点位置估计具体方法为：

利用像顶点坐标p_c(x_c，y_c，z_c)，求得直线l在像平面S_C内的极坐标方程xcosθ+ysinθ＝ρ与平面的平面方程及平面法向量

$>A_{i}x+B_{i}y+C_{i}z=0,\overrightarrow{n_i}=(A_i,B_i,C_i),i=1,2,3,4---\left(7\right)$>

由于A、B、D、E分别是以及S_ABDE平面的交点，S_ABDE平面方程为Ax+By+Cz＝1，带入平面方程便可求出其坐标可表示为

$>P_{i|C}(X_C,Y_C,Z_C)=k·\left(\begin{array}{ccc}\frac{w_{i1}}{\left|W_i\right|}& \frac{w_{i2}}{\left|W_i\right|}& \frac{w_{i3}}{\left|W_i\right|}\end{array}\right),i=1,2,3,4,k=1/C$>

$>\begin{array}{cc}{W}_i=\left(\begin{array}{ccc}m& n& 1\\ A_i& B_i& C_i\\ A_{i+1}& B_{i+1}& C_{i+1}\end{array}\right)& m=\frac{A}{C},n=\frac{B}{C}\end{array}---\left(8\right)$>

其中，w_ij是W_i的代数余子式；

四面体ABDO_C、BDEO_C、ADEO_C、ABEO_C的体积为V_i，S_d为ABDE的面积，h为坐标原点O_C到平面S_ABDE的距离，则四棱锥ABDEO_C的体积

$>V=\underset{i=1}{\overset{4}{\Sigma }}{V}_i=\frac{1}{3}{S}_{d}h---\left(9\right)$>

根据透视关系和立体几何原理将已知量S_d带入体积公式求出h以及矩形的4个顶点在摄像机坐标系下的坐标P_C(X_C，Y_C，Z_C)；

在上述步骤中，世界坐标系和摄像机坐标系的旋转矩阵R已求出，根据O_W在摄像机坐标系和世界坐标系下的不同坐标值P_C和P_W的转化关系得相机到合作目标的相对位置T_CW＝[T_x，T_y，T_z]：

T_CW＝P_W-R_CWP_C(10)

其中P_C＝[0,0,0]^T，

本发明旋翼式无人机的视觉着陆位姿估计方法，将Tsai方法与快速四点方法融合起来计算位姿参数，该融合的方式既可以避免Tsai方法中迭代求解所消耗的时间，也能够提高Tsai方法和快速四点方法中位姿参数计算的精度，并基于该方法设计了一种分级合作目标，该合作目标能适应不同降落高度下的位姿估计，最后进行了大量仿真实验进行方法验证。

附图说明

图1分级合作目标设计和特征点标记

(a)合作目标等级标识

(b)特征点顺序标记

图2特征提取算法流程图

图3不同位姿下的图像处理结果

图4合作目标投影成像示意图

(a)偏航角；(b)X平移距离；(c)俯仰角；(d)Y平移距离；(e)翻滚角；(f)Z平移距离；

图5三种位姿估计方法的计算结果对比

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明根据上述两种基于投影关系的位姿估计方法对图像特征的要求以及普通合作目标对无人机适用距离限制的情况，在设计合作目标时，考虑了以下3点：(1)合作目标易区别于其它物体和环境，特征明显，易于提取与识别；(2)合作目标中至少需包含5个特征点(每三个互不共线)以及其中四点能形成矩形；(3)合作目标的尺寸能适应无人机降落高度以及舰船等运动无人机着陆平台上的要求，在降落距离范围内可使用的目标图像占整幅图像的10％-50％。

基于对上述条件的考虑和已有的合作目标，设计了如图1(a)所示的合作目标。该合作目标采用分级策略，将尺寸大小不同的标准六边形合并，并按尺寸大小划分等级，每一等级目标的6个顶点适应不同距离下的位姿估计，如图1(b)所示。对于2m-100m范围内的着陆高度，共设计了5级不同大小的标准六边形作为合作目标，其尺寸根据相机尺寸、焦距、无人机着陆范围等参数计算，该图标采用白色打底，用蓝、绿、红三种颜色进行特征点的顺序标记，定义蓝色为合作目标正方向。该图标可用相应颜色的LED发光板替代，从而进行不同光照下的自主着陆。

特征提取与标记

在无人机的视觉自主着陆过程中，采用合作目标形式的视觉导航需将无人机粗定位到降落区域内，通过机载视觉设备采集合作目标图像，图像采集后进行合作目标的特征提取，其特征点的提取精度直接影响位姿估计算法的精度。特征提取主要分为三部分：图像预处理、目标域划分、特征点提取。主要流程如下图2所示。

(1)图像预处理：使用最大类间方差(Otsu)算法求取最佳阈值对图像进行自动阈值分割，获取二值图像；对二值图像进行连通区域提取，去掉面积较小和长宽比相差过大的连通域，若无连通域，则认定图像中无合作目标，调整无人机姿态重新获取图像检索目标。

(2)目标域划分：利用每级六边形合作目标质心接近的特点，计算剩余连通区域的质心距离D_i,j

D_i,j＝[(x_i-x_j)²+(y_i-y_j)²]^1/2>

若其质心距离D_i,j小于设定阈值，则认定该连通区域为合作目标；进行合作目标二次筛选后，利用合作目标固定的面积比确定合作目标等级label。

(3)特征点提取：确定着陆目标等级后，先对目标进行LSD直线提取[8]，利用图像的梯度幅值和方向检测直线l；若该级六边形目标提取的直线数量N_l＜6，则选取label+1等级目标重新提取直线；若直线数量N_l≥6，再通过颜色信息对提取的直线进行标记；对标记的直线求交点并按顺序排列p(u_i,v_i),i＝1,2,...6，作为计算位姿参数的特征点。

不同高度的特征提取结果如图3所示，结果表明在不同高度下，均有不同等级的完整合作目标提供足够的特征用于计算位姿。

位姿估计方法

本发明采用Tsai方法和快速四点方法两种位姿估计方法的融合算法，根据合作目标的图像坐标与世界坐标信息，利用Tsai方法计算无人机相对于合作目标坐标系的姿态角、快速四点方法计算无人机相对位置参数，该融合方法具有以下优点：

(1)利用Tsai方法中的径向排列一致约束(RAC)建立坐标关系的方程求得姿态角，具有快速、误差小的优点，同时避免了Tsai方法中利用搜索优化方式估计位置参数时，无法全局优化收敛到最优解且耗时长等问题；

(2)在计算出姿态角的情况下，使用快速四点方法通过四个特征点与摄像机的投影几何关系计算高度，其计算精度较高，且避免了该方法在计算姿态角的过程中对特征点坐标提取精度过度依赖，导致精度不足的问题；

(3)融合方法所需特征较少，且全过程使用线性方程求解，运算量低，能够实现实时的位姿估计。本发明的融合算法步骤如下：

本发明合作目标(六边形)共有6个共面顶点进行姿态估计，由理想的透视投影关系，空间特征点世界坐标为P_i|w(x_w，y_w，z_w),i＝1,2,...6，其中z_w＝0，投影到摄像机图像坐标为p(u_i,v_i),i＝1,2,...6(简称像顶点)，示意图如图4所示。利用摄像机已标定的内参矩阵K，将像顶点的图像坐标p(u_i,v_i)转化为其在摄像机坐标系下的坐标

P_i|C(x_c，y_c，z_c)＝fK^-1(u_i,v_i),i＝1,2,...6

(2)

4.1Tsai姿态估计

根据摄像机坐标系与世界坐标系关系式可以得到

$>\left(\begin{array}{c}x=r_1{x}_w+r_2{y}_w+r_3{z}_w+T_x\\ y=r_4{x}_w+r_5{y}_w+r_6{z}_w+T_y\\ z=r_7{x}_w+r_8{y}_w+r_9{z}_w+T_z\end{array}\right)---\left(3\right)$>

由径向约束关系(RAC)可得

$>\frac{x}{y}=\frac{x_c}{y_c}=\frac{r_1{x}_w+r_2{y}_w+r_3{z}_w+T_x}{r_4{x}_w+r_5{y}_w+r_6{z}_w+T_y}---\left(4\right)$>

由于本发明设定合作目标所在平面z_w＝0，则上式可整理得

$>\left[\begin{array}{ccccc}{x}_w{y}_c& y_w{y}_c& y_c& -x_w{x}_c& -y_w{x}_c\end{array}\right]\left(\begin{array}{c}{r}_1/T_y\\ r_2/T_y\\ T_x/T_y\\ r_4/T_y\\ r_5/T_y\end{array}\right)=x_c---\left(5\right)$>

每个特征点都可列出对应式(5)的方程，则用最小二乘法解超定方程组(N≥5)得到未知参数，利用旋转矩阵的性质可求得旋转矩阵R_CW：

进而求得俯仰角θ＝arcsin(-r₃)、偏航角ψ＝arcsin(r₂/cosθ)、翻滚角

4.2快速四点位置估计

利用像顶点坐标p_c(x_c，y_c，z_c)，求得直线l在像平面S_C内的极坐标方程xcosθ+ysinθ＝ρ与平面的平面方程及平面法向量

$>A_{i}x+B_{i}y+C_{i}z=0,\overrightarrow{n_i}=(A_i,B_i,C_i),i=1,2,3,4---\left(7\right)$>

由于A、B、D、E分别是以及S_ABDE平面的交点，S_ABDE平面方程为Ax+By+Cz＝1，带入平面方程便可求出其坐标可表示为

$>P_{i|C}(X_C,Y_C,Z_C)=k·\left(\begin{array}{ccc}\frac{w_{i1}}{\left|W_i\right|}& \frac{w_{i2}}{\left|W_i\right|}& \frac{w_{i3}}{\left|W_i\right|}\end{array}\right),i=1,2,3,4,k=1/C$>

$>\begin{array}{cc}{W}_i=\left(\begin{array}{ccc}m& n& 1\\ A_i& B_i& C_i\\ A_{i+1}& B_{i+1}& C_{i+1}\end{array}\right)& m=\frac{A}{C},n=\frac{B}{C}\end{array}---\left(8\right)$>

其中，w_ij是W_i的代数余子式。

四面体ABDO_C、BDEO_C、ADEO_C、ABEO_C的体积为V_i。S_d为ABDE的面积，h为坐标原点O_C到平面S_ABDE的距离，则四棱锥ABDEO_C的体积

$>V=\underset{i=1}{\overset{4}{\Sigma }}{V}_i=\frac{1}{3}{S}_{d}h---\left(9\right)$>

根据透视关系和立体几何原理将已知量S_d带入体积公式便可求出h以及矩形的4个顶点在摄像机坐标系下的坐标P_C(X_C，Y_C，Z_C)。

在上述步骤中，世界坐标系和摄像机坐标系的旋转矩阵R已求出，根据O_W在摄像机坐标系和世界坐标系下的不同坐标值P_C和P_W的转化关系可得相机到合作目标的相对位置T_CW＝[T_x，T_y，T_z]：

T_CW＝P_W-R_CWP_C(10)

其中P_C＝[0,0,0]^T，

实验结果及分析

为验证上述提出的视觉位姿估计方法的有效性，将二轴云台与摄像机结合，从不同位姿拍摄位于水平面的合作目标，并利用融合的位姿估计算法进行了大量试验与对比。实验采用的摄像机为Baumer高清面阵CCD摄像机、16mm定焦镜头，图像大小为1292×960像素，使用Matlab2014a在配置为Intel(R)Pentium(R)CPU G2030@3.00GHz内存4GB的计算机上编程运算。

考虑旋翼式无人机的垂直起降功能，实验中，俯仰角和翻滚角的角度变化范围为[-30°～30°]，步长为1.3°，距离变化范围为[7m～50m]，我们选取10m、20m、28m、36m、44m五个高度进行结果统计，每个高度点选取不同位姿参数的20组数据统计误差。具体计算结果见表1所示。

表1位姿参数估计的平均误差

结果表明，本发明融合方法能够实现不同高度下的位姿参数估计，且角度误差控制在2°以内，位置误差控制在4％以内。为了验证本发明融合方法在位姿估计上相对于其他两种方法的优势，随机抽取140组数据进行三种方法的位姿参数估计的精度与时间效率的对比，结果如表2、表3所示。

表2三种方法位姿参数估计误差结果比较

表3三种位姿估计方法时间效率比较

在精度上，本发明融合方法估计的姿态参数与快速四点方法相比误差分别减少了1.8°、1.3°、4.82°；本发明融合方法估计的位置参数精度较Tsai方法与快速四点方法提高了12.13％、18.85％、13.12％和0.97％、6.49％、1.36％。图5为其中45组数据的位姿参数真实值与估计值。可以看出在位置参数估计上，本发明融合方法效果明显好于Tsai方法。

在时间效率上，如表3Tsai方法由于需迭代求解最优解，耗时较长，难以满足对位姿估计实时性的要求；而融合算法和快速四点方法求解过程无需迭代，耗时较短，能够满足位姿估计的实时性。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。