CN109211276A - 基于gpr与改进的srckf的sins初始对准方法 - Google Patents

Abstract

本发明提供了一种基于GPR与改进的SRCKF的SINS初始对准方法，首先获取各传感器数据，并对数据预处理；建立载体系统的状态方程和量测方程；采用SSR规则选取SRCKF的容积点，建立改进的SRCKF递推方程；利用改进SRCKF递推方程递推，最终递推获取载体的初始对准姿态误差角。在需要快速对准环境下，建立GPR模型；利用系统当前的量测数据，估计系统当前对准过程的姿态误差角，求解姿态矩阵以完成初始对准任务。本发明提高SRCKF的鲁棒性，增强滤波算法的数值稳定性，并能降低高阶SRCKF的计算量，加快初始对准速度且能获得与改进的SRCKF精度相当的初始对准结果。

Description

基于GPR与改进的SRCKF的SINS初始对准方法

技术领域

本发明属于导航系统初始对准技术领域，一种基于高斯过程回归(Gauss ProcessRegression，GPR)与改进的平方根容积卡尔曼滤波(Square-Root Cubature KalmanFilter，SRCKF)的捷联惯性导航系统(SINS)初始对准方法。

背景技术

惯性导航系统在开始进行导航解算之前，需要进行初始对准。初始对准是惯性导航的关键技术。初始对准精度的高低很大程度上决定了导航的精度，初始对准的速度快慢一定程度上影响着导航系统的应用范围。惯性导航系统初始对准可分为粗对准和精对准，通常采用解析法、非线性滤波方法等进行粗对准，采用罗经法、非线性滤波方法等方法进行精对准。通过初始对准，获得初始姿态角，从而确定导航解算所需要的初始姿态矩阵。

在实际的应用环境中，特别是导航系统存在大失准角和剧烈晃动的环境，如行驶过程中的舰船初始对准中，工程上常使用非线性滤波方法进行初始对准。常见的非线性滤波方法有扩展卡尔曼滤波(ExtendedKalmanFilter，EKF)、无迹卡尔曼滤波(UnscentedKalmanFilter，UKF)和容积卡尔曼滤波(Cubuture Kalman Filter，CKF)以及平方根容积卡尔曼滤波(Square-Root Cubuture KalmanFilter，SRCKF)等。传统的EKF、UKF等非线性滤波方法存在对高度非线性的系统对准精度低和数值不稳定问题，CKF同样存在数值不稳定的情况，SRCKF能够改善数值不稳定的情况，但是对于模型发生变化，或者模型不准确时，SRCKF不能正确跟踪模型，且SRCKF收敛过程较慢，影响了其应用。

发明内容

为改善SRCKF应对不确定因素能力差和收敛过程较慢的缺点，本发明提供了一种基于GPR与改进的SRCKF的SINS初始对准方法，能够提高SINS适应能力，并在需要快速对准的环境下进行快速初始对准。

为了达到上述目的，本发明提供如下技术方案：

基于GPR与改进的SRCKF的SINS初始对准方法，包括如下步骤：

步骤1：建立导航系统的初始对准模型，所述对准模型包括捷联惯性导航系统的非线性误差模型、滤波模状态模型以及量测模型；

步骤2：将步骤所得的状态方程和量测方程离散化；

步骤3：在不需要快速对准的情况下，以状态方程和量测方程执行改进的SRCKF的时间更新过程，获取状态的预测估计值和预测误差协方差的平方根；

步骤4：以改进SRCKF时间更新的预测估计值和预测误差协方差的平方根为初始值进行迭代，迭代至最大次数，完成量测更新；

步骤5：由步骤4获取的状态量中提取欧拉角平台估计值和速度估计值修正SINS解算的姿态矩阵和速度，将修正后的值作为下一次SINS解算的初始值，并利用当前获得的陀螺仪和加速度计常值误差估计值修正下一时刻的陀螺仪输出和加速度计输出；

步骤6：当改进SRCKF运行一定时间后，每隔一个固定的时间间隔保存系统测量数据，构建以后需要快速对准环境下采用GPR模型算法需要的训练数据集，共保存N组数据；

步骤7：在需要快速对准的环境下，根据上一个初始对准过程中保存的训练数据集D采用GPR模型算法进行学习；同时将当前SINS实时解算的量测数据作为输入将姿态误差角作为输出，更新GPR的训练数据集进行学习，获得系统的状态转移GPR模型和量测GPR模型；再利用获得系统的状态转移GPR模型和量测GPR模型，获取系统当前对准过程的姿态误差角以及误差方差，完成系统的初始对准。

进一步的，所述步骤1具体包括如下子步骤：

步骤1.1：建立捷联惯性导航系统的非线性误差模型：

选择东-北-天坐标系地理坐标系为理想导航坐标系，以载体右前上方向构建载体坐标系，记SINS模拟解算的数学平台坐标系为n’系；记b系与n系之间的真实姿态角为b系为载体坐标系，n系为理想导航坐标系，载体相对于导航坐标系的真实速度为载体所在的真实地理坐标为p＝[L λ H]^T，SINS实际解算出的姿态为速度为地理坐标为记SINS解算的姿态角误差为其中φ_e,φ_n,φ_u分别为，纵摇角误差、横摇角误差和航向角误差；速度误差其中δv_e,δv_n,δv_u分别为东向、北向和天向速度误差，则：

