CN103411609B - 一种基于在线构图的飞行器返航路线规划方法 - Google Patents

Abstract

一种基于在线构图的飞行器返航路线规划方法，（1）事先测绘出关于整个飞行线路上或仅仅是选择的典型区域的地理信息存储在机载计算机中；（2）实时获取飞行器的惯性导航信息；（3）得到实时图；（4）确定飞行当前位置；（5）估计惯性导航误差，并利用估计的惯性导航误差修正飞行器的导航参数；（6）飞行器飞行过程中，机载计算机按照预先设定的规则将步骤（5）经过信息融合的飞行器位置信息存储在机载计算机的地理信息中，并根据经过信息融合的飞行器位置信息进行在线构图；（7）飞行器在返航过程中，根据在线构图的结果以及飞行器的任务特点实时进行航迹规划，完成飞行引导。

Description

一种基于在线构图的飞行器返航路线规划方法

技术领域

本发明属于自主导航与航迹规划技术范畴，具体是采用了INS/GIS组合导航方式，通过利用地理信息系统（简称GIS）和在线构图技术，实现对无人机、空中运载器等飞行器的自主导航和返航路线快速规划。

背景技术

（1）地理信息系统GIS

地理信息是有关地理实体的性质、特征和运动状态的表征及一切有用的知识。地理信息系统是描述、存储、分析和输出空间信息的理论和方法的一门新兴的交叉学科；是以地理空间数据库为基础，在计算机软硬件支持下，对空间相关数据进行采集、管理、操作、模拟和显示，为地理研究和地理决策服务的计算机技术系统。

目前，地理信息系统已在环境保护、灾害预测、城市规划、交通运输等众多领域得到了十分广泛的应用，但还未见到将其应用于飞行器导航与返航路线快速规划的研究。

（2）数字式景像匹配辅助导航

采用数字式景像匹配辅助导航，需要事先获取飞行器的预定飞行线路上某些地区的地面景物图像，存储在机载计算机中，当飞行器飞临这些地区上方时，利用机载光学、红外或者SAR成像传感器实时获取地面景象，并与预存在机载计算机中的图像进行配准比较，确定出飞行器的当前位置。

数字式景像匹配辅助导航的优点是精度较高，缺点是需要存储的信息量很大，实时采集到的图像数据量大，相应的处理数据量的工作量也很大，为满足飞行器导航制导系统要求，需要具有较高的图像处理和视频信号处理速度。

（3）航迹规划技术

飞行器航迹规划的主要功能是根据任务目标和约束规则规划一条从起始点到目标点的最优飞行轨迹。它一般分两个层次进行：第一，根据飞行任务要求、飞行区域、时间等因素组合，以此最优性能指标为标准，通过规划算法生成一条参考轨迹；第二，综合考虑参考轨迹周围的威胁情况和飞行器自身的性能指标，对参考轨迹进行局部动态优化，最后生成最优轨迹。

由于飞行器航迹规划涉及到的约束条件较多，建立数学模型存在一定的困难，并涉及大量不同信息的处理，目前，在规划系统的精确度、实时性和任务适应性等方面还存在一系列的问题有待解决。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种基于在线构图的飞行器返航路线规划方法。

本发明的技术解决方案是：一种基于在线构图的飞行器返航路线规划方法，步骤如下：

（1）在飞行器的预定飞行线路上选择一系列典型区域，事先测绘出关于整个飞行线路上或仅仅是选择的典型区域的地理信息存储在机载计算机中；

（2）在飞行器飞行过程中，由机载惯性测量装置实时敏感飞行器的运动变化，根据飞行器的运动变化，实时获取飞行器的惯性导航信息；

（3）在飞行器飞行过程中或仅在飞临典型区域上方时，利用机载传感器实时获取地面景像或其它地理信息，得到实时图；

（4）从实时图中提取地面要素的特征数据，将提取的特征数据与机载计算机中存储的地理信息进行特征匹配，确定飞行器的当前位置；

（5）根据步骤（2）中获取的飞行器当前飞行状态参数与步骤（4）中确定的飞行器当前位置进行信息融合，估计惯性导航误差，并利用估计的惯性导航误差修正飞行器的导航参数；

（6）飞行器飞行过程中，机载计算机按照预先设定的规则将步骤（5）经过信息融合的飞行器位置信息存储在机载计算机的地理信息中，并根据经过信息融合的飞行器位置信息进行在线构图；

（7）飞行器在返航过程中，根据在线构图的结果以及飞行器的任务特点实时进行航迹规划，完成飞行引导。

