一种基于统计相似度量的组合导航鲁棒滤波方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN113819906A(43)申请公布日2021.12.21(21)申请号202111115235.1(22)申请日2021.09.23(71)申请人哈尔滨工程大学地址150001黑龙江省哈尔滨市南岗区南通大街145号哈尔滨工程大学科技处知识产权办公室(72)发明人徐博郭瑜赵玉新吴磊胡俊苗陈崇王连钊李盛新王潇雨王朝阳(51)Int.Cl.G01C21/16(2006.01)G01C21/20(2006.01)权利要求书3页说明书11页附图4页(54)发明名称一种基于统计相似度量的组合导航鲁棒滤波方法(57)摘要本发明属于非理想条件下的组合导航技术领域，具体涉及一种基于统计相似度量的组合导航鲁棒滤波方法。本发明考虑SINS/DVL紧组合系统中正常的DVL波束测量信息和大误差的波束测量信息同时出现的情形，针对目前现有的组合导航系统鲁棒滤波器对量测信息处理粗糙，导致正常量测信息易丢失的问题，基于多维量测方程分解和统计相似度量提出了新的鲁棒滤波方法。本发明在将SINS/DVL紧组合导航系统的多维量测方程进行分解的同时，引入统计相似度量(SSM)理论，协助每个波束的测量噪声方差在大测量误差出现时完成各自的自适应更新，最终保证每个波束测量信息处理的独立性。本发明可用于非理想条件下的水下航行器组合导航领域。CN113819906ACN113819906A权利要求书1/3页1.一种基于统计相似度量的组合导航鲁棒滤波方法，其特征在于，包括以下步骤：步骤1：定义坐标系；定义载体坐标系采用b表示，地心地固坐标系用e表示，“东‑北‑天”地理坐标系作为导航系采用n表示，惯性坐标系用i表示，DVL框架坐标系用d表示；定义不同坐标系下的速度：其中，表示捷联惯导系统在b系中的速度矢量；表示n系内捷联惯导系统的速度矢量；表示捷联惯导系统在d系中的速度矢量；表示d系中DVL波束的速度矢量；步骤2：建立SINS/DVL紧组合导航系统状态方程；选取SINS的误差作为状态，包括失准角φ、速度误差δVn、位置误差δp、陀螺常值漂移εb和加速度计常值零偏状态方程如下：其中，εb表示陀螺常值漂移矢量；是陀螺随机漂移矢量；是加速度计常值零偏矢2CN113819906A权利要求书2/3页量；是加速度计随机零偏矢量；φ表示由俯仰、横滚和偏航失准角组成的失准角矢量；是方向余弦矩阵，表示从b系到n系的变换；表示b系下b系相对于i系的角速率矢量，对应的计算误差为是n系中的载体相对于地面的速度矢量，对应的误差矢量是δvn；fb是加速度计的输出；δgn是重力误差向量在n系中的投影；δp表示由纬度误差δL、经度误差δλ和高度误差δh组成的位置误差矢量；RM和RN分别表示子午线圆和本初垂直圆的曲率半径；步骤3：建立SINS/DVL紧组合导航系统量测方程，并分解；量测方程：Z＝HX+v其中，表示测量噪声；I3×3是三维单位矩阵；[·×]表示向量的叉乘运算；α是波束和载体之间的水平角；设置为0°或45°；DVL各个波束的测量噪声通常情况下是不相关的，其量测噪声协方差阵R＝E[vvT]是一个对角阵，因此将量测方程进行等效分解：其中，Hj表示量测矩阵的第j行；vj是相应的测量噪声，不同的DVL波束的测量噪声不相关，这里是将DVL的四个波束测量值假设为由不同的传感器分别进行测量获得；步骤4：基于统计相似度量构建状态估计的代价函数；3CN113819906A权利要求书3/3页其中，和分别表示状态一步预测结果和一步预测协方差阵；N(X；Xk+1,Pk+1)表示关于X的高斯分布，均值是Xk+1，方差是Pk+1；相似度函数fx(t)选择为fx(t)＝‑0.5t；相似度函数fz(t)选为ω是自由度参数，m是量测维数，m＝1；q＝4；步骤5：应用高斯‑牛顿迭代求取代价函数的最优值；步骤5.1：初始化迭代次数t＝0，设定迭代初值为步骤5.2：计算滤波增益其中，和相当于用来调节一步预测协方差阵和量测噪声方差的辅助参数；步骤5.3：计算后验状态步骤5.4：计算估计误差协方差阵步骤5.5：判断是否满足若不满足，则令t＝t+1，返回步骤5.2；否则，令输出SINS的误差X；步骤6：将估计的SINS的误差X反馈给SINS，进行修正；根据估计的SINS的误差X，获取失准角φ、速度误差δVn、位置误差δp；对于SINS解算出的姿态阵速度位置姿态误差的反馈修正方法为：速度误差δVn和位置误差δp直接在SINS的输出中直接减去即可：4CN113819906A说明书1/11页一种基于统计相似度量的组合导航鲁棒滤波方法技术领域[0001]本发明属于非理想条件下的组合导航技术领域，具体涉及一种基于统计相似度量的组合导航鲁棒滤波方法。背景技术[0002]捷联惯导/多普