n系到n’系依次旋转φ_u,φ_e,φ_n的坐标系转换矩阵为：

欧拉角微分方程的系数矩阵C_ω为：

姿态误差微分方程为：

其中为b系下的三轴陀螺常值误差，为b系下三轴陀螺随机误差，为计算出的n系相对惯性坐标系的旋转角速度，为b系到n’系状态转移矩阵，也即SINS解算的姿态矩阵；

速度误差方程为：

其中为b系下测得的加速度计实际输出，为b系下三轴加速度计的常值误差，为b系下三轴加速度计随机误差，分别为对应的计算误差，g为重力加速度，T表示转置；

步骤1.2：建立SINS状态方程模型如下：

取状态向量为其中由于整个对准过程中近似为零，则状态方程可化为如下形式：

其中根据选取的状态量，上式速度相关的分量为设过程噪声向量为零均值高斯白噪声；地球相对惯性系的角速度ω_ie在n系下的投影L为载体所在的纬度，上述状态方程可记为：

其中f(x,t)为根据前面状态方程所得的函数，g(t)为根据上述系统过程噪声向量确定的系数矩阵；

步骤1.3：建立量测方程如下：

其中v^b为b系下的真实速度，为b系下计算用的实际速度，SINS将转换为w_z为零均值高斯白噪声过程，仅以和中的东向和北向速度误差分量作为匹配信息源；上述量测方程简记为：

z(t)＝h(x,t)+w_z(t)。

其中h(x,t)为根据前面量测方程所得的函数。

进一步的，所述步骤2中采样周期与滤波周期均取为T_s，本步骤将步骤1中的状态方程和量测方程离散化为：

其中x_k为k时刻系统的状态量，z_k为k时刻的系统状态的量测值，w_x；k-1～N(0,Q_k-1)为系统过程噪声，w_z；k～N(0,R_k)为系统观测噪声，F(x_k-1),G_k-1,H(x_k-1)为将步骤1对应的方程离散化后所得的函数。

进一步的，所述步骤3中改进的SRCKF框架下时间更新的具体步骤如下：

步骤3.1：设置系统状态初始值初始误差协方差阵

P₀＝diag{(1°)²(1°)²(10°)²(0.1m/s)²(0.1m/s)²(0.01°/h)²

(0.01°/h)²(0.01°/h)²(100μg/h)²(100μg/h)²}

上式中°为角度单位，m/s为速度单位，h为时间单位，μg＝10^-6g。对上述初始误差协方差阵P₀进行Cholesky分解得初始误差协方差矩阵的特征平方根S₀＝Chol(P₀)，其中Chol(·)表示矩阵的Cholesky分解；

步骤3.2：按照球面最简规则径向(SSR)容积规则选取改进SRCKF的采样点，如下：

取向量a_j＝[a_j,1 a_j,1 … a_j,n]^T,j＝1,2,...,n+1，其中n＝10为状态量的个数，有

记ξ_i为第i个容积点，则得到m＝2(n+1)个容积点为：

步骤3.3：计算状态的一步预测值和一步预测误差协方差矩阵的平方根因子：

当前状态容积点与预测状态容积点为：

k时刻状态的一步预测值和一步预测误差协方差的平方根为：

其中S＝Tria(A)表示由A^T进行QR分解得到的上三角阵R'＝QR(A^T)的转置S＝R'^T，S_Q,k-1为矩阵的Cholesky分解。即上式中S_k|k-1的平方根因子由

及

给出。此处Q_k-1为如前面所述的系统过程噪声方差阵。

进一步的，所述步骤4中改进SRCKF框架下量测更新的过程采用迭代算法，具体步骤如下：

步骤4.1：以步骤3中得到的状态预测值和预测误差协方差S_k|k-1为迭代的初始值，记k时刻第j次迭代的状态估计值和误差协方差分别为第0次迭代的状态估计值和误差协方差，即迭代的初始值

步骤4.2：对于j＝0,1,...,N_max执行以下过程：

计算新的容积点：

按如下步骤计算k时刻第j次迭代的状态和误差协方差平方根：

计算k时刻第j次迭代的第i个预测量测容积点：

计算预测量测值：

计算新息协方差矩阵的平方根：

其中上式中的平方根因子由

及

给出；

计算估计互协方差矩阵：

其中加权中心矩阵：

估计CKF增益：

估计更新状态：

估计更新后的状态误差协方差平方根因子：

步骤4.3：迭代的终止条件为其中Δ为预先设定的迭代阈值；或迭代次数达到最大值N_max，若未满足上述两个条件之一，则重复权利要求4的步骤；

步骤4.4：终止迭代时假设迭代次数j＝N，则设置k时刻的状态估计值和状态估计误差的协方差平方根分别为：

进一步的，所述步骤4.3中当迭代方法由于不能加速新息的反馈效率而失效时，取N_max＝1，即不进行迭代。

