专利名称：适合于光纤陀螺的载体姿态测量方法
技术领域：
本发明提供的是一种测量方法，具体地说是一种使用光纤陀螺测量载体相对 于导航坐标系姿态参数的方法。
(二)
背景技术：
在捷联惯性导航系统中，惯性传感器(陀螺和加速度计)直接固联在载体上。 它们敏感载体相对于惯性坐标系的姿态变化及速度变化信息，这些信息经过导航 计算机的处理输出导航参数，从而完成导航制导任务。由于采用惯性传感器固联 的方式，惯性传感器会直接敏感载体的振动和环境的干扰，对捷联惯性导航系统 的测量精度带来一定的影响。在捷联惯性导航系统中，采用的是数学平台，主要 依靠陀螺的输出信息来构建数学平台，以描述载体相对导航坐标系的姿态。而陀 螺的输出为角增量或角速度，不能直接用于描述坐标系的相对关系。在常规的捷 联惯性导航系统的姿态测量方法中，总是认为载体坐标系和导航坐标系间的转换 是通过一系列的转动来实现的。所以，为了测量载体的姿态，需要由相应的角速 度或角增量来求得描述坐标系关系的四元数或旋转矢量。但是在高动态环境下， 在姿态更新周期内的每次陀螺采样对应的载体坐标系的位置是不同的。另外，传 统方法中是假设从载体坐标系到参考坐标系间的转动的次序是可以不予考虑的， 这是基于无限小转动是矢量的原理来得到的。但实际的工程中，特别是在载体高 动态机动情形或恶劣振动环境下，载体的转动常常是有限转动，而有限转动不是 矢量，其转动次序不能交换。因而将姿态更新周期中的所有陀螺输出的角增量求 和后再求解四元数的传统姿态测量方法将带来较大的误差，即圆锥误差。圆锥误 差类似于陀螺漂移，给载体的姿态测量带来负面影响。补偿圆锥误差，高质量地 测量载体的姿态就必须设计高性能的姿态测量方法。
在传统的姿态测量方法中，利用陀螺输出的角增量去估算出圆锥补偿项，从
而估算出旋转向量的增量。该基本思路是基于在传统陀螺的输出中，角增量的获 取是很方便的。即要想得到旋转向量的增量必须知道陀螺输出的角增量。而在目 前广泛使用的光纤陀螺捷联惯性导航系统中，光纤陀螺的输出信号为角速度信 息，为此，通常的做法是采用分段线性化，认为在每一个采样周期内载体的角速 度是常量。这种假设在采样周期足够小且载体的角速度变化较慢的时候是成立的，但对于高动态的场合就必须另加考虑了。已有的研究成果表明，在光纤陀螺 输出角速度的情形下，对于传统的基于旋转矢量的姿态测量方法来说，性能不如 常规的基于四元数的姿态测量方法，无法体现在应对圆锥效应的优越性。
发明内容
本发明的目的在于提供一种能够解决在载体高动态环境或是高频率振动环 境中，圆锥效应对于载体姿态测量精度产生影响的问题的适合于光纤陀螺的载体 姿态测量方法。
本发明的目的是这样实现的-
步骤1、通过外部设备确定载体的初始位置参数与初始速度值；
步骤2、光纤陀螺捷联惯性导航系统进行初始对准，确定载体相对导航坐标 系的初始姿态，得到姿态四元数的初始值；
步骤3、确定姿态更新周期// = ^-^)_1,所述姿态更新周期/Z等于AM咅的 光纤陀螺采样周期& = 所述7V为大于零的整数；
步骤4、采集光纤陀螺输出的载体相对于惯性坐标系的角速度计算旋转矢量 的增量A^;
步骤5、通过旋转矢量与四元数的关系，得到姿态更新周期/f内姿态更新四 元数g(//)，
<formula>formula see original document page 8</formula>其中p为旋转矢量增量的模，<formula>formula see original document page 8</formula>
步骤6、由姿态四元数更新方程更新姿态四元数
其中姿态更新四元数《(//)由步骤5得到，0(、) 、 g(^)分别表示载体在G 、々M 时刻的姿态四元数；
步骤7、利用步骤6获得的《,时刻姿态四元数^(^)=[《。&《2 ^:r计算 载体坐标系b系相对于导航坐标系n系的捷联矩阵r，
<formula>formula see original document page 9</formula>步骤8、利用步骤7获得的捷联矩阵r求得载体相对导航坐标系的姿态角， 导航坐标系选取为游动方位坐标系，则载体的格网航向角^、纵摇角e、横摇角 y都可由捷联矩阵r中的元素来表示，
<formula>formula see original document page 9</formula>33
其中格网航向角^的定义域为(0。,360。)；纵摇角^的定义域为(-90°，90°);横摇 角/的定义域为(-卯°,90°)，
由姿态角的定义域得到它们的真值， 格网航向角的真值为
<formula>formula see original document page 9</formula>纵摇角的真值为 横摇角的真值为
<formula>formula see original document page 9</formula>本发明还可以包括
1、 在步骤1中所述的载体的初始位置由GPS装置或者是外部的高精度组合
导航设备提供，所述载体的初始速度值由DVL多普勒计程仪或者是外部的高精 度组合导航设备提供。
2、 在步骤2中所述的载体相对导航坐标系的初始姿态确定分为两种情形
当载体处于静基座时，采用基于古典控制理论的自对准方法；当载体处于动基座
时，采用基于卡尔曼滤波理论的组合对准方法。
3、在步骤4中所述的旋转矢量增量/^的计算具体步骤如下 步骤41: 由光纤陀螺输出的输出计算A-中的惯性可测项，即角增量a;由
姿态更新周期和光纤陀螺采样周期的关系= 7W2;每个姿态更新周期内己知
W + 1个光纤陀螺采样角速度叫,^，L 则姿态更新周期i/内的角增量"为
<formula>formula see original document page 10</formula>其中<formula>formula see original document page 10</formula>步骤42:由光纤陀螺输出的输出计算A^中的非惯性可测项，即圆锥补偿项 々；由姿态更新周期和光纤陀螺采样周期的关系// = 7^;每个姿态更新周期// 内已知JV + 1个光纤陀螺采样角速度叫,^,L ；
在姿态更新周期H内，圆锥补偿项々由光纤陀螺采样角速度叉乘项的线性 组合来计算
其中《^为优化系数，在典型圆锥环境中确定；
步骤43:将步骤41中得到的姿态更新周期/7内角增量cr，与步骤41中得 到的姿态更新周期//内圆锥补偿项々相加，得到姿态更新周期7/内旋转矢量增
设置A^=3，陀螺在/时刻采样角速度w。，并且每个姿态更新周期//，在 /+2f//3、 ^//时刻采样q、 ％、 得到姿态更新周期if内旋转矢量增量A-为
<formula>formula see original document page 10</formula>
设置A^2，陀螺在,时刻采样角速度叫，并且每个计算周期/f内，在/+///入200710144846.2 说明书第5/8页
+//时刻采样两次《,、 ％;得到姿态更新周期//内旋转矢量增量厶^为
<formula>formula see original document page 11</formula>
其中
<formula>formula see original document page 11</formula>
本发明与传统方法相比较所的优点主要体现在
(1) 针对光纤陀螺输出为角速度的情形，不同于传统测量方法以角增量作 为输入，发明方法直接以角速度输入，以角速度的叉乘项来拟和圆锥补偿项。避 免了使用光纤陀螺时，传统方法对于划船补偿项的测量偏差。
(2) 相对于提高捷联惯性导航系统姿态计算精度的另一种途径选用高性 能的光纤陀螺。发明的测量方法只需要作导航软件设计，不需要增加捷联惯性导 航系统的制造成本。
(四)
图1为本发明的适合光纤陀螺捷联惯性导航系统的姿态测量方法流程图。 图2为典型圆锥环境中，发明的姿态测量方法；传统的姿态测量方法的实验
曲线。典型圆锥环境定义为载体的两个正交轴上存在同频率，相位差90度的
角振动。光纤陀螺的采样频率为100Hz。
(五)
具体实施例方式
下面结合附图举例对本发明做更详细地描述
结合图1，本发明的适合于光纤陀螺捷联惯性导航系统的姿态测量方法包括 如下步骤
步骤1、通过外部设备确定载体的初始位置参数与初始速度值。
步骤2、光纤陀螺捷联惯性导航系统进行初始对准，确定载体相对导航坐标
系的初始姿态，得到姿态四元数的初始值。
步骤3、确定姿态更新周期/f-^-、—,，所述姿态更新周期i/等于W倍的
光纤陀螺采样周期/2 所述iV为大于零的整数。
步骤4、采集光纤陀螺输出的载体相对于惯性坐标系的角速度计算旋转矢量 的增量A^。
步骤5、由旋转矢量与四元数的关系，得到姿态更新周期Z/内姿态更新四元数剩。<formula>formula see original document page 12</formula>2
(1)
其中p为旋转矢量增量的模。
步骤6、由姿态四元数更新方程更新姿态四元数
抓) 剩 (2)
其中姿态更新四元数《(//)由步骤5求得。、 2d)分别表示载体在^ 、 ^
时刻的姿态四元数。
步骤7、利用步骤6获得的^时刻姿态四元数2^J二[《。& &《3:^计算 载体坐标系b系相对于导航坐标系n系的捷联矩阵r。
2(她+顿3) 《o212 +《22 + 2(秘-錢)
2(《《3—&《2)2(《2《3+《0仏) 《。2—《2—《22 +《32
(3)
步骤8、利用步骤7获得的捷联矩阵r求得载体相对导航坐标系的姿态角。
导航坐标系选取为游动方位坐标系，则载体的格网航向角^、纵摇角^、横摇角
^都可由捷联矩阵r中的元素来表示。
<formula>formula see original document page 12</formula>
(4)
其中格网航向角^的定义域为(0°,360°);纵摇角^的定义域为(-卯。,9(0;横摇 角y的定义域为(-90°，90°)。
由姿态角的定义域可以确定它们的真值。格网航向角的真值为<formula>formula see original document page 12</formula>纵摇角的真值为
<formula>formula see original document page 13</formula>
横摇角的真值为
<formula>formula see original document page 13</formula>
在步骤1中所述的载体的初始位置由GPS装置或者是外部的高精度组合导 航设备提供，所述载体的初始速度值由DVL多普勒计程仪或者是外部的高精度 组合导航设备提供。
在步骤2中所述的载体相对导航坐标系的初始姿态确定分为两种情形当载 体处于静基座时，可以采用基于古典控制理论的自对准方法；当载体处于动基座 时，可以采用基于卡尔曼滤波理论的组合对准方法。
在步骤4中所述的旋转矢量增量A^的计算具体步骤如下
步骤41:由光纤陀螺输出的输出计算A^中的惯性可测项，即角增量a。由 姿态更新周期和光纤陀螺采样周期的关系= 。每个姿态更新周期内已知 W + l个光纤陀螺采样角速度cv^,L c^。则姿态更新周期if内的角增量a为
<formula>formula see original document page 13</formula>(5)
其中
步骤42:由光纤陀螺输出的输出计算A-中的非惯性可测项，即圆锥补偿项 々。由姿态更新周期和光纤陀螺采样周期的关系// = ^//。每个姿态更新周期// 内已知W + l个光纤陀螺采样角速度w。，^，L cv。
在姿态更新周期//内，圆锥补偿项/ 可以由光纤陀螺采样角速度叉乘项的 线性组合来计算
<formula>formula see original document page 13</formula> (7) 其中/^_,为优化系数，可以在典型圆锥环境中确定。
步骤43:将步骤41中得到的姿态更新周期/Z内角增量a，与步骤41中得 到的姿态更新周期内圆锥补偿项-相加，得到姿态更新周期内旋转矢量增
A—a +々 (8) 设置W=3，陀螺在/时刻采样角速度《。，并且每个姿态更新周期//，在/+///3、 r+2f// 、 /+//时刻采样^、 w2、 《3。得到姿态更新周期//内旋转矢量增量厶^为
<formula>formula see original document page 14</formula>
.叫x叫+—叫x叫+ 2240 Q3 56 13 2240
<formula>formula see original document page 14</formula>
(9)
其中
设置A^2，陀螺在Z时刻采样角速度叫，并且每个计算周期/f内，在/+///2、 ,+//时刻采样两次^、 《2。得到姿态更新周期/Z内旋转矢量增量A^为
其中
<formula>formula see original document page 14</formula>(10)
在姿态更新周期//内，姿态测量方法所使用的光纤陀螺采样数越多，姿态 测量方法应对圆锥补偿的效果就越好，光纤陀螺捷联惯性导航系统输出的姿态精 度就越高。
权利要求
1、一种适合于光纤陀螺的载体姿态测量方法，其特征是步骤1、通过外部设备确定载体的初始位置参数与初始速度值；步骤2、光纤陀螺捷联惯性导航系统进行初始对准，确定载体相对导航坐标系的初始姿态，得到姿态四元数的初始值；步骤3、确定姿态更新周期H＝tm-tm-1，所述姿态更新周期H等于N倍的光纤陀螺采样周期h＝tl-tl-1，所述N为大于零的整数；步骤4、采集光纤陀螺输出的载体相对于惯性坐标系的角速度计算旋转矢量的增量Δφ；步骤5、通过旋转矢量与四元数的关系，得到姿态更新周期H内姿态更新四元数q(H)，id="icf0001" file="S2007101448462C00011.gif" wi="32" he="20" top="5" left = "5" img-content="drawing" img-format="tif" orientation="portrait" inline="no"/>其中为旋转矢量增量的模，id="icf0002" file="S2007101448462C00012.gif" wi="52" he="6" top="5" left = "5" img-content="drawing" img-format="tif" orientation="portrait" inline="no"/>步骤6、由姿态四元数更新方程更新姿态四元数其中姿态更新四元数q(H)由步骤5得到，Q(tm)、Q(tm-1)分别表示载体在tm、tm-1时刻的姿态四元数；步骤7、利用步骤6获得的tm时刻姿态四元数Q(tm)＝[q0q1q2q3]T计算载体坐标系b系相对于导航坐标系n系的捷联矩阵T，
2、 根据权利要求1所述的适合于光纤陀螺的载体姿态测量方法，其特征是在步骤l中所述的载体的初始位置由GPS装置或者是外部的高精度组合导航设 备提供，所述载体的初始速度值由DVL多普勒计程仪或者是外部的高精度组合 导航设备提供。
3、 根据权利要求1或2所述的适合于光纤陀螺的载体姿态测量方法，其特征是在步骤2中所述的载体相对导航坐标系的初始姿态确定分为两种情形当载体处于静基座时，采用基于古典控制理论的自对准方法；当载体处于动基 座时，采用基于卡尔曼滤波理论的组合对准方法。
4、 根据权利要求1或2所述的适合于光纤陀螺的载体姿态测量方法，其特征是在步骤4中所述的旋转矢量增量A^的计算具体步骤如下步骤41:由光纤陀螺输出的输出计算 中的惯性可测项，即角增量a; 由姿态更新周期和光纤陀螺采样周期的关系// = 7^;每个姿态更新周期内 己知^ + 1个光纤陀螺采样角速度^,0;1山fi^:则姿态更新周期i/内的角增量cr 为其中C>4^，-綠,) ；步骤42:由光纤陀螺输出的输出计算A^中的非惯性可测项，即圆锥补偿 项"；由姿态更新周期和光纤阵螺采样周期的关系// = 7^;每个姿态更新周期 Z/内已知iV + l个光纤陀螺采样角速度叫,q，L在姿态更新周期内，圆锥补偿项々由光纤陀螺采样角速度叉乘项的线性 组合来计算AM其中i^—,为优化系数，在典型圆锥环境中确定；步骤43:将步骤41中得到的姿态更新周期/Z内角增量a，与步骤41中得 到的姿态更新周期内圆锥补偿项"相加，得到姿态更新周期//内旋转矢量增 量 ；设置A^3，陀螺在^时刻采样角速度w。，并且每个姿态更新周期//，在r+W/J、 汁2/// 、 /+//时刻采样^、 《2、 w3;得到姿态更新周期7/内旋转矢量增量A^ 为<formula>formula see original document page 4</formula>其中<formula>formula see original document page 4</formula>设置iV=2，陀螺在/时刻采样角速度cy。，并且每个计算周期//内，在/+//时刻采样两次^,、份2;得到姿态更新周期/f内旋转矢量增量A-为<formula>formula see original document page 5</formula> 其中<formula>formula see original document page 5</formula>6
5、根据权利要求3所述的适合于光纤陀螺的载体姿态测量方法，其特征是: 在步骤4中所述的旋转矢量增量A^的计算具体步骤如下步骤41:由光纤陀螺输出的输出计算A^中的惯性可测项，即角增量"； 由姿态更新周期和光纤陀螺采样周期的关系/Z-iW/;每个姿态更新周期//内 已知7^ + 1个光纤陀螺采样角速度0。,^,山6^;则姿态更新周期Z/内的角增量a 为<formula>formula see original document page 5</formula>其中<formula>formula see original document page 5</formula>步骤42:由光纤陀螺输出的输出计算A-中的非惯性可测项，即圆锥补偿 项/ ;由姿态更新周期和光纤陀螺采样周期的关系/Z-A^;每个姿态更新周期 a内已知7V + I个光纤陀螺采样角速度叫，w,，L c^;在姿态更新周期//内，圆锥补偿项-由光纤陀螺采样角速度叉乘项的线性 组合来计算<formula>formula see original document page 5</formula>其中i^—,为优化系数，在典型圆锥环境中确定；步骤43:将步骤41中得到的姿态更新周期/Z内角增量"，与步骤41中得 到的姿态更新周期//内圆锥补偿项-相加，得到姿态更新周期H内旋转矢量增<formula>formula see original document page 5</formula>设置W=3,陀螺在/时刻采样角速度0。，并且每个姿态更新周期//，在 /+2///3、 /+//时刻采样6>1、 w2、 《3;得到姿态更新周期i/内旋转矢量增量A^ 为其中<formula>formula see original document page 2</formula>设置7V=2，陀螺在Z时刻采样角速度叫，并且每个计算周期W内，在 Z+i/时刻采样两次o;,、 w2;得到姿态更新周期i/内旋转矢量增量A^为<formula>formula see original document page 2</formula>其中<formula>formula see original document page 2</formula>
全文摘要
本发明提供的是一种适合于光纤陀螺的载体姿态测量方法。包括通过外部设备确定载体的初始位置参数与初始速度值；光纤陀螺捷联惯性导航系统进行初始对准，确定载体相对导航坐标系的初始姿态，得到姿态四元数的初始值；确定姿态更新周期H＝t<sub>m</sub>-t<sub>m-1</sub>；采集光纤陀螺输出的载体相对于惯性坐标系的角速度计算旋转矢量的增量Δφ；通过旋转矢量与四元数的关系，得到姿态更新周期H内姿态更新四元数q(H)；由姿态四元数更新方程更新姿态四元数；计算载体坐标系b系相对于导航坐标系n系的捷联矩阵T；求载体相对导航坐标系的姿态角等步骤。本发明解决了在载体高动态环境或是高频率振动环境中，圆锥效应对于载体姿态测量精度产生影响的问题。
文档编号G01C21/18GK101187561SQ20071014484
公开日2008年5月28日 申请日期2007年12月18日 优先权日2007年12月18日
发明者强 于, 磊 吴, 周广涛, 奔粤阳, 枫 孙, 博 徐, 程建华, 陈世同, 伟 高, 高洪涛 申请人:哈尔滨工程大学