CN110398257B - Gps辅助的sins***快速动基座初始对准方法 - Google Patents
Gps辅助的sins***快速动基座初始对准方法 Download PDFInfo
- Publication number
- CN110398257B CN110398257B CN201910644265.8A CN201910644265A CN110398257B CN 110398257 B CN110398257 B CN 110398257B CN 201910644265 A CN201910644265 A CN 201910644265A CN 110398257 B CN110398257 B CN 110398257B
- Authority
- CN
- China
- Prior art keywords
- matrix
- formula
- attitude
- initial
- gyroscope
- 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
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/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
- 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
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manufacturing & Machinery (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
本发明涉及一种GPS辅助的SINS***快速动基座初始对准方法,属于组合导航的技术领域。包括使用GPS数据和SINS数据计算导航系姿态变化和载体系姿态变化;构建SINS/GPS***的状态方程和量测方程;利用简化的自适应无迹卡尔曼滤波算法对失准角和陀螺仪常值漂移进行估计;将估计的失准角和陀螺仪常值漂移对SINS***对准的姿态和陀螺仪的输出进行矫正;实现SINS***的快速动基座初始对准。本发明通过对算法中失准角和陀螺仪误差进行估计,提高了对准的精度和收敛性能,使用自适应算法对量测噪声进行估计,避免了以往算法中量测噪声协方差矩阵设置不准确的问题,提高了对准算法的稳定性,对无迹卡尔曼滤波进行化简,减少了对准算法的计算量;应用前景广阔。
Description
技术领域
本发明涉及一种GPS辅助的SINS***快速动基座初始对准方法,属于组合导航的技术领域。
背景技术
捷联式惯性导航***(SINS)根据陀螺仪输出的角速率和加速度计输出的比力可以计算出载体的姿态、速度和位置,能够为许多军事和民用设备提供丰富的导航信息。SINS***在导航前需要知道准确的初始导航信息,初始导航信息的准确与否决定了接下来的导航是否可靠。因此,精确的初始对准是SINS***准确导航的关键。对于高精度SINS***,***可以在静基座环境下通过感应地球自转和重力加速度确定出初始姿态,***的位置和速度可以通过GPS***得到。但是在一些紧急情况下,SINS***需要在运动过程中进行初始对准,不能满足静基座对准的条件。对与低精度的SINS***,例如MEMS-IMU,***本身的噪声大于地球自转角速率,同样无法进行静基座对准,需要在外部传感器辅助的情况下通过设备的运动完成初始对准。
在动基座对准过程中,由于载体在实时的运动,无法像静基座对准一样获取载体系和导航系之间的捷联矩阵,从而导致传统的惯性导航误差方程不再适用。现阶段,动基座对准算法的解决方法可大致分为两类:一种是导航系下使用非线性卡尔曼滤波进行对准的算法;另一种是惯性系下基于Wahba's problem的姿态矩阵求解算法。传统的惯性导航误差方程是建立在静态对准完成后姿态误差是小角的前提下,而导航系下非线性滤波算法则是假设航向角误差是大失准角或是所有姿态角误差均为大失准角,这种情况下建立的***误差模型为非线性,需要使用非线性滤波算法进行估计,且对准的速度与初始姿态误差有很大的关系。导航系下非线性滤波算法没有了传统算法的小失准角假设,误差模型精确,即使有大失准角也能完成动基座对准,但是算法的计算量大且收敛速度慢。与导航系下非线性对准算法不同,惯性系下初始对准算法是将载体系与导航系之间的姿态计算问题转化为惯性系下两惯性坐标系之间的常值姿态矩阵的计算问题,并利用矢量观测对其进行估计,所以收敛速度快,但是该方法原理上是一个开环的算法,不具备对惯性器件误差估计和补偿的能力,因此该算法的误差会随着时间增大,当这种方法用在低精度的惯性导航***中时,对准的精度会降低,且会随着对准时间的延长而逐渐发散。
发明内容
本发明的目的是为了提高基座对准的精度以及收敛性能而提供一种GPS辅助的SINS***快速动基座初始对准方法,本发明对基于优化的对准算法中的误差进行了分析,建立了新的SINS/GPS***误差模型,并利用简化的自适应无迹卡尔曼滤波算法进行滤波估计,将估计结果对惯导解算结果进行反馈校正,从而在保证对准速度的同时提高初始对准的精度和收敛性能,以达到在任意失准角条件下快速动基座对准的目的。
本发明的目的是这样实现的,GPS辅助的SINS***快速动基座初始对准方法,具体包括以下步骤:
步骤1、采集SINS和GPS***输出数据;
步骤2、SINS***对准参数初始化;
步骤3、使用GPS数据和SINS数据分别计算出导航系姿态变化和载体系姿态变化;
步骤4、确定初始载体系和初始导航系下的αv和βv矢量,并根据最小二乘原理估计初始姿态矩阵;
步骤5、选取状态变量,构建SINS/GPS***的状态方程和量测方程;
步骤6、使用简化的自适应无迹卡尔曼滤波算法估计失准角和陀螺仪常值漂移;
步骤7、用估计的失准角对载体系姿态变化矩阵进行反馈,用估计的陀螺仪常值漂移对陀螺仪输出进行校正;
步骤8、使用反馈后的导航系和载体系变化矩阵以及估计的初始姿态矩阵计算姿态矩阵;
步骤9、根据对准时间判断动基座对准是否完成,若没完成则返回步骤1继续对准,若对准完成,则结束动基座对准。
本发明还包括这样一些结构特征:
1、所述步骤3计算出导航系姿态变化和载体系姿态变化具体包括以下步骤:
3.1、导航系姿态变化矩阵更新:
式中,n表示导航系,tk=kT,k是采样次数,T是采样时间,为当前时刻tk到初始0时刻的导航系姿态变化矩阵,为时刻tk-1到初始0时刻的导航系姿态变化矩阵,矩阵为时刻tk到时刻tk-1的导航系姿态变化矩阵,其计算公式为:
式中,vE和vN是东向和北向速度,ωie是地球自转角速率,L和h是纬度和高度,RM和RN是子午圈和卯酉圈主曲率半径;
3.2、载体系姿态变化矩阵更新:
式中,φb是由b(tk-1)至b(tk)的等效旋转矢量,Δθ1和Δθ2是陀螺仪两个相邻时刻的角增量输出。
4.1、矢量计算:
式中,αv(t)是初始载体系下的矢量,βv(t)是初始导航系下的矢量,fb是加速度计输出,gn是重力加速度;
其中I为单位矩阵,矢量αv和βv可以看成标量部分为0的四元数,定义代价函数:
3、所述步骤5中构建SINS/GPS***的状态方程和量测方程具体包括以下步骤:
5.1、选择***状态变量X:
5.2、建立SINS/GPS对准***的状态方程:
式中,f(·)表示非线性状态方程;Wk是均值为零、协方差为Qk的***白噪声,其大小由陀螺仪的噪声决定;
5.3、建立SINS/GPS对准***的量测方程:
Zk=HXk+Vk
式中,量测方程中的Vk是均值为零的量测白噪声,其协方差记为Rk,矩阵Zk和Hk分别为:
4、所述步骤6中简化的自适应无迹卡尔曼滤波算法具体包括以下步骤:
6.1、选择滤波初值和权值Wi (c)以及Wi (m)计算:
式中,是状态变量X在第0时刻估计值即所选状态变量滤波初始值,P0是状态变量误差方差矩阵在0时刻的滤波初始值,n是状态变量的维数,滤波参数α可取值为10-4≤α≤1,滤波参数κ=3-n,当X服从正太分布时,滤波参数β取值为2,λ=α2(n+κ)-n;
6.5、根据式***状态方程和量测方程可看出,***的状态方程为非线性,量测方程为线性,所以在使用UKF算法时可对量测更新部分做进一步简化,减少采样次数和计算量,简化后的量测更新公式为:
6.6、滤波增益Kk计算:
6.8、在进行UKF算法滤波时,量测噪声方差矩阵R设置的不准确会导致***滤波的不稳定,在***建模过程中,量测方程由速度、比力等的积分构成,很难通过手动调试将量测噪声方差矩阵设置为准确值,针对这一问题,需使用Sage-Husa自适应滤波算法对量测噪声方差矩阵R进行实时估计,从而提高***的滤波精度和稳定性,对量测噪声方差矩阵R的估计公式为:
5、所述步骤7用估计的失准角对载体系姿态变化矩阵进行反馈,用估计的陀螺仪常值漂移对陀螺仪输出进行校正具体包括以下步骤:
7.1、姿态失准角反馈:
因为所估计的姿态失准角是小角,所以将反馈公式近似为:
7.2、陀螺仪误差反馈:
εb=εb+[Xk(4:6)]T
6、所述步骤8中姿态矩阵为:
与现有技术相比,本发明的有益效果是:本发明推导了一种新的SINS/GPS***状态方程和量程方程,对算法中的失准角和陀螺仪误差进行估计,进而提高对准的精度以及收敛性能。并且,本发明根据量测方程是积分形式的特点,使用自适应算法对量测噪声进行估计,避免了以往算法中量测噪声协方差矩阵设置不准确的问题,提高了对准算法的稳定性。因为建立的量测方程为线性,本发明对无迹卡尔曼滤波进行化简,减少了对准算法的计算量。
附图说明
图1是本发明GPS辅助的SINS***快速动基座初始对准方法原理图;
图2是仿真运动轨迹;
图3是仿真航向角变化;
图4是本发明FIMA对准算法与OBA算法姿态仿真误差对比;
图5是本发明FIMA对准算法与EKF算法姿态仿真误差对比;
图6是本发明FIMA对准算法和EKF算法估计的常值漂移与理论常值漂移的仿真对比;
图7是跑车实验过程中的运动轨迹;
图8是参考姿态实验变化;
图9是本发明FIMA对准算法与OBA算法姿态实验误差对比;
图10是FIMA对准算法对准姿态实验误差放大图;
图11是FIMA对准算法对陀螺仪常值漂移的实验估计;
图12是纯惯导算法姿态实验误差对比;
图13是本发明FIMA对准算法与EKF算法姿态实验误差对比。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述。
本发明为GPS辅助的SINS***快速动基座初始对准方法,图1是本发明流程图,具体步骤包括:
(1)采集SINS和GPS***输出数据;
(2)SINS***对准参数初始化;
(3)使用GPS数据和SINS数据分别计算出导航系姿态变化和载体系姿态变化;
(4)确定初始载体系和初始导航系下的和矢量,并根据最小二乘原理估计初始姿态矩阵;
(5)选取状态变量,构建SINS/GPS***的状态方程和量测方程;
(6)使用简化的自适应无迹卡尔曼滤波算法估计失准角和陀螺仪常值漂移;
(7)用估计的失准角对载体系姿态变化矩阵进行反馈,用估计的陀螺仪常值漂移对陀螺仪输出进行校正;
(8)使用反馈后的相应参数计算姿态矩阵,实现动基座对准。
本发明GPS辅助的SINS***快速动基座初始对准方法还包括:
1.计算导航系和载体系姿态变化
(1)导航系姿态变化矩阵更新:
其中,n表示导航系,tk=kT,k是采样次数,T是采样时间,为当前时刻tk到初始0时刻的导航系姿态变化矩阵,为时刻tk-1到初始0时刻的导航系姿态变化矩阵,矩阵为时刻tk到时刻tk-1的导航系姿态变化矩阵,其计算公式为:
的角速率,φn×是φn的反对称矩阵,||φn||是φn的模,其计算公式为:
其中,vE和vN是东向和北向速度,ωie是地球自转角速率,L和h是纬度和高度,RM和RN是子午圈和卯酉圈主曲率半径。
(2)载体系姿态变化矩阵更新:
(1)矢量计算:
其中,αv(t)是初始载体系下的矢量,βv(t)是初始导航系下的矢量,fb是加速度计输出,gn是重力加速度。
其中I为单位矩阵,矢量αv和βv可以看成标量部分为0的四元数,定义代价函数:
3.构建SINS/GPS***状态方程和量测方程
(1)选择***状态变量X:
(2)建立SINS/GPS对准***的状态方程:
其中,f(·)表示非线性状态方程;Wk是均值为零、协方差为Qk的***白噪声,其大小由陀螺仪的噪声决定。
(3)建立SINS/GPS对准***的量测方程:
Zk=HXk+Vk
其中,量测方程中的Vk是均值为零的量测白噪声,其协方差记为Rk,矩阵Zk和Hk分别为:
4.简化的自适应无迹卡尔曼滤波算法
(1)选择滤波初值和权值Wi (c)以及Wi (m)计算:
其中,是状态变量X在第0时刻估计值即所选状态变量滤波初始值,P0是状态变量误差方差矩阵在0时刻的滤波初始值,n是状态变量的维数,滤波参数α可取值为10-4≤α≤1,滤波参数κ=3-n,当X服从正太分布时,滤波参数β取值为2,λ=α2(n+κ)-n。
(5)根据式***状态方程和量测方程可以看出,***的状态方程为非线性,量测方程为线性,所以在使用UKF算法时可以对量测更新部分做进一步简化,减少采样次数和计算量。简化后的量测更新公式为:
(6)滤波增益Kk计算:
(8)在进行UKF算法滤波时,量测噪声方差矩阵R设置的不准确会导致***滤波的不稳定。在***建模过程中,量测方程是由速度、比力等的积分构成的,很难通过手动调试将量测噪声方差矩阵设置为准确值。针对这一问题,需使用Sage-Husa自适应滤波算法对量测噪声方差矩阵R进行实时估计,从而提高***的滤波精度和稳定性。对量测噪声方差矩阵R的估计公式为:
5.状态估计的反馈和初始对准姿态的输出
(1)姿态失准角反馈:
因为所估计的姿态失准角是小角,所以可以将反馈公式近似为:
(2)陀螺仪误差反馈:
εb=εb+[Xk(4:6)]T
(3)初始对准姿态输出:
本发明是在分析了现有的OBA算法中存在的误差后进行***建模,然后结合无迹卡尔曼滤波进行误差的估计和补偿,从而提高OBA算法在低精度SINS***中应用时的精度和收敛性能,同时本发明是利用无迹卡尔曼滤波对误差的估计,不会影响姿态的收敛速度。所以本发明在利用MATLAB软件仿真和实验设备实验时,将本发明与现有的OBA算法和EKF动态对准算法进行了比较。
本发明实施例用来解释本发明,而不是对本发明进行限制,在发明的精神和权利要求的保护范围内,对本发明做出的任何修改和改变,都落入本发明的保护范围。
一、用MATLAB仿真对本发明进行验证:
仿真参数设置为:陀螺仪的常值漂移和随机噪声分别为1°/h和0.1°/h,加速度计的常值漂移和随机噪声分别为100ug和50ug,陀螺仪和加速度计的输出频率均为100Hz;GPS***的速度误差为0.05m/s,位置误差为5m,GPS***的输出频率为10Hz。SINS***的初始状态设置为:SINS***的初始位置为北纬45°和东经126°,初始姿态为航向角45°、俯仰角0°和横滚角0°,初始速度为东向速度5m/s、北向速度5m/s和天向速度0m/s。SINS***的状态变化为:载体匀速运动50s→左转弯90°,用时5s→匀速运动50s→右转弯90°,用时5s→匀速运动50s→左转弯90°,用时5s→匀速运动50s→右转弯90°,用时5s→匀速运动50s,总的运动时长为270s。图2为载体运动轨迹变化,图3为载体的航向角变化。
图4为仿真过程中OBA算法与所提出的FIMA算法的姿态误差对比图,由图中效果可以看出,使用所提出的FIMA算法航向角误差在80s可以收敛到0.2°,并且航向角误差能够始终保持在0.2°以内,而OBA算法虽然也可以快速收敛,但是其航向角误差会随着对准时间的延长而变大。图中两算法的俯仰角误差和横滚角误差均能快速收敛,且收敛的精度相差不大,但是FIMA算法的水平姿态误差更稳定。
图5为FIMA算法与EKF动基座对准算法的姿态误差对比图,因为FIMA算法针对的是任意初始姿态,所以为了对比算法的效果,在仿真时EKF算法的姿态初始误差设置为航向角误差50°、俯仰角误差15°和横滚角误差15°的大失准角误差。由图中效果可以看出,与非线性EKF动态对准算法相比,当姿态初始值存在大失准角时,FIMA算法的航向角对准速度更快,水平姿态的对准效果更稳定。
图6是FIMA算法与EKF算法关于陀螺仪零偏估计的对比图,EKF算法的仿真条件与姿态误差对比的仿真条件相同。图5中,黑色虚线是仿真轨迹中加入的陀螺仪常值漂移,大小为1°/h,图中的零偏估计效果显示,FIMA算法对陀螺仪x轴和y轴的估计精度高于EKF算法,而两种算法对z轴陀螺仪零偏的估计都不太准确,这一现象与仿真轨迹运动方式的设计过于单一有关。图5的仿真效果说明,FIMA算法可以实现对陀螺仪零偏的估计。
二、对本发明进行跑车实验验证:
为验证本发明的动基座对准性能,跑车实验使用ADIS16488生产的MEMS惯性器件作为SINS***进行动基座对准,实验中以高精度的光纤惯导***作为姿态参考***,图7为跑车实验过程中汽车的运动轨迹,图8为运动过程中参考姿态的变化,整个实验过程大约1300s,汽车在180s开始运动,算法在汽车运动后13s开始运行。
由图9中姿态误差可以看出,对于低精度的SINS***,OBA算法虽然能够快速的确定出姿态角,但是姿态角并没有随着时间的延长而趋于稳定,且航向角误差会越来越大;本发明中的FIMA则克服了OBA算法的这一缺点,能够保证算法在低精度的SINS***中依然能够稳定的收敛。图10为FIMA算法姿态误差放大图,由图中航向角误差可以知道,在没有任何初始姿态信息的条件下,算法用15s的时间便可将航向角误差收敛到3°以内,俯仰角误差和横滚角误差收敛到0.4°以内。在FIMA对准算法运行190s以后,航向角误差可收敛到1°以内。
图11为FIMA算法对陀螺仪零偏的估计结果,估计结果表明,FIMA算法能够实现对陀螺仪零偏的估计。为证明零偏估计的有效性,将FIMA算法估计的陀螺仪零偏写入纯惯导算法中进行补偿,补偿后的效果如图12中所示。经过陀螺仪误差补偿后的纯惯导算法的航向角误差明显好于补偿前的效果,陀螺仪误差的补偿对水平姿态误差影响不大,由此可以看出,FIMA算法对陀螺仪零偏的估计是有效的。
为了体现本文中算法在任意失准角条件下快速对准的优越性,本文对FIMA算法与EKF对准算法用跑车实测数据进行比较。在EKF对准算法运行前,设置航向角误差70°、俯仰角误差和横滚角误差20°的大失准角误差。两种算法的姿态误差对比如图13所示,由于大失准角的原因,EKF算法的航向误差收敛速度明显比FIMA算法慢,EKF算法的水平姿态误差在对准的前期也不如FIMA算法的稳定。
综上,本发明提供了一种全球定位***(GPS)辅助的捷联式惯导***(SINS)快速动基座对准(Fast In-motion Alignment.FIMA)方法,主要解决SINS***运动条件下的初始对准问题。对于低精度的微机电***(MEMS),基于优化的对准算法(OBA)无法估计陀螺仪误差,会导致初始对准精度降低。本发明对陀螺仪误差的影响进行分析,建立了OBA算法对准过程中失准角误差方程和量测方程,同时利用简化的自适应无迹卡尔曼滤波算法(AUKF)对失准角和陀螺仪常值漂移进行估计,将估计的失准角和陀螺仪常值漂移对SINS***对准的姿态和陀螺仪的输出进行矫正,从而实现SINS***的快速动基座初始对准。
Claims (2)
1.GPS辅助的SINS***快速动基座初始对准方法,其特征在于,具体包括以下步骤:
步骤1、采集SINS和GPS***输出数据;
步骤2、SINS***对准参数初始化;
步骤3、使用GPS数据和SINS数据分别计算出导航系姿态变化和载体系姿态变化;
3.1、导航系姿态变化矩阵更新:
式中,n表示导航系,tk=kT,k是采样次数,T是采样时间,为当前时刻tk到初始0时刻的导航系姿态变化矩阵,为时刻tk-1到初始0时刻的导航系姿态变化矩阵,矩阵为时刻tk到时刻tk-1的导航系姿态变化矩阵,其计算公式为:
式中,vE和vN是东向和北向速度,ωie是地球自转角速率,L和h是纬度和高度,RM和RN是子午圈和卯酉圈主曲率半径;
3.2、载体系姿态变化矩阵更新:
式中,φb是由b(tk-1)至b(tk)的等效旋转矢量,Δθ1和Δθ2是陀螺仪两个相邻时刻的角增量输出;
步骤4、确定初始载体系和初始导航系下的αv和βv矢量,并根据最小二乘原理估计初始姿态矩阵;
4.1、矢量计算:
式中,αv(t)是初始载体系下的矢量,βv(t)是初始导航系下的矢量,fb是加速度计输出,gn是重力加速度;
其中I为单位矩阵,矢量αv和βv看成标量部分为0的四元数,定义代价函数:
步骤5、选取状态变量,构建SINS/GPS***的状态方程和量测方程;
5.1、选择***状态变量X:
5.2、建立SINS/GPS对准***的状态方程:
式中,f(·)表示非线性状态方程;Wk是均值为零、协方差为Qk的***白噪声,其大小由陀螺仪的噪声决定;
5.3、建立SINS/GPS对准***的量测方程:
Zk=HkXk+Vk
式中,量测方程中的Vk是均值为零的量测白噪声,其协方差记为Rk,矩阵Zk和Hk分别为:
步骤6、使用简化的自适应无迹卡尔曼滤波算法估计失准角和陀螺仪常值漂移;
6.1、选择滤波初值和权值Wi (c)以及Wi (m)计算:
式中,是状态变量X在第0时刻估计值即所选状态变量滤波初始值,P0是状态变量误差方差矩阵在0时刻的滤波初始值,n是状态变量的维数,滤波参数α取值为10-4≤α≤1,滤波参数κ=3-n,当X服从正太分布时,滤波参数β取值为2,λ=α2(n+κ)-n;
6.5、根据式***状态方程和量测方程,***的状态方程为非线性,量测方程为线性,使用UKF算法时对量测更新部分做进一步简化,减少采样次数和计算量,简化后的量测更新公式为:
6.6、滤波增益Kk计算:
6.8、进行UKF算法滤波时,量测噪声方差矩阵R设置的不准确会导致***滤波的不稳定,在***建模过程中,量测方程由速度、比力的积分构成,很难通过手动调试将量测噪声方差矩阵设置为准确值,针对这一问题,需使用Sage-Husa自适应滤波算法对量测噪声方差矩阵R进行实时估计,从而提高***的滤波精度和稳定性,对量测噪声方差矩阵R的估计公式为:
步骤7、用估计的失准角对载体系姿态变化矩阵进行反馈,用估计的陀螺仪常值漂移对陀螺仪输出进行校正;
7.1、姿态失准角反馈:
因为所估计的姿态失准角是小角,所以将反馈公式近似为:
7.2、陀螺仪误差反馈:
εb=εb+[Xk(4:6)]T
步骤8、使用反馈后的导航系和载体系变化矩阵以及估计的初始姿态矩阵计算姿态矩阵;
步骤9、根据对准时间判断动基座对准是否完成,若没完成则返回步骤1继续对准,若对准完成,则结束动基座对准。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910644265.8A CN110398257B (zh) | 2019-07-17 | 2019-07-17 | Gps辅助的sins***快速动基座初始对准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910644265.8A CN110398257B (zh) | 2019-07-17 | 2019-07-17 | Gps辅助的sins***快速动基座初始对准方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110398257A CN110398257A (zh) | 2019-11-01 |
CN110398257B true CN110398257B (zh) | 2022-08-02 |
Family
ID=68324520
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910644265.8A Active CN110398257B (zh) | 2019-07-17 | 2019-07-17 | Gps辅助的sins***快速动基座初始对准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110398257B (zh) |
Families Citing this family (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111307114B (zh) * | 2019-11-29 | 2022-09-27 | 哈尔滨工程大学 | 基于运动参考单元的水面舰船水平姿态测量方法 |
CN111290007A (zh) * | 2020-02-27 | 2020-06-16 | 桂林电子科技大学 | 一种基于神经网络辅助的bds/sins组合导航方法和*** |
CN111397603B (zh) * | 2020-04-24 | 2022-07-12 | 东南大学 | 载体姿态动态情况下的惯性/多普勒动基座粗对准方法 |
CN111337056B (zh) * | 2020-05-19 | 2020-08-25 | 北京数字绿土科技有限公司 | 基于优化的LiDAR运动补偿位置姿态***对准方法 |
CN111707292B (zh) * | 2020-07-18 | 2022-04-08 | 东南大学 | 一种自适应滤波的快速传递对准方法 |
CN112013873A (zh) * | 2020-08-13 | 2020-12-01 | 哈尔滨工业大学 | 一种基于梯度下降优化的静基座快速对准方法 |
CN112882118B (zh) * | 2020-12-30 | 2022-12-06 | 中国人民解放军海军工程大学 | 地固坐标系下动基座重力矢量估计方法、***及存储介质 |
CN113175926B (zh) * | 2021-04-21 | 2022-06-21 | 哈尔滨工程大学 | 一种基于运动状态监测的自适应水平姿态测量方法 |
CN113503891B (zh) * | 2021-04-22 | 2022-11-29 | 中国人民解放军海军工程大学 | 一种sinsdvl对准校正方法、***、介质及设备 |
CN113670335B (zh) * | 2021-08-18 | 2023-10-24 | 河海大学 | 基于dvl辅助和向量截断化k矩阵的水下载体初始对准方法 |
CN113834499A (zh) * | 2021-08-26 | 2021-12-24 | 北京航天发射技术研究所 | 一种车载惯组与里程计行进间对准方法及*** |
CN113916261B (zh) * | 2021-10-09 | 2023-06-27 | 上海交通大学 | 基于惯性导航优化对准的姿态误差评估方法 |
CN114061621B (zh) * | 2021-11-11 | 2023-11-17 | 东南大学 | 一种基于强跟踪动机座旋转调制大失准角初始对准方法 |
CN114111771A (zh) * | 2021-11-25 | 2022-03-01 | 九江中船仪表有限责任公司(四四一厂) | 一种双轴稳定平台的动态姿态测量方法 |
CN114264304B (zh) * | 2021-12-23 | 2023-07-14 | 湖南航天机电设备与特种材料研究所 | 复杂动态环境高精度水平姿态测量方法与*** |
CN116182900B (zh) * | 2022-12-16 | 2023-09-19 | 哈尔滨工程大学 | 纬度未知条件下动基座大失准角一体化对准方法 |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106840211A (zh) * | 2017-03-24 | 2017-06-13 | 东南大学 | 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法 |
Family Cites Families (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
GB2391732B (en) * | 2002-05-16 | 2005-09-07 | Furuno Electric Co | Attitude sensing apparatus for determining the attitude of a mobile unit |
JP2007232444A (ja) * | 2006-02-28 | 2007-09-13 | Yokogawa Electric Corp | 慣性航法装置およびその誤差補正方法 |
CN102221366B (zh) * | 2011-03-11 | 2013-07-03 | 哈尔滨工程大学 | 一种基于模糊变地球自转角速度的快速精对准方法 |
CN102706366B (zh) * | 2012-06-19 | 2015-02-25 | 北京航空航天大学 | 一种基于地球自转角速率约束的sins初始对准方法 |
CN103471616B (zh) * | 2013-09-04 | 2016-01-27 | 哈尔滨工程大学 | 一种动基座sins大方位失准角条件下初始对准方法 |
CN105371871B (zh) * | 2015-12-02 | 2018-01-02 | 中国矿业大学 | 井下采煤机捷联惯导***的组合初始对准***及对准方法 |
CN107479076B (zh) * | 2017-08-08 | 2020-11-03 | 北京大学 | 一种动基座下联合滤波初始对准方法 |
CN107830872A (zh) * | 2017-10-26 | 2018-03-23 | 哈尔滨工程大学 | 一种舰船捷联惯性导航***自适应初始对准方法 |
US11422004B2 (en) * | 2017-12-22 | 2022-08-23 | Mohammad Shakibay Senobari | Initializing an inertial measurement unit |
CN108981696B (zh) * | 2018-08-01 | 2022-02-18 | 西北工业大学 | 一种sins任意失准角无奇异快速传递对准方法 |
CN109556632B (zh) * | 2018-11-26 | 2021-01-29 | 北方工业大学 | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 |
-
2019
- 2019-07-17 CN CN201910644265.8A patent/CN110398257B/zh active Active
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106840211A (zh) * | 2017-03-24 | 2017-06-13 | 东南大学 | 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法 |
Also Published As
Publication number | Publication date |
---|---|
CN110398257A (zh) | 2019-11-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110398257B (zh) | Gps辅助的sins***快速动基座初始对准方法 | |
CN109211276B (zh) | 基于gpr与改进的srckf的sins初始对准方法 | |
CN107525503B (zh) | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 | |
US11187535B1 (en) | Attitude measurement method | |
CN111024064B (zh) | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 | |
CN109163735B (zh) | 一种晃动基座正向-正向回溯初始对准方法 | |
CN109596144B (zh) | Gnss位置辅助sins行进间初始对准方法 | |
CN109945895B (zh) | 基于渐消平滑变结构滤波的惯性导航初始对准方法 | |
CN103822633A (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN110954102B (zh) | 用于机器人定位的磁力计辅助惯性导航***及方法 | |
CN101949703A (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
CN113063429B (zh) | 一种自适应车载组合导航定位方法 | |
CN104344837A (zh) | 一种基于速度观测的冗余惯导***加速度计***级标定方法 | |
CN110702113B (zh) | 基于mems传感器的捷联惯导***数据预处理和姿态解算的方法 | |
CN110851776B (zh) | 一种高动态变转速载体的姿态解算方法 | |
Xue et al. | In-motion alignment algorithm for vehicle carried SINS based on odometer aiding | |
CN111189442A (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
CN111795708B (zh) | 晃动基座条件下陆用惯性导航***的自适应初始对准方法 | |
CN111190207B (zh) | 基于pstcsdref算法的无人机ins bds组合导航方法 | |
CN110285830B (zh) | 基于mems传感器的sins/gps速度匹配对准方法 | |
CN116429095A (zh) | 一种基于主子惯导结合的行进间炮口振动测量方法 | |
CN114061574B (zh) | 一种基于位置不变约束及零速校正的采煤机定姿定向方法 | |
Zarei et al. | Performance improvement for mobile robot position determination using cubature Kalman filter | |
CN114111840B (zh) | 一种基于组合导航的dvl误差参数在线标定方法 | |
CN112683265B (zh) | 一种基于快速iss集员滤波的mimu/gps组合导航方法 |
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 |