进一步的，所述步骤5中修正公式如下：

其中，和分别为SINS解算的姿态矩阵和速度，为当前获得的陀螺仪和加速度计常值误差估计值，为下一时刻的陀螺仪输出和加速度计输出。

进一步的，所述步骤6中系统测量数据包括东向和北向速度误差，以及SINS解算的姿态误差角。

进一步的，当对准精度没有达到预定要求则循环执行执行步骤3-6直至SINS初始对准结束。

进一步的，所述步骤7中GPR模型算法具体步骤如下：

步骤7.1：在需要快速对准的环境下，根据上一个初始对准过程中保存的N组输入输出数据构成的数据训练集D进行GPR模型学习，获取系统的状态转移GPR模型和量测GPR模型，更具体的步骤为：

步骤7.1.1：其中数据训练集D＝{D_e,D_n,D_u}有下述数据训练子集D_e＝{ΔV,Φ_e}；D_e＝{ΔV,Φ_e}为用于估计建立东向误差角φ_e的GPR模型采用的数据训练集，其中ΔV＝[Δv₁Δv₂ … Δv_N]^T，Δv_κ＝(δv_eκ,δv_nκ)^T∈R²(κ＝1,...,N)为保存的所有N组量测(δv_e,δv_n)中的第κ组；Φ_e＝[φ_e1 φ_e2 … φ_eN]^T，φ_eκ∈R(κ＝1,...,N)为保存的N个东向误差角中的第κ组；针对东北天三个方向的姿态误差角φ_e,φ_n,φ_u相应有输出向量Φ_e,Φ_n,Φ_u共保存了3N个姿态误差角数据，共有三个数据训练子集D_e,D_n,D_u；

步骤7.1.2：根据步骤7.1.1的数据集进行GPR模型学习，初始化GPR模型：采用高斯核函数作为GPR模型的初始化核函数：

式中Δv_p,Δv_q为数据训练集量测ΔV中的任意元素，为信号方差，l₁,l₂为信号方差；上述未知参数集记为超参数其中为数据训练集的样本噪声方差；

步骤7.1.3：建立东向姿态误差角的GPR模型：以量测ΔV和东向姿态误差角Φ_e＝[φ_e1 φ_e2 … φ_eN]构成的数据训练集D_e＝(ΔV,Φ_e)作为GPR模型的输入输出对建立训练样本条件函数p(Θ_e|ΔV_κ,Θ_κ)的对数似然函数：

式中K＝K(ΔV,ΔV)＝{k(Δv_p,Δv_q)|p,q＝1,...,N}为N×N阶对称正定协方差矩阵，K(p,q)＝k(Δv_p,Δv_q)度量了Δv_p,Δv_q的相关性；

通过最大化似然函数求取超参数超参数，即对似然函数求偏导：

其中α＝E^-1Φ_e，tr(·)为求矩阵迹运算，Θ_i表示中的第i＝1,2,3个元素，K(ΔV,ΔV)为前面定义的协方差矩阵；

采用共轭梯度法对上述偏导数进行最小化，从而得到超参数的最优解；

步骤7.1.4：针对三个姿态误差角，重复以上步骤分别获得相应参数；

步骤7.2：在当前对准过程中，利用改进的SRCKF获取当前时刻的量测值Δv^*，对系统实时的量测与截止至前一时刻的姿态误差角输出更新训练数据集D，根据GPR模型获取当前量测Δv^*下的姿态误差角及其误差方差，进行SINS初始对准，获取系统当前对准过程的姿态误差角以及相应的误差方差；具体的步骤如下：

步骤7.2.1：由高斯过程回归模型中数据训练集的输出矢量Φ_e和预测值φ_e ^*之间满足：

其中

K(Δv^*,ΔV)＝K(ΔV,Δv^*)^T＝(k(Δv^*,Δv₁),...,k(Δv^*,Δv_N))^T，k(Δv^*,Δv_i)(i＝1,...,N)为当前量测值Δv^*与训练数据集中的量测值Δv_i的协方差，k(Δv^*,Δv^*)为实时量测值的自协方差；由此得出在当前时刻量测值Δv^*下的预测估计输出值φ_e ^*的均值和协方差如下：

由此可以完成当前对准过程姿态误差角φ_e的估计；

步骤7.2.2：对于其他两个姿态误差角φ_n,φ_u，通过上述步骤，获取其当前对准过程中的估计均值和相应的误差方差从而完成SINS初始对准过程。

与现有技术相比，本发明具有如下优点和有益效果：

1.本发明将球面最简径向容积规则SSR融入平方根容积卡尔曼滤波算法，提高SRCKF的鲁棒性，增强滤波算法的数值稳定性，并能降低高阶SRCKF的计算量。

2.本发明在SRCKF的量测更新阶段采用迭代算法，当量测方程为非线性方程时，能够提高新息的反馈效率，加快SRCKF的收敛过程。

3.本发明依据对准环境的不同，在需要以对准精度作为绝对的考虑因素的情况下采用改进的SRCKF算法，着重降低初始对准结果误差；而在需要提高初始对准速度的环境下，采用高斯过程回归与改进的SRCKF相融合的方式，加快初始对准速度且能获得与改进的SRCKF精度相当的初始对准结果。