所述步骤（1）中的地理信息存储在机载计算机前进行预处理，该预处理包括以下步骤：

（1.1）从典型区域中的地理信息数据中提取能够用于匹配定位的地理目标；

（1.2）对上述地理目标中的线状目标几何数据进行化简，保留曲线上形态特征明显的点，舍去曲线上形态特征不明显的点；

（1.3）对地理目标的属性特征数据进行化简，保留地理目标的类型和匹配中所需的描述信息；

（1.4）提取地理目标的几何特征信息；

（1.5）将不同数据源、不同图层的地理目标按照统一的特征编码进行重新组织，重新组合和分解相互覆盖或叠加的地理目标，并重新建立各个地理目标之间的相互关系，以将所需的不同数据源、不同层的地理目标统一融合到一个图层中。

本发明与现有技术相比有益效果为：

本发明方法考虑到无人机、运载器等飞行器在飞行过程中对导航的实时性、高精度要求，以及快速返航规划的需要，采取了相应的措施，取得了有益的效果，具体效果如下：

（1）本发明在进行匹配时，仅将从实时图中提取出的地面要素的轮廓特征数据与预先存储的相关地理信息进行比较，减少了需要处理的数据量，降低了设计出效率、精度较高的匹配算法的难度，从而能够减少飞行软件的时空开销，降低满足飞行器导航实时性要求的难度。

（2）在本发明中，通过采用存储飞行器的位置信息和在线构图的手段，实现了返航路线能够根据去往目的地时的飞行路线进行规划的功能，降低了航迹规划的难度，减少了航迹规划的计算量，从而提高了航迹规划的实时性。

（3）目前的数字式景像匹配辅助导航中使用的基准图需要根据数字地图对数字式地面景像图进行标定而产生，这一过程不可避免的会引入误差。而本发明中使用的地理数据不需要进行如此标定，因此消除了该类误差，提高了定位的精度。目前地面定位的手段十分先进，测量精度也很高，因此，可以有效地提高本发明事先存储在机载计算机中的地理信息精度。

（4）在本发明中，地理信息在存储到机载计算机中前，可以根据匹配算法的需求对某些地面要素的特征进行预先处理，包括地面要素轮廓线的提取、曲率计算、特征点提取等，这样，在进行匹配时，对实时图进行处理后，可直接使用匹配算法进行匹配，从而大大减少了匹配的实时计算量。

附图说明

图1为本发明流程图；

图2为本发明数字影像线特征提取算法流程图；

图3为本发明滤波器设计流程示意图；

图4为本发明非等间隔卡尔曼滤波算法原理示意图。

具体实施方式

下面结合附图对本发明做详细说明，本发明的主要工作流程如图1所示，具体实施方法是：

（1）在飞行器的预定飞行线路上选择一系列典型区域，事先测绘出关于整个飞行线路上或仅仅是选择的典型区域的地理信息存储在机载计算机中；

地理信息在存储到机载计算机前，可以根据匹配算法的需求对地理数据进行预先处理，主要包括数据化简、特征提取、数据融合三个过程。

预处理具体步骤如下：

（1.1）从典型区域中的地理信息数据中提取能够用于匹配定位的地理目标；

地理信息系统（GIS）往往力求对于现实世界表达的完备性，因此地理数据的内容通常都十分丰富，以标准地形图为例，通常包括植被、境界、交通、行政区划、水系等多个要素图层。在这些图层中有些完全是人们进行抽象的产物，如行政区划的边界等，在航空摄影与遥感图像中根本没有相应的实体要素。有一些是经过人为划定的，如植被覆盖区域等，它们本来没有明显的边界，但在GIS中为了表达植被区域的需要人为的划定了边界。这些无法应用于匹配定位。数据化简的任务就是提取那些能够用于匹配定位的地理目标。

（1.2）对上述地理目标中的线状目标几何数据进行化简，保留曲线上形态特征明显的点，舍去曲线上形态特征不明显的点；

在进行地理信息匹配时，主要利用的是线状目标以及可以看作线状目标的面域的边界线。进行线状目标化简的基本原则是：保留曲线上形态特征明显的点，舍去曲线上形态特征不明显的点。本方法中主要采用了道格拉斯-普克算法进行数据化简，具体过程为：为每一条曲线的首末点虚连一条直线，求该曲线上所有的点到这一直线的距离，并找出最大距离值dmax，用dmax与限差D相比。其中，限差D通常根据精度的要求来确定，可以根据匹配影像的分辨率，取3到5个像素所代表的实际距离作为限差。

