基于双天线GNSS航向和轮速辅助的车辆定位方法及系统与流程

本发明涉及自动驾驶技术领域，尤其是涉及一种基于双天线gnss航向和轮速辅助的车辆定位方法及系统。

背景技术：

自动驾驶汽车近年来飞速发展，已成为整车企业甚至是it企业竞争的焦点。准确的车辆位置和航向对于自动的驾驶实现至关重要。融合多源信息对位置、航向和位置延迟时间进行估计成为了亟需解决的问题。

目前国内外在本领域低速电动车辆的定位研究点大多集中在位置和航向误差估计上，现有的位置误差和航向估计的主要方法有：1、利用卫星导航定位系统(gnss)通过rtk载波相位差分实现精确位置和航向计算，但其鲁棒性较差，gnss信号极易受干扰；2、采用惯性导航系统(ins)实现对位置和航向计算，然而该方法存在累积误差问题；3、基于轮速和ins组合的位置和航向估计，由于该方法没有全局位置修正，存在累积误差问题；4、基于激光雷达和视觉等传感器的组合位置和航向估计，受环境特征点分布影响严重，并且各个传感器性能要求较高。

技术实现要素：

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于双天线gnss航向和轮速辅助的车辆定位方法及系统。

本发明的目的可以通过以下技术方案来实现：

基于双天线gnss航向和轮速辅助的车辆定位方法，包括下列步骤：

步骤一、在车辆静止条件下，根据gnss天线位置和航向计算计算位置和航向的初始值获取车辆位置和航向的初始值。

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

车辆航向角的估计表达式为：

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

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

根据车辆电机转速和陀螺仪角速度对惯导处位置和航向进行推算的具体步骤包括：

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

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

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

位置经度λk、纬度lk的表达式为：

式中，lk-1、λk-1分别为上一时刻的纬度和经度，vn·dt/(rm+h)和ve·dt/(rn+h)/coslk-1分别为东向和北向速度产生的纬度增量和经度增量，和分别为惯导纬度估计误差和惯导经度估计误差，kl为纬度误差反馈系数，kλ为经度误差反馈系数，ve为后轴中心处的东向速度，vn为后轴中心处的北向速度，和分别为惯导纬度估计误差和惯导经度估计误差，rm为地球卯酉圈半径，rn为地球子午圈半径。

利用自适应卡尔曼滤波算法进行位置和航向模块的修正，修正后航向角误差估计的过程模型为：

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

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

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

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

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

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

步骤四、将步骤三修正的位置和航向输入步骤二，重复上述步骤，输出最终的航向和位置，进而获取车辆定位信息。

基于双天线gnss航向和轮速辅助的车辆定位系统，该系统包括：

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

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

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

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

与现有技术相比，本发明具有以下优点：

(1)本发明方法在低速下引入车辆电机转速，通过惯导航向分解得到东北方向速度，进而通过积分得到位置，可削弱惯性元件的误差如零偏不稳定性、非正交误差和加速度效应等误差对于速度估计的影响，提高车辆位置估计精度；

(2)本发明方法通过gnss位置作为测量反馈，并对位置误差和航向角误差进行估计，从而反馈修正积分模块估计得到的位置误差和航向误差，可消除积分模块累积误差，实现输出频率高、可靠性好、实时连续的位置航向信息；

(3)本发明方法充分融合车辆信息、惯导信息以及gnss信息，并通过自适应卡尔曼滤波方法使得算法适应与gnss异常工况，实现了对位置和航向的准确估计，进而提高了车辆定位的准确度。

附图说明

图1为本发明方法的流程示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。

如图1所示，本发明涉及一种基于双天线gnss航向和轮速辅助的车辆定位方法，该方法包括下列步骤：

步骤一、在车辆静止条件下，在一段时间内(约20～30秒)，根据gnss天线位置和航向计算计算位置和航向的初始值，计算方法如下：

式中，lini、λini和ωini分别表示车辆的航向、纬度、经度和横摆角速度初始值，lgnss、λgnss和ωins分别表示双天线rtkgnss测得的航向、纬度、经度和惯导模块测得的角速度。

步骤二、根据惯导测得的角速度对航向进行估计，并推算航向对应位置。

航向角由四部分构成，即：