附图说明

图1为本发明提供的基于GPR与改进的SRCKF的SINS初始对准方法整体流程图。

图2为图1中改进的SRCKF模型算法流程图。

图3为图1中GPR融入改进的SRCKF模型算法流程图。

具体实施方式

以下将结合具体实施例对本发明提供的技术方案进行详细说明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

本发明提供的基于GPR与改进的SRCKF的SINS初始对准方法，首先获取各传感器数据，并对数据预处理；建立载体系统的状态方程和量测方程；采用SSR规则选取SRCKF的容积点，建立改进的SRCKF递推方程；利用改进SRCKF递推方程递推，最终递推获取载体的初始对准姿态误差角。在需要快速对准环境下，建立GPR模型；利用系统当前的量测数据，估计系统当前对准过程的姿态误差角，求解姿态矩阵以完成初始对准任务。

由于获取数据及预处理过程较为常规，不属于本发明重点，因此本实施例着重对后续步骤进行详细描述，具体的说，本发明整体流程如图1所示，包括以下步骤：

步骤1：建立导航系统的初始对准模型，所述对准模型包括捷联惯性导航系统(SINS)的非线性误差模型、滤波模状态模型以及量测模型。

步骤1.1：其中捷联惯性导航系统的非线性误差模型通过如下过程建立：

选择东-北-天坐标系地理坐标系(g系)为理想导航坐标系(n系)，以载体右前上方向构建载体坐标系(b系)，记SINS模拟解算的数学平台坐标系为n’系；记b系与n系之间的真实姿态角为载体相对于导航坐标系的真实速度为载体所在的真实地理坐标为p＝[L λ H]^T，SINS实际解算出的姿态为速度为地理坐标为由于受到各种误差源的影响，SINS的解算值与真实值之间存在误差，记SINS解算的姿态角误差为其中φ_e,φ_n,φ_u分别为，纵摇角误差(东向水平失准角)、横摇角误差(北向水平失准角)和航向角误差(方位失准角)；速度误差其中δv_e,δv_n,δv_u分别为东向、北向和天向速度误差，则：

n系到n’系依次旋转φ_e,φ_n,φ_u的坐标系转换矩阵为：

欧拉角微分方程的系数矩阵C_ω为：

姿态误差微分方程为：

其中为b系下的三轴陀螺常值误差，为b系下三轴陀螺随机误差，为计算出的n系相对惯性坐标系的旋转角速度，为b系到n’系状态转移矩阵，也即SINS解算的姿态矩阵。

速度误差方程为：

其中为b系下测得的加速度计实际输出，为b系下三轴加速度计的常值误差，为b系下三轴加速度计随机误差，分别为对应的计算误差，g为重力加速度，T表示转置。

步骤1.2：所述SINS状态方程模型如下：

取状态向量为其中由于整个对准过程中近似为零，则状态方程可化为如下形式：

其中根据选取的状态量，上式速度相关的分量为设过程噪声向量为零均值高斯白噪声；地球相对惯性系的角速度ω_ie在n系下的投影L为载体所在的纬度，上述状态方程可记为：

其中f(x,t)为根据前面状态方程所得的函数，g(t)为根据上述系统过程噪声向量确定的系数矩阵。

步骤1.3：所述量测方程如下：

其中v^b为b系下的真实速度，为b系下计算用的实际速度，SINS将转换为w_z为零均值高斯白噪声过程，仅以和中的东向和北向速度误差分量作为匹配信息源。上述量测方程简记为：

z(t)＝h(x,t)+w_z(t)

其中h(x,t)为根据前面量测方程所得的函数。

步骤2：将步骤所得的状态方程和量测方程离散化。其中采样周期与滤波周期均取为T_s，步骤1中的状态方程和量测方程离散化为

其中x_k为k时刻系统的状态量，z_k为k时刻的系统状态的量测值，w_x；k-1～N(0,Q_k-1)为系统过程噪声，w_z；k～N(0,R_k)为系统观测噪声，F(x_k-1),G_k-1,H(x_k-1)为将步骤1对应的方程离散化后所得的函数。

步骤3：在不需要快速对准的情况下，以状态方程和量测方程执行改进的SRCKF的时间更新过程，获取状态的预测估计值和预测误差协方差的平方根S_k|k-1。

改进的SRCKF框架下时间更新的具体步骤如下：

步骤3.1：设置系统状态初始值初始误差协方差阵

P₀＝diag{(1°)²(1°)²(10°)²(0.1m/s)²(0.1m/s)²(0.01°/h)²

(0.01°/h)²(0.01°/h)²(100μg/h)²(100μg/h)²}

上式中°为角度单位，m/s为速度单位，h为时间单位，μg＝10^-6g。对上述初始误差协方差阵P₀进行Cholesky分解得初始误差协方差矩阵的特征平方根S₀＝Chol(P₀)，其中Chol(·)表示矩阵的Cholesky分解。