若dmax＜D，这条曲线上的中间点全部舍去；

若dmax≥D，保留dmax对应的坐标点，并以该点为界，把曲线分为两部分，对这两部分重复使用该方法。

道格拉斯-普克算法能够在保证曲线特征的前提下尽可能的化简不必要的坐标点数据。

对上述地理目标中的线状目标几何数据进行化简除了上述道格拉斯-普克算法外，还可以采用其它效果一致的算法。

（1.3）对地理目标的属性特征数据进行化简，保留地理目标的类型和匹配中所需的描述信息；

在对数字影像的特征提取过程中，根据数字影像的特点有时能够获得所提取特征的类型信息。因此本方法在进行地理信息数据的预处理时，保留了地理目标的类型信息和匹配中所需的描述信息（如线段的长度、转角的曲率、面的面积等）。

（1.4）提取地理目标的几何特征信息；

传统的GIS中地理目标的几何形状特征一般都隐含在地理目标坐标点串数据中，并不显式的进行记录，需要这些特征数据时，可以根据其坐标信息进行计算和提取。在本方法中需要用到大量的地理目标的几何特征信息，为了提高匹配定位的效率，本方法在数据预处理阶段将地理目标的几何特征信息提取出来中。这些特征包括曲线的长度、弯曲度、拐点信息、曲率和曲率半径等。

地理目标特征提取的另一个重要任务是根据地理目标的位置特征进一步对地理数据进行化简。在传统的GIS中，不同的区域分布的地理目标的密度也不尽相同，有些区域的特征目标很多，有些区域则很少。当根据匹配到的地理目标进行定位时，如果这些目标过于集中则会严重影响定位的精度。因此在进行特征提取时，要根据地理目标的位置特征使其尽可能均匀的分布在匹配区域。即需要根据地理目标的位置分布，删除冗余的地理目标，使地理目标均匀的分布在匹配区；

（1.5）将不同数据源、不同图层的地理目标按照统一的特征编码进行重新组织，重新组合和分解相互覆盖或叠加的地理目标，并重新建立各个地理目标之间的相互关系，以将所需的不同数据源、不同层的地理目标统一融合到一个图层中。

在传统的GIS中往往会根据人们对地理目标的认知习惯将不同种类的地理目标划分为一层，按层来进行存储和管理。但是一方面由于对现实世界的建模不同，不同的数据源通常会将现实世界划分为不同的层；另一方面在进行地理信息匹配时，关注的不再是这些依据属性信息划分的层，而是具体地理目标的特征信息。因此在数据预处理阶段，需要将原有不同数据源不同层的地理目标统一融合到一个图层中。进行数据融合的主要工作包括：

将不同数据源不同层的地理目标按照统一的特征编码进行重新组织；

将相互覆盖和叠加的地理目标进行重新组合和分解；

重新建立各个地理目标之间的相互关系。

机载计算机存储的地理信息中包括各个地理目标的地面坐标，地理目标中的线状目标几何数据、地理目标的属性特征数据等。

（2）飞行器惯性导航信息的获得；

惯性导航信息包括飞行器姿态角、位置和速度。在飞行器飞行过程中，安装在飞行器上的惯性测量装置，即陀螺仪和加速度计，实时敏感飞行器的运动变化。陀螺仪测量飞行器角速度信息，经过计算可以得到飞行器姿态角，计算过程见《弹道导弹、运载火箭控制系统设计与分析》第113页～115页。加速度计测量飞行器的线加速度信息，经过计算可以得到飞行器的位置和速度，计算过程见《弹道导弹、运载火箭控制系统设计与分析》第115页～117页。即可获得飞行器的惯性导航信息。

（3）在飞行器飞行过程中或仅在飞临典型区域上方时，利用机载传感器实时获取地面景像信息，得到实时图；

（4）从实时图中提取地面要素的特征数据，将提取的特征数据与机载计算机中存储的地理信息进行特征匹配，确定飞行当前位置；

（4.1）地理目标与实时图的粗匹配

这一过程的目的是为实时图的特征提取和特征地理信息特征匹配做前期处理，以减少特征提取和地理信息特征匹配所需的时间。数字影像(实时图)四个角点的像平面坐标（在像片上用以表示像点位置的坐标）是已知的，故可利用其四个角点的像平面坐标、影像实时图中心在地面坐标系中的位置和概略数字高程模型（DEM），计算实时图影像四个角点对应的地面坐标的概略值，即所覆盖的地面区域的概略值。利用机载计算机中存储的实时图覆盖区域内的地理目标的地面坐标和飞行器在获取实时图时刻的概略位置，计算地理目标在实时图上对应的像平面坐标的概略值，将地理目标概略的投射到实时图上，进而实现地理目标与实时图对应像点的粗匹配。

（4.2）实时图的特征提取

特征提取的整个过程是在粗匹配的基础上，对实时图进行金字塔分层，再通过影像增强、边缘检测、边缘细化、边缘跟踪和精化处理等进行线特征提取，处理流程如图2所示，当然还可以采用目前其它方式实现。

影像增强是指采用一系列技术改善图像的视觉效果，提高图像的清晰度，以便于分析某些感兴趣的信息，抑制一些无用的信息。边缘检测是指利用图像一阶导数的极值或二阶导数的过零点信息来提供判断边缘点的基本依据。边缘细化是指把检测出来的双线宽的点变成单线宽。边缘跟踪的目的是获取图像的外部轮廓。边缘跟踪完成后，记录下大量的点，这不利于后续对跟踪结果的处理，精化处理就是进行数据压缩，以便于后续处理。

实时图线特征提取的主要实现过程如下：

（4.2.1）影像金字塔分层

建立影像金字塔的主要目的是降低影像分辨率、减小特征提取和影像匹配搜索的范围，提高特征提取和地理信息匹配的速度。本方法中，采用二像元平均建立影像金字塔的方式，即每2*2=4个像元取平均构成上一层影像的一个像素，每一层影像的像素总数均是其下一层影像像素总数的1/4。

（4.2.2）Canny边缘检测

利用Canny算子进行边缘检测的过程主要包括：利用高斯函数进行低通滤波，计算梯度的幅值及方向，对梯度幅值进行非极大值抑制、检测和连接边缘等。

a.对图像进行低通滤波

Canny算子首先利用二维高斯函数的一阶导数对图像进行低通滤波，设二维高斯函数为：

$G(x,y)=\frac{1}{{2\mathrm{\π\σ}}^2}\exp (-\frac{x^2+y^2}{{2\mathrm{\σ}}^2})$

其梯度矢量为：

$\▿G=\left[\begin{array}{c}\∂G/\∂x\\ \∂G/\∂y\end{array}\right]$

其中：

$\frac{\∂G}{\∂x}=\mathrm{kxexp}(-\frac{x^2}{{2\mathrm{\σ}}^2})\exp (-\frac{y^2}{{2\mathrm{\σ}}^2})=h_1\left(x\right)h_2\left(y\right)$

$\frac{\∂G}{\∂y}=\mathrm{kyexp}(-\frac{x^2}{{2\mathrm{\σ}}^2})\exp (-\frac{y^2}{{2\mathrm{\σ}}^2})=h_1\left(y\right)h_2\left(x\right)$

式中，k为常数，σ为高斯滤波器参数，控制着对图像进行平滑的程度。

b.计算梯度的幅值和方向

Canny算法采用2×2邻域一阶偏导的有限差分来计算平滑后的图像的梯度幅值和方向，x方向和y方向偏微分的一阶近似通过下式得到：

$P_x=\frac{1}{2}\×\left[\begin{array}{cc}-1& 1\\ -1& 1\end{array}\right]$ $P_y=\frac{1}{2}\×\left[\begin{array}{cc}1& 1\\ -1& -1\end{array}\right]$

梯度的大小和方向计算公式如下：

$M(i,j)=\sqrt{P_x^2[i,j]+P_y^2[i,j]}$

$\mathrm{\θ}(i,j)=\arctan \left(P_y^2\right[i,j]/P_x^2[i,j\left]\right)$

c.对梯度幅值进行非极大值抑制

幅值阵列M[i,j]的值越大，其对应得图像梯度值也越大，但这不足以确定边缘。为了精确定位边缘，必须细化幅值图像M[i,j]的屋脊带，只保留幅值局部变化最大的点，这一过程称为非极大值抑制。Canny算子在此过程中使用3×3大小，包括8个方向的邻域对梯度幅值阵列M[i,j]的所有像素沿梯度方向进行梯度幅值的插值。如果邻域中心点的幅值m[i,j]不比梯度方向上的2个插值结果大，则将m[i,j]对应的边缘标志位赋值为0，把M[i,j]宽屋脊带细化为一个像素宽，并且保留了屋脊的梯度幅值。

d.检测和连接边缘

