CN108122255B - 一种基于梯形与圆形组合地标的无人机位姿估计方法 - Google Patents

Abstract

一种基于梯形与圆形组合地标的无人机位姿估计方法，属于图像处理技术领域。所述方法如下：步骤1、无人机对地标图案成像二值化处理分割出地标图案，基于形态学滤波去除孤立噪声；步骤2、提取地标边缘，利用霍夫变换提取地标中梯形轮廓直线信息，利用最小二乘法拟合椭圆方程并计算椭圆参数；步骤3、根据步骤2解算出的椭圆参数估计无人机姿态参数；步骤4、建立无人机对地成像模型，利用梯形轮廓直线信息确定梯形四个顶点坐标估计位置参数。本发明针对无人机视觉导航中的位姿精确估计和以及自主着陆等问题，基于梯形和圆形组合的地标图案，根据几何成像特性估计姿态参数，简化共线方程求解模型，进而求解位置参数，计算过程简单，更加适合实际工程应用。

Description

一种基于梯形与圆形组合地标的无人机位姿估计方法

技术领域

本发明属于图像处理技术领域，具体涉及一种基于梯形与圆形组合地标的无人机位姿估计方法。

背景技术

随着无人机及其上搭载的信息处理技术的不断发展和军事战略思想的转变，无人机因具备昼夜可用、机动灵活、使用便捷、成本低、效率比高等特点，已广泛应用于军事侦察、区域监视、地理测绘、智慧交通、电力巡线、农业植保等军民领域。与此同时，无人机也存在对军事要地、机场、大型展会等区域造成重大威胁的可能性，因此准确、实时获取无人机飞行过程中的姿态和位置，对于无人机的自主控制、智能化处理等实际应用和跟踪定位具有重要意义。

目前无人机系统通常携带获取自身位置的GPS数据，但难以保证定位的精确性，无法精确、实时地获取无人机飞行过程中的高程信息以及姿态参数。尽管目前已有基于双目视觉、模板匹配、特征跟踪颜色分割以及地平线检测等姿态估计方法和算法研究，但仍存在计算复杂、处理速度慢等问题，难以支持实际工程中应用。

发明内容

本发明针对无人机飞行过程中位姿精确自主估计的迫切需求，提供一种基于梯形与圆形组合地标的无人机位姿估计方法，该方法开展基于固定图形地标的无人机位姿参数自主估计方法研究，依据对圆形地标成像结果的椭圆长短轴之比仅与俯仰角有关且长轴倾角仅与滚转角有关的成像特性和直角梯形几何形状简单且满足解算无人机位置参数的最小顶点数目的几何特性，设计了梯形和圆形组合的地标图案。基于该地标图案，首先根据圆形投影到像面上的椭圆特征估计无人机飞行过程中的姿态参数，进而求得旋转变换矩阵带入成像共线方程，避免了正弦、余弦方程求解问题，再将梯形顶点代入无人机对地成像模型解算无人机的位置参数，从而提出基于梯形与圆形组合地标的无人机位姿估计方法。该方法能够在保证估计精度的同时，避免直接求解共线方程时的繁杂计算过程，更加适合实际工程应用。

为实现上述目的，本发明采取的技术方案如下：

一种基于梯形与圆形组合地标的无人机位姿估计方法，所述方法步骤如下：

步骤一：无人机对地成像结果二值化处理分割出地标图案，基于形态学滤波去除孤立噪声；

步骤二：提取地标边缘，利用霍夫变换提取地标中梯形轮廓直线信息，利用最小二乘法拟合椭圆方程并计算椭圆参数；

步骤三：根据步骤二计算出的椭圆参数求解无人机姿态参数；

步骤四：利用步骤三的无人机姿态参数建立无人机对地成像模型，利用梯形轮廓直线信息确定梯形四个顶点坐标估计位置参数。

本发明相对于现有技术的有益效果是：

(1)本发明针对无人机视觉导航中的位姿精确估计以及自主着陆等问题，基于梯形和圆形组合的地标图案，根据地标图案几何成像特性估计出无人机姿态参数，简化共线方程求解模型，进而求解无人机位置参数。与现有位姿估计方法相比，该方法具有较高估计精度的同时，计算过程简单、时效性强，更加适合实际工程应用。

(2)本发明针对无人机飞行过程中姿态的高时效、高精度自主估计问题，设计了圆形和梯形组合的简单图形地标，通过提取圆形和梯形地标经无人机成像投影至像面上的轮廓，根据圆形地标几何成像特性，利用像面椭圆长短轴比、长轴倾角等几何特征反演无人机姿态信息，同时引入误差补偿提高解算精度，从而实现无人机姿态角的快速、精确求解。