步骤3.2：按照球面最简规则径向(SSR)容积规则选取改进SRCKF的采样点，如下：

取向量a_j＝[a_j,1 a_j,1 … a_j,n]^T,j＝1,2,...,n+1，其中n＝10为状态量的个数，有

记ξ_i为第i个容积点，则得到m＝2(n+1)个容积点为：

步骤3.3：计算状态的一步预测值和一步预测误差协方差矩阵的平方根因子：

当前状态容积点与预测状态容积点为：

k时刻状态的一步预测值和一步预测误差协方差的平方根为：

其中S＝Tria(A)表示由A^T进行QR分解得到的上三角阵R'＝QR(A^T)的转置S＝R'^T，S_Q,k-1为矩阵的Cholesky分解。即上式中S_k|k-1的平方根因子由

及

给出，其中Q_k-1为系统过程噪声方差阵。

步骤4：以改进SRCKF时间更新的预测估计值和预测误差协方差的平方根S_k|k-1为初始值进行迭代，迭代至最大次数N_max，完成量测更新。

改进SRCKF框架下量测更新的过程采用迭代算法，具体步骤如下：

步骤4.1：以步骤3中得到的状态预测值和预测误差协方差S_k|k-1为迭代的初始值，记k时刻第j次迭代的状态估计值和误差协方差分别为第0次迭代的状态估计值和误差协方差，即迭代的初始值

步骤4.2：对于j＝0,1,...,N_max执行以下过程：

计算新的容积点：

按如下步骤计算k时刻第j次迭代的状态和误差协方差平方根：

计算k时刻第j次迭代的第i个预测量测容积点：

计算预测量测值：

计算新息协方差矩阵的平方根：

其中上式中的平方根因子由

及

给出。

计算估计互协方差矩阵：

其中加权中心矩阵：

估计CKF增益：

估计更新状态：

估计更新后的状态误差协方差平方根因子：

步骤4.3：迭代的终止条件为其中Δ为预先设定的迭代阈值；或迭代次数达到最大值N_max，若未满足上述两个条件之一，则重复权利要求4的步骤。在具体应用中，可能因初始对准量测采用速率误差、位置误差为观测量时量测方程为线性方程，使上述迭代方法由于不能加速新息的反馈效率而失效，可取N_max＝1，即不进行迭代。

步骤4.4：终止迭代时假设迭代次数j＝N，则设置k时刻的状态估计值和状态估计误差的协方差平方根分别为：

步骤5：由前面步骤4获取的状态量中提取欧拉角平台估计值和速度估计值修正SINS解算的姿态矩阵和速度将修正后的值作为下一次SINS解算的初始值，并利用当前获得的陀螺仪和加速度计常值误差估计值修正下一时刻的陀螺仪输出和加速度计输出修正公式如下：

步骤6：当改进SRCKF运行一定时间后，每隔一个固定的时间间隔保存系统测量数据，即东向和北向速度误差等，以及SINS解算的姿态误差角，构建以后需要快速对准环境下采用GPR模型算法需要的训练数据集，共保存N组数据。根据步骤4过程，如果对准精度没有达到预定要求则循环执行执行步骤3-6直至SINS初始对准结束。

步骤7：在需要快速对准的环境下，根据上一个初始对准过程中保存的训练数据集D采用GPR模型算法进行学习；同时将当前SINS实时解算的量测数据作为输入将姿态误差角作为输出，更新GPR的训练数据集进行学习，获得系统的状态转移GPR模型和量测GPR模型；再利用获得系统的状态转移GPR模型和量测GPR模型，获取系统当前对准过程的姿态误差角以及误差方差，完成系统的初始对准。

其中，GPR模型算法具体步骤如下：

步骤7.1：在需要快速对准的环境下，根据上一个初始对准过程中保存的N组输入输出数据构成的数据训练集D进行GPR模型学习，获取系统的状态转移GPR模型和量测GPR模型。更具体的步骤为：

步骤7.1.1：其中数据训练集D＝{D_e,D_n,D_u}有下述数据训练子集D_e＝{ΔV,Φ_e}；D_e＝{ΔV,Φ_e}为用于估计建立东向误差角φ_e的GPR模型采用的数据训练集，其中ΔV＝[Δv₁Δv₂ … Δv_N]^T，Δv_κ＝(δv_eκ,δv_nκ)^T∈R²(κ＝1,...,N)为保存的所有N组量测(δv_e,δv_n)中的第κ组；Φ_e＝[φ_e1 φ_e2 … φ_eN]^T，φ_eκ∈R(κ＝1,...,N)为保存的N个东向误差角中的第κ组。针对东北天三个方向的姿态误差角φ_e,φ_n,φ_u相应有输出向量Φ_e,Φ_n,Φ_u共保存了3N个姿态误差角数据，共有三个数据训练子集D_e,D_n,D_u。

步骤7.1.2：根据步骤7.1.1的数据集进行GPR模型学习，初始化GPR模型：采用高斯核函数作为GPR模型的初始化核函数：

式中Δv_p,Δv_q为数据训练集量测ΔV中的任意元素，为信号方差，l₁,l₂为信号方差；上述未知参数集记为超参数其中为数据训练集的样本噪声方差。

