车辆航位推算方法、装置、设备以及存储介质与流程

本申请涉及车辆技术领域，尤其是涉及车辆控制、智能驾驶、自动驾驶领域。

背景技术：

车辆航位推算系统大多包括imu(inertialmeasurementunit，惯性测量单元)与车速传感器。首先，对imu的三轴陀螺仪和三轴加速度信息进行积分，获得车辆姿态、速度和位置的先验预测。然后，采用卡尔曼滤波模型，对车辆的速度状态进行修正，获得后验位姿估计。

但是，这种观测仅有车速信息，仅能对速度状态进行修正，在较长距离的dr(dead-reckoning，航位推算)中，可能存在较大的累积误差，难以满足长隧道、山谷等定位信号较差的场景的航位推算精度要求。

技术实现要素：

本申请提供了一种车辆航位推算方法、装置、设备以及存储介质。

根据本申请的一方面，提供了一种车辆航位推算方法，包括：

基于车辆的轮速信息，得到该车辆的位姿增量；

基于该车辆的位姿增量和上一时刻的位姿，对该车辆在当前时刻的位姿进行航位推算。

根据本申请的另一方面，提供了一种车辆航位推算装置，包括：

增量模块，用于基于车辆的轮速信息，得到该车辆的位姿增量；

推算模块，用于基于该车辆的位姿增量和上一时刻的位姿，对该车辆在当前时刻的位姿进行航位推算。

根据本申请的技术方案，利用轮速信息得到位姿增量，再利用位姿增量进行航位推算，能够减少累积误差，提高航位推算结果的精度。

应当理解，本部分所描述的内容并非旨在标识本申请的实施例的关键或重要特征，也不用于限制本申请的范围。本申请的其它特征将通过以下的说明书而变得容易理解。

附图说明

附图用于更好地理解本方案，不构成对本申请的限定。其中：

图1是根据本申请一实施例的车辆航位推算方法的流程图；

图2是根据本申请另一实施例的车辆航位推算方法的流程图；

图3是根据本申请另一实施例的车辆航位推算方法的流程图；

图4是根据本申请另一实施例的车辆航位推算方法的流程图；

图5是一种示例性的定位系统架构的示意图；

图6是一种示例性的定位系统运行流程的示意图；

图7是轮速推导原理的示意图；

图8是轮速位姿推算原理的示意图；

图9是航位推算纵向位置误差随推算距离的变化曲线的示例图；

图10是航位推算横向位置误差随推算距离的变化曲线的示例图；

图11是根据本申请一实施例的车辆航位推算装置的框图；

图12是根据本申请一实施例的车辆航位推算装置的框图；

图13是用来实现本申请实施例的车辆航位推算的方法的电子设备的框图。

具体实施方式

以下结合附图对本申请的示范性实施例做出说明，其中包括本申请实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本申请的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。

图1是根据本申请一实施例的车辆航位推算方法的流程图，该方法可以包括：

s101、基于车辆的轮速信息，得到该车辆的位姿增量；

s102、基于该车辆的位姿增量和上一时刻的位姿，对该车辆在当前时刻的位姿进行航位推算。

航位推算一般是在初始位置上累加位移矢量计算当前位置，航位推算是一个信息累加的过程。在gps(globalpositioningsystem，全球定位系统)/gnss(globalnavigationsatellitesystem，全球导航卫星系统)信号质量较差导致定位困难或不可能定位的情况下，通过使用来自各种传感器(陀螺仪传感器，加速度计，速度脉冲等)的信息进行航位推算得到当前位置，可以保持高精度定位。航位推算可以应用于车辆导航等场景，例如通过车辆控制器执行本申请实施例的车辆航位推算方法，以获得准确的位姿和速度等信息，辅助进行导航。本申请实施例中的车辆可以包括人工操作的车辆、自动驾驶的车辆等。

车辆的轮速信息包括车轮的转速，通过轮速传感器等可以测量车轮的转速。例如，如果车辆的有四个车轮，在车辆行驶过程中，能够利用轮速传感器测量每个车轮的转速。在同一时刻车辆通常只有一个车速，但是车辆的不同车轮的转速可能是不同的，尤其是在转弯等场景下，左轮和右轮的转速可能不同。

示例性地，车辆的位姿信息包括车辆的位置坐标和车身姿态角。位姿增量可以包括位置增量和姿态增量。例如，姿态增量可以包括航向增量。

在本实施例中，在gps/gnss信号质量较差的情况下，可以采用本申请实施例的车辆航位推算方法，基于轮速信息，得到该车辆的位姿增量，再基于该车辆的位姿增量和上一时刻的位姿，得到车辆在当前时刻的位姿观测。基于当前时刻的位姿观测和位姿先验，以及卡尔曼滤波模型，获得后验位姿估计，能够提高航位推算精度。

在gps/gnss信号质量较好的情况下，可以采用相关技术中的车辆航位推算方法：对imu的三轴陀螺仪和三轴加速度信息进行积分，获得车辆姿态、速度和位置的先验预测；采用卡尔曼滤波模型，将车速观测转换到导航坐标系，对车辆速度状态进行修正，获得后验位姿估计。

本申请实施例，利用轮速信息得到位姿增量，再利用位姿增量进行航位推算，能够减少累积误差，提高航位推算结果的精度。

图2是根据本申请另一实施例的车辆航位推算方法的流程图。该实施例的车辆航位推算方法可以包括上述实施例的各步骤。在本实施例中，在一种可能的实施方式中，s101包括：

s201、基于该轮速信息，得到该车辆的位置增量。

在本实施例中，利用轮速信息得到位置增量，再利用位置增量进行航位推算，能够减少车辆位置的累积误差，提高航位推算的车辆位置的精度。

在一种可能的实施方式中，s201包括：

基于左轮轮速、右轮轮速、车轮半径和相邻时刻时间长度，计算后轴中心的运动弧长；

基于该后轴中心的运动弧长和航向角，计算该车辆的位置增量。

一般车辆的后轴非转向，后轴连接两个后轮。在本实施例中，左轮轮速可以为后轴连接的左边后轮的轮速，右轮轮速可以为后轴连接的右边后轮的轮速。

示例性地，如果左轮轮速为wl，右轮轮速为wr，车轮半径为r，相邻时刻时间长度为dt，可以采用下式计算后轴中心的运动弧长dcenter：

示例性地，如果后轴中心的运动弧长为dcenter，航向角为θ，可以采用下式计算该车辆的位置增量δwheel_pos：

例如，导航坐标系为东北天坐标系，x轴表示东向，y轴表示北向，z轴表示天向，dcentercos(θ)表示车轮在x轴方向的位置增量，dcentersin(θ)表示车轮在y轴方向的位置增量，0表示车辆在z轴方向的位置增量。

在本实施例中，基于后轴的左轮轮速、右轮轮速、车轮半径和相邻时刻时间长度，计算后轴中心的运动弧长；再基于后轴中心的运动弧长和航向角计算车辆的位置增量，可以利用位置增量进行航位推算，能够减少车辆位置的累积误差，提高航位推算的车辆位置的精度。

在一种可能的实施方式中，s102包括：

s202、基于该车辆的位置增量和上一时刻的位置，得到该车辆在当前时刻的位置观测。

示例性地，如果_state_at_wheel_prev表示车辆在上一时刻的位置，zwheel_pos表示当前时刻由轮速推导出的位置观测，则zwheel_pos＝_state_at_wheel_prev+δwheel_pos。

在本实施例中，可以利用位置增量和上一时刻的位置，得到该车辆在当前时刻的位置观测，能够得到更加准确的车辆位置。

在一种可能的实施方式中，s102还包括：

s203、计算该车辆在当前时刻的位置观测和该车辆在当前时刻的位置先验的位置差值，将该位置差值输入卡尔曼滤波模型，获得当前时刻的最优位置估计。

示例性地，如果_state_at_wheel表示当前时刻车辆的位置，hwheel_pos表示当前时刻的位置先验，则hwheel_pos＝_state_at_wheel。位置观测zwheel_pos和位置先验的差值innowheel_pos可以表示为：

innowheel_pos＝zwheel_pos-hwheel_pos；

将该innowheel_pos输入卡尔曼滤波模型，可以获得车辆在当前时刻的最优位置估计。

示例性地，当前时刻的位置先验hwheel_pos可以采用运动方程得到，参见下式的位置预测方程：

pk+1＝pk+vk×dt

其中，pk+1为当前时刻(k+1时刻)的位置先验hwheel_pos，pk为上一时刻(k时刻)的车辆位置，vk为上一时刻车辆在东向、北向和天向速度，dt为相邻时刻时间长度。其中，k+1时刻和k时刻的时间间隔为dt。

在本实施例中，将当前时刻的位置观测和该车辆在当前时刻的位置先验的位置差值，输入卡尔曼滤波模型，获得当前时刻的最优位置估计更加准确。

图3是根据本申请另一实施例的车辆航位推算方法的流程图。该实施例的车辆航位推算方法可以包括上述任一实施例的各步骤。在本实施例中，在一种可能的实施方式中，s101包括：

s301、基于该轮速信息，得到该车辆的航向增量。

在本实施例中，利用轮速信息得到航向增量，再利用航向增量进行航位推算，能够减少车辆航向的累积误差，提高航位推算的车辆航向的精度。

在一种可能的实施方式中，s301包括：

基于左轮轮速和右轮轮速的差值，轮距，以及车轮半径，计算所述车辆的航向角速度；

基于该车辆的航向角速度和相邻时刻时间长度，计算该车辆的航向增量。

虽然采用轮速进行位置估计存在累积误差，但是在相邻时刻的推算较为稳定可信。因此可以选取相邻时刻的轮速进行位置增量推算。

示例性地，基于左轮轮速wl和右轮轮速wr的差值，以及车辆后轴的轮距l，车轮半径为r，车辆的航向角速度ωt可以表示为：

示例性地，dθ表示航向增量，dt表示相邻时刻时间长度，dt可以趋近于0。航向增量δwheel_theta(也即dθ)可以表示为：δwheel_theta＝ωtdt。

在本实施例中，左轮轮速和右轮轮速的差值，轮距，以及车轮半径计算所述车辆的航向角速度；基于车辆的航向角速度和相邻时刻时间长度，计算车辆的航向增量。然后，可以利用航向增量进行航位推算，能够减少车辆航向的累积误差，提高航位推算的车辆航向的精度。

在一种可能的实施方式中，s102包括：

s302、基于该车辆的航向增量和上一时刻的航向角，得到该车辆在当前时刻的航向观测。

虽然采用轮速进行航向估计存在累积误差，但是在相邻时刻的推算较为稳定可信。因此可以选取相邻时刻的轮速进行航向增量推算。

示例性地，如果_state_at_wheel_prev表示上一时刻车辆的航向，δwheel_heading表示由轮速从上一时刻到当前时刻推出的航向增量，zwheel_heading表示当前时刻由轮速推导出的航向观测，则zwheel_heading＝_state_at_wheel_prev+δwheel_theta。

在本实施例中，可以利用航向增量和上一时刻的位置，得到该车辆在当前时刻的航向观测，能够得到更加准确的车辆航向。

在一种可能的实施方式中，s102还包括：

s303、计算该车辆在当前时刻的航向观测和该车辆在当前时刻的航向先验的航向差值，将该航向差值输入卡尔曼滤波模型，获得当前时刻的最优航向估计。

示例性地，如果hwheel_heading表示当前时刻的航向先验，_state_at_wheel表示当前时刻车辆的航向，则hwheel_heading＝_state_at_wheel。航向观测和航向先验的差值innowheel_heading可以表示为：

innowheel_heading＝zwheel_heading-hwheel_heading；

将该innowheel_heading输入卡尔曼滤波模型，可以获得车辆在当前时刻的最优航向估计。

示例性地，当前时刻的航向先验hwheel_heading可以采用运动方程得到，参见下式的姿态预测方程：

其中，qk+1表示当前时刻车辆姿态四元数，qk表示上一时刻车辆姿态四元数，ωk+1表示当前时刻imu三轴陀螺仪的测量值，为算法估计的imu三轴陀螺游走噪声，表示四元数的乘法运算。例如，以车辆姿态包括航向角为例，qk+1代入当前时刻的航向角，qk代入上一时刻的航向角。

示例性地，将位置差值和航向差值分别输入卡尔曼滤波模型，可以获得车辆在当前时刻的最优位置估计和最优航向估计。参见下式：

后验位姿估计

其中，为位姿状态的先验预测，kk+1为卡尔曼滤波增益，zk+1当前时刻的状态观测，为观测坐标系下的位姿状态的先验预测。例如，为上一时刻的位置先验，zk+1为当前时刻的位置观测，为位置差值。再如，为上一时刻的航向先验，zk+1为当前时刻的航向观测.为航向差值。

更新后验位姿协方差矩阵

其中kk+1为卡尔曼滤波增益，h为观测矩阵，为上一时刻的协方差矩阵。

在本实施例中，将当前时刻的航向观测和该车辆在当前时刻的位置先验的航向差值，输入卡尔曼滤波模型，获得的当前时刻的最优航向估计更加准确。

图4是根据本申请另一实施例的车辆航位推算方法的流程图。该实施例的车辆航位推算方法可以包括上述任一实施例的各步骤。在本实施例中，在一种可能的实施方式中，该方法还包括：

s401、基于该轮速信息，计算该车辆的速度。

在本实施例中，利用轮速信息得到车辆的速度，再利用车辆的速度进行航位推算，能够丰富航位推算的观测量，减少imu引起的累积误差，提高航位推算的车辆速度的精度。

在一种可能的实施方式中，s401包括：

基于左轮轮速、右轮轮速和车轮半径，计算左轮线速度和右轮线速度；

基于该左轮线速度和该右轮线速度，计算后轴中心线速度。

在本实施例中，基于左轮轮速、右轮轮速和车轮半径计算左轮线速度和右轮线速，再计算该左轮线速度和该右轮线速度的平均值得到后轴中心线速度，将后轴中心线速度作为车辆的速度，得到的车辆的速度更加准确。

示例性地，如果左轮轮速为wl和右轮轮速为wr，左轮的线速度vl和右轮的线速度vr可以分别表示为：

vl＝wl×r；

vr＝wr×r。

这样，后轴中心的线速度vm等于左轮的线速度vl和右轮的线速度vr的平均值，即

