专利名称：惯性/天文/卫星高精度组合导航系统及其导航方法
技术领域：
本发明涉及一种高精度的惯性/天文/卫星信息融合组合导航系统及导航方法，
属组合导航领域，其中，'7"表示"和"的关系。
背景技术：
导航系统的精度和稳定性是导航系统的首要指标。目前一般采用以惯性导航系 统为核心，与卫星导航、天文导航等系统进行组合的组合导航技术，提高系统的精度和稳定 性。 捷联惯性导航系统的精度是导航的关键。捷联算法不但要考虑到圆锥误差、划船 效应等误差的影响，也要考虑捷联周期对导航精度和实时性的影响。高速高动态的情况对 捷联解算的实时性即捷联解算周期提出了很高的要求。目前广泛使用的多子样算法，需要 在一个捷联解算周期内多次采样陀螺输出来提高解算精度。因此，随着计算机能力的提高， 捷联解算周期必然受限于惯性器件的输出速率。惯性器件的输出速率成为制约传统的捷联 算法实时性的重要因素。因此需要提出一种快速的捷联导航算法，降低惯性器件输出速率 对捷联解算周期的限制，提高导航的精度和实时性，以满足高速高动态情况下的导航需要。
捷联惯导系统能提供连续的导航信息，但误差随时间累积；星敏感器可以提供不 随时间发散的姿态信息，但受环境影响较大；卫星导航能提供准确的不随时间发散的位置 和速度信息，但易被电子信号干扰。因此，以惯性导航为核心的采用信息融合方式的组合导 航技术得到了重视和迅速发展。目前的组合导航系统将卫星导航系统、天文导航系统等与 惯导系统进行组合，取长补短，扩大系统的使用范围、提高导航系统的余度和容错能力，以 充分满足现代对导航系统性能长时间稳定可靠工作的要求。 多传感器组合导航系统结构可以采用集中滤波结构，也可以采用联邦滤波结构。 由于联邦滤波系统结构复杂，工程实现困难，因此，集中滤波结构在组合导航系统中被大量 采用。由于传感器的输出速率各不相同，因此多传感器的输出不同步和量测滞后是集中滤 波器必须解决的问题。但目前的惯性/天文/卫星组合导航系统中，对非等间隔输出特性 的滤波研究较少。
发明内容
技术问题 本发明的目的是针对现有技术存在的缺陷与不足，利用四阶龙格库塔算法改进传 统的捷联算法，提高捷联导航系统的实时性和精度；设计非等间隔集中滤波器的结构，满足 多传感器信息融合的需要；利用投影技术，直观显示三维航迹。
技术方案为达到上述发明目的，通过以下步骤实现 惯性/天文/卫星高精度组合导航系统，由传感器模块、组合导航解算模块、实时 显示模块依次串接组成，其特征在于所述传感器模块包括惯性传感器、星敏感器和卫星接 收机；所述组合导航解算模块包括四阶龙格库塔捷联导航解算模块、天文定姿解算模块、非等间隔卡尔曼滤波模块以及位置和速度补偿模块；所述实时显示模块由三维实时显示模块 构成；其中四阶龙格库塔捷联导航解算模块的输入端分别接惯性传感器的输出端和非等间 隔卡尔曼滤波模块的输出端；位置和速度补偿模块的输入端分别接卫星接收机的输出端和 四阶龙格库塔捷联导航解算模块的输出端；天文定姿解算模块的输入端分别接星敏感器的 输出端和位置和速度补偿模块的输出端；非等间隔卡尔曼滤波模块的输入端分别接四阶龙 格库塔捷联导航解算模块的输出端、位置和速度补偿算法模块的输出端和天文定姿解算模 块的输出端；三维实时显示模块的输入端接四阶龙格库塔捷联导航解算模块的输出端。
所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述导航 方法如下 四阶龙格库塔捷联导航解算模块利用惯性传感器输出的角速率和加速度信息进 行基于四阶龙格库塔算法的捷联导航解算，并利用非等间隔卡尔曼滤波模块输出的反馈校 正信息进行校正，输出导航速度、位置、姿态及加速度信息； 位置和速度补偿模块利用四阶龙格库塔捷联导航解算模块输出的导航速度和加 速度信息计算卫星速度和位置的补偿量，再结合卫星接收机输出的卫星速度和位置信息输 出补偿后的卫星速度和位置信息； 天文定姿解算模块利用星敏传感器输出的信息以及所述补偿后的卫星位置信息 解算得到天文导航系统的姿态信息； 非等间隔卡尔曼滤波模块将四阶龙格库塔捷联导航解算模块输出的导航速度、 位置和姿态信息，补偿后的卫星速度和位置信息以及天文导航系统的姿态信息进行卡尔曼 滤波得到反馈校正信息； 三维实时显示模块将方法所述导航位置信息实时进行三维图形显示。 所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述四阶
龙格库塔捷联导航解算模块的解算方法如下 选取"东北天"地理坐标系作为导航坐标系；
速度的微分方程为
"dK+O v + g (3.1)
位置微分方程为
〖=
(3.2)
十/7)C0S丄A = V
其中，v = [、 vn vu]， 、 Vn Vu分别表示地理系下东、北、天向的速度…表示V的
对时间的导数，以下参数上的符号" "均表示该参数对时间的导数；fibb为加速度计输出，
(^为载体系到地理系的姿态转移矩阵，"J为地球自转角速率在地理系下的投影，"J为
地理坐标系相对地球坐标系的转动角速率在地理系下的投影；且有
<formula>formula see original document page 9</formula>
" L， h分别为地理系下的经度、纬度、高度；RN为卯酉圈曲率半径， 一_&
^-[G-乂)2sin2z + cos2丄]1。 ， Re为WGS84大地坐标系地球参考椭球的赤道平
面半径，fr为为WGS84大地坐标系地球参考椭球的椭圆度；RM为子午圈曲率半径，
姿态微分方程为
57T '下同；g =
， g。表示重力加速度，下同；
(3.4)
其中，
(35)
cnb为cbn的转置，"ibb为陀螺仪的输出；
在某一时刻t，地理系下的速度，位置分别为V(t) = [Ve(t)Vn(t) Vu(t)]，p(t)
=[A(t) L(t) h(t)]，载体系到地理系的姿态转移矩阵为Cbn(t)，经过At时间后，在 t+At时刻地理系的速度，位置分别变为v(t+At) = [Ve(t+At) vn(t+At) vu(t+At)]， p(t+At)=[入(t+At)L(t+At)h(t+At)]，Cbn(t+At)，贝U由式(3. l)，式(3. 2)和式
(3. 4
(及w +WcosZ
nbb(l)，CbnX"nbb(2)，
(3.10)
取/ tt+AtK(t)dt的一阶解析式，有l^(0* = "0><仏则式(3. 10)为 F(t+A t) = F(t)+K(t) X A t (3.11)
利用式(3. 11)，得到t时刻，两次t+ A t/2时亥lj， t+ A t时刻对应的F分别为F。 (t)，Fjt+At/2)， F2(t+At/2)， F(t+At)，再利用式(3. 3)和式(3. 8)得到对应时刻的K分别
为K0 (t) ， & (t+ A t/2) ， K2 (t+ A t/2) ， K (t+ A t); 利用四阶龙格库塔算法，得到每个A t时间内的更新式 + A,) = + 二[K。 (f) + 24 (" Af / 2) + 2K2 (, + A〃 2) + J5T(f + Af)]
6 令At等于捷联解算周期T，则利用式(3. 12)，得到-航运算的更新式
(3.12)
个捷联解算周期内，捷联导 + 77)=刚+ — [K0(,) + 2/^ (, + 77 2) + 2K2(f + 77 2) + T)]
6
(3.13) 按照式(3. 13)进行迭代计算，得到每个捷联周期的速度、位置和姿态信息；同时，在非等间隔卡尔曼滤波模块输出的时刻，利用输出的反馈校正信息对对应的捷联解算输出的速度、位置和姿态信息进行校正。 所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述位置和速度补偿模块的解算方法如下 tkp时刻获得卫星导航信息，tks时刻获得星敏感器信息，tkp时刻到tks时刻时间段捷联惯导系统每个周期T输出的信息如下、i、 vni、 vui表示每个周期输出的东向、北向和天向速度，a^ani、aui表示对应的加速度信息，Lphi表示对应的纬度和高度；RMi、RNi表示对应的子午圈半径和卯酉圈半径，i = tkp， tkp+T， tkp+2T，…tks ;
则建立速度补偿量计算公式如下一 ffa
△、 = IXx:r
'、
位置补偿量计算公式如下'fa 1
(4.1)
1 T
(4.2)
*= 其中，Av。 Avn、 Avu分别表示东向、北向、天向速度在tkp时刻到tks时刻这段时间内的速度补偿量；A A ， AL， AH分别表示经度、纬度和高度在tkp时刻到tks时刻这段时间内的位置补偿量； Le、 A。he表示卫星接收机tkp时刻输出的纬度、经度、高度位置，Vee、Vn(;、 l表示卫星接收机tkp时刻输出的地理坐标系下东向、北向、天向的速度，则tks时刻对应的位置和速度补偿模块的输出分别为Le+ A L、 A e+ A A 、 he+ A H， vee+ A ve、 vne+ A vn、 Vue+ A vu。
所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述的非等间隔卡尔曼滤波模块的解算方法如下
选取"东北天"地理坐标系作为导航坐标系组合导航系统的状态方程为
其中，
A " "
(5. 2)
(5.1)
义,
V▽▽ l7
"/y ' '7
小u表示东向、北向、天向平台误差角，SVe SVn Svu表示东向、北向、天
向速度误差，SL Sh表示经度、纬度、高度位置误差，ebx eby e^表示三轴陀螺的
常值漂移，、x 、y 、,表示三轴陀螺的一阶马尔科夫过程、Vn v,z表示三轴加速度的一阶马尔科夫过程；A表示系统的状态转移矩阵，W表示系统的噪声矢量，G表示系统的噪声系数矩阵； Z =
方程为仏
瓦
仏
(5.3) 其中，Z = [ S Vevn入Sh
_100—，010，o3x3，o3x9
001
_4 cos Z0 0A =o3x3，o3x3，0& o
00 1
(5. 4)
(5.5)
，0
3x9
(5.6) =
-1
，03x3 ，0
3x3
，0
3x9
(5.7)
siny cosy 0
cosycos^ -sinycosP 0sin y sin Pcosy sin夕 一cosPY 、 e 、 v表示载体真实的横滚角、俯仰角与航向角；s Y ， s e ， S 表示横滚
角、俯仰角、航向角误差；V表示系统的测量噪声矢量；03><3表示三行三列的全零矩阵；03X9表示三行九列的全零矩阵； 根据式(5. 7)，当俯仰角9为90°时，cos e = 0， Ha矩阵会产生奇点。当俯仰角
e为90°时，将e =90°代入下述两式
sin^/cosS cos^/cose sin夕 :
cos^siny—sin^/sin0cos;K —cos^sinPcosy—sin^/sin;^ cosPcosy
(5.8)<formula>formula see original document page 13</formula>息的时刻，利用四阶龙格库塔捷联解算模块的输出，位置和速度补偿模块的解算的输出，以
及天文定姿解算模块的输出，同时进行卡尔曼滤波器的时间更新和量测更新。 所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述实时
三维显示方法如下 首先按正等侧投影关系，绘制出真实的三维航迹，然后将所述导航位置信息按正
等侧投影关系动实时绘制出导航后的航迹，通过比较两条航迹得到导航定位的直观比较结
果；所述正等侧投影方法如下 x2d = x3d+z3dcos45° ; y2d = y3d+z3dcos45° 其中(x3d， y3d， z3d)为所述输出的导航三维位置信息在三维直角坐标系内的坐标， (x2d， y2d)为将所述输出的导航三维位置信息按正等侧投影方法在二维直角坐标系内的坐 标。
有益效果 本发明从实际的捷联/天文/卫星组合导航系统工程应用角度出发，实现了高精 度的捷联/天文/卫星组合导航系统。该系统实现了基于四阶龙格库塔算法的捷联导航解 算，提高了捷联解算的实时性和精度；实现了载体垂直机动时的姿态组合；实现了针对非 等间隔集中卡尔曼滤波器；利用投影算法实现了组合导航结果实时三维显示。 本发明使用的陀螺等效漂移为o.or /h，加速度计等效零偏为1X10—4g。采用四 阶龙格库塔算法的捷联解算，在一小时内的位置漂移小于iooo米，有效提高了捷联解算的 精度。惯性/天文/卫星组合导航系统，在载体垂直机动时，其姿态依然可以快速收敛，精 度保持和其他机动状态一致。在使用的星敏感器精度20角秒，GPS位置精度为10米，速度 精度为0. 2米/秒的情况下，组合导航系统定位精度优于5米，速度精度0. 1米/秒，姿态 角精度达到15角秒。本发明可以作为惯性/天文/卫星组合导航系统的演示和验证平台， 同时，具有很强的工程应用价值。
图1惯性/天文/卫星组合导航系统结构图。
图2是四阶龙格库塔捷联算法解算流程图。
图3器件不同步时的组合导航系统时间关系图。
图4惯性/天文/卫星组合导航系统实时显示界面。
具体实施例方式
下面结合附图对本发明的技术方案进行详细说明 本发明惯性/天文/卫星高精度组合导航系统如图l所示。系统由惯性传感器、星 敏感器、卫星接收机、导航计算机即组合导航解算模块和显示计算机即实时显示模块组成。 系统完全具备实际工程应用能力。 导航计算机包括组合导航解算模块，所述组合导航解算模块包括四阶龙格库塔捷 联导航解算模块、天文定姿解算模块、非等间隔卡尔曼滤波模块以及位置和速度补偿模块；所述实时显示模块由三维实时显示模块构成。其中四阶龙格库塔捷联导航解算模块的输 入端分别接惯性传感器的输出端和非等间隔卡尔曼滤波器模块的输出端；位置和速度补偿 算法模块的输入端分别接卫星接收机的输出端和四阶龙格库塔捷联导航解算模块的输出 端；天文定姿解算模块的输入端分别接星敏感器的输出端和位置和速度补偿算法模块的输 出端；非等间隔卡尔曼滤波器模块的输入端分别接四阶龙格库塔捷联导航解算模块的输出 端、位置和速度补偿算法模块的输出端和天文定姿解算模块的输出端；三维实时显示模块 的输入端接四阶龙格库塔捷联导航解算模块的输入端。 所述的惯性/天文/卫星高精度组合导航系统的导航方法如下 四阶龙格库塔捷联导航解算模块利用惯性传感器(包括陀螺仪和加速度计)输
出的角速率和加速度信息进行基于四阶龙格库塔算法的捷联导航解算，并利用非等间隔卡
尔曼滤波模块输出的反馈校正信息进行校正，输出导航速度、位置、姿态及加速度信息； 位置和速度补偿模块利用四阶龙格库塔捷联导航解算模块输出的速度和加速度
信息计算卫星速度和位置的补偿量，结合卫星接收机输出的卫星速度和位置信息输出补偿
后的卫星速度和位置信息； 天文定姿解算模块利用星敏传感器输出的信息以及所述补偿后的卫星速度和位 置信息解算得到天文导航系统的姿态信息； 非等间隔卡尔曼滤波模块将四阶龙格库塔捷联导航解算模块输出的导航速度、 位置、姿态，补偿后的卫星速度和位置信息以及天文导航系统的姿态信息利用设计的非等 间隔卡尔曼滤波器进行滤波； 三维实时显示模块将方法所述输出的三维位置信息实时进行三维图形显示。
以下结合附图具体叙述本发明实施方法的过程 1惯性测量器件数据、卫星数据和天文数据的实时采集 实时采集惯性传感器数据、卫星接收机数据和星敏感器数据。 2基于四阶龙格库塔算法的捷联惯导解算 选取"东北天"地理坐标系作为导航坐标系 速度的微分方程为 !)-CH《+《"+ g (2.1)
位置微分方程为
〖=
C^+Z0cosZ / = v (2.2)
其中，V = [、 Vn Vu]， 、 Vn Vu表示地理系下东、北、天向的速度；1)表示V的对时
间的导数，以下参数上的符号" "均表示该参数对时间的导数，不再单独说明；f^为加速
度计输出，(^为载体系到地理系的姿态转移矩阵，"J为地球自转角速率在地理系下的投
影，"J为地理坐标系相对地球坐标系的转动角速率在地理系下的投影；，且有
0
<yfe cos丄 sin Z
姊
(2.3)
4 =
入，L， h分别为地理系下的经度、纬度、高度；RN为卯酉圈曲率半径， _^_
「门 "2 . 2 f丄~~' Re为WGS84大地坐标系地球参考椭球的赤道平 (1 —sin i> + cos丄」
面半径，4为为WGS84大地坐标系地球参考椭球的椭圆度，RM为子午圈曲率半径， (1-X)乂
4 = ， g。表示重力加速度，下同；
(2.5)
其中，
必K《(《+《)
cnb为cbn的转置，"ibb为陀螺仪的输出；
假设某时刻t，地理系下的速度，位置分别为V(t) = [Ve(t) Vn(t) Vu(t)]，p(t)=，载体系到地理系的姿态转移矩阵为Cbn(t)，经过At时间后，在t+At 时刻分另变为v(t+At) = [Ve(t+At) Vn(t+At) vu(t+At)]， p(t+At)=[入(t+At) L(t+At) h(t+At)]， Cbn(t+At)，贝U由式(2. 1)，式 (2. 2)和式(2. 4)，可以得到变化前后的关系为: Cbn(l)，Cbn(2)，Cbn(3)分别表示(;n矩阵的第l，2，3行；( CbnX "nbb(3)分别表示(^矩阵与"2矩阵乘积的第1，2，3行。
(%+/ )cosZ ， VJ
、hb(l)，ChnX"nhb(2)，
由式(2. 6) ， (2. 7)禾P (2. 8)可以得到式(2. 9)在t时刻和t+A t时刻的关系为 <formula>formula see original document page 17</formula>
取/ tt+AtK(t)dt的一阶解析式，有J[^i(W&M)xA"则式(2. 9)可简化为
<formula>formula see original document page 17</formula>
由K的表达式可知，K可以由对应时刻的F，陀螺仪输出"ibb和加速度输出f^通 过式(2. 1)，式(2. 3)和式(2. 5)求得。考虑利用(2. 5)式求取"nbb时，由于每个捷联计算 周期只采集一次陀螺仪输出，即只有一个"ibb值，因此，假设每个捷联周期内角速率是恒定 的，利用式(2. 3)和式(2.8)通过更新(;n， "J， "J来更新"nbb。
解算流程如图2所示。 按照如图2所示的流程通过计算分别得到t时刻，两次t+A t/2时刻，t+A t时 刻对应的F分别为<formula>formula see original document page 17</formula>以及对应时刻的K分别为<formula>formula see original document page 17</formula>后，即可利用四阶龙格库塔算法，得到每个A t 时间内的更新式
<formula>formula see original document page 17</formula> 令At等于捷联解算周期T，则可以利用式(2. ll)，得到一个捷联解算周期内，捷 联导航运算的更新式
<formula>formula see original document page 17</formula> 若非等间隔卡尔曼滤波模块无反馈校正信息输出，按照式(3. 13)进行计算，即可 得到四阶龙格库塔捷联导航解算模块输出的速度、位置和姿态信息；在非等间隔卡尔曼滤 波模块输出反馈校正信息的时刻，利用输出的反馈校正信息对按照式(3. 13)计算得到的 对应时刻的的速度、位置和姿态信息进行校正，即可得到四阶龙格库塔捷联导航解算模块 输出的速度、位置和姿态信息； 3.捷联/天文/卫星组合导航卡尔曼滤波器设计步骤
选取"东北天"地理坐标系作为导航坐标系。
组合导航系统的状态方程为<formula>formula see original document page 17</formula> 其中，
<formula>formula see original document page 17</formula> t (K表示东向、北向、天向平台误差角，Sve SVn Svu表示东向、北向、天
向速度误差，SL Sh表示东向、北向、天向位置误差，ebx eby e^表示三轴陀螺的
常值漂移，、x 、y 、,表示三轴陀螺的一阶马尔科夫过程、V" V7 Vc表示三轴加速度 的一阶马尔科夫过程。 A表示系统的状态转移矩阵，W表示系统的噪声矢量，G表示系统的噪声系数矩阵。
定义量测方程为<formula>formula see original document page 18</formula> < W; +邻 ^/' = ^ + ， 当俯仰角为90度时，将e 起代入(3.8)式，可以得到
—cosy0 —=00
i0_1
(3.12)
90°代入式(3. 9)和(3. 10)，化简后和式(3. 11) 一
(3.13) 由上式可以看出，H',不满秩，因此其逆矩阵是无法求得的。在俯仰角为9(T 考虑到实际上航向角实际是无法准确确定的，因此，为使H',满秩，将H' $变为 siny/ -cos^/ 0
—' (3.14)
时，
=
cosy/ 1
0
0 —1
则可以求得其逆矩阵为 sin^cosy/ 0 -cos^/ si呼 0 sinycosy/ -l
则在俯仰角为90。时， Ha — [H^ ， 03X3， 03X3， 03Xl
在建立了系统方程和量
(3.15)
(3. 16)
方程后，将系统方程和:
J方程离散化，即可建立卡尔
曼滤波器。 4.速度位置补偿算法设计步骤 在进行卡尔曼滤波时，必须保证所有输入的信息是同一时刻的。天文定姿解算的 周期取决于星敏感器的输出周期。目前星敏感器的输出周期一般大于卫星导航的输出周 期。假设tkp时刻获得卫星导航信息，tks时刻获得星敏感器信息。为使滤波器在星光输出 时刻进行滤波，必须将卫星数据补偿到tks时亥lj。考虑到步骤2中捷联惯导系统的输出在短 时间精度较高，因此利用tkp时刻到tks时刻这段时间内的捷联惯导系统输出的速度和加速 度信息，将卫星导航信息外推到tks时刻。 vei、 vni、 vui表示tkp时刻到tks时刻时间段步骤2中的捷联惯导系统每个周期T输出 的东向、北向和天向速度；^、ani、aui表示对应的加速度输出信息，i = tkp， tkp+T， tkp+2T，… tks ;Li、hi表示对应的纬度和高度；RMi、RNi表示对应的子午圈半径和卯酉圈半径。
则可以建立速度补偿量计算公式如下
_、
△v =
，
a"
X
J-
、
,4
19位置补偿:
t计算公式如下 1
1
(4.2)
其中，A、、 Avn、 Avu分别表示东向、北向、天向速度在tkp时刻到^时刻这段时 A入，AL， AH分别表示经度、纬度和高度在tkp时刻到tks时刻这段时
间内的速度补偿』 间内的位置补偿量。 假设L。 A。he表示卫星接收机tkp时刻输出的纬度、经度、高度位置，veC、 vnC、 vuC 表示卫星接收机tkp时刻输出的地理坐标系下东向、北向、天向的速度，则利用式(4. 1)和式 (4. 2) tks时刻对应的输出分别为LG+ A L、 A G+ A A 、 hG+ A H， ve(;+ A ve、 vn(;+ A vn、 vu(;+ A vu。
5.器件不同步的组合滤波步骤 利用卡尔曼滤波器进行滤波时，必须保证输入信息是同一时刻的。假设步骤2中 捷联解算模块的的输出周期为T，星敏感器的输出周期(即天文姿态解算模块的输出周期) 为T^r，卫星接收机的输出周期为TePS，目前TePS < Tst ;卡尔曼滤波离散化周期为TD。同时 满足T。 = LXT， Tstar = MXTD， TGPS = NXTD， AT = (M_N) XTD， L、M、N为正整数。
以一个离散周期的处理过程为例。假设此次滤波周期的起始时刻为tk，从图3可 以清楚地看出，当星光仪测量输出时，此时卫星接收机的输出信息超前于星光仪的输出。
当没有星敏感器和卫星接收机信息的输出时，即在tk+m *TD时刻，其中，m = 1，2… N，只利用系统状态转移矩阵的特性，进行卡尔曼滤波器的时间更新；在接收到卫星接收机 信息而没有接收到星敏感器信息的AT时间段内，同样只进行卡尔曼滤波器的时间更新； 在接收到星敏感器信息的tk+M TD时刻，首先利用步骤3将tk+N TD得到的卫星接收机数 据利用步骤4的方法外推到tk+M TD时刻，同时，结合步骤2以及天文定姿解算模块的输 出，同时进行卡尔曼滤波器的时间更新和量测更新。
6.实时显示步骤 对任意一个由多面体组成的三维图形，在三维直角坐标系内，可以分解为许多条 由空间的两个点组成的直线。而PC机屏幕是一个平面，它上面只有二维坐标。因此利用正 等侧投影方法将三维坐标投影到二维平面坐标内。导航航迹的绘制采用的是VC++6. OMFC 中的绘图类库。 假设空间某点的空间坐标为(x3d， y3d， z3d)，则其正等侧投影后的平面坐标为
x2d = x3d+z3dcos45°
y2d = y3d+z3dcos45° 绘制曲线时，首先根据选择的航迹数据在屏幕上的三维地理坐标系内画出一条真 实的航迹，然后以组合导航解算模块输出的数据为基础作动画显示，画出另一条航迹，通过 比较这两条航迹，可以非常直观地看出导航定位的结果。
下面说明在Visual C++中程序的具体实现步骤
首先要建立一个位图对象，再定义并创建一个与位图对象相兼容的内存设备描述 对象。 1.按正等侧投影关系，先将设置的三维真实航迹画好。为了具有通用性，使载体无 论运行多大区域，始终能处于三维坐标系的中间，首先将全部航迹读入，确定三维方向的最 大值和最小值，然后根据指定区域的大小，计算出比例系数，再绘制出真实航迹曲线。
2.同样按照正等侧投影关系，将组合导航解算模块输出的数据实时显示在平面坐 标系中，绘制出实时航迹曲线。同时，为了显示出航迹的实时运动，采用符号"众"指示载体 当前时刻所在的位置。符号"众"在运行中根据组合导航解算模块输出的数据实时刷新，就 形成了载体的连续运动。 设计完成的实时显示界面如图4所示。 本发明说明书中，四阶龙格库塔捷联解算模块中利用非等间隔卡尔曼滤波模块输 出的反馈校正信息进行校正，以及非等间隔卡尔曼滤波模块中对状态方程和量测方程进行 离散化的方法，都属于本领域专业技术人员公知的现有技术。
权利要求
一种惯性/天文/卫星高精度组合导航系统，由传感器模块、组合导航解算模块、实时显示模块依次串接组成，其特征在于所述传感器模块包括惯性传感器、星敏感器和卫星接收机；所述组合导航解算模块包括四阶龙格库塔捷联导航解算模块、天文定姿解算模块、非等间隔卡尔曼滤波模块以及位置和速度补偿模块；所述实时显示模块由三维实时显示模块构成；其中四阶龙格库塔捷联导航解算模块的输入端分别接惯性传感器的输出端和非等间隔卡尔曼滤波模块的输出端；位置和速度补偿模块的输入端分别接卫星接收机的输出端和四阶龙格库塔捷联导航解算模块的输出端；天文定姿解算模块的输入端分别接星敏感器的输出端和位置和速度补偿模块的输出端；非等间隔卡尔曼滤波模块的输入端分别接四阶龙格库塔捷联导航解算模块的输出端、位置和速度补偿算法模块的输出端和天文定姿解算模块的输出端；三维实时显示模块的输入端接四阶龙格库塔捷联导航解算模块的输出端。
2. —种如权利要求1所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特 征在于所述导航方法如下四阶龙格库塔捷联导航解算模块利用惯性传感器输出的角速率和加速度信息进行基 于四阶龙格库塔算法的捷联导航解算，并利用非等间隔卡尔曼滤波模块输出的反馈校正信 息进行校正，输出导航速度、位置、姿态及加速度信息；位置和速度补偿模块利用四阶龙格库塔捷联导航解算模块输出的导航速度和加速度 信息计算卫星速度和位置的补偿量，结合卫星接收机输出的卫星速度和位置信息输出补偿 后的卫星速度和位置信息；天文定姿解算模块利用星敏感器输出的信息以及所述补偿后的卫星位置信息解算得 到天文导航系统的姿态信息；非等间隔卡尔曼滤波模块将四阶龙格库塔捷联导航解算模块输出的导航速度、位置和姿态信息，补偿后的卫星速度和位置信息以及天文导航系统的姿态信息进行滤波得到反 馈校正信息；三维实时显示模块将所述导航位置信息实时进行三维图形显示。
3. 根据权利要求2所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征 在于所述四阶龙格库塔捷联导航解算模块方法如下选取"东北天"地理坐标系作为导航坐标系 速度微分方程为1> = 0(20 v + g (3.1)位置微分方程为Z = V" 乂 =_^￡_力一 ■ 。"其中，V= [、 Vn Vu]， 、 Vn Vu分别表示地理系下东、北、天向的速度…表示V对时间的导数，以下参数上的符号" "均表示该参数对时间的导数；fibb为加速度计输出，Cbn为载 体系到地理系的姿态转移矩阵，"J为地球自转角速率在地理系下的投影，"J为地理坐 标系相对地球坐标系的转动角速率在地理系下的投影；且有<formula>formula see original document page 3</formula>入，L， h分别为地理系下的经度、纬度、高度；RN为卯酉圈曲率半径， 尺177 ， Re为WGS84大地坐标系地球参考椭球的赤道平<formula>formula see original document page 3</formula>面半径，fr为WGS84大地坐标系地球参考椭球的椭圆度；RM为子午圈曲率半径，<formula>formula see original document page 3</formula>姿态微分方程为i '下同；g =
， g。表示重力加速度，下同；(3.4)其中，(3.5)Cnb为Cbn的转置，"ibb为陀螺仪的输出；在某一时刻t，地理系下的速度，位置分别为V(t) = [Ve(t)Vn(t) Vu(t)]，p(t)=[A(t) L(t) h(t)]，载体系到地理系的姿态转移矩阵为Cbn(t)，经过At时间后，在 t+At时刻地理系的速度，位置分别变为v(t+At) = [Ve(t+At) vn(t+At) vu(t+At)]， p(t+At)=[人(t+At) L(t+At) (3. 4)，变化前后的关系为:<formula>formula see original document page 3</formula>贝U由式(3. l)，式(3. 2)和式<formula>formula see original document page 3</formula>其中，a e。表示/ tt+At"nbbdUMl联立式(3. 1)，式(3. 2)，式(3. 4)则有:(3.9)其中，<formula>formula see original document page 3</formula>Cbn(l)， Cbn(2)， Cbn(3)分别表示C矩阵的第1，2，3行；CX CbnX "nbb(3)分别表示(;n矩阵与"2矩阵乘积的第1，2，3行；由式(3. 6) ， (3. 7)和(3. 8)得到式(3. 9)在t时刻和t+A t时刻的关系为:W + A,):，+r 、(, (3.10)取/ tt+AtK(t)dt的一阶解析式，有f+"iS:(0^:XWxA"则式(3. 10)变为F(t+A t) = F(t)+K(t) X A t (3.11)利用式(3. 11)，得到t时刻，两次t+A t/2时刻，t+A t时刻对应的F分别为F。(t)， Fjt+At/2)， F2(t+At/2)， F(t+At)，再利用式(3. 3)和式(3. 8)，得到对应时刻的K即 K0 (t) ， K丄(t+ A t/2) ， K2 (t+ A t/2) ， K (t+ A t);利用四阶龙格库塔算法，得到每个At时间内的更新式+ AO = F(O + ^ [l0 (0 + 0 + A〃 2) + 212 0 + A, / 2) + "(, + AO] 6(3.12)令At等于捷联解算周期T，则利用式(3. 12)，得到一个捷联解算周期内，捷联导航运 算的更新式7\f(,+r) = f(o+i[，)++772)+2W+r/2)++r)] (3.13)按照式(3. 13)进行迭代计算，得到每个捷联周期的速度、位置和姿态信息；同时，在非等间隔卡尔曼滤波模块输出的时刻，利用输出的反馈校正信息对对应的捷联解算输出的速 度、位置和姿态信息进行校正。
4.根据权利要求2所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述位置和速度补偿模块的解算方法如下tkD时刻获得卫星导航信息，tks时刻获得星敏感器信息，tkD时刻到tks时刻时间段捷联惯导系统每个周期T输出的信息如下、i、 vni、 vui表示每个周期输出的东向、北向和天向速度，a^ani、aui表示对应的加速度信息，Lphi表示对应的纬度和高度；RMi、RNi表示对应的子午圈半径和卯酉圈半径， 则建立速度补偿』(fa* 位置补偿量计算公式如下tkp， tkp+T， tkn+2T，kp计算公式如下(4.1)'、*m+^' (4.2)其中，Av。 Avn、 Avu分别表示东向、北向、天向速度在tkp时刻到k时刻这段时间内 的速度补偿量；A A ， AL， AH分别表示经度、纬度和高度在tkp时刻到tks时刻这段时间内的位置补偿量；Le、 A。he表示卫星接收机tkp时刻输出的纬度、经度、高度位置，Vee、Vn(;、 v^表示卫星 接收机tkp时刻输出的地理坐标系下东向、北向、天向的速度，则tks时刻对应的位置和速度 补偿模块的输出分别为LG+ A L、 、+ A入、hG+ A H， veG+ A ve、 vnG+ A vn、 vuG+ A vu。
5.根据权利要求2所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征 在于所述的非等间隔卡尔曼滤波模块的解算方法如下选取"东北天"地理坐标系作为导航坐标系组合导航系统的状态方程为<formula>formula see original document page 5</formula>(5.1)其中，<formula>formula see original document page 5</formula>(5.2)<formula>formula see original document page 5</formula>小u表示东向、北向、天向平台误差角，SVe SVn S Vu表示东向、北向、天向速 Sh表示经度、纬度、高度位置误差，ebx eby h,表示三轴陀螺的常值 表示三轴陀螺的一阶马尔科夫过程、V^ V^ v^表示三轴加速度的一& 度误差，SL S入漂移，en 、y e 阶马尔科夫过程；A表示系统的状态转移矩阵，W表示系统的噪声矢j 数矩阵；量测方程为:，G表示系统的噪声系好v反其中，<formula>formula see original document page 5</formula>及-<formula>formula see original document page 5</formula>(5.5)<formula>formula see original document page 5</formula>(5.6)<formula>formula see original document page 5</formula>(5.7)siny cosy 0H = - cosycos0 —si輝cos0 0si輝sin夕cosy sin 0 -cosPy 、 e 、 v表示载体真实的横滚角、俯仰角与航向角；s Y ， s e ， s 表示横滚角、俯仰角、航向角误差；V表示系统的测量噪声矢量；03><3表示三行三列的全零矩阵；03><9表示三 行九列的全零矩阵；根据式(5. 7)，当俯仰角e为90°时，cos e = 0， Ha矩阵产生奇点当俯仰角9为90°时，将e =90°代入下述两式Cf= s—oosP cos^/cosP sin^ (5.8)8111^/8511《5111/+€05^/0 / cos^/sin^sin/-sin^/cos/ —cos《sin/ g= sin一cos《 cos^/cos" sin^ (5.9)oos^/sin/—sin^/sin6^cos;/ —cos—in《cos;/—sin^/sin;/ cos《cos;/其中，y' 、 e' 、 v'分别为载体实际的横滚、俯仰和航向角，与载体真实的横滚角、 俯仰角与航向角y 、 e 、 v之间的关系为 ,=尸+々6>'^ +朋 (5.10)Cpb为平台坐标系到载体坐标系的姿态转移矩阵，为地理坐标系到载体坐标系的姿态转移矩阵，且满足C;=《 (5.11)C〕为平台坐标系到地理坐标系的姿态转移矩阵， 1—A A' (5.12)、1由式(5.8) 式(5. 12)可以得到:-A 1—0—cosy0_5;k0si呼0朋= 10—1(5.13)将H'0 0—1(5.14) =(5.15)$变为 siny/ —cosy1 0则可以求得其逆矩阵为 siny/cos^/ 0 一cos^ siny 0则在俯仰角为90。时， Ha = [^，03><3，03><3，03 在建立了系统方程和3 器进行滤波。利用卡尔曼滤波器进行滤波时，四阶龙格库塔捷联解算模块的的输出周期为T，星敏感 器的输出周期即天文姿态解算模块的输出周期为； ，卫星接收机的输出周期为TePS，目前 TCPS < Tstar ;卡尔曼滤波离散化周期为TD，同时满足TD = LXT， Tstar = MXTD， TePS = NXTD， AT= (M-N) XTD， L、M、N为正整数；6j(5. 16)方程后，将系统方程和:J方程离散化，再利用卡尔曼滤波当没有星敏感器和卫星接收机信息的输出时，在每个离散化周期TD只利用系统状态转移矩阵的特性，进行卡尔曼滤波器的时间更新；在接收到卫星接收机信息而没有接收到星敏感器信息的时间段内，同样只进行卡尔曼滤波器的时间更新；在接收到星敏感器信息的时刻，利用四阶龙格库塔捷联解算模块的输出，位置和速度补偿模块的解算的输出，以及天文定姿解算模块的输出，同时进行卡尔曼滤波器的时间更新和量测更新。
6.根据权利要求2所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述实时三维显示方法如下首先按正等侧投影关系，绘制出真实的三维航迹，然后将所述导航位置信息按正等侧投影关系实时绘制出导航后的航迹，通过比较两条航迹得到导航定位的比较结果；所述正等侧投影方法如下X2d — X3d+Z3dCOS45y2d = y3d+z3dcos45°其中(x3d，y3d， z3d)为所述的导航位置信息在三维直角坐标系内的坐标，(x2d， y2d)为将所述导航位置信息按正等侧投影方法在二维直角坐标系内的坐标。
全文摘要
本发明提供一种惯性/天文/卫星高精度组合导航系统及其导航方法。本发明系统由传感器模块依次串接组合导航解算模块、实时显示模块组成，所述传感器模块包括惯性传感器、星敏感器和卫星接收机，所述组合导航解算模块包括四阶龙格库塔捷联导航解算模块、天文定姿解算模块、非等间隔卡尔曼滤波模块以及位置和速度补偿算法模块，所述实时显示模块由三维实时显示模块构成。本发明方法实现传感器数据的实时采集；利用惯性传感器数据进行基于四阶龙格库塔算法的捷联导航解算；能够对惯性传感器数据、星敏感器数据和卫星接收机数据进行组合导航解算；对组合导航结果利用投影算法进行实时三维显示。
文档编号G01C21/20GK101706281SQ20091021261
公开日2010年5月12日 申请日期2009年11月13日 优先权日2009年11月13日
发明者于永军, 刘建业, 曾庆化, 李荣冰, 熊智, 高春雷 申请人:南京航空航天大学