(3)本发明针对无人机飞行过程中位置的高实时、精确估计问题，将求出的姿态信息带入无人机对地成像模型，避免成像模型中存在正弦和余弦函数项，将无人机位置信息反演问题转化为简单的线性方程求解问题，然后利用梯形顶点的地面与像面坐标得到多组线性方程，最后基于最小二乘法计算位置参数减小误差，从而实现无人机位置信息的快速、精确求解。

(4)本发明能够基于简单地标中的图形特征，实现无人机姿态与位置参数的高实时、高准确性的估计，采用的算法复杂度低、计算效率高，更加适合工程应用。

附图说明

图1为无人机位姿估计方法流程图；

图2为无人机对地标图案成像结果的二值图像；

图3为基于形态学滤波的孤立噪声去除结果图；

图4为不同水平视场角下误差拟合曲线图；

图5为误差曲线拟合结果示意图；

图6为俯仰角估计方法的几何示意图；

图7为偏航角引起梯形底边倾斜示意图；

图8为偏航角估计方法的几何示意图一；

图9为偏航角估计方法的几何示意图二；

图10为地标平面示意图；

图11为地面坐标系示意图；

图12为相机坐标系示意图；

图13为地面坐标系与相机坐标转换示意图；

图14为相机坐标系到像面坐标系中心投影模型图。

具体实施方式

下面结合附图对本发明的技术方案作进一步的说明，但并不局限于此，凡是对本发明技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，均应涵盖在本发明的保护范围中。

具体实施方式一：本实施方式记载的是一种基于梯形与圆形组合地标的无人机位姿估计方法，可应用于无人机自主姿态控制、自动着陆等方面，由梯形和圆形组合成地标图案，其估算流程图如图1，所述方法步骤如下：

步骤一：无人机对地标图案成像二值化处理分割出地标图案，基于形态学滤波去除孤立噪声；

步骤二：提取地标边缘，利用霍夫变换提取地标中梯形轮廓直线信息，利用最小二乘法拟合椭圆方程并计算椭圆参数；

步骤三：根据步骤二计算出的椭圆参数求解无人机姿态参数；

步骤四：利用步骤三的无人机姿态参数建立无人机对地成像模型，利用梯形轮廓直线信息确定梯形四个顶点坐标估计位置参数。

具体实施方式二：具体实施方式一所述的一种基于梯形与圆形组合地标的无人机位姿估计方法，步骤一的具体步骤如下：

(1)进行图像二值化

针对无人机对地成像结果设定阈值，图像灰度值大于或者等于该阈值的像素可视为目标图形，其灰度值用1表示，图像灰度值小于该阈值的像素点被判定为背景区域，其灰度值用0表示；

其中，f表示输入图像，Th为二值化阈值；x，y表示像素点坐标；成像结果二值化图像如图2所示；

(2)进行形态学滤波

对经步骤(1)处理后的二值化图像先后进行开、闭运算去除孤立噪声，处理结果如图3所示。

具体实施方式三：具体实施方式一所述的一种基于梯形与圆形组合地标的无人机位姿估计方法，步骤二的具体步骤如下：

(1)基于Canny算子对步骤一形态学滤波后的二值化图像进行边缘检测，提取出梯形和椭圆轮廓；

(2)设椭圆轮廓坐标点(x₁,y₁),(x₂,y₂),…(x_n,y_n)，利用最小二乘法可得法方程：

其中，A，B，C，D，E为椭圆轮廓曲线方程待定系数，

为法方程系数矩阵的矩阵转置，[·]^T为矩阵转置；

法方程系数矩阵：

利用法方程求得待定系数，则椭圆轮廓曲线方程为：

Ax²+Bxy+Cy²+Dx+Ey+1＝0

进而得椭圆几何中心坐标(x_eo,y_eo)：

长轴倾斜角度

长轴a：

短轴b：

具体实施方式四：具体实施方式一所述的一种基于梯形与圆形组合地标的无人机位姿估计方法，步骤三的具体步骤如下：

(1)滚转角估计

理想情况下，初步假设仅滚转角会引起椭圆长轴倾斜，即滚转角在数值上即等于椭圆长轴倾角，将图像旋转使长轴水平，则此时椭圆中心相对于图像中心O(x_o,y_o)的水平、竖直方向偏移量Δx',Δy'为：

其中，

为长轴倾斜角度；

