具体实施方式

下面结合附图对本发明作进一步详细说明。

本发明针对卫星拒止条件下无人机的自主着陆导航问题，开展基于线特征的惯性/视觉着陆导航方法研究，首先提出了无穷远元素以及其Plücker坐标表示方法，解决了物空间中平行线在像平面上的交点坐标无法求解问题。其次由跑道左右边线及中线一组等距平行线求解无穷远处的消隐线方程，解决了无法通过特征提取方法对消隐线进行识别的问题。最后通过消隐线方程进行无人机位姿求解并与惯性融合，解决了惯性导航误差随时间累积发散与视觉导航解算结果噪声较大的问题。

1.无穷远元素及Plücker表示法

(1)坐标系定义

如图1所示，建立机场坐标系、视觉坐标系、世界坐标系、摄像机坐标系、图像坐标系。

其中机场坐标系(a系)：以跑道着陆端起始线与跑道中心线的交点为原点o_a；轴沿跑道中心线，前向为正；y_a轴垂直于跑道平面，向上为正；z_a轴与跑道起始线重合，右向为正；o_ax_ay_az_a构成右手坐标系；机场坐标系下某点的坐标用(x_a,y_a,z_a)表示。

视觉坐标系(v系)：着陆视觉导航系统坐标系，简称视觉坐标系；以光学系统的像方主点为原点o_v；x_v轴平行于光轴，前向为正；y_v轴平行于成像平面坐标系的横轴，向上为正；z_v轴与x_v轴和y_v轴构成右手坐标系，右向为正。

世界坐标系(w系)：以跑道着陆端瞄准点起始线与跑道中心线的交点为原点o_w；x_w轴与跑道起始线重合，右向为正；y_w轴垂直于跑道平面，向下为正；z_w轴沿跑道中心线，前向为正；o_wx_wy_wz_w构成右手坐标系；世界坐标系下某点的坐标用(x_w,y_w,z_w)表示。

摄像机坐标系(c系)：以光学系统的像方主点为原点o_c；当正对光学系统观察时，x_c轴平行于成像平面坐标系的水平轴，左向为正；y_c轴平行于成像平面坐标系的垂直轴，向下为正；z_c轴指向观察者，并与x_c轴和y_c轴构成右手坐标系。

图像坐标系(i系)：在摄像机光敏面所在的平面内建立图像坐标系，它是一个二维平面坐标系，以图像左上角为原点，沿图像水平方向向右为图像坐标系的x_i轴，沿图像垂直方向向下为图像坐标系的y_i轴，图像坐标系的单位是像素。

(2)无穷远元素

在物空间中，两条平行线永不相交，在欧式空间基础上通过引入无穷远元素构建射影空间，平面内一组平行线相交于无穷远处唯一一点，称之为消隐点(VanishingPoint)。如图2所示。该点在像平面上的位置只跟摄像机的姿态有关而与摄像机的位置无关。

消隐点代表对应平行线的方向，不平行直线的无穷远点不同，平面上所有的无穷远点构成一条直线，即消隐线(Vanishing Line)。消隐线是空间内一组平行面在无穷远处的唯一交线。

(3)Plücker表示法

在像空间图像坐标系下直线方程可描述为：

ax_i+by_i+c＝0

因此，直线可用三维向量表示：

l＝[a,b,c]^T

而在物空间世界坐标系中设两点A、B的三维坐标分别为(3×1矩阵)，那么它们的齐次坐标为：过这两点的直线可用一个4×4的反对称齐次矩阵L表示，该矩阵称之为Plücker矩阵。

L＝AB^T-BA^T

另外，直线L可用其方向向量与矩m来表示，称之为Plücker坐标，记为：

其中，是直线的方向向量，矩m(可表征ΔABC的面积或者O到直线L的距离)是该直线和原点确定平面的法向量，即(如图3所示)：

由此可得Plücker矩阵与Plücker坐标间的关系为：

在摄像机映射T的作用下，用Plücker矩阵定义的直线L表示图像坐标系下对应直线的像l:

其中，K为相机内参矩阵：

为世界坐标系到摄像机坐标系的姿态转移矩阵，为摄像机坐标系原点在世界坐标系中的位置向量。s²只是直线中各参数的共有系数，因此[l]_×可简化为：

