2017/4/6本章的学习目的全日制专业硕士必修课《导航系统设计》1、掌握捷联惯导系统力学编排和程序设计:第4章捷联惯导程序设计2、编程实现捷联惯导系统解算程序。1概述1概述Koyta(1)d8at兰Secolt玩动ADa国国HX=X+fr(0)dt0Vo加速度计V, =Veo +fadt'Vesecodt(t)= +RJOl'ydtV=-Vno+[axdt(t)=Po+陀螺仪-D!1 概述1概述0fuEAEr0a, cos.C(0)a-北肉阳城Crm-ogcr0-卤一重方包元一“日车件Vr(0)ii心一邮国9二1
2017/4/6 1 第4章 捷联惯导程序设计 全日制专业硕士必修课《导航系统设计》 本章的学习目的 1、掌握捷联惯导系统力学编排和程序设计; 2、编程实现捷联惯导系统解算程序。 x y 0 运动 轨迹 x1 y1 t v v a t dt 0 0 ( ) t X X v t dt 0 0 ( ) 1 概述 ∫ a V ∫ X 加速度计 ∫ ,, Z X 陀螺仪 Y t VE VE aE dt 0 0 t VE dt R t 0 0 sec 1 ( ) t VN VN aN dt 0 0 V dt R t t N 0 0 1 ( ) 1 概述 t Vx (0) t Vx R h N 1 sec L (0) t y f t ay t Vy t Vy (0) t Vy R h M 1 L L(0) L 1 t itx 至东向陀螺 ie cos L t ity 至北向陀螺 tan L ie sin L t itz 至方位陀螺 t x a t Vx t x f 1 概述 p x f p x a p Vx p Vx s 1 (0) p Vx 导航计算 p y f p y a p Vy p Vy s 1 (0) p Vy p epx 至东向陀螺 ie 至北向陀螺 ie c13 p ipy xp f f yp R R 1 1 1 1 C (t) C (t) p e p ep p e p epy ie c23 p ipx (0) p Ce s 1 L f 地球速率计 算 ie c13 ie c23 至方位陀螺ie p ipz c33 1 概述
2017/4/61概述1方向余弦法国成(Tu ZzZa) (cosycosy-sinysingsing(cos9singsinycosy+cossin9sinyFWRd国国国公速率cos.9osycosysiny+sinysincosysinysinycosysin cosy的计sin9)siny cos .9cs7 cos.9A+翼超动用± = tan -T/2[0,360′]的T22Ts2[-90,90′]9=tan-l图国0a+T国?开话-1-T1[-180′,180]Y± = tan~lT31四元数微分方程2四元数初始值四元教微分方程0.-0-T.Co+)(T T2 )10-1Q0T-T,TsO(TnTs)40FoLO-0040a+i--e2(i929oq;)2(qq;+92)4.9200otq2-m922(29s-909)2(q2+qo9)qq +q2-00o[93]L932(qi93-9o92)2(+)--+[46+gi-95-g]2(gi92q9)2(g:9; +692)+qi-q-qi-.-+-=2T'=2(9,9 +9.9,)q-qf +qi-q,2(9,9;4.g,)2(4.9-9092)2(9:9;+909)--q2++q+q+q=1-g-q+g-T2四元数初始值2四元数初始值初始四元数初始四元数 +q-q2q = T[1g/- /1+71727g+ksincos+isincos+jsin0=cosqi+q=22g-//T,+72 T,+40=cos号cos号cos号sinsin号sin号Igs/-→ /1-T.--T2 +7,sg+gi+q+=1[90/=/1qqgciisignqo = +signq, = sign(T32T23):csssso22signq2 = sign(T1s -T,)9号sin+simcos号cossignq, = sign(Tar-T2)q,=cos兰sin2
2017/4/6 2 加 速 度 计 误 差 补 偿 地球速率 计 算 比力坐标 转换 速度的即 时修正 位置速率 的计算 位置矩阵 C 的即时修正 位置 计算 g 的计算 外 部 高度信息 高 度 计 算 速度计算 地球自转 角速度 陀 螺 仪 误 差 补 偿 姿 态 计 算 速度 纬度 经度 偏航角 滚动角 俯仰角 高度 姿态速率 计 算 四元数 Q 的 即时修正 计 算 四元数 Q 的 最佳归一化 计 算 姿态阵 T 的计算 1 概述 tan 90 ,90 2 22 2 12 1 32 T T T 主 tan 0 ,360 22 1 12 T T 主 tan 180 ,180 33 1 31 T T 主 1 方向余弦法 sin cos sin cos cos cos sin sin sin cos cos cos sin sin cos sin cos cos cos sin sin sin cos sin sin cos cos sin sin 3 1 3 2 3 3 2 1 2 2 2 3 1 1 1 2 1 3 T T T T T T T T T T n b Q Q 2 1 0 0 1 1 2 2 3 3 0 1 0 2 0 0 b b b nbx nby nbz b b b nbx nbz nby b b b nby nbz nbx b b b nbz nby nbx q q q q q q q q 2 3 2 2 2 1 2 1 3 0 2 2 3 0 1 0 2 3 0 1 2 3 2 2 2 1 2 1 2 0 3 0 1 2 0 3 1 3 0 2 2 3 2 2 2 1 2 0 2( ) 2( ) 2( ) 2( ) 2( ) 2( ) q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q T n b 四元数微分方程 ( ) n en e ie n e b n b ib b nb T C n in 1 四元数微分方程 31 32 33 21 22 23 11 12 13 T T T T T T T T T T n b 2 3 2 2 2 1 2 1 3 0 2 2 3 0 1 0 2 3 0 1 2 3 2 2 2 1 2 1 2 0 3 0 1 2 0 3 1 3 0 2 2 3 2 2 2 1 2 0 2( ) 2( ) 2( ) 2( ) 2( ) 2( ) q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q 2 四元数初始值 11 2 3 2 2 2 1 2 q0 q q q T 22 2 3 2 2 2 1 2 q0 q q q T 33 2 3 2 2 2 1 2 q0 q q q T 1 2 3 2 2 2 1 2 q0 q q q 初始四元数 11 2 3 2 2 2 1 2 q0 q q q T 22 2 3 2 2 2 1 2 q0 q q q T 33 2 3 2 2 2 1 2 q0 q q q T 1 2 3 2 2 2 1 2 q0 q q q 1 1 11 22 33 2 1 q T T T 2 1 11 22 33 2 1 q T T T 3 1 11 22 33 2 1 q T T T 2 3 2 2 2 q0 1 q1 q q sign sign ( ) sign sign ( ) sign sign ( ) sign 3 21 12 2 13 31 1 32 23 0 q T T q T T q T T q 2 四元数初始值 初始四元数 2 sin 2 cos 2 sin 2 cos 2 sin 2 cos Q k i j 2 sin 2 sin 2 sin 2 cos 2 cos 2 cos 0 q 2 sin 2 cos 2 sin 2 cos 2 sin 2 cos 1 q 2 cos 2 sin 2 sin 2 sin 2 cos 2 cos 2 q 2 cos 2 cos 2 sin 2 sin 2 sin 2 cos 3 q 2 四元数初始值
2017/4/63惯导的基本方程3指北方位系统的基本原理及力学编排pp-f-(20g×0p)xp+g#A最米标位g=gm-(oxR)沿飞行器轴计算M同方向余改降加速度分量速度——载体相对地球的加速度A出路示一哥式加速度在兒鑫20.x0g华ogxV载体围绕地球转动运动所产G裕飞行器轴泰霜鑫套位生的向心加速度(或称为牵连加速度)要志航向利得角速G0x(OxR)一由地球运动而产生的法计算机平台向加速度(或称牵连加速度)惯性元件计算机3指北方位系统的基本原理及力学编排3指北方位系统的基本原理及力学编排Veo[o00白oOcosLC"osa(tNoLedEo0LOsinLP(r)AD国国PVoVn0元(R +h)cosLVe=Veo +l'agd(t)=% +'VsecdiV"Pdr国=oxri=dtRμ+hV-Vo+f'adtVdi00)=0+3指北方位系统的基本原理及力学编排3指北方位系统的基本原理及力学编排(2)对地速度的计算(1)平台指令角速度的计算.0-(20+0m.)V20..V"vVa1n20.+0.0AcosLtanLf20+0"=TLRu+hR+hR+h0-(20, +o.)20g+0m0AsinvRu,+ho"+o”V"显oo,+o.o,cosL+Ry+ho""IV"O, sinL.tanLRy+h3
2017/4/6 3 ——载体相对地球的加速度 ——哥式加速度 ——载体围绕地球转动运动所产 生的向心加速度(或称为牵连加速度) ——由地球运动而产生的法 向加速度(或称牵连加速度) Vep ie ep 2 ep Vep ( R) ie ie 3 惯导的基本方程 Vep f ie ep Vep g (2 ) g g ( R) m ie 3 指北方位系统的基本原理及力学编排 t VE VE aE dt 0 0 t VE dt R t 0 0 sec 1 ( ) t VN VN aN dt 0 0 V dt R t t N 0 0 1 ( ) 3 指北方位系统的基本原理及力学编排 R h V L M n y L C C L ie ie ie n e e ie n e n iez n iey n iex sin cos 0 0 0 R h L V N n x ( ) cos 3 指北方位系统的基本原理及力学编排 r V r dt dr V (1)平台指令角速度的计算 L R h V L R h V L R h V L R h V R h V R h V L L L N n x ie N n x ie M n y n enz n iez n eny n iey n enx n iex n inz n iny n inx T N n x N n x M n y n enz n eny n enx sin tan cos tan sin cos 3 指北方位系统的基本原理及力学编排 (2)对地速度的计算 V g V V f f f T V V V n z n y n x n enx n iex n eny n iey n enx n iex n enz n iez n eny n iey n enz n iez b z b y b x n b n z n y n x 0 0 (2 ) 2 0 2 0 2 0 (2 ) 2 V dt V dt V dt V V V V V V n z n y n x n z n y n x n z n y n x (0) (0) (0) 3 指北方位系统的基本原理及力学编排
2017/4/63指北方位系统的基本原理及力学编排3指北方位系统的基本原理及力学编排(3)位量计算(方法1)(3)位置计算(方法2)V^= α(0)+[idt-sinacos ^0.(ChCCn)(R +h)cos LC"=sin Lcos -sin Lsincos LCi2CnCn2CuC.CacosLcoscosLsinAsinL位置矩阵的微分方程v"L= L(O)+[Ldti:Cn =-QCnRu+h0-OmOmy0Q=Om-Omh= h(O)+[v"dt-OmOm03指北方位系统的基本原理及力学编排3指北方位系统的基本原理及力学编排-sin acosA(C.Cicu0 Te T.) (asycosy-sinysin SsinyT.cos.9sinysinycosy+cosysinsin-C=Casin Lcos.a-sin Lsina cosLTaTaT-cosysiny+sinysin9cosycos.S.cosysinysingossinScosyCCnTaJsin.9TTesinycos.Scosycos.9-cosLcosAcosLsinAsinL(CC.[-90,90r] 主值城定义城9,=tanJa+TS[90′,90′][-90′,90′]Ly= tan,=a"-[or,360]VCi+CaTea74=tan~-=tan"S[-180,180r][90′,90][-180,180′]TCs表1经度的真值判别表2航向角的真值判别表3横滚角的真值判别入取值佳的符号Cn的符号主的符号的符号取值主的符号,的符号Y取值4-180W+Y±180°+-+W++360±+180°±-180°.-+F180*++4+,+, -743指北方位系统的基本原理及力学编排3指北方位系统的基本原理及力学编排中国社租(5)高度通道计算G晋沙果RWEPC国电庆的国的计算的图时捷正正28goXLR电国要态V(0)h(0)a:的计氧"上日X-图国国周X+人B国高度E4
2017/4/6 4 (3)位置计算(方法1) dt (0) R h L V N n x ( ) cos L L Ldt (0) R h V L M n t h h V dt n z (0) 3 指北方位系统的基本原理及力学编排 (3)位置计算(方法2) L L L L L L C C C C C C C C C C n e cos cos cos sin sin sin cos sin sin cos sin cos 0 1 3 2 3 3 3 1 2 2 2 3 2 1 1 2 1 3 1 位置矩阵的微分方程 0 0 0 n enx n enz n enx n enz n eny n enz n e n C e C 3 指北方位系统的基本原理及力学编排 tan 90 ,90 180 ,180 tan 90 ,90 90 ,90 cos cos cos sin sin sin cos sin sin cos sin cos 0 3 1 1 3 2 2 3 2 2 3 1 1 3 3 3 1 3 2 3 3 2 1 2 2 2 3 1 1 1 2 1 3 C C C C C L L L L L L L C C C C C C C C C C n e 主 主 主值域 定义域 +- 的符号 的符号 取值 + - - - - + +,- + 表1 经度的真值判别 主 C31 主 180 主 180 主 3 指北方位系统的基本原理及力学编排 tan 180 ,180 tan 0 ,360 tan 90 ,90 sin cos sin cos cos cos sin sin sin cos cos cos sin sin cos sin cos cos cos sin sin sin cos sin sin cos cos sin sin 3 3 1 3 1 2 2 1 1 2 2 2 2 2 1 2 1 3 2 3 1 3 2 3 3 2 1 2 2 2 3 1 1 1 2 1 3 T T T T T T T T T T T T T T T T T n b 主 主 主 表2 航向角的真值判别 表3 横滚角的真值判别 的符号 的符号 取值 + + - + + +,- - + 的符号 的符号 取值 + - - - - - +,- + 主 主 主 主 180 360 主 T33 T22 主 主 主 180 180 3 指北方位系统的基本原理及力学编排 (5)高度通道计算 3 指北方位系统的基本原理及力学编排 n z f VN s 1 n az (0) n Vz s 1 h(0) h R 2g0 g0 hc 1 k2 k 加 速 度 计 误 差 补 偿 地球速率 计 算 比力坐标 转换 速度的即 时修正 位置速率 的计算 位置矩阵 C 的即时修正 位置 计算 g 的计算 外 部 高度信息 高 度 计 算 速度计算 地球自转 角速度 陀 螺 仪 误 差 补 偿 SINS 导航信息解算模块 姿 态 计 算 速度 纬度 经度 偏航角 滚动角 俯仰角 高度 姿态速率 计 算 四元数 Q 的 即时修正 计 算 四元数 Q 的 最佳归一化 计 算 姿态阵 T 的计算 3 指北方位系统的基本原理及力学编排
2017/4/63指北方位系统解算的机上流程3指北方位系统解算的机上流程开始开始)测量,和入2(0)、2(0)、(0), V,(0) ,(0) 和F,(0)SIS胎化超线##8℃内部联道#、了、o、武,和g养基费)计#户#C即始对事,获得健最犯库了(向):开装率有期同元质,成验人初始变志,#、和%·因专中的计室元大日科健联范序T,(0)和扣始限光款提.8..1855555555955558为0归和的仓置起降C(0)和平台指令角注度。同光每一化网R元力化计ER-R,#gh,a)+#站地速:检置速率、检置地所、5
2017/4/6 5 3 指北方位系统解算的机上流程 是 否 开始 输入:(0)、L(0)、h(0)、 (0) VE 、 (0) VN 和 (0) VU 内部赋值: 、 f 、 e ie 、 Re 和 g 初始对准,获得捷联矩阵 (0) n Tb ,并获取初始 四元数;或输入初始姿态: 0 、 0 和 0 , 获 得捷联矩阵 (0) n Tb 和初始四元数 初始位置矩阵 (0) p Ce 和平台指令角速度 p ip 计算 RM 、 RN 和 g(i, h) 计算 n ie 、 n en 和 b in 测量: b ib 和 b f 计算 b nb 、 n Tb 和 n Ce 计算 n V 、 n V 、 n Tb 和 n Ce 提取: 、 、 、 和 L 四元数归一化吗? ① 结束 ① 四元数归一化 是 否 继续吗? 初 始 化 更 新1 更 新 和 计 算 归 一 化 2 t 余为 0? 开始 SINS 初始化 (位置、速度) 静基座初始对准 (姿态、四元数初始化) 速率提取与 姿态速率的计算 结束 四元数、数学平台修正、 比力坐标转换 Y N 地速、位置速率、位置矩阵、 g、高度和地球速率的更新 Y N 四元素归一 Y N 导航参数计算 Y N N Y 3 t 余为 0? 4 t 余为 0? 时间到? 1 t 余为 0? ① ① 3 指北方位系统解算的机上流程