CN114061621B

CN114061621B - 一种基于强跟踪动机座旋转调制大失准角初始对准方法

Info

Publication number: CN114061621B
Application number: CN202111330426.XA
Authority: CN
Inventors: 陈熙源; 刘建国
Original assignee: Southeast University
Current assignee: Southeast University
Priority date: 2021-11-11
Filing date: 2021-11-11
Publication date: 2023-11-17
Anticipated expiration: 2041-11-11
Also published as: CN114061621A

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'系的旋转矩阵，δvⁿ为速度误差，/>表示加速度测量值，/>表示地球自转，/>表示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时刻的状态估计值,/>为一步状态估计值，为一步量测预测误差,/>为量测预测误差，z_k为量测，P_k-1为预测均方误差阵,P_k/k-1为一步预测均方误差阵,/>为状态预测误差与量测预测误差之间协方差阵，/>为量测预测误差均方差阵，Q_k-1量测噪声分配矩阵,R_k为量测状态噪声，H_k为量测转移矩阵,K_k为增益矩阵,/>为由/>组成的L列矩阵，χ_k-1为sigma点矩阵，L，W_i ^(m)与W_i ^(c)为待求解相关系数。

步骤3)，建立多渐消因子强跟踪卡尔曼滤波器模型

步骤3.1)强跟踪卡尔曼滤波器的策略是是通过渐消因子调整P_k/k-1以保持残差相互正交，抑制外部干扰，防止系统发散。k时刻的新息矢量为：

步骤3.2)k时刻新息矢量的协方差阵估计值为：

其中，C_0,k计算方法如下：

其中，b为常值系数，其取值范围一般为：0.8～0.9。

步骤3.3)，建立基于多渐消因子的P_k/k-1的修正公式为：

其中，λ_k＝diag(s₁,s₂,…s_n)为渐消因子组成的对角矩阵，s_i≥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)行数及列数，s_ij是矩阵h(k)的元素。则渐消因子的寻优过程即使fit取最小值。

步骤6)，建立基于改进PSO算法的多渐消因子更新方法如下：

步骤6.1)，随机初始化M个n维粒子的位置与移动速度：X_i及V_i,i＝1,2,…,M。

步骤6.2)，运用式(18)计算M个粒子对应的渐消因子矩阵，运用式(17)计算M个粒子对应的评价函数值fit(X_i)，记录M个粒子的全局最优位置为gbest及其评价函数值fit(gbest)，存储M个粒子的当前位置pbest_i及其评价函数值fit(pbest_i)，其中pbest_i＝X_i，i＝1,2,…,M。

λ_k(j,j)＝V_i(j),j＝1,2,…n (18)

步骤6.3)，随机生成M个粒子的左、右位置如下：

其中，LX_i为左位置，RX_i为右位置，d_t为移动步距，τ_i为n维单位向量，计算粒子左、右位置对应的评价函数值fit(LX_i)及fit(RX_i)。按如下原则更新M个粒子的新位置：

若fit(LX_i)<fit(X_i),则X_i＝LX_i，fit(X_i)＝fit(LX_i)；

若fit(RX_i)<fit(X_i),则X_i＝RX_i，fit(X_i)＝fit(RX_i)。

步骤6.4)，更新M个粒子的全局最优位置gbest及其评价函数值fit(gbest)；按如下准则更新M个粒子的最优历史位置pbest_i及其评价函数值fit(pbest_i)：

若fit(X_i)<fit(pbest_i),则pbest_i＝X_i，fit(pbest_i)＝fit(X_i)。

步骤6.5)：更新M个粒子的移动速度如下：

其中，c₁与c₂为学习因子，为惯性系数，rand₁与rand₂为随机生成的n维单位向量。

按式(21)计算M个粒子的新位置X_i，并计算其对应的评价函数值fit(X_i)：

X_i＝X_i+V_i (21)