实际上，圆形地标成像结果往往不是严格的椭圆，导致真实滚转角与像面椭圆倾角之间存在一定差异，该误差与地标图案成像水平方向视场角近似成正比关系，为提高位姿估计精度，引入误差补偿项，将滚转角γ表示为：

式中，第二项即为误差补偿项，水平方向视场角

其中d_x为探测器像元水平尺寸，F为镜头焦距；

当无人机内部参数确定时，

可通过仿真实验拟合得到，具体方法为：仿真一系列不同位姿状态下的成像结果，按照前面步骤一到步骤三的具体步骤(1)分别计算各图像

的值和对应的

进而拟合出二者关系曲线，如图4、图5所示；

(2)俯仰角估计

首先根据已经求得的滚转角，修正椭圆中心相对于水平和竖直方向的偏移量Δx,Δy：

然后分别修正地标位置水平和竖直方向视场角：

其中，d_y为探测器像元竖直方向尺寸，F为镜头焦距；

由图6中给出的地面原点O_g的成像光线和光轴位置以及地面坐标系与像极坐标系相对位置空间几何关系可知：椭圆短轴和长轴之比等于α₁的正弦值，即：

进而可得俯仰角：α＝α₁+ω_y

(3)偏航角

基于霍夫变换进行直线检测，并根据梯形的几何特性提取出所需的四条线段L₁L₂L₃和L₄的参数信息如图7所示，进而计算梯形顶点坐标为P₁(x_p1,y_p1)、P₂(x_p2,y_p2)、P₃(x_p3,y_p3)和P₄(x_p4,y_p4)，则x_g轴在像面上倾角β_xg等于向量

和向量

倾角的平均值：

其中(x_p1,y_p1)、(x_p2,y_p2)、(x_p3,y_p3)和(x_p4,y_p4)为四边形顶点P₁、P₂、P₃和P₄的坐标；

则由偏航角引起的x_g轴倾斜角为β₁：β₁＝-γ-β_xg

如图8，9所示，根据空间几何关系知偏航角β：

具体实施方式五：具体实施方式一所述的一种基于梯形与圆形组合地标的无人机位姿估计方法，步骤四的具体步骤如下：

(1)确定地面坐标系和相机坐标系变换矩阵：

本发明地标图案如图10所示，采用直角梯形与圆形的组合，如图11所示，以圆心为原点建立地面坐标系(右手坐标系)，X_g轴平行于梯形底边，Z_g轴垂直于地平面向上，设相机与成像平台固连，相机质心与平台质心重合，相机坐标系与无人机平台坐标系重合，建立如图12所示相机坐标系O_c-X_cY_cZ_c，Z_c轴平行于光轴方向，无人机对地拍摄时，地面坐标系与相机坐标系位置关系如图13所示，定义α、β和γ分别为俯仰角、偏航角和滚转角，设地面原点在相机坐标系下坐标T_gc＝(T_Xc,T_Yc,T_Zc)，则从地面坐标系变换到相机坐标系的过程为：首先将坐标系绕Z轴旋转β(从Z轴正半轴向原点以逆时针方向为正)，得变换矩阵R₁：R₁＝R_Z(β)，其中，R_z(·)表示绕Z轴旋转变换函数，

若旋转角度为ω，则

再绕X轴旋转90-α，得变换矩阵R₂：R₂＝R_X(90°-α)，其中，R_X(·)表示绕X轴旋转变换函数，

若旋转角度为ω，则

然后绕Z轴旋转滚转角γ，变换矩阵为R₃：R₃＝R_z(γ)

最后平移到T_gc处，变换矩阵为S_Tgc：

则从地面坐标系到相机坐标系的变换关系为：

[X_c1,Y_c1,Z_c1,1]^T＝S_TgcR₃R₂R₁[X_g1,Y_g1,Z_g1,1]^T＝M_gc[X_g1,Y_g1,Z_g1,1]^T

其中，M_gc表示地面坐标系到相机坐标系的变换矩阵，(X_g1,Y_g1,Z_g1)为点在地面坐标系的坐标。

通常情况下，已知参数为无人机在地面坐标系下的位置坐标T_cg，此时应先求出从相机坐标系到地面坐标系的变换矩阵M_cg：

M_cg＝S_TcgR_Z(-β)R_X(α-90°)R_Z(-γ)，

其中，S_Tgc表示从相机坐标系到地面坐标系平移变换矩阵。

将求得的姿态角带入上式求出旋转矩阵R_gc：

R_gc＝R_Z(γ)R_X(90°-α)R_Z(β)

