李 洪，王大元，明 丽，童 栎

（1.北京自动化控制设备研究所，北京 100074；2.陆军装备部航空军事代表局驻北京地区航空军事代表室，北京 100012）

卫星拒止条件下无人机高精度自主导航技术是决定无人机能否顺利完成战略任务的关键，其中着陆阶段更是飞行全过程中的研究难点之一[1]。根据无人机着陆剖面可知，无人机着陆过程的各阶段对姿态和位置的测量均具有严格的要求[2]。

目前常用的无人机着陆导航方式有惯性导航（Inertial Navigation System, INS），全球定位系统（Global Positioning System, GPS）[3]等。但是惯性导航误差会随时间累积而发散，GPS战时易受干扰，两种导航方法均具有明显缺点。视觉导航是基于计算机视觉技术提出，通过视觉传感器采集无人机着陆过程中的实时图像，并计算无人机与机场跑道之间的相对几何关系，从而求取二者之间的相对姿态与位置的导航方式。如何实时准确地获取无人机相对着陆场站的位姿信息是视觉导航的关键技术与主要瓶颈[4,5]。

视觉导航不依赖于地面设备完全自主，在测量精度方面也具有明显优势。近年来基于机场跑道点特征的无人机视觉着陆技术迅速发展，但是点特征本身对环境依赖性很大，在季节交替、天气变化和跑道磨损时极易发生误匹配现象，而线特征作为更高层次的特征具有较高的光照不变性，在图像中位置也更为精确；国防科技大学张小苗等[6]利用机场跑道平行信息求解消隐点[7]与无人机位姿之间的关系，由单幅图像解算无人机相对位姿。周郎明等[8]在无人机降落前期利用完整的结构化线特征配置解算出无人机的六自由度位姿参数。在着陆末段中，无人机越过跑道起始线后存在视觉测量盲区[9]，无法得到准确的位姿解算结果。相比之下，惯性导航方式虽然误差会随时间累积而发散，但是不受光照、视线等因素影响且具有完全自主性[10,11]，与视觉导航具有很好的互补性[12,13]。因此，本文针对无人机视觉着陆导航过程中点特征鲁棒性差、着陆末段存在视觉测量盲区等问题，开展基于线特征的无人机自主着陆惯性/视觉导航方法研究。

本文提出了一种基于机场跑道三条等距平行线（包括机场跑道左右边线以及中心线）的惯性/视觉相对位姿测量模型。通过三条等距平行线确定像平面的消隐点坐标与消隐线方程，随后将其代入视觉测量模型实时估计相机投影转换矩阵，并以此求解无人机相对机场跑道的位姿以及垂向和侧向距离。在此基础上利用扩展卡尔曼滤波方法（Extend Kalman Filter, EKF）实现惯性导航信息与视觉测量信息的融合，解决无人机着陆末段越过跑道起始线后视觉测量存在盲区以及纯惯性误差随时间发散等问题。最终通过飞行试验对本文所提方法的有效性进行验证。

1 基础理论

1.1 坐标系定义

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

图1 坐标系示意图Fig.1 Coordinate system diagram

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

视觉坐标系（v系）：着陆视觉导航系统坐标系，简称视觉坐标系；以光学系统的像方主点为原点ov；xv轴平行于光轴，前向为正；yv轴平行于成像平面坐标系的横轴，向上为正；zv轴与xv轴和yv轴构成右手坐标系，右向为正。

世界坐标系（w系）：以跑道着陆端瞄准点起始线与跑道中心线的交点为原点ow；xw轴与跑道起始线重合，右向为正；yw轴垂直于跑道平面，向下为正；zw轴沿跑道中心线，前向为正；owxwywzw构成右手坐标系；世界坐标系下某点的坐标用（xw,yw,zw）表示。

摄像机坐标系（c系）：以光学系统的像方主点为原点oc；当正对光学系统观察时，xc轴平行于成像平面坐标系的水平轴，左向为正；yc轴平行于成像平面坐标系的垂直轴，向下为正；zc轴指向观察者，并与xc轴和yc轴构成右手坐标系。

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

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

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

