CN108692727A - 一种带有非线性补偿滤波器的捷联惯导系统 - Google Patents

一种带有非线性补偿滤波器的捷联惯导系统 Download PDF

Info

Publication number
CN108692727A
CN108692727A CN201810350847.0A CN201810350847A CN108692727A CN 108692727 A CN108692727 A CN 108692727A CN 201810350847 A CN201810350847 A CN 201810350847A CN 108692727 A CN108692727 A CN 108692727A
Authority
CN
China
Prior art keywords
matrix
attitude
nonlinear compensation
angle
compensation filter
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.)
Granted
Application number
CN201810350847.0A
Other languages
English (en)
Other versions
CN108692727B (zh
Inventor
富立
刘永永
王玲玲
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Publication of CN108692727A publication Critical patent/CN108692727A/zh
Application granted granted Critical
Publication of CN108692727B publication Critical patent/CN108692727B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种带有非线性补偿滤波器的捷联惯导系统，是在传统捷联惯导系统中增加了非线性补偿滤波器。非线性补偿滤波器一方面接收加速度计组合输出的比力信息另一方面接收姿态阵解算模块输出的姿态矩阵第三方面是将比力信息与姿态矩阵进行姿态非线性补偿，得到补偿后姿态矩阵所述补偿后姿态矩阵输出给比力坐标变换模块。本发明是将加速度计输出作为约束，对角速率解算得到的姿态矩阵进行非线性补偿滤波，抑制了姿态的发散，提高姿态精度。

Description

一种带有非线性补偿滤波器的捷联惯导系统
技术领域
本发明涉及一种非线性补偿滤波器，更确切地说，是一种基于加速度计输出能提高捷联惯导系统姿态精度的非线性无源补偿滤波器。
背景技术
捷联惯导系统(SINS，strapdown inertial navigation system)是在平台式惯导系统基础上发展而来的。它是一种无框架系统，由三个速率陀螺、三个线加速度计和微型导航计算机组成。捷联式惯导系统原理简图(在本申请中称为图1)请参考科学出版社于2006年5月第一版，作者秦永元，《惯性导航》第287页。
制约传统的SINS姿态精度主要有以下两个因素：一是惯性元件(包括加速度计和陀螺仪)本身的精度；二是导航姿态解算的算法精度。对于惯性元件而言，可靠性、可维修性和成本依然制约着惯性元件精度的提高，因此，惯性元件本身精度的提高是困难的、有限的。而提高姿态算法精度成本低，但是实现难度大，这主要是由姿态算法本身的成熟以及复杂性决定的。
发明内容
本发明的目的是设计一种能提高捷联惯性导航系统姿态精度的非线性补偿滤波器。传统的SINS姿态解算是利用陀螺输出进行姿态矩阵解算，进而进行姿态、航向解算，而忽略了加速度计输出对于姿态的反映。本发明是将加速度计输出作为约束，对角速率解算得到的姿态矩阵进行非线性补偿滤波，抑制了姿态的发散，提高姿态精度。
本发明是一种带有非线性补偿滤波器的捷联惯导系统，其特征在于：非线性补偿滤波器PCF设置在传统SINS的姿态阵解算模块与比力坐标变换模块之间；
非线性补偿滤波器一方面接收加速度计组合输出的比力信息另一方面接收姿态阵解算模块输出的姿态矩阵第三方面是将比力信息与姿态矩阵进行姿态非线性补偿，得到补偿后姿态矩阵所述补偿后姿态矩阵输出给比力坐标变换模块。
在本发明中，非线性补偿滤波器PCF进行的非线性补偿处理步骤为：
步骤一，构造测量矩阵C测量；
根据加速度计输出的X轴的比力Y轴的比力和Z轴的比力计算姿态角的约束和θ表示俯仰角，γ表示横滚角，g表示重力加速度；根据姿态角θ和γ构造得到测量姿态矩阵C测量；
sinγ表示横滚角的正弦；sinθ表示姿态角的正弦；sinψ表示航向角的正弦；cosγ表示横滚角的余弦；cosθ表示姿态角的余弦；cosψ表示航向角的余弦；
步骤二，比力对于姿态矩阵的约束；
步骤21，将姿态阵解算模块输出的姿态矩阵作为非线性补偿滤波器中估计矩阵的初值，记为即i＝0；
i为迭代次数；
步骤22，应用与C测量来计算误差估计矩阵且 为估计矩阵的转置；
步骤23，判断的对角线元素否是为1；
是，则在非线性补偿滤波器中将赋值给补偿后姿态矩阵然后将补偿后姿态矩阵输出给比力坐标变换模块；
否，则执行步骤三；
步骤三，非线性滤波补偿；
步骤31，计算残差矢量 为误差估计矩阵的转置；ka为残差矢量增益系数；
步骤32，计算陀螺仪偏差kg为陀螺仪偏差增益系数；
步骤33，计算中间矩阵矢量其中为角速度计输出，的反对称矩阵记为Ai；
步骤34，更新估计矩阵得到更新后的估计矩阵然后将返回步骤二；
为以自然对数e为底、以反对称矩阵Ai为变更的对数函数值；
Δt为惯性设备的输出周期。
经本发明设计得到的应用于提高捷联惯性导航系统姿态解算精度的非线性补偿滤波器的优点在于：
①成本低；本发明是从捷联惯性导航姿态解算结构方面提高系统姿态精度，其成本远远低于一般从硬件方面提高捷联惯导系统姿态精度。
②本发明设计的非线性补偿滤波器易于实现。
③精度明显提高；本发明基于加速度计约束姿态，明显抑制了姿态的发散(即达到姿态收敛)，相比传统姿态解算方法有了明显的精度提升。
附图说明
图1是传统SINS的结构框图。
图2是本发明设计的具有非线性补偿滤波器的SINS的结构框图。
图3是本发明中非线性补偿滤波器的流程图。
图4A是本发明中加入非线性补偿滤波器后的俯仰角结果图。
图4B是本发明中加入非线性补偿滤波器前的俯仰角结果图。
图5A是本发明中加入非线性补偿滤波器后的横滚角结果图。
图5B是本发明中加入非线性补偿滤波器前的横滚角结果图。
具体实施方式
下面将结合附图和实施例对本发明做进一步的详细说明。
参见图2所示，本发明设计了一种带有非线性补偿滤波器的捷联惯导系统(简称为PCF_SINS)，该PCF_SINS能够提高传统SINS的姿态精度。本发明的非线性补偿滤波器PCF设置在传统SINS的姿态阵解算模块与比力坐标变换模块之间。
本发明的非线性补偿滤波器(简称为PCF)一方面接收加速度计组合输出的比力信息另一方面接收姿态阵解算模块输出的姿态矩阵第三方面是将比力信息与姿态矩阵进行姿态非线性补偿，得到补偿后姿态矩阵所述补偿后姿态矩阵输出给比力坐标变换模块。
在本发明中，姿态矩阵请参考科学出版社于2006年5月第一版，作者秦永元，《惯性导航》第297页的公式9.2.40。上角标n表示导航坐标系，下角标b表示载体坐标系。
传统SINS输出有姿态角(θ和γ)和航向角ψ，姿态角包括有俯仰角θ和横滚角γ，ψ表示航向角。
参见图3所示，非线性补偿滤波器PCF进行的非线性补偿处理步骤为：
步骤一，构造测量矩阵C测量；
在本发明中，根据加速度计输出的X轴的比力Y轴的比力和Z轴的比力计算姿态角的约束和θ表示俯仰角，γ表示横滚角，g表示重力加速度；根据姿态角θ和γ构造得到测量姿态矩阵C测量；
sinγ表示横滚角的正弦；
sinθ表示姿态角的正弦；
sinψ表示航向角的正弦；
cosγ表示横滚角的余弦；
cosθ表示姿态角的余弦；
cosψ表示航向角的余弦。
构造的C测量充值利用了加速度计输出的比力信息，而传统SINS中的姿态阵解算模块是不应用比力信息的。在本发明中应用比力信息，相当于在姿态解算中增加了三个约束条件，从而使得解算出的姿态矩阵更加符合实际情况。
步骤二，比力对于姿态矩阵的约束；
步骤21，将姿态阵解算模块输出的姿态矩阵作为非线性补偿滤波器中估计矩阵的初值，记为 中i为迭代次数；
在本发明中，只有首次进行非线性补偿滤波时，才用姿态矩阵作为初始值，继首次后的均用前一次作为当前次的初值。
步骤22，应用与C测量来计算误差估计矩阵且 为估计矩阵的转置；
步骤23，判断的对角线元素否是为1；
是，则在非线性补偿滤波器中将赋值给补偿后姿态矩阵然后将补偿后姿态矩阵输出给比力坐标变换模块；
否，则执行步骤三；
步骤三，非线性滤波补偿；
步骤31，计算残差矢量 为误差估计矩阵的转置；ka为残差矢量增益系数，与所用惯性设备有关。
在本发明中，应用残差矢量克服传统SINS单纯依靠陀螺信息进行姿态解算，有效地抑制了利用姿态矩阵解算出的姿态角(θ和γ)的发散。
步骤32，计算陀螺仪偏差kg为陀螺仪偏差增益系数，与所用惯性设备有关；
步骤33，计算中间矩阵矢量其中为角速度计输出，的反对称矩阵记为Ai；
在本发明中，通过对中间矩阵的引用，有效地对传统SINS中姿态解算时陀螺仪误差进行补偿。
步骤34，更新估计矩阵得到更新后的估计矩阵然后将返回步骤二；
为以自然对数e为底、以反对称矩阵Ai为变更的对数函数值；
Δt为惯性设备的输出周期，本发明中选用0.05s。
实施例1
本发明中定义导航坐标系(n系)与地理坐标系(g系)重合，其xn轴指东，yn轴指北，zn轴指天；机体坐标系(b系)的xb轴指向机体的右侧，yb轴沿机体纵轴指向机头，zb轴指向机体的上方。采集了MEMS陀螺在低动态情况的输出，MEMS加速度计零偏为2mg，陀螺零偏为60°/h，采样时长1200s，采样时间间隔0.05s，即1s内采集200个加速度计信息和陀螺仪信息
采用图2所示PCF_SINS，分别进行加入非线性补偿滤波器(执行图3流程)与不加入非线性补偿器的导航姿态解算，得到解算之后的姿态，并进行对比，结果如图4A、图4B和图5A、图5B所示。
表1显示了MEMS陀螺在1200s之后的姿态发散的结果。
表1MEMS陀螺1200s后姿态发散结果
传统SINS PCF－SINS
俯仰角(度) 5.9971 －0.3971
横滚角(度) 2.6790 1.5672
从上表可知，采用PCF_SINS之后，对低精度MEMS陀螺进行1200s导航之后，姿态精度有了极大的提高，其中俯仰角的精度提高尤为明显。根据图4A、图4B和图5A、图5B可以看出，采用PCF_SINS之后，姿态角不在是一直发散，而是呈现出一种姿态收敛的趋势，最终能够达到一个稳定的值，而且这种值明显的小于SINS本身发散的结果。

Claims (4)

1.一种带有非线性补偿滤波器的捷联惯导系统，其特征在于：非线性补偿滤波器PCF设置在传统SINS的姿态阵解算模块与比力坐标变换模块之间；
非线性补偿滤波器一方面接收加速度计组合输出的比力信息另一方面接收姿态阵解算模块输出的姿态矩阵第三方面是将比力信息与姿态矩阵进行姿态非线性补偿，得到补偿后姿态矩阵所述补偿后姿态矩阵输出给比力坐标变换模块。
2.根据权利要求1所述的带有非线性补偿滤波器的捷联惯导系统，其特征在于：非线性补偿滤波器PCF进行的非线性补偿处理步骤为：
步骤一，构造测量矩阵C测量；
根据加速度计输出的X轴的比力Y轴的比力和Z轴的比力计算姿态角的约束和θ表示俯仰角，γ表示横滚角，g表示重力加速度；根据姿态角θ和γ构造得到测量姿态矩阵C测量；
sinγ表示横滚角的正弦；sinθ表示姿态角的正弦；sinψ表示航向角的正弦；cosγ表示横滚角的余弦；cosθ表示姿态角的余弦；cosψ表示航向角的余弦；
步骤二，比力对于姿态矩阵的约束；
步骤21，将姿态阵解算模块输出的姿态矩阵作为非线性补偿滤波器中估计矩阵的初值，记为即i＝0；
i为迭代次数；
步骤22，应用与C测量来计算误差估计矩阵且 为估计矩阵的转置；
步骤23，判断的对角线元素否是为1；
是，则在非线性补偿滤波器中将赋值给补偿后姿态矩阵然后将补偿后姿态矩阵输出给比力坐标变换模块；
否，则执行步骤三；
步骤三，非线性滤波补偿；
步骤31，计算残差矢量 为误差估计矩阵的转置；ka为残差矢量增益系数；
步骤32，计算陀螺仪偏差kg为陀螺仪偏差增益系数；
步骤33，计算中间矩阵矢量其中为角速度计输出，的反对称矩阵记为Ai；
步骤34，更新估计矩阵得到更新后的估计矩阵然后将返回步骤二；
为以自然对数e为底、以反对称矩阵Ai为变更的对数函数值；
Δt为惯性设备的输出周期。
3.根据权利要求2所述的带有非线性补偿滤波器的捷联惯导系统，其特征在于：惯性设备的输出周期优选为0.05s。
4.根据权利要求2所述的带有非线性补偿滤波器的捷联惯导系统，其特征在于：姿态角收敛。
CN201810350847.0A 2017-12-25 2018-04-18 一种带有非线性补偿滤波器的捷联惯导系统 Active CN108692727B (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201711422114 2017-12-25
CN2017114221145 2017-12-25

Publications (2)

Publication Number Publication Date
CN108692727A true CN108692727A (zh) 2018-10-23
CN108692727B CN108692727B (zh) 2021-06-29

Family

ID=63845067

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810350847.0A Active CN108692727B (zh) 2017-12-25 2018-04-18 一种带有非线性补偿滤波器的捷联惯导系统

Country Status (1)

Country Link
CN (1) CN108692727B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111156994A (zh) * 2019-12-31 2020-05-15 西安航天华迅科技有限公司 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN112902955A (zh) * 2021-03-05 2021-06-04 上海竹格智能传感技术有限公司 风机叶片转速传感器和风机叶片姿态测量方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1851406A (zh) * 2006-05-26 2006-10-25 南京航空航天大学 基于捷联惯性导航系统的姿态估计和融合的方法
CN101561496A (zh) * 2009-05-21 2009-10-21 北京航空航天大学 一种伪卫星和惯性组合导航系统的非线性补偿方法
CN102519460A (zh) * 2011-12-09 2012-06-27 东南大学 一种捷联惯性导航系统非线性对准方法
CN102654406A (zh) * 2012-04-11 2012-09-05 哈尔滨工程大学 基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法
CN102830414A (zh) * 2012-07-13 2012-12-19 北京理工大学 一种基于sins/gps的组合导航方法
CN103471616A (zh) * 2013-09-04 2013-12-25 哈尔滨工程大学 一种动基座sins大方位失准角条件下初始对准方法
CN103644913A (zh) * 2013-12-25 2014-03-19 东南大学 基于直接导航模型的无迹卡尔曼非线性初始对准方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1851406A (zh) * 2006-05-26 2006-10-25 南京航空航天大学 基于捷联惯性导航系统的姿态估计和融合的方法
CN101561496A (zh) * 2009-05-21 2009-10-21 北京航空航天大学 一种伪卫星和惯性组合导航系统的非线性补偿方法
CN102519460A (zh) * 2011-12-09 2012-06-27 东南大学 一种捷联惯性导航系统非线性对准方法
CN102654406A (zh) * 2012-04-11 2012-09-05 哈尔滨工程大学 基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法
CN102830414A (zh) * 2012-07-13 2012-12-19 北京理工大学 一种基于sins/gps的组合导航方法
CN103471616A (zh) * 2013-09-04 2013-12-25 哈尔滨工程大学 一种动基座sins大方位失准角条件下初始对准方法
CN103644913A (zh) * 2013-12-25 2014-03-19 东南大学 基于直接导航模型的无迹卡尔曼非线性初始对准方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
王玲玲等: "基于低成本IMU的捷联航姿系统软件设计与实现", 《中国惯性技术学报》 *
郁光辉等: "捷联惯导系统非线性误差补偿的随机采样法", 《北京理工大学学报》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111156994A (zh) * 2019-12-31 2020-05-15 西安航天华迅科技有限公司 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN111156994B (zh) * 2019-12-31 2023-10-27 上海星思半导体有限责任公司 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN112902955A (zh) * 2021-03-05 2021-06-04 上海竹格智能传感技术有限公司 风机叶片转速传感器和风机叶片姿态测量方法

Also Published As

Publication number Publication date
CN108692727B (zh) 2021-06-29

Similar Documents

Publication Publication Date Title
CN107525503B (zh) 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
CN106052685B (zh) 一种两级分离融合的姿态和航向估计方法
CN103917850B (zh) 一种惯性导航系统的运动对准方法
CN103940425B (zh) 一种磁‑惯性组合捷联测量方法
CN106052682B (zh) 一种混合式惯性导航系统及导航方法
CN104698485B (zh) 基于bd、gps及mems的组合导航系统及导航方法
CN107063262A (zh) 一种用于无人机姿态解算的互补滤波方法
CN104567930A (zh) 一种能够估计和补偿机翼挠曲变形的传递对准方法
CN103954303B (zh) 一种用于磁力计导航系统航向角动态计算及校正方法
CN105180937A (zh) 一种mems-imu初始对准方法
CN103822633A (zh) 一种基于二阶量测更新的低成本姿态估计方法
CN110058288B (zh) 无人机ins/gnss组合导航系统航向误差修正方法及系统
CN110044385B (zh) 一种大失准角情况下的快速传递对准方法
CN108344413A (zh) 一种水下滑翔器导航系统及其低精度与高精度转换方法
JP2007232443A (ja) 慣性航法装置およびその誤差補正方法
Jwo et al. Development of a strapdown inertial navigation system simulation platform
CN103674059A (zh) 一种基于外测速度信息的sins水平姿态误差修正方法
CN103175528A (zh) 基于捷联惯导系统的捷联罗经姿态测量方法
CN108627152A (zh) 一种微型无人机基于多传感器数据融合的导航方法
CN103776449A (zh) 一种提高鲁棒性的动基座初始对准方法
CN102168978A (zh) 一种船用惯性导航系统摇摆基座开环对准方法
CN108592943A (zh) 一种基于opreq方法的惯性系粗对准计算方法
CN108692727A (zh) 一种带有非线性补偿滤波器的捷联惯导系统
Jameian et al. A robust and fast self-alignment method for strapdown inertial navigation system in rough sea conditions
CN104501809B (zh) 一种基于姿态耦合的捷联惯导/星敏感器组合导航方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant