[发明专利]基于SRQKF的SINS/DVL水下大失准角对准方法

摘要

本发明公开一种基于SRQKF的SINS/DVL水下大失准角对准方法，包括如下步骤：步骤1：建立捷联惯性导航系统SINS在大失准角下的非线性误差模型和非线性滤波方程；步骤2：利用Gauss‑Hermite求积分中多变量Gauss点及其系数配置方法以及平方根滤波方法，构建平方根求积分卡尔曼滤波器SRQKF；步骤3：利用平方根求积分卡尔曼滤波SRQKF估计出失准角，并修正捷联姿态矩阵，得到精确的捷联姿态矩阵与姿态角。本发明提高了载体捷联系统水下对准精度和对准速度。

主权项

1.一种基于SRQKF的SINS/DVL水下大失准角对准方法，其特征在于，包括如下步骤：步骤1：建立捷联惯性导航系统SINS在大失准角下的非线性误差模型和非线性滤波方程；步骤2：利用Gauss‑Hermite求积分中多变量Gauss点及其系数配置方法以及平方根滤波方法，构建平方根求积分卡尔曼滤波器SRQKF；步骤3：利用平方根求积分卡尔曼滤波SRQKF估计出失准角，并修正捷联姿态矩阵，得到精确的捷联姿态矩阵与姿态角；其中，所述步骤1包括：步骤1.1：选取“东北天”地理坐标系为导航坐标系n；选取载体“右前上”坐标系为载体坐标系b；n系先后经过3次欧拉角转动至b系；n系与b系之间的姿态矩阵记为真实姿态角记为其中，ψ为真实航向角，θ为真实纵摇角，γ为真实横摇角；真实速度记为SINS解算出的姿态角记为：SINS解算速度记为：SINS解算出的数学平台记为n'系，n'系与b系之间的姿态矩阵记为n系先后经过3次转动至n'系，三次转动角分别为φ_u、φ_e、φ_n，称它们为欧拉平台误差角，记矢量定义速度误差为：则非线性误差模型如下：其中，为b系下三轴陀螺的常值误差，其中为陀螺x轴常值误差，为陀螺y轴常值误差，为陀螺z轴常值误差，为b系下三轴陀螺的随机误差，为b系下三轴加速度常值误差，为b系下三轴加速度的随机误差，为加速度计的实际输出，为解算出的数学平台旋转角速度，为地球旋转角速度，为解算出的数学平台相对于地球的旋转角速度，为对应的计算误差；为欧拉角微分方程系数矩阵的逆矩阵；为n'系与n系之间的姿态矩阵，C_ω和具体为：步骤1.2：所述非线性滤波方程建立过程如下：选取速度误差欧拉角平台误差角φ_u、φ_e、φ_n；b系下的陀螺x轴、y轴、z轴常值误差以及b系下的加速度计常值误差构成10维状态向量：在晃动基座下近似为零，非线性滤波状态方程可建立为：和只取前两维状态，和为零均值白噪声过程，该非线性状态方程可简记为：设滤波周期为T，可以使用四阶龙格‑库塔积分方法，以滤波周期T的值为步长将其离散化，具体步骤如下：假设选取的状态向量在初始时刻的值x₀已知，且记t₁＝t+T/2，则可以通过以下迭代公式将离散：Δx1＝[f(x,t)+ω(t)]TΔx2＝[f(x,t1)+Δx1/2+ω(t1)]TΔx3＝[f(x,t1)+Δx2/2+ω(t1)]TΔx4＝[f(x,t+T)+Δx3+ω(t+T)]Txk+1＝xk+(Δx1+2Δx2+2Δx3+Δx4)/6记离散后状态滤波方程为：xk＝f(xk‑1)+ωk‑1选取速度作为观测量建立非线性量测方程，具体步骤如下：以载体SINS和DVL输出的东向速度和北向速度分量作为匹配信息源，建立的非线性量测方程为：其中，ν^b为b系下的真实速度；为b系下的实际速度，即DVL输出的速度；取z前两维为观测值；ν为零均值高斯白噪声过程，该非线性滤波量测方程可简记为：z＝h(x,t)+ν(t)；同样以滤波周期T的值作为步长进行简单离散化，将z＝h(x,t)+ν(t)离散化为：zk＝h(xk)+νk；由状态方程和量测方程组成如下非线性滤波方程：