步骤7.1.3：以下以建立东向姿态误差角的GPR模型进行说明。以量测ΔV和东向姿态误差角Φ_e＝[φ_e1 φ_e2 … φ_eN]构成的数据训练集D_e＝(ΔV,Φ_e)作为GPR模型的输入输出对建立训练样本条件函数p(Θ_e|ΔV_κ,Θ_κ)的对数似然函数：

式中K＝K(ΔV,ΔV)＝{k(Δv_p,Δv_q)|p,q＝1,...,N}为N×N阶对称正定协方差矩阵，K(p,q)＝k(Δv_p,Δv_q)度量了Δv_p,Δv_q的相关性。

通过最大化似然函数求取超参数超参数，即对似然函数求偏导：

其中α＝E^-1Φ_e，tr(·)为求矩阵迹运算，Θ_i表示中的第i＝1,2,3个元素，K(ΔV,ΔV)为前面定义的协方差矩阵。

采用共轭梯度法对上述偏导数进行最小化，从而得到超参数的最优解。

步骤7.1.4：针对三个姿态误差角，重复以上步骤可以分别获得相应参数。以上步骤可以在当前初始对准过程之前完成。

步骤7.2：在当前对准过程中，利用改进的SRCKF获取当前时刻的量测值Δv^*，对系统实时的量测与截止至前一时刻的姿态误差角输出更新训练数据集D，根据GPR模型获取当前量测Δv^*下的姿态误差角及其误差方差，进行SINS初始对准，获取系统当前对准过程的姿态误差角以及相应的误差方差。具体的步骤如下：

步骤7.2.1：由高斯过程回归模型中数据训练集的输出矢量Φ_e和预测值φ_e ^*之间满足：

其中

K(Δv^*,ΔV)＝K(ΔV,Δv^*)^T＝(k(Δv^*,Δv₁),...,k(Δv^*,Δv_N))^T，k(Δv^*,Δv_i)(i＝1,...,N)为当前量测值Δv^*与训练数据集中的量测值Δv_i的协方差，k(Δv^*,Δv^*)为实时量测值的自协方差。由此可以得出在当前时刻量测值Δv^*下的预测估计输出值φ_e ^*的均值和协方差如下：

由此可以完成当前对准过程姿态误差角φ_e的估计。

步骤7.2.2：对于其他两个姿态误差角φ_n,φ_u，可以类似上述步骤，获取其当前对准过程中的估计均值和相应的误差方差从而完成SINS初始对准过程。

通过以上方法，本发明能够增强SRCKF的鲁棒性，并减少高阶SSRCKF的计算量，在需要快速对准的环境下能快速获取较准确的初始对准姿态误差角。

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (10)

1.基于GPR与改进的SRCKF的SINS初始对准方法，其特征在于，包括如下步骤：

步骤1：建立导航系统的初始对准模型，所述对准模型包括捷联惯性导航系统的非线性误差模型、滤波模状态模型以及量测模型；

步骤2：将步骤所得的状态方程和量测方程离散化；

步骤3：在不需要快速对准的情况下，以状态方程和量测方程执行改进的SRCKF的时间更新过程，获取状态的预测估计值和预测误差协方差的平方根；

步骤4：以改进SRCKF时间更新的预测估计值和预测误差协方差的平方根为初始值进行迭代，迭代至最大次数，完成量测更新；

步骤5：由步骤4获取的状态量中提取欧拉角平台估计值和速度估计值修正SINS解算的姿态矩阵和速度，将修正后的值作为下一次SINS解算的初始值，并利用当前获得的陀螺仪和加速度计常值误差估计值修正下一时刻的陀螺仪输出和加速度计输出；

步骤6：当改进SRCKF运行一定时间后，每隔一个固定的时间间隔保存系统测量数据，构建以后需要快速对准环境下采用GPR模型算法需要的训练数据集，共保存N组数据；

步骤7：在需要快速对准的环境下，根据上一个初始对准过程中保存的训练数据集D采用GPR模型算法进行学习；同时将当前SINS实时解算的量测数据作为输入将姿态误差角作为输出，更新GPR的训练数据集进行学习，获得系统的状态转移GPR模型和量测GPR模型；再利用获得系统的状态转移GPR模型和量测GPR模型，获取系统当前对准过程的姿态误差角以及误差方差，完成系统的初始对准。

2.根据权利要求1所述的基于GPR与改进的SRCKF的SINS初始对准方法，其特征在于，所述步骤1具体包括如下子步骤：

步骤1.1：建立捷联惯性导航系统的非线性误差模型：

选择东-北-天坐标系地理坐标系为理想导航坐标系，以载体右前上方向构建载体坐标系，记SINS模拟解算的数学平台坐标系为n’系；记b系与n系之间的真实姿态角为b系为载体坐标系，n系为理想导航坐标系，载体相对于导航坐标系的真实速度为载体所在的真实地理坐标为p＝[L λ H]^T，SINS实际解算出的姿态为速度为地理坐标为记SINS解算的姿态角误差为其中φ_e,φ_n,φ_u分别为，纵摇角误差、横摇角误差和航向角误差；速度误差其中δv_e,δv_n,δv_u分别为东向、北向和天向速度误差，则：

