卡尔曼滤波与组合导航原理—初始对准

— 先水平后方位（经典）
二、惯导系统的静基座初始对准方法 2.2 静基座初始对准方案
静基座初始对准分为两大类：
频域法 或经典法
最优估计法 或卡尔曼滤波法
•基于经典控制理论
本课程研究的 • 基于现重代控点制！理论
二、惯导系统的静基座初始对准方法 2.2 静基座初始对准方案(续)
惯导系统 误差根源
加速度计偏置 陀螺漂移
常用坐标系：地心惯性坐标系、地球坐标系、地理坐标系、 载体坐标系。
参考坐标系
Z
2、几个参考坐标系的定义
★惯性坐标系
通常把使得牛顿力学定律成立的参考坐标系,称为惯性坐 标系，简称惯性系；
根据选取的坐标系原点不同，分为日心惯性坐标系赤和道地 心惯性坐标系。
日心惯性坐标系：原点取在太阳的中心,三根轴指向确定的
粗对准 精对准
• 要求尽快地将平台调整到 通常一在个精精对度准范过围程内中，要进行陀 螺的测• 漂缩和短定对标准，时进间一是步主补要偿指陀标螺 漂移率和标定刻度系数，以提高对
准精度• 在粗对准基础上进行，
• 对准精度是主要指标
• 平台，先水平（调平），后方位，使系统有较好的动态性能 • 捷联：精确建立姿态矩阵 — 水平和方位对准同时（现代）
rV D N
0 N
V V D E
0 0
g/R 0 (2)siL n
0
(2)coLs fD
0 2g/R L (2)coLs 0
fE
0 fN
fN 0
V VD E D E
N E
0 0
0 0
0 0
0 0
0 0
0 0
0
()siL n
()siL n
0
(L )coLs N E
N E
D
0
0
0
0
0
0
L ()coLs 0 D D
可以证明两种模型是等价的！
2.3 惯导系统的误差方程
描述惯导系统误差特性的微分方程可分为：
两种
平动误差方程 表示形式
变量取为位置误差 变量取为速度误差
两种
姿态误差方程 表示形式
误差角取为 Φ 误差角取为 Ψ
目前大多采ΨΦ：用：平平Ψ台台角坐坐误标标系差系与与模真计型实算和地地理速理坐坐度标标误系系差之之间间表的的达误误形差差角式角 ！
动可言的,只有在相对的意义下才可以谈运动.一个物体在空间 的位置只能相对于另一个物体而确定,这样,后一个物体就构成 了描述前一个物体运动的参考系.
参考系通常采用直角坐标系来代表,称为参考坐标系或简 称参考系.在研究陀螺仪或运载体的运动时,同样需要有参考 坐标系才成.
陀螺仪最重要的功用之一就是用它在运载体上模拟地理 坐标系或惯性坐标系。
2.3 惯导系统的误差方程
惯导系统的Ψ角误差方程：
V V f g
rrV
在北东地坐标系中，有：
（2.3.1）
cos L
0
（2.3.2）
sin L
（2.3.3）
cos L
0 sin
L
（2.3.4）
将2.3.2~2.3.4代入2.3.1，可得状态空间模型：
基本解决
惯
理论问题
理论、方法、指导
导
系
统
的
问
题
工程技术问题
难度（实现）
一、惯导系统初始对准概述 1.1 初始对准的必要性（续）
惯导系统
姿态矩阵 计算
一、惯导系统初始对准概述 1.1 初始对准的必要性（续）
加速度计
a
∫V ∫ P
Z
∫ ,,
Y
X
积分运算必须知道
初始值！
1.2 初始对准的分类
粗对准
精对准
卡尔曼滤波与组合导航原理
Theory of Kalman Filter and Integrated Navigation
第五章 卡尔曼滤波在惯性导航初始对准中的应用
一、惯导系统初始对准概述 二、惯导系统的静基座初始对准 三、惯导系统的动基座对准 四、惯导系统的传递对准
参考坐标系
1、建立参考坐标系的意义 宇宙间的一切物体都是在不断地运动,但对单个物体是无运
2.1 粗对准与精对准 2.2 静基座初始对准方案 2.3 惯导系统的误差方程 2.4 卡尔曼滤波方程的建立 2.5 计算机仿真研究 2.6 静基座初始对准的可观测度分析 2.7 提高静基座初始对准精度与速度的方法
二、惯导系统的静基座初始对准方法
2.1 粗对准与精对准
根据对准精度的要求，静基座对准过程分为：
静基座条件下位置误差方程：
（2.3.9）
L
1 R
VN
VE secL
R
2.3 惯导系统的误差方程
最终可得，平台惯导系统的Φ角误差方程： 不考虑δλ平台惯导系统的Φ角误差方程可简化为：
L 0 0 1/R
0
0
0
0 L
0
V VN E
0 0
00
seLc
0
R
0 0 2sinL 0
0 2sinL 0
2.3 惯导系统的误差方程
静基座初始对准时，位置和垂直方向速度可准确知道 惯导系统的误差方程可简化为：
rN 0 siL n L
1
0
0
0
0
0 rN 0
rE
rV D N
siL n 0 coLs
L coLs 0
g/R 0
0
0 0 0
1
0
(2)siL n
0
1 L
0
0
0 rE 0
0 0
0 fD
0 fE
随惯机导误系差统 为随机系统
若采用状态反馈控制，就必须对状态进行估计！
卡尔曼滤波器
二、惯导系统的静基座初始对准方法
2.2 静基座初始对准方案(续)
（1）采用KALMAN滤波进行初始对准，就是将平台误差角 ΨN，ΨE，ΨD从随机误差和随机干扰中估计出来， 通过系统的校正使平台坐标系与导航坐标系对准；
地球坐标系参与地球自转，它 相对于惯性坐标系的转动角速度 就等于地球自转角速度。
地球相对惯性空间的转动，可 以用地球坐标系相对于惯性坐标 系的转动来表示。
X Ω*t
Xe
本初子午线
Ze Ω
赤道
Ye
参考坐标系
★地理坐标系（ONEZ）
其原点与运载体的重心
Leabharlann Baidu
重合,E轴沿当地纬线指东,N
轴沿当地子午线指北,Z轴沿
E N sL i V n R E ta L n D cL o V R E s E （2.3.7）
D N V R N E co L V s R E D
将2.3.7代入2.3.6，在静基座条件下，得：
NV R E siL n L E siL n N E V R N N siL n D co L sE
舒勒摆原理在惯导系统中的应用 普通地平液体摆做敏感元件受加速度影响较大，需用舒 勒摆原理
有害加速度的消除 消除由于地球自转、飞机飞行引起的牵连、哥氏、重力 加速度等
初始对准问题 惯导系统要正确而精确的工作，必须精确给定初始条件
捷联惯导解算问题 数学平台代替机电平台
一、惯导系统初始对准概述 1.1 初始对准的必要性
当地地垂线指天.其中E轴与
N轴构成的平面即为当地水
平面,N轴与Z轴构成的平面
即为当地子午面.
这种地理坐标系是跟随
运载体运动的,更确切地说
应称为动地理坐标系或当地
1
地理坐标系.
参考坐标系
当运载体在地球上运动时,运载体相对地球的位置不断改 变;而地球上不同地点的地理坐标系,其相对地球坐标系的角 位置是不相同的.也就是说，运载体相对地球运动引起地理 坐标系相对地球坐标系转动.这时地理坐标系相对惯性坐标 系的转动角速度应包括两个部分：一是地理坐标系相对地 球坐标系的转动角速度:另一是地球坐标系相对惯性坐标系 的转动角速度.
（2）同时，尽可能估计出陀螺漂移和加速度计偏置；
（3）时间不长，因此陀螺漂移和加速度计偏置可看作常值；
（4）根据分离定理，对随机系统的最优估计和最优控制 可以分开单独考虑，故可用卡尔曼滤波器对平台误差 角及惯性仪表的误差进行单独研究。
二、惯导系统的静基座初始对准方法 2.3 惯导系统的误差方程
惯导系统误差源
仪表误差
安装误差
初始条件 误差
运动干扰 （有害）
其他误差
如地球曲率半径描述误差； 有害加速度补偿中忽略二阶小量
2.3 惯导系统的误差方程
Φ角误差模型和Ψ角误差模型
初始对准
基础 惯导误差方程
推导
Φ角误差模型 方法 真实地理坐标系法
正确反映惯导系统的误差特性， 便推于导分析和应用！
Ψ角误差模型 方法 计算地理坐标系法
恒星。
X
Y
地心惯性坐标系（OXiYiZi）：原点取在地球的中心,Xi和 Yi轴位于赤道平面内并指向确定的恒星，Zi轴与地轴（地 球自转轴）重合。地心惯性坐标系不参与地球自转。
惯性空间：惯性坐标系三根轴所代表的空间。
参考坐标系
★地球坐标系（OXeYeZe）
与地球固连，原点取在地球的 中心,Xe和Ye轴位于赤道平面内， 分别指向本初子午线和东经90° 子午线，Ze轴与地轴重合。
g
0 g 0
0
0 0
V VN E
N E
sinL 0
0 0
0 1/R
1/R 0
0 sinL
sinL 0
c0oLs N E
D coLs 0 0 tanL/R 0 coLs 0 D
二、惯导系统的静基座初始对准方法
2.4 卡尔曼滤波方程的建立
（1）系统方程
X A X W
X 系统状态向量 X V N V E N E D N E N E D
地理坐标系的三根轴构成右手直角坐标系，可以按“北、 东、天”、“北、西、天”或“北、东、地”顺序构成。
参考坐标系
★载体坐标系（OXbYbZb） 与载体固连，其原点与载体的重心重合, Xb轴沿载 体纵轴方向, Yb轴沿载体横轴方向，Zb轴沿载体竖轴方向。
Zb
Yb
Xb
实现惯导要解决的几个问题
平台跟踪坐标系 平台跟踪什么样的坐标系是平台式惯导系统的首要问题
惯导系统的Ψ角误差方程：
惯导系统的误差模型可由下列3个基本方程表示： V V f g
rrV
（2.3.1）
• δV、r和Ψ分别为速度、位置和姿态矢量 • Ω为地球自转角速度 • ω为导航坐标系相对惯性坐标系的角速度矢量 • ▽是加速度计常值偏值，ε是陀螺常值漂移 • f是比力，△g是重力矢量计算误差， • ρ是地理系相对地球转动速度矢量
N Nc o Ls
E EL
D D siL n
将2.3.5微分：
（2.3.5）
N N cL o s sL i L n
E EL
（2.3.6）
D D sL i n cL o L s
2.3 惯导系统的误差方程
惯导系统的Ψ角误差方程：
N E sL i n V R E ta L n D V R N N
•扩充为系统状态变量
•不完全为白噪声
2.3 惯导系统的误差方程
最终可得惯导系统的Ψ角误差方程：
VVNE
0 2sinL
2sinL 0
0 g
g 0
0 1 0 0 0 0VN
0
0
1
0
0
0
VE
N
E
0 0
0 0
0 sinL
sinL 0
0 cosL
0 0
0 0
1 0
0 1
0 0
NE
D
0
N 0
0
0
cosL
0
0
0
0
0
1
D
0
0
0
0 0 0 0 0 0N
E
0
0
0
0
0
0
0
0
0
0
E
N 0
E
0
0
0
0
0 0 0 0 0 0 N
0
0
0
0
0
0
0
0
0
E
D 0
0
0
0
0 0 0 0 0 0 D
2.3 惯导系统的误差方程
惯导系统的Φ角误差方程：
Φ角与Ψ角之间的关系：
2.3 惯导系统的误差方程
Ψ角误差模型+速度误差表达形式的优点：
（1）平动误差不会耦合到姿态误差方程中， 特别便于动基座对准问题的分析和研究。
（2）Φ动角可通过位置误差和Ψ角得到： Φ=Ψ+θ
（3）静基座时，惯导所处地理位置可精确获得， 且对准时间较短，可忽略位置误差，此时： Φ=Ψ
2.3 惯导系统的误差方程
自主对准
对准阶段
静基座
对外信息 的需求
初始对准
基座 运动状态
非自主 对准
对准轴系
动基座
水平对准
方位对准
一、惯导系统初始对准概述 1.3 初始对准的要求
初
对准精度
始
对
准
的
要
求
对准时间
快又准
对准精度与对准时间相互制约，不同场合侧重点不同
一、惯导系统初始对准概述
1.4 初始对准的发展趋势
初
新的滤波方法
（2.3.8）
D V R E ta L n cL oL s E cL o s D
2.3 惯导系统的误差方程
静基座条件下速度误差方程：
速度误差定义为计算速度与真实速度之差
V N 2 sL iV E n E g N
V E 2 sL iV N n N g E
始
对
准
的
发
展
可观测性分析和
可观测度研究
自适应滤波 H∞滤波 神经网络
非线性滤波 预测滤波
从根本提高对准 的精度和速度
第五章 卡尔曼滤波在惯性导航初始对准中的应用
一、惯导系统初始对准概述 二、惯导系统的静基座初始对准 三、惯导系统的动基座对准 四、惯导系统的传递对准
二、惯导系统的静基座初始对准方法