CN104482942B - 一种基于惯性系的最优两位置对准方法 - Google Patents
一种基于惯性系的最优两位置对准方法 Download PDFInfo
- Publication number
- CN104482942B CN104482942B CN201410765061.7A CN201410765061A CN104482942B CN 104482942 B CN104482942 B CN 104482942B CN 201410765061 A CN201410765061 A CN 201410765061A CN 104482942 B CN104482942 B CN 104482942B
- Authority
- CN
- China
- Prior art keywords
- inertial system
- inertial
- state
- equation
- sins
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Abstract
本发明公开了一种基于惯性系的最优两位置对准方法。包括以下几个步骤:以速度误差、失准角、陀螺常值漂移和加速度零偏为状态量,建立状态方程;以惯性系下SINS结算的速度和GPS测得的惯性系下的速度的差值作为量测量,建立量测方程;将状态方程和量测方程构成两位置对准的卡尔曼滤波模型;根据得到的卡尔曼滤波模型,对状态量进行估计;在设定的对准时间,分别将IMU绕纵摇轴、横摇轴和航向轴旋转180°;再利用卡尔曼滤波器估计出的失准角精确估计值对转换矩阵进行修正,得到载体系到惯性系的转换矩阵结合惯性系到导航系的转换矩阵求解载体系到惯性系的转换矩阵实现基于惯性系的两位置对准。本发明提高了初始对准的精度。
Description
技术领域
本发明属于一种捷联惯导系统初始对准方法,尤其涉及针对两位置的一种基于惯性系的最优两位置对准方法。
背景技术
捷联惯导系统(strapdown inertial navigation system,SINS)以其特有的优点,广泛应用在飞机、舰船、导弹等装备上。初始对准是SINS关键技术之一,对准的精度直接影响到其工作精度。目前,对于舰船SINS而言,在没有外观测信息的情况下,其对准方法以解析式对准和罗经对准为主;在有外观测信息的情况下,以卡尔曼滤波对准为主;并且这些方法都是基于地理坐标系。
随着初始对准技术的发展,惯性坐标系对准(包括粗对准和精对准)以其特点成为初始对准的一个新的研究方向。在理论研究方面,认为惯性系对准可以有效的隔离载体角速度和线运动干扰的影响;研究表明,将重力加速度放在惯性系内分析,并结合外观测信息进行卡尔曼滤波对准,其航向的对准精度要优于在地理系下的对准精度。
发明内容
本发明的目的是提供一种具有高初始对准精度的,基于惯性系的最优两位置对准方法。
本发明是通过以下技术方案实现的:
一种基于惯性系的最优两位置对准方法,包括以下几个步骤:
步骤一:以速度误差、失准角、陀螺常值漂移和加速度零偏为状态量,建立状态方程;以惯性系下SINS结算的速度和GPS测得的惯性系下的速度的差值作为量测量,建立量测方程;将状态方程和量测方程构成两位置对准的卡尔曼滤波模型;
步骤二:根据得到的卡尔曼滤波模型,对状态量进行估计;
步骤三:在设定的对准时间,分别将IMU绕纵摇轴、横摇轴和航向轴旋转180°,使SINS系统为完全可观测;
步骤四:再利用卡尔曼滤波器估计出的失准角精确估计值对当前时刻载体系到计算惯性系的转换矩阵进行修正,得到载体系到惯性系的转换矩阵
步骤五:利用得到的转换矩阵结合惯性系到导航系的转换矩阵求解载体系到惯性系的转换矩阵实现基于惯性系的两位置对准。
本发明一种基于惯性系的最优两位置对准方法,还可以包括:
状态方程为:
其中X(t)为t时刻捷联惯导系统的状态变量,A(t)和B(t)分别为捷联惯导系统的状态矩阵和噪声驱动矩阵,W(t)为捷联惯导系统噪声序列,
以速度误差、失准角、陀螺常值漂移和加速度零偏为状态变量:
其中,失准角为:
为惯性坐标系相对于理想坐标系的失准角,为t时刻载体坐标系到惯性坐标系的真实方向余弦阵,为导航解算得到的姿态阵,为Gauss白噪声,陀螺漂移εb包括陀螺常值漂移和陀螺随机漂移
速度误差为:
式中δVi为计算速度与GPS测得速度的差值,即δVi=Vi′-Vi,gi为地球重力加速度在惯性系的投影,为Gauss白噪声,加速度计偏差▽b由加速度计零位误差和随机偏差构成;
以惯性系下SINS结算的速度和GPS测得的惯性系下的速度的差值作为量测量,建立量测方程:
式中Z(t)为t时刻的观测值,ηw(t)为观测噪声序列,H(t)为观测阵,H(t)=[I3×303×3 03×3 03×3]。
有益效果:
惯性系下最优两位置对准是在第一次滤波达到稳定后,通过将IMU绕航向轴旋转来实现的,本发明可以使原来不完全可观测的系统变得完全可观测,从而有效的提高了初始对准的精度。
附图说明
图1为本发明惯性系下卡尔曼滤波对准的流程图。
图2为本发明惯性系下固定位置和两位置对准的横摇误差角的仿真曲线。
图3为本发明惯性系下固定位置和两位置对准的纵摇误差角的仿真曲线。
图4为本发明惯性系下固定位置和两位置对准的航向误差角的仿真曲线。
具体实施方式
下面将结合附图对本发明做进一步详细说明。
如图1所示,本发明包括以下几个步骤
步骤1:以速度误差、失准角、陀螺常值漂移和加速度零偏为状态量,建立状态方程;以惯性系下SINS解算的速度和GPS测得的惯性系下的速度的差值作为量测量,建立量测方程;状态方程和量测方程构成两位置对准的卡尔曼滤波模型;
(1)失准角方程
式中为惯性坐标系相对于理想坐标系的失准角;为t时刻载体坐标系到惯性坐标系的真实方向余弦阵;为导航解算得到的姿态阵;假设为Gauss白噪声,陀螺漂移εb包括陀螺常值漂移和陀螺随机漂移
(2)速度误差方程
式中δVi为计算速度与GPS测得速度的差值,即δVi=Vi′-Vi;gi为地球重力加速度在惯性系的投影;假设为Gauss白噪声,加速度计偏差▽b由加速度计零位误差和随机偏差构成。
式(1)、(2)便构成惯性系下捷联惯导系统静基座的误差方程。
(3)状态方程
根据惯性系下SINS静基座的失准角方程和速度误差方程,便可建立对应的状态空间模型。
状态方程的形式为:
式中X(t)为t时刻系统的状态变量;A(t)和B(t)分别为系统的状态矩阵和噪声驱动矩阵;W(t)为系统噪声序列。
根据误差模型,将陀螺常值漂移和加速度计常值偏差均扩充到状态变量中,构建状态变量:
状态矩阵A(t)为:
其中
A2(t)中Cks为中对应的各元素,A1(t)中,
噪声驱动阵B(t)为:
其中
噪声序列W(t)为:
其中为载体坐标系的陀螺随机漂移,为Gauss白噪声过程;为载体坐标系的加速度计随机偏差,为Gauss白噪声过程。
(4)量测方程
取惯性系下计算地速Vi′和理想地速Vi的差值作为观测量,其中Vi′为惯性系下SINS解算的载体速度,Vi为GPS测得的载体在惯性系下的速度,可得惯性系下卡尔曼滤波的观测方程为:
式中Z(t)为t时刻的观测值;ηw(t)为观测噪声序列;H(t)为观测阵,其具体表达式为:
H(t)=[I3×3 03×3 03×3 03×3]。
步骤2:利用步骤1得到的卡尔曼滤波模型,根据卡尔曼滤波基本方程对状态量进行估计;
(1)对连续的滤波模型进行离散化处理,可得离散卡尔曼滤波器的状态方程和量测方程:
式中Xk为k时刻状态量的估计值;Φk,k-1表示k-1时刻至k时刻的一步转移矩阵;Γk-1表示k-1时刻的系统噪声驱动阵;Wk-1表示k-1时刻的系统激励噪声序列;Hk表示k时刻的量测阵;为k时刻的量测噪声序列。
(2)设k时刻的量测值为Zk,根据卡尔曼滤波基本方程,状态Xk的估计可按下述过程求解:
状态一步预测:
状态估计:
滤波增益:
一步预测均方差:
估计均方误差:
式中Kk为k时刻的增益矩阵;Pk,k-1表示k-1时刻至k时刻的一步预测估计方差阵;Pk-1表示k-1时刻的状态估计方差;Qk为系统噪声序列的方差阵,非负定;Rk为量测噪声序列的方差阵,正定。
步骤3:在设定的对准时间达到中间某一时段时(第一次达到稳定后),分别将IMU绕纵摇轴、横摇轴和航向轴旋转180°,检验系统的可观测性;
惯性系下PWCS(piece-wise constant system)可观测性定理:如果其中1≤j≤r,则
Rank(U)=Rank(US)
其中,Aj为第j个时间段的状态矩阵,U为TOM(total observability matrix),US为SOM(stripped observability matrix)。
由于则对Uj进行矩阵的行初等变换,可以得到简化的Uj如下:
对于两位置对准,取惯性系误差模型中(Aj,H)对,定义U(α1,α2)为TOM,α1,α2分别表示第一个位置和第二个位置的欧拉角。同理,根据定理可以利用简化的代替TOMU(α1,α2)进行两位置对准可观测性分析,这里
式中,当欧拉角为αj时,可由式(5)确定。
对准过程中,滤波器第一次达到稳定后分别将IMU绕纵摇轴、横摇轴和航向轴旋转180°有如下结论:
式中,θ,φ,ψ分别表示纵摇角、横摇角和航向角。
可见,而对于两位置对准,无论绕三个轴中哪个轴旋转,系统的12个状态量都可以变得完全可观测;显然,两位置对准提高了系统的可观测性。
步骤4:再利用卡尔曼滤波模型进行估计,利用得到的失准角的精确估计值对进行修正,得到较精确的
用卡尔曼滤波器估计出的失准角为在小角度情况下可得用失准角表示的误差矩阵:
式中为失准角的估计值在惯性系x、y、z三轴上的投影。
对进行修正,即:
式中为精对结束时刻载体系b至计算惯性系i′的转换矩阵,通过陀螺仪的输出更新计算得到。
步骤5:利用以上步骤4得到的结合求解捷联矩阵的初始值实现基于惯性系的两位置对准。
本发明的有益效果通过如下方法得以验证:
通过Matlab仿真,陀螺和加速度计的输出信号由SINS模拟器产生。仿真条件设置如下:
陀螺和加速度计的参数如下:
陀螺的常值漂移为:εbj=0.01°/h,其中(j=x,y,z);
陀螺的角随机游走系数为:其中(j=x,y,z);
加速度计的常值偏置为:▽bj=1×10-4g,其中(j=x,y,z);
加速度计测量白噪声为:其中(j=x,y,z)。
(2)卡尔曼滤波器中P、Q、R阵初始取值如下:
P(0)=diag{(0.1)2,(0.1)2,(0.1)2,(0.0067°)2,(0.0067°)2,(0.0087°)2,(0.01°/h)2,(0.01°/h)2,(0.01°/h)2,(10-4g)2,(10-4g)2,(10-4g)2}
Q(0)=diag{(10-5g)2,(10-5g)2,(10-5g)2,(0.001°/h)2,(0.001°/h)2,(0.001°/h)2,0,0,0,0,0,0}
R(0)=diag{(0.01)2,(0.01)2,(0.01)2}
(3)设置粗对准结束后水平失准角为0.1°,方位失准角为0.5°;初始的经度126°,纬度45°。
(4)仿真时间1200s,对准进行到500s时旋转对应的轴180°,旋转时间10s。
仿真结果:从图2至图4可知,固定位置对准结束时横摇误差角为0.0071°、纵摇误差角为-0.0073°、航向误差角为-0.077°;最优两位置对准结束时横摇误差角为0.00039°、纵摇误差角为-0.0002°、航向误差角为-0.0013°。可见对准的精度得到了明显的提高:横摇角和纵摇角的对准精度分别提高了18倍和35倍,航向角的对准精度提高了60倍。
Claims (2)
1.一种基于惯性系的最优两位置对准方法,其特征在于,包括以下几个步骤:
步骤一:以速度误差、失准角、陀螺常值漂移和加速度零偏为状态量,建立状态方程;以惯性系下SINS的速度和GPS测得的惯性系下的速度的差值作为量测量,建立量测方程;将状态方程和量测方程构成两位置对准的卡尔曼滤波模型;
步骤二:根据得到的卡尔曼滤波模型,对状态量进行估计;
步骤三:在设定的对准时间,分别将IMU绕纵摇轴、横摇轴和航向轴旋转180°,使SINS系统为完全可观测;
步骤四:再利用卡尔曼滤波器估计出的失准角精确估计值对当前时刻载体系到计算惯性系的转换矩阵进行修正,得到载体系到惯性系的转换矩阵
步骤五:利用得到的转换矩阵结合惯性系到导航系的转换矩阵求解载体系到导航系的转换矩阵实现基于惯性系的两位置对准。
2.根据权利要求1所述的一种基于惯性系的最优两位置对准方法,其特征在于:所述的状态方程为:
其中X(t)为t时刻捷联惯导系统的状态变量,A(t)和B(t)分别为捷联惯导系统的状态矩阵和噪声驱动矩阵,W(t)为捷联惯导系统噪声序列,
以速度误差、失准角、陀螺常值漂移和加速度零偏为状态变量:
其中,失准角为:
为惯性坐标系相对于理想坐标系的失准角,为t时刻载体坐标系到惯性坐标系的真实方向余弦阵,为导航解算得到的姿态阵,陀螺漂移εb包括陀螺常值漂移和陀螺随机漂移
速度误差为:
式中δVi为计算速度与GPS测得速度的差值,即δVi=Vi′-Vi,gi为地球重力加速度在惯性系的投影,加速度计偏差由加速度计零位误差和随机偏差构成;
以惯性系下SINS计算的速度和GPS测得的惯性系下的速度的差值作为量测量,建立量测方程:
式中Z(t)为t时刻的观测值,ηw(t)为观测噪声序列,H(t)为观测阵,H(t)=[I3×3 03×303×3 03×3]。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410765061.7A CN104482942B (zh) | 2014-12-11 | 2014-12-11 | 一种基于惯性系的最优两位置对准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410765061.7A CN104482942B (zh) | 2014-12-11 | 2014-12-11 | 一种基于惯性系的最优两位置对准方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104482942A CN104482942A (zh) | 2015-04-01 |
CN104482942B true CN104482942B (zh) | 2017-06-20 |
Family
ID=52757512
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410765061.7A Active CN104482942B (zh) | 2014-12-11 | 2014-12-11 | 一种基于惯性系的最优两位置对准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104482942B (zh) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106989761B (zh) * | 2017-05-25 | 2019-12-03 | 北京航天自动控制研究所 | 一种基于自适应滤波的空间飞行器制导工具在轨标定方法 |
CN107270938B (zh) * | 2017-06-13 | 2020-07-03 | 西北工业大学 | 基于泰勒级数拟合的单轴旋转惯导系统姿态解调方法 |
CN111024071A (zh) * | 2019-12-25 | 2020-04-17 | 东南大学 | Gnss辅助的加速度计和陀螺仪常值漂移估算的导航方法及系统 |
CN111351508B (zh) * | 2020-04-22 | 2023-10-03 | 中北大学 | 一种mems惯性测量单元系统级批量标定方法 |
CN111982151B (zh) * | 2020-07-17 | 2022-07-22 | 中科长城海洋信息系统有限公司 | 一种光纤捷联惯导系统的自标定方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2001153680A (ja) * | 1999-11-26 | 2001-06-08 | Japan Aviation Electronics Industry Ltd | 慣性装置 |
CN103453917A (zh) * | 2013-09-04 | 2013-12-18 | 哈尔滨工程大学 | 一种双轴旋转式捷联惯导系统初始对准与自标校方法 |
CN103471616A (zh) * | 2013-09-04 | 2013-12-25 | 哈尔滨工程大学 | 一种动基座sins大方位失准角条件下初始对准方法 |
CN103557864A (zh) * | 2013-10-31 | 2014-02-05 | 哈尔滨工程大学 | Mems捷联惯导自适应sckf滤波的初始对准方法 |
CN103791918A (zh) * | 2014-02-10 | 2014-05-14 | 哈尔滨工程大学 | 一种舰船捷联惯导系统极区动基座对准方法 |
CN103900611A (zh) * | 2014-03-28 | 2014-07-02 | 北京航空航天大学 | 一种惯导天文高精度复合两位置对准及误差标定方法 |
-
2014
- 2014-12-11 CN CN201410765061.7A patent/CN104482942B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2001153680A (ja) * | 1999-11-26 | 2001-06-08 | Japan Aviation Electronics Industry Ltd | 慣性装置 |
CN103453917A (zh) * | 2013-09-04 | 2013-12-18 | 哈尔滨工程大学 | 一种双轴旋转式捷联惯导系统初始对准与自标校方法 |
CN103471616A (zh) * | 2013-09-04 | 2013-12-25 | 哈尔滨工程大学 | 一种动基座sins大方位失准角条件下初始对准方法 |
CN103557864A (zh) * | 2013-10-31 | 2014-02-05 | 哈尔滨工程大学 | Mems捷联惯导自适应sckf滤波的初始对准方法 |
CN103791918A (zh) * | 2014-02-10 | 2014-05-14 | 哈尔滨工程大学 | 一种舰船捷联惯导系统极区动基座对准方法 |
CN103900611A (zh) * | 2014-03-28 | 2014-07-02 | 北京航空航天大学 | 一种惯导天文高精度复合两位置对准及误差标定方法 |
Non-Patent Citations (4)
Title |
---|
基于PWCS理论的单轴旋转惯导系统初始对准的可观测性分析;钟斌等;《海军工程大学学报》;20121231;第24卷(第6期);11-15 * |
旋转式捷联惯导系统精对准方法;关劲等;《中国惯性技术学报》;20100831;第18卷(第4期);396-400 * |
船用光纤陀螺航姿系统动态对准;蔡晓佳等;《中国惯性技术学报》;20070430;第15卷(第2期);143-146 * |
船用捷联惯性导航系统在系泊状态下快速初始对准与标定;何昆鹏等;《哈尔滨工程大学学报》;20080930;第29卷(第9期);944-950 * |
Also Published As
Publication number | Publication date |
---|---|
CN104482942A (zh) | 2015-04-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104655131B (zh) | 基于istssrckf的惯性导航初始对准方法 | |
CN104482942B (zh) | 一种基于惯性系的最优两位置对准方法 | |
CN103575299B (zh) | 利用外观测信息的双轴旋转惯导系统对准及误差修正方法 | |
CN103759742B (zh) | 基于模糊自适应控制技术的捷联惯导非线性对准方法 | |
CN101514900B (zh) | 一种单轴旋转的捷联惯导系统初始对准方法 | |
CN105698822B (zh) | 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法 | |
CN104374388B (zh) | 一种基于偏振光传感器的航姿测定方法 | |
CN102706366B (zh) | 一种基于地球自转角速率约束的sins初始对准方法 | |
CN103727941B (zh) | 基于载体系速度匹配的容积卡尔曼非线性组合导航方法 | |
CN104344837B (zh) | 一种基于速度观测的冗余惯导系统加速度计系统级标定方法 | |
CN106153073B (zh) | 一种全姿态捷联惯导系统的非线性初始对准方法 | |
CN105371844B (zh) | 一种基于惯性/天文互助的惯性导航系统初始化方法 | |
CN105806363B (zh) | 基于srqkf的sins/dvl水下大失准角对准方法 | |
CN108318038A (zh) | 一种四元数高斯粒子滤波移动机器人姿态解算方法 | |
CN104019828A (zh) | 高动态环境下惯性导航系统杆臂效应误差在线标定方法 | |
CN106595711A (zh) | 一种基于递推四元数的捷联惯性导航系统粗对准方法 | |
CN104697526A (zh) | 用于农业机械的捷联惯导系统以及控制方法 | |
CN105136166B (zh) | 一种指定惯导位置精度的捷联惯导系统误差模型仿真方法 | |
CN107390247A (zh) | 一种导航方法、系统及导航终端 | |
CN103727940B (zh) | 基于重力加速度矢量匹配的非线性初始对准方法 | |
CN105737823A (zh) | 基于五阶ckf的gps/sins/cns组合导航方法 | |
CN103674064B (zh) | 捷联惯性导航系统的初始标定方法 | |
CN103245359A (zh) | 一种惯性导航系统中惯性传感器固定误差实时标定方法 | |
CN101915579A (zh) | 一种基于ckf的sins大失准角初始对准新方法 | |
CN109059914B (zh) | 一种基于gps和最小二乘滤波的炮弹滚转角估计方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |