基于双天线GNSS航向和轮速辅助的车辆定位方法及系统技术方案

本发明专利技术涉及基于双天线GNSS航向和轮速辅助的车辆定位方法及系统，方法包括步骤：1)在车辆静止条件下，根据GNSS天线位置和航向计算计算位置和航向的初始值获取车辆位置和航向的初始值；2)根据位置和航向的初始值及惯导测得的角速度，对车辆航向进行估计，并推算航向对应位置；3)根据电机转速及推算的航向对应位置对惯导位置和惯导航向进行推算，利用自适应卡尔曼滤波算法对位置和航向进行修正；4)将修正的位置和航向输入步骤2)，重复上述步骤，输出最终的航向和位置，进而获取车辆定位信息。与现有技术相比，本发明专利技术充分利用车载信息源，弥补了低速下GNSS接收机输出速度精度较低的问题，具备易于调试、估计精度高和计算量小等优点。

Vehicle Positioning Method and System Based on Dual-antenna GNSS Heading and Wheel Speed Assistance

【技术实现步骤摘要】
基于双天线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λ为经度误差反馈系数。步骤三本文档来自技高网...

【技术保护点】
1.基于双天线GNSS航向和轮速辅助的车辆定位方法，其特征在于，该方法包括下列步骤：1)在车辆静止条件下，根据GNSS天线位置和航向计算计算位置和航向的初始值获取车辆位置和航向的初始值；2)根据步骤1)中位置和航向的初始值以及惯导测得的角速度，对车辆航向进行估计，并推算航向对应位置；3)根据电机转速及推算的航向对应位置对惯导位置和惯导航向进行推算，并利用自适应卡尔曼滤波算法对位置和航向进行修正；4)将步骤3)修正的位置和航向输入步骤2)，重复上述步骤，输出最终的航向和位置，进而获取车辆定位信息。

【技术特征摘要】
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、纬度Lk的表达式为：式中，Lk-1、λk-1分别为上一时刻的纬度和经度，vN·dt/(RM+h)和vE·dt/(...

【专利技术属性】
技术研发人员：熊璐，夏新，陆逸适，高乐天，韩燕群，胡英杰，魏琰超，刘伟，宋舜辉，余卓平，
申请(专利权)人：同济大学，
类型：发明
国别省市：上海,31