一种基于位姿图优化的路网地图辅助车辆定位方法

1.本发明涉及地图匹配和导航定位领域，尤其涉及一种基于位姿图优化的路网地图辅助车辆定位方法。


背景技术：

2.高精度的车辆导航定位技术是完成各项工程作业的关键。全球定位系统利用地球轨道上的卫星提供位置信息，凭借其高性价比广泛应用于车辆导航系统。然而，在卫星信号拒止的情况下，车辆面临较大的定位困难。相机凭借其使用方便、重量轻、性价比高等优势，被广泛应用于解决车辆定位问题，通过向视觉里程计(visual odometry，简称vo)中输入一系列图像以跟踪车辆的运动状态。视觉里程计依赖于航位推算，随着时间和行驶距离的增加，微小的误差会逐渐累积从而导致定位效果下降。
3.为了解决视觉里程计漂移问题，部分学者提出了基于视觉同步定位与建图(visual simultaneous localization and mapping，简称vslam)的方法，但是此方法需要车辆重复驶过历史场景，构建闭环来降低定位误差，影响车辆的行驶规划。还有一些研究将视觉里程计和其他传感器的附加信息结合以缓解漂移问题，但引入额外传感器会大大增加系统的硬件成本和数据处理复杂程度。


技术实现要素：