第一部分是上一时刻航向值第二部分是横摆角增量由惯导传感器直接获取；第三部分为航向角估计误差为反馈系数，由误差估计模块中航向角误差估计结果提供；第四部分是角速度零偏由静止时初始化模块输出的角速度零偏获取，δt为惯导采样时间间隔。

根据电机转速以及估计所得航向对位置进行推算，推算方法如下：

当估计得到航向角后，使用该航向可将电机转速换算得到的合速度分解到导航坐标系下来计算经纬度。转向时，车辆的转向中心处于后轴之上，因此对于后轴中心而言，在忽略侧偏的情况下，后轴中心点的航向即为速度方向。令后轴轮速换算的车速为vw，则后轴中心处的东、北方向速度分别为：

而gnss天线测得的位置和速度是gnss天线安放处的位置和速度，为了和gnss天线的位置作位置融合，需要将后轴中心处的速度转换到gnss天线处。

假设在车辆前左上坐标系下，天线相对于后轴中心处的三个方向距离分别为lx，ly，lz，则由于该距离产生的速度误差为：

式中，vu为天向速度。

则经过该补偿后，后轴中心处的东、北方向速度公式变为：

基于该速度，位置经纬度的积分公式为：

该积分公式由三部分构成：第一部分为上一时刻纬度lk-1和经度λk-1；第二部分为东向和北向速度产生的纬度增量vn·dt/(rm+h)和经度增量ve·dt/(rn+h)/coslk-1；第三部分为惯导纬度估计误差和惯导经度估计误差kl为纬度误差反馈系数，kλ为经度误差反馈系数。

步骤三、根据双天线rtkgnss航向和惯导推算航向对惯导航向误差进行估计，并利用自适应卡尔曼滤波算法进行位置和航向模块的修正，修正后航向角误差估计的过程模型为：

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

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

其中，为双天线gnss航向的测量噪声。

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

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

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

在本发明利用自适应卡尔曼滤波算法进行位置和航向的修正，其中标准卡尔曼滤波算法如下：

p0|0＝var(x0)

pk|k＝(i-gkck|k-1)pk|k-1

其中，p0|0为初始时刻状态误差的协方差矩阵，x0为初始状态，var(x0)为初始状态的方差，e(x0)为初始状态的期望，为k时刻的状态预测值，ak-1为k-1时刻的系统矩阵，为k-1时刻的估计值，pk|k-1为k时刻协方差矩阵预测值，pk-1|k-1为k-1时刻的协方差矩阵，γk-1为过程噪声的输入矩阵，qk-1为过程噪声的协方差矩阵，ck|k-1为测量矩阵，gk为卡尔曼滤波增益矩阵，rk为测量噪声的协方差矩阵，pk|k为k时刻协方差矩阵，i为单位阵，为k时刻状态的最优估计值，zk为k时刻的测量。

基于标准卡尔曼滤波算法，通过对其残差进行处理从而实现测量噪声的自适应提升算法的动态性能，自适应方法如下：

定义残差为然后通过其残差估计测量噪声的协方差矩阵rk：

从而实现了误差估计模块卡尔曼滤波算法的自适应。

本发明还涉及一种基于双天线gnss航向和轮速辅助的车辆定位系统，用于实现上述定位方法，该系统包括初始化模块、惯性导航模块、双天线rtkgnss接收机模块和自适应卡尔曼滤波误差估计模块。

双天线rtkgnss接收机模块用于输出gnss主天线处位置和双天线航向；初始化模块根据双天线rtkgnss接收机模块输出的航向、经、纬度信息以及惯导输出角速度信息对惯导进行初始化；惯导模块根据车辆电机转速和陀螺仪角速度对惯导处位置和航向进行推算；自适应卡尔曼滤波惯导误差估计模块对惯导模块的位置和航向模块进行修正，从而实现连续、可靠和实时的惯导位置和航向输出。

本发明在低速下引入车辆电机转速，通过惯导航向分解得到东北方向速度，进而通过积分得到位置，可削弱惯性元件的误差对于速度估计的影响，提高车辆位置估计精度；通过充分融合车辆信息、惯导信息以及gnss信息，并通过自适应卡尔曼滤波方法使得算法适应与gnss异常工况，实现了对位置和航向的准确估计，进而提高了车辆定位的准确度。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。