步骤6.6)：依据步骤6.4)更新M个粒子的全局最优位置gbest及其评价函数值fit(gbest)，更新M个粒子的历史最优位置pbest_i及其对应评价函数值fit(pbest_i)。

步骤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)，建立非线性状态方程如下：

α＝[α_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时刻的状态估计值,/>为一步状态估计值，为一步量测预测误差,/>为量测预测误差，z_k为量测，P_k-1为预测均方误差阵,P_k/k-1为一步预测均方误差阵,/>为状态预测误差与量测预测误差之间协方差阵，/>为量测预测误差均方差阵，Q_k-1量测噪声分配矩阵,R_k为量测状态噪声，H_k为量测转移矩阵,K_k为增益矩阵,为由/>组成的L列矩阵，χ_k-1为sigma点矩阵，L为状态量的维数，W_i ^(m)与W_i ^(c)为待求解相关系数，其计算方法如下：

其中,比例因子a，κ与β分别取值为：0.01，0和2。

步骤3)，建立多渐消因子强跟踪卡尔曼滤波器模型

步骤3.2)k时刻新息矢量的协方差阵估计值为：

其中，C_0,k计算方法如下：

其中，b为常值系数，其取值为：0.85。

步骤3.3)，建立基于多渐消因子的P_k/k-1的修正公式为：

其中，λ_k＝diag(s₁,s₂,…s_n)为渐消因子组成的对角矩阵，s_i≥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；s_ij是矩阵h(k)的元素。则渐消因子的寻优过程即使fit取最小值。

步骤6)，建立基于改进PSO算法的多渐消因子更新方法如下：

步骤6.1)，随机初始化M个n维粒子的位置与移动速度：X_i及V_i,i＝1,2,…,M,此方法中M＝60。

步骤6.2)，运用式(25)计算M个粒子对应的渐消因子矩阵，运用式(24)计算M个粒子对应的评价函数值fit(X_i)，记录M个粒子的全局最优位置为gbest及其评价函数值fit(gbest)，存储M个粒子的当前位置pbest_i及其评价函数值fit(pbest_i)，其中pbest_i＝X_i，i＝1,2,…,M。

λ_k(j,j)＝V_i(j),j＝1,2,…n (25)

步骤6.3)，随机生成M个粒子的左、右位置如下：

其中，LX_i为左位置，RX_i为右位置，d_t为移动步距，取值为d_t＝0.5，τ_i为n维单位向量，计算粒子左、右位置对应的评价函数值fit(LX_i)及fit(RX_i)。按如下原则更新M个粒子的新位置：

若fit(LX_i)<fit(X_i),则X_i＝LX_i，fit(X_i)＝fit(LX_i)；

若fit(RX_i)<fit(X_i),则X_i＝RX_i，fit(X_i)＝fit(RX_i)。

若fit(X_i)<fit(pbest_i),则pbest_i＝X_i，fit(pbest_i)＝fit(X_i)。

步骤6.5)：更新M个粒子的移动速度如下：

其中，c₁与c₂为学习因子，取值为c₁＝c₂＝1，为惯性系数，取值为/>rand₁与rand₂为随机生成的n维单位向量。按下式(28)计算M个粒子的新位置X_i，并计算其对应的评价函数值fit(X_i)：

X_i＝X_i+V_i (28)

步骤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)，建立单轴旋转调制无迹卡尔曼滤波(UKF)模型，

步骤1.1)，建立非线性状态方程如下：

其中，s系为MEMS惯性单元坐标系，b为载体坐标系，n与n'分别为理想导航坐标系与实际数学平台坐标系，i与e分别为惯性坐标系与地心坐标系，是n系到n'系的旋转矩阵，/>与/>分别表示n系相对于i系的角速度计算值与计算误差，/>是b系到n'系的旋转矩阵，δvⁿ为速度误差，/>表示加速度测量值，/>表示地球自转，/>表示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时刻的状态估计值,/>为一步状态估计值，/>为一步量测预测误差,/>为量测预测误差，z_k为量测，P_k-1为预测均方误差阵,P_k/k-1为一步预测均方误差阵,/>为状态预测误差与量测预测误差之间协方差阵，/>为量测预测误差均方差阵，Q_k-1量测噪声分配矩阵,R_k为量测状态噪声，H_k为量测转移矩阵,K_k为增益矩阵,为由/>组成的L列矩阵，χ_k-1为sigma点矩阵，L，W_i ^(m)与W_i ^(c)为待求解相关系数，

