一种基于非线性模型的SINS/DVL组合导航方法技术

本发明专利技术公开了一种基于非线性模型的SINS/DVL组合导航方法，根据捷联惯导系统和多普勒计程仪的工作原理，建立基于四元数的SINS非线性速度、姿态以及位置误差模型，并确定DVL的误差模型；利用两个系统的误差模型建立系统的状态方程，并将SINS以及DVL两者实际测得的速度作差作为量测量，建立系统的量测方程；将实际的连续系统进行离散化，得到便于计算的离散的非线性模型；初始化系统，利用离散非线性模型，通过Unscented变换计算采样点及相应权值；根据构造的Sigma点按照离散化非线性模型依次进行无迹卡尔曼滤波的时间更新和量测更新。本发明专利技术中的SINS/DVL组合导航系统能有效地利用各子系统的信息，相互取长补短，使得定位的总精度大大提高，同时利用SINS/DVL组合导航系统的非线性模型进行UKF估计，可以有效降低系统定位误差，更好的实现导航系统的精确定位。

【技术实现步骤摘要】

【技术保护点】
一种基于非线性模型的SINS/DVL组合导航方法，其特征在于，包括如下步骤：步骤一：建立SINS的非线性误差模型，首先将需要用到的导航坐标系做如下定义：i——惯性坐标系e——地球坐标系b——载体坐标系p——计算得出的平台坐标系n——与东北天地理坐标系重合的导航坐标系通过理想惯导比力方程的速度微分方程以及捷联系统实际的速度微分方程，推导出四元数表示的速度误差方程：δV·n=(I-C(Qpn))C(Qbp)fb-(2ωien+ωenn)×δVn-(2δωien+δωenn)×Vn-(2δωien+δωenn)×δVn+C(Qbp)▿b+δgn其中：Vn为载体在n系下的理想速度，是n系下的地球自转角速度，为n系相对于e系的角速度在n系下的投影，fb为b系下的比力；式中：Qpn=q0q1q2q3T为p系到n系的转换四元数，Qbp=qb0qb1qb2qb3T为b系到p系的转换四元数，分别代表p系到n系以及b系到p系的转换矩阵，δωien=ω~ien-ωien,δωenn=ω~enn-ωenn,f~n=fp+▿p,δgn=g~n-gn式中：“～”为载体的实际测量值，δ·表示载体的理想值与实际测量值间的误差，gn为n系下的重力加速度，为b系下的加速度计误差向量；四元数姿态误差方程：Q·pn=B(I-C(Qnp))ωinn-BC(Qbp)ϵb其中：表示n系相对于i系的旋转角速度在n系下的投影，为n系到p系的方向余弦矩阵，εb为陀螺仪误差向量在b系下的投影，B为关于四元数的4×3维矩阵，位置误差方程：其中：RM和RN分别表示地球的子午圈曲率半径和卯酉圈曲率半径，λ为载体的经度，为载体的纬度，Vx和Vy分别为运载体的东向和北向速度；步骤二、建立DVL误差模型，DVL测量的是载体相对海底或某一流层的速度Vd和偏流角Δ，其测量误差主要由刻度系数误差δC，速度偏移误差δVd和偏流角误差δΔ产生，δC为随机常数，δVd和δΔ可用一阶马尔可夫过程表示，则相应的误差模型为；δV·d=-βdδVd+ωdδV·Δ=-βΔδVΔ+ωΔδC·=0其中：βd，βΔ表示速度偏移误差和偏流角误差相关时间的倒数，ωd和ωΔ为激励白噪声；步骤三、利用步骤一获得的SINS系统模型和步骤二获得的DVL误差模型确定系统的非线性状态方程和量测方程；系统状态方程为：X·(t)=f[X(t),t]+Γ(t)W(t)其中：X(t)为系统状态量，W(t)为系统噪声向量，Γ(t)为系统噪声阵；取SINS系统的解算速度和DVL测量速度之差作为观测量，并考虑测量噪声，则系统量测方程为：Z(t)＝h[X(t)，t]+V(t)其中：Z(t)为系统量测量，KV表示考虑偏流角的航迹向，V(t)为量测噪声向量；步骤四、将步骤三中的系统状态方程和量测方程离散化的系统模型为：Xk+1＝f[Xk，k]+ΓkWkZk＝h[Xk，k]+Vk其中：Xk和Zk分别为系统在tk时刻的状态向量以及量测向量，Wk和Vk分别为系统噪声阵和量测噪声阵，且均值为零，Q(k)和R(k)分别为系统噪声协方差阵和量测噪声协方差阵；步骤五、基于步骤四中的离散化数学模型，利用Unscented变换，开始进行UKF滤波算法，系统的状态Xk为n维随机变量，其均值为方差为Px，那么2n+1个n维Sigma点χi为：χi=X‾,i=0χi=X‾+((n+λ)Px)i,i=1,2,...,nχi=X‾-((n+λ)Px)i,i=n+1,n+2,...,2n其中：λ为尺度因子，表示方根矩阵的第i列；各点所对应的权值为：Wim=λn+λ,i=0Wic=λn+λ+(1-&alpha...

【技术特征摘要】

【专利技术属性】
技术研发人员：高伟，王文佳，于飞，徐博，池姗姗，肖永平，陈春，郭宇，杨建，田学林，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：