在一种可能的实施方式中，该方法还包括：

s402、基于后轴测量纵向速度与真实速度的比例系数，后轴中心线速度，以及从车身坐标系到导航坐标系的转换矩阵，得到该车辆的车速观测。

示例性地，假设后轴测量纵向速度与真实速度的比例系数为scale，从车身坐标系到导航坐标系的转换矩阵为n指navigation(导航)，b指body(车身)，后轴中心线速度为vm。

如果取车身坐标系纵向为x轴，横向为y轴，竖直方向为z轴，则vx，vy，vz表示车辆纵向、横向和竖直方向的速度，车辆的车速观测zwheel可以为：其中，vy和vz可以为0。

在本实施例中，利用轮速信息得到车辆的速度，再利用卡尔曼滤波模型的转换矩阵将该速度从车身坐标系转换到导航坐标系，能够丰富航位推算的观测量，减少imu引起的累积误差，提高航位推算的车辆速度的精度。

在一种应用示例中，如图5所示，基于低成本imu与轮速的定位系统可以包括：消费级imu，轮速can(controllerareanetwork，控制器局域网络)和gnss(globalnavigationsatellitesystem，全球导航卫星系统)传感器数据。根据imu可以获得三轴角速度和加速度，用于位姿的先验估计。其中，轮速can能够解析出后轮轮速，用于航位推算的位姿观测。gnss能够获得当前车辆的速度、位置和航向，用于正常差分下的数据融合与精准定位。

第一部分：位姿先验和卡尔曼滤波模型

1.1姿态预测：

其中，qk+1表示当前时刻车辆姿态四元数，qk表示上一时刻车辆姿态四元数，ωk+1表示当前时刻imu三轴陀螺仪的测量值，为算法估计的imu三轴陀螺游走噪声，表示四元数的乘法运算。

1.2速度预测：

其中，vk+1表示当前时刻车辆在东向、北向和天向速度，vk表示上一时刻车辆速度，ak+1表示当前时刻imu三轴加速度的测量值，为算法估计的imu三轴加速度游走噪声，车身坐标系到导航坐标系的转换矩阵为

1.3位置预测：

pk+1＝pk+vk×dt

其中，pk+1为当前时刻的位置先验hwheel_pos，pk为上一时刻的车辆位置，vk为上一时刻车辆在东向、北向和天向速度，dt为相邻时刻时间长度。

1.4预测状态和协方差矩阵

若将上述姿态预测表示为其中为最优位姿估计，uk+1为表示当前时刻的系统输入。例如，系统输入可以包括陀螺仪测量的角速度、加速度等。

将上一时刻的最优位姿估计，代入该式，可以得到当前时刻的位姿的先验预测。

其中的为状态协方差，f由状态转移方程导出，例如：

该方程表示姿态预测f(xk，uk+1)对位姿状态x的偏导数，其中xk表示上一时刻的位姿状态，uk+1表示当前时刻的系统输入。

其中的gqkg^t为噪声项的协方差更新，g由噪声驱动方程导出，例如：

该方程表示姿态预测f(xk，uk+1)对系统输入u的偏导数。

1.5更新卡尔曼增益kk+1：

其中，h为观测矩阵，rk+1为观测噪声矩阵，为上一时刻的协方差矩阵。h^r为h的转置。

1.6后验位姿估计

其中为位姿状态的先验预测，kk+1为卡尔曼滤波增益，zk+1当前时刻的状态观测，为观测坐标系下的位姿状态的先验预测。

1.7更新后验位姿协方差矩阵

其中kk+1为卡尔曼滤波增益，h为观测矩阵，为上一时刻的协方差矩阵，i为单位矩阵。

后验位姿协方差矩阵可以表示当前位姿估计的不确定性程度。如果协方差矩阵越大，不确定性越高。如果协方差矩阵越小，不确定性越低。另外，利用可以更新下一时刻的卡尔曼滤波增益(参见1.5的公式)，进而计算下一时刻的后验位姿估计。

第二部分：利用轮速信息进行航位推算的原理

2.1定位算法流程。

参见图6，为定位算法流程的一种示例：首先，可以获取imu三轴陀螺(角速度)和加速度数据。对三轴陀螺仪和加速度数据进行积分，获得车辆速度和位姿的先验估计。然后，系统判断gnss的差分状态，以确定gnss数据是否可用。例如：若gnss质量良好，gnss数据可用，则利用gnss获得的速度、位置、航向信息和轮速计算的纵向速度，通过卡尔曼滤波模型进行数据融合，估计imu的三轴陀螺、加速度游走噪声、车速比例系数、车辆速度和位姿误差。若gnss质量不佳，gnss数据不可用，则采用本申请实施例的航位推算方法，利用轮速信息例如后轮轮速，估计车俩后轴横纵向速度、航向角速度、位置增量和航向增量。然后，根据轮速估计出的速度、位置增量和航向增量，采用卡尔曼滤波模型进行航位推算。进而，得到车辆的速度、位姿的最优估计。

