CN106500699A - 一种适用于无人机室内自主降落的位姿估计方法 - Google Patents

Abstract

本发明公开的一种适用于无人机室内自主降落的位姿估计方法，包括以下步骤：1)图像采集步骤；2)直线段检测步骤；3)合作标志识别步骤；4)相对滚转角估计步骤；5)相对高度估计步骤。本发明采用一种基于单目测量新型合作标志的相对位姿估计方法，可以为无人机自主降落等任务提供滚转角和相对高度等信息，并设计了一种新型合作标志，无需色彩信息，具有指向性，且适用于不同距离情况下的测量，利用一个摄像机对合作标志进行成像，降低了系统复杂度，算法求解上耗时更少，本发明可用于无人机需要定向定位的场合。

Description

一种适用于无人机室内自主降落的位姿估计方法

技术领域

本发明属于无人机飞行控制与机器视觉交叉融合的技术领域，具体涉及一种适用于无人机室内自主降落的位姿估计方法。

背景技术

近年来，无人机得到了快速的发展，具有飞行稳定、易操控、低噪声、安全危害性小等特点，在航空摄影、电力巡检、灾情考察、军事侦察等多个领域得到越来越广的应用。

无人机在室内进行自主起降过程中，某些场景情况下需要自动将无人机引导至指定降落点进行回收。室外经常使用的导航方式有GPS、北斗等，但其在室内均不可用。定高采用的气压计仅可用来做定高度保持；测距传感器如超声波测距仪、激光测距仪，存在测量范围有限、功耗大等问题；且以上传感器均不能实现对已设定降落点的精确识别。

利用机器视觉为无人机相对位姿估计具有诸多好处，如易实现、功耗低、价格便宜。此技术通过摄像头获取合作标志的图像，通过图像分析获得无人机相对指定坐标系的相对位姿。从而引导其逐渐平稳降落至指定点。

目前对于非GPS/IMU的无人机相对位姿估计技术，现状如下:

专利申请号为201410526631.7的中国专利申请于2015年1月21日公开了一种旋翼无人机精确视觉定位定向方法，其通过视觉分析比对当前所摄图案的几何特征与基准图案的差异，实现定位定向。该方法中使用了彩色的不规则几何图案作为合作标志，仅利用边长信息估算相对距离存在较大误差。由于仅采用了单个固定尺寸的图案，其适用于定距离测量，而无法克服变距离测量时存在的尺度变化问题。

专利申请号为201310444134.8的中国专利申请于2014年1月1日公开了一种四旋翼无人机的超声波定位装置及方法，其通过为无人机安装一组多个超声波模块，利用超声波测距获得与障碍物间的距离，并利用多个模块同时测距，从而得到无人机的相对位置信息。该方法采用超声波传感器测距，超声波传感器测距范围有限、而且容易受到干扰，该定位装置及方法应用场景有限，存在一定不足。

专利号为201210418160.9的中国专利于2013年3月13日公开了一种基于大小回字标志物的多旋翼无人机位姿获取方法，起通过识别一个指定的印有回字的标识牌，用一系列的运算获得无人机当前的位姿信息。该方法中，回字标志不具有方向特征，难以实现无人机的航向获取与定向控制。

以上文献中对于无人机定位定向的研究，对于引导无人机进行自主降落至指定点的应用中，存在一定不足之处。为此，申请人进行了有益的探索和尝试，找到了解决上述问题的办法，下面将要介绍的技术方案便是在这种背景下产生的。

发明内容

本发明所要解决的技术问题：针对现有的无人机自主降落过程中相对位姿估计技术的不足，而提供一种无需人工干预、计算量小、精度较高的适用于无人机室内自主降落的位姿估计方法。

本发明所解决的技术问题可以采用以下技术方案来实现：

一种适用于无人机室内自主降落的位姿估计方法，包括以下步骤：

1)图像采集步骤，当无人机飞行到合作标志正上方并开始缓慢垂直降落的过程中，固定设置在无人机下方的单目相机对包含合作标志的区域进行采集，得到图像I，图像尺寸宽度为M，高度为N，其中单目相机内参数已标定，焦距f、x和y方向单个像元物理尺寸d_x和d_y均为已知；

2)直线段检测步骤，利用基于梯度生长的直线段检测方法对图像I进行直线段检测，并将长度小于阈值的直线段忽略，将检测出的直线段表示为{l₁,l₂,…,l_m}；

