一种波浪滑翔器短时定位缺失的导航方法及系统.pdf

(19)国家知识产权局(12)发明专利申请(10)申请公布号CN114689039A(43)申请公布日2022.07.01(21)申请号202210610414.0(22)申请日2022.06.01(71)申请人青岛海舟科技有限公司地址266235山东省青岛市即墨区鳌山卫街道办事处海科创业中心D座406、407、409室(72)发明人孙秀军桑宏强周莹于佩元张帅孙超(74)专利代理机构北京高沃律师事务所11569专利代理师赵兴华(51)Int.Cl.G01C21/00(2006.01)G01C21/20(2006.01)G01S19/45(2010.01)权利要求书4页说明书9页附图2页(54)发明名称一种波浪滑翔器短时定位缺失的导航方法及系统(57)摘要本发明公开了一种波浪滑翔器短时定位缺失的导航方法及系统，涉及导航与控制领域，导航方法包括以下步骤：判断GPS定位模块是否正常工作；若是，则根据GPS定位模块提供的位置信息进行导航路径解算，得到导航路径；若否，则分别获取由GPS定位模块提供的最后有效位置信息、由多普勒计程仪提供航行的速度信息以及由航姿参考模块提供的姿态信息与角速度信息；采用滤波算法对最后有效位置信息、速度信息、姿态信息与角速度信息进行航位推算，得到波浪滑翔器的当前位置信息；根据波浪滑翔器的当前位置信息进行导航路径解算，得到导航路径。本发明能够实现在定位信息暂时性缺失情况下波浪滑翔器的位置推算与导航功能，保证波浪滑翔器的航行安全。CN114689039ACN114689039A权利要求书1/4页1.一种波浪滑翔器短时定位缺失的导航方法，其特征在于，所述波浪滑翔器搭载GPS定位模块、多普勒计程仪和航姿参考模块，所述导航方法包括以下步骤：判断所述GPS定位模块是否正常工作；若是，则根据所述GPS定位模块提供的位置信息进行导航路径解算，得到导航路径；若否，则分别获取由所述GPS定位模块提供的最后有效位置信息、由所述多普勒计程仪提供航行的速度信息以及由所述航姿参考模块提供的姿态信息与角速度信息；采用滤波算法对所述最后有效位置信息、所述速度信息、所述姿态信息与所述角速度信息进行航位推算，得到波浪滑翔器的当前位置信息；根据所述波浪滑翔器的当前位置信息进行导航路径解算，得到导航路径。2.根据权利要求1所述的一种波浪滑翔器短时定位缺失的导航方法，其特征在于，所述导航方法还包括：将所述多普勒计程仪提供航行的速度信息进行坐标变换，得到东向速度信息与北向速度信息。3.根据权利要求1所述的一种波浪滑翔器短时定位缺失的导航方法，其特征在于，所述采用滤波算法对所述最后有效位置信息、所述速度信息、所述姿态信息与所述角速度信息进行航位推算，得到波浪滑翔器的位置信息，具体包括：构建波浪滑翔器航位推算的离散状态方程和观测方程；所述离散状态方程为：x(k+1)=Ak+1,kx(k)+Bk+1,kw(k)其中，x(k+1)为离散状态方程矩阵，k为更新次数，Ak+1,k为状态转移矩阵，Ak+1,k=diag[Ae(k+1,k),An(k+1,k)]，，Ae(k+1,k)为东向状态转移矩阵，An(k+1,k)为北向状态转移矩阵，为采样时间，τ为定值，x(k)为状态量，Bk+1,k为控制输入矩阵，2CN114689039A权利要求书2/4页，w(k)为过程噪声，，为加速度的标准差；所述观测方程为：z(k+1)=h(x(k))+v(k)其中，z(k+1)为观测方程矩阵，h(x(k))为状态观测矩阵，，，x为观测量的定义，xe为东向位置，为东向速度，为东向加速度，xn为北向位置，为北向速度，为北向加速度，v(k)为测量噪声矩阵，，a1为GPS定位模块的第一测量噪声，a2为GPS定位模块的第二测量噪声，，为GPS定位模块的噪声值，v1为多普勒计程仪的第一测量噪声，v2为多普勒计程仪的第二测量噪声，，为多普勒计程仪的噪声值，w1为航姿参考模块的角速度3CN114689039A权利要求书3/4页测量噪声，，为航姿参考模块的角速度的噪声值；根据所述离散状态方程和所述观测方程确定所述波浪滑翔器的位置信息。4.根据权利要求1所述的一种波浪滑翔器短时定位缺失的导航方法，其特征在于，所述滤波算法为扩展卡尔曼滤波。5.根据权利要求4所述的一种波浪滑翔器短时定位缺失的导航方法，其特征在于，所述扩展卡尔曼滤波的递推公式为：其中，为k‑1时刻对k时刻状态值的估计，f为状态转移函数，Pk|k‑1为k‑1时刻预测的k时刻的误差协方差，为k‑1时刻到k时刻的一步转移矩阵，，为k‑1时刻的状态估计值，Pk‑1为k‑1时刻的系统推测的均方误差，，xk为状态方程，为k‑1时刻到k时刻的一步转移矩阵的转置，Rwk‑1为过程噪声协方差，Kk为增益矩阵，Hk为量测矩阵，，h为测量函数，为量测矩阵的转置，R