2.2轮速观测融合的实现方式

基于后轮(左后轮、右后轮)轮速信息，结合车辆运动学模型，对车辆纵向、横向速度、航向角速度和位姿进行估计。将估计的位姿作为位置和航向观测，引入卡尔曼滤波模型，提升算法的航位推算性能以及在低速、拥堵场景定位能力。

例如，对轮速传感器的观测融合主要包括以下几个部分：

基于轮速信息的车辆纵向、横向速度以及航向角速度估计；

基于轮速信息的位置增量和航向增量位姿估计；

基于车辆纵向、横向速度和位置增量、航向增量的观测更新。

2.3航位推算的优点

由于航位推算能力的提升，能有效提升以下场景的定位能力：

提升gps数据质量差时定位系统的航位推算能力；

提升低速拥堵场景定位能力(例如车速小于5m/s)；

提升大弯(两条高架道路交汇时的交流道，称为系统交流道或枢纽立交，英文为junction，可以简称为jct)、急弯等场景的定位能力。

2.4基于轮速的车辆纵向、横向速度以及航向角速度估计原理

以后轴两轮轮速为例，车辆横纵向速度和航向角速度推导如下：

参见图7，车辆物理参数轮距为l，车轮半径为r。

通过轮速传感器(也可以称为轮速计)可以获得左轮轮速wl和右轮轮速wr，若不考虑车轮打滑，则左右轮的线速度可以表示为：

vl＝wl×r

vr＝wr×r

其中，vl表示左轮的线速度，vr表示右轮的线速度。

由左右轮线速度可以推出后轴中心的线速度vm，方向与后轴垂直：

在一种可能的实施方式中，航向增量的推导原理可以包括：

基于左轮轮速wl、右轮轮速wr、车轮半径r和相邻时刻时间长度dt，可以得到左轮和右轮的运动弧长增量dsl和dsr。基于该左轮和右轮的运动弧长增量，以及轮距l，可以得到车辆的航向增量dθ。

示例性地，由于后轴为非转向轮，可以近似认为后轴中心横向速度为零。参见图7，左轮的运动弧长增量dsl和右轮的运动弧长增量dsr可以分别表示为：

dsl＝limδt→0wl×r×δt＝wl×r×dt

dsr＝limδt→0wr×r×δt＝wr×r×dt

其中，δt表示相邻时刻时间长度，趋近于0，因此δt也可以表示为dt。

车辆在任一时刻转过的航向增量dθ为：

此外，基于车辆的航向增量和相邻时刻时间长度，可以得到车辆的航向角速度ω。基于后轴中心线速度vm和航向角速度，可以得到转弯半径r。

示例性地，车辆的航向角速度可以表示ω为：

进而，转弯半径r为后轴中心线速度vm与航向角速度ω之比，参见下式：

2.5基于轮速的位姿推算原理

在一种可能的实施方式中，位置增量的推导原理可以包括：

基于上一时刻后轴中心的位置(x，y)、上一时刻的后轴中心的航向角θ和转弯半径r，可以得到旋转中心p的位置(px，py)。基于旋转中心p的位置(px，py)、上一时刻后轴中心的位置(x，y)、航向增量以及后轴中心运动弧长，可以得到当前时刻后轴中心的位置(x′，y′)。基于当前时刻后轴中心的位置和上一时刻后轴中心的位置，可以得到位置增量。

示例性地，如图8所示，假设车辆经dt时刻绕旋转中心p旋转角度rcenter为后轴中心的转弯半径，rleft为后轴左轮的转弯半径，rright为后轴右轮的转弯半径。

从上一时刻后轴中心的位姿(x，y，θ)移动到当前时刻后轴中心的位姿(x′，y′，θ′)，则旋转中心p的坐标(px，py)可以表示为：

其中，rcenter为转弯半径r，或称为旋转半径。p到后轴中心(x，y)或(x′，y′)的距离为rcenter。(x，y)为上一时刻后轴中心的位置，(x′，y′)为当前时刻后轴中心的位置。θ为上一时刻后轴中心的航向角，θ′为当前时刻后轴中心的航向角。

根据旋转中心p的坐标，可以推出(x′，y′，θ′)的坐标：

由于轮速数据采样间隔很小(例如，＞＝10hz)，因此航向增量(即之前计算的dθ)为接近0的小量，可以近似认为：

因此，上述(x′，y′，θ′)的坐标可以化简为：

上述式子中：

dcenter表示后轴中心运动的弧长，dleft(即上述的dsl)、dright(即上述的dsr)分别表示后轴左右轮运动的弧长。

此外，基于上一时刻的后轴中心的航向角θ、航向增量(也即dθ)，可以得到当前时刻后轴中心的航向角θ′，参见下式：

