%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 卡尔曼滤波器在INS-GPS组合导航中应用仿真
% Author : lylogn
% Email : [email protected]
% Company: BUAA-Dep3
% Time : 2013.01.06
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 参考文献:
% [1]. 邓正隆. 惯导技术, 哈尔滨工业大学粗仔出版社.2006.
clear all
%% 惯性-GPS组合导航模型参数初始化
we = 360/24/60/60*pi/180%地球自转角速度，弧度/s
psi = 10*pi/180 %psi角度 / 弧度
Tge = 0.12
Tgn = 0.10
Tgz = 0.10 %这三个参数的含义详见参考文献
sigma_ge=1
sigma_gn=1
sigma_gz=1
%% 连续空间系统状态方程
% X_dot(t) = A(t)*X(t) + B(t)*W(t)
A=[0 we*sin(psi) -we*cos(psi) 1 0 0 1 0 0
-we*sin(psi) 0 0 0 1 0 0 1 0
we*cos(psi) 0 0 0 0 1 0 0 1
00 0 -1/Tge 0 0 0 0 0
00 0 0 -1/Tgn 0 0 0 0
00 0 0 0 -1/Tgz 0 0 0
00 0 0 0 0 0 0 0
00 0 0 0 0 0 0 0
00 0 0 0 0 0 0 0]%状态转移矩阵
B=[00 0sigma_ge*sqrt(2/Tge) 0 0 0 0 0
00 00 sigma_gn*sqrt(2/Tgn) 0 0 0 0
00 00 0 sigma_gz*sqrt(2/Tgz) 0 0 0]'%输入控制矩阵
%% 转化为离散时间系统状态方程
% X(k+1) = F*X(k) + G*W(k)
T = 0.1
[F,G]=c2d(A,B,T)
H=[10 0 0 0 0 0 0 0
0 -sec(psi) 0 0 0 0 0 0 0]%观测矩稿纳阵
%% 卡尔曼滤波器参数初始化
t=0:T:50-T
length=size(t,2)
y=zeros(2,length)
Q=0.5^2*eye(3)%系统噪声协方差
R=0.25^2*eye(2) %测量噪声协方差
y(1,:)=2*sin(pi*t*0.5)
y(2,:)=2*cos(pi*t*0.5)
Z=y+sqrt(R)*randn(2,length) %生成的含有噪声的假定观测值，2维
X=zeros(9,length) %状态估计值，9维
X(:,1)=[0,0,0,0,0,0,0,0,0]' %状态估计初始值设定
P=eye(9) %状态估计协方差
%% 卡尔曼滤波算法迭代过程
for n=2:length
X(:,n)=F*X(:,n-1)
P=F*P*F'+ G*Q*G'
Kg=P*H'/(H*P*H'+R)
X(:,n)=X(:,n)+Kg*(Z(:,n)-H*X(:,n))
P=(eye(9,9)-Kg*H)*P
end
%% 绘图代码
figure(1)
plot(y(1,:))
hold on
plot(y(2,:))
hold off
title('理想的观测量')
figure(2)
plot(Z(1,:))
hold on
plot(Z(2,:))
hold off
title('带有噪声的观测量')
figure(3)
plot(X(1,:))
hold on
plot(X(2,:))
hold off
title('滤波后的观测量')
GNSS+IMU+MM车载组合导航系统
前言：近年来，随着定位业务的迅速发展，用户对于车载端定位精度提出了越来越高的要求，由原来的导航级逐渐更替到车道级。特别是在城市峡谷环境下（高楼、高架），用户无法接收到GNSS信号或GNSS信号受干扰，导致GNSS无定位结果或定位精度差。这是“有源定位”固有的缺点，无法从算法上来克服。针对这个问题，以GNSS+IMU等多传感器融合方案越来越受到重视，因为“无源定位”的IMU恰好可以弥补卫星定位的短板。
基础原理
导航卫星系统(GNSS)
全球导航卫星系统（Global Navigation Satellite System）是一种依靠卫星卫星的伪距载波、星历、时间以及钟差等信息进行实时定位的空基无线电导航系统，能在地球表面或近地空间的任何地点为用户提供全天候的三维坐标和速度以及时间信息。GNSS系统的优点是精度高、误差稳定不发散，但容易受到周围环境影响，比如树木楼房遮挡，镜面等高反射物体引起的多路径效应。
惯性导航系统(IMU)
惯性导航系统（宴信销Inertial Navigation System）是一种不依赖于外部信息、也不向外部辐射能量（如无线电导航那样）的自主式导航系统，主要使用惯性测量单元IMU（Inertial measurement unit）。其工作环境不仅包括空中、地面，还可以在水下。惯性导航的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。其优点是工作不需要通时，安装位置随意，定位范围全场景，但定位精度不高，且误差随时间发散。与GNSS导航系统互补。
地图匹配技术(MM)
地图匹配技术MM（Map matching）是结合用户位置信息和地图数据，推算用户在地图上道路的准确位置，辅助车载导航的精准控制。
航位推算法(DR)
航位推算法DR（Dead Reckoning）是一种跟踪导航算法，在获取载体当前时刻坐标位置的前提下，依靠惯性测量单元IMU取得的同周期内载体移动的距离和方位，进而推算下一时刻位置。在此文介绍中，主要讲建立在已有 GNSS系统 解算下，IMU辅助进行组合导航的算法。
车载定位的痛点
车载导航定位发展已经很久，但随着精度要求越来越高，车载定位的一些问题也逐渐浮现：
偏航重算：是指在高架或城市峡谷，信号遮挡引起位置点漂移；
无法定位：是指在无信号区域（停车场、隧道）推算的精度低，导致出口误差大；
抓路错误：是指主辅路、高架上下抓路错误。
其中偏航重算和无法定位主要是GNSS定位原理决定，GNSS定位精度受观测环境影响，难以改善；对于抓路错误，直接原因是正确道路与误抓道路相隔太近，受定位精度限制无法区分；根本原因是只使用位置信息进行抓路，没有发挥其它数据的价值。
技术方案
以上介绍的关键技术中，在场景覆盖以及精度上，各有所长，互相补充。
根据主流这三种定位技术进行融合，提出GNSS+IMU+MM方案，依靠算法(DR)+数据(POS/HEAD)提高定位的可靠性。
从上述车载定位的几大问题，可以逐步拆分解决：
数据晌游融合：这一部分主要是计算 GNSS模块 输出的位坦宏置、速度、时间和航向信息，将其数据传递至数据处理终端进行实时数据融合计算，判定当前GNSS数据质量的好坏，根据其数据质量组合不同的定位判断策略。
器件补偿：在GNSS信号质量不好或无法定位的时候，只能依靠IMU的DR算法进行补偿。补偿模块的主要功能是利用GNSS数据来补偿速度敏感器误差参数（比例因子）和IMU的误差参数（陀螺仪天向比例因子和陀螺仪三轴零偏）。补偿的目的是在无GPS信号或弱GPS信号的场景，仅靠DR算法也能得到较为可靠的导航信息（通常短时间也能保证厘米级定位）。
场景识别：依靠内置场景化地图数据源以及实时外部传感器收集的环境信息进行场景判断，确定此刻载体地图位置，辅助系统对于周围环境感知进行行为判断。一般采用高精度街景地图源、激光雷达和毫米波雷达进行环境感知。
以 K8模块 为例，采用自适应组合导航设计，支持RTCM2.X/3.X差分数据格式接入，在空旷环境可实现厘米级的定位精度；内置一体化惯导模块，可以实现在复杂环境下的高精度导航。
依靠于自主研发的高精度定位算法，根据车载载体当前运行环境，系统自适应对当前卫星质量进行评估，依据卫星质量进行组合导航。
当卫星条件良好时，以卫星导航为主，结合 高精度RTK 算法，实时定位精度≤±2.5cm，测速精度优于0.03m/s；当卫导无法正常工作时，以惯性导航为主导，3S内精度保持厘米级，10S内精度保持米级。
组合导航技术重点领域： 航空电子与武器技术
技术方向： 导航技术
研究内容： 组合导航技术
技术内涵概述：
将两种或两种以上导航系统以适当方式组合为一种导航系统，以达到提高系统精度和改善系统可靠性等目的，这种系统被称为组合(或综合)导航系统。惯性导航系统由于其工作的完全自主性、以及所提供信息的多样性(位置、速度及姿态)，已成为当前各种航行体上应用的一种主要导航设备；并且，在现已得到应用的机载组合导航系统中，绝大部分为惯性为基的组合系统，其中惯性与GPS两者组合的导航系统是组合导航技术发展的一个重要方向。
组合系兆盯告统的优点可归纳如下：
1、能有效利用各子系统的导航信息，提高组合系统定位精度；
2、允许在子系统工作模式间进行自动转换，从而进一步提高系统工作可靠性；
3、可实现对各子系统及其元件的校准，从而放宽了对子系统技术指标的要求；
4、允许惯导系统进行空中对准和调整，有利于缩短惯导系统的地面对准时间。
目前技术水平(包括与国内外水平对比)：
惯性导航的基本工作原理是以牛顿力学定律为基础的，即在载体内部测量载体运动加速度，经积分运算后得到载体的速度和位置等导航信息。惯性导航是一种完全自主的导航方法，其主要缺点是导航定位误差随时间增长，因而难以长时间独立工作。解决这一问题的途径有两个：一是提高惯导系统本身的精度，一是采用组合导航技术。而实践证明，主要通过软件技术来提高导航精度的组合导航，是一种行之有效的方法。目前在飞机上的通常作法是，在一种中等精度惯导仪基础上，通过卡尔曼滤波器结合进一个或多个辅助传感器，这些传感器将为惯导提供有界信息，从而最终构成一种对短期和长期稳定性以及系统精度都是最佳的组合系统。
军民用前景分析：
自80年代始，组合导航系统日益扩展其应用，尤其受到航空界的重视。在军用方面，美国和北约国家的军用飞机大量装备的是以惯性为基的组合导航系统，其中GPS与惯性的组合更是占有特殊重要的地位。至2004年，一种称为“族明嵌入GPS接收机的惯导系统”的装置(即EGI)将完全取代单独的机上GPS接收机，而成为美国和北约军用飞机的主要导航设备。另外，在战术导d上，这些国家也不允许用GPS作为其唯一制导装置。俄罗斯由于其飞机上的传感器或单项装置普遍来说性能不高，所以特别强调对系统综合能力的研究。通过综合利用现有传感器的信息以构成组合导航系统，这是俄罗斯在现役军用机上广泛采用的一种作法。
定义与概念：
将两种或两种以上导航系统以适当方式组合为一种导航系统，以达到提高系统精度和改善系统可靠性等目的，这种系统被称为组合（或综合）导航系统。至于哪些导航系统可相互结合成为组合导航系统，一般是没有什么限制的。但惯性导航系统由于其工作的完全自主性、以及所提供信息的多样性（位置、速度及姿态），已成为当前各种航行体上应用的一种主要导航设备；并且，在现已得到应用的机载组合导航系统中，绝大部分为惯性为基的组合系统，其中惯性与GPS两者组合的导航系统是组合导航技术发展的一个重要方向。
国外概况：
有三个重要前提推动了组合导航的发展：首先，远程/长航时以及武器投放、侦察/反潜以及变轨控制等任务对导航系统提出了更高的要求；第二，现代控制理论的兴起和发展，特别是卡尔曼滤波技术的出现，为组合导航提供了理论基础和数学工具；第三，数字计算机的蓬勃发展为应用卡尔曼滤波方法解决组合导航问题提供了现实可行的条件。
在以惯性为基的机载组合导航系统中，可提供组合的典型传感器有：GPS（或以后的Glonass）、多普勒、罗兰、星体跟踪器、数字地图、雷达高度表、大气数据计算机、合成孔径雷达（SAR）和光电传感器等。
组合系统的优点可归纳如下：
1、能有效利用各子系统的导航信息，提高组合系统定位精度；
2、允许在子系统工作模式间进行自动转换，从而进一步提高系统工作可靠性；
3、可实现对各子系统及其元件的校准，从而放宽了对子系统技术指标的要求；
4、允许惯导系统进行空中对准和调整，有利于缩短惯导系统的地面对准时间。
早期飞机主要靠目视导航。20世纪20年代开始发展仪表导航，30年代出现无线电导航，40年则模代开始研制超短波的伏尔（VOR）导航系统，50年代惯性导航进入飞机应用，50年代末多普勒导航问世，60年代开始使用远程无线电罗兰C导航系统，60年代中"子午仪"卫星导航正式投入使用，70年代联合战术信息分发系统（JTIDS）得到研制，80年代初出现地形辅助导航，80年代末GPS全球定位系统逐渐进入航空领域。与此同时，从80年代初以来至今，发挥不同导航系统特点的组合导航逐渐得到应用且发展迅速。另外，在30年代无线电导航技术问世之前，天文导航是各种航行体主要（甚至是唯一）的导航手段；但直到今天，无文导航仍在使用，且多以与其它导航相结合的形式出现。
下面简介几种主要导航系统，以及它们与惯性系统组合的情况。
1、VOR/DME 近距无线电导航
VOR和DME是两种近距无线电测量系统。VOR为甚高频全向信标系统，测量飞机磁方位角；DME为测距系统，测量飞机与地面DME台间的斜距。DME作用距离为300～500公里，最远700公里，测距误差为0.1～0.4海里。VOR/DME组成近距无线电导航系统，在其信号覆盖区内还可与惯导组合，以提高飞机区域导航或进场着陆前所需导航信息的精度。
2、多普勒导航
其工作原理是，用多普勒雷达测量航行体相对地球的速度（地速）和偏流角，再从航向系统引入航向信息，然后通过积分运算，最后得到航行体的位置信息。多普勒导航与惯性导航一样，都是一种航位推算定位系统。而多普勒/惯性是一种速度综合模式，它只能减小位置误差随时间增长的速度值，不能改变位置误差随时间增长的基本特性（如惯性系统），这是速度综合导航系统的主要不足之处。
3、远程无线电导航系统
主要指罗兰－C双曲线无线电导航和奥米伽甚低频远距无线电导航。罗兰－C作用距离为1200海里，定位精度为0.25海里（460米）（2维、均方根）。奥米伽导航靠8个地面台实现全球覆盖，定位精度为1～2海里（1.85～3.7公里）（2维、均方根）。当罗兰工作于测距方式时，罗兰/惯性组合是一种类似于GPS/惯性的伪距综合模式，它可消除惯导位置误差随时间增长的性质，使组合后的位置误差变为有界，因而更适于长时间工作航行体的应用。
4、地形辅助导航（TA）
用无线电高度表和数字地图来辅助惯性导航的技术称为地形辅助惯性导航，简称为地形辅助导航（TAN）或地形基准导航（TRN），通俗称为地形匹配。该技术可用来实现精确导航，精度取决于地图精度和地形变化情况，通常为几十米至100米。但TA基本上是一种低高度系统，在300米以上高度时系统精度降低，800～1500米高度时系统无法使用。另外，TAN/惯性/GPS是现代军用飞机常用的一种组合方案。
5、天文导航
天文导航是一种根据天体的精确坐标位置及其已知运动规律，测量天体相对于航行体参考基准面的高度角，从而计算出航行体位置与航向的导航方法。天文导航是一种古老而又崭新的导航技术，又是一种高精度自主式导航手段。当与惯性系统组合时，它可产生一个极其精确的导航解；而且星体的方位和高度数据还可用来向惯性系统提供调平信息。这种组合系统适合于高空远程飞机和要求具有隐身作战能力的战略轰炸机应用。
6、相对导航
JTIDS是把通信、导航和识别综合在一起的一种三军共用的战术多功能综合电子系统，其用户终端分为三类：I类终端供大型飞机（如预警机）和大型舰艇使用，现已装备部队；II类终端供战斗机和一般舰艇使用，已小批投产；III类终端供陆军小分队使用，尚在研制中。JTIDS有一个高精度的导航功能，被称为相对导航，通过测量信号到达时间来测量伪距，最终向用户提供位置、速度和时间信息。由于该系统具有高精度（20纳秒）统一时序，利用多边测距和卡尔曼滤波技术，可实现高精度、多维导航，精度为几十～100米。但由于其导航算法通常适于低动态用户，对高动态尤其是高机动用户，导航算法会产生较大滞后误差。为克服这一缺点，通常将其与惯导相组合，以便在JTIDS丢失信号或坏的测量几何情况下，依靠惯导的航位推算来保持导航精度。
7、GPS全球定位系统
GPS是一种以空间为基的卫星导航系统，在引入"伪距"和"伪距率"概念后，用户接收机只要能同时接收来自空中4颗卫星的信号，就能精确解算出自身所处的三维地理坐标。根据美国政策，GPS可提供两种精度等级的服务：采用商业码（C/A码）的定位精度为100米，军用码（P码）的为16米。虽然GPS具有其它导航设备无法比拟的优点（如极精确的三维位置、速度和时间数据，无源、全球、全天候工作等），但其本质是一种无线电导航系统。在未来战场的电子战环境下，干扰信号将严重影响GPS的工作有效性。为此，美国防部于1996年提出了以GPS为核心的"导航战"思想；并明确，GPS与惯性相组合的方案是干扰环境下一项重要的抗干扰战术。
8、惯性导航系统
惯性导航的基本工作原理是以牛顿力学定律为基础的，即在载体内部测量载体运动加速度，经积分运算后得到载体的速度和位置等导航信息。惯性导航是一种完全自主的导航方法，其主要缺点是导航定位误差随时间增长，因而难以长时间独立工作。解决这一问题的途径有两个：一是提高惯导系统本身的精度，一是采用组合导航技术。而实践证明，主要通过软件技术来提高导航精度的组合导航，是一种行之有效的方法。目前在飞机上的通常作法是，在一种中等精度惯导仪基础上，通过卡尔曼滤波器结合进一个或多个辅助传感器，这些传感器将为惯导提供有界信息，从而最终构成一种对短期和长期稳定性以及系统精度都是最佳的组合系统。
关键技术：
1、将多种系统集成在一起，以构成广义组合能力的数据融合技术；
2、以惯性为基组合导航系统识别欺骗性干扰和抗干扰的技术；
3、将GPS载波相位引入惯性组合系统的技术；
4、利用分散估计理论或联邦滤波器/多模态滤波器进行组合的技术；
5、组合导航系统中惯性系统空中快速对准技术；
6、卡尔曼滤波器的工程化应用，以及有关组合系统可靠性、多维余度、容错能力等的理论与方法的研究。
应用与影响：
自80年代始，组合导航系统日益扩展其应用，尤其受到航空界的重视。在军用方面，美国和北约国家的军用飞机大量装备的是以惯性为基的组合导航系统，其中GPS与惯性的组合更是占有特殊重要的地位。至2004年，一种称为"嵌入GPS接收机的惯导系统"的装置（即EGI）将完全取代单独的机上GPS接收机，而成为美国和北约军用飞机的主要导航设备。另外，在战术导d上，这些国家也不允许用GPS作为其唯一制导装置。俄罗斯由于其飞机上的传感器或单项装置普遍来说性能不高，所以特别强调对系统综合能力的研究。通过综合利用现有传感器的信息以构成组合导航系统，这是俄罗斯在现役军用机上广泛采用的一种作法。
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）