则地面坐标系下点P_g(X_pg,Y_pg,Z_pg)经旋转变换后坐标(X'_pg,Y_p'_g,Z'_pg)为：

[X'_pg,Y'_pg,Z'_pg]^T＝R_gc[X_pg,Y_pg,Z_pg]^T

(2)确定图像物理坐标系与图像像素坐标系转化关系：

如图14所示，摄像机坐标系中的物点Q_c(X_c1,Y_c1,Z_c1)在图像物理坐标系中的像点Q_u(X_u,Y_u,Z_u)坐标为：

其中，F为相机焦距；

将图像物理坐标系转化为图像像素坐标系：

其中，(x₀，y₀)是光轴与图像数字平面的交点的坐标，d_x1和d_y1分别为一个像素在X_u与Y_u方向上的物理尺寸；

(3)利用直线信息计算标识点坐标，反解位置参数

若P_g对应的像素坐标为P(x_p，y_p)，设地面坐标系原点在相机坐标系下位置坐标为T_gc＝(T_Xc,T_Yc,T_Zc)，则有方程组：

将梯形地标四个顶点的地面坐标以及对应的像面坐标带入方程组，再次利用最小二乘法求出(T_Xc,T_Yc,T_Zc)的值，从地面坐标系到相机坐标系变换矩阵为：

故从相机坐标系到地面坐标系变换矩阵为：

则T_cg即为所求的无人机位置参数，即无人机在地面坐标系下位置坐标。

Claims (3)

1.一种基于梯形与圆形组合地标的无人机位姿估计方法，其特征在于：所述方法步骤如下：

步骤一：无人机对地成像结果二值化处理分割出地标图案，基于形态学滤波去除孤立噪声；

步骤二：提取地标边缘，利用霍夫变换提取地标中梯形轮廓直线信息，利用最小二乘法拟合椭圆方程并计算椭圆参数；

步骤三：根据步骤二计算出的椭圆参数求解无人机姿态参数；步骤三的具体步骤如下：

(1)滚转角估计

将图像旋转使长轴水平，则此时椭圆中心相对于图像中心O(x_o,y_o)的水平、竖直方向偏移量Δx',Δy'为：

其中

为长轴倾斜角度，x_eo,y_eo为椭圆几何中心坐标；

为提高位姿估计精度，引入误差补偿项，将滚转角γ表示为：

式中，第二项即为误差补偿项，水平方向视场角

其中d_x为探测器像元水平尺寸，F为镜头焦距，a为椭圆长轴，b为椭圆短轴；

(2)俯仰角估计

首先根据已经求得的滚转角，修正椭圆中心相对于水平和竖直方向的偏移量Δx,Δy：

然后分别修正地标位置水平和竖直方向视场角：

其中，d_y为探测器像元竖直方向尺寸，F为镜头焦距；

由空间几何关系可知，椭圆短轴和长轴之比等于α₁的正弦值，即：

进而可得俯仰角：α＝α₁+ω_y

(3)偏航角

基于霍夫变换进行直线检测，并根据梯形的几何特性提取出所需的四条线段L₁L₂L₃和L₄的参数信息，进而计算梯形顶点坐标P₁(x_p1,y_p1)、P₂(x_p2,y_p2)、P₃(x_p3,y_p3)和P₄(x_p4,y_p4)，则x_g轴在像面上倾角β_xg等于向量

和向量

倾角的平均值：

其中(x_p1,y_p1)、(x_p2,y_p2)、(x_p3,y_p3)和(x_p4,y_p4)为四边形顶点P₁、P₂、P₃和P₄的坐标；

则由偏航角引起的x_g轴倾斜角为β₁：β₁＝-γ-β_xg

根据空间几何关系知偏航角β：

步骤四：利用步骤三的无人机姿态参数建立无人机对地成像模型，利用梯形轮廓直线信息确定梯形四个顶点坐标估计位置参数；步骤四的具体步骤如下：

(1)确定地面坐标系和相机坐标系变换矩阵：

采用直角梯形与圆形的组合，以圆心为原点建立地面坐标系，X_g轴平行于梯形底边，Z_g轴垂直于地平面向上，设相机与成像平台固连，相机质心与平台质心重合，相机坐标系与无人机平台坐标系重合，建立相机坐标系O_c-X_cY_cZ_c，Z_c轴平行于光轴方向，设地面原点在相机坐标系下坐标T_gc＝(T_Xc,T_Yc,T_Zc)，则从地面坐标系变换到相机坐标系的过程为：首先将坐标系绕Z轴旋转β，从Z轴正半轴向原点以逆时针方向为正，得变换矩阵R₁：R₁＝R_z(β)，其中，R_Z(·)表示绕Z轴旋转变换函数，若旋转角度为ω，则

再绕X轴旋转90°-α，得变换矩阵R₂：R₂＝R_X(90°-α)，其中，R_X(·)表示绕X轴旋转变换函数，若旋转角度为ω，则

然后绕Z轴旋转滚转角γ，变换矩阵为R₃：R₃＝R_Z(γ)

最后平移到T_gc处，变换矩阵为S_Tgc：

则从地面坐标系到相机坐标系的变换关系为：

[X_c1,Y_c1,Z_c1,1]^T＝S_TgcR₃R₂R₁[X_g1,Y_g1,Z_g1,1]^T＝M_gc[X_g1,Y_g1,Z_g1,1]^T

其中，M_gc表示地面坐标系到相机坐标系的变换矩阵，(X_g1,Y_g1,Z_g1)为点在地面坐标系的坐标；

通常情况下，已知参数为无人机在地面坐标系下的位置坐标T_cg，此时应先求出从相机坐标系到地面坐标系的变换矩阵M_cg：

M_cg＝S_TcgR_Z(-β)R_X(α-90°)R_Z(-γ)，

其中，S_Tgc表示从相机坐标系到地面坐标系平移变换矩阵；

将求得的姿态角带入上式求出旋转矩阵R_gc：

R_gc＝R_Z(γ)R_X(90°-α)R_Z(β)

则地面坐标系下点P_g(X_pg,Y_pg,Z_pg)经旋转变换后坐标(X′_pg,Y′_pg,Z′_pg)为：

[X'_pg,Y'_pg,Z'_pg]^T＝R_gc[X_pg,Y_pg,Z_pg]^T

(2)确定图像物理坐标系与图像像素坐标系转化关系：

摄像机坐标系中的物点Q_c(X_c1,Y_c1,Z_c1)在图像物理坐标系中的像点Q_u(X_u,Y_u,Z_u)坐标为：

其中，F为相机焦距；

将图像物理坐标系转化为图像像素坐标系：

其中，(x₀，y₀)是光轴与图像数字平面的交点的坐标，d_x1和d_y1分别为一个像素在X_u与Y_u方向上的物理尺寸；

(3)利用直线信息计算标识点坐标，反解位置参数

若P_g对应的像素坐标为P(x_p，y_p)，设地面坐标系原点在相机坐标系下位置坐标为T_gc＝(T_Xc,T_Yc,T_Zc)，则有方程组：

将梯形地标四个顶点的地面坐标以及对应的像面坐标带入方程组，再次利用最小二乘法求出(T_Xc,T_Yc,T_Zc)的值，从地面坐标系到相机坐标系变换矩阵为：

故从相机坐标系到地面坐标系变换矩阵为：

则T_cg即为所求的无人机位置参数，即无人机在地面坐标系下位置坐标。

2.根据权利要求1所述的一种基于梯形与圆形组合地标的无人机位姿估计方法，其特征在于：步骤一的具体步骤如下：

(1)进行图像二值化

针对无人机对地成像结果设定阈值，图像灰度值大于或者等于该阈值的像素可视为目标图形，其灰度值用1表示，图像灰度值小于该阈值的像素点被判定为背景区域，其灰度值用0表示；

其中，f表示输入图像，Th为二值化阈值；x，y表示像素点坐标；

(2)进行形态学滤波

对经步骤(1)处理后的二值化图像先后进行开、闭运算去除孤立噪声。

3.根据权利要求1所述的一种基于梯形与圆形组合地标的无人机位姿估计方法，其特征在于：步骤二的具体步骤如下：

(1)基于Canny算子对步骤一形态学滤波后的二值化图像进行边缘检测，提取出梯形和椭圆轮廓；

(2)设椭圆轮廓坐标点(x₁,y₁),(x₂,y₂),…(x_n,y_n)，利用最小二乘法可得法方程：

其中，A，B，C，D，E为椭圆轮廓曲线方程待定系数，

为法方程系数矩阵的矩阵转置，[·]^T为矩阵转置；

法方程系数矩阵：

利用法方程求得待定系数，则椭圆轮廓曲线方程为：

Ax²+Bxy+Cy²+Dx+Ey+1＝0

进而得椭圆几何中心坐标(x_eo,y_eo)：

长轴倾斜角度

长轴a：

短轴b：

Images