其中，ω为车辆的航向角速度，dt为相邻时刻时间长度。

2.6基于车辆纵向、横向速度和后轴纵向速度scale的观测更新

根据轮速模型，我们估计出了车辆后轴中心的横纵向速度，因此可以将估计出的横纵向速度作为速度观测，用于状态的修正和更新。

由前文轮速模型推导可得车辆纵向速度，由于后轴为非转向轴，可以近似认为横向和竖直方向速度为零。假设取车身坐标系纵向为x轴，横向为y轴，竖直向为z轴，则vx，vy，vz表示车辆纵向、横向和竖直方向的速度。其中，vx可以参考上述后轴中心线速度vm的计算方式。vx，vy，vz的计算方程可以参见下式：

vy＝vz＝0

另外，车辆的车速和真实的纵向速度存在一个比例误差：即后轴测量纵向速度与真实速度存在一个比例系数scale，根据经验可以估计这个比例系数，使车辆在隧道等场景实现更稳定的推算。

因此，可以认为对于车速的观测方程有：

其中，坐标旋转公式，表示从车身坐标系到导航坐标系的转换矩阵。

2.7基于轮速估计的位置增量对位置进行观测更新

参见上述推导过程，根据轮速模型可以估计出相邻时刻车辆位置的变换关系：

因此，相邻时刻，由轮速推出的位置增量(例如，如果导航坐标系为东北天坐标系，x轴表示东向，y轴表示北向，z轴表示天向)可以表示为，其中dcenter表示相邻时刻车辆运动的弧长，θ表示车辆航向(与正东方向夹角)：

其中，根据前文推导可知

如果导航坐标系发生改变，上述的车辆航向等计算公式也可以相应的发生改变。

虽然采用轮速进行位置估计存在累积误差，但是在相邻时刻的推算较为稳定可信。因此可以选取相邻时刻的轮速进行位置增量推算，其中_state_at_wheel_prev表示上一时刻车辆的位置，δwheel_pos表示由轮速从上一时刻到当前时刻推出的位置增量，_state_at_wheel表示当前时刻车辆的位置：

zwheel_pos＝_state_at_wheel_prev+δwheel_pos

hwheel_pos＝_state_at_wheel

zwheel_pos表示当前时刻由轮速推导出的位置观测，hwheel_pos表示当前时刻的位置先验，则位置观测和位置先验的差值可以表示为：

innowheel_pos＝zwheel_pos-hwheel_pos

2.8基于轮速估计的航向增量对航向进行观测更新

根据轮速模型估计出了相邻时刻车辆航向的变换关系，因此可以使用轮速模型估计出的航向增量进行观测更新，提升航向的预测精度：

由前文推导，车辆的航向角速度ωt可以由左右轮轮速wl、wr的差值推出，其中l表示轮距，r为车轮半径：

则相邻时刻航向增量δwheel_theta(即dθ)可以表示为：

δwheel_theta＝ωtdt

虽然采用轮速进行航向估计存在累积误差，但是在相邻时刻的推算较为稳定可信。因此可以选取相邻时刻的轮速进行航向增量推算。

假设_state_at_wheel_prev表示上一时刻车辆的航向，δwheel_heading表示由轮速从上一时刻到当前时刻推出的航向增量，_state_at_wheel表示当前时刻车辆的航向，dt为相邻时刻时间长度：

zwheel_heading＝_state_at_wheel_prev+δwheel_theta

hwheel_heading＝_state_at_wheel

zwheel_heading表示当前时刻由轮速推导出的航向观测，hwheel_heading表示当前时刻的航向先验，则航向观测和航向先验的差值可以表示为：

innowheel_heading＝zwheel_heading-hwheel_heading

此外，上述的位置观测和位置先验的差值，以及航向观测和航向先验的差值，可以代入卡尔曼滤波模型中，得到更准确的位姿估计。

例如：

后验位姿估计

其中为位姿状态的先验预测，kk+1为卡尔曼滤波增益，zk+1当前时刻的状态观测，为观测坐标系下的位姿状态的先验预测。

更新后验位姿协方差矩阵

其中kk+1为卡尔曼滤波增益，h为观测矩阵，为上一时刻的协方差矩阵，i为单位矩阵。协方差矩阵表示当前位姿估计的不确定性程度。如果协方差矩阵越大，不确定性越高。如果协方差矩阵越小，不确定性越低。另外，利用可以更新下一时刻的卡尔曼滤波增益，进而计算下一时刻的后验位姿估计。

示例性地，以下将具体数值代入本申请实施例中的主要公式说明车辆航位推算过程：

利用轮速传感器可以获得某一时刻左轮线速度vl＝wl×r＝14.063m/s，右轮线速度vr＝wr×r＝13.953m/s，相邻时刻采样间隔dt＝101ms，后轮轴距l为1.935m，上一采样间隔车辆航向θ为160.337°，则：

