CN112629538B - 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 - Google Patents

基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 Download PDF

Info

Publication number
CN112629538B
CN112629538B CN202011462399.7A CN202011462399A CN112629538B CN 112629538 B CN112629538 B CN 112629538B CN 202011462399 A CN202011462399 A CN 202011462399A CN 112629538 B CN112629538 B CN 112629538B
Authority
CN
China
Prior art keywords
attitude
navigation
quaternion
coordinate system
ship
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
Application number
CN202011462399.7A
Other languages
English (en)
Other versions
CN112629538A (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering 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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202011462399.7A priority Critical patent/CN112629538B/zh
Publication of CN112629538A publication Critical patent/CN112629538A/zh
Application granted granted Critical
Publication of CN112629538B publication Critical patent/CN112629538B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Abstract

本发明属于舰船导航制导与控制技术领域，具体涉及一种基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法。本发明利用实时采集的微机电惯性测量单元陀螺仪输出信号和加速度计输出信号进行捷联惯性导航解算，综合利用各传感器的优点，实现了系统高精度的水平姿态测量。本发明通过卡尔曼滤波后的导航参数对机动状态下载体的线加速度和哥氏加速度进行补偿，并采用互补滤波补偿陀螺积分误差，使水平姿态保持较高精度输出，即使系统存在运动线加速度时，依然保证互补滤波的效果以及失准角的最优计算，有效的提高了系统姿态测量精度，具有一定的工程应用价值。

Description

基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
技术领域
本发明属于舰船导航制导与控制技术领域，具体涉及一种基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法。
背景技术
随着微机电系统技术的发展，低成本MEMS IMU在导航领域有着越来越多的应用，通过利用基于微机电系统的惯性传感器进行运动参数测量，可以检测船舶在海中复杂的运动状态，实时输出以四元数、欧拉角等表示的载体姿态角和线性位移数据，从而实现用户对水面舰船的运动数据采集。
微机电陀螺仪具有随机漂移特性，其积分误差随时间累积，加速度计不存在累积误差，但是易受到载体震动影响。常用的将二者数据融合的算法是卡尔曼滤波和互补滤波，例如在专利申请号为201811070907.X,名称为“基于机动状态判断的MEMS惯性导航系统水平姿态自修正方法”的专利文件中，通过比较加速度计输出和当地重力加速度幅值，将载体运动分为低，中，高动态。在低和中动态时，实时调整量测噪声矩阵，在高动态时只进行时间更新。又如在专利申请号为201911277173.7，名称为“一种高精度无人机系统及智能控制方法”的专利文件中，基于GPS模块接受卫星信号的精度因子和速度信息对互补滤波器的截止频率建立了自适应函数来满足不同运动状态下的解算精度。又如在专利申请号为201810875386.9,名称为“一种基于单天线GPS和IMU的大机动条件下无人机姿态估计方法”的专利文件中，将GPS测得的速度值差分，得到的结果对IMU测得的加速度数据进行补偿，再利用修正后的加速度信息修正陀螺仪。
发明内容
本发明的目的在于提供一种基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法。
本发明的目的通过如下技术方案来实现：包括以下步骤：
步骤1：获取初始捷联惯性导航系统的导航参数；
步骤2：采集加速度计输出的比力fb和陀螺仪输出的角速度
Figure GDA0003930197840000011
步骤3：进行捷联惯性导航系统初始对准，得到载体系相对于导航坐标系的初始捷联姿态矩阵
Figure GDA0003930197840000012
b系代表载体系；n系代表导航坐标系，取地理坐标系作为导航坐标系；
步骤4：根据初始捷联姿态矩阵
Figure GDA0003930197840000013
和采集到的角速度
Figure GDA0003930197840000014
递推更新得到姿态四元数并对其进行归一化处理，归一化后的姿态四元数q为：
q＝[q0 q1 q2 q3]T
其中，q0、q1、q2和q3是归一化后四元数q的元素；
步骤5：根据归一化后的姿态四元数q更新捷联姿态矩阵
Figure GDA0003930197840000021
Figure GDA0003930197840000022
步骤6：利用捷联姿态矩阵
Figure GDA0003930197840000023
将采集到的加速度计输出的比力fb转换到导航坐标系下；
Figure GDA0003930197840000024
其中，fn表示加速度计输出比力fb在导航坐标系的投影；
步骤7：根据加速度计输出比力fb在导航坐标系的投影fn去除有害加速度后得到舰船的加速度，并计算得到舰船的速度Vn(t)；
Figure GDA0003930197840000025
其中，
Figure GDA0003930197840000026
为舰船的东向速度；
Figure GDA0003930197840000027
为舰船的北向速度；
Figure GDA0003930197840000028
为舰船的天向速度；t表示当前时刻；
步骤8：利用舰船的速度Vn(t)更新捷联惯性导航系统的导航纬度
Figure GDA0003930197840000029
和经度λ(t)；
步骤9：选取位置误差
Figure GDA00039301978400000210
东北天三个方向的速度误差δV＝[δVEδVN δVU]T、平台失准角误差φ＝[φx φy φz]T、载体系三轴的加速度计的零位偏移ΔA＝[ΔAbx ΔAby ΔAbz]T和陀螺仪的常值漂移ε＝[εbx εby εbz]T，作为卡尔曼滤波的状态估计量X＝[δP δV φ ΔA ε]T；
步骤10：选取卡尔曼滤波的系统噪声向量WB为：
WB＝[wax way waz wgx wgy wgz]T
其中，wax、way和waz为捷联惯性导航系统在载体系中三轴加速度计的随机噪声；wgx、wgy和wgz为捷联惯性导航系统在载体系中三轴陀螺仪的随机噪声，均为高斯白噪声；
步骤11：将捷联惯性导航系统导航解算的纬度
Figure GDA00039301978400000211
和经度λ(t)与GPS提供的纬度
Figure GDA00039301978400000212
和经度λA(t)的差值作为卡尔曼滤波的量测值Z(t)，量测值Z(t)和对应量测矩阵H(t)分别为：
Figure GDA0003930197840000031
步骤12：采用卡尔曼滤波算法对选取的状态量进行实时估计，得到捷联惯性导航系统的位置误差
Figure GDA0003930197840000032
速度误差
Figure GDA0003930197840000033
平台失准角的状态估计值
Figure GDA0003930197840000034
步骤13：利用捷联惯性导航系统的位置误差
Figure GDA0003930197840000035
和速度误差
Figure GDA0003930197840000036
校正捷联惯性导航系统解算的位置和速度信息；
Figure GDA0003930197840000037
步骤14：利用捷联惯性导航系统的平台失准角的状态估计值
Figure GDA0003930197840000038
构造四元数
Figure GDA0003930197840000039
对捷联惯性导航系统解算的当前时刻捷联姿态矩阵
Figure GDA00039301978400000310
对应的姿态四元数q＝[q0 q1 q2 q3]T进行补偿得到修正后的四元数q′＝[q′0 q′1 q′2q′3]T；
Figure GDA00039301978400000311
步骤15：将修正后的四元数q′进行归一化处理，得到归一化后的四元数
Figure GDA00039301978400000312
Figure GDA00039301978400000313
步骤16：根据归一化后的姿态四元数
Figure GDA00039301978400000314
计算校正后的捷联姿态矩阵
Figure GDA00039301978400000315
Figure GDA0003930197840000041
步骤17：利用校正后的捷联惯性导航系统的导航参数，借助于比力方程对舰船机动引起的线加速度和哥氏加速度进行补偿，得到重力加速度在导航系的理论输出gn：
Figure GDA0003930197840000042
其中，V1 n为经卡尔曼滤波校正后的地速；
Figure GDA0003930197840000043
为相邻两个姿态解算时刻的速度变化率；
Figure GDA0003930197840000044
为校正后的地球自转角速度在导航坐标系的投影；
Figure GDA0003930197840000045
为舰船的运动引起导航坐标系相当于地球坐标系的旋转角速度在导航坐标系的投影；
步骤18：将重力加速度在导航系的理论输出gn和重力加速度g两矢量归一化并进行叉乘运算得到陀螺积分后的姿态误差
Figure GDA0003930197840000046
Figure GDA0003930197840000047
其中，||||表示取向量模值；
步骤19：利用陀螺积分后的姿态误差
Figure GDA0003930197840000048
求取陀螺积分误差Δωb：
Figure GDA0003930197840000049
其中，dt为两个姿态解算时刻差值，kp，ki为比例和积分系数；
步骤20：利用陀螺积分误差Δωb补偿陀螺仪输出的角速度；
Figure GDA00039301978400000410
步骤21：将陀螺仪数据去除地球自转角速度在导航坐标系n的投影以及载体运动导致的导航坐标系n相对地球坐标系e的旋转角速度在导航坐标系n系的投影，得到
Figure GDA00039301978400000411
Figure GDA00039301978400000412
步骤22：利用角速度数据
Figure GDA00039301978400000413
更新修正后的四元数，得到
Figure GDA00039301978400000414
Figure GDA00039301978400000415
步骤23：将四元数
Figure GDA0003930197840000051
进行归一化处理，得到归一化后的四元数
Figure GDA0003930197840000052
Figure GDA0003930197840000053
步骤24：根据归一化后的四元数
Figure GDA0003930197840000054
计算校正后的捷联姿态矩阵
Figure GDA0003930197840000055
完成舰船水平姿态测量的更新和修正；
Figure GDA0003930197840000056
本发明的有益效果在于：
本发明利用实时采集的微机电惯性测量单元陀螺仪输出信号和加速度计输出信号进行捷联惯性导航解算，初步得出运载体位置速度和姿态信息；再利用GPS给出的位置数据作为系统的外界辅助信息，采用间接卡尔曼滤波方法对速度误差，位置误差，平台失准角等状态量进行实时估计，并对初步得出的导航参数进行校正。最后借助比力方程，补偿运载体线加速度以及哥氏加速度得到重力加速度在导航系的理论输出，采用比例积分校正补偿陀螺仪积分误差从而得到更高测量精度的水平姿态信息。本发明综合利用各传感器的优点，实现了系统高精度的水平姿态测量。通过卡尔曼滤波后的导航参数对机动状态下载体的线加速度和哥氏加速度进行补偿，并采用互补滤波补偿陀螺积分误差，使水平姿态保持较高精度输出，即使系统存在运动线加速度时，依然保证互补滤波的效果以及失准角的最优计算，有效的提高了系统姿态测量精度，具有一定的工程应用价值。
附图说明
图1为本发明的架构图。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明属于舰船导航制导与控制技术领域，提供了一种基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法，涉及以微机电惯性测量单元为核心器件的姿态测量系统。
本发明首先利用实时采集的微机电惯性测量单元陀螺仪输出信号
Figure GDA0003930197840000057
和加速度计输出信号fb进行捷联惯性导航解算，初步得出运载体位置速度和姿态信息；再利用GPS给出的位置数据作为系统的外界辅助信息，采用间接卡尔曼滤波方法对速度误差，位置误差，平台失准角等状态量进行实时估计，并对初步得出的导航参数进行校正。最后借助比力方程，补偿运载体线加速度以及哥氏加速度得到重力加速度在导航系的理论输出，采用比例积分校正补偿陀螺仪积分误差从而得到更高测量精度的水平姿态信息。本发明综合利用各传感器的优点，同时对机动状态下载体的线加速度进行补偿，保证系统在不同运动状态下均具有较高的姿态测量精度，具有一定的工程应用价值。本发明具体步骤如下：
步骤1、给定初始捷联惯性导航系统的导航参数；
步骤2、采集加速度计输出的比力fb和陀螺仪输出的角速度
Figure GDA0003930197840000061
步骤3、进行捷联惯性导航系统初始对准，得到载体系(b系)相对于导航坐标系(n系，取地理坐标系作为导航坐标系)的初始捷联姿态矩阵
Figure GDA0003930197840000062
步骤4、进行捷联惯性导航解算，得到运载体速度，位置和姿态等导航参数；
步骤5、选取捷联惯导系统解算的位置误差、速度误差、平台失准角误差、三轴加速度计的零位偏移和陀螺仪的常值漂移为卡尔曼滤波的状态估计量；
步骤6、将捷联惯性导航系统导航解算的位置信息(纬度
Figure GDA0003930197840000063
和经度λ(t))与GPS提供的纬度
Figure GDA0003930197840000064
和经度λA(t)的差值作为卡尔曼滤波的量测值Z(t)，量测值Z(t)和对应量测矩阵H(t)分别为：
Figure GDA0003930197840000065
步骤7、采用卡尔曼滤波算法对选取的状态量进行实时估计，得到捷联惯性导航系统的位置误差、速度误差和平台失准角的状态估计值；
步骤8、利用步骤7中估计的状态来校正捷联惯性导航系统得到的导航参数；
步骤9、利用步骤8中校正的捷联惯性导航系统的导航参数和地球曲率半径，地球自转角速度等常量借助于比力方程对运载体机动引起的线加速度和哥氏加速度进行补偿，得到重力加速度在导航系的理论输出gn：
Figure GDA0003930197840000066
其中，V1 n为经卡尔曼滤波校正后的地速，
Figure GDA0003930197840000067
为相邻两个姿态解算时刻的速度变化率，
Figure GDA0003930197840000068
为校正后的地球自转角速度在导航坐标系的投影,
Figure GDA0003930197840000069
为运载体的运动引起导航坐标系相当于地球坐标系(e系)的旋转角速度在导航坐标系的投影，
Figure GDA00039301978400000610
为卡尔曼滤波校正后姿态对应的捷联姿态矩阵，gn为重力加速度在导航系的理论输出，fb为加速度计输出的比力；
步骤10、对步骤9中求得的gn和实际重力加速度g两矢量归一化并进行叉乘运算得陀螺积分后的姿态误差
Figure GDA00039301978400000611
Figure GDA00039301978400000612
其中||||表示取向量模值；
步骤11、利用步骤10得到的姿态误差
Figure GDA0003930197840000071
求取陀螺积分误差Δωb：
Figure GDA0003930197840000072
其中dt为两个姿态解算时刻差值，kp，ki为比例和积分系数；比例环节系数取kp＝0.5，积分环节系数取ki＝0.1；
步骤12、利用步骤11中求取的陀螺积分误差Δωb来补偿陀螺仪积分误差；
步骤13、利用步骤12中补偿后的陀螺仪输出更新姿态四元数，从而校正捷联惯性导航系统的捷联姿态矩阵，获得更高的测量精度；
至此就完成了基于融合互补滤波和卡尔曼滤波算法的水面舰船水平姿态测量方法更新和修正。
本发明综合利用各传感器的优点，实现了系统高精度的水平姿态测量。通过卡尔曼滤波后的导航参数对机动状态下载体的线加速度和哥氏加速度进行补偿，并采用互补滤波补偿陀螺积分误差，使水平姿态保持较高精度输出，即使系统存在运动线加速度时，依然保证互补滤波的效果以及失准角的最优计算，有效的提高了系统姿态测量精度，具有一定的工程应用价值。
实施例1：
本发明利用实时采集的微机电惯性测量单元陀螺仪输出信号
Figure GDA0003930197840000073
和加速度计输出信号fb(b表示载体坐标系)进行捷联惯性导航解算，初步得出运载体位置速度和姿态信息；再利用GPS给出的位置数据作为系统的外界辅助信息，采用间接卡尔曼滤波方法对速度误差，位置误差，平台失准角等状态量进行实时估计，并对初步得出的导航参数进行校正。最后借助比力方程，补偿运载体线加速度以及哥氏加速度得到重力加速度在导航系的理论输出，采用比例积分校正补偿陀螺仪积分误差从而得到更高测量精度的水平姿态，具体步骤如下：
步骤1、给定初始捷联惯性导航系统的导航参数；
步骤2、采集加速度计输出的比力fb和陀螺仪输出的角速度
Figure GDA0003930197840000074
步骤3、进行捷联惯性导航系统初始对准，得到载体系(b系)相对于导航坐标系(n系，取地理坐标系作为导航坐标系)的初始捷联姿态矩阵
Figure GDA0003930197840000075
步骤4、根据步骤3中得到的初始捷联姿态矩阵
Figure GDA0003930197840000076
和步骤2中实时采集到的角速度
Figure GDA0003930197840000077
递推更新得到姿态四元数并对其进行归一化处理，归一化后的姿态四元数q具体表达式为：
q＝[q0 q1 q2 q3]T (5)
其中，q0、q1、q2和q3是归一化后四元数q的元素；
步骤5、根据步骤4中得到的归一化后的姿态四元数q更新捷联姿态矩阵
Figure GDA0003930197840000078
Figure GDA0003930197840000081
步骤6、利用步骤5得到的捷联姿态矩阵
Figure GDA0003930197840000082
将步骤2采集到的加速度计输出的比力fb转换到导航坐标系下：
Figure GDA0003930197840000083
其中，fn表示加速度计输出比力在导航坐标系的投影；
步骤7、根据步骤6中转化到导航坐标系的比力fn去除有害加速度后得到系统的加速度，进一步更新计算得到速度，记为Vn(t)：
Figure GDA0003930197840000084
其中，
Figure GDA0003930197840000085
为系统解算的东向速度、
Figure GDA0003930197840000086
为其北向速度、
Figure GDA0003930197840000087
为其天向速度，t表示当前时刻；
步骤8、利用步骤7更新计算后的速度Vn(t)更新捷联惯性导航系统的导航纬度
Figure GDA0003930197840000088
和经度λ(t)；
步骤9、选取位置误差
Figure GDA0003930197840000089
(纬度误差
Figure GDA00039301978400000818
经度误差δλ和高度误差δh)、东北天三个方向的速度误差δV＝[δVE δVN δVU]T、平台失准角误差φ＝[φx φy φz]T、载体系三轴的加速度计的零位偏移ΔA＝[ΔAbx ΔAby ΔAbz]T和陀螺仪的常值漂移ε＝[εbxεby εbz]T为卡尔曼滤波的状态估计量X：
X＝[δP δV φ ΔA ε]T (9)
步骤10、选取卡尔曼滤波的系统噪声向量WB为：
WB＝[wax way waz wgx wgy wgz]T (10)
其中，wax、way和waz为捷联惯性导航系统在载体系中三轴加速度计的随机噪声，wgx、wgy和wgz为捷联惯性导航系统在载体系中三轴陀螺仪的随机噪声，均为高斯白噪声；
步骤11、将捷联惯性导航系统导航解算的位置信息(纬度
Figure GDA00039301978400000810
和经度λ(t))与GPS提供的纬度
Figure GDA00039301978400000811
和经度λA(t)的差值作为卡尔曼滤波的量测值Z(t)，量测值Z(t)和对应量测矩阵H(t)分别为：
Figure GDA00039301978400000812
步骤12、采用卡尔曼滤波算法对选取的状态量进行实时估计，得到捷联惯性导航系统的位置误差
Figure GDA00039301978400000813
速度误差
Figure GDA00039301978400000814
平台失准角的状态估计值
Figure GDA00039301978400000815
步骤13、利用步骤12中估计出来的位置误差
Figure GDA00039301978400000816
速度误差
Figure GDA00039301978400000817
来校正捷联惯性导航系统解算的位置和速度信息；
Figure GDA0003930197840000091
步骤14、利用步骤12实时估计出来的平台失准角
Figure GDA0003930197840000092
来构造四元数
Figure GDA0003930197840000093
对捷联惯性导航系统解算的当前时刻捷联姿态矩阵
Figure GDA0003930197840000094
对应的姿态四元数q＝[q0 q1 q2 q3]T进行补偿得到修正后的四元数q′＝[q′0 q′1 q′2 q′3]T：
Figure GDA0003930197840000095
Figure GDA0003930197840000096
表示四元数乘法。其中，修正后的四元数q′的元素为：
Figure GDA0003930197840000097
步骤15、将步骤14中得到的修正后的四元数q′进行归一化处理：
Figure GDA0003930197840000098
得到归一化后的四元数为：
Figure GDA0003930197840000099
其中，
Figure GDA00039301978400000910
q′2、
Figure GDA00039301978400000911
和
Figure GDA00039301978400000912
是归一化后四元数
Figure GDA00039301978400000913
的元素；
步骤16、根据归一化后的姿态四元数
Figure GDA00039301978400000914
计算得到校正后的捷联姿态矩阵
Figure GDA00039301978400000915
Figure GDA00039301978400000916
至此为卡尔曼滤波器对捷联惯性导航系统的导航参数校正的过程；
步骤17、利用步骤13至步骤16中校正的捷联惯性导航系统的导航参数和地球曲率半径，地球自转角速度等常量，借助于比力方程对运载体机动引起的线加速度和哥氏加速度进行补偿，得到重力加速度在导航系的理论输出gn：
Figure GDA00039301978400000917
其中，V1 n为经卡尔曼滤波校正后的地速，
Figure GDA00039301978400000918
为相邻两个姿态解算时刻的速度变化率，
Figure GDA00039301978400000919
为校正后的地球自转角速度在导航坐标系的投影,
Figure GDA00039301978400000920
为运载体的运动引起导航坐标系相当于地球坐标系(e系)的旋转角速度在导航坐标系的投影，
Figure GDA00039301978400000921
为卡尔曼滤波校正后姿态对应的捷联姿态矩阵，gn为重力加速度在导航系的理论输出，fb为加速度计输出的比力；
步骤18、对步骤17中求得的gn和重力加速度g两矢量归一化并进行叉乘运算得陀螺积分后的姿态误差
Figure GDA0003930197840000101
Figure GDA0003930197840000102
其中||||表示取向量模值；
步骤19、利用步骤18得到的姿态误差
Figure GDA0003930197840000103
求取陀螺积分误差Δωb：
Figure GDA0003930197840000104
其中dt为两个姿态解算时刻差值，kp，ki为比例和积分系数；比例环节系数取kp＝0.5，积分环节系数取ki＝0.1；
步骤20、利用步骤19计算的陀螺积分误差补偿陀螺仪输出角速度：
Figure GDA0003930197840000105
步骤21、步骤20得到的补偿后的陀螺仪数据去除地球自转角速度在导航坐标系n的投影和载体运动导致的导航坐标系n相对地球坐标系e的旋转角速度在导航坐标系n系的投影：
Figure GDA0003930197840000106
步骤22、利用步骤21得到的角速度数据
Figure GDA0003930197840000107
更新步骤15得到的四元数：
Figure GDA0003930197840000108
步骤23、将步骤22中得到的修正后的四元数q″进行归一化处理：
Figure GDA0003930197840000109
得到归一化后的四元数为：
Figure GDA00039301978400001010
步骤24、根据归一化后的姿态四元数
Figure GDA00039301978400001011
计算得到校正后的捷联姿态矩阵
Figure GDA00039301978400001012
Figure GDA00039301978400001013
至此就完成了基于融合互补滤波和卡尔曼滤波算法的水面舰船水平姿态测量方法更新和修正。
本发明结合互补滤波和卡尔曼滤波器，通过卡尔曼滤波得到的导航参数借助比力方程补偿载体线加速度和哥氏加速度得到理论重力加速度在导航系的投影，再与实际重力加速度叉乘得出偏差，最后通过比例积分校正修正陀螺输出数据，得出校正后的水平姿态。优点是通过与卡尔曼滤波结合的互补滤波有效地补偿载体的线性加速度，在不同运动状态下都能保持水平姿态较高精度输出，有效提高了系统姿态测量精度，具有一定的工程应用价值。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法，其特征在于，包括以下步骤：
步骤1：获取初始捷联惯性导航系统的导航参数；
步骤2：采集加速度计输出的比力fb和陀螺仪输出的角速度
Figure FDA0003905599880000011
步骤3：进行捷联惯性导航系统初始对准，得到载体系相对于导航坐标系的初始捷联姿态矩阵
Figure FDA0003905599880000012
b系代表载体系；n系代表导航坐标系，取地理坐标系作为导航坐标系；
步骤4：根据初始捷联姿态矩阵
Figure FDA0003905599880000013
和采集到的角速度
Figure FDA0003905599880000014
递推更新得到姿态四元数并对其进行归一化处理，归一化后的姿态四元数q为：
q＝[q0 q1 q2 q3]T
其中，q0、q1、q2和q3是归一化后四元数q的元素；
步骤5：根据归一化后的姿态四元数q更新捷联姿态矩阵
Figure FDA0003905599880000015
Figure FDA0003905599880000016
步骤6：利用捷联姿态矩阵
Figure FDA0003905599880000017
将采集到的加速度计输出的比力fb转换到导航坐标系下；
Figure FDA0003905599880000018
其中，fn表示加速度计输出比力fb在导航坐标系的投影；
步骤7：根据加速度计输出比力fb在导航坐标系的投影fn去除有害加速度后得到舰船的加速度，并计算得到舰船的速度Vn(t)；
Figure FDA0003905599880000019
其中，
Figure FDA00039055998800000110
为舰船的东向速度；
Figure FDA00039055998800000111
为舰船的北向速度；
Figure FDA00039055998800000112
为舰船的天向速度；t表示当前时刻；
步骤8：利用舰船的速度Vn(t)更新捷联惯性导航系统的导航纬度
Figure FDA00039055998800000113
和经度λ(t)；
步骤9：选取位置误差
Figure FDA00039055998800000114
东北天三个方向的速度误差δV＝[δVE δVN δVU]T、平台失准角误差φ＝[φx φy φz]T、载体系三轴的加速度计的零位偏移ΔA＝[ΔAbxΔAby ΔAbz]T和陀螺仪的常值漂移ε＝[εbx εby εbz]T，作为卡尔曼滤波的状态估计量X＝[δPδV φ ΔA ε]T；
步骤10：选取卡尔曼滤波的系统噪声向量WB为：
WB＝[wax way waz wgx wgy wgz]T
其中，wax、way和waz为捷联惯性导航系统在载体系中三轴加速度计的随机噪声；wgx、wgy和wgz为捷联惯性导航系统在载体系中三轴陀螺仪的随机噪声，均为高斯白噪声；
步骤11：将捷联惯性导航系统导航解算的纬度
Figure FDA0003905599880000021
和经度λ(t)与GPS提供的纬度
Figure FDA0003905599880000022
和经度λA(t)的差值作为卡尔曼滤波的量测值Z(t)，量测值Z(t)和对应量测矩阵H(t)分别为：
Figure FDA0003905599880000023
步骤12：采用卡尔曼滤波算法对选取的状态量进行实时估计，得到捷联惯性导航系统的位置误差
Figure FDA0003905599880000024
速度误差
Figure FDA0003905599880000025
平台失准角的状态估计值
Figure FDA0003905599880000026
步骤13：利用捷联惯性导航系统的位置误差
Figure FDA0003905599880000027
和速度误差
Figure FDA0003905599880000028
校正捷联惯性导航系统解算的位置和速度信息；
Figure FDA0003905599880000029
步骤14：利用捷联惯性导航系统的平台失准角的状态估计值
Figure FDA00039055998800000210
构造四元数
Figure FDA00039055998800000211
对捷联惯性导航系统解算的当前时刻捷联姿态矩阵
Figure FDA00039055998800000212
对应的姿态四元数q＝[q0 q1 q2 q3]T进行补偿得到修正后的四元数q′＝[q′0 q′1 q′2 q′3]T；
Figure FDA0003905599880000031
步骤15：将修正后的四元数q′进行归一化处理，得到归一化后的四元数
Figure FDA0003905599880000032
Figure FDA0003905599880000033
步骤16：根据归一化后的姿态四元数
Figure FDA0003905599880000034
计算校正后的捷联姿态矩阵
Figure FDA0003905599880000035
Figure FDA0003905599880000036
步骤17：利用校正后的捷联惯性导航系统的导航参数，借助于比力方程对舰船机动引起的线加速度和哥氏加速度进行补偿，得到重力加速度在导航系的理论输出gn：
Figure FDA0003905599880000037
其中，V1 n为经卡尔曼滤波校正后的地速；
Figure FDA0003905599880000038
为相邻两个姿态解算时刻的速度变化率；
Figure FDA0003905599880000039
为校正后的地球自转角速度在导航坐标系的投影；
Figure FDA00039055998800000310
为舰船的运动引起导航坐标系相当于地球坐标系的旋转角速度在导航坐标系的投影；
步骤18：将重力加速度在导航系的理论输出gn和重力加速度g两矢量归一化并进行叉乘运算得到陀螺积分后的姿态误差
Figure FDA00039055998800000311
Figure FDA00039055998800000312
其中，|| ||表示取向量模值；
步骤19：利用陀螺积分后的姿态误差
Figure FDA00039055998800000313
求取陀螺积分误差Δωb：
Figure FDA00039055998800000314
其中，dt为两个姿态解算时刻差值，kp，ki为比例和积分系数；
步骤20：利用陀螺积分误差Δωb补偿陀螺仪输出的角速度；
Figure FDA0003905599880000041
步骤21：将陀螺仪数据去除地球自转角速度在导航坐标系n的投影以及载体运动导致的导航坐标系n相对地球坐标系e的旋转角速度在导航坐标系n系的投影，得到
Figure FDA0003905599880000042
Figure FDA0003905599880000043
步骤22：利用角速度数据
Figure FDA0003905599880000044
更新修正后的四元数，得到
Figure FDA0003905599880000045
Figure FDA0003905599880000046
步骤23：将四元数
Figure FDA0003905599880000047
进行归一化处理，得到归一化后的四元数
Figure FDA0003905599880000048
Figure FDA0003905599880000049
步骤24：根据归一化后的四元数
Figure FDA00039055998800000410
计算校正后的捷联姿态矩阵
Figure FDA00039055998800000411
完成舰船水平姿态测量的更新和修正；
Figure FDA00039055998800000412
CN202011462399.7A 2020-12-11 2020-12-11 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 Active CN112629538B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011462399.7A CN112629538B (zh) 2020-12-11 2020-12-11 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011462399.7A CN112629538B (zh) 2020-12-11 2020-12-11 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法

Publications (2)

Publication Number Publication Date
CN112629538A CN112629538A (zh) 2021-04-09
CN112629538B true CN112629538B (zh) 2023-02-14

Family

ID=75312371

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011462399.7A Active CN112629538B (zh) 2020-12-11 2020-12-11 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法

Country Status (1)

Country Link
CN (1) CN112629538B (zh)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113175926B (zh) * 2021-04-21 2022-06-21 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法
CN113447018B (zh) * 2021-07-06 2023-05-26 北京理工导航控制科技股份有限公司 一种水下惯性导航系统的姿态实时估计方法
CN113701747A (zh) * 2021-07-20 2021-11-26 北京航天控制仪器研究所 一种基于离心机激励的惯性测量系统姿态角误差分离方法
CN113776529B (zh) * 2021-09-13 2024-04-19 中国人民解放军海军工程大学 一种基于载体系四元数姿态误差的非线性误差模型
CN114061571B (zh) * 2021-11-12 2023-08-04 同济大学 一种自适应梯度下降惯性测量单元的姿态解算方法及系统
CN114111770B (zh) * 2021-11-19 2024-04-09 清华大学 一种水平姿态测量方法、系统、处理设备及存储介质
CN114166216B (zh) * 2021-11-25 2023-07-21 哈尔滨工程大学 一种aru正置与倒置安装的水平姿态测量方法
CN114111771A (zh) * 2021-11-25 2022-03-01 九江中船仪表有限责任公司（四四一厂） 一种双轴稳定平台的动态姿态测量方法
CN114323007A (zh) * 2021-12-30 2022-04-12 西人马帝言(北京)科技有限公司 一种载体运动状态估计方法及装置
CN114383613B (zh) * 2022-01-18 2023-12-15 广东工业大学 一种基于北斗定向的电力线舞动监测系统和方法
CN114485641B (zh) * 2022-01-24 2024-03-26 武汉梦芯科技有限公司 一种基于惯导卫导方位融合的姿态解算方法及装置
CN115031785B (zh) * 2022-06-21 2023-05-16 浙江大学 一种基于多传感器融合技术的软土勘测方法
CN114911252B (zh) * 2022-07-15 2022-09-30 北京航天驭星科技有限公司 基于遥测数据确定火箭姿态的方法、装置、设备、介质
CN116674701A (zh) * 2023-04-26 2023-09-01 上海新纪元机器人有限公司 减摇防晕座椅及其控制方法和系统
CN116588261B (zh) * 2023-07-03 2024-02-09 上海新纪元机器人有限公司 座椅主动补偿控制方法及系统

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6014103A (en) * 1997-05-29 2000-01-11 Lockheed Martin Corporation Passive navigation system
KR20020080829A (ko) * 2001-04-18 2002-10-26 주식회사 네비콤 오차보정시스템을 구비하는 관성측정유닛-지피에스통합시스템과 미지정수 검색범위 축소방법 및 사이클 슬립검출방법, 및 그를 이용한 항체 위치, 속도,자세측정방법
CN101187567A (zh) * 2007-12-18 2008-05-28 哈尔滨工程大学 基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法
CN101793523A (zh) * 2010-03-10 2010-08-04 北京航空航天大学 一种组合导航和光电探测一体化系统
CN104374388A (zh) * 2014-11-10 2015-02-25 大连理工大学 一种基于偏振光传感器的航姿测定方法
CN105021192A (zh) * 2015-07-30 2015-11-04 华南理工大学 一种基于零速校正的组合导航系统的实现方法
CN107015560A (zh) * 2017-03-02 2017-08-04 浙江大学 一种基于无人机的光伏阵列的巡检方法
CN107941216A (zh) * 2017-09-18 2018-04-20 哈尔滨工程大学 一种基于自适应互补滤波的捷联惯导系统外水平阻尼方法
CN108759845A (zh) * 2018-07-05 2018-11-06 华南理工大学 一种基于低成本多传感器组合导航的优化方法
CN109001787A (zh) * 2018-05-25 2018-12-14 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器
CN110031882A (zh) * 2018-08-02 2019-07-19 哈尔滨工程大学 一种基于sins/dvl组合导航系统的外量测信息补偿方法
CN111307114A (zh) * 2019-11-29 2020-06-19 哈尔滨工程大学 基于运动参考单元的水面舰船水平姿态测量方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6879875B1 (en) * 2003-09-20 2005-04-12 American Gnc Corporation Low cost multisensor high precision positioning and data integrated method and system thereof
CN103245357A (zh) * 2013-04-03 2013-08-14 哈尔滨工程大学 一种船用捷联惯导系统二次快速对准方法
CN107621264B (zh) * 2017-09-30 2021-01-19 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN108413960A (zh) * 2018-01-25 2018-08-17 深圳市博安智控科技有限公司 姿态计算方法和相关设备
CN108534783A (zh) * 2018-05-11 2018-09-14 安徽尼古拉电子科技有限公司 一种基于北斗导航技术的飞行器导航方法

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6014103A (en) * 1997-05-29 2000-01-11 Lockheed Martin Corporation Passive navigation system
KR20020080829A (ko) * 2001-04-18 2002-10-26 주식회사 네비콤 오차보정시스템을 구비하는 관성측정유닛-지피에스통합시스템과 미지정수 검색범위 축소방법 및 사이클 슬립검출방법, 및 그를 이용한 항체 위치, 속도,자세측정방법
CN101187567A (zh) * 2007-12-18 2008-05-28 哈尔滨工程大学 基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法
CN101793523A (zh) * 2010-03-10 2010-08-04 北京航空航天大学 一种组合导航和光电探测一体化系统
CN104374388A (zh) * 2014-11-10 2015-02-25 大连理工大学 一种基于偏振光传感器的航姿测定方法
CN105021192A (zh) * 2015-07-30 2015-11-04 华南理工大学 一种基于零速校正的组合导航系统的实现方法
CN107015560A (zh) * 2017-03-02 2017-08-04 浙江大学 一种基于无人机的光伏阵列的巡检方法
CN107941216A (zh) * 2017-09-18 2018-04-20 哈尔滨工程大学 一种基于自适应互补滤波的捷联惯导系统外水平阻尼方法
CN109001787A (zh) * 2018-05-25 2018-12-14 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器
CN108759845A (zh) * 2018-07-05 2018-11-06 华南理工大学 一种基于低成本多传感器组合导航的优化方法
CN110031882A (zh) * 2018-08-02 2019-07-19 哈尔滨工程大学 一种基于sins/dvl组合导航系统的外量测信息补偿方法
CN111307114A (zh) * 2019-11-29 2020-06-19 哈尔滨工程大学 基于运动参考单元的水面舰船水平姿态测量方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
Ben Yueyang et al..Ocean Current Model Using SINS-GPS-DVL Integrated Navigation.《Applied Mechanics and Materials》.2013,第316-317卷 *
Yue Yang et al..A Nonlinear Double Model for Multisensor-Integrated Navigation Using the Federated EKF Algorithm for Small UAVs：web of science integrated kalman strapdown GPS.《Sensors》.2020, *
张栋等.互补滤波和卡尔曼滤波的融合姿态解算方法.《传感器与微系统》.2017,第36卷(第3期), *
朱托.GPS/SINS组合导航与姿态测量的设计.《中国优秀硕士学位论文全文数据库（电子期刊）基础科技辑》.2015,(第3期), *
杜署明.基于捷联惯性传感的吊钩姿态估计技术研究.《中国优秀硕士学位论文全文数据库（电子期刊）工程科技II辑》.2016,(第3期), *

Also Published As

Publication number Publication date
CN112629538A (zh) 2021-04-09

Similar Documents

Publication Publication Date Title
CN112629538B (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN112504275B (zh) 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN107655476B (zh) 基于多信息融合补偿的行人高精度足部导航方法
CN110501024B (zh) 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
CN109459044B (zh) 一种gnss双天线辅助的车载mems惯导组合导航方法
CN109813311B (zh) 一种无人机编队协同导航方法
CN110221332B (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
US6459990B1 (en) Self-contained positioning method and system thereof for water and land vehicles
CN109000640B (zh) 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
CN111121766B (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN110017837B (zh) 一种姿态抗磁干扰的组合导航方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
KR20070032988A (ko) 차량의 위치, 자세 및 헤딩을 추측하는 시스템 및 방법
EP1585939A2 (en) Attitude change kalman filter measurement apparatus and method
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN112432642B (zh) 一种重力灯塔与惯性导航融合定位方法及系统
CN111722295B (zh) 一种水下捷联式重力测量数据处理方法
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN110285815A (zh) 一种可在轨全程应用的微纳卫星多源信息姿态确定方法
CN111141273A (zh) 基于多传感器融合的组合导航方法及系统
CN112880669A (zh) 一种航天器星光折射和单轴旋转调制惯性组合导航方法
CN116105730A (zh) 基于合作目标卫星甚短弧观测的仅测角光学组合导航方法
CN114964222A (zh) 一种车载imu姿态初始化方法、安装角估计方法及装置
CN108416387B (zh) 基于gps与气压计融合数据的高度滤波方法
Wang et al. State transformation extended Kalman filter for SINS based integrated navigation system

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