Canny算子采用双阈值法对经过非极大值抑制后的结果进行分割得到两个阈值边缘图像T_h[i,j]和T_l[i,j]。图像T_h[i,j]由高阈值得到，因此不含假边缘，但可能在边缘位置上存在间断。双阈值法是在T_h[i,j]图像中把边缘连接成为轮廓，当达到轮廓端点时就在由低阈值得到的边缘图像T_l[i,j]的相应八邻域位置搜索可以连接到轮廓上的边缘，利用递归跟踪的算法直到将高阈值边缘图像T_h[i,j]中的间隙连接起来为止。

（4.2.3）五邻域边缘跟踪

五邻域边缘跟踪算法减少了跟踪过程的运算量，其跟踪链的长度能够适应特征匹配对提取结果的要求。这一算法对跟踪方向进行了限制，在前进方向y值单调不减,即保证跟踪的链表中下一个点的y坐标不小于前一个点的y坐标，减少了跟踪过程的时间冗余。

（4.2.4）数据简化

由于在本方法中对匹配速度要求很高，因此在保证匹配点数量的前提下，需尽量减少数据量。数据简化过程对提取结果进一步进行了优化，一是筛选提取结果中，数据链相对较长（采用像元个数计量）的线特征，并对线特征的数量加以限制；二是对满足条件的线特征做压缩处理，系统采用了道格拉斯普克算法。

（4.3）实时图与地理目标的精确匹配

这一过程主要是实现地理信息的全自动精确匹配，即在实时图与地理目标之间建立一种数据对应关系，为后续的定位提供精确的地面点坐标和相应的像点坐标。

本方法采用分层分级匹配和整体匹配相结合的匹配策略，利用实时图提取的线特征作为匹配的共轭实体；以线特征的角度、线特征的长度、相交线特征之间的夹角作为匹配的匹配实体；以构造代价函数作为相似性测度；通过计算匹配实体间的相似性测度来实现共轭实体间的精确匹配。分层分级匹配是指利用影像金字塔分层模型，先与较低分辨率实时图提取的特征进行匹配，匹配明显的、粗壮的特征，然后利用前一步的匹配结果，进行下一级匹配，提高匹配的精度和可靠性。

具体实现过程如下：

a.对提取线特征上的一个拐角参量，遍历一条GIS坐标链的每一个拐角，计算提取拐角与GIS坐标链每个拐角的相似性度量，对相似性度量设定一阈值，记录相似性度量阈值范围内对应的GIS数据拐角信息（不唯一）、提取拐角信息（唯一）。相似性度量是由拐角大小和组成拐角的两线段中其中一条线段的方向角的函数，记为：

g(j)=p₁|θ_t-θ_g|+p₂|α_t-α_g|

θ_t为提取拐角的大小；θ_g对应GIS数据拐角大小；α_t为组成提取拐角的两线段中其中一条线段的方向角的大小；α_g为对应GIS数据组成拐角的两线段中其中一条线段的方向角的大小；p₁、p₂为相应的权值；

b.该提取线特征上的其他拐角，对上述GIS坐标链进行上述a中的处理；

c.计算该提取线特征与上述GIS坐标链的整体的相似性度量，相似性度量记为：

$f\left[j\right]=\min \mathrm{\Σ}\left\{p_1\frac{|d_t-d_{g_i}|}{\mathrm{\φ}}{+p}_2\right|{\mathrm{\α}}_t-{{\mathrm{\α}}_g}_i\left|\right\}$

d_t是步骤a中记录的提取线特征中，两个相邻角点之间的距离，是对应的GIS数据两个相邻角点之间的距离（可能不唯一）；α_t是步骤a中记录的提取线特征中，两个相邻角点之间连线的方向角；是对应的GIS数据两个相邻角点之间连线的方向角（可能不唯一），p₁、p₂为相应的权值，φ由GIS数据的精度确定；

d.对其余的GIS坐标链进行a、b、c，求minf[j]，并记录相应的GIS数据拐角信息（唯一）、提取拐角信息；

e.对提取的其他线特征重复以上步骤。

判断精确匹配后得到的地理目标控制点和其对应的实时图像点的坐标的个数是否满足空间后方交会的要求，若能够满足则转步骤（4.5）；否则转步骤（4.4），对地理目标进行检测和匹配，得到至少三对不在同一条直线的地理目标控制点与对应的实时图像点，转步骤（4.5）；

（4.4）利用hough变换进行检测和匹配