n系到n’系依次旋转φ_u,φ_e,φ_n的坐标系转换矩阵为：

欧拉角微分方程的系数矩阵C_ω为：

姿态误差微分方程为：

其中为b系下的三轴陀螺常值误差，为b系下三轴陀螺随机误差，为计算出的n系相对惯性坐标系的旋转角速度，为b系到n’系状态转移矩阵，也即SINS解算的姿态矩阵；

速度误差方程为：

其中为b系下测得的加速度计实际输出，为b系下三轴加速度计的常值误差，为b系下三轴加速度计随机误差，分别为对应的计算误差，g为重力加速度，T表示转置；

步骤1.2：建立SINS状态方程模型如下：

取状态向量为其中由于整个对准过程中近似为零，则状态方程可化为如下形式：

其中根据选取的状态量，上式速度相关的分量为设过程噪声向量为零均值高斯白噪声；地球相对惯性系的角速度ω_ie在n系下的投影L为载体所在的纬度，上述状态方程可记为：

其中f(x,t)为根据前面状态方程所得的函数，g(t)为根据上述系统过程噪声向量确定的系数矩阵；

步骤1.3：建立量测方程如下：

其中v^b为b系下的真实速度，为b系下计算用的实际速度，SINS将转换为w_z为零均值高斯白噪声过程，仅以和中的东向和北向速度误差分量作为匹配信息源；上述量测方程简记为：

z(t)＝h(x,t)+w_z(t)

其中h(x,t)为根据前面量测方程所得的函数。

3.根据权利要求2所述的基于GPR与改进的SRCKF的SINS初始对准方法，其特征在于，所述步骤2中采样周期与滤波周期均取为T_s，本步骤将步骤1中的状态方程和量测方程离散化为：

其中x_k为k时刻系统的状态量，z_k为k时刻的系统状态的量测值，w_x；k-1～N(0,Q_k-1)为系统过程噪声，w_z；k～N(0,R_k)为系统观测噪声，F(x_k-1),G_k-1,H(x_k-1)为将步骤1对应的方程离散化后所得的函数。

4.根据权利要求3所述的基于GPR与改进的SRCKF的SINS初始对准方法，其特征在于，所述步骤3中改进的SRCKF框架下时间更新的具体步骤如下：

步骤3.1：设置系统状态初始值初始误差协方差阵

P₀＝diag{(1°)²(1°)²(10°)²(0.1m/s)²(0.1m/s)²(0.01°/h)²

(0.01°/h)²(0.01°/h)²(100μg/h)²(100μg/h)²}

上式中°为角度单位，m/s为速度单位，h为时间单位，μg＝10^-6g；对上述初始误差协方差阵P₀进行Cholesky分解得初始误差协方差矩阵的特征平方根S₀＝Chol(P₀)，其中Chol(·)表示矩阵的Cholesky分解；

步骤3.2：按照球面最简规则径向(SSR)容积规则选取改进SRCKF的采样点，如下：

取向量a_j＝[a_j,1 a_j,1 … a_j,n]^T,j＝1,2,...,n+1，其中n＝10为状态量的个数，有

记ξ_i为第i个容积点，则得到m＝2(n+1)个容积点为：

步骤3.3：计算状态的一步预测值和一步预测误差协方差矩阵的平方根因子：

当前状态容积点与预测状态容积点为：

k时刻状态的一步预测值和一步预测误差协方差的平方根为：

其中S＝Tria(A)表示由A^T进行QR分解得到的上三角阵R'＝QR(A^T)的转置S＝R'^T，S_Q,k-1为矩阵的Cholesky分解；即上式中S_k|k-1的平方根因子由

及

给出，其中Q_k-1为系统过程噪声方差阵。

5.根据权利要求4所述的基于GPR与改进的SRCKF的SINS初始对准方法，其特征在于，所述步骤4中改进SRCKF框架下量测更新的过程采用迭代算法，具体步骤如下：

步骤4.1：以步骤3中得到的状态预测值和预测误差协方差S_k|k-1为迭代的初始值，记k时刻第j次迭代的状态估计值和误差协方差分别为第0次迭代的状态估计值和误差协方差，即迭代的初始值

步骤4.2：对于j＝0,1,...,N_max执行以下过程：

计算新的容积点：

按如下步骤计算k时刻第j次迭代的状态和误差协方差平方根：

计算k时刻第j次迭代的第i个预测量测容积点：

计算预测量测值：

计算新息协方差矩阵的平方根：

其中上式中的平方根因子由

及

给出；

计算估计互协方差矩阵：

其中加权中心矩阵：

估计CKF增益：

估计更新状态：

估计更新后的状态误差协方差平方根因子：

步骤4.3：迭代的终止条件为其中Δ为预先设定的迭代阈值；或迭代次数达到最大值N_max，若未满足上述两个条件之一，则重复权利要求4的步骤；

步骤4.4：终止迭代时假设迭代次数j＝N，则设置k时刻的状态估计值和状态估计误差的协方差平方根分别为：