车辆纵向速度

后轴中心的运动弧长

位置增量

车辆航向角速度

因此，航向增量δwheel_theta＝ωtdt＝-3.258°/s×101×10^-3＝-0.329°。

此外，如果上一时刻车辆导航坐标位置为_state_at_wheel_prev＝(-452.789，122.191，0)，则由轮速获得的当前时刻位姿观测为：

zwheel_heading＝_state_at_wheel_prev+δwheel_theta＝160.337°-0.329°＝160.008°。

然后，将上述的位姿观测输入卡尔曼滤波模型，可以得到最优位姿估计。

如图9、图10所示，为相关方法和本申请的航位推算方法在2.5km隧道场景中初始化并进行dr推算的横纵向推算误差对比，航位推算横纵向误差随推算距离的变化曲线(真值为高精度组合惯导span-cpt数据)，单位为m，可见本申请的航位推算方法在2.5km推算距离内，能够显著降低定位系统横纵向位置的推算误差。

本申请充分利用轮速信息，首先根据后轮轮速差估计车辆航向角速度和纵向速度，然后根据轮速模型，推出相邻两次轮速数据的位置和航向变化量。本申请的方法相比于相关技术的航位推算方法，可以利用轮速估计出的速度、位置增量和航向信息共同作为卡尔曼滤波模型的观测，丰富了位姿估计模型的速度、位置和航向观测，同时对imu三轴的陀螺、加速度游走噪声进行估计，能够在远距离航位推算过程中显著减少由imu的三轴陀螺仪和三轴加速度信息积分引起的累积误差。

图11是根据本申请一实施例的车辆航位推算装置的框图。该装置可以包括：

增量模块41，用于基于车辆的轮速信息，得到该车辆的位姿增量；

推算模块42，用于基于该车辆的位姿增量和上一时刻的位姿，对该车辆在当前时刻的位姿进行航位推算。

在一种可能的实施方式中，如图12所示，该增量模块41包括：

位置增量子模块411，用于基于该轮速信息，得到该车辆的位置增量。

在一种可能的实施方式中，该位置增量子模块411具体用于基于左轮轮速、右轮轮速、车轮半径和相邻时刻时间长度，计算后轴中心的运动弧长；基于该后轴中心的运动弧长和航向角，计算该车辆的位置增量。

在一种可能的实施方式中，该推算模块42包括：

位置观测子模块421，用于基于该车辆的位置增量和上一时刻的位置，得到该车辆在当前时刻的位置观测。

在一种可能的实施方式中，该推算模块42还包括：

位置估计子模块422，用于计算该车辆在当前时刻的位置观测和该车辆在当前时刻的位置先验的位置差值，将该位置差值输入卡尔曼滤波模型，获得当前时刻的最优位置估计。

在一种可能的实施方式中，该增量模块41还包括：

航向增量子模块412，用于基于该轮速信息，得到该车辆的航向增量。

在一种可能的实施方式中，该航向增量子模块412具体用于基于左轮轮速和右轮轮速的差值，轮距，以及车轮半径，计算所述车辆的航向角速度；基于该车辆的航向角速度和相邻时刻时间长度，计算该车辆的航向增量。

在一种可能的实施方式中，该推算模块42还包括：

航向观测子模块423，用于基于该车辆的航向增量和上一时刻的航向角，得到该车辆在当前时刻的航向观测。

在一种可能的实施方式中，该推算模块42还包括：

航向估计子模块424，用于计算该车辆在当前时刻的航向观测和该车辆在当前时刻的航向先验的航向差值，将该航向差值输入卡尔曼滤波模型，获得当前时刻的最优航向估计。

在一种可能的实施方式中，该装置还包括：

速度模块51，用于基于该轮速信息，计算该车辆的速度。

在一种可能的实施方式中，该速度模块51具体用于基于该左轮轮速、右轮轮速和车轮半径，计算左轮线速度和右轮线速度；基于该左轮线速度和该右轮线速度，计算后轴中心线速度。

在一种可能的实施方式中，该装置还包括：

速度观测模块52，用于基于后轴测量纵向速度与真实速度的比例系数，后轴中心线速度，以及从车身坐标系到导航坐标系的转换矩阵，得到该车辆的车速观测。

本申请实施例各装置中的各模块的功能可以参见上述方法中的对应描述，在此不再赘述。

根据本申请的实施例，本申请还提供了一种电子设备和一种可读存储介质。

如图13所示，是根据本申请实施例的车辆航位推算方法的电子设备的框图。电子设备旨在表示各种形式的数字计算机，诸如，膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置，诸如，个人数字处理、蜂窝电话、智能电话、可穿戴设备和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本申请的实现。