4.发明目的：本发明的目的是提供一种能实时提供稳定且准确的车辆位置估计的基于位姿图优化的路网地图辅助车辆定位方法。
5.技术方案：本发明的路网地图辅助车辆定位方法，是在已知车辆的初始位置、初始姿态角和车辆所属路网基元的情况下，从路网地图中引入约束抑制视觉里程计累积误差，结合视觉里程计原始轨迹和地图修正点的影响，输出修正轨迹，最后根据视觉里程计推算结果与修正轨迹预测车辆定位结果，包括步骤如下：
6.s1，根据车辆初始全局位置加载区域性路网地图；
7.s2，以道路交汇点为支撑点，以相邻支撑点之间的道路作为路网基元，将区域性路网地图转化为路网基元图；
8.s3，根据路网基元首尾节点的位置关系，计算不同路网基元之间的连通性；
9.s4，依据各路网基元内部相邻骨架点间距离，判断是否需要增加各路网基元内部骨架点数量；
10.s5，根据车辆初始姿态角和传感器外参，计算初始转换矩阵
11.s6，根据车辆初始阶段行驶航向，正向化车辆所属路网基元；
12.s7，根据将视觉里程计输出位姿转换到东北天坐标系下；
13.s8，从路网地图中引入约束，设立判别条件，筛选地图修正点；
14.s9，构建轨迹的位姿图优化模型，加入局部视觉因子和全局地图修正点因子，通过求解轨迹的位姿图优化问题，修正历史轨迹；
15.s10，根据视觉里程计推算结果和修正轨迹预测最新时刻车辆位姿；
16.s11，接收视觉里程计输出，重复步骤s7至s10，直至车辆停止。
17.进一步，步骤s2中，将路网支撑点定义为拐点tp，任一路网基元的首尾节点均为拐点，拐点包含不同路网基元之间的拓扑连接关系；在路网基元内部，以一系列节点的连线来近似表示道路形状，定义此系列节点为骨架点sp；则有单一路网基元的结构组成表示如下：
18.{tp
start
,sp1,sp2,
…
,spn,tp
end
}
19.其中，tp
start
表示路网基元首拐点，tp
end
表示路网基元尾拐点，{sp1,sp2,
…
,spn}表示路网基元的骨架点组；
20.若骨架点组为空，则此路网基元的首部朝向和尾部朝向皆为若骨架点组不为空，则此路网基元的首部朝向为尾部朝向为
21.进一步，步骤s3中，对于单一路网基元，首先提取其首拐点tp
start
和尾拐点tp
end
，遍历路网基元集合内其余路网基元的首尾拐点：
22.若存在某路网基元的首拐点与tp
start
或tp
end
位于同一道路交叉口，则判定此两路网基元是连通的；
23.或者，若存在某路网基元的尾拐点与tp
start
或tp
end
位于同一道路交叉口，则判定此两路网基元是连通的。
24.进一步，步骤s4中，依据各路网基元内部相邻骨架点间距离，判断是否需要增加各路网基元内部骨架点数量的详细步骤如下：
25.s41，对于路网基元{tp
start
,sp1,sp2,
…
,spn,tp
end
}，将其分解为折线段组{tp
start-sp1，sp
1-sp2，
…
，sp
n-tp
end
}；
26.s42，分别计算每一折线段的折线距离lenz，其中z＝1，2，
…
，n+1；若lenz＜15，判定两节点之间距离较小，不需要增加骨架点；若lenz＞15，则需要增加骨架点。
27.进一步，步骤s6中，正向化车辆所属路网基元的实现过程如下：
28.已知车辆初始时刻所属路网基元，记为rnbe
init
；车辆在初始阶段处于停滞状态，记初始位置为p
w-init
，经过转化矩阵转化为enu坐标系下的位置p
g-init
；
29.累计位移首次超过2米的车辆位置记为p
w-end
，经转化矩阵转化为enu坐标系下的位置为p
g-end
；经ecef坐标系过渡，将p
g-init
和p
g-end
转换为lla坐标p
l-init
和p
l-end
；
30.计算车辆行进方向与所属折线正向的夹角为θ
+
，计算车辆行进方向与所属折线逆向的夹角为θ-：
31.若θ
+
＞θ-，保持rnbe
init
原结构不变；若θ
+
＜θ-，反向调整rnbe
init
原结构，与车辆行驶方向保持一致。
32.进一步，车辆累计位移lenall(p
w1
,p
wm
)的计算表达式如下：
33.lenall(p
w1
,p
wm
)＝len(p
w1
,p
w2
)+len(p
w2
,p
w3
)+
…
+len(p
wm-1
,p
wm
)
34.其中，len()表示两点直线距离，p
wm
表示第m帧时刻车辆在相机世界坐标系下的位置。
35.进一步，步骤s8中，所述筛选修正点包括路网基元尾tp修正点筛选和路网基元内部sp修正点筛选；
36.s81，所述tp修正点包括转向tp修正点和近直tp修正点，分别对应于转向路网基元和近直路网基元；实现过程如下：
37.s811，设定只有单一预行驶路网基元的情况
38.s8111，若预行驶路网基元为转向路网基元，预行驶基元的首部朝向与rnbe
cur
的尾部朝向夹角为分别计算车辆当前行驶朝向与rnbe
cur
尾部朝向夹角β1、车辆当前行驶朝向与预行驶基元首部朝向夹角β2，若满足条件a，则判定满足转向tp修正点筛选条件；条件a如下：
[0039][0040]
其中，rnbe
cur
为车辆当前所属路网基元；
[0041]
将tp沿转向路网基元首部朝向平移一半路宽距离后粒子化，最后以rnbe
cur
首tp为参照，应用相似度计算方法求得满足设定条件的粒子，为转向tp修正点；
[0042]
s8112，若预行驶基元为近直基元，记车辆上一时刻与rnbe
cur
首tp的距离为len
vo_last
，车辆当前时刻与rnbe
cur
首tp的距离为len
vo_cur
；设车辆处于匀速状态推测下一时刻车辆与rnbe
cur
首tp的距离为len
vo_next
，rnbe
cur
首尾tp之间的距离为len
tps
，若满足条件b或c，则判定满足近直tp修正点筛选条件；
[0043]
条件b为：(len
vo_last
＜len
tps
)&&(len
vo_cur
＞len
tps
)
[0044]
条件c为：
[0045]
条件b表示车辆首次驶过路口；条件c表示虽然当前时刻车辆没有驶过路口，但是预测下一时刻车辆即将驶过，且下一时刻车辆距离尾tp更为接近；
[0046]
粒子化rnbe
cur
尾tp，最后以rnbe
cur
首tp为参照，应用相似度计算方法求得满足设定条件的粒子，为近直tp修正点；
[0047]
s812，存在多个预行驶基元
[0048]
若车辆满足转向tp修正条件，判定车辆驶入此转向路网基元；
[0049]
若车辆满足近直tp修正条件，保留车辆进入其他路网基元的可能性，直到车辆驶过尾tp超过10米且航向与近直路网基元首部朝向小于10
°
，判定车辆驶入此近直路网基元；
[0050]
s82，sp修正点的筛选
[0051]
记当前车辆所属基元骨架点组为{sp1,sp2,
…
,spn}，记sp与rnbe
cur
首tp的距离为len
sp-tp
，如满足条件d或e，则判定满足sp修正点筛选条件；
[0052]
条件d为：(len
vo_last
＜len
sp-tp
)&&(len
vo_cur
＞len
sp-tp
)
[0053]
条件e为：
[0054]
从骨架点组末尾开始倒序筛选，粒子化满足筛选条件的sp，分别以rnbe
cur
首tp和尾tp为参照，应用相似度计算方法求得满足设定条件的粒子，为sp修正点。
[0055]
进一步，步骤s9中，设定每一时刻的车辆位姿均作为位姿图中的一个节点，两个连续节点之间的局部约束由vo系统提供，不定区间的全局约束由地图修正点提供；构建只有轨迹的位姿图优化问题，并将位姿图优化问题转化为极大似然估计问题；最大似然估计由
车辆在一段时间内的联合概率分布组成，变量是所有节点的全局位姿，tg＝{t
g0
,t
g1
,t
g2
,
…
,t
gl
}，其中t
gl
∈se(3)表示第l帧时刻车辆在地理系下的位姿；
[0056]
在所有测量概率都是独立的情况下，极大似然估计问题为：
[0057][0058]
其中，s是一组测量值，包括vo系统输出和地图修正点测量值，t表示所有节点全局位姿的最优估计，表示t时刻所能观测到的测量值。
[0059]
本发明与现有技术相比，其显著效果如下：
[0060]
1、本发明提供了一种路网地图辅助车辆定位方案，以视觉里程计提供的初始轨迹为基础，从路网地图中引入约束抑制视觉里程计累积误差，能够有效提高车辆的定位精度，并提供长时间的定位保障；
[0061]
2、本发明仅依赖于“点—线”式路网地图，无需更丰富的地图信息，所需地图数据量极小，适用于大范围场景；
[0062]
3、本发明针对定位需求设计了路网地图形式——路网基元图，能够准确描述单个路网基元的形状及各路网基元之间的拓扑关系；
[0063]
4、本发明设计了基于路网基元图的长度/角度联合判断准则，综合考虑视觉里程计位姿与路网基元关键节点之间的长度相似度与角度相似度，能够准确地从路网基元图中筛选地图修正点；
[0064]
5、本发明构建了位姿优化因子图模型，综合考虑视觉因子的短时精确性和地图修正点因子的全局一致性，能够对历史轨迹进行优化，求取最佳历史轨迹；
[0065]
6、本发明设计了优化预测模型，通过引入地图修正点因子的约束来优化视觉里程计原始轨迹，从而得到修正轨迹，再根据视觉里程计推算结果与修正轨迹预测车辆定位结果。
附图说明
[0066]
图1为本发明的总体方案示意图；
[0067]
图2为osm地图示意图；
[0068]
图3为路网地图示意图；
[0069]
图4为路网基元示意图；
[0070]
图5为节点粒子化示意图；
[0071]
图6为节点相似度示意图；
[0072]
图7为道路交汇示意图；
[0073]
图8为路网地图辅助车辆定位系统全局位姿因子图；
[0074]
图9为基于角度准则修正效果图；
[0075]
图10为基于长度准则修正效果图。
具体实施方式
[0076]
下面结合说明书附图和具体实施方式对本发明做进一步详细描述。
[0077]
本技术领域技术人员可以理解的是，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。
[0078]
下面结合附图对本发明的技术方案做进一步的详细说明：
[0079]
如图1所示，本发明的本质是设计一个优化预测模型，在已知车辆初始位置、初始姿态角和车辆所属路网基元的情况下，从路网地图中引入约束抑制视觉里程计累积误差，实时提供稳定且准确的车辆位置估计。详细步骤如下：
[0080]
步骤1，根据车辆初始全局位置加载区域性路网地图；
[0081]
已知车辆初始时刻所处地理位置，根据经纬位置信息从openstreetmap网站下载区域性路网地图。osm可通过人工测量、卫星测量、航拍等免费来源收集数据，这些数据被作为开放的数据库允许全世界用户使用。openstreetmap网站地图如图2所示，网站使用拓扑数据结构表示道路，包含三个核心元素：node、way和relation。其中，node(节点)表示地图上具有地理位置的点，使用经纬度来定义其位置，记为节点node＝(lat,lon)，其中lat表示此节点的纬度坐标，lon表示此节点的经度坐标；way是由一系列node组成的有序列表，表示地图上的一条折线段；relation表示现有node和way之间的关系。选取某道路密度较高区域，所加载的区域性路网地图如图3所示。
[0082]
步骤2，将区域性路网地图转换为路网基元图；
[0083]
为了简化并高效利用路网地图信息，本发明提出一种以道路交汇点为支撑点，以相邻支撑点之间的道路作为路网基元(roadnet basic element，简称为rnbe)的路网模型。进一步的，将路网支撑点定义为拐点(turning point，简称为tp)，任一路网基元的首尾节点均为拐点，拐点包含不同路网基元之间的拓扑连接关系。在路网基元内部，以一系列节点的连线来近似表示道路形状，定义这一系列节点为骨架点(skeleton point，简称sp)。综上，单一路网基元的结构组成表示如下：
[0084]
{tp
start
,sp1,sp2,
…
,spn,tp
end
}
ꢀꢀ
(1)
[0085]
其中，tp
start
表示路网基元首拐点，tp
end
表示路网基元尾拐点，{sp1,sp2,
…
,spn}表示路网基元的骨架点组。若骨架点组为空，则此路网基元的首部朝向和尾部朝向皆为若骨架点组不为空，则此路网基元的首部朝向为尾部朝向为
[0086]
区域内way元素相交点为路网交汇点，将此类相交点提取为拐点并将道路网络划分为多个路网基元，路网基元内部节点为骨架点。如图4所示为本实施例设计的路网模型下某区域道路网络，用12例独立路网基元表示原始道路网络，具有相同拐点端点的路网基元具有连通性，路网基元的内部形状通过骨架点连线近似代替，近直路网基元内部骨架点较少，弧段路网基元内部骨架点较多，最大程度保证骨架点互连折线段与原始道路轨迹的相似度。
[0087]
步骤3，计算不同路网基元之间的连通性；
[0088]
对于单一路网基元，首先提取其首拐点tp
start
和尾拐点tp
end
，遍历路网基元集合内其余路网基元的首尾拐点，若存在某路网基元的首拐点或尾拐点与tp
start
(或tp
end
)位于同一道路交叉口，则判定此两例路网基元是连通的。重复上述过程，遍历路网基元集，计算区域性路网内所有路网基元之间的连通性。
[0089]
步骤4，增加各路网基元内部骨架点数量；
[0090]
步骤2中提到，近直路网基元内部骨架点数量较少，因为近直路段道路形状单一，少量节点连线即可较为准确地还原道路形状。本发明增加各路网基元内部骨架点数量，为轨迹修正提供更多的数据支撑。
[0091]
对于路网基元{tp
start
,sp1,sp2,
…
,spn,tp
end
}，可将其分解为折线段组：
[0092]
{tp
start-sp1，sp
1-sp2，
…
，sp
n-tp
end
}
ꢀꢀ
(2)
[0093]
以折线段tp
start-sp1为例说明骨架点增加方法，具体如下：
[0094]
记tp
start
的坐标为(lat1,lon1)，其中lat1表示tp
start
的纬度坐标，lon1表示tp
start
的经度坐标。记sp1的坐标为(lat2,lon2)，其中lat2表示sp1的纬度坐标，lon2表示sp1的经度坐标。此折线段tp
start-sp1纬度方向距离如式(3)所示，经度方向距离如式(4)所示，其中meter_latitude表示纬度1秒的距离差，一般取30.8702623。
[0095]
len
lat
＝|lat
1-lat2|*3600*meter_latitude
ꢀꢀ
(3)
[0096]
len
lon
＝|lon
1-lon2|*3600*meter_latitude*cos(lat1)
ꢀꢀ
(4)
[0097]
折线距离表达式如下：
[0098][0099]
若len1＜15，判定两节点之间距离较小，不需要增加骨架点；若len1＞15，则需要增加的骨架点数量计算如下：
[0100]
num＝(len1/10+0.5)
floor
ꢀꢀ
(6)
[0101]
其中，()
floor
表示向下取整。
[0102]
增加的骨架点的坐标位置为：
[0103][0104]
其中，i＝1,2,
…
,num。
[0105]
其余折线段骨架点增加方法同折线段tp
start-sp1。
[0106]
步骤5，根据车辆初始姿态角和传感器外参，计算初始转换矩阵
[0107]
视觉里程计输出位姿为当前帧相对于第一帧相机坐标系(世界坐标系)的位姿，记为tw，包括旋转分量rw和平移分量tw。车辆的初始姿态角已知，可由初始航向角ψ、俯仰角θ和横滚角γ求得转换矩阵车辆机体坐标系取常见“右前上”坐标系，世界坐标系取常见“右下前”坐标系，可得旋转矩阵由相机安装位置可得平移矩阵联立得到初始转换矩阵的计算方法如下：
[0108][0109]
其中，()-1
表示矩阵求逆。
[0110]
步骤6，根据车辆初始阶段行驶航向，正向化车辆所属路网基元；
[0111]
已知车辆初始时刻所属路网基元，记为rnbe
init
，可进一步根据步骤4输出结果确定车辆所属折线段，假设为sp
a-spb。车辆在初始阶段处于停滞状态，记初始位置为p
w-init
，经过转化矩阵转化为enu(东北天坐标系)坐标系下的位置p
g-init
。此时，由于视觉里程计误
差的存在，车辆位置在起点附近做不规则运动。本发明自初始时刻起计算初始帧至第m帧车辆累计位移如式(9)所示：
[0112]
lenall(p
w1
,p
wm
)＝len(p
w1
,p
w2
)+len(p
w2
,p
w3
)+
…
+len(p
wm-1
,p
wm
)
ꢀꢀ
(9)
[0113]
其中，len()表示两点直线距离，p
wm
表示第m帧时刻车辆在相机世界坐标系下的位置。
[0114]
记累计位移首次超过2米的车辆位置为p
w-end
，经转化矩阵转化为enu坐标系下的位置p
g-end
。路网基元节点坐标为lla(纬经高坐标系)坐标，lla坐标系与enu坐标系之间的转化是非线性的，经ecef(earth-centered，earth-fixed地心地固坐标系)坐标系过渡将p
g-init
和p
g-end
转换为lla坐标p
l-init
和p
l-end
。分别计算车辆行进方向与所属折线正向的夹角为θ
+
，计算车辆行进方向与所属折线逆向的夹角为θ-：若θ
+
＞θ-，保持rnbe
init
原结构不变；若θ
+
＜θ-，反向调整rnbe
init
原结构，与车辆行驶方向保持一致。
[0115]
步骤7，根据转换矩阵将视觉里程计输出位姿转换到东北天坐标系下；
[0116]
将视觉里程计输出的世界坐标系位姿tw转换为东北天坐标系位姿tg，包括旋转向量rw转rg和平移向量tw转tg。需要特别说明的是，此处的表示从相机世界坐标系到东北天坐标系的转化矩阵，在初始时刻由步骤5求得。
[0117]
步骤8，从路网地图中引入约束，设立判别条件，筛选地图修正点；
[0118]
筛选修正点包括路网基元尾tp修正点筛选和路网基元内部sp修正点筛选，其中tp修正点还将负责车辆当前所属路网基元的更迭，下面对两种修正点的筛选过程进行说明。
[0119]
首先介绍tp修正点和sp修正点通用的粒子化方法和相似度计算方法。对于“点—线”式路网地图，tp通常位于道路交汇中央，sp通常位于道路中线处。由于车辆行驶的不确定性，要想保证车辆位于道路中线行驶是难以实现的，本发明设计粒子化道路网络节点，以覆盖车辆所有可能的位置。
[0120]
如图5所示，中心五角星为道路网络节点，圆点为该节点衍生的粒子。假设节点坐标为(latz,lonz)，其中latz表示此节点的纬度坐标，lonz表示此节点的经度坐标，粒子距离节点的距离dis为一个均值0，标准差为路宽road_width六分之一的高斯分布。由于节点位于道路中央，二分之一的路宽是节点到道路边线的距离，再利用高斯分布的性质：99.73％的数据分布在区间(μ-3σ,μ+3σ)内，此方法可以保证绝大多数粒子在道路区域范围内，且越靠近道路中线的位置有更多的粒子。节点粒子连线与经度正向的角度为随机变量0-2π，假设取θ，则粒子的纬度lat
p
计算如式(10)所示，经度lon
p
计算如式(11)所示：
[0121]
lat
p
＝latz+dis
·
sinθ/meter_latitude/3600
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0122]
lon
p
＝lonz+dis
·
cosθ/meter_latitude/3600/cos(lat
p
)
ꢀꢀꢀꢀꢀꢀ
(11)
[0123]
其中meter_latitude表示纬度1秒的距离差，一般取30.8702623。
[0124]
如图6所示，p1和p2分别是骨架点的两例衍生粒子，t1和t2分别是此段路网基元的首尾拐点。从连线朝向方面考量，p1与vo(visual odometry视觉里程计)输出位置更为相似；从连线长度方面考量，p2与vo输出位置更为相似。
[0125]
以t1为参考点，p1和vo的相对误差如式(12)所示，角度误差如式(13)所示：
[0126][0127][0128]
p1与vo相对于参考点t1的相似度度量根据式(12)、(13)误差定义设置，其计算方式如式(14)所示。
[0129][0130]
其中α表示长度因子，取值为0～1。
[0131]
接下来分别介绍tp修正点和sp修正点的筛选方法。
[0132]
(1)tp修正点的筛选方法
[0133]
如图7所示为路网地图某道路交汇区域，箭头所指为车辆行进方向，记当前所属路网基元为rnbe
cur
。在交汇区域车辆有3例路网基元可选，分别标记为rnbe1、rnbe2和rnbe3。当车辆所属路网基元发生改变后，根据步骤3所计算的连通性获取预行驶路网基元并对其进行正向化。将首部朝向与当前路网基元尾部朝向角度差大于40
°
的预行驶路网基元标记为转向路网基元，如图7中的rnbe1和rnbe3；将角度差小于40
°
的预行驶路网基元标记为近直路网基元，如图7中的rnbe2。
[0134]
本发明设计的tp修正点包括转向tp修正点和近直tp修正点，分别对应于转向路网基元和近直路网基元。设定只有单一预行驶路网基元的情况，则：
[0135]
若预行驶路网基元为转向路网基元，预行驶路网基元的首部朝向与rnbe
cur
的尾部朝向夹角为分别计算车辆当前行驶朝向与rnbe
cur
尾部朝向夹角β1、计算车辆当前行驶朝向与预行驶基元首部朝向夹角β2。若满足式(15)，则判定满足转向tp修正点筛选条件。
[0136][0137]
车辆在完成大部分转向动作后才能满足上述条件，本发明将tp沿转向路网基元首部朝向平移一半路宽距离后粒子化，如图7中rnbe1和rnbe3处黑色圆圈标记。最后以rnbe
cur
首tp为参照，应用相似度计算方法求得相似度最高的粒子，即为转向tp修正点。
[0138]
若预行驶路网基元为近直路网基元，记车辆上一时刻与rnbe
cur
首tp的距离为len
vo_last
，车辆当前时刻与rnbe
cur
首tp的距离为len
vo_cur
，通过假设车辆处于匀速状态推测下一时刻车辆与rnbe
cur
首tp的距离为len
vo_next
，rnbe
cur
首尾tp之间的距离为len
tps
。若上述变量满足式(16)或式(17)，则判定满足近直tp修正点筛选条件。式(16)表示车辆首次驶过路口，式(17)表示虽然当前车辆没有驶过路口，但是预测下一时刻车辆即将驶过，且下一时刻车辆距离尾tp更为接近。
[0139]
(len
vo_last
＜len
tps
)&&(len
vo_cur
＞len
tps
)
ꢀꢀ
(16)
[0140][0141]
车辆行驶至道路交汇处即可满足上述条件，本发明粒子化rnbe
cur
尾tp，如图7中rnbe2处黑色三角形标记。最后以rnbe
init
首tp为参照，应用相似度计算方法求得相似度最高的粒子，即为近直tp修正点。
[0142]
若存在多个预行驶路网基元，一旦车辆满足转向tp修正条件，判定车辆驶入此转向路网基元；若车辆仅满足近直tp修正条件，保留车辆进入其他路网基元的可能性，直到车辆驶过尾tp超过10米且航向与近直路网基元首部朝向小于10
°
，判定车辆驶入此近直路网基元。
[0143]
(2)sp修正点的筛选方法
[0144]
对于sp修正点，本发明采用类似于近直tp的判定方法。len
vo_last
、len
vo_cur
和len
vo_next
与前文所述定义相同，记sp与rnbe
cur
首tp的距离为len
sp-tp
。若上述变量满足式(18)或式(19)，则判定满足sp修正点筛选条件。
[0145]
(len
vo_last
＜len
sp-tp
)&&(len
vo_cur
＞len
sp-tp
)
ꢀꢀ
(18)
[0146][0147]
为了避免近直tp判定带来的影响，本发明从骨架点组末尾开始倒序筛选，粒子化满足筛选条件的sp，分别以rnbe
cur
首tp和尾tp为参照，应用相似度计算方法求得相似度最高的粒子，即为sp修正点。
[0148]
步骤9，构建轨迹的位姿图优化模型，加入局部视觉因子和全局地图修正点因子，通过求解轨迹的位姿图优化问题，修正历史轨迹；
[0149]
本发明设计路网地图辅助车辆定位系统全局姿态图结构如图8所示，每一时刻的车辆位姿均作为位姿图中的一个节点，两个连续节点之间的局部约束由vo系统提供，不定区间的全局约束由地图修正点提供。
[0150]
本发明构建一个只有轨迹的位姿图优化问题，其本质是一个极大似然估计问题，最大似然估计由车辆在一段时间内的联合概率分布组成，变量是所有节点的全局位姿，tg＝{t
g0
,t
g1
,t
g2
,
…
,t
gl
}，其中t
gl
∈se(3)表示第l帧时刻车辆在地理系下的位姿。在假设所有测量概率都是独立的情况下，极大似然估计问题可以导出如式(20)所示，其中s是一组测量值，包括视觉里程计输出和地图修正点测量值，t表示所有节点全局位姿的最优估计：
[0151][0152]
其中，表示t时刻所能观测到的测量值。
[0153]
本发明假设视觉里程计(vo)是短时精确的，此假设适用于绝大多数现有的vo算法。vo输出结果在小范围内是精确的，利用两帧之间的相对位姿，考虑连续帧t-1和帧t。分别用t
ienu
和表示i时刻和j时刻的vo输出转换位姿，两帧之间的变换关系可以表示为
转化为李代数的写法为其中[]
∨
表示从四维矩阵到六维向量的变换，ln()表示以e为底的自然对数。
[0154]
在此基础上，给εi和εj各添加一个左扰动δεi和δεj，如式(21)所示，其中εi和εj分别表示t
ienu
和的李代数形式。
[0155][0156]
其中，表示添加扰动之后的变换项，()
∧
表示从六维向量到四维矩阵的变换，exp()表示自然指数函数，i表示单位矩阵。
[0157]
误差关于t
ienu
和的雅可比矩阵如下式(22)和式(23)所示：
[0158][0159][0160]
其中φe表示ε
ij
的旋转分量，ρe表示ε
ij
的平移分量。的平移分量。r
x
表示位姿t
x
的旋转分量，t
x
表示位姿t
x
的平移分量。
[0161]
步骤8筛选的tp修正点和sp修正点仅包含经度和纬度信息，本发明将满足筛选条件的车辆位置高度信息赋予修正点，求取修正点enu坐标作为地图修正点因子加入位姿图中。
[0162]
全局位姿图构建后，其优化过程相当于搜索最大可能匹配所有边的节点配置。本发明设计使用滑动窗口法，为位姿图保留一个巨大的计算窗口，以获取准确且全局无漂移的位姿估计。转向tp修正点的筛选主要依赖于角度判断准则，具有较高的稳定性，主要对轨迹长度进行修正，如图9所示。本发明设计其计算窗口始于上一例转向tp修正点，在初始阶段以车辆初始位置为第一例转向tp修正点。sp修正点和近直tp修正点的筛选主要依赖于长度判断准则，主要对轨迹方向进行修正，如图10所示。同样的行驶距离，微小的航向偏差便可能导致较大的定位误差，sp修正点和近直tp修正点的计算窗口设定为当前时刻前1000帧，若前1000帧内不足5例转向tp修正点，则扩展至1500帧，不足的情况下以实际已有帧数为窗口。
[0163]
步骤10，根据视觉里程计推算结果和修正轨迹预测最新时刻车辆位姿；
[0164]
假设l时刻车辆位姿满足修正点筛选条件，经位姿图优化后修正轨迹l时刻位姿为t
lenu
。分别将l时刻和l+1时刻相机采集的两帧图像输入视觉里程计，视觉里程计输出两帧间的增量运动估计，定义这个增量运动估计为则可预测l+1时刻车辆位姿：
[0165][0166]
其中，表示l+1时刻车辆位姿。

