一种双天线GNSS位置延迟时间动态估计系统及方法与流程

本发明涉及一种车辆gnss位置延迟时间估计方法，尤其是涉及一种双天线gnss位置延迟时间动态估计系统及方法。

背景技术：

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

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

技术实现要素：

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种双天线gnss位置延迟时间动态估计系统及方法。

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

一种双天线gnss位置延迟时间动态估计系统，该系统包括初始化模块、惯性导航模块、双天线rtkgnss接收机模块和自适应卡尔曼滤波误差估计模块。

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

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

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

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

一种双天线gnss位置延迟时间动态估计方法，该方法包括下列步骤：

步骤一、在车辆静止条件下，在一段时间内采用双天线rtkgnss接收机模块输出的gnss主天线位置和gnsss双天线航向获取车辆位置和航向的初始值。

步骤二、惯导模块根据获取的角速度对车辆航向角进行估计。

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

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

步骤三、根据车辆电机转速和陀螺仪角速度，利用自适应卡尔曼滤波模块对惯导处位置误差和航向误差进行推算，并获取gnss位置延迟时间误差。

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

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

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

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

其中，惯导处航向推算的表达式为：

式中，为真实航向，为航向角误差。

惯导处位置推算的表达式为：

其中，lwdec和λwdec分别为惯导推算得到的纬度、经度，ldec和λdec分别为真实的纬度、经度，δldec和δλdec分别为惯导的纬度和经度测量误差；

gnss位置的测量方程为：

式中，为gnss测量纬度噪声，为gnss测量经度噪声，an为北向加速度，ae为东向加速度，δt为gnss的位置延迟时间误差，vnw为由轮速计算的北向速度，vew为由轮速计算的东向速度；

取惯导处位置推算结果与gnss位置的测量方程之差作为观测量，获取纬度、经度、航向、角速度零偏以及位置延迟时间误差的观测方程为：

式中，l为纬度，下标dec表示纬度小数部分，rm为地球卯酉圈半径，rn为地球子午圈半径，h表示车辆距水平面高度，ve、vn分别为后轴中心处的东、北方向速度；δt即为gnss位置延迟时间的估计结果。

步骤四、将步骤三获取的惯导处位置误差和航向误差反馈至惯导模块，惯导模块获取惯导航向，重新执行步骤二。

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

(1)本发明方法在低速下引入车辆电机转速通过惯导航向分解得到东北方向速度，进而通过积分得到位置，可以削弱惯性元件的误差对于速度估计的影响，提高位置估计精度；

(2)本发明方法充分融合车辆信息、惯导信息以及gnss信息，并通过自适应卡尔曼滤波方法使得算法适应与gnss异常工况，使用轮速辅助对gnss的位置延迟时间进行估计，实现了对gnss位置延迟时间的准确估计，且对实现gnss与惯导时间同步具有重要的工程意义。

附图说明

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

具体实施方式

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

如图1所示，本发明涉及一种双天线gnss位置延迟时间动态估计系统，包括初始化模块、惯性导航模块、双天线rtkgnss接收机模块和自适应卡尔曼滤波误差估计模块。

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

一种双天线gnss位置延迟时间动态估计方法，该方法基于上述系统，包括下列步骤：

步骤一、在车辆静止条件下，在一段时间内(约20～30秒)，采用双天线rtkgnss接收机模块输出的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λ为经度误差反馈系数。

步骤三、利用自适应卡尔曼滤波模块对位置误差、航向误差和gnss位置延迟时间误差进行估计。估计方法如下：

为了对gnss接收机输出位置的延迟时间进行估计，使用车辆轮速进行辅助，且认为轮胎半径已知，轮速换算得到的车速准确，这里在小纵向动态和小侧向动态工况下，使用估计所得航向角替代轨迹角，用来将轮速分解至导航坐标系下，并认为估计所得的航向角存在一个误差角，则即惯导推算航向的表达式为：

其中为真实航向，为航向角误差，根据该惯导推算航向可得东、北向速度的估计值分别为和

假设真实的东、北方向速度分别为ve和vn：

则得到东向和北向速度误差方程分别为δvew和δvnw：

选取状态变量便得到如下状态方程，即误差估计的过程模型。

其中，δt为gnss的位置延迟时间误差，为角速度的零偏误差，δldec和δλdec分别为惯导纬度误差、惯导经度误差，和wδt分别为纬度误差的过程噪声、经度误差的过程噪声、航向角误差的过程噪声、角速度零偏误差的过程噪声和位置延迟时间误差的过程噪声。

由于在位置误差动态方程中，已将经纬度的整数部分和小数部分拆开并做过放大1e7处理，测量方程中也要做相应处理。

由轮速推算所得位置方程为：

其中，lwdec和λwdec分别为惯导推算得到的纬度、经度，ldec和λdec分别为真实的纬度、经度，δldec和δλdec分别为惯导的纬度和经度测量误差。

而gnss位置测量方程为：

其中，为gnss测量纬度噪声，为gnss测量经度噪声，an为北向加速度，ae为东向加速度；vnw为由轮速计算的北向速度，vew为由轮速计算的东向速度。

取二者之差作为观测量，得到纬度、经度、航向、角速度零偏以及位置延迟时间误差的观测方程为：

式中，l为纬度，下标dec表示纬度小数部分，rm为地球卯酉圈半径，rn为地球子午圈半径，h表示车辆距水平面高度。

基于以上的状态方程和观测方程便可以通过自适应卡尔曼滤波算法对这些误差进行估计。

步骤四、在误差估计模块中所使用的是自适应卡尔曼滤波算法，其中标准卡尔曼滤波算法如下：

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：

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

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