如图13所示，该电子设备包括：一个或多个处理器901、存储器902，以及用于连接各部件的接口，包括高速接口和低速接口。各个部件利用不同的总线互相连接，并且可以被安装在公共主板上或者根据需要以其它方式安装。处理器可以对在电子设备内执行的指令进行处理，包括存储在存储器中或者存储器上以在外部输入/输出装置(诸如，耦合至接口的显示设备)上显示gui的图形信息的指令。在其它实施方式中，若需要，可以将多个处理器和/或多条总线与多个存储器和多个存储器一起使用。同样，可以连接多个电子设备，各个设备提供部分必要的操作(例如，作为服务器阵列、一组刀片式服务器、或者多处理器系统)。图13中以一个处理器901为例。

存储器902即为本申请所提供的非瞬时计算机可读存储介质。其中，该存储器存储有可由至少一个处理器执行的指令，以使该至少一个处理器执行本申请所提供的车辆航位推算方法。本申请的非瞬时计算机可读存储介质存储计算机指令，该计算机指令用于使计算机执行本申请所提供的车辆航位推算方法。

存储器902作为一种非瞬时计算机可读存储介质，可用于存储非瞬时软件程序、非瞬时计算机可执行程序以及模块，如本申请实施例中的车辆航位推算方法对应的程序指令/模块(例如，附图11所示的增量模块41和推算模块42)。处理器901通过运行存储在存储器902中的非瞬时软件程序、指令以及模块，从而执行服务器的各种功能应用以及数据处理，即实现上述方法实施例中的车辆航位推算方法。

存储器902可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储根据车辆航位推算方法的电子设备的使用所创建的数据等。此外，存储器902可以包括高速随机存取存储器，还可以包括非瞬时存储器，例如至少一个磁盘存储器件、闪存器件、或其他非瞬时固态存储器件。在一些实施例中，存储器902可选包括相对于处理器901远程设置的存储器，这些远程存储器可以通过网络连接至车辆航位推算方法的电子设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

车辆航位推算方法的电子设备还可以包括：输入装置903和输出装置904。处理器901、存储器902、输入装置903和输出装置904可以通过总线或者其他方式连接，图13中以通过总线连接为例。

输入装置903可接收输入的数字或字符信息，以及产生与车辆航位推算方法的电子设备的用户设置以及功能控制有关的键信号输入，例如触摸屏、小键盘、鼠标、轨迹板、触摸板、指示杆、一个或者多个鼠标按钮、轨迹球、操纵杆等输入装置。输出装置904可以包括显示设备、辅助照明装置(例如，led)和触觉反馈装置(例如，振动电机)等。该显示设备可以包括但不限于，液晶显示器(lcd)、发光二极管(led)显示器和等离子体显示器。在一些实施方式中，显示设备可以是触摸屏。

此处描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、专用asic(专用集成电路)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括：实施在一个或者多个计算机程序中，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。

这些计算程序(也称作程序、软件、软件应用、或者代码)包括可编程处理器的机器指令，并且可以利用高级过程和/或面向对象的编程语言、和/或汇编/机器语言来实施这些计算程序。如本文使用的，术语“机器可读介质”和“计算机可读介质”指的是用于将机器指令和/或数据提供给可编程处理器的任何计算机程序产品、设备、和/或装置(例如，磁盘、光盘、存储器、可编程逻辑装置(pld))，包括，接收作为机器可读信号的机器指令的机器可读介质。术语“机器可读信号”指的是用于将机器指令和/或数据提供给可编程处理器的任何信号。

为了提供与用户的交互，可以在计算机上实施此处描述的系统和技术，该计算机具有：用于向用户显示信息的显示装置(例如，crt(阴极射线管)或者lcd(液晶显示器)监视器)；以及键盘和指向装置(例如，鼠标或者轨迹球)，用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈(例如，视觉反馈、听觉反馈、或者触觉反馈)；并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。

可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如，作为数据服务器)、或者包括中间件部件的计算系统(例如，应用服务器)、或者包括前端部件的计算系统(例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如，通信网络)来将系统的部件相互连接。通信网络的示例包括：局域网(lan)、广域网(wan)和互联网。

计算机系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器，又称为云计算服务器或云主机，是云计算服务体系中的一项主机产品，以解决了传统物理主机与虚拟专用服务器(vps)服务中，存在的管理难度大，业务扩展性弱的缺陷。

根据本申请实施例的技术方案，利用轮速信息得到位姿增量，再利用位姿增量进行航位推算，能够减少累积误差，提高航位推算结果的精度。

应该理解，可以使用上面所示的各种形式的流程，重新排序、增加或删除步骤。例如，本发申请中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行，只要能够实现本申请公开的技术方案所期望的结果，本文在此不进行限制。

上述具体实施方式，并不构成对本申请保护范围的限制。本领域技术人员应该明白的是，根据设计要求和其他因素，可以进行各种修改、组合、子组合和替代。任何在本申请的精神和原则之内所作的修改、等同替换和改进等，均应包含在本申请保护范围之内。