一种车辆定位方法、计算机设备、计算机可读存储介质与流程

1.本发明涉及车辆定位技术领域，具体涉及一种车辆定位方法、计算机设备、 计算机可读存储介质。


背景技术：

2.传统的车辆定位，主要应用于车辆的导航、追踪，停车场寻车等场合，对 精度要求一般在十米级或者米级，可靠性要求也不高，偶然的信号丢失也能够 接受。但自动驾驶场景下，对定位精度要求在分米级甚至厘米级，位置信号的 输出必须连续稳定，可靠性要求非常高，而且实时性要求也比其他应用场合要 高。这是因为，自动驾驶车辆必须足够精确地获得自身位置用以制定驾驶策略， 在众多场景中实现比人更加精准的操控，定位精度越高，稳定性越好，对自动 驾驶越有利。


技术实现要素：

3.本发明的目的在于提出一种车辆定位方法、计算机设备、计算机可读存储 介质，以提高车辆室内外定位精度，以适应自动驾驶场景的定位精度要求。
4.为实现上述目的，根据第一方面，本发明实施例提出一种车辆定位方法， 包括周期性地执行以下步骤：
5.获取当前时刻所对应的最近一个预设周期内的车辆行驶里程，并根据所述 车辆行驶里程计算当前时刻的第一航向角偏移以及位置坐标偏移值；
6.获取当前时刻的前轮转角，并根据所述当前时刻的前轮转角计算当前时刻 的第二航向角偏移值；
7.根据所述第一航向角偏移值与所述第二航向角偏移值计算得到第三航向角 偏移值，将上一时刻的航向角与所述第三航向角偏移值叠加得到当前时刻的航 向角预测值；并将上一时刻的位置坐标与当前时刻的所述位置坐标偏移值叠加 得到当前时刻的位置坐标预测值；
8.接收当前使用的车载定位系统的帧数据，根据接收到的最近一帧数据及其 接收时间、以及当前时刻所对应的时间计算车辆当前时刻的位置坐标观测值， 并根据所述当前时刻、上一时刻的位置坐标观测值计算当前时刻的航向角观测 值；其中根据gnss定位系统和uwb定位系统的信号强度选择其中之一作为当 前使用的车载定位系统；
9.根据所述当前时刻的航向角预测值、航向角观测值计算当前时刻的航向角， 并根据所述当前时刻的位置坐标预测值、位置坐标观测值计算当前时刻的位置 坐标。
10.可选地，所述方法还包括：
11.当车辆上电时，对车辆位置坐标进行初始化，根据gnss定位系统和uwb 定位系统的信号强度选择其中之一作为当前使用的车载定位系统，获取车载定 位系统的当前帧数据，根据该当前帧数据获得对应的车辆绝对位置坐标，并将 该车辆绝对位置坐标作为初始时刻的车辆位置坐标。
12.可选地，所述gnss定位系统和uwb定位系统均采用双天线结构，包括设 置于车辆前部的前天线和设置于车辆后部的后天线，所述gnss定位系统和 uwb定位系统的帧数据包括前、后天线坐标信息；
13.所述方法还包括：
14.获取当前使用的车载定位系统的后天线坐标信息，根据所述后天线坐标信 息计算对应的车辆绝对位置坐标，并将该车辆绝对位置坐标作为初始时刻的车 辆位置坐标；
15.获取当前使用的车载定位系统的前天线坐标信息，根据所述前、后天线坐 标信息计算初始时刻的车辆航向角；
[0016][0017]
其中，yaw0为初始时刻的车辆航向角，δx0和δy0分别为所述前、后天线坐 标之间的横、纵坐标差值。
[0018]
可选地，当车辆位于停车场时，如果车辆进入停车场的uwb信号有效覆盖 区域或者车速处于预设低速区间，则选择uwb定位系统作为当前使用的车载定 位系统。
[0019]
可选地，所述方法还包括：
[0020]
在车辆上电时，对车辆航向角进行初始化，获取当前时刻的车载电子罗盘 的航向角，将该航向角作为初始时刻的车辆航向角。
[0021]
可选地，所述当前时刻的位置坐标偏移值的计算过程具体如下：
[0022]
δx1＝s
×
cos(yaw
t-1
)
[0023]
δy1＝s
×
sin(yaw
t-1
)
[0024]
其中，δx1为横向坐标偏移值，δy1为纵向坐标偏移值，s为在最近一个预 设周期内的车辆行驶里程，yaw
t-1
为上一时刻的航向角；
[0025]
所述当前时刻的第一航向角偏移的计算过程具体如下：
[0026][0027][0028]
其中，δyaw为第一航向角偏移，d1和d2分别为车辆左、右后轮在最近一个 预设周期内的行驶里程，w为车辆左、右后轮轮距。。
[0029]
可选地，所述当前时刻的第二航向角偏移值的计算过程具体如下：
[0030][0031]
其中，δyaw
δ
为第二航向角偏移值，s为在最近一个预设周期内的车辆行驶 里程，δ为前轮转角，l为车辆轴距。
[0032]
可选地，所述第三航向角偏移值的计算过程具体如下：
[0033]
δyaw
′
t
＝ky×
δyaw
t
(1-ky)
×
δyaw
δ
[0034]
其中，δyaw
′
t
为第二航向角偏移值，ky为第一融合系数，0《ky《1。
[0035]
可选地，所述车辆当前时刻的位置坐标观测值的计算过程具体如下：
[0036][0037]
δt＝t-t1[0038]
其中，(x
ob
,y
ob
)为车辆当前时刻的位置坐标观测值，t为当前时刻所对应的 时间，t1为接收最近一帧数据的时间，为当前时刻的车速，(x1,y1)为最近一帧 数据所对应的车辆位置坐标。
[0039]
可选地，所述当前时刻的航向角观测值的具体计算过程如下：
[0040][0041]
其中，yaw
ob
为当前时刻的航向角观测值，δx2和δy2分别为所述当前时刻 和上一时刻的位置坐标观测值之间的横、纵坐标偏移。
[0042]
可选地，所述当前时刻的航向角的计算过程具体如下：
[0043]
yaw
t
＝k
p
×
yaw
ob
(1-k
p
)
×
yaw
pr
[0044]
其中，yaw
t
为当前时刻的航向角，yaw
ob
为当前时刻的航向角观测值， yaw
pr
为当前时刻的航向角预测值，k
p
为第二融合系数，0《k
p
《1；
[0045]
所述当前时刻的位置坐标的计算过程具体如下：
[0046]
(x
t
,y
t
)＝k
p
×
(x
ob
,y
ob
) (1-k
p
)
×
(x
pr
,y
pr
)
[0047]
其中，(x
t
,y
t
)为当前时刻的位置坐标，(x
ob
,y
ob
)为当前时刻的位置坐标观 测值，(x
pr
,y
pr
)为当前时刻的位置坐标预测值。
[0048]
根据第二方面，本发明实施例提出一种计算机设备，包括：存储器和处理 器，所述存储器中存储有计算机可读指令，所述计算机可读指令被所述处理器 执行时，使得所述处理器执行根据第一方面实施例所述车辆定位方法的步骤。
[0049]
根据第三方面，本发明实施例提出一种计算机可读存储介质，其上存储有 计算机程序，所述计算机程序被处理器执行时实现第一方面实施例所述车辆定 位方法。
[0050]
本发明实施例提出一种车辆定位方法、计算机设备、计算机可读存储介质， 通过根据最近一个预设时间周期的车辆行驶里程来计算当前时刻的第一航向角 偏移以及位置坐标偏移值；根据所述当前时刻的前轮转角计算当前时刻的第二 航向角偏移值；根据所述第一航向角偏移值与所述第二航向角偏移值计算得到 第三航向角偏移值；将上一时刻的航向角与所述第三航向角偏移值叠加得到当 前时刻的航向角预测值；并将上一时刻的位置坐标与当前时刻的所述位置坐标 偏移值叠加得到当前时刻的位置坐标预测值；根据接收到的车载定位系统最近 一帧数据及其接收时间、以及当前时刻所对应的时间计算车辆当前时刻的位置 坐标观测值，并根据所述当前时刻、上一时刻的位置坐标观测值计算当前时刻 的航向角观测值；最终，根据所述当前时刻的航向角预测值、航向角观测值计 算当前时刻的航向角，并根据所述当前时刻的位置坐标预测值、位置坐标观测 值计算当前时刻的位置坐标。本发明实施例根据gnss定位系统和uwb定位系 统的信号强度选择其中之一作为当前使用的车载定位系统，能够适应室内外不 同场地的定位系统信号强弱问题，同时利用车辆行驶里程计算第一航向角偏移 以及位置坐标偏移值，提出了位置坐标和航向角的观测值以及预测值的融合来 得到最终的位置坐标和航向角，使得最终定位的车辆位置坐标和航向角更为准 确，提高车辆室内外定位精度，以适应自动驾驶场景的定位精度要求。
[0051]
本发明的其它特征和优点将在随后的说明书中阐述。
附图说明
[0052]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施 例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述 中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付 出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0053]
图1为本发明一实施例中一种车辆定位方法的流程图。图2为本发明一实施例中车辆ctrv恒定转率和速度模型示意图。图3为本发明一实施例中根据所述前、后天线坐标信息计算初始时刻的车 辆航向角的原理示意图。图4为本发明一实施例中停车场示例图。
具体实施方式
[0054]
以下将参考附图详细说明本公开的各种示例性实施例、特征和方面。另外， 为了更好的说明本发明，在下文的具体实施例中给出了众多的具体细节。本领 域技术人员应当理解，没有某些具体细节，本发明同样可以实施。在一些实例 中，对于本领域技术人员熟知的手段未作详细描述，以便于凸显本发明的主旨。
[0055]
参阅图1，本发明的一实施例提出一种车辆定位方法，包括周期性地执行以 下步骤s1～s5：
[0056]
步骤s1、获取当前时刻所对应的最近一个预设周期内的车辆行驶里程，并 根据所述车辆行驶里程计算当前时刻的第一航向角偏移以及位置坐标偏移值。
[0057]
具体而言，本实施中周期性地读取车轮编码器数据，分别计算车辆左后轮 和右后轮在一个周期内的累积行程，假设车轮半径为r，车轮转动一圈编码器输 出脉冲数为1，n为一个周期内的车轮编码器输出的脉冲数，对应累积行程为d， 则有如下关系：
[0058][0059]
据此可以计算出左、右后轮的在一个周期内的累积行程d1和d2，则最近一 个预设周期内的车辆行驶里程s为：
[0060][0061]
可选地，本实施例中所述当前时刻的位置坐标偏移值的计算过程具体如下：
[0062]
δx1＝s
×
cos(yaw
t-1
)
[0063]
δy1＝s
×
sin(yaw
t-1
)
[0064]
其中，δx1为横向坐标偏移值，δy1为纵向坐标偏移值，yaw
t-1
为上一时刻 的航向角；如果是首次运算，则对应初始时刻的航向角。
[0065]
可选地，本实施例中所述当前时刻的第一航向角偏移的计算过程具体如下：
[0066][0067]
其中，δyaw为第一航向角偏移，d1和d2分别为车辆左、右后轮在最近一个 预设周期内的行驶里程，w为车辆左、右后轮轮距。
[0068]
步骤s2、获取当前时刻的前轮转角，并根据所述当前时刻的前轮转角计算 当前时刻的第二航向角偏移值。
[0069]
可选地，本实施例中所述当前时刻的第二航向角偏移值的计算过程具体如 下：
[0070][0071]
其中，δyaw
δ
为第二航向角偏移值，s为在最近一个预设周期内的车辆行驶 里程，δ为前轮转角，l为车辆轴距。
[0072]
示例性地，步骤中可以周期性地读取当前方向盘转角传感器数据，结合车 辆ctrv模型，如图2所示，通过预先对车身建立的标定即可计算出对应的前 轮转角δ；当然，前轮转角δ也可以通过其他方式获得。
[0073]
需说明的是，步骤s1和步骤s2之间没有先后执行顺序上的限定。
[0074]
步骤s3、根据所述第一航向角偏移值与所述第二航向角偏移值计算得到第 三航向角偏移值，将上一时刻的航向角与所述第三航向角偏移值叠加得到当前 时刻的航向角预测值；并将上一时刻的位置坐标与当前时刻的所述位置坐标偏 移值叠加得到当前时刻的位置坐标预测值；
[0075]
可选地，本实施例中将第二航向角偏移值δyaw
δ
作为先验估计预测值，将第 第一航向偏移值δyaw
t
作为观测值，运用贝叶斯定理进行后验修正融合，融合过 程可简化为二者根据方差的加权，假设融合系数为ky，其中0《ky《1，该值可通 过计算δyaw
δ
和δyaw
t
各自的均方差e
δ
和e
t
得到，均方差越大代表数据抖动越大， 越不可靠，权重越低：
[0076][0077]
从而得到更精确的航向角偏移δyaw
t
′
：
[0078]
δyaw
′
t
＝ky×
δyaw
t
(1-ky)
×
δyaw
δ
[0079]
本步骤中，将上一时刻的航向角与所述第三航向角偏移值叠加得到当前时 刻的航向角预测值，即：
[0080]
yaw
pr
＝yaw
t-1
δyaw
′
t
[0081]
其中，yaw
pr
为当前时刻的航向角预测值，yaw
t-1
为上一时刻的航向角。
[0082]
本步骤中，将上一时刻的位置坐标与当前时刻的所述位置坐标偏移值叠加 得到当前时刻的位置坐标预测值，即：
[0083]
x
pr
＝x
t-1
δx1
[0084]ypr
＝y
t-1
δy1
[0085]
其中，x
pr
,y
pr
分别为当前时刻的横、纵位置坐标预测值，x
t-1
，y
t-1
分别为 上一时刻的横、纵位置坐标值。
[0086]
步骤s4、接收当前使用的车载定位系统的帧数据，根据接收到的最近一帧 数据及
其接收时间、以及当前时刻所对应的时间计算车辆当前时刻的位置坐标 观测值，并根据所述当前时刻、上一时刻的位置坐标观测值计算当前时刻的航 向角观测值；其中根据gnss定位系统和uwb定位系统的信号强度选择其中之 一作为当前使用的车载定位系统。
[0087]
具体而言，根据当前时间和车载定位系统最近一帧数据，估算出其当前绝 对坐标，以此作为当前时刻的位置坐标的观测值，同时把该观测值保存下来作 为下个处理周期的历史数值。
[0088]
其中，gnss定位系统和uwb定位系统周期性输出数据的帧率为10hz， 系统通过串口接收，数据内容包含世界坐标系下的绝对坐标信息，相邻数据帧 之间有100ms的间隔，处理周期和数据帧输出周期并不同步，这就意味着程序 读到的最近一帧数据不一定是代表当前的实际位置，因此需要对数据进行时间 轴上的平移，所述车辆当前时刻的位置坐标观测值的计算过程具体如下：
[0089][0090]
δt＝t-t1[0091]
其中，(x
ob
,y
ob
)为车辆当前时刻的位置坐标观测值的横、纵坐标，t为当前 时刻所对应的时间，t1为接收最近一帧数据的时间，为当前时刻的车速， (x1,y1)为最近一帧数据所对应的车辆位置横、纵坐标。
[0092]
步骤中，根据所述当前时刻、上一时刻的位置坐标观测值计算当前时刻的 航向角观测值具体如下：
[0093][0094]
其中，yaw
ob
为当前时刻的航向角观测值，δx2和δy2分别为所述当前时刻 和上一时刻的位置坐标观测值之间的横、纵坐标偏移。
[0095]
步骤s5、根据所述当前时刻的航向角预测值、航向角观测值计算当前时刻 的航向角，并根据所述当前时刻的位置坐标预测值、位置坐标观测值计算当前 时刻的位置坐标。
[0096]
可选地，步骤s5运用贝叶斯定理进行后验修正，假设融合系数为k
p
， 0《k
p
《1，k
p
值可通过计算位资预测值和位资观测值各自的均方差e
pr
和 e
ob
得到，均方差越大代表数据抖动越大，越不可靠，权重越低：
[0097][0098]
得到更精确的车辆位姿
[0099][0100]
其中，位资预测值包括位置坐标预测值和航向角预测值，位资观测值包括位置坐标观测值和航向角观测值。
[0101]
本实施例中所述当前时刻的航向角的计算过程具体如下：
[0102]
yaw
t
＝k
p
×
yaw
ob
(1-k
p
)
×
yaw
pr
[0103]
其中，yaw
t
为当前时刻的航向角，yaw
ob
为当前时刻的航向角观测值， yaw
pr
为当前时刻的航向角预测值，k
p
为第二融合系数，0《k
p
《1；
[0104]
所述当前时刻的位置坐标的计算过程具体如下：
[0105]
(x
t
,y
t
)＝k
p
×
(x
ob
,y
ob
) (1-k
p
)
×
(x
pr
,y
pr
)
[0106]
其中，(x
t
,y
t
)为当前时刻的位置坐标，(x
ob
,y
ob
)为当前时刻的位置坐标观 测值，(x
pr
,y
pr
)为当前时刻的位置坐标预测值。
[0107]
保存计算得到的当前时刻的位置坐标以及航向角，用于下一时刻的位置坐 标以及航向角计算。
[0108]
可选地，本实施例所述方法还包括：
[0109]
当车辆上电时，对车辆位置坐标进行初始化，根据gnss定位系统和uwb 定位系统的信号强度选择其中之一作为当前使用的车载定位系统，获取车载定 位系统的当前帧数据，根据该当前帧数据获得对应的车辆绝对位置坐标，并将 该车辆绝对位置坐标作为初始时刻的车辆位置坐标。
[0110]
具体而言，根据信号状况决定从gnss定位系统还是uwb定位系统获取绝 对位置坐标信息，对绝对位置坐标进行初始化。读取gnss定位系统的接收卫 星数和rtk解算的定位状态，根据信号质量的好坏决定是否屏蔽gnss的信号； 读取uwb定位系统的数据有效标志位判断是否处于uwb定位系统覆盖的有效 区域；综合gnss定位系统和uwb定位系统信号强弱有效情况，只有当二者满 足其一的时候才对系统进行初始化。一般情况下在室外空旷处会通过gnss定 位系统进行初始化，而室内则会通过预先部署的uwb定位系统初始化。
[0111]
基于上述实施例方法，在一种示例中：
[0112]
所述gnss定位系统和uwb定位系统均采用双天线结构，包括设置于车辆 前部的前天线和设置于车辆后部的后天线，所述gnss定位系统和uwb定位系 统的帧数据包括前、后天线坐标信息。
[0113]
所述方法还包括：
[0114]
获取当前使用的车载定位系统的后天线坐标信息，根据所述后天线坐标信 息计算对应的车辆绝对位置坐标，并将该车辆绝对位置坐标作为初始时刻的车 辆位置坐标；
[0115]
其中，所述方法还包括：
[0116]
获取当前使用的车载定位系统的前天线坐标信息，根据所述前、后天线坐 标信息计算初始时刻的车辆航向角；计算方法参阅图3，通过车辆行驶过程中前 后相邻的两个坐标点on(xn,yn)与o
n-1
(x
n-1
,y
n-1
)所确定的向量算出其相对世界坐标 系横坐标的夹角α，即：
[0117][0118]
其中，yaw0为初始时刻的车辆航向角，δx0和δy0分别为所述前、后天线坐 标之间的横、纵坐标差值。
[0119]
可选地，参阅图4，当车辆位于停车场时，如果车辆进入停车场的uwb信 号有效覆盖区域或者车速处于预设低速区间，例如图中圈出的矩形方框区域， 则选择uwb定位系统作为当前使用的车载定位系统。
[0120]
基于上述实施例方法，在另一种示例中：
[0121]
在车辆上电时，对车辆航向角进行初始化，获取当前时刻的车载电子罗盘 的航向
角，将该航向角作为初始时刻的车辆航向角。
[0122]
可选地，参阅图4，当车辆位于停车场时，如果车辆进入停车场的uwb信 号有效覆盖区域或者车速处于预设低速区间，例如图中圈出的矩形方框区域， 则选择uwb定位系统作为当前使用的车载定位系统。
[0123]
具体而言，与前面的示例不同的是，本示例中使用车载电子罗盘来初始化 车辆航向角。航向角的初始化对系统收敛时间有相当大的影响，因此在车辆刚 刚上电的时候就可以对其进行初始化；又由于电子罗盘信号有延时并且容易受 干扰影响，因此只把其作为航向角的粗略初始化，后续根据航向角偏移情况进 行修正。
[0124]
本发明实施例还提出一种计算机设备，包括：存储器和处理器，所述存储 器中存储有计算机可读指令，所述计算机可读指令被所述处理器执行时，使得 所述处理器执行根据上述实施例所述车辆定位方法的步骤。
[0125]
当然，所述计算机设备还可以具有有线或无线网络接口、键盘以及输入输 出接口等部件，以便进行输入输出，该计算机设备还可以包括其他用于实现设 备功能的部件，在此不做赘述。
[0126]
示例性的，所述计算机程序可以被分割成一个或多个单元，所述一个或者 多个单元被存储在所述存储器中，并由所述处理器执行，以完成本发明。所述 一个或多个单元可以是能够完成特定功能的一系列计算机程序指令段，该指令 段用于描述所述计算机程序在所述计算机设备中的执行过程。
[0127]
所述处理器可以是中央处理单元(central processing unit，cpu)，还可以是 其他通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电 路(application specific integrated circuit，asic)、现成可编程门阵列(field
-ꢀ
programmable gate array，fpga)或者其他可编程逻辑器件、分立门或者晶体管 逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以 是任何常规的处理器等，所述处理器是所述计算机设备的控制中心，利用各种 接口和线路连接整个所述计算机设备的各个部分。
[0128]
所述存储器可用于存储所述计算机程序和/或单元，所述处理器通过运行或 执行存储在所述存储器内的计算机程序和/或单元，以及调用存储在存储器内的 数据，实现所述计算机设备的各种功能。此外，存储器可以包括高速随机存取 存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存 储卡(smart media card,smc)，安全数字(secure digital,sd)卡，闪存卡(flashcard)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
[0129]
本发明实施例还提出一种计算机可读存储介质，其上存储有计算机程序， 所述计算机程序被处理器执行时实现上述实施例所述车辆定位方法。
[0130]
示例性地，所述计算机可读存储介质可以包括：能够携带所述计算机程序 代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存 储器、只读存储器(rom，read-only memory)、随机存取存储器(ram， random access memory)、电载波信号、电信信号以及软件分发介质等。
[0131]
以上已经描述了本发明的各实施例，上述说明是示例性的，并非穷尽性的， 并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的 情况下，对于本
技术领域的普通技术人员来说许多修改和变更都是显而易见的。 本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场 中的技术改进，或者使本技术领域的其它普通技术人员能理解本文披露的各实 施例。