利用hough变换进行检测和匹配，得到至少三对不在同一条直线的地理目标点与对应像点。由于利用hough变换进行检测和匹配的方法要求地理目标对应在实时图上像点坐标的精度较高，而利用概略的飞行器摄影时刻的位置信息解得的像点坐标可能达不到精度要求，故可利用已经得到的一对或两对控制点及其对应像点坐标解得一组粗匹配参数对地理目标对应像点进行改正，以方便后续hough变换的检测和匹配。

（4.5）确定飞行器的当前位置和姿态

采用带粗差剔除的空间后方交会方法确定飞行器的当前位置和姿态，步骤概括如下：首先对精确匹配中得到的地理目标点与对应像点的精确坐标进行筛选分组（每组至少有三对地理目标点与对应像点，且使得点的分布尽量均匀），对每一组点经过确定飞行器位置信息初值、组建误差方程、线性化、迭代求解等一系列过程确定摄影时刻飞行器的位置和姿态，然后剔除含粗差的位置和姿态所对应的地理目标点和相应像点，最终解算得到精确的结果。带粗差剔除的空间后方交会方法实现过程可以参见张保明、龚志辉、郭海涛编著的由测绘出版社2008年出版的《摄影测量学》，66-71页。

（5）根据步骤（2）中获取的飞行器惯性导航信息与步骤（4）中确定的飞行器当前位置、姿态进行信息融合，估计惯性导航误差，并利用估计的惯性导航误差修正飞行器的导航参数；

信息融合具体实现步骤如下。

a.地理信息匹配定位信息包括飞行器精确的位置和姿态角信息，

b.惯性导航信息包括飞行器的位置、姿态角、速度信息，惯性导航信息与飞行器真实状态之间存在误差，且该误差具有随时间积累的特点。

c.将地理信息匹配定位信息作为滤波器的量测信息，进行滤波计算，得到经过信息融合的飞行器位置点和姿态，将该位置点和姿态与步骤（2）中获取的飞行器惯性导航信息求差，估计出惯性导航系统的误差，修正惯性导航参数。

由于地理信息匹配定位信息具有非均匀间隔输出且量测滞后的特点。针对这些特点，本方法设计了改进的卡尔曼滤波算法，在解决量测信息非均等间隔输出问题的基础上，解决了量测信息延时的问题。滤波器模型的设计过程如图3所示，具体如下：

（5.1）状态方程的建立

通过对飞行器惯性导航系统（INS）的性能及误差源分析，可以获得INS系统的状态方程。本方法选用捷联惯性导航系统SINS，选取15个状态量，分别为飞行器三维速度误差、三维位置误差、平台失准角、加速度计常值偏置和陀螺的常值漂移。

a.SINS速度误差方程

$\mathrm{\δ}\stackrel{\·}{V}=\stackrel{\·}{W}\×\mathrm{\Φ}-\frac{\mathrm{\μ}}{{\left|R\right|}^3}(I-\frac{3}{{\left|R\right|}^2}{\mathrm{RR}}^T)\mathrm{\δR}+C_B^I(K_0+{\▿}_1)$

其中，加速度计噪声、引力模型误差和引力异常等误差统一用白噪声描述。

b.位置误差方程

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{X}\\ \mathrm{\δ}\stackrel{\·}{Y}\\ \mathrm{\δ}\stackrel{\·}{Z}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\δV}}_x\\ {\mathrm{\δV}}_y\\ {\mathrm{\δV}}_z\end{array}\right]$

c.平台误差角误差方程

$\stackrel{\·}{\mathrm{\Φ}}{C}_B^I{D}_0+C_B^I{\mathrm{\ϵ}}_1$

式中，陀螺的测量误差用白噪声ε₁描述。

d.惯性元件误差方程

$\begin{array}{ccc}{\stackrel{\·}{K}}_{0x}=0& {\stackrel{\·}{K}}_{0y}=0& {\stackrel{\·}{K}}_{0z}=0\\ {\stackrel{\·}{D}}_{0x}=0& {\stackrel{\·}{D}}_{0y}=0& {\stackrel{\·}{D}}_{0z}=0\end{array}$

（5.2）量测方程的建立

本过程的目的在于建立量测量与状态量的关系。本方法选取观测量为三个位置误差量和三个姿态角误差量。

a.位置量测方程

$\left[\begin{array}{c}{X}_C-X_{\mathrm{GIS}}\\ Y_C-Y_{\mathrm{CIS}}\\ Z_C-Z_{\mathrm{GIS}}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]*\left[\begin{array}{c}{\mathrm{\δV}}_x\\ {\mathrm{\δV}}_y\\ {\mathrm{\δV}}_z\end{array}\right]+\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]$