3)合作标志识别步骤，规定一由五条等长的直线段组成的类似于正方形结合三角形的五边形合作标志，并利用检测出的{l₁,l₂,…,l_m}，求解出外圈五个顶点的二维坐标{p₁(x₁,y₁)、p₂(x₂,y₂)、p₃(x₃,y₃)、p₄(x₄,y₄)、p₅(x₅,y₅)}或者内圈五个顶点的二维坐标{p_A(x_A,y_A)、p_B(x_B,y_B)、p_C(x_C,y_C)、p_D(x_D,y_D)、p_E(x_E,y_E)}；

4)相对滚转角估计步骤，构建无人机机体坐标系O_UX_UY_UZ_U和降落点坐标系o_cx_cy_cz_c，在计算机图像坐标系中计算由求解出的五点坐标计算出无人机机体坐标系相对于降落点坐标系的相对滚转角ψ；

5)相对高度估计步骤，由五点坐标求解出计算机图像坐标系中已识别出的合作标志的面积S_i，并根据其与合作标志物理面积的比值大小求解出相对高度h。

在本发明的一个优选实施例中，在所述步骤3)中，所述合作标志识别的具体方法如下：

求取{l₁,l₂,…,l_m}中长度相等的五条直线段的集合，分别可以获得外圈长直线段集合{l_i1,l_i2,l_i3,l_i4,l_i5}和内圈短直线段集合{l_j1,l_j2,l_j3,l_j4,l_j5}，其中，求取外圈长直线段集合{l_i1,l_i2,l_i3,l_i4,l_i5}的各直线段间夹角，如果两条直线段间夹角等于60°，则其交点为顶点p₁(x₁,y₁)；如果两条直线段间夹角等于30°，则其交点为顶点p₂(x₂,y₂)；如果两条直线段间夹角等于150°，则其交点为顶点p₅(x₅,y₅)；如果一条直线段与另外的两条直线段夹角都是90°，则两个交点中与顶点p₂(x₂,y₂)共线的是顶点p₃(x₃,y₃)，与顶点p₅(x₅,y₅)共线的是顶点p₄(x₄,y₄)；求取内圈短直线段集合{l_j1,l_j2,l_j3,l_j4,l_j5}的各直线段间夹角，如果两条直线段间夹角等于60°，则其交点为顶点p_A(x_A,y_A)；如果两条直线段间夹角等于30°，则其交点为顶点p_B(x_B,y_B)；如果两条直线段间夹角等于150°，则其交点为顶点p_E(x_E,y_E)；如果一条直线段与另外的两条直线段夹角都是90°，则两个交点中与顶点p_B(x_B,y_B)共线的是顶点p_C(x_C,y_C)，与顶点p_E(x_E,y_E)共线的是顶点p_D(x_D,y_D)。

在本发明的一个优选实施例中，在所述步骤4)中，所述相对滚转角估计的具体方法如下：

连接顶点p₂(x₂,y₂)和p₄(x₄,y₄)构成直线段L₁，连接顶点p₃(x₃,y₃)和p₅(x₅,y₅)构成直线段L₂，由L₁和L₂的交点p_L(x_L,y_L)坐标由下式可以得到：

x_L＝0.5×(x₃－x₅),y_L＝0.5×(y₃－y₅)

p_L(x_L,y_L)定义为降落点坐标系o_cx_cy_cz_c的原点o_c，轴o_cy_c定义为点o_c与顶点p₁(x₁,y₁)的连线，且正方向为由o_c指向顶点p₁(x₁,y₁)；轴o_cx_c定义为点o_c到p₂(x₂,y₂)、p₃(x₃,y₃)连线的中垂线，其垂直于轴o_cy_c，且正方向为由o_c指向垂足；由右手坐标系准则可以得到轴o_cz_c的指向；

无人机机体坐标系定义如下：水平放置的无人机质心处为原点O_U，由质心沿着中轴线指向机头方向为轴O_UY_U，原点O_U指向竖直向上为轴O_UZ_U，由右手坐标系法则可以得到轴O_UX_U；

单目相机固定在无人机机体下方，其安置方式如下：CCD长边平行于轴O_UX_U，短边平行于轴O_UY_U，且CCD平面平行于面O_UX_UY_U；

无人机围绕轴o_cz_c转动的角度为滚转角ψ；

计算机图像坐标系中顶点p₁(x₁,y₁)与图像中心(0.5×M,0.5×N)的连线与计算机图像坐标系ov的夹角即作为滚转角ψ。

在本发明的一个优选实施例中，在所述步骤5)中，所述相对高度估计的具体方法如下：

