基于双激光多普勒测速仪和惯导系统组合导航方法及装置.pdf

(19)国家知识产权局(12)发明专利申请(10)申请公布号CN114812545A(43)申请公布日2022.07.29(21)申请号202210410200.9G06F17/16(2006.01)(22)申请日2022.04.19G06F17/18(2006.01)(71)申请人中国人民解放军国防科技大学地址410073湖南省长沙市开福区德雅路109号(72)发明人向志毅周健王琦聂晓明金世龙程吉利黄荣席崇宾陈兰剑王汉睦(74)专利代理机构长沙国科天河知识产权代理有限公司43225专利代理师周达(51)Int.Cl.G01C21/16(2006.01)G01C21/00(2006.01)G06F17/11(2006.01)权利要求书3页说明书11页附图2页(54)发明名称基于双激光多普勒测速仪和惯导系统组合导航方法及装置(57)摘要本发明提出了一种基于双激光多普勒测速仪和惯导系统组合导航方法及装置，先构建两激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型；将两激光多普勒测速仪的刻度系数误差和安装角误差以及惯导系统的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为状态向量，构建滤波系统的状态方程；将惯导系统在载体坐标系中的位移增量与两激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导系统得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为观测量，构建滤波系统的观测方程；由滤波得到的状态向量对双激光多普勒测速仪和惯导系统进行反馈校正，输出校正后的定位结果。本发明能够提高组合导航的精度。CN114812545ACN114812545A权利要求书1/3页1.基于双激光多普勒测速仪和惯导系统组合导航方法，其特征在于，包括：考虑两个激光多普勒测速仪的刻度系数误差以及安装角误差，分别构建第一激光多普勒测速仪以及第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型；将两个激光多普勒测速仪的刻度系数误差和安装角误差以及惯导系统的惯导姿态误差、速度误差、位置误差、陀螺测量误差、加速度计测量误差作为组合导航的状态向量，构建滤波系统的状态方程；将惯导系统在载体坐标系中的位移增量与第一激光多普勒测速仪在载体坐标系下的位移增量之差、惯导系统在载体坐标系中的位移增量与第二激光多普勒测速仪在载体坐标系下的位移增量之差以及由惯导系统得到的航向角与由两个激光多普勒测速仪得到的航向角之差作为三个观测量，构建滤波系统的观测方程；基于所构建的滤波系统进行滤波，利用滤波得到的状态向量对双激光多普勒测速仪和惯导系统进行反馈校正，输出校正后的定位结果。2.根据权利要求1所述的基于双激光多普勒测速仪和惯导系统组合导航方法，其特征在于，还包括：在组合导航开始前，对惯导系统和两个激光多普勒测速仪进行标定以获得陀螺和加速度计的刻度系数误差、安装误差角、零偏以及激光多普勒测速仪的刻度系数和安装误差角。3.根据权利要求1所述的基于双激光多普勒测速仪和惯导系统组合导航方法，其特征在于，第一激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型，分别为：其中为第一激光多普勒测速仪的输出速度在载体坐标系中的投影，I3为3×3单位T矩阵，×表示求解斜对称矩阵，φm1＝[φmx10φmz1]表示载体坐标系与第一激光多普勒测速仪所在坐标系的安装差角误差，φmx1为第一激光多普勒测速仪的俯仰安装角误差，φmz1为第一激光多普勒测速仪的航向安装角误差，表示从第一激光多普勒测速仪所在坐标系到载体坐标系的姿态转换矩阵，δK1表示第一激光多普勒测速仪的刻度系数误差，K1表示第一激光多普勒测速仪的刻度系数，表示第一激光多普勒测速仪的输出速度，表示载体坐标系中第一激光多普勒测速仪的速度误差。4.根据权利要求3所述的基于双激光多普勒测速仪和惯导系统组合导航方法，其特征在于，第二激光多普勒测速仪在载体坐标系下的输出速度以及速度误差模型，分别为：其中为第二激光多普勒测速仪的输出速度在载体坐标系中的投影，φm2＝[φmx20Tφmz2]表示载体坐标系与第二激光多普勒测速仪所在坐标系的安装差角误差，φmx2为第二激光多普勒测速仪的俯仰安装角误差，φmz2为第二激光多普勒测速仪的航向安装角误差，表示从第二激光多普勒测速仪所在坐标系到载体坐标系的姿态转换矩阵，δK2表示第二2CN114812545A权利要求书2/3页激光多普勒测速仪的刻度系数误差，K2表示第二激光多普勒测速仪的刻度系数，表示第二激光多普勒测速仪的输出速度，表示载体坐标系中第二激光多普勒测速仪的速度误差。5.根据权利要求1或2或3或4所述的基于双激光多普勒测速仪和惯导系统组合导航方法，其特征在于，所述滤波系统为卡尔曼滤波器，卡尔曼滤波器的状态方程为：其中F(t)为系统状态转移矩阵，X(t)为组合导航的状态向量，G