6.根据权利要求5所述的基于GPR与改进的SRCKF的SINS初始对准方法，其特征在于，所述步骤4.3中当迭代方法由于不能加速新息的反馈效率而失效时，取N_max＝1，即不进行迭代。

7.根据权利要求5所述的基于GPR与改进的SRCKF的SINS初始对准方法，其特征在于，所述步骤5中修正公式如下：

其中，和分别为SINS解算的姿态矩阵和速度，为当前获得的陀螺仪和加速度计常值误差估计值，为下一时刻的陀螺仪输出和加速度计输出。

8.根据权利要求1所述的基于GPR与改进的SRCKF的SINS初始对准方法，其特征在于，所述步骤6中系统测量数据包括东向和北向速度误差，以及SINS解算的姿态误差角。

9.根据权利要求1所述的基于GPR与改进的SRCKF的SINS初始对准方法，其特征在于，当对准精度没有达到预定要求则循环执行执行步骤3-6直至SINS初始对准结束。

10.根据权利要求7所述的基于GPR与改进的SRCKF的SINS初始对准方法，其特征在于，所述步骤7中GPR模型算法具体步骤如下：

步骤7.1：在需要快速对准的环境下，根据上一个初始对准过程中保存的N组输入输出数据构成的数据训练集D进行GPR模型学习，获取系统的状态转移GPR模型和量测GPR模型，更具体的步骤为：

步骤7.1.1：其中数据训练集D＝{D_e,D_n,D_u}有下述数据训练子集D_e＝{ΔV,Φ_e}；D_e＝{ΔV,Φ_e}为用于估计建立东向误差角φ_e的GPR模型采用的数据训练集，其中ΔV＝[Δv₁Δv₂ … Δv_N]^T，Δv_κ＝(δv_eκ,δv_nκ)^T∈R²(κ＝1,...,N)为保存的所有N组量测(δv_e,δv_n)中的第κ组；Φ_e＝[φ_e1 φ_e2 … φ_eN]^T，φ_eκ∈R(κ＝1,...,N)为保存的N个东向误差角中的第κ组；针对东北天三个方向的姿态误差角φ_e,φ_n,φ_u相应有输出向量Φ_e,Φ_n,Φ_u共保存了3N个姿态误差角数据，共有三个数据训练子集D_e,D_n,D_u；

步骤7.1.2：根据步骤7.1.1的数据集进行GPR模型学习，初始化GPR模型：采用高斯核函数作为GPR模型的初始化核函数：

式中Δv_p,Δv_q为数据训练集量测ΔV中的任意元素，为信号方差，l₁,l₂为信号方差；上述未知参数集记为超参数其中为数据训练集的样本噪声方差；

步骤7.1.3：建立东向姿态误差角的GPR模型：以量测ΔV和东向姿态误差角Φ_e＝[φ_e1φ_e2 … φ_eN]构成的数据训练集D_e＝(ΔV,Φ_e)作为GPR模型的输入输出对建立训练样本条件函数p(Θ_e|ΔV_κ,Θ_κ)的对数似然函数：

式中K＝K(ΔV,ΔV)＝{k(Δv_p,Δv_q)|p,q＝1,...,N}为N×N阶对称正定协方差矩阵，K(p,q)＝k(Δv_p,Δv_q)度量了Δv_p,Δv_q的相关性；

通过最大化似然函数求取超参数超参数，即对似然函数求偏导：

其中α＝E^-1Φ_e，tr(·)为求矩阵迹运算，Θ_i表示中的第i＝1,2,3个元素，K(ΔV,ΔV)为前面定义的协方差矩阵；

采用共轭梯度法对上述偏导数进行最小化，从而得到超参数的最优解；

步骤7.1.4：针对三个姿态误差角，重复以上步骤分别获得相应参数；

步骤7.2：在当前对准过程中，利用改进的SRCKF获取当前时刻的量测值Δv^*，对系统实时的量测与截止至前一时刻的姿态误差角输出更新训练数据集D，根据GPR模型获取当前量测Δv^*下的姿态误差角及其误差方差，进行SINS初始对准，获取系统当前对准过程的姿态误差角以及相应的误差方差；具体的步骤如下：

步骤7.2.1：由高斯过程回归模型中数据训练集的输出矢量Φ_e和预测值φ_e ^*之间满足：

其中

K(Δv^*,ΔV)＝K(ΔV,Δv^*)^T＝(k(Δv^*,Δv₁),...,k(Δv^*,Δv_N))^T，k(Δv^*,Δv_i)(i＝1,...,N)为当前量测值Δv^*与训练数据集中的量测值Δv_i的协方差，k(Δv^*,Δv^*)为实时量测值的自协方差；由此得出在当前时刻量测值Δv^*下的预测估计输出值φ_e ^*的均值和协方差如下：

由此可以完成当前对准过程姿态误差角φ_e的估计；

步骤7.2.2：对于其他两个姿态误差角φ_n,φ_u，通过上述步骤，获取其当前对准过程中的估计均值和相应的误差方差从而完成SINS初始对准过程。