外圈长直线段{l_i1,l_i2,l_i3,l_i4,l_i5}在计算机图像坐标系中的长度为L_l，外圈五边形面积为V_l＝1.433×L_l ²；内圈短直线段{l_j1,l_j2,l_j3,l_j4,l_j5}在计算机图像坐标系中的长度为L_s，内圈五边形面积为V_s＝1.433×L_s ²；

外圈五边形的物理面积为S_lcm³；内圈20cm直线段组成的五边形的物理面积为S_scm³；

由相机内参数焦距f、x和y方向单个像元物理尺寸d_x和d_y可以求得，

f_x＝f/d_x

f_y＝f/d_y

当无人机与合作标志之间距离大于一定阈值时，由下式求出无人机与合作标志之间的距离，也即无人机的高度h：

当无人机与合作标志之间距离小于等于一定阈值时，由下式求出无人机的高度h：

由于采用了如上的技术方案，本发明的有益效果在于：本发明采用一种基于单目测量新型合作标志的相对位姿估计方法，可以为无人机自主降落等任务提供滚转角和相对高度等信息。发明中设计了一种新型合作标志，无需色彩信息，具有指向性，且适用于不同距离情况下的测量。该发明仅利用一个摄像机对合作标志进行成像，降低了系统复杂度，算法求解上耗时更少。本发明可用于无人机需要定向定位的场合。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明的应用场景示意图。

图2为本发明设计的合作标志示意图。

图3为本发明的坐标系定义。

图4为本发明的合作标志成像示意图。

具体实施方式

为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体图示，进一步阐述本发明。

图1为本发明的一个典型应用场景，在室内多旋翼无人机由H₁降落至H₂再到H₃。

本发明的适用于无人机室内自主降落的位姿估计方法包括以下步骤：

1、图像采集步骤

当无人机飞行到合作标志正上方并开始缓慢垂直降落的过程中，固定设置在无人机下方的单目相机对包含合作标志的区域进行采集，得到图像I，图像尺寸宽度为M，高度为N，其中相机内参数已标定，焦距f、x和y方向单个像元物理尺寸d_x和d_y可标定出，视为已知；

其中，关于单目摄像机标定如何具体操作，为本领域公知技术，可参考文献1“Zhengyou Zhang.A Flexible New Technique for Camera Calibration[J].IEEETransaction on Pattern Analysis and Machine Intellgence,2000,22(11):1330-1334.”，此处不再赘述。

2、直线段检测步骤

利用基于梯度生长的直线段检测方法对图像I进行直线段检测，并将长度小于40pixel的直线段忽略，将检测出的直线段表示为{l₁,l₂,…,l_m}；

其中关于上述步骤如何具体操作，为本领域公知技术，可参考文献1“覃勋辉，马戎，付维平，等.一种基于梯度的直线段检测算法[J].光子学报,2012,41(2),205-209.”，此处不再赘述。

3、合作标志识别步骤

设计了一种由五条等长的直线段组成的类似于正方形结合三角形的五边形合作标志，如图2所示，采用边长40cm和边长20cm两种不同尺寸的标志进行组合使用。

求取{l₁,l₂,…,l_m}中长度相等的五条直线段的集合，分别可以获得外圈长直线段集合{l_i1,l_i2,l_i3,l_i4,l_i5}和内圈短直线段集合{l_j1,l_j2,l_j3,l_j4,l_j5}。

求取外圈长直线段集合{l_i1,l_i2,l_i3,l_i4,l_i5}的各直线段间夹角，如果两条直线段间夹角等于60°，则其交点为顶点p₁(x₁,y₁)；如果两条直线段间夹角等于30°，则其交点为顶点p₂(x₂,y₂)；如果两条直线段间夹角等于150°，则其交点为顶点p₅(x₅,y₅)；如果一条直线段与另外的两条直线段夹角都是90°，则两个交点中与顶点p₂(x₂,y₂)共线的是顶点p₃(x₃,y₃)，与顶点p₅(x₅,y₅)共线的是顶点p₄(x₄,y₄)。

求取内圈短直线段集合{l_j1,l_j2,l_j3,l_j4,l_j5}的各直线段间夹角，如果两条直线段间夹角等于60°，则其交点为顶点p_A(x_A,y_A)；如果两条直线段间夹角等于30°，则其交点为顶点p_B(x_B,y_B)；如果两条直线段间夹角等于150°，则其交点为顶点p_E(x_E,y_E)；如果一条直线段与另外的两条直线段夹角都是90°，则两个交点中与顶点p_B(x_B,y_B)共线的是顶点p_C(x_C,y_C)，与顶点p_E(x_E,y_E)共线的是顶点p_D(x_D,y_D)。