步骤2)，建立多渐消因子强跟踪卡尔曼滤波器模型，

步骤2.1)强跟踪卡尔曼滤波器的策略是是通过渐消因子调整P_k/k-1以保持残差相互正交，抑制外部干扰，防止系统发散，k时刻的新息矢量为：

步骤2.2)k时刻新息矢量的协方差阵估计值为：

其中，C_0,k计算方法如下：

其中，b为常值系数，

步骤2.3)，建立基于多渐消因子的P_k/k-1的修正公式为：

其中，λ_k＝diag(s₁,s₂,…s_n)为渐消因子组成的对角矩阵，s_i≥1,i＝1,2,…n，n为状态向量的维数，

步骤2.4)新息序列互不相关的等价条件是：

其中，符号E表示求期望，式(11)的等价形式为：

强跟踪卡尔曼滤波的原理即通过选取合适的λ_k使式(12)近似成立，

步骤3)，建立基于卡方检验的异常检测准则如下：

步骤4)，定义目标矩阵如下：

代入式(10)到式(15)得：

其中，n和m分别为矩阵h(k)行数及列数，s_ij是矩阵h(k)的元素，渐消因子的寻优过程即使fit取最小值，

步骤5)，建立基于改进的具有全局和局部搜索能力的粒子群搜索算法(PSO)求解多渐消因子，

步骤5.1)，随机初始化M个n维粒子的位置与移动速度：X_i及V_i,i＝1,2,…,M，

步骤5.2)，运用式(18)计算M个粒子对应的渐消因子矩阵，运用式(17)计算M个粒子对应的评价函数值fit(X_i)，记录M个粒子的全局最优位置为gbest及其评价函数值fit(gbest)，存储M个粒子的当前位置pbest_i及其评价函数值fit(pbest_i)，其中pbest_i＝X_i，i＝1,2,…,M，

λ_k(j,j)＝V_i(j),j＝1,2,…n (18)

步骤5.3)，随机生成M个粒子的左、右位置如下：

其中，LX_i为左位置，RX_i为右位置，d_t为移动步距，τ_i为n维单位向量，计算粒子左、右位置对应的评价函数值fit(LX_i)及fit(RX_i)，按如下原则更新M个粒子的新位置：

若fit(LX_i)<fit(X_i),则X_i＝LX_i，fit(X_i)＝fit(LX_i)，

若fit(RX_i)<fit(X_i),则X_i＝RX_i，fit(X_i)＝fit(RX_i)，

步骤5.4)，更新M个粒子的全局最优位置gbest及其评价函数值fit(gbest)；按如下准则更新M个粒子的最优历史位置pbest_i及其评价函数值fit(pbest_i)：

若fit(X_i)<fit(pbest_i),则pbest_i＝X_i，fit(pbest_i)＝fit(X_i)，

步骤5.5)：更新M个粒子的移动速度如下：

其中，c₁与c₂为学习因子，为惯性系数，rand₁与rand₂为随机生成的n维单位向量，按式(21)计算M个粒子的新位置X_i，并计算其对应的评价函数值fit(X_i)：

X_i＝X_i+V_i (21)

步骤5.6)：依据步骤5.4)更新M个粒子的全局最优位置gbest及其评价函数值fit(gbest)，更新M个粒子的历史最优位置pbest_i及其对应评价函数值fit(pbest_i)，

步骤5.7)，若达到停止条件，则结束迭代循环并输出渐消因子，即λ_k(i,i)＝gbest(i),i＝1,2,…n，否则，返回步骤5.3)继续下一个循环。