CN114061621B - 一种基于强跟踪动机座旋转调制大失准角初始对准方法 - Google Patents
一种基于强跟踪动机座旋转调制大失准角初始对准方法 Download PDFInfo
- Publication number
- CN114061621B CN114061621B CN202111330426.XA CN202111330426A CN114061621B CN 114061621 B CN114061621 B CN 114061621B CN 202111330426 A CN202111330426 A CN 202111330426A CN 114061621 B CN114061621 B CN 114061621B
- Authority
- CN
- China
- Prior art keywords
- fit
- matrix
- particles
- follows
- pbest
- 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
- 238000000034 method Methods 0.000 title claims abstract description 36
- 239000002245 particle Substances 0.000 claims abstract description 41
- 238000005562 fading Methods 0.000 claims abstract description 35
- 238000000546 chi-square test Methods 0.000 claims abstract description 5
- 238000005457 optimization Methods 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 62
- 230000006870 function Effects 0.000 claims description 32
- 238000011156 evaluation Methods 0.000 claims description 30
- 239000013598 vector Substances 0.000 claims description 25
- 238000005259 measurement Methods 0.000 claims description 16
- 238000001914 filtration Methods 0.000 claims description 15
- 238000004364 calculation method Methods 0.000 claims description 11
- 238000001514 detection method Methods 0.000 claims description 6
- 238000013461 design Methods 0.000 claims description 4
- 230000008569 process Effects 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 3
- 238000009826 distribution Methods 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 2
- 238000010845 search algorithm Methods 0.000 claims 1
- 230000000694 effects Effects 0.000 abstract description 2
- 230000007547 defect Effects 0.000 abstract 1
- 238000004422 calculation algorithm Methods 0.000 description 7
- 238000005516 engineering process Methods 0.000 description 5
- 230000003044 adaptive effect Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000005295 random walk Methods 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
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
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于强跟踪动机座旋转调制大失准角初始对准方法。首先,基于动基座大失准角快速初始对准的需求,推导了基于旋转调制的无迹卡尔曼滤波模型(UKF),其次,研究了基于卡方检验的全维数渐消因子强跟踪卡尔曼滤波器,提高滤波器的抗干扰能力及鲁棒性,针对传统渐消因子求解精度较差的缺陷,建立基于改进的具有全局和局部搜索能力的粒子群算法(PSO)的渐消因子高精度迭代求解方法。通过本发明方法对具有确定参数单轴旋转调制动机座大失准角初始对准实验数据进行验证,取得效果能够满足高精度初始对准要求。
Description
技术领域
本发明属于惯性导航领域,具体涉及一种基于强跟踪动机座旋转调制大失准角初始对准方法,针对无人艇的动基座大失准角初始对准耗时长、精度差等特征,实现快速、良好的对准精度。
背景技术
捷联惯导***是一种自主、信息量全的导航***,初始对准是捷联惯导***的关键技术之一。初始对准的精度影响捷联惯导***的导航精度,初始对准的速度影响载体的快速反应能力。在恶劣海况中,无人艇的初始对准需要面临动基座扰动及数学模型不准确等问题。常规初始对准分为粗对准和精对准,而常规粗对准在极端条件下(如突然的大机动和大侧滑)无法满足精对准对初始姿态的小角度要求。另一种方法是将非线性方程转化为线性的误差模型,以减少对粗对准的依赖,但此方法要求水平姿态角为小角度。非线性卡尔曼滤波模型如EKF(扩展卡尔曼滤波),UKF(无迹卡尔曼滤波)及CKF(容积卡尔曼滤波)是解决大失准角初始对准问题的常用方法,但上述方法在基于MEMS的单轴旋转调制捷联惯导初始对准上的相关应用较少。由于MEMS(微机电***)噪声具有时变特性以及旋转调制带来的高动态干扰成分,卡尔曼滤波模型的***及量测噪声统计特性不能准确获取,这将导致滤波模型精度下降甚至发散。AKF(自适应卡尔曼滤波)及SKF(强跟踪卡尔曼滤波)是解决卡尔曼滤波模型失配的常用方法,但不适用于非线性模型。因此,需要建立基于非线性卡尔曼滤波与自适应卡尔曼滤波融合模型,以实现大失准角动基座高精度初始对准。在对准时间方面,国内外舰载捷联惯导***快速对准时间可控制在10分钟以内,但针对复杂海况下的无人艇快速初始对准研究及应用较为缺乏。
近年来,MEMS捷联惯导因其低成本、小体积以及高可靠性等特征,成为无人艇导航定位装置的理想选择,但MEMS陀螺存在随机噪声大、零偏大,零偏易受环境温度影响等问题,严重影响了其初始对准精度。当前,提高MEMS捷联惯导精度的方法主要有两种:①从结构工艺上出发,研发新型的、性能更高的惯性器件;②采用“***级自补偿技术”减小陀螺及加速度计误差对惯导***精度的影响。针对捷联惯导***本身,继续从结构和工艺上提高其器件精度,技术难度大,研发周期长,成本较高,且难有突破。而“***级自补偿技术”基于当前器件,运用旋转辅助机构与算法有效提高精度,是一种经济实效的方法。旋转调制技术是一种消除陀螺常值漂移的有效***级自补偿方法,主要分为单轴、双轴及三轴旋转调制。双轴及三轴旋转调制***虽能够调制惯性器件所有轴的常值漂移,但其结构复杂,制造成本及工艺要求较高,主要装备于大型舰船及潜艇上;单轴旋转调制***能够调制除旋转轴方向外的惯性器件常值漂移,结构简单,体积紧凑,性能可靠,工程上较易实现,因此更适用于无人艇的应用需求。初始对准中,天向失准角可观测度较低,求解时间长,精度差,其最终对准精度由等效东向陀螺漂移决定。单轴旋转调制***可调制陀螺东向及北向漂移,有效提高天向失准角求解精度,同时可提高天向失准角的可观测度,缩短对准时间。
单轴旋转调制技术可有效提高初始对准精度,但旋转***会在陀螺输入轴中带入高动态干扰成分,改变陀螺原有随机误差特性,同时无人艇的非常规运动(突然的大机动和大侧滑)会带来很强的干扰项,严重影响滤波模型的准确性,甚至引起***发散。SKF模型可有效补偿干扰项,其基本思想是通过渐消因子调节状态估计误差的均方误差阵以强迫残差序列相互正交,从而提取新息的有用信息,同时滤除有害的干扰项。但当前SKF的渐消因子求解维数依赖于量测量的维数,其有效维数远远小于状态估计误差的均方误差阵的维数,因此不能有效调节状态估计误差的均方误差阵。因此,需研究全维渐消因子的求解方法,以提高SKF的滤波精度。
提高初始对准的实时计算精度也可以在硬件上进行改进,例如采用高性能CPU及GPU的计算机,这种方案在一定程度上可以提高算法的更新迭代速度,却增加了硬件的复杂性,提高了成本,不利于在实践中进行推广应用。
发明内容
为解决上述问题,本发明公开了一种基于强跟踪动机座旋转调制大失准角初始对准方法,对具有确定参数单轴旋转调制动机座大失准角初始对准方法进行验证,取得效果能够满足高精度初始对准要求。
为达到上述目的,本发明的技术方案如下:
一种基于强跟踪动机座旋转调制大失准角初始对准方法,包括下列步骤:
步骤1),绕Z轴(天向轴)旋转的单轴旋转矩阵转换模型如下:
其中,s系为MEMS惯性单元坐标系,b为载体坐标系,Ω为旋转调制角速度。
步骤2),建立单轴旋转调制UKF卡尔曼滤波模型
步骤2.1),建立非线性状态方程如下:
其中,n与n'分别为理想导航坐标系与实际数学平台坐标系,i与e分别为惯性坐标系与地心坐标系,是n系到n'系的旋转矩阵,/>与/>分别表示n系相对于i系的角速度计算值与计算误差,/>是b系到n'系的旋转矩阵,δvn为速度误差,/>表示加速度测量值,/>表示地球自转,/>表示n系相对于e系的角速度计算值,I表示单位矩阵,εs与/>分别表示陀螺常值漂移与加速度计常值偏置,/>与/>分别表示陀螺与加速度计的零均值的高斯白噪声,α及Cω计算式如下:
α=[αx,αy,αz]T (3)
αx,αy及αz表示表示n系到n'系的三次转动角。
步骤2.2),依据式(2)建立卡尔曼滤波方程如下:
及/>是由MEMS及GNSS(全球卫星定位***)计算的载体在n系中的线速度,f(x)与g(x)由式(2)计算得到,状态向量/>噪声向量H=[0 I 0 0]T,v为量测噪声向量。
步骤2.3),建立离散UKF卡尔曼滤波模型如下:
其中,与/>分别是是k-1时刻与k时刻的状态估计值,/>为一步状态估计值,为一步量测预测误差,/>为量测预测误差,zk为量测,Pk-1为预测均方误差阵,Pk/k-1为一步预测均方误差阵,/>为状态预测误差与量测预测误差之间协方差阵,/>为量测预测误差均方差阵,Qk-1量测噪声分配矩阵,Rk为量测状态噪声,Hk为量测转移矩阵,Kk为增益矩阵,/>为由/>组成的L列矩阵,χk-1为sigma点矩阵,L,Wi (m)与Wi (c)为待求解相关系数。
步骤3),建立多渐消因子强跟踪卡尔曼滤波器模型
步骤3.1)强跟踪卡尔曼滤波器的策略是是通过渐消因子调整Pk/k-1以保持残差相互正交,抑制外部干扰,防止***发散。k时刻的新息矢量为:
步骤3.2)k时刻新息矢量的协方差阵估计值为:
其中,C0,k计算方法如下:
其中,b为常值系数,其取值范围一般为:0.8~0.9。
步骤3.3),建立基于多渐消因子的Pk/k-1的修正公式为:
其中,λk=diag(s1,s2,…sn)为渐消因子组成的对角矩阵,si≥1,i=1,2,…n,n为状态向量的维数。
步骤3.4)新息序列互不相关的等价条件是:
其中,符号E表示求期望。式(11)的等价形式为:
强跟踪卡尔曼滤波的原理即通过选取合适的λk使式(12)近似成立。
步骤4),建立基于卡方检验的异常检测准则如下:
其中,为检测阈值,α为置信水平,m为自由度,θk为滤波***计算的卡方值,其计算方法如下:
步骤5),定义目标矩阵如下:
代入式(10)到式(15)得:
调整渐消因子矩阵λk使矩阵h(k)所有元素绝对值取值最小,使等式(12)近似成立,则设计评价函数的等价形式为:
其中,n和m分别为矩阵h(k)行数及列数,sij是矩阵h(k)的元素。则渐消因子的寻优过程即使fit取最小值。
步骤6),建立基于改进PSO算法的多渐消因子更新方法如下:
步骤6.1),随机初始化M个n维粒子的位置与移动速度:Xi及Vi,i=1,2,…,M。
步骤6.2),运用式(18)计算M个粒子对应的渐消因子矩阵,运用式(17)计算M个粒子对应的评价函数值fit(Xi),记录M个粒子的全局最优位置为gbest及其评价函数值fit(gbest),存储M个粒子的当前位置pbesti及其评价函数值fit(pbesti),其中pbesti=Xi,i=1,2,…,M。
λk(j,j)=Vi(j),j=1,2,…n (18)
步骤6.3),随机生成M个粒子的左、右位置如下:
其中,LXi为左位置,RXi为右位置,dt为移动步距,τi为n维单位向量,计算粒子左、右位置对应的评价函数值fit(LXi)及fit(RXi)。按如下原则更新M个粒子的新位置:
若fit(LXi)<fit(Xi),则Xi=LXi,fit(Xi)=fit(LXi);
若fit(RXi)<fit(Xi),则Xi=RXi,fit(Xi)=fit(RXi)。
步骤6.4),更新M个粒子的全局最优位置gbest及其评价函数值fit(gbest);按如下准则更新M个粒子的最优历史位置pbesti及其评价函数值fit(pbesti):
若fit(Xi)<fit(pbesti),则pbesti=Xi,fit(pbesti)=fit(Xi)。
步骤6.5):更新M个粒子的移动速度如下:
其中,c1与c2为学习因子,为惯性系数,rand1与rand2为随机生成的n维单位向量。
按式(21)计算M个粒子的新位置Xi,并计算其对应的评价函数值fit(Xi):
Xi=Xi+Vi (21)
步骤6.6):依据步骤6.4)更新M个粒子的全局最优位置gbest及其评价函数值fit(gbest),更新M个粒子的历史最优位置pbesti及其对应评价函数值fit(pbesti)。
步骤6.7),若达到停止条件,则结束迭代循环并输出渐消因子,即λk(i,i)=gbest(i),i=1,2,…n。否则返回步骤6.3)继续下一个循环。
本发明的有益效果是:
1.克服了常规动机座大失准角初始对准耗时长,对准精确较差等问题,本发明所述初始对准策略不依赖于粗对准,可以适用复杂海况下小型舰艇的初始对准,其对准时间短,且对准精度有显著提高;
2.对传统的强跟踪滤波器而言,其渐消因子的有效维数取决于量测量的数量,其调节能力有限,限制了滤波器的抗干扰能力,本发明所述基于改进PSO求解渐消因子的方法,可实现全维数渐消因子的求解,有效提高了强跟踪滤波器的性能;
3.本发明所述的基于卡方检验的渐消因子计算策略,克服了常规强跟踪滤波器渐消因子过度调节等问题,且有效提高了计算效率;
4.本发明所述添加随机左、右位置等策略的改进PSO算法,可克服常规PSO算法易陷入局部极小值等问题,使其同时具有较强的全局和局部搜索能力。
附图说明
图1是本发明方法的算法设计流程图。
图2是本发明方法的单轴旋转调制原理图。
图3是本发明方法东向失准角估计误差曲线。
图4是本发明方法北向失准角估计误差曲线。
图5是本发明方法天向失准角估计误差曲线。
具体实施方式
下面结合附图和具体实施方式,进一步阐明本发明,应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
如图1所示,本发明公开了一种基于强跟踪动机座旋转调制大失准角初始对准方法,包括以下步骤:
步骤1),首先推导单轴旋转调制原理,如图2所示。沿Z轴的单轴旋转调制矩阵转换模型如下:
其中,s系为MEMS单元坐标系,b为载体坐标系,Ω为调制旋转角速度。陀螺及加速度计的常值误差在导航坐标系n系中经过调制后为:
其中,为陀螺常值漂移向量,/>为加速度计常值偏置向量。E,N及U分别表示东,北,天方向。
步骤2),建立单轴旋转调制UKF卡尔曼滤波模型
步骤2.1),建立非线性状态方程如下:
其中,n与n'分别为理想导航坐标系与实际数学平台坐标系,i与e分别为惯性坐标系与地心坐标系,是n系到n'系的旋转矩阵,/>与/>分别表示n系相对于i系的角速度计算值与计算误差,/>是b系到n'系的旋转矩阵,δvn为速度误差,/>表示加速度测量值,/>表示地球自转,/>表示n系相对于e系的角速度计算值,I表示单位矩阵,εs与/>分别表示陀螺常值漂移与加速度计常值偏置,/>与/>分别表示陀螺与加速度计的零均值的高斯白噪声,α及Cω计算式如下:
α=[αx,αy,αz]T (5)
αx,αy及αz表示表示n系到n'系的三次转动角。
步骤2.2),依据式(4)建立卡尔曼滤波方程如下:
及/>是由MEMS及GNSS(全球卫星定位***)计算的载体在n系中的线速度,f(x)与g(x)由式(4)计算得到,状态向量/>噪声向量H=[0 I 0 0]T,v为量测噪声向量。
步骤2.3),建立离散UKF卡尔曼滤波模型如下:
其中,与/>分别是是k-1时刻与k时刻的状态估计值,/>为一步状态估计值,为一步量测预测误差,/>为量测预测误差,zk为量测,Pk-1为预测均方误差阵,Pk/k-1为一步预测均方误差阵,/>为状态预测误差与量测预测误差之间协方差阵,/>为量测预测误差均方差阵,Qk-1量测噪声分配矩阵,Rk为量测状态噪声,Hk为量测转移矩阵,Kk为增益矩阵,为由/>组成的L列矩阵,χk-1为sigma点矩阵,L为状态量的维数,Wi (m)与Wi (c)为待求解相关系数,其计算方法如下:
其中,比例因子a,κ与β分别取值为:0.01,0和2。
步骤3),建立多渐消因子强跟踪卡尔曼滤波器模型
步骤3.1)强跟踪卡尔曼滤波器的策略是是通过渐消因子调整Pk/k-1以保持残差相互正交,抑制外部干扰,防止***发散。k时刻的新息矢量为:
步骤3.2)k时刻新息矢量的协方差阵估计值为:
其中,C0,k计算方法如下:
其中,b为常值系数,其取值为:0.85。
步骤3.3),建立基于多渐消因子的Pk/k-1的修正公式为:
其中,λk=diag(s1,s2,…sn)为渐消因子组成的对角矩阵,si≥1,i=1,2,…n,n为状态向量的维数,本方法中n=12。
步骤3.4)新息序列互不相关的等价条件是:
其中,符号E表示求期望。式(18)的等价形式为:
强跟踪卡尔曼滤波的原理即通过选取合适的λk使式(19)近似成立。
步骤4),建立基于卡方检验的异常检测准则如下:
其中,为检测阈值,取值为7.95;α为置信水平,取值为0.05;m为自由度,取值为3;θk为滤波***计算的卡方值,其计算方法如下:
步骤5),定义目标矩阵如下:
代入式(17)到式(22)得:
调整渐消因子矩阵λk使矩阵h(k)所有元素绝对值取值最小,使等式(19)近似成立,则设计评价函数的等价形式为:
其中,n和m分别为矩阵h(k)行数及列数,此方法中m=n=12;sij是矩阵h(k)的元素。则渐消因子的寻优过程即使fit取最小值。
步骤6),建立基于改进PSO算法的多渐消因子更新方法如下:
步骤6.1),随机初始化M个n维粒子的位置与移动速度:Xi及Vi,i=1,2,…,M,此方法中M=60。
步骤6.2),运用式(25)计算M个粒子对应的渐消因子矩阵,运用式(24)计算M个粒子对应的评价函数值fit(Xi),记录M个粒子的全局最优位置为gbest及其评价函数值fit(gbest),存储M个粒子的当前位置pbesti及其评价函数值fit(pbesti),其中pbesti=Xi,i=1,2,…,M。
λk(j,j)=Vi(j),j=1,2,…n (25)
步骤6.3),随机生成M个粒子的左、右位置如下:
其中,LXi为左位置,RXi为右位置,dt为移动步距,取值为dt=0.5,τi为n维单位向量,计算粒子左、右位置对应的评价函数值fit(LXi)及fit(RXi)。按如下原则更新M个粒子的新位置:
若fit(LXi)<fit(Xi),则Xi=LXi,fit(Xi)=fit(LXi);
若fit(RXi)<fit(Xi),则Xi=RXi,fit(Xi)=fit(RXi)。
步骤6.4),更新M个粒子的全局最优位置gbest及其评价函数值fit(gbest);按如下准则更新M个粒子的最优历史位置pbesti及其评价函数值fit(pbesti):
若fit(Xi)<fit(pbesti),则pbesti=Xi,fit(pbesti)=fit(Xi)。
步骤6.5):更新M个粒子的移动速度如下:
其中,c1与c2为学习因子,取值为c1=c2=1,为惯性系数,取值为/>rand1与rand2为随机生成的n维单位向量。按下式(28)计算M个粒子的新位置Xi,并计算其对应的评价函数值fit(Xi):
Xi=Xi+Vi (28)
步骤6.6):依据步骤6.4)更新M个粒子的全局最优位置gbest及其评价函数值fit(gbest),更新M个粒子的历史最优位置pbesti及其对应评价函数值fit(pbesti)。
步骤6.7),若达到最大迭代次数(设置为100),则结束循环迭代并输出渐消因子,即λk(i,i)=gbest(i),i=1,2,…,n。否则返回步骤6.3)继续下一个循环。
本发明中用于仿真验证的参数如下:
本***中单轴旋转调制采用正反整周旋转法,旋转角速度为Ω=20°/s。MEMS陀螺零偏稳定性为:0.5°/h,随机游走系数为:加速度计零偏稳定性为:100ug,随机游走系数为:/>地理位置为:32.12°N(纬度),118.24°E(经度),23.15m(高度)。载体运动为:θ=5°sin(2πt/6),γ=8°sin(2πt/7),ψ=10°sin(2πt/5)。对准开始前姿态误差角为:αx=5°,αy=7°,αz=40°。采样时间T=5ms,总实验时间为400s。对准过程结束后,东向及北向失准角误差约为1.2′,天向失准角误差约为30′。其结果表明本发明方法可以达到较高的精对准要求。
需要说明的是,以上内容仅仅说明了本发明的技术思想,不能以此限定本发明的保护范围,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰均落入本发明权利要求书的保护范围之内。
Claims (1)
1.一种基于强跟踪动机座旋转调制大失准角初始对准方法,其特征在于:包括下列步骤:
步骤1),建立单轴旋转调制无迹卡尔曼滤波(UKF)模型,
步骤1.1),建立非线性状态方程如下:
其中,s系为MEMS惯性单元坐标系,b为载体坐标系,n与n'分别为理想导航坐标系与实际数学平台坐标系,i与e分别为惯性坐标系与地心坐标系,是n系到n'系的旋转矩阵,/>与/>分别表示n系相对于i系的角速度计算值与计算误差,/>是b系到n'系的旋转矩阵,δvn为速度误差,/>表示加速度测量值,/>表示地球自转,/>表示n系相对于e系的角速度计算值,I表示单位矩阵,εs与/>分别表示陀螺常值漂移与加速度计常值偏置,/>与/>分别表示陀螺与加速度计的零均值的高斯白噪声,α,Cω及/>计算式如下:
α=[αx,αy,αz]T (2)
αx,αy及αz表示表示n系到n'系的三次转动角,Ω为旋转调制角速度,
步骤1.2),依据式(1)建立卡尔曼滤波方程如下:
及/>是由MEMS及GNSS(全球卫星定位***)计算的载体在n系中的线速度,f(x)与g(x)由式(1)计算得到,状态向量/>噪声向量/>H=[0 I 0 0]T,v为量测噪声向量,
步骤1.3),基于离散UKF卡尔曼滤波模型如下:
其中,与/>分别是是k-1时刻与k时刻的状态估计值,/>为一步状态估计值,/>为一步量测预测误差,/>为量测预测误差,zk为量测,Pk-1为预测均方误差阵,Pk/k-1为一步预测均方误差阵,/>为状态预测误差与量测预测误差之间协方差阵,/>为量测预测误差均方差阵,Qk-1量测噪声分配矩阵,Rk为量测状态噪声,Hk为量测转移矩阵,Kk为增益矩阵,为由/>组成的L列矩阵,χk-1为sigma点矩阵,L,Wi (m)与Wi (c)为待求解相关系数,
步骤2),建立多渐消因子强跟踪卡尔曼滤波器模型,
步骤2.1)强跟踪卡尔曼滤波器的策略是是通过渐消因子调整Pk/k-1以保持残差相互正交,抑制外部干扰,防止***发散,k时刻的新息矢量为:
步骤2.2)k时刻新息矢量的协方差阵估计值为:
其中,C0,k计算方法如下:
其中,b为常值系数,
步骤2.3),建立基于多渐消因子的Pk/k-1的修正公式为:
其中,λk=diag(s1,s2,…sn)为渐消因子组成的对角矩阵,si≥1,i=1,2,…n,n为状态向量的维数,
步骤2.4)新息序列互不相关的等价条件是:
其中,符号E表示求期望,式(11)的等价形式为:
强跟踪卡尔曼滤波的原理即通过选取合适的λk使式(12)近似成立,
步骤3),建立基于卡方检验的异常检测准则如下:
其中,为检测阈值,α为置信水平,m为自由度,θk为滤波***计算的卡方值,其计算方法如下:
步骤4),定义目标矩阵如下:
代入式(10)到式(15)得:
调整渐消因子矩阵λk使矩阵h(k)所有元素绝对值取值最小,使等式(12)近似成立,则设计评价函数的等价形式为:
其中,n和m分别为矩阵h(k)行数及列数,sij是矩阵h(k)的元素,渐消因子的寻优过程即使fit取最小值,
步骤5),建立基于改进的具有全局和局部搜索能力的粒子群搜索算法(PSO)求解多渐消因子,
步骤5.1),随机初始化M个n维粒子的位置与移动速度:Xi及Vi,i=1,2,…,M,
步骤5.2),运用式(18)计算M个粒子对应的渐消因子矩阵,运用式(17)计算M个粒子对应的评价函数值fit(Xi),记录M个粒子的全局最优位置为gbest及其评价函数值fit(gbest),存储M个粒子的当前位置pbesti及其评价函数值fit(pbesti),其中pbesti=Xi,i=1,2,…,M,
λk(j,j)=Vi(j),j=1,2,…n (18)
步骤5.3),随机生成M个粒子的左、右位置如下:
其中,LXi为左位置,RXi为右位置,dt为移动步距,τi为n维单位向量,计算粒子左、右位置对应的评价函数值fit(LXi)及fit(RXi),按如下原则更新M个粒子的新位置:
若fit(LXi)<fit(Xi),则Xi=LXi,fit(Xi)=fit(LXi),
若fit(RXi)<fit(Xi),则Xi=RXi,fit(Xi)=fit(RXi),
步骤5.4),更新M个粒子的全局最优位置gbest及其评价函数值fit(gbest);按如下准则更新M个粒子的最优历史位置pbesti及其评价函数值fit(pbesti):
若fit(Xi)<fit(pbesti),则pbesti=Xi,fit(pbesti)=fit(Xi),
步骤5.5):更新M个粒子的移动速度如下:
其中,c1与c2为学习因子,为惯性系数,rand1与rand2为随机生成的n维单位向量,按式(21)计算M个粒子的新位置Xi,并计算其对应的评价函数值fit(Xi):
Xi=Xi+Vi (21)
步骤5.6):依据步骤5.4)更新M个粒子的全局最优位置gbest及其评价函数值fit(gbest),更新M个粒子的历史最优位置pbesti及其对应评价函数值fit(pbesti),
步骤5.7),若达到停止条件,则结束迭代循环并输出渐消因子,即λk(i,i)=gbest(i),i=1,2,…n,否则,返回步骤5.3)继续下一个循环。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111330426.XA CN114061621B (zh) | 2021-11-11 | 2021-11-11 | 一种基于强跟踪动机座旋转调制大失准角初始对准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111330426.XA CN114061621B (zh) | 2021-11-11 | 2021-11-11 | 一种基于强跟踪动机座旋转调制大失准角初始对准方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114061621A CN114061621A (zh) | 2022-02-18 |
CN114061621B true CN114061621B (zh) | 2023-11-17 |
Family
ID=80274898
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111330426.XA Active CN114061621B (zh) | 2021-11-11 | 2021-11-11 | 一种基于强跟踪动机座旋转调制大失准角初始对准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114061621B (zh) |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107707220A (zh) * | 2017-08-31 | 2018-02-16 | 东南大学 | 一种应用在gnss/ins中的改进型ckf方法 |
CN107830872A (zh) * | 2017-10-26 | 2018-03-23 | 哈尔滨工程大学 | 一种舰船捷联惯性导航***自适应初始对准方法 |
CN110398257A (zh) * | 2019-07-17 | 2019-11-01 | 哈尔滨工程大学 | Gps辅助的sins***快速动基座初始对准方法 |
CN110567490A (zh) * | 2019-08-29 | 2019-12-13 | 桂林电子科技大学 | 一种大失准角下sins初始对准方法 |
CN110823217A (zh) * | 2019-11-21 | 2020-02-21 | 山东大学 | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 |
CN112284414A (zh) * | 2020-10-10 | 2021-01-29 | 东南大学 | 一种基于多渐消因子自适应动机座旋转调制精对准方法 |
-
2021
- 2021-11-11 CN CN202111330426.XA patent/CN114061621B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107707220A (zh) * | 2017-08-31 | 2018-02-16 | 东南大学 | 一种应用在gnss/ins中的改进型ckf方法 |
CN107830872A (zh) * | 2017-10-26 | 2018-03-23 | 哈尔滨工程大学 | 一种舰船捷联惯性导航***自适应初始对准方法 |
CN110398257A (zh) * | 2019-07-17 | 2019-11-01 | 哈尔滨工程大学 | Gps辅助的sins***快速动基座初始对准方法 |
CN110567490A (zh) * | 2019-08-29 | 2019-12-13 | 桂林电子科技大学 | 一种大失准角下sins初始对准方法 |
CN110823217A (zh) * | 2019-11-21 | 2020-02-21 | 山东大学 | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 |
CN112284414A (zh) * | 2020-10-10 | 2021-01-29 | 东南大学 | 一种基于多渐消因子自适应动机座旋转调制精对准方法 |
Non-Patent Citations (6)
Title |
---|
Carrier frequency disturbance distributions on GPS during equatorial ionospheric scintillation;ZHU Xuefen;Journal of Systems Engineering and Electronics;第31卷(第6期);全文 * |
Generalized Dynamic Fuzzy NN Model Based on Multiple Fading Factors SCKF and Its Application in Integrated Navigation;Junwei Wang;IEEE;第21卷(第3期);全文 * |
一种强跟踪UKF及其在GPS/SINS深组合导航中的应用;叶晨;崔双喜;;导弹与航天运载技术(02);全文 * |
基于改进强跟踪ASCKF算法的SINS/GNSS组合对准方法;刘轶;程旭红;程建华;;导航定位与授时(05);全文 * |
基于改进径向基神经网络的MEMS惯导***误差抑制方法;陈光武;李文元;于月;刘孝博;;中国惯性技术学报(01);全文 * |
强跟踪CKF及其在惯导***初始对准中的应用;郭士荦;许江宁;李峰;;中国惯性技术学报(04);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN114061621A (zh) | 2022-02-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
WO2022073376A1 (zh) | 一种基于多渐消因子自适应动机座旋转调制精对准方法 | |
CN110398257B (zh) | Gps辅助的sins***快速动基座初始对准方法 | |
CN102519450B (zh) | 一种用于水下滑翔器的组合导航装置及方法 | |
CN108827310B (zh) | 一种船用星敏感器辅助陀螺仪在线标定方法 | |
WO2020062791A1 (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN106052686B (zh) | 基于dsptms320f28335的全自主捷联惯性导航*** | |
Wang et al. | A GNSS/INS integrated navigation algorithm based on Kalman filter | |
CN103245360A (zh) | 晃动基座下的舰载机旋转式捷联惯导***自对准方法 | |
Huang et al. | Application of adaptive Kalman filter technique in initial alignment of strapdown inertial navigation system | |
CN109506660B (zh) | 一种用于仿生导航的姿态最优化解算方法 | |
CN109945860A (zh) | 一种基于卫星紧组合的ins和dr惯性导航方法与*** | |
CN111102993A (zh) | 一种旋转调制型捷联惯导***晃动基座初始对准方法 | |
CN104374388A (zh) | 一种基于偏振光传感器的航姿测定方法 | |
CN103292812A (zh) | 一种微惯性sins/gps组合导航***的自适应滤波方法 | |
Xue et al. | In-motion alignment algorithm for vehicle carried SINS based on odometer aiding | |
CN107193023A (zh) | 一种具有闭式解的高精度北斗卫星***单点定位方法 | |
CN104949687A (zh) | 一种长时间导航***全参数精度评估方法 | |
CN106802143A (zh) | 一种基于惯性仪器和迭代滤波算法的船体形变角测量方法 | |
CN112798016A (zh) | 一种基于sins与dvl组合的auv行进间快速初始对准方法 | |
CN105300407B (zh) | 一种用于单轴调制激光陀螺惯导***的海上动态启动方法 | |
CN111207773A (zh) | 一种用于仿生偏振光导航的姿态无约束优化求解方法 | |
Wang et al. | Application of gravity passive aided strapdown inertial navigation in underwater vehicles | |
CN114061621B (zh) | 一种基于强跟踪动机座旋转调制大失准角初始对准方法 | |
CN110873577B (zh) | 一种水下快速动基座对准方法及装置 | |
CN113008229A (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 |