技术特征：
1.一种基于位姿图优化的路网地图辅助车辆定位方法，其特征在于，在已知车辆的初始位置、初始姿态角和车辆所属路网基元的情况下，从路网地图中引入约束抑制视觉里程计累积误差，结合视觉里程计原始轨迹和地图修正点的影响，输出修正轨迹，最后根据视觉里程计推算结果与修正轨迹预测车辆定位结果，包括步骤如下：s1，根据车辆初始全局位置加载区域性路网地图；s2，以道路交汇点为支撑点，以相邻支撑点之间的道路作为路网基元，将区域性路网地图转化为路网基元图；s3，根据路网基元首尾节点的位置关系，计算不同路网基元之间的连通性；s4，依据各路网基元内部相邻骨架点间距离，判断是否需要增加各路网基元内部骨架点数量；s5，根据车辆初始姿态角和传感器外参，计算初始转换矩阵s6，根据车辆初始阶段行驶航向，正向化车辆所属路网基元；s7，根据将视觉里程计输出位姿转换到东北天坐标系下；s8，从路网地图中引入约束，设立判别条件，筛选地图修正点；s9，构建轨迹的位姿图优化模型，加入局部视觉因子和全局地图修正点因子，通过求解轨迹的位姿图优化问题，修正历史轨迹；s10，根据视觉里程计推算结果和修正轨迹预测最新时刻车辆位姿；s11，接收视觉里程计输出，重复步骤s7至s10，直至车辆停止。2.根据权利要求1所述基于位姿图优化的路网地图辅助车辆定位方法，其特征在于，步骤s2中，将路网支撑点定义为拐点tp，任一路网基元的首尾节点均为拐点，拐点包含不同路网基元之间的拓扑连接关系；在路网基元内部，以一系列节点的连线来近似表示道路形状，定义此系列节点为骨架点sp；则有单一路网基元的结构组成表示如下：{tp
start
,sp1,sp2,
…
,sp
n
,tp
end
}其中，tp
start
表示路网基元首拐点，tp
end
表示路网基元尾拐点，{sp1,sp2,
…
,sp
n
}表示路网基元的骨架点组；若骨架点组为空，则此路网基元的首部朝向和尾部朝向皆为若骨架点组不为空，则此路网基元的首部朝向为尾部朝向为3.根据权利要求2所述基于位姿图优化的路网地图辅助车辆定位方法，其特征在于，步骤s3中，对于单一路网基元，首先提取其首拐点tp
start
和尾拐点tp
end
，遍历路网基元集合内其余路网基元的首尾拐点：若存在某路网基元的首拐点与tp
start
或tp
end
位于同一道路交叉口，则判定此两路网基元是连通的；或者，若存在某路网基元的尾拐点与tp
start
或tp
end
位于同一道路交叉口，则判定此两路网基元是连通的。4.根据权利要求2所述基于位姿图优化的路网地图辅助车辆定位方法，其特征在于，步骤s4中，依据各路网基元内部相邻骨架点间距离，判断是否需要增加各路网基元内部骨架点数量的详细步骤如下：s41，对于路网基元{tp
start
,sp1,sp2,
…
,sp
n
,tp
end
}，将其分解为折线段组{tp
start-sp1，
sp
1-sp2，
…
，sp
n-tp
end
}；s42，分别计算每一折线段的折线距离len
z
，其中z＝1，2，
…
，n+1；若len
z
＜15，判定两节点之间距离较小，不需要增加骨架点；若len
z
＞15，则需要增加骨架点。5.根据权利要求2所述基于位姿图优化的路网地图辅助车辆定位方法，其特征在于，步骤s6中，正向化车辆所属路网基元的实现过程如下：已知车辆初始时刻所属路网基元，记为rnbe
init
；车辆在初始阶段处于停滞状态，记初始位置为p
w-init
，经过转化矩阵转化为enu坐标系下的位置p
g-init
；累计位移首次超过2米的车辆位置记为p
w-end
，经转化矩阵转化为enu坐标系下的位置为p
g-end
；经ecef坐标系过渡，将p
g-init
和p
g-end
转换为lla坐标p
l-init
和p
l-end
；计算车辆行进方向与所属折线正向的夹角为θ
+
，计算车辆行进方向与所属折线逆向的夹角为θ-：若θ
+
＞θ-，保持rnbe
init
原结构不变；若θ
+
＜θ-，反向调整rnbe
init
原结构，与车辆行驶方向保持一致。6.根据权利要求5所述基于位姿图优化的路网地图辅助车辆定位方法，其特征在于，车辆累计位移lenall(p
w1
,p
wm
)的计算表达式如下：lenall(p
w1
,p
wm
)＝len(p
w1
,p
w2
)+len(p
w2
,p
w3
)+
…
+len(p
wm-1
,p
wm
)其中，len()表示两点直线距离，p
wm
表示第m帧时刻车辆在相机世界坐标系下的位置。7.根据权利要求2所述基于位姿图优化的路网地图辅助车辆定位方法，其特征在于，步骤s8中，所述筛选修正点包括路网基元尾tp修正点筛选和路网基元内部sp修正点筛选；s81，所述tp修正点包括转向tp修正点和近直tp修正点，分别对应于转向路网基元和近直路网基元；实现过程如下：s811，设定只有单一预行驶路网基元的情况s8111，若预行驶路网基元为转向路网基元，预行驶基元的首部朝向与rnbe
cur
的尾部朝向夹角为分别计算车辆当前行驶朝向与rnbe
cur
尾部朝向夹角β1、车辆当前行驶朝向与预行驶基元首部朝向夹角β2，若满足条件a，则判定满足转向tp修正点筛选条件；条件a如下：其中，rnbe
cur
为车辆当前所属路网基元；将tp沿转向路网基元首部朝向平移一半路宽距离后粒子化，最后以rnbe
cur
首tp为参照，应用相似度计算方法求得满足设定条件的粒子，为转向tp修正点；s8112，若预行驶基元为近直基元，记车辆上一时刻与rnbe
cur
首tp的距离为len
vo_last
，车辆当前时刻与rnbe
cur
首tp的距离为len
vo_cur
；设车辆处于匀速状态推测下一时刻车辆与rnbe
cur
首tp的距离为len
vo_next
，rnbe
cur
首尾tp之间的距离为len
tps
，若满足条件b或c，则判定满足近直tp修正点筛选条件；条件b为：(len
vo_last
＜len
tps
)&&(len
vo_cur
＞len
tps
)条件c为：
条件b表示车辆首次驶过路口；条件c表示虽然当前时刻车辆没有驶过路口，但是预测下一时刻车辆即将驶过，且下一时刻车辆距离尾tp更为接近；粒子化rnbe
cur
尾tp，最后以rnbe
cur
首tp为参照，应用相似度计算方法求得满足设定条件的粒子，为近直tp修正点；s812，存在多个预行驶基元若车辆满足转向tp修正条件，判定车辆驶入此转向路网基元；若车辆满足近直tp修正条件，保留车辆进入其他路网基元的可能性，直到车辆驶过尾tp超过10米且航向与近直路网基元首部朝向小于10
°
，判定车辆驶入此近直路网基元；s82，sp修正点的筛选记当前车辆所属基元骨架点组为{sp1,sp2,
…
,sp
n
}，记sp与rnbe
cur
首tp的距离为len
sp-tp
，如满足条件d或e，则判定满足sp修正点筛选条件；条件d为：(len
vo_last
＜len
sp-tp
)&&(len
vo_cur
＞len
sp-tp
)条件e为：从骨架点组末尾开始倒序筛选，粒子化满足筛选条件的sp，分别以rnbe
cur
首tp和尾tp为参照，应用相似度计算方法求得满足设定条件的粒子，为sp修正点。8.根据权利要求2所述基于位姿图优化的路网地图辅助车辆定位方法，其特征在于，步骤s9中，设定每一时刻的车辆位姿均作为位姿图中的一个节点，两个连续节点之间的局部约束由vo系统提供，不定区间的全局约束由地图修正点提供；构建只有轨迹的位姿图优化问题，并将位姿图优化问题转化为极大似然估计问题；最大似然估计由车辆在一段时间内的联合概率分布组成，变量是所有节点的全局位姿，t
g
＝{t
g0
,t
g1
,t
g2
,
…
,t
gl
}，其中t
gl
∈se(3)表示第l帧时刻车辆在地理系下的位姿；在所有测量概率都是独立的情况下，极大似然估计问题为：其中，s是一组测量值，包括vo系统输出和地图修正点测量值，t表示所有节点全局位姿的最优估计，表示t时刻所能观测到的测量值。

技术总结
本发明公开了一种基于位姿图优化的路网地图辅助车辆定位方法，包括步骤：S1，根据车辆初始全局位置加载区域性路网地图；S2，将区域性路网地图转化为路网基元图；S3，计算不同路网基元之间的连通性；S4，依据各路网基元内部相邻骨架点间距离，判断是否需要增加各路网基元内部骨架点数量；S5，计算初始转换矩阵T


技术研发人员：许舒晨 孙永荣 王帅帅
受保护的技术使用者：南京航空航天大学
技术研发日：2023.06.15
技术公布日：2023/10/7