式中，带下标C表示惯导计算的位置信息，带下标GIS表示地理信息辅助导航输出的位置信息。ν(t)=[ν_x,ν_y,ν_z]^T为地理信息辅助导航输出的位置量测噪声。

b.姿态角量测方程

其中：为地理信息辅助导航输出的姿态量测噪声。

（5.3）卡尔曼滤波算法的设计

设计卡尔曼滤波算法，解决量测滞后和量测信息非等间隔输出的滤波问题。本方法利用保存的上一时刻的滤波状态参数和当前时刻获取的量测信息进行滤波计算，推导出当前时刻的状态量，对INS状态参数作修正，以解决量测滞后的问题。本方法利用系统状态转移阵设计非等间隔卡尔曼滤波算法，以解决非等间隔量测的问题，算法原理如图4所示。

当没有量测信息输出时，在每个滤波周期只进行时间更新。考虑到组合系统误差主要由惯导自身的误差状态方程决定，故仍可对误差阵不断的进行递推计算，可以推导出协方差阵P(k)的形式为：

$P_k={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

非等间隔卡尔曼滤波算法的具体实现过程如下：

a.系统从组合工作状态进入纯INS状态

设k时刻由组合状态转入纯惯导状态，系统内保留了（k-1）时刻的P_k-1,k-1的值，令P_k-1=P_k-1,k-1，则纯惯导工作时的协方差阵计算可以继续。

b.系统从纯INS状态进入组合工作状态

设k时刻由纯惯导状态进入组合工作状态，系统内保留了（k-1）时刻的P_k-1，令P_k-1,k-1=P_k-1，此时K_k不再为零，组合卡尔曼滤波器的计算可以继续下去。

（5.4）滤波估计

设计滤波器模型涉及到状态方程、量测方程和滤波算法的设计，这些都已经在步骤（5.1）-（5.3）中得到实现。设置滤波器参数初始值，就可以进行滤波估计计算。

（6）飞行器飞行过程中，机载计算机按照预先设定的规则（以等时间间隔或其它约束方式）将步骤（5）经过信息融合的飞行器位置信息存储在机载计算机的地理信息中，并根据经过信息融合的飞行器位置信息进行在线构图；

在本方法中在线构图的实现原理如下：

飞行器在飞行过程中按照等时间间隔将经过信息融合的飞行器位置点依次存储在机载计算机的地理信息中，并根据各位置点信息，利用曲线拟合的方法，实时生成飞行器的航迹线，以实现在线构图。所述的曲线拟合方法可参考由王尊正主编的《数值分析基本教程》（哈尔滨工业大学出版社，1993）P220～P234。本方法所述的在线构图方法是不唯一的，只要能够准确、快速生成飞行器航迹线的方法均可使用。

（7）飞行器在返航过程中，根据在线构图的结果以及飞行器的任务特点实时进行航迹规划，完成飞行引导。

飞行器的工作方式根据飞行器的任务特点存在两种，一种为标准轨迹跟踪，另一种为基于航路点的预测制导。

方式一：

标准轨迹跟踪

根据在线构图的结果，即返航参考轨迹，选择或设计相应的算法，进行返航轨迹跟踪。算法的设计需要考虑软件的时空开销、机载计算机的硬件条件、飞行器控制系统的要求等。上述的规则和算法不是唯一的，可根据具体需求进行设计。本发明选用了LQR方法，即线性二次调节器跟踪参考轨迹，跟踪控制律为：

δu=-K(t)δx(t)

其中δx表示实际轨迹与参考轨迹的状态偏差，δu=δσ为相对标准控制量的修正量，K(t)为反馈增益。

求解K(t)的算法流程如下：

a.对飞行器运动方程在参考轨迹附近线性化，得到求解反馈增益的线性时变系统；

b.建立标准的线性二次型性能指标：

$J(t,t_f)={\∫}_t^{t_f}[{\mathrm{\δx}}^T\left(\mathrm{\τ}\right)\mathrm{Q\δx}\left(\mathrm{\τ}\right)+{\mathrm{\δu}}^T\left(\mathrm{\τ}\right)\mathrm{R\δu}\left(\mathrm{\τ}\right)]\mathrm{d\τ}$

为使性能指标最小，得到Riccati方程：

PA-PBR^-1B^TP+Q+A^TP=0

反馈增益为：K(t)=-R^-1B^T(t)P(t)

c.将飞行器当前时刻的飞行状态与参考状态比较，得到状态偏差，计算求解A,B,Q,R矩阵；

d.将得到的A,B,Q,R矩阵代入Riccati方程和反馈增益计算方程得到K(t)。

将求解的控制量应用到飞行器，控制飞行器飞向预定的飞行路线或目标，实现返航引导。

方式二：

基于航路点的预测制导

以在线构图的结果为依据，根据需要，按照相关准则将飞行器参考飞行轨迹分成若干段，每段的结束点设为航路点。飞行过程中，以航路点为目标点，设计基于航路点的预测制导算法，获取满足要求的控制量，控制飞行器飞向预定的目标。本发明采用了静态分段规划方法，即利用参考轨迹中对控制量的分析，以控制量的分段点作为轨迹的航路点，航路点的轨迹状态信息事先存储在机载计算机中，以到达第i个航路点时的状态参数为初始状态，以第i+1个航路点状态参数为目标状态。求解控制量的算法步骤如下：

a.控制变量参数化

控制量参数化的形式为：

其中k为调节参数，为修正函数。这时的制导方程是寻找满足一定性能指标要求的调节参数k和修正函数选取k和有不同的方法，例如可提出某一性能指标，用优化的方法选择使之满足要求。本发明中取最简单的情况，令

b.控制量迭代校正算法

飞行器飞行过程中，实时计算目标参数，与预先存储的航路点目标状态比较，依据终端偏差迭代计算出满足目标点精度要求的控制量，算法可以采用各种优化算法实现，例如牛顿迭代法。

将得到的控制量应用到飞行器，控制飞行器到达第i+1个航路点，重复上面的步骤直至到达最后一个航路点即终点，实现返航。

本发明未详细说明部分属于本领域技术人员公知常识。

Claims (2)

1.一种基于在线构图的飞行器返航路线规划方法，其特征在于步骤如下：

(1)在飞行器的预定飞行线路上选择一系列典型区域，事先测绘出关于整个飞行线路上或仅仅是选择的典型区域的地理信息存储在机载计算机中；

(2)在飞行器飞行过程中，由机载惯性测量装置实时敏感飞行器的运动变化，根据飞行器的运动变化，实时获取飞行器的惯性导航信息；

(3)在飞行器飞行过程中或仅在飞临典型区域上方时，利用机载传感器实时获取地面景像或其它地理信息，得到实时图；

(4)从实时图中提取地面要素的特征数据，将提取的特征数据与机载计算机中存储的地理信息进行特征匹配，确定飞行器的当前位置；

(5)根据步骤(2)中获取的飞行器当前飞行状态参数与步骤(4)中确定的飞行器当前位置进行信息融合，估计惯性导航误差，并利用估计的惯性导航误差修正飞行器的导航参数；

(6)飞行器飞行过程中，机载计算机按照预先设定的规则将步骤(5)经过信息融合的飞行器位置信息存储在机载计算机的地理信息中，并根据经过信息融合的飞行器位置信息进行在线构图；

(7)飞行器在返航过程中，根据在线构图的结果以及飞行器的任务特点实时进行航迹规划，完成飞行引导；具体如下：

(7.1)以在线构图的结果为依据，将飞行器参考飞行轨迹分成若干段，每段的结束点设为航路点；预先在机载计算机中存储航路点的轨迹状态信息；

(7.2)飞行过程中，以航路点为目标点，以到达第i个航路点时的状态参数为初始状态，以第i+1个航路点状态参数为目标状态，实时计算目标参数，与预先存储的航路点目标状态比较，依据终端偏差迭代计算出满足目标点精度要求的控制量；

(7.3)将得到的控制量应用到飞行器，控制飞行器到达第i+1个航路点，重复上面的步骤直至到达最后一个航路点即终点，实现返航。

2.根据权利要求1所述的一种基于在线构图的飞行器返航路线规划方法，其特征在于：所述步骤(1)中的地理信息存储在机载计算机前进行预处理，该预处理包括以下步骤：

(1.1)从典型区域中的地理信息数据中提取能够用于匹配定位的地理目标；

(1.2)对上述地理目标中的线状目标几何数据进行化简，保留曲线上形态特征明显的点，舍去曲线上形态特征不明显的点；

(1.3)对地理目标的属性特征数据进行化简，保留地理目标的类型和匹配中所需的描述信息；

(1.4)提取地理目标的几何特征信息；

(1.5)将不同数据源、不同图层的地理目标按照统一的特征编码进行重新组织，重新组合和分解相互覆盖或叠加的地理目标，并重新建立各个地理目标之间的相互关系，以将所需的不同数据源、不同层的地理目标统一融合到一个图层中。