2.视觉测量位姿解算

(1)消隐点及消隐线成像方程

在无人机着陆过程中，视觉着陆系统对跑道线特征进行提取，其中跑道左右边线及中心线为一组平行线，可用来进行无穷远处消隐点坐标和消隐线方程的计算，设物空间内跑道上三条等距平行线为L_0w、L_1w、L_2w，其在像平面内成像为l_0i、l_1i、l_2i，则像空间内消隐点坐标可由以下关系求解：

消隐线方程为：

l_∞i＝[(l_0i×l_2i)^T(l_1i×l_2i)]l_1i+2[(l_0i×l_1i)^T(l_2i×l_1i)]l_2i

假设物空间中一点A的四维齐次坐标那么过点A且方向为

(三维单位列向量)，可表示为：

当参数λ由0变化到∞时，A点由有限点变化到无穷远点，该点在世界坐标系下的坐标为：

根据图像共轭方程获得消隐点与无人机姿态转移矩阵的关系为：

进一步整理方程得：

假设像空间消隐直线l_∞i上某点为x，其在物空间的反向投影为一条方向为的直线。由点x在直线上，可得：

x^Tl_∞i＝0

利用与平面的法向量n_π正交可得：

利用物空间中方向为的直线投影为像空间的点x，得：

对上式进行转置变换得：

与前文联立可得像空间消隐线方程：

(2)无人机位姿解算

联立前文中公式可得方程组如下：

式中，为姿态转移矩阵：

机场跑道场景下，是物空间跑道中心线的单位方向向量，n_π是物空间跑道平面的单位法向量，n_π＝[0,1,0]^T，同理可得

令K^-1p_∞＝[g₁,g₂,g₃]^T,K^Tl_∞＝[h₁,h₂,h₃]^T,(K^-1p_∞)×(K^Tl_∞)＝[e₁,e₂,e₃]^T，同时姿态转移矩阵C_wc为反对称阵，每一行、列元素的平方和为1，由上式可解：

由此解得三个姿态角:

利用线方程计算相对位置：

其中：

将跑道边线方程带入上式：

同理由直线L₂确定：

由上两式求解α₀、α₂：

最后解得无人机在世界坐标系下的垂向、侧向位置t_y、t_x：

3.惯性/视觉融合方法

惯性/视觉组合导航系统的卡尔曼滤波模型连续状态方程如下：

式中，F(t)为t时刻连续状态方程状态转移矩阵，为t时刻系统随机噪声向量。

滤波状态量分别为北天东速度误差(单位：m/s)、维度误差(单位：rad)、高度误差(单位：m)、经度误差(单位：rad)、北天东向失准角误差(单位：rad)、载体系XYZ方向陀螺漂移(单位：rad/s)、载体系XYZ方向加速度计零位(单位：m/s²)。

系统状态转移矩阵

其中：

观测方程定义如下：

为观测噪声阵。

组合导航系统的观测量为机场坐标系惯性导航输出的垂向侧向位置与视觉着陆系统导航结果的差值，：

H(t)＝[0_3×3 M_3×3 0_3×9]

其中，

综上，给出了无人机着落阶段利用跑道左右边线和中线三条等距平行线的惯性/视觉导航方法。

本发明具体通过以下4个流程实现无人机自主着陆：

(1)机场跑道数据采集：

通过安装在无人机机头的前视红外视觉导航系统对机场跑道进行图像采集，并对跑道边线进行实时特征提取，获取边线及中线的直线方程。

(2)Plücker坐标表示：

通过提前装订的机场跑道宽度与相机内参矩阵，根据上述所推公式计算边线方程的Plücker坐标。

(3)视觉测量位姿解算：

得到跑道边线的Plücker坐标后，由两条等距平行线计算无穷远处消隐点及消隐线的方程，通过联立方程组解算无人机实时的世界坐标系与相机坐标系之间的姿态转移矩阵并进行姿态与侧向、垂向位置的求解。

(4)基于惯性/视觉融合的组合导航：

将视觉着陆系统解算出的位置信息作为观测量，与惯性导航输出的导航信息构建卡尔曼滤波器进行融合，实现连续自主的导航定位功能。