一种基于车载惯组与多普勒雷达的组合导航方法及系统.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN111912405A(43)申请公布日2020.11.10(21)申请号201910388074.X(22)申请日2019.05.10(71)申请人中国人民解放军火箭军工程大学地址710000陕西省西安市灞桥区同心路2号(72)发明人杨波单斌席建祥杨剑薛亮徐军辉熊陶(74)专利代理机构北京高沃律师事务所11569代理人程华(51)Int.Cl.G01C21/16(2006.01)G01C21/20(2006.01)权利要求书6页说明书15页附图1页(54)发明名称一种基于车载惯组与多普勒雷达的组合导航方法及系统(57)摘要本发明公开了一种基于车载惯组与多普勒雷达的组合导航方法及系统。该方法包括：获取当前时刻的惯组数据和雷达数据；采用惯组数据中的角增量和雷达数据对载车当前时刻的姿态和位置进行航位解算；采用惯组数据对载车当前时刻的姿态、位置和速度进行惯组解算；将惯组解算对应的姿态数据与航位解算对应的姿态数据做差，将惯组解算对应的位置数据和航位解算对应的位置数据做差；依据差值，采用卡尔曼滤波算法得到当前时刻的系统误差估计值；采用当前时刻的系统误差估计值对解算得到的数据进行误差校正；将经过校正的惯组解算的姿态数据、位置数据和速度数据作为当前时刻的导航数据输出。本发明抗干扰能力强、独立自主性强、导航定位精度高。CN111912405ACN111912405A权利要求书1/6页1.一种基于车载惯组与多普勒雷达的组合导航方法，其特征在于，包括：获取当前时刻的惯组数据和雷达数据；所述惯组数据为由惯组采集到的速度增量和角增量；所述雷达数据为由多普勒雷达采集到的载车的速度；采用所述角增量和所述雷达数据对载车在当前时刻的姿态和位置进行航位解算，得到第一姿态矩阵和第一位置数据；采用所述惯组数据对所述载车在当前时刻的姿态、位置和速度进行惯组解算，得到的第二姿态矩阵、第二位置数据和速度数据；将所述第一姿态数据与所述第二姿态数据做差，得到第一差值，将所述第一位置数据和所述第二位置数据做差，得到第二差值；所述第一姿态数据是依据所述第一姿态矩阵得到的；所述第二姿态数据是依据所述第二姿态矩阵得到的；依据所述第一差值和所述第二差值，采用卡尔曼滤波算法得到当前时刻的系统误差估计值；采用当前时刻的系统误差估计值对所述第一姿态矩阵、所述第一位置数据、所述第二姿态矩阵、所述第二位置数据和所述速度数据进行误差校正，得到第一姿态校正数据、第一位置校正数据、第二姿态校正数据、第二位置校正数据和速度校正数据；将所述第二姿态校正数据、所述第二位置校正数据和所述速度校正数据确定为当前时刻输出的导航数据。2.根据权利要求1所述的一种基于车载惯组与多普勒雷达的组合导航方法，其特征在于，所述采用所述角增量和所述雷达数据对载车在当前时刻的姿态和位置进行航位解算，得到第一姿态矩阵和第一位置数据，具体包括：依据所述角增量，采用等效旋转矢量法，得到当前时刻载车的第一姿态矩阵；依据所述雷达数据和所述第一姿态矩阵对载车在当前时刻的位置进行航位解算，得到第一位置数据。3.根据权利要求2所述的一种基于车载惯组与多普勒雷达的组合导航方法，其特征在于，所述依据所述角增量，采用等效旋转矢量法，得到当前时刻载车的第一姿态矩阵，具体包括：依据所述角增量，采用等效旋转矢量法，计算当前时刻载车的姿态四元数；所述姿态四元数的公式为其中，表示tj时刻载车的姿态四元数，表示tj-1时刻载车的姿态四元数，表示车体系b系从tj-1时刻到tj时刻的变换四元数，ηj表示为tj-1到tj时刻车体系b系相对于导航系n系的等效旋转矢量；依据所述当前时刻载车的姿态四元数，确定当前时刻载车的姿态矩阵；所述载车的姿态矩阵为第一姿态矩阵。2CN111912405A权利要求书2/6页4.根据权利要求3所述的一种基于车载惯组与多普勒雷达的组合导航方法，其特征在于，所述依据所述雷达数据和所述第一姿态矩阵对载车在当前时刻的位置进行航位解算，得到第一位置数据，具体包括：采用所述第一姿态矩阵和所述雷达数据对载车在当前时刻的位置进行航位解算，得到当前时刻载车所在位置的纬度、所在位置的经度和所在位置的高度；hj＝hj-1+ΔSUj，其中，Lj表示在tj时刻载车所在位置的纬度，λj表示在tj时刻载车所在位置的经度，hj表示在tj时刻载车所在位置的高度，Lj-1表示在tj-1时刻载车所在位置的纬度，λj-1表示在tj-1时刻载车所在位置的经度，hj-1表示在tj-1时刻载车所在位置的高度，ΔSEj、ΔSNj、ΔSUj分别为[tj-1,tj]时间段内载车行驶路程增量在东北天地理坐标系的三个轴上的投影分量，RM表示当地子午圈的主曲率半径，RN表示与子午面垂直的卯酉圈主曲率半径，令其中，表示tj