[发明专利]一种地形辅助惯导紧组合的水下潜器导航定位方法

摘要

本发明公开了一种地形辅助惯导紧组合的水下潜器导航定位方法，主要步骤包括：初始化组合导航系统；利用非线性离散状态方程进行状态量一步预测；利用非线性离散量测方程计算粒子滤波器的重要性权值；归一化重要性权值并估计潜器的导航参数；进行重要性重采样；完成非线性粒子滤波算法。本发明方法能有效、充分地利用各子系统的信息，相比地形辅助惯导松组合方式，定位的精度有明显提高，保证了水下潜器的长航时导航定位精度。

主权项

1.一种地形辅助惯导紧组合的水下潜器导航定位方法，其特征在于，该方法包括以下步骤：步骤1)初始化地形辅助惯导组合导航定位系统，为系统的导航参数更新提供初始值：通过全球导航卫星系统获取潜器的初始位置参数；根据惯性测量单元采集的潜器角速率和比力信息，以及多普勒测速仪测得的载体系速度，进行捷联惯性导航系统的初始对准，获取潜器的初始姿态矩阵和导航系初始速度信息；初始时刻k＝0，初始化M个粒子，根据先验概率分布进行粒子初始化；步骤2)令k:＝k+1，利用以潜器位置、姿态角和陀螺漂移为状态量的非线性离散状态方程进行一步预测，得到k时刻的一步预测状态量xk；步骤3)利用非线性离散量测方程处理水深数据并提取水平距离，计算波束投影点水深误差、潜器的东向距离误差和北向距离误差，然后计算重要性权值，所述非线性离散量测方程为：式中：yk为k时刻波束投影点到潜器的天向距离，yk＝[0 0 1]R(qk)ark；qk为一步预测姿态角，R(qk)为捷联惯性导航系统计算的潜器由b系到n系的一步预测姿态矩阵；rk为k时刻单波束测深仪测量的波束距离，在b系下为负值，a为b系下波束方位向量；为波束投影点在海图上的水深值，通过双线性插值获取该水深值；xE,k和xN,k分别表示k时刻一步预测状态量xk中的东向位置和北向位置分量；zk是k时刻压力传感器测得的潜器的下潜深度；dk为多普勒测速仪推算的潜器在k和k‑1相邻水深测量时刻的水平距离，d＝[dE,dN]T，dE和dN分别为相邻水深测量时刻潜器的东向距离和北向距离分量；为第i个粒子一步预测的水平位置和k‑1时刻潜器的水平位置估计值之间的距离，包括东向距离和北向距离分量；为第i个粒子的一步预测状态量，这里具体指其中的东向位置和北向位置分量；为k‑1时刻潜器的状态量估计值，这里具体指其中的东向位置和北向位置估计值；ηk是距离测量噪声，ηk～Ν(0,Ση)，Ση为距离噪声方差；步骤4)归一化重要性权值，得到k时刻状态量的最小均方估计为将其作为组合后的潜器导航参数输出；步骤5)进行重要性重采样：得到新的粒子集合步骤6)判断潜器航行时间是否结束，若是则结束本方法，否则返回步骤2)。