4、相对滚转角估计步骤

坐标系定义如图3所示，连接顶点p₂(x₂,y₂)和p₄(x₄,y₄)构成直线段L₁，连接顶点p₃(x₃,y₃)和p₅(x₅,y₅)构成直线段L₂。由L₁和L₂的交点p_L(x_L,y_L)坐标由下式可以得到：

x_L＝0.5×(x₃－x₅),y_L＝0.5×(y₃－y₅)

p_L(x_L,y_L)定义为降落点坐标系o_cx_cy_cz_c的原点o_c，轴o_cy_c定义为点o_c与顶点p₁(x₁,y₁)的连线，且正方向为由o_c指向顶点p₁(x₁,y₁)；轴o_cx_c定义为点o_c到p₂(x₂,y₂)、p₃(x₃,y₃)连线的中垂线，其垂直于轴o_cy_c，且正方向为由o_c指向垂足；由右手坐标系准则可以得到轴o_cz_c的指向；

无人机机体坐标系定义如下：水平放置的无人机质心处为原点O_U，由质心沿着中轴线指向机头方向为轴O_UY_U，原点O_U指向竖直向上为轴O_UZ_U，由右手坐标系法则可以得到轴O_UX_U；

单目相机固定在无人机机体下方，其安置方式如下：CCD长边平行于轴O_UX_U，短边平行于轴O_UY_U，且CCD平面平行于面O_UX_UY_U；

无人机围绕轴o_cz_c转动的角度为滚转角ψ；

计算机图像坐标系中顶点p₁(x₁,y₁)与图像中心(0.5×M,0.5×N)的连线与计算机图像坐标系ov的夹角即作为滚转角ψ。

5、相对高度估计步骤

外圈长直线段{l_i1,l_i2,l_i3,l_i4,l_i5}在计算机图像坐标系中的长度为L_l，外圈五边形面积为V_l＝1.433×L_l ²；内圈短直线段{l_j1,l_j2,l_j3,l_j4,l_j5}在计算机图像坐标系中的长度为L_s，内圈五边形面积为V_s＝1.433×L_s ²。

外圈40cm直线段组成的五边形的物理面积为S_l＝2293cm³；内圈20cm直线段组成的五边形的物理面积为S_s＝573cm³，成像示意图如图4所示。

由相机内参数焦距f、x和y方向单个像元物理尺寸d_x和d_y可以求得，

f_x＝f/d_x

f_y＝f/d_y

当无人机与合作标志之间距离大于2m时，由下式求出无人机与合作标志之间的距离，也即无人机的高度h(单位：cm)：

当无人机与合作标志之间距离小于等于2m时，由下式求出无人机的高度h：

以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (4)

1.一种适用于无人机室内自主降落的位姿估计方法，其特征在于，包括以下步骤：

1)图像采集步骤，当无人机飞行到合作标志正上方并开始缓慢垂直降落的过程中，固定设置在无人机下方的单目相机对包含合作标志的区域进行采集，得到图像I，图像尺寸宽度为M，高度为N，其中单目相机内参数已标定，焦距f、x和y方向单个像元物理尺寸d_x和d_y均为已知；

2)直线段检测步骤，利用基于梯度生长的直线段检测方法对图像I进行直线段检测，并将长度小于阈值的直线段忽略，将检测出的直线段表示为{l₁,l₂,…,l_m}；

3)合作标志识别步骤，规定一由五条等长的直线段组成的类似于正方形结合三角形的五边形合作标志，并利用检测出的{l₁,l₂,…,l_m}，求解出外圈五个顶点的二维坐标{p₁(x₁,y₁)、p₂(x₂,y₂)、p₃(x₃,y₃)、p₄(x₄,y₄)、p₅(x₅,y₅)}或者内圈五个顶点的二维坐标{p_A(x_A,y_A)、p_B(x_B,y_B)、p_C(x_C,y_C)、p_D(x_D,y_D)、p_E(x_E,y_E)}；

4)相对滚转角估计步骤，构建无人机机体坐标系O_UX_UY_UZ_U和降落点坐标系o_cx_cy_cz_c，在计算机图像坐标系中计算由求解出的五点坐标计算出无人机机体坐标系相对于降落点坐标系的相对滚转角ψ；

5)相对高度估计步骤，由五点坐标求解出计算机图像坐标系中已识别出的合作标志的面积S_i，并根据其与合作标志物理面积的比值大小求解出相对高度h。

