[发明专利]基于双天线GNSS航向和轮速辅助的车辆定位方法及系统

权利要求书

1.基于双天线GNSS航向和轮速辅助的车辆定位方法，其特征在于，该方法包括下列步骤：

1)在车辆静止条件下，根据GNSS天线位置和航向计算计算位置和航向的初始值获取车辆位置和航向的初始值；

2)根据步骤1)中位置和航向的初始值以及惯导测得的角速度，对车辆航向进行估计，并推算航向对应位置；

3)根据电机转速及推算的航向对应位置对惯导位置和惯导航向进行推算，并利用自适应卡尔曼滤波算法对位置和航向进行修正；

4)将步骤3)修正的位置和航向输入步骤2)，重复上述步骤，输出最终的航向和位置，进而获取车辆定位信息。

2.根据权利要求1所述的基于双天线GNSS航向和轮速辅助的车辆定位方法，其特征在于，步骤2)中，车辆航向角的估计表达式为：

式中，为上一时刻的车辆航向值，为横摆角增量，为航向角估计误差，为反馈系数，为车辆角速度零偏，Δt为惯导模块采采样时间间隔。

3.根据权利要求2所述的基于双天线GNSS航向和轮速辅助的车辆定位方法，其特征在于，步骤3)中，根据车辆电机转速和陀螺仪角速度对惯导处位置和航向进行推算的具体内容为：

31)估计得到车辆航向角后，使用该航向角将电机转速换算得到的合速度分解到导航坐标系下，计算经纬度；

32)转向时，令车辆的转向中心处于后轴之上，获取后轴中心处的东、北方向速度；

33)将后轴中心处的东、北方向速度转换到GNSS天线处，使后轴中心与GNSS天线的位置作位置融合，获取位置经纬度。

4.根据权利要求3所述的基于双天线GNSS航向和轮速辅助的车辆定位方法，其特征在于，步骤33)中，位置经度λ_k、纬度L_k的表达式为：

式中，L_k-1、λ_k-1分别为上一时刻的纬度和经度，v_N·dt/(R_M+h)和v_E·dt/(R_N+h)/cosL_k-1分别为东向和北向速度产生的纬度增量和经度增量，和分别为惯导纬度估计误差和惯导经度估计误差，k_L为纬度误差反馈系数，k_λ为经度误差反馈系数，v_E为后轴中心处的东向速度，v_N为后轴中心处的北向速度，和分别为惯导纬度估计误差和惯导经度估计误差，R_M为地球卯酉圈半径，R_N为地球子午圈半径。

5.根据权利要求4所述的基于双天线GNSS航向和轮速辅助的车辆定位方法，其特征在于，步骤3)中，利用自适应卡尔曼滤波算法进行位置和航向模块的修正，修正后航向角误差估计的过程模型为：

其中，为航向角误差导数，为角速度零偏，为航向角误差动态的过程噪声，为角速度零偏动态的过程噪声；

航向角误差估计的测量模型为：

其中，为双天线GNSS航向的测量噪声；

根据GNSS主天线位置和惯导位置对惯导位置误差进行估计，位置误差估计过程模型为：

其中，为纬度误差动态的过程噪声，为经度误差动态，位置误差估计测量模型为：

其中，和分别为纬度和经度测量误差噪声。

6.一种实现如权利要求1-5任一项所述的基于双天线GNSS航向和轮速辅助的车辆定位方法的定位系统，其特征在于，包括：

双天线RTK GNSS接收机模块，用于输出GNSS主天线处位置、GNSSS双天线、GNSSS双天线经度信息和纬度信息；

惯导模块，用于获取车辆角速度并根据车辆电机转速和陀螺仪角速度对惯导处位置和航向进行推算；

初始化模块，用于根据双天线RTK GNSS接收机模块输出的GNSSS双天线航向、GNSSS双天线经度信息、纬度信息以及惯导模块获取的角速度信息对惯导模块进行初始化；

自适应卡尔曼滤波惯导误差估计模块，用于对惯导模块的惯导处位置误差、航向误差以及位置延迟时间误差进行估计。