图2 一组平行线相交于消隐点示意图Fig.2 Schematic diagram of a set of parallel lines intersecting at the vanishing point

消隐点代表对应平行线的方向，不平行直线的无穷远点不同，平面上所有的无穷远点构成一条直线，即消隐线（Vanishing Line）。消隐线是空间内一组平行面在无穷远处的唯一交线。在像空间图像坐标系下直线方程可描述为：

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

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

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

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

图3 直线的Plücker坐标含义Fig.3 Plücker coordinate meaning of a straight line

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

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

式(6)中，K为相机内参矩阵：

为世界坐标系到摄像机坐标系的姿态转移矩阵，为摄像机坐标系原点在世界坐标系中的位置向量。s2只是直线中各参数的共有系数，因此式(6)可简化为：

2 视觉测量位姿解算

2.1 消隐点及消隐线成像方程

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

消隐线方程为：

假设物空间中一点A的四维齐次坐标那么过点A且方向为(三维单位列向量)，可表示为：

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

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

进一步整理方程得：

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

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

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

对式(17)进行转置变换得：

由式(16)(18)可得：

根据式(15)(19)可得像空间消隐线方程：

2.2 无人机位姿解算

联立式(9)(10)(14)(20)得方程组如下：

式(21)中，为姿态转移矩阵：

其中，

由式(21)(24)解得姿态转移矩阵：

由此解得三个姿态角：

由式(8)利用线方程计算相对位置方法：

其中：

即：

同理，由直线L2确定：

由式(30)(31)求解α0、α2：

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

3 惯性/视觉融合方法

由惯性导航系统可获得无人机瞬时姿态、速度、位置及原始惯性传感器信息，组合导航EKF状态方程：

其中，F(t)为t时刻状态转移矩阵，为t时刻系统随机噪声向量，系统状态向量如式(35)所示：

其中：

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

观测方程定义如下：

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

综上，给出了无人机着落末段越过起始线后，视觉存在盲区时，利用跑道左右边线和中线三条等距平行线的惯性/视觉导航方法。

4 试验验证

本试验选用某型号固定翼无人机为试验载机，试验地点为中国西北部某民用机场。试验判定基准为差分卫星后处理获取的垂向、侧向位置信息。试验结果如图4、图5所示。

图4 垂向、侧向相对位移Fig.4 Vertical and lateral relative displacement

图5 垂向、侧向误差Fig.5 Vertical and lateral error

图4为差分卫星基准、着陆视觉导航、惯性/视觉组合导航三种导航方式结果与机场相对垂向、侧向位置测量值曲线，三种导航方式结果分别对应图中红色、蓝色、黑色曲线。图5为以差分卫星为基准，视觉导航与惯性/视觉组合导航两种导航方式的垂向、侧向误差曲线。两幅图像横坐标均为无人机与机场跑道起始线间的距离，在距离跑道826 m时开始应用本文算法。由图5可知，在本文提出的算法进行无人机实时位姿解算，无人机垂向定位精度可达10 m，侧向定位精度可达5 m。在图5中可以看出，在视觉信息发生跳变时，惯性/视觉融合方法可以有效抑制视觉跳变，试验结果验证了本文提出算法的有效性。

5 结 论

本文针对固定翼无人机着陆场景下传统点特征提取受光照、跑道磨损等外界因素干扰，影响定位精度的问题，提出了一种基于线特征的无人机位姿解算方法。引入无穷远元素，通过跑道内三条等距平行线的方程计算图像空间内无穷远处的消隐点坐标及消隐线方程，从而解算出实时的无人机位姿，并基于EKF算法将视觉位姿与惯性导航输出结果进行融合，经机载试验验证，本文提出的算法可以使无人机垂向定位精度达10 m、侧向定位精度达5 m，解决了无人机着陆过程中提取点特征受外界因素干扰、视觉信息易发生跳变、惯性导航误差随时间发散等问题。