专利名称：基于联邦滤波的sins/smans/trns组合导航方法及系统的制作方法
技术领域：
本发明涉及组合导航定位技术领域，尤其涉及一种基于联邦滤波的SINS/SMANS/ TRNS组合导航方法及系统。
背景技术：
“导航是正确引导飞行器沿着预定的航线在规定时间内到达目的地。为了完成这个任务，需要随时知道飞行器的瞬时地理位置、航行速度、航行的姿态航向等参数。这些参数，通常称作导航参数。对有人驾驶的飞行器，这些导航参数可由领航员通过观察仪表和计算得到。但是，随着速度和航程的不断增大，对导航的要求越来越高；为了减轻和代替领航员的工作，就出现了各种导航系统，可以自动的提供需要的各种导航参数。惯性导航是一种自主性的导航方法。它完全依靠机载设备自主的完成导航任务， 和外界不发生任何光、电联系。因此，隐蔽性好，工作不受气象条件的限制。这一独特的优点，使惯导系统在导弹、舰船、飞机、宇宙航行器上得到了广泛的应用，在导航技术中占有突出的地位。惯性导航是用陀螺仪和加速度计提供的测量数据确定所在运载体的位置、速度和姿态导航参数。通过这两种测量的组合，就可以确定该运载体在惯性坐标系的平移运动并计算它的位置。捷联惯性导航(SINQ系统通过把敏感器固连(或固定)在运载体的壳体上而去除了平台系统大部分的机械复杂性。这种方法的潜在好处是成本降低、尺寸减少、可靠性提高。小型、精确的捷联惯性导航系统可以装到各种飞行器上，所带来的主要问题是计算复杂性显著增加，而且需要能测量高转速的器件。然而，计算机技术的不断进步与适用敏感器的开发相结合，使这种设计成为现实。捷联惯性导航的缺点是它所提供的位置估算精度会随时间而漂移。在长时间范围内，导航误差增长的速度主要由初始对准精度、系统所使用的惯性敏感器缺陷和运载体运动轨迹的动态特性决定。虽然采用更精确的敏感器可以提高精度，但惯性系统的成本会变得极为昂贵，且提高的精度也是有限的。近年来，为了解决捷联惯导系统的误差漂移问题， 一种适于多种应用的方法是组合导航技术，组合导航是采用一种或多种辅助导航系统对惯性导航进行修正。其中，景象匹配辅助导航系统和地形参考导航系统就是两种高自主性的导航系统。景象匹配辅助导航(SMANQ是利用机载或弹载图像传感器在飞行过程中采集的实时地形景象图与预先制备的基准地形景象图进行实时匹配计算而获得精确定位信息的技术。景象匹配导航属自主定位，可以为飞行器提供高精度制导，导航精度与飞行距离无关，且成本相对较低。景象匹配辅助导航采用一个成像系统建立起飞机向前飞行时其下方的地形图片，需要位置坐标时，一部分扫描图像被存储起来形成飞机下方地形的“景象”。通过这个过程，图像被转化成一个“像素”阵列，每个像素都有一个表示那部分图像亮度的数值。对“捕获”的景象进行处理，以去除噪声和增强那些可能提供导航信息的特征，再采用相关算法寻找预先存储在地面特征数据库中可识别的图形。发现景象中的特征与数据库中的特征匹配后，根据飞机的姿态和离地高度进行几个计算，就能够计算出景象在被捕获瞬间的位置。地形参考导航(TRNS)技术是近年来受到广泛重视并已成功使用的辅助导航系统，也是一种自主、隐蔽、全天候、导航定位精度与航程无关的低空导航技术。最常用的地形参考系统采用一个无线电高度表、一个机载气压惯性导航系统和一个存储的飞行器飞过区域的地形轮廓图。无线电高度表测量出地面的高度，结合惯性导航系统对海拔高度的估值， 可以在运载体上的计算机再现飞行路线下面的地面轮廓。然后将得到的地面轮廓与存储的地形图数据进行比较以实现相符性匹配，由此可以确定运载体的位置。
发明内容
(一)要解决的技术问题本发明要解决的技术问题是提供一种基于联邦滤波的SINS/SMANS/TRNS组合导航方法及系统，以有效提高导航系统的精度，并具有高容错性、高自主性和高可靠性。(二)技术方案为解决上述问题，本发明提供了一种基于联邦滤波的SINS/SMANS/TRNS组合导航方法，包括以下步骤Sl 景象匹配辅助导航采用角点方法进行图像匹配，通过飞行器的姿态和高度转换，来确定飞行器的位置；S2 地形参考导航采用地形匹配方法进行地形匹配，通过比较实际相对高度和惯导相对高度，最终确定飞行器的位置；S3 建立捷联惯性导航系统的误差模型，以及景象匹配辅助导航和地形参考导航的观测模型；S4:对捷联惯性导航、景象匹配辅助导航和地形参考导航的输出进行信息融合，得出最优估计结果，并对捷联惯性导航系统进行校正。优选地，所述步骤Sl具体包括以下步骤Sll 输入航拍图像和待匹配区域图像，分别对两个图像进行特征点的提取，对图像进行尺度空间表示并进行三维定位；S12:通过确定兴趣点的方向特征来构建兴趣点的描述子向量，采用基于最近距离比次近距离的匹配方法进行特征点匹配，并对误匹配点对进行剔除；S13:基于匹配成功的兴趣点对来确定所述航拍图像和待匹配区域图像的单应矩阵，根据所述单应矩阵提供的两幅图像的位置和旋转关系，确定航拍图在待匹配区域地图中的位置，从而确定飞行器的飞行位置。优选地，所述兴趣点的描述子向量可为64维或128维构成。优选地，所述步骤S2具体包括以下步骤S21 输入地形高程数据，并建立地形线性化模型；S22 根据飞行器水平位置误差计算地形高程差，再根据飞行器的绝对高度差和所述地形高程差计算地形相对高度差；S23:将计算所得地形相对高度差与观测所得相对高度差进行比较，来修正并最终确定飞行器的位置。
优选地，所述步骤S4具体包括以下步骤S41 对信息融合后的状态和方差阵进行初始化；S42:构成捷联惯导/景象匹配辅助导航子滤波器和捷联惯导/地形参考导航子滤波器，对各子滤波器的状态、方差阵和状态噪声阵进行信息分配；S43 将捷联惯导/景象匹配辅助导航子滤波器和捷联惯导/地形参考导航子滤波器分别进行各自的时间更新和观测更新，得到各子滤波器的估计信息；S44 将所有子滤波器的估计信息融合成为全局状态最优估计信息。另一方面，本发明还提供了一种实现上述组合导航方法的组合导航系统，包括大气惯性导航系统，用于得到惯导位置信息并输出；飞行轨迹发生器模块，用于模拟飞行器飞行轨迹，得到飞行器的位置、速度和姿态 fn息；SINS/SMANS组合导航系统，包括图像传感器视区及定位参数计算模块，用于根据所述大气惯性导航系统输出的位置信息计算机载图像传感器的视区及定位参数；数字参考地图数据库，用于根据所述图像传感器视区及定位参数计算模块的计算结果得到合适的数字参考地图；图像传感器模拟模块，用于根据所述飞行轨迹发生器模块得到的飞行器的姿态和高度转换信息，模拟生成机载图像传感器拍摄的实时图像；图像匹配模块，用于对由所述数字参考地图数据库获得的所述数字参考地图和由所述图像传感器模拟模块模拟的实时图像进行配准，计算得出飞行器的实时位置；SINS/SMANS卡尔曼子滤波器模块，用于根据大气惯性导航系统输出的惯导位置信息和图像匹配模块输出的飞行器实时位置信息进行信息融合；SINS/TRNS组合导航系统，包括激光测高仪，用于接收飞行轨迹发生器模块获得的实际绝对高度信息和地形匹配模块获得的实际高程信息，得到实测相对高度并输出；地形高程数据库，用于提供地形数据；地形匹配模块，用于根据大气惯性导航系统输出的惯导位置信息、飞行轨迹发生器模块输出的实际位置信息以及所述地形高程数据库提供的地形数据输出实际高程信息、 输出地形斜率和惯导高程信息；SINS/TRNS卡尔曼子滤波器模块，用于根据所述激光测高仪、地形匹配模块和大气惯性导航系统的输出信号进行信息融合；联邦滤波模块，用于对SINS/SMANS卡尔曼子滤波器模块和SINS/TRNS卡尔曼子滤波器模块输出的信号进行融合，得到最终融合的位置信息结果，同时对所述大气惯性导航系统进行误差校正。(三)有益效果本发明将惯性导航与适用于大面积平坦或无特征的地域的景象匹配辅助导航和适用于地形比较粗糙或变化剧烈的区域的地形参考导航形成组合导航系统，通过景象匹配辅助导航和地形参考导航有效修正惯性导航的漂移误差，有效提高导航系统的精度，并具有高容错性、高自主性和高可靠性。
图1为根据本发明实施例组合导航方法的步骤流程图；图2为根据本发明实施例组合导航方法步骤1的流程图；图3为根据本发明实施例组合导航方法步骤2的流程图；图4为根据本发明实施例组合导航方法步骤4的流程图；图5为实施本发明方法的组合导航系统的联邦滤波融合结构的简化示意图；图6为实施本发明方法的组合导航系统的结构示意图。
具体实施例方式下面结合附图及实施例对本发明进行详细说明如下。实施例一如图1所示，本实施例记载了一种基于联邦滤波的SINS/SMANS/TRNS组合导航方法，包括以下步骤Sl 景象匹配辅助导航采用角点方法进行图像匹配，通过飞行器的姿态和高度转换，来确定飞行器的位置；如图2所示，所述步骤Sl具体包括以下步骤Sll 输入航拍图像和待匹配区域图像，分别对两个图像采用例如框状滤波器近似 Hessian矩阵的方法进行特征点的提取，对图像进行尺度空间表示并进行三维定位；S12:通过确定兴趣点的方向特征来构建兴趣点的描述子向量，采用基于最近距离比次近距离的匹配方法进行特征点匹配，并通过RANSAC算法对误匹配点对进行剔除；S13:基于匹配成功的兴趣点对来确定所述航拍图像和待匹配区域图像的单应矩阵，根据所述单应矩阵提供的两幅图像的位置和旋转关系，确定航拍图在待匹配区域地图中的位置，从而确定飞行器的飞行位置。S2 地形参考导航采用 SITAN(Sandia Inertial Terrain Aided Navigation，桑迪亚惯性地形辅助导航系统)地形匹配方法进行地形匹配，通过比较实际相对高度和惯导相对高度，并最终确定飞行器的位置；如图3所示，所述步骤S2具体包括以下步骤S21 输入地形高程数据，并建立地形线性化模型；S22 根据飞行器水平位置误差(Δ χ，Δ y)计算地形高程差Δ h1 则地形相对高度差Δ、可表示为Ahr = Δ h- Δ hx其中，Ah为飞行器绝对高度差；S23:将计算所得地形相对高度差与观测所得相对高度差进行比较，来修正飞行器的位置，最终确定飞行器的位置。S3 建立捷联惯性导航系统的误差模型，以及景象匹配辅助导航和地形参考导航的观测模型；其中，捷联惯性导航系统的误差模型为X = FX+Gw
X = [δφ, δλ, Sh,SvE, 5vNf其中X为系统状态向量，F为5X5系统矩阵，G为系统噪声输入阵，w为系统噪声向量，知，δ λ, 5h, δνΕ, δ νΝ分别为纬度误差，经度误差，高度误差，东向速度误差和北向速度误差，其非零元为3) = - V"5) = -^―
(Rn+hfRm+hF(2，O = JE , tan φ sec cpF(X 3) = - Vg sec φ
Rn+h(Rn+h)
Rn+h
F(4,1) = 2coievN cos φ + VeVn sec2 φ + IoieVv sin φ
RN+h^(4,3) = ^v -vgv=
(Rn+hfRm+h Rm+h
F(4,5) = 2ω sin ￠7 H———~tan 炉
Rn+hF(5,l) =cos^4^)F(5,3) = #^ + 7
Rn+h(Rn+hf (Rm+hf芦(5,4)= sin φ + —tan cp)F、5,5) = Ju 。
RN+hRM+h景象匹配辅助导航的观测模型为Zsmans — Hsmansx+VsmansHsmans = [I2X2 O2X3]其中，X为捷联惯性导航系统的误差模型，Zsmans为景象匹配辅助导航的观测量， Vsmans为景象匹配辅助导航的观测噪声。地形参考导航的观测模型为Ztens 一 Htensx+VtensHtens = [-kx -ky 1 0 0]其中，Ztkns为地形参考导航的观测量，Vtkns为地形参考导航的的观测噪声，kx、ky分别为X方向和y方向的地形斜率。S4:对捷联惯性导航、景象匹配辅助导航和地形参考导航输出进行信息融合，得出最优估计结果，并对捷联惯性导航系统进行校正。所述兴趣点的描述子向量可为64维或128维构成。如图4所示，所述步骤S4具体包括以下步骤S41 设置信息融合后的状态函数^和方差阵函数Pf，并分别对二者进行初始化； 即令
Xf (0) = X(O)，Pf (0) = P (0)S42 构成SINS/SMANS子滤波器和SINS/TRNS子滤波器，对各子滤波器的状态、方差阵和状态噪声阵进行信息分配即对第i个子滤波器的状态&、方差阵？1和状态噪声阵A 进行信息分配令Xl(^k) = Xf(k)P1(R) = p:\k)Pf{k)QAk) = p:\k)Qf{k)其中~、Pf和％分别为融合后的状态、方差阵和状态噪声阵，β i为信息分配因子， k 彡 1，i = 1，2。β i 满足β !+β 2 = 1。S43:将捷联惯导/景象匹配辅助导航子滤波器和捷联惯导/地形参考导航子滤波器分别进行各自的时间更新和观测更新，得到各子滤波器的估计信息(1)时间更新后得到X (k Ik-D = φ (k |k-l) X (k-1)P (k I k-1) = φ (k I k-1) P (k-1) φT (k | k-1)+Q (k-1)X (k-1) = X (k I k-1)P (k-1) = P (k I k-1)其中，χ为滤波器的状态向量，P为滤波器的方差阵，Φ为F矩阵对应的系统状态转移阵；(2)观测更新后得到K (k) =P (k I k-1) Ht (k) (H (k) P (k I k-1) Ht (k) +R (k))x(k) =x (k I k-1)+K (k) (ζ (k) -H (k) χ (k I k-1))P(k) = (I-K (k) H (k)) P (k I k-1)其中，K为卡尔曼滤波增益阵，H为观测矩阵。S44 将各子滤波器的估计信息融合成为全局状态最优估计信息 P}l{k) = p :{ky±p-\k)
i=\X7(k) = Pf (k)\pml {k)xm(k)+ ^(k) X1 (k)
LZ=I_
ο实现步骤4的联邦滤波融合结构如图5所示，SINS和SMANS系统获得的信号被输入SINS/SMANS子滤波器，SINS和TRNS系统获得的信号被输入SINS/TRNS子滤波器，经过更新等处理后，融合后输出。实施例二如图6所示，本实施例记载了一种实现上述组合导航方法的组合导航系统，包括大气惯性导航系统，用于得到惯导位置信息并输出；飞行轨迹发生器模块，用于模拟飞行器飞行轨迹，得到飞行器的位置、速度和姿态 fn息；SINS/SMANS组合导航系统，包括
图像传感器视区及定位参数计算模块，用于根据所述大气惯性导航系统输出的位置信息计算机载图像传感器的视区及定位参数；数字参考地图数据库，用于根据所述图像传感器视区及定位参数计算模块的计算结果得到合适的数字参考地图；图像传感器模拟模块，用于根据所述飞行轨迹发生器模块得到的飞行器的姿态和高度转换信息，模拟生成机载图像传感器拍摄的实时图像；图像匹配模块，用于对由所述数字参考地图数据库获得的所述数字参考地图和由所述图像传感器模拟模块模拟的实时图像进行配准，计算得出飞行器的实时位置；SINS/SMANS卡尔曼子滤波器模块，用于根据大气惯性导航系统输出的惯导位置信息和图像匹配模块输出的飞行器实时位置信息进行信息融合；SINS/TRNS组合导航系统，包括激光测高仪，用于接收飞行轨迹发生器模块获得的实际绝对高度信息和地形匹配模块获得的实际高程信息，得到实测相对高度并输出；地形高程数据库，用于提供地形数据；地形匹配模块，用于根据大气惯性导航系统输出的惯导位置信息、飞行轨迹发生器模块输出的实际位置信息以及所述地形高程数据库提供的地形数据输出实际高程信息、 输出地形斜率和惯导高程信息；SINS/TRNS卡尔曼子滤波器模块，用于根据所述激光测高仪、地形匹配模块和大气惯性导航系统的输出信号进行信息融合；联邦滤波模块，用于对SINS/SMANS卡尔曼子滤波器模块和SINS/TRNS卡尔曼子滤波器模块输出的信号进行融合，得到最终融合的位置信息结果，同时对所述大气惯性导航系统进行误差校正。以上实施方式仅用于说明本发明，而并非对本发明的限制，有关技术领域的普通技术人员，在不脱离本发明的精神和范围的情况下，还可以做出各种变化和变型，因此所有等同的技术方案也属于本发明的范畴，本发明的专利保护范围应由权利要求限定。
10
权利要求
1.一种基于联邦滤波的SINS/SMANS/TRNS组合导航方法，其特征在于，包括以下步骤 Sl 景象匹配辅助导航采用角点方法进行图像匹配，通过飞行器的姿态和高度转换，来确定飞行器的位置；S2:地形参考导航采用地形匹配方法进行地形匹配，通过比较实际相对高度和惯导相对高度，最终确定飞行器的位置；S3 建立捷联惯性导航系统的误差模型，以及景象匹配辅助导航和地形参考导航的观测模型；S4:对捷联惯性导航、景象匹配辅助导航和地形参考导航的输出进行信息融合，得出最优估计结果，并对捷联惯性导航系统进行校正。
2.如权利要求1所述的组合导航方法，其特征在于，所述步骤Sl具体包括以下步骤 Sll 输入航拍图像和待匹配区域图像，分别对两个图像进行特征点的提取，对图像进行尺度空间表示并进行三维定位；S12:通过确定兴趣点的方向特征来构建兴趣点的描述子向量，采用基于最近距离比次近距离的匹配方法进行特征点匹配，并对误匹配点对进行剔除；S13 基于匹配成功的兴趣点对来确定所述航拍图像和待匹配区域图像的单应矩阵，根据所述单应矩阵提供的两幅图像的位置和旋转关系，确定航拍图在待匹配区域地图中的位置，从而确定飞行器的飞行位置。
3.如权利要求2所述的组合导航方法，其特征在于，所述兴趣点的描述子向量可为64 维或1 维构成。
4.如权利要求1所述的组合导航方法，其特征在于，所述步骤S2具体包括以下步骤521输入地形高程数据，并建立地形线性化模型；522根据飞行器水平位置误差计算地形高程差，再根据飞行器的绝对高度差和所述地形高程差计算地形相对高度差；523将计算所得地形相对高度差与观测所得相对高度差进行比较，来修正并最终确定飞行器的位置。
5.如权利要求1所述的组合导航方法，其特征在于，所述步骤S4具体包括以下步骤541对信息融合后的状态和方差阵进行初始化；542构成捷联惯导/景象匹配辅助导航子滤波器和捷联惯导/地形参考导航子滤波器，对各子滤波器的状态、方差阵和状态噪声阵进行信息分配；543将捷联惯导/景象匹配辅助导航子滤波器和捷联惯导/地形参考导航子滤波器分别进行各自的时间更新和观测更新，得到各子滤波器的估计信息；544将所有子滤波器的估计信息融合成为全局状态最优估计信息。
6.一种实现权利要求1-5中任一项所述的组合导航方法的组合导航系统，其特征在于，包括大气惯性导航系统，用于得到惯导位置信息并输出；飞行轨迹发生器模块，用于模拟飞行器飞行轨迹，得到飞行器的位置、速度和姿态信息；SINS/SMANS组合导航系统，包括图像传感器视区及定位参数计算模块，用于根据所述大气惯性导航系统输出的位置信息计算机载图像传感器的视区及定位参数；数字参考地图数据库，用于根据所述图像传感器视区及定位参数计算模块的计算结果得到合适的数字参考地图；图像传感器模拟模块，用于根据所述飞行轨迹发生器模块得到的飞行器的姿态和高度转换信息，模拟生成机载图像传感器拍摄的实时图像；图像匹配模块，用于对由所述数字参考地图数据库获得的所述数字参考地图和由所述图像传感器模拟模块模拟的实时图像进行配准，计算得出飞行器的实时位置；SINS/SMANS卡尔曼子滤波器模块，用于根据大气惯性导航系统输出的惯导位置信息和图像匹配模块输出的飞行器实时位置信息进行信息融合； SINS/TRNS组合导航系统，包括激光测高仪，用于接收飞行轨迹发生器模块获得的实际绝对高度信息和地形匹配模块获得的实际高程信息，得到实测相对高度并输出； 地形高程数据库，用于提供地形数据；地形匹配模块，用于根据大气惯性导航系统输出的惯导位置信息、飞行轨迹发生器模块输出的实际位置信息以及所述地形高程数据库提供的地形数据输出实际高程信息、输出地形斜率和惯导高程信息；SINS/TRNS卡尔曼子滤波器模块，用于根据所述激光测高仪、地形匹配模块和大气惯性导航系统的输出信号进行信息融合；联邦滤波模块，用于对SINS/SMANS卡尔曼子滤波器模块和SINS/TRNS卡尔曼子滤波器模块输出的信号进行融合，得到最终融合的位置信息结果，同时对所述大气惯性导航系统进行误差校正。
全文摘要
本发明公开了一种基于联邦滤波的SINS/SMANS/TRNS组合导航方法及系统，所述方法包括以下步骤景象匹配辅助导航进行图像匹配，通过数字地图和拍摄图像间的仿射变换关系确定飞行器的位置；地形参考导航采用地形匹配方法进行地形匹配，通过高程数据确定飞行器的位置；建SINS的误差模型，以及SMANS和TRNS的观测模型；对SINS、SMANS、TRNS的输出进行信息融合，得出最优估计结果，并对捷联惯性导航系统进行校正。所述系统包括大气惯性导航系统、飞行轨迹发生器模块、SINS/SMANS组合导航系统、SINS/TRNS组合导航系统以及联邦滤波模块。本发明有效提高导航系统的精度，并具有高容错性、高自主性和高可靠性。
文档编号G01C21/24GK102506868SQ20111037186
公开日2012年6月20日 申请日期2011年11月21日 优先权日2011年11月21日
发明者李威, 杨霄, 程农, 胡海东 申请人:清华大学