2.如权利要求1所述的适用于无人机室内自主降落的位姿估计方法，其特征在于，在所述步骤3)中，所述合作标志识别的具体方法如下：

求取{l₁,l₂,…,l_m}中长度相等的五条直线段的集合，分别可以获得外圈长直线段集合{l_i1,l_i2,l_i3,l_i4,l_i5}和内圈短直线段集合{l_j1,l_j2,l_j3,l_j4,l_j5}，其中，求取外圈长直线段集合{l_i1,l_i2,l_i3,l_i4,l_i5}的各直线段间夹角，如果两条直线段间夹角等于60°，则其交点为顶点p₁(x₁,y₁)；如果两条直线段间夹角等于30°，则其交点为顶点p₂(x₂,y₂)；如果两条直线段间夹角等于150°，则其交点为顶点p₅(x₅,y₅)；如果一条直线段与另外的两条直线段夹角都是90°，则两个交点中与顶点p₂(x₂,y₂)共线的是顶点p₃(x₃,y₃)，与顶点p₅(x₅,y₅)共线的是顶点p₄(x₄,y₄)；求取内圈短直线段集合{l_j1,l_j2,l_j3,l_j4,l_j5}的各直线段间夹角，如果两条直线段间夹角等于60°，则其交点为顶点p_A(x_A,y_A)；如果两条直线段间夹角等于30°，则其交点为顶点p_B(x_B,y_B)；如果两条直线段间夹角等于150°，则其交点为顶点p_E(x_E,y_E)；如果一条直线段与另外的两条直线段夹角都是90°，则两个交点中与顶点p_B(x_B,y_B)共线的是顶点p_C(x_C,y_C)，与顶点p_E(x_E,y_E)共线的是顶点p_D(x_D,y_D)。

3.如权利要求2所述的适用于无人机室内自主降落的位姿估计方法，其特征在于，在所述步骤4)中，所述相对滚转角估计的具体方法如下：

连接顶点p₂(x₂,y₂)和p₄(x₄,y₄)构成直线段L₁，连接顶点p₃(x₃,y₃)和p₅(x₅,y₅)构成直线段L₂，由L₁和L₂的交点p_L(x_L,y_L)坐标由下式可以得到：

x_L＝0.5×(x₃－x₅),y_L＝0.5×(y₃－y₅)

p_L(x_L,y_L)定义为降落点坐标系o_cx_cy_cz_c的原点o_c，轴o_cy_c定义为点o_c与顶点p₁(x₁,y₁)的连线，且正方向为由o_c指向顶点p₁(x₁,y₁)；轴o_cx_c定义为点o_c到p₂(x₂,y₂)、p₃(x₃,y₃)连线的中垂线，其垂直于轴o_cy_c，且正方向为由o_c指向垂足；由右手坐标系准则可以得到轴o_cz_c的指向；

无人机机体坐标系定义如下：水平放置的无人机质心处为原点O_U，由质心沿着中轴线指向机头方向为轴O_UY_U，原点O_U指向竖直向上为轴O_UZ_U，由右手坐标系法则可以得到轴O_UX_U；

单目相机固定在无人机机体下方，其安置方式如下：CCD长边平行于轴O_UX_U，短边平行于轴O_UY_U，且CCD平面平行于面O_UX_UY_U；

无人机围绕轴o_cz_c转动的角度为滚转角ψ；

计算机图像坐标系中顶点p₁(x₁,y₁)与图像中心(0.5×M,0.5×N)的连线与计算机图像坐标系ov的夹角即作为滚转角ψ。

4.如权利要求3所述的适用于无人机室内自主降落的位姿估计方法，其特征在于，在所述步骤5)中，所述相对高度估计的具体方法如下：

外圈长直线段{l_i1,l_i2,l_i3,l_i4,l_i5}在计算机图像坐标系中的长度为L_l，外圈五边形面积为V_l＝1.433×L_l ²；内圈短直线段{l_j1,l_j2,l_j3,l_j4,l_j5}在计算机图像坐标系中的长度为L_s，内圈五边形面积为V_s＝1.433×L_s ²；

外圈五边形的物理面积为S_lcm³；内圈20cm直线段组成的五边形的物理面积为S_scm³；

由相机内参数焦距f、x和y方向单个像元物理尺寸d_x和d_y可以求得，

f_x＝f/d_x

f_y＝f/d_y

当无人机与合作标志之间距离大于一定阈值时，由下式求出无人机与合作标志之间的距离，也即无人机的高度h：

当无人机与合作标志之间距离小于等于一定阈值时，由下式求出无人机的高度h：

。