04第四部分+卡尔曼滤波在组合导航中的应用
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
北京航空航天大学宇航学院 王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.1 概述
– 卫星导航
• • • • • • • • • 输出姿态信息困难 导航数据输出率低 工作可靠性较差，容易受遮挡和干扰 动态性能差 系统成本高 数据输出率低 体积较大、成本高 动态性能差 在大气层内应用精度低
Kalman Filtering
§9.3 天文导航原理简介
• 定位
– 星光角距
r TS s arccos s r
– 星光折射
k1ek h
2
h r Tu r Ts tan Re vh
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.6 组合算法
GM x2 3 (1 3 2 ) r r GM x( y R0 ) 3 3 r r2 GM xz 3 3 2 r r g x f15 y f 26 g y GM z ( y R0 ) r3 r2 g x f16 z g y f 26 z 3
– 提供载体位置、速度和姿态等线运动和角运动 信息的过程
• 导航技术发展历史
– – – – 古人观星导航，北斗星、启明星 司南和指南针 六分仪 高精度时表
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.1 概述
– 陀螺仪
• 被中香炉 • Leon Foucault • Sperry
§9.2 惯性导航原理简介
T
dr dr i e ωie r v e ωie r dt dt ωie 0 0 ie dv e dv e d 2r d (ωie r ) i i i ωie v e ωie (ωie r ) 2 i dt dt dt dt dv e i f g ωie v e ωie (ωie r ) f ωie v e g l dt g l g ωie (ωie r ) 0 r q i i ie f i ωie v vie gil Cibf b ωie vie gil Ωiib r 0 p q p 0 i Ci Ωi C b b ib
– 深组合（Ultra-tightly-coupled GPS/INS）
卫星星历 位置，速度 计算INS 伪距伪距率 天线 惯性辅助 GPS接收机相 关控制回路 INS 速度位置 估计输出
I I ,
G G ,
Kalman 滤波器
反馈校正
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
王可东
– 天文导航
北京航空航天大学宇航学院
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.1 概述
• 解决方案
– 组合 – 组合方法：优势互补
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
– 加速度计
• Schuler摆
– 惯导
• • • • Boykow V2火箭 框架式 捷联
王可东
北京航空航天大学宇航学院
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.1 概述
– 卫星导航
• GPS • GLONASS
– 现代天文导航
• 星敏感器 • 太阳敏感器 • 地平仪
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
– 紧耦合（Tightly-coupled GPS/INS）
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
ω p q r
i ib T
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.2 惯性导航原理简介
– 在地球固联坐标系内
dr ve dt
e
v i ωie r
T
ωie 0 0 ie
dv e dv e e i ω ie v e f 2ω ie v e g l dt dt e e e e e b e e e e v f 2 ω v g C f 2 ω v g e ie e l b ie e l e Ce Ω b C
0 V x Vy 0 0 V z 1 x y 0 0 z
0 0 f34 0 0 0 1 0 0 0 0 1
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.4 卫星导航原理简介
• 定位
( x xs )2 ( y ys )2 ( z zs )2 ctr lx ( x x s ) l y ( y y s ) lz ( z z s ) v
北京航空航天大学宇航学院
Kalman Filtering
天线
§9.5 组合模式
误 差 检 测 器
基带部分 射频 前端 I Q
相关部分 (wenku.baidu.com000Hz)
自适应 码跟踪 误差 估计器
Kalman 滤波器
Cos
Sin
E
P
L
码发生器 载波 NCO
码NCO
观 测 量
导航计算 (50Hz)
MIMU
误差修正
用户 的位 置、 速度 和用 户钟 差
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
第九章在组合导航中的应用
• 要点
– 掌握建模方法 – 掌握卡尔曼滤波的应用方法
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.1 概述
• 导航
b b nb b b n n ωb nb ω ib C n (ω ie ω en )
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.3 天文导航原理简介
• 定姿
v
zs
pi ui , vi
pui ui , 0
§9.2 惯性导航原理简介
• 坐标系
– – – – 惯性坐标系 地球固联坐标系 地理坐标系 载体坐标系
d 2r ai 2 i f g dt dr vi i dt
王可东
• 惯性方程
– 在惯性坐标系内
北京航空航天大学宇航学院
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
o
w
i
i
光轴
os
ys
第i颗恒星 CCD面阵
f
xs
透镜
u
sin i cos i pos cos cos i i sin i
北京航空航天大学宇航学院
b poi CibCs pos
王可东
卡 尔 曼 滤 波基础 Fundamentals of
最优滤波
位置、姿态 姿态
• 闭环反馈修正
CNS
姿态
最优估计
陀螺漂移修正
北京航空航天大学宇航学院 王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
– 位姿观测
INS 位置、姿态
位置、姿态
CNS
最优估计
位置、姿态反馈修正
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
• GPS/INS
– 松散组合（Loosely-coupled GPS/INS）
反馈校正 速度位置 估计输出
INS
速度，位置
Kalman 滤波器
GPS
速度，位置
北京航空航天大学宇航学院
王可东
– 辅助导航
• 地形/地磁/重力辅助导航
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.1 概述
• 现代导航的需求
– – – – 长航时 高精度 高可靠性 低成本
• 单一导航方式存在的问题
– 惯性导航
• 误差随时间累积发散 • 高精度的惯导成本高、体积大
INS
位置、姿态
位置、姿态
CNS
位置、姿态
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
– CNS与INS组合用于修正陀螺仪的漂移
• 开环修正
INS
位置、姿态 姿态
位置、姿态 陀螺漂移和姿 态修正
CNS
INS
姿态
b b eb b e b e ω eb ωib Ce ωie
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.2 惯性导航原理简介
– 在地理坐标系内
dr vn n v i (ω ie ω en ) r dt dv e dv e n i (ω ie ω en ) v e f (2ω ie ω en ) v e g l dt dt n n n n b n n n n e v f n (2ωie ω en ) ve g ln Cn b f (2ω ie ω en ) v e g l n Cn Ω b C
码和载波 NCO控制 计算
接收机原始数 据的预测 卫星位置 速度计算
卫星星历 数据解调
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.6 组合算法
• CNS/INS
– 系统建模
• 运动模型 x T φ y φω ε z vx 0 sin tan ie Rh v ie sin x tan 0 Rh vy cos vx ie Rh Rh
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
• CNS/INS
– INS与CNS独立工作，CNS用于修正INS
• 已知载体的准确位置，通过CNS修正姿态，并修正INS的输出 • 根据INS提供的地平信息，进行CNS定位，并修正INS的输出
北京航空航天大学宇航学院
ie cos
vx R h x vy y ε Rh z 0
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.6 组合算法
0 0 0 0 f14 f 24 f15 f 25 f35 0 0 0 f16 Vx V f 26 y f36 Vz 0 x 0 y 0 z
g x f14 x g f15 x y g f16 x z g y f 24 x g y f 25 y
z g f34 z x g f35 z y
g z GM z2 f36 3 (1 3 2 ) z r r
( R0 y ) 2 r x 2 ( y R0 ) 2 z 2 GM 3 (1 3 ) 2 r r
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.1 概述
– 卫星导航
• • • • • • • • • 输出姿态信息困难 导航数据输出率低 工作可靠性较差，容易受遮挡和干扰 动态性能差 系统成本高 数据输出率低 体积较大、成本高 动态性能差 在大气层内应用精度低
Kalman Filtering
§9.3 天文导航原理简介
• 定位
– 星光角距
r TS s arccos s r
– 星光折射
k1ek h
2
h r Tu r Ts tan Re vh
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.6 组合算法
GM x2 3 (1 3 2 ) r r GM x( y R0 ) 3 3 r r2 GM xz 3 3 2 r r g x f15 y f 26 g y GM z ( y R0 ) r3 r2 g x f16 z g y f 26 z 3
– 提供载体位置、速度和姿态等线运动和角运动 信息的过程
• 导航技术发展历史
– – – – 古人观星导航，北斗星、启明星 司南和指南针 六分仪 高精度时表
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.1 概述
– 陀螺仪
• 被中香炉 • Leon Foucault • Sperry
§9.2 惯性导航原理简介
T
dr dr i e ωie r v e ωie r dt dt ωie 0 0 ie dv e dv e d 2r d (ωie r ) i i i ωie v e ωie (ωie r ) 2 i dt dt dt dt dv e i f g ωie v e ωie (ωie r ) f ωie v e g l dt g l g ωie (ωie r ) 0 r q i i ie f i ωie v vie gil Cibf b ωie vie gil Ωiib r 0 p q p 0 i Ci Ωi C b b ib
– 深组合（Ultra-tightly-coupled GPS/INS）
卫星星历 位置，速度 计算INS 伪距伪距率 天线 惯性辅助 GPS接收机相 关控制回路 INS 速度位置 估计输出
I I ,
G G ,
Kalman 滤波器
反馈校正
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
王可东
– 天文导航
北京航空航天大学宇航学院
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.1 概述
• 解决方案
– 组合 – 组合方法：优势互补
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
– 加速度计
• Schuler摆
– 惯导
• • • • Boykow V2火箭 框架式 捷联
王可东
北京航空航天大学宇航学院
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.1 概述
– 卫星导航
• GPS • GLONASS
– 现代天文导航
• 星敏感器 • 太阳敏感器 • 地平仪
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
– 紧耦合（Tightly-coupled GPS/INS）
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
ω p q r
i ib T
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.2 惯性导航原理简介
– 在地球固联坐标系内
dr ve dt
e
v i ωie r
T
ωie 0 0 ie
dv e dv e e i ω ie v e f 2ω ie v e g l dt dt e e e e e b e e e e v f 2 ω v g C f 2 ω v g e ie e l b ie e l e Ce Ω b C
0 V x Vy 0 0 V z 1 x y 0 0 z
0 0 f34 0 0 0 1 0 0 0 0 1
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.4 卫星导航原理简介
• 定位
( x xs )2 ( y ys )2 ( z zs )2 ctr lx ( x x s ) l y ( y y s ) lz ( z z s ) v
北京航空航天大学宇航学院
Kalman Filtering
天线
§9.5 组合模式
误 差 检 测 器
基带部分 射频 前端 I Q
相关部分 (wenku.baidu.com000Hz)
自适应 码跟踪 误差 估计器
Kalman 滤波器
Cos
Sin
E
P
L
码发生器 载波 NCO
码NCO
观 测 量
导航计算 (50Hz)
MIMU
误差修正
用户 的位 置、 速度 和用 户钟 差
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
第九章在组合导航中的应用
• 要点
– 掌握建模方法 – 掌握卡尔曼滤波的应用方法
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.1 概述
• 导航
b b nb b b n n ωb nb ω ib C n (ω ie ω en )
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.3 天文导航原理简介
• 定姿
v
zs
pi ui , vi
pui ui , 0
§9.2 惯性导航原理简介
• 坐标系
– – – – 惯性坐标系 地球固联坐标系 地理坐标系 载体坐标系
d 2r ai 2 i f g dt dr vi i dt
王可东
• 惯性方程
– 在惯性坐标系内
北京航空航天大学宇航学院
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
o
w
i
i
光轴
os
ys
第i颗恒星 CCD面阵
f
xs
透镜
u
sin i cos i pos cos cos i i sin i
北京航空航天大学宇航学院
b poi CibCs pos
王可东
卡 尔 曼 滤 波基础 Fundamentals of
最优滤波
位置、姿态 姿态
• 闭环反馈修正
CNS
姿态
最优估计
陀螺漂移修正
北京航空航天大学宇航学院 王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
– 位姿观测
INS 位置、姿态
位置、姿态
CNS
最优估计
位置、姿态反馈修正
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
• GPS/INS
– 松散组合（Loosely-coupled GPS/INS）
反馈校正 速度位置 估计输出
INS
速度，位置
Kalman 滤波器
GPS
速度，位置
北京航空航天大学宇航学院
王可东
– 辅助导航
• 地形/地磁/重力辅助导航
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.1 概述
• 现代导航的需求
– – – – 长航时 高精度 高可靠性 低成本
• 单一导航方式存在的问题
– 惯性导航
• 误差随时间累积发散 • 高精度的惯导成本高、体积大
INS
位置、姿态
位置、姿态
CNS
位置、姿态
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
– CNS与INS组合用于修正陀螺仪的漂移
• 开环修正
INS
位置、姿态 姿态
位置、姿态 陀螺漂移和姿 态修正
CNS
INS
姿态
b b eb b e b e ω eb ωib Ce ωie
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.2 惯性导航原理简介
– 在地理坐标系内
dr vn n v i (ω ie ω en ) r dt dv e dv e n i (ω ie ω en ) v e f (2ω ie ω en ) v e g l dt dt n n n n b n n n n e v f n (2ωie ω en ) ve g ln Cn b f (2ω ie ω en ) v e g l n Cn Ω b C
码和载波 NCO控制 计算
接收机原始数 据的预测 卫星位置 速度计算
卫星星历 数据解调
北京航空航天大学宇航学院
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.6 组合算法
• CNS/INS
– 系统建模
• 运动模型 x T φ y φω ε z vx 0 sin tan ie Rh v ie sin x tan 0 Rh vy cos vx ie Rh Rh
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.5 组合模式
• CNS/INS
– INS与CNS独立工作，CNS用于修正INS
• 已知载体的准确位置，通过CNS修正姿态，并修正INS的输出 • 根据INS提供的地平信息，进行CNS定位，并修正INS的输出
北京航空航天大学宇航学院
ie cos
vx R h x vy y ε Rh z 0
王可东
卡 尔 曼 滤 波基础 Fundamentals of
Kalman Filtering
§9.6 组合算法
0 0 0 0 f14 f 24 f15 f 25 f35 0 0 0 f16 Vx V f 26 y f36 Vz 0 x 0 y 0 z
g x f14 x g f15 x y g f16 x z g y f 24 x g y f 25 y
z g f34 z x g f35 z y
g z GM z2 f36 3 (1 3 2 ) z r r
( R0 y ) 2 r x 2 ( y R0 ) 2 z 2 GM 3 (1 3 ) 2 r r