一种基于顶视图语义对象的建图方法与流程

1.本发明涉及自动驾驶领域中的自主泊车技术及计算机视觉技术领域，具体涉及一种基于顶视图语义对象的建图方法。


背景技术：

2.在自动驾驶的自主泊车技术应用领域中，需要使用多种传感器来完成周围环境的建图及定位的功能。目前以slam算法为基础的地图构建与自定位技术，按传感器不同可以分为激光slam、双目slam、单目slam，这三种地图构建与自定位技术各有不足：其中，激光slam受到设备成本所限，难以部署到量产车型中；双目slam的计算复杂度较高，难以实时处理；单目slam受到尺度漂移问题的影响，难以获得绝对尺度。
3.为了克服以上缺陷和不足，目前推出了以slam算法为基础的环视视觉融合imu(惯性测量单元)、轮速计的方案，该方案由于采用不同的畸变图片的处理方法、不同的匹配算法、不同的关键帧处理策略、添加不同的优化因素以及不同的语义对象约束，都会影响建图和定位过程中的精度。如何选取和设计更加合理的图像矫正方法、匹配算法、关键帧处理策略、语义对象约束和地图优化成为至关重要的问题。
4.以slam算法为基础的环视视觉融合imu(惯性测量单元)、轮速计的方案，其实现的难点在于：基于环视视觉的方法的建图和激光建图相比，前者受环境影响很大、即便是采取了很多补救措施，也还是难以达到激光建图的效果。激光建图数据处理量较大，进行点云匹配需要计算平台有较高的算力，但是建图精度较高。环视视觉方案中因为图像畸变、图像模糊不清时，匹配难度较大，并且受图像质量影响，建图精度较难提升。因此，基于环视视觉的建图相比激光建图难度和复杂度要大很多。具体如下：
5.第一个难点：图像矫正的难点：建图车辆在进入地下停车场建图过程中，由于必经停车场的上、下坡颠簸地带，在上、下坡和停车位的交接处，由于鱼眼相机摄像头的位置会随着车辆的颠簸而上下移动，导致采用鱼眼相机拍摄的顶视图会发生拼接错误，进一步导致基于顶视图的停车场上下坡及颠簸处附近的停车位建图错误。现有技术专利号为cn111862673a的《基于顶视图的停车场车辆自定位及地图构建方法》采用了建图过程中实时矫正的方法：该方法通过计算单应矩阵得到当前帧与上一帧相对位姿变化，充分考虑了车辆发生颠簸或者上下坡的情况而采取了实时矫正。但是对图像进行矫正过程较为复杂，需要实时计算单应矩阵及鱼眼相机的拼接矩阵，导致车辆发生颠簸时运算量远高于平坦路面；
6.第二个难点：匹配算法的难点：任何一种单一的匹配算法都很难不发生匹配错误。当新建地图车位时，要用当前顶视图中的看到的车位和地图中的已经建立的车位进行匹配，没有匹配上则作为新的车位建立到地图中，这种单一条件的匹配很难做到精确匹配，例如，现有技术对于没有匹配上的车位，进一步计算该车位两个角点的中点之间的距离，若两个角点的中点之间的距离满足要求就认为是新的车位，但是，如果在识别过程中将两个角点的位置识别错位，虽然它们之间的距离满足要求，但是1个角点落在已有车位的2个角点
中间上，另一个角点落在新车位的2个角点中间，虽然新建了车位，但是两个车位不是相邻而是重叠。如果增加识别条件，以当前车位的2个角点 车位号进行匹配，仍然有识别错误的风险，例如，车位号“1”和“7”在识别时容易识别为同一种，在建立“7”号地图车位时，如果该车位的2个角点的位置识别无误，但是车位号识别为“1”，仍然不能判定当前车位是“7”号车位。
7.第三个难点：关键帧处理的难点。一般将重要的“帧”称之为关键帧，“帧数据”是指该帧观测到的“车位信息”和持有该帧的“车辆位姿”这二个基本信息。关键帧处理的难点就在于“车位信息”和持有该帧的“车辆位姿信息”这二个基本信息是动态变化的，所述“车位位置”包括该车位的2个角点和车位号检测框的中点，所述“车位信息”是动态变化的，例如，地图上同一个车位位置，由于车辆是一边行驶一边拍照，不同位置的顶视图关键帧所看到的同一个车位3个点的位置是不同的，怎样根据多个关键帧所记录的同一个车位的不同位置，最终得出一个和实际车位最为贴近的值，其实是难的，因为不仅仅受到当前车辆与地图车位远近的影响，还受到车辆行驶状况(车辆颠簸情况)和光照的影响，同时还受到观测顶视图的尺度因子的影响。
8.第四个难点：地图优化的难点。地图优化是在已经建立的地图车位基础上，对地图中每个车位的位置、关键帧车辆姿态再次进行优化。即使增量建图时找到了优化匹配的方法、实现了精准建图，但仅仅通过一次“优化”是不能实现精准建图的，而必须采用“多次优化”的方法。例如，已经建好的地图，实际车位是相邻车位，但地图上两个相邻车位却没有公共角点；实际车位的两个相邻车位三点共线，但地图上两个相邻车位的三个顶点却形成夹角，造成这些误差的原因，不仅和鱼眼相机的图像矫正有关、匹配算法有关、关键帧数据处理有关，还和还和关键帧本身的位姿有关。一种关键帧的位姿会对应一组观测值，检测车辆轮胎充气很足和充气不足，对相机的高低位置以及关键帧的观测值都会有影响，因此，基于视觉建图的难点在于实现深层次的优化和多维度的优化。


技术实现要素：

9.本发明针对现有技术的不足，提出一种基于顶视图语义对象的建图方法，目的在于解决现有技术基于环视视觉的建图过程中难以解决的图像矫正难、匹配算法难、关键帧处理难、地图优化难的问题。
10.本发明为解决其技术问题，提出以下技术方案
11.一种基于顶视图语义对象的建图方法，其特征在于，包括以下步骤：
12.步骤一、基于检测的增强特征车位信息，进行初始化建图；
13.步骤二、基于km算法匹配结果，进行增量建图；
14.步骤三、基于关键帧观测信息，进行地图优化；
15.所述步骤一的基于检测的增强特征车位信息，进行初始化建图，具体过程如下：
16.1)完成建图以前的初始化步骤；
17.所述建图以前的初始化，既是确定当前车辆的初始位置和预定轨迹；
18.2)基于车端采集的多张鱼眼相机拍摄的图像数据生成顶视图，并完成对顶视图的实时矫正；
19.3)基于实时矫正后的顶视图，得到增强特征车位信息；
20.4)根据增强特征车位信息，并结合惯性测量单元、轮速计信息，完成初始化建图；该初始化建图称为实例地图，该实例地图作为后续增量建图的基础；
21.所述步骤二的基于km算法匹配结果，进行增量建图，具体过程如下：
22.1)重复步骤一的过程2)、过程3)，得到新的实时矫正后的顶视图以及该顶视图中增强特征车位信息；
23.2)用km算法对实例地图的增强特征车位和新的实时矫正后的顶视图的增强特征车位进行匹配；
24.3)新建实例车位：将当前顶视图的增强特征车位信息由顶视图坐标系转换到世界坐标系；若新的顶视图的车位图像特征与初始化实例地图的实例车位匹配过程中，出现没有匹配到的新的车位图像特征，则在实例地图中新建实例车位，并将当前顶视图的增强特征车位信息由顶视图坐标系转换到世界坐标系；
25.所述步骤三的基于关键帧观测信息，进行地图优化，具体过程如下：
26.1)完成实例地图中新增车位和相邻车位之间的角点融合；
27.2)标记相邻车位之间角点共线的相对关系；
28.3)确认有车位的关键帧、以及没有车位的关键帧，剔除关键帧，用以后期优化地图所用；
29.4)利用有车位的关键帧进行局部地图优化，对地图中相邻车位的每个车位位置、以及相邻车位的三点共线、图像尺度因子进行优化；所述每个车位位置包括每个车位的两个车位角点、车位号检测框中点；
30.所述步骤一的过程2)的基于车端采集的多张鱼眼相机拍摄的图像数据生成顶视图，并完成对顶视图的实时矫正，具体过程如下：
31.1)将鱼眼相机坐标转换为顶视图坐标；
[0032][0033]
其中，最右边的u和v代表鱼眼相机上的坐标，通过π-1
的括号变换将鱼眼相机坐标转换为矫正后的图像坐标，再通过[r
p t
p
]的逆变换将矫正后的图像坐标转换到顶视图坐标下，也就是将矫正后的图像中标定范围内的区域保留，[x
p y
p
]代表顶视图下的坐标；
[0034]
2)根据公式(2)对公式(1)不同情况下的r
p
、t
p
求解，得到所对应的外参矩阵；
[0035]
p＝hp....(2)
[0036][0037]
公式(1)和公式(2)的关联关系为：通过将公式(2)矩阵中h进行分解、得到公式(1)的r
p
、t
p
；从而得到不同情况下的[r
p t
p
]，该不同情况下的[r
p t
p
]包括：
[0038]
a、平坦路面下的鱼眼相机外参矩阵；
[0039]
b、车辆在不同俯仰角、侧倾角工况下鱼眼相机外参矩阵；
[0040]
3)基于以上计算结果生成相应的几何查找表；
[0041]
4)对颠簸时的顶视图图像信息进行实时矫正；
[0042]
5)得到每一时刻的实时矫正后多张鱼眼相机图片拼接后的顶视图。
[0043]
所述步骤一过程3)的基于实时矫正后的顶视图，得到增强特征车位信息，具体过程如下：
[0044]
1)获得每一时刻实时矫正后的顶视图；
[0045]
2)基于深度神经网络模型，在该顶视图上进行车位特征检测：包括车位位置信息检测和车位类型分类；所述车位位置信息为车位入口线的两个角点，所述车位类型为：根据车位与道路的相对位置关系分为水平车位、垂直车位或斜车位，具体表示为slot(x1,y1；x2,y2；type)；其中，(x1,y1)和(x2,y2)为顺时针两个角点的顶视图坐标系下的位置信息坐标，type是车位类型；
[0046]
3)基于深度神经网络模型，在该顶视图上进行车位号特征检测：包括车位号检测和车位号识别，从而获得车位号特征的检测框位置信息及车位号识别结果；所述检测框位置信息包含检测框的中点和长、宽，具体表示为number(x,y,w,h,α；num)，其中(x,y)为车位号检测框的中点，(w,h)是车位号检测中的长和宽，α是检测框相对于竖直方向的顺时针的旋转角度值，num代表车位号的识别结果；
[0047]
4)将车位特征和车位号特征进行整合，从而得到该顶视图上增强特征车位信息：根据顶视图坐标系下的车位两个角点坐标与车位号检测框的位置信息，将车位与车位号信息进行关联，从而得到带有车位号信息的增强特征车位，具体表示为(x1,y1；x2,y2；type；x,y,w,h,α；num)。
[0048]
所述步骤一过程4)的根据增强特征车位信息，并结合惯性测量单元、轮速计信息完成初始化建图，具体如下：
[0049]
1)初始化建图：基于增强特征车位信息，结合惯性测量单元和轮速计信息，完成初始化建图，所述初始化建图，即是获取第一次观测到车位的关键帧、用第一次观测到车位的关键帧建立实例地图；此处所述关键帧为第一次观测到车位的帧，该关键帧包含观测到的车位信息和持有该帧的车辆位姿信息。
[0050]
2)实例地图具体是将第一次观测到车位的关键帧中顶视图坐标系下的增强特征车位投影到车辆坐标系下、再将车辆坐标系下的增强特征车位转换到世界坐标系下。
[0051]
所述步骤二过程2)的用km算法对实例地图的实例车位和新的实时矫正后的顶视图的增强特征车位进行匹配，具体为：
[0052]
1)用当前新的实时矫正后的顶视图增强特征车位信息和实例地图的实例车位进行多维度信息匹配，具体为：利用以下5点信息进行匹配：所述新的实时矫正后的顶视图简称为新的顶视图，新的顶视图包含增强特征车位信息；
[0053]
a.利用车位位置信息进行匹配，得到fd,fd代表当前基于新的顶视图检测到的车位位置信息和实例地图的车位位置信息重合程度；fd也称为位置误差代价；
[0054]
b.利用车位类别信息进行匹配，得到f
t
,f
t
代表当前基于新的顶视图检测到的车位类别和实例地图的车位类别是否相同，f
t
也称为车位类别代价；
[0055]
c.利用车位号相似度进行匹配，得到fb,fb代表当前基于新的顶视图检测到的车位号和实例地图的车位号是否相似；fb也称为车位号相似代价；
[0056]
d.利用车位号检测框的重叠度进行匹配，得到fn，fn代表当前基于新的顶视图检测到的车位号检测框和实例地图的车位号检测框重叠程度；fn也称为检查框重叠代价；
[0057]
e.利用车位相对位置信息进行匹配，得到fr，fr代表当前基于顶视图检测到的车位
相对位置信息和实例地图的车位相对位置信息的相似程度；fr也称为相对位置代价；
[0058]
所述的相邻车位指的是真实世界空间下，两个车位之间存在共用角点的情况，按顺时针方向，每个车位都可能存在上相邻车位和下相邻车位，相邻车位的具体判定公式如下所示：
[0059]
||p
a-pb||＜δs....(4)
[0060]
其中，pa代表a车位的某一角点位置，即pa
＝
[xa，ya]，pb代表b车位的某一角点位置，即pb＝[xb，yb]，δs代表两个车位相邻角点距离阈值；
[0061]
2)通过km算法进行最优匹配计算：综合以上5种信息，得出实例地图的增强特征车位和新的顶视图的增强特征车位之间进行匹配的总的关联代价函数，具体公式如下：(km(kuhn and munkres)算法，用于二部图的最优匹配)
[0062]
f＝ωdfd ω
tft
ωbfb ω
nfn
ω
rfr
....(5)
[0063]
其中，f为总代价函数，ωd、ω
t
、ωb、ωn、ωr分别为以上五种因素的权重系数；根据总代价函数f的公式，可以求出当前新的顶视图中所有潜在匹配关联的增强特征车位与地图实例的增强特征车位之间的代价，从而构造出相应的关联矩阵，最后带入km算法进行最优匹配的计算；所述潜在匹配关联既是：空间距离相近或者车位号相似则认为是潜在匹配关联；
[0064]
3)将当前新的顶视图的增强特征车位投影到世界坐标系下。
[0065]
所述fd、f
t
、fb、fn、fr的计算公式如下：
[0066][0067]
其中，xa、ya是实例地图车位两个角点的中点位置信息，xb、yb是新的顶视图中车位两角点的中点投影到世界坐标系下的位置信息；
[0068][0069]
其中，typea是实例地图车位的车位类型，typeb是新的顶视图中的车位类型；
[0070][0071]
其中，a是实例地图车位的车位号字符串，b是新的顶视图中车位的车位号字符串，表示异或，i是车位号字符串的索引；
[0072][0073]
其中，a是实例地图中车位号检测框面积，b是新的顶视图中车位号检测框投影到世界坐标系下的面积；
[0074]fr
＝ω
nlfnl
ω
nnfnn
ω
atfat
....(10)
[0075][0076][0077]
[0078]
其中，ω
nl
、ω
nn
、ω
at
分别为对应的权重系数，f
nl
是该车位上相邻车位的实例地图车位与观测特征车位的车位号相似程度，f
nn
是该车位下相邻车位的实例地图车位与观测特征车位的车位号相似程度；f
at
表示该车位在滑动窗口内的车位空间分布类型与处于地图中的实例车位空间分布类型是否相同，type
ar
是实例地图车位的车位空间分布类型，type
br
是滑动窗口内的车位空间分布类型。
[0079]
所述步骤三过程1)的完成实例地图中新增车位和相邻车位之间的角点融合，具体过程如下：
[0080]
对相邻车位的公用角点进行融合：用新的顶视图中对相邻车位相对关系的观测结果，对实例地图车位之间的共用角点进行调整：若新的顶视图中相邻两个车位在滑动窗口内的多次观测中存在共用一个角点的情况，则在实例地图中，对相应的相邻两个车位存在误差的两个角点进行融合，使该两个车位都拥有该角点，以便后期优化过程中仅对一个角点位置信息进行优化；
[0081]
所述步骤三的过程2)的标记相邻车位之间角点共线的相对关系，根据新的顶视图中的车位之间的相对关系，标记实例地图中车位之间角点共线的相对关系，具体过程如下：
[0082]
所述新的顶视图中相邻地图车位，包括该车位在实例地图中已经匹配上和没有匹配上的车位，没有匹配上的车位既是地图中待建立的新的车位；所述相对关系指的是相近车位之间的角点是否共线；所述相邻车位指的是在顶视图中的两个车位存在共用的角点，按顺时针方向，每个车位都可能存在有上相邻车位和下相邻车位，具体定义如权利要求5的1)中的e步骤；
[0083]
相邻车位角点共线的具体判定公式如下所示：
[0084][0085]
其中，x
a1
、y
a1
和x
a2
、y
a2
是车位a的两个角点位置信息，x
b1
、y
b1
和x
b2
、y
b2
是车位b的两个角点位置信息，δω代表两个相邻车位各自两个角点连线的夹角阈值，若相邻车位各自两个角点连线的夹角绝对值小于夹角阈值，则认为两个车位的角点共线，把两个车位的角点标记为共线。
[0086]
所述步骤三过程3)的确认有车位的关键帧和没有车位的关键帧、剔除关键帧，用以后期优化地图所用，具体过程如下：
[0087]
1)确认关键帧
[0088]
a.根据是否有新建车位确定关键帧
[0089]
判断当前新的顶视图所对应的帧是否在实例地图中新建车位，若新建车位，则确认当前新的顶视图所对应的帧为关键帧、并加以保存；
[0090]
b.根据距离确定关键帧
[0091]
如果当前新顶视图所对应的帧没有观测到车位或者所观测到的车位不是实例地图中新建的车位而是历史车位，则在当前帧与上一关键帧的距离或者航向角差值大于一定阈值时，确认关键帧，若该关键帧没有观测到车位信息则关键帧信息不包括图像数据仅包括当前车辆的位姿信息，该当前车辆的位姿即是当前车辆拍摄图像时的位姿；
[0092]
根据距离插入关键帧的公式如下所示：
[0093]
‖p
k 1-pk‖＞δp....(15)
[0094]
其中，pk代表k时刻的车辆中心位置，即pk＝[xk，yk]，p
k 1
代表k 1时刻的车辆中心位置，即p
k 1
＝[x
k 1
，y
k 1
]，δp代表设定的车辆中心距离阈值，若k时刻与k 1时刻的车辆中心距离大于距离阈值，则确认新的关键帧；
[0095]
根据航向角差值插入关键帧的公式如下所示：
[0096]
‖θ
k 1-θk‖＞δθ....(16)
[0097]
其中，θk代表k时刻的车辆航向角，θ
k 1
代表k 1时刻的车辆航向角，δθ代表设定的航向角阈值，若k时刻与k 1时刻的航向角之差的绝对值大于航向角阈值，则确认新的关键帧；
[0098]
2)剔除关键帧:
[0099]
a.计算当前新的顶视图所对应的帧对该同一个实例车位的观测分数：实例地图的每个车位记录了多个关键帧对同一个车位的观测结果：记录了其中最远观测和最近观测：根据以下公式记录最近观测和最远观测的观测数据，并计算每一关键帧对该实例车位的观测分数：
[0100]
l
min
＝min(l
min
,l)....(17)
[0101]
l
max
＝max(l
max
,l)....(18)
[0102][0103]
其中，l
min
为最近观测，即该顶视图中的车位角点或车位号检测框中点到顶视图中心的最小距离；l
max
为最远观测，即该顶视图中的车位角点或车位号检测框中点到顶视图中心的最大距离，l为当前被计算分数的关键帧的顶视图中的车位角点或车位号检测框中点到顶视图中心的距离，g是当前关键帧观测权重中的观测分数；
[0104]
b.根据观测分数的计算结果，赋予不同的观测权重；
[0105]
c.若某关键帧的观测权重低于设定阈值，则该关键帧被剔除；所述观测权重的计算是基于特征车位的角点距图像中心的距离、车位号检测框中点距图像中心的距离，同时也受到观测时车辆的行驶状况、观测时光照情况的影；
[0106]
所述观测权重的计算主要分为两个部分：对顶视图车位角点观测、对顶视图车位号检测框的中点观测；以顶视图中心到图像特征车位角点的最短距离为角点最近观测，以顶视图中心到图像特征车位号检测框的中点最短距离为车位号最近观测，关键帧的观测权重的具体计算公式如下：
[0107]
f＝wbw
l
(∑(wcgc) ∑(w
ngn
))....(20)
[0108]
其中，f代表该关键帧的观测权重，wb、w
l
代表车辆颠簸、光照影响的权重系数，gc、gn代表角点、车位号观测分数，wc、wn代表角点、车位号是否为最近观测，为最近观测置为10，不为最近观测为1；
[0109]
所述步骤三过程4)的利用有车位的关键帧进行局部地图优化，对地图中相邻车位的每个车位位置、以及相邻车位的三点共线、图像尺度因子进行优化；所述相邻车位的每个车位位置包括每个车位的车位角点、车位号检测框中点；
[0110]
具体表达式如下:
[0111][0112]
以上公式(21)是求实例地图第i个车位坐标信息与第i个车位在第j关键帧观测信息转换到世界坐标系车位坐标信息的差值,所述差值是世界坐标系下的差值,具体为：公式(21)等号右边第1项为实例地图中第i个车位的世界坐标信息，该值的初值为步骤三的过程3)被保留的多个关键帧对同一个车位的观测结果中观测分数最高的观测信息转换到世界坐标系下的坐标信息；公式(21)等号右边第2项的大括号内的数据为将第j个关键帧观测到车辆坐标系下的第i个车位转换为世界坐标系下的坐标信息，用公式(21)等号右边第1项减去第2项，就能求出实例地图第i个车位与第i个车位在第j关键帧观测信息转换到世界坐标系车位坐标信息的差值。
[0113]
具体如下：
[0114]
a.等号右边的第1项代表第i个实例地图车位中的两个角点或者车位号检测框中点在世界坐标系的坐标，l的取值范围是{a，b，c}，分别代表车位的两个角点及车位号检测框中点，w代表在世界坐标系；
[0115]
b.等号右边第2项大括号里面的sf代表图像尺度因子，由公式(25)求解得到sf值；
[0116]
c.等号右边第2项大括号里面的第1项、第3项矩阵代表车辆的旋转和平移的变化，其中，第1项为旋转的变化，第3项为平移的变化，矩阵[x
j y
j θj]是滑动窗口内第j关键帧车辆在世界坐标系中的位姿，所述滑动窗口既是指车辆在移动状态下连续数帧关键帧观测信息与关键帧位姿信息构成的组合；
[0117]
d.等号右边大括号里面的第2项矩阵代表第i个实例地图车位中的两个角点或者车位号检测框中点在滑动窗口内第j关键帧的车辆坐标系的坐标，l的取值范围是{a,b,c}，分别代表车位的两个角点及车位号检测框中点；代表车辆在不同位姿状态下观测到不同车位的坐标信息；
[0118][0119][0120][0121]
以上以上公式(23)、公式(24)分别代表相邻车位各自2个角点构成的向量，公式(22)表示公式(23)和公式(24)两个向量之间的叉乘；若向量叉乘结果趋近于零，则三点趋于共线；其中，i和i-1代表第i和车位和第i-1个车位；
[0122]
分别代表第i个实例车位的两个角点坐标，对于第i-1个车位，同样分别代表第i-1个实例车位的两个角点坐标；
[0123][0124]
以上公式(25)是总的优化函数，等号右边第一部分是对公式(21)不同关键帧对当前车位观测到的位置误差的平方再求和，然后再乘以λ
ijl
，λ
ijl
用于分配权重，λ
ijl
是对角矩阵，仅在对角线处存在元素，该矩阵的元素为对应公式(21)中使用的观测信息的观测权重；等号右边第二部分是对公式(22)不同关键帧对当前车位观测到三点共线误差的平方再求和、然后再乘以λk，λk仅对角线存在元素，且对角线元素均为1，对每一项三点共线误差平方进行权重分配。
[0125]
所述步骤三还包括过程5)：利用有车位信息的关键帧，进行回环检测，利用有车位信息的关键帧和没有车位的关键帧，对地图中关键帧的位姿进行优化，对地图中的车位角点、车位号检测框中点再次进行优化：所述回环检测包括：对滑动窗口内的车位与地图中车位号相似或者距离相近的车位进行搜索，使用km算法对滑动窗口内的车位与地图中的实例车位进行匹配关联，若存在一定数量的匹配，则认为发生回环，具体过程如下：
[0126]
1)对关键帧的位姿进行优化，对关键帧位姿优化的具体公式如下所示：
[0127][0128]
其中，t
ij
代表第i和第j帧关键帧之间的相对运动，ti和tj分别代表第i和第j帧关键帧的位姿；为ti的逆矩阵，相乘等于单位矩阵；
[0129][0130]
其中，e
ij
代表第i帧与第j帧关键帧之间的位姿变换构成的误差；为t
ij
的逆矩阵；ln表示求对数操作；∨符号代表由李群形式变换为李代数形式；
[0131][0132]
其中，f为总的代价函数，代表位姿变换构成误差的平方和；e
ij
代表第i帧与第j帧关键帧之间的位姿变换构成的误差，为e
ij
对角转换矩阵；
[0133][0134][0135]
以上公式中，i、j均代表关键帧序号，ti和tj分别代表第i和第j帧关键帧的位姿；[x
j y
j θj]是滑动窗口内第j关键帧车辆在世界坐标系中的位姿，通过形式变换，从而构造成tj的形式；
[0136]
2)返回步骤三的过程4)，重新进行实例地图车位位置信息优化，其中，尺度因子不
作变化。
[0137]
本发明的优点效果
[0138]
1、本发明基于车辆在发生颠簸时的顶视图出现较大误差的情况下，通过使用预先计算的鱼眼相机外参矩阵对拼接后的顶视图进行矫正。在保证精度的前提下，加快了计算速度。
[0139]
2、本发明在建图过程中使用km(kuhn-munkras)算法完成当前帧的特征和地图的实例的匹配。相比于icp(迭代最邻近)算法，km算法可以基于更多维度的信息，如：车位类别、车位号检测框重叠度、车位号相似程度、车位的相对位置信息等。在初值较差的情况下，依旧可以得到更鲁棒的结果。
[0140]
3、本发明提出了一种关键帧的确认、剔除策略。当一帧拼接好的顶视图输入，对其观测的特征车位进行是否新建地图实例车位、是否更新观测的判断，并对其计算权重；若需要新建或者更新，当前帧权重则增加，之前新建或观测该特征车位的关键帧的权重则减少。若关键帧的权重低于阈值则进行关键帧移除。新建地图实例车位发生在局部地图实例车位中没有与当前帧特征车位匹配的情况下，更新观测发生在当前帧的特征车位与匹配的地图实例车位的欧式距离较之前的观测距离更小。在提升精度的同时，合理控制计算量的规模。
[0141]
4、本发明使用了实例车位之间的位置约束，根据多次的观测情况来调整实例车位之间的相对关系，提高了建图的精度。
[0142]
5、本发明在构建建图优化函数时，将由于车辆的相机高度变化、胎压发生改变及轮速计出现误差时引起尺度变化问题考虑在内，从而将尺度因素加入到优化函数中，以完成更高精度的建图。
附图说明
[0143]
图1为本发明方法概要框图；
[0144]
图1-1为本发明图1步骤一的具体过程；
[0145]
图1-2为本发明图1步骤二的具体过程；
[0146]
图1-3为本发明图1步骤三的具体过程；
[0147]
图2-1为本发明根据新增车位确认关键帧示意图一；
[0148]
图2-2为本发明根据新增车位确认关键帧示意图二；
[0149]
图2-3为本发明根据距离确认关键帧示意图；
[0150]
图2-4为本发明根据航向角确认关键帧示意图；
[0151]
图2-5为本发明一个车位对应多个关键帧示意图；
[0152]
图3-1为本发明车位号检测框的重叠度匹配示意图；
[0153]
图3-2为本发明车位之间相对位置信息匹配示意图；
[0154]
图4为本发明对关键帧进行位姿优化示意图；
[0155]
图5-1为本发明神经网络学习以前不包括车位位置信息的顶视图；
[0156]
图5-2为本发明神经网络学习以后包括车位位置信息的顶视图。
具体实施方式
[0157]
本发明设计原理：
[0158]
下面结合附图对本发明作出进一步解释：
[0159]
1、动态关键帧设计原理
[0160]
第一、概述：关键帧就是最重要的帧。动态设计关键帧的目的就是找到对应同一个车位的多个关键帧中最优秀的关键帧，找到最优秀的关键帧的目的是以“它”所“看到”的车位位姿为基准，对地图中那些与实际车位位姿不相符合的歪歪扭扭的车位进行优化，优化后达到与实际车位位姿最为接近的程度。
[0161]
第二、一个车位对应多个关键帧的原理。如图2-5所示，检测车辆从发现一个车位、到离开这个车位的观测视线，是一个从远到近、再从近到远的过程。这个过程将会有多个关键帧“看到”同一个车位，因此，地图中一个车位保留了多个关键帧。
[0162]
第三、设计难点：难点在于，同一个关键帧，当车辆在不同位置时，它的分数是随着车的位置而变化的，如果只是每个关键帧计算1次分数，那就永远不会得到分值最高的关键帧。关键帧得分是根据它与目标点的距离、以及最大值、最小值决定的，所述最大值就是“当前所有关键帧”与目标点距离最大的值，所述最小值就是“当前所有关键帧”与目标点距离最小的值，以图2-5的2号关键帧为例：当检测车在2号关键帧位置时，“当前所有关键帧”只包括1号和2号，最大值就是它本身与目标点的距离，最小值就是1号关键帧与目标点的距离，因此，2号关键帧相对于1号关键帧分值是高的(假设为0.8分)，但是当车辆到达3号关键帧位置时，3号关键帧的分值应该比2号的分值要高，因为它距离目标点的距离更接近，但此时2号关键帧的分值可能会和3号关键帧的分值相同，这样就不能得出3号关键帧的分值高于2号的结论。因此，对于2号关键帧的分数不能只计算一次。因为第一次计算2号关键帧时，其分值是相对于1号关键帧计算而来的，但在车辆到达3号关键帧位置时，最小值发生了变化，因此要重新计算2号关键帧的分数，此时最小值应该是3号关键帧到目标点的距离，因此，重新计算2号关键帧的分数时，分数会下降。同理，当车辆到达7号关键帧位置时，重新计算2号关键帧的分数，其分值会更低。
[0163]
第四、本发明的解决方案：如图2-5所示，除了计算当前关键帧的分数以外，还要对已经计算过分数的关键帧重新计算。设置一个重新计算关键帧的回环计算区间，这个回环计算区间可以是10帧数据的区间或5帧数据的区间。例如图2-5的2号关键帧，其回环计算区间为5帧数据，这就意味着计算3、4、5、6、7号关键帧的自身分数时，还要重新计算2号关键帧的分数。当车辆从2号关键帧位置到达7号关键帧位置时，2号关键帧的分数随着每次的计算会越来越低，同理，3、4、5、6关键帧也采用同样的回环计算方法，才能在7号关键帧位置时得出7号关键帧分数最高的结论。
[0164]
第五、剔除关键帧。找到最优秀关键帧的目的是为了剔除分数最低的关键帧，但还不能用所谓最优秀的关键帧进行定位，因为仅仅凭着一帧数据哪怕是非常好的数据也不能定位，所以，剔除最差的关键帧后还要保留比较好的和最好的多个关键帧数据。
[0165]
2、地图优化设计原理
[0166]
1)地图优化的必要性。地图的每个车位都保留了多个“看到”这个车位的关键帧，这些关键帧都是质量比较好的没有被剔除的关键帧。虽然地图里面通过增量建图已经建立好很多车位，但是这些车位的位姿是不理想的，可能是歪歪扭扭，和实际车位相差较大，比如，实际相邻车位是存在公共角点的并且三点共线，但是在地图里面相邻车位是分开的没有公共角点，而且三点的两线之间存在角度。此时就需要通过地图中每个车位保留的多个
比较好的关键帧对该车位的物理位姿进行优化。
[0167]
2)地图优化的方法。地图优化分为三个步骤，第一、相邻车位的角点融合，将分离的两个角点融合在一起成为一个公共角点，但融合后相邻两个车位只是有了公共角点，但三个角点的位置还没有优化，第二、优化三个角点的位置：其一、如公式(21)所示，用最优秀的分值最高的关键帧所“看到的”车位的位姿作为基础(x,y,角度)，然后用它分别与其它关键帧的数据作比较，分别求出一组误差值；其二，如公式(25)等号右边第一部分所示，当采用公式(21)分别求出一组误差值后，再通过平方和、然后再乘以λ
ijl
，(λ
ijl
用于分配权重，λ
ijl
是对角矩阵以及分配权重的方法)，求出最优值，最后以这个最优值作为优化后的与实际车位最为贴近的当前车位的物理位置。第三、三点共线的优化。虽然相邻车位三个角点的位置被优化变得准确很多了，但最终判定三个点的位置是否准确还要看三点是否共线，如果不共线、存在角度，就说明三个角点的位置还不够准确，但此时采用另外一种方法进行角点共线的优化，优化后不但实现了三点共线，而且进一步优化了三个角点的位置。三点共线优化分为二个步骤，先分别求出多个关键帧三点共线的误差，如公式(22)、(23)、(24)所示，再通过公式(25)等号右边第二部分，再将这些误差通过平方和、然后再乘以λk，(λk仅对角线存在元素，且对角线元素均为1，对每一项三点共线误差平方进行权重分配的方法，)最终实现三点共线的优化。
[0168]
3、多维度匹配设计原理
[0169]
第一、设计难点。所述增量建图就是用顶视图当前看到的车位和地图中的实例地图进行匹配，没有匹配上则认为是新的车位。难点在于：单凭任何一种单一的信息都很难判断是新增车位、还是已有的车位、还是既不是新增也不是已有的第三种待定的车位：1)受到多种因素影响，“新车位”的位置往往不在旧车位以外或者相邻，而是其2个角点一个落在了地图中已有的车位上，另一个落在该车位以外，各占50％，因此不能仅仅凭着2个角点的位置判定是否是新车位；2)如果仅仅凭着车位上的车位号也不可以判断，因为车位号“1”和“7”，“3”和“8”往往识别错误；3)即使顶视图新车位的2个角点满足新建车位的条件(相邻角点或公共角点)，但是顶视图车位姿态和实例地图车位姿态相差90度，不能确定新车位的真实姿态确实相差90度，还是误判造成的，在这种情况下，增量建图还是遇到问题。
[0170]
第二、解决方案：如公式(6)-公式(14)所示，1)采用5种信息综合判断。除了采用车位位置、车位号、车位姿态(车位类型)三种信息判断以外，还采用第四种信息：相邻车位信息进行判断：当车位号“1”和“7”，“3”和“8”混淆时，除了根据角点的位置，还可以根据当前车位上下左右相邻车位的车位号来判断，“7”号车位的左右应该是“6”和“8”，而“1”号车位的相邻车位应该是“3”，以此解决“1”和“7”，“3”和“8”识别错误的问题。还采用第五种信息：车位号检测框的重叠度，如果当前顶视图是新车位，那么该车位看到的车位号检测框(紧紧贴着车位号设计的方框，为车位号检测框)一定不会和已有车位重合，应该是在相邻车位的设定位置上，当新车位的2个角点的一个落在已有的实例地图车位上时，也可通过新车位的车位号检测框落在什么位置来进一步判断，因为检测框的面积相比2个角点的面积更小、更精确，如果该检测框落在新车位的位置而不在实例地图已有车位的位置，那么尽管一个角点位置落在了实例地图已有车位上，也可以判定该车位一定是新车位，从而进行增量建图。2)给以上5种分配权重系数，权重是不固定的，可以根据情况动态调整5个权重系数，不在此赘述。
[0171]
基于以上发明原理，本发明设计了一种基于顶视图语义对象的建图方法。
[0172]
一种基于顶视图语义对象的建图方法如图1、图1-1、图1-2、图1-3所示，其特点是，包括以下步骤：
[0173]
步骤一、基于检测的增强特征车位信息，进行初始化建图；
[0174]
步骤二、基于km算法匹配结果，进行增量建图；
[0175]
步骤三、基于关键帧观测信息，进行地图优化；
[0176]
所述步骤一的基于检测的增强特征车位信息，进行初始化建图，具体过程如下：
[0177]
1)完成建图以前的初始化步骤；
[0178]
所述建图以前的初始化，既是确定当前车辆的初始位置和预定轨迹；
[0179]
2)基于车端采集的多张鱼眼相机拍摄的图像数据生成顶视图，并完成对顶视图的实时矫正；
[0180]
补充说明：
[0181]
图5-1的顶视图和5-2是不同的。图5-1的顶视图是最初的顶视图，最初的顶视图由多路鱼眼相机在一定范围内的俯视图图像拼接形成，其效果如图5-1所示。默认该顶试图的物理空间范围为10米*10米，图像分辨率为720*720，每个像素空间尺度为13.88毫米，即尺度因子scale为13.88。此时顶视图保留有图片信息，但并没有检测出的车位角点坐标、检测框中点坐标及车位号识别结果等信息。最初的顶视图只包括图像中心坐标、顶视图车辆后轴中心坐标。
[0182]
3)基于实时矫正后的顶视图，得到增强特征车位信息；
[0183]
4)根据增强特征车位信息，并结合惯性测量单元、轮速计信息，完成初始化建图；该初始化建图称为实例地图，该实例地图作为后续增量建图的基础；
[0184]
所述步骤二的基于km算法匹配结果，进行增量建图，具体过程如下：
[0185]
1)重复步骤一的过程2)、过程3)，得到新的实时矫正后的顶视图以及该顶视图中增强特征车位信息；
[0186]
2)用km算法对实例地图的增强特征车位和新的实时矫正后的顶视图的增强特征车位进行匹配；
[0187]
3)新建实例车位：将当前顶视图的增强特征车位信息由顶视图坐标系转换到世界坐标系；若新的顶视图的车位图像特征与初始化实例地图的实例车位匹配过程中，出现没有匹配到的新的车位图像特征，则在实例地图中新建实例车位，并将当前顶视图的增强特征车位信息由顶视图坐标系转换到世界坐标系；
[0188]
所述步骤三的基于关键帧观测信息，进行地图优化，具体过程如下：
[0189]
1)完成实例地图中新增车位和相邻车位之间的角点融合；
[0190]
2)标记相邻车位之间角点共线的相对关系；
[0191]
3)确认有车位的关键帧、以及没有车位的关键帧，剔除关键帧，用以后期优化地图所用；
[0192]
4)利用有车位的关键帧进行局部地图优化，对地图中相邻车位的每个车位位置、以及相邻车位的三点共线、图像尺度因子进行优化；所述每个车位位置包括每个车位的两个车位角点、车位号检测框中点；
[0193]
所述步骤一的过程2)的基于车端采集的多张鱼眼相机拍摄的图像数据生成顶视
图，并完成对顶视图的实时矫正，具体过程如下：
[0194]
1)将鱼眼相机坐标转换为顶视图坐标；
[0195][0196]
其中，最右边的u和v代表鱼眼相机上的坐标，通过π-1
的括号变换将鱼眼相机坐标转换为矫正后的图像坐标，再通过[r
p t
p
]的逆变换将矫正后的图像坐标转换到顶视图坐标下，也就是将矫正后的图像中标定范围内的区域保留，[x
p y
p
]代表顶视图下的坐标；
[0197]
2)根据公式(2)对公式(1)不同情况下的r
p
、t
p
求解，得到所对应的外参矩阵；
[0198]
p＝hp....(2)
[0199][0200]
公式(1)和公式(2)的关联关系为：通过将公式(2)矩阵中h进行分解、得到公式(1)的r
p
、t
p
；从而得到不同情况下的[r
p t
p
]，该不同情况下的[r
p t
p
]包括：
[0201]
a、平坦路面下的鱼眼相机外参矩阵；
[0202]
b、车辆在不同俯仰角、侧倾角工况下鱼眼相机外参矩阵；
[0203]
3)基于以上计算结果生成相应的几何查找表；
[0204]
4)对颠簸时的顶视图图像信息进行实时矫正；
[0205]
5)得到每一时刻的实时矫正后多张鱼眼相机图片拼接后的顶视图。
[0206]
所述步骤一过程3)的基于实时矫正后的顶视图，得到增强特征车位信息，具体过程如下：
[0207]
1)获得每一时刻实时矫正后的顶视图；
[0208]
2)基于深度神经网络模型，在该顶视图上进行车位特征检测：包括车位位置信息检测和车位类型分类；所述车位位置信息为车位入口线的两个角点，所述车位类型为：根据车位与道路的相对位置关系分为水平车位、垂直车位或斜车位，具体表示为slot(x1,y1；x2,y2；type)；其中，(x1,y1)和(x2,y2)为顺时针两个角点的顶视图坐标系下的位置信息坐标，type是车位类型；
[0209]
3)基于深度神经网络模型，在该顶视图上进行车位号特征检测：包括车位号检测和车位号识别，从而获得车位号特征的检测框位置信息及车位号识别结果；所述检测框位置信息包含检测框的中点和长、宽，具体表示为number(x,y,w,h,α；num)，其中(x,y)为车位号检测框的中点，(w,h)是车位号检测中的长和宽，α是检测框相对于竖直方向的顺时针的旋转角度值，num代表车位号的识别结果；
[0210]
补充说明：
[0211]
如图5-2，这里面的顶视图为经过神经网络学习后的顶视图，和图5-1的顶视图相比，顶视图中增加了车位位置信息，这些都是深度神经网络学习后识别出来的车位位置信息。图5-2为顶视图中检测车位角点和车位号检测框和识别结果的示意图，图中圆形小点代表车位角点坐标，相邻车位之间比较接近，角点可能出现互相遮挡。图中三角形小点代表检测框中点坐标。图中菱形小点代表图像中心坐标。图中六边形小点代表顶视图中车辆后轴中心坐标。虚线指向的部分是对检测框内容的识别结果。经过深度神经网络模型检测之后，
顶视图的信息不仅包括图像中心坐标、顶视图车辆后轴中心坐标，还包括车位角点、车位号检测框中点在顶视图中坐标及车位号识别结果等信息。
[0212]
同样，神经网络还可以识别出顶视图车位类型信息，车位类型主要包括水平车位、垂直车位和斜车位三种类型，图5-2只是展示了深度神经网络学习后识别出来的车位位置，而不包括车位类型。
[0213]
4)将车位特征和车位号特征进行整合，从而得到该顶视图上增强特征车位信息：根据顶视图坐标系下的车位两个角点坐标与车位号检测框的位置信息，将车位与车位号信息进行关联，从而得到带有车位号信息的增强特征车位，具体表示为(x1,y1；x2,y2；type；x,y,w,h,α；num)。
[0214]
所述步骤一过程4)的根据增强特征车位信息，并结合惯性测量单元、轮速计信息完成初始化建图，具体如下：
[0215]
1)初始化建图：基于增强特征车位信息，结合惯性测量单元和轮速计信息，完成初始化建图，所述初始化建图，即是获取第一次观测到车位的关键帧、用第一次观测到车位的关键帧建立实例地图；此处所述关键帧为第一次观测到车位的帧，该关键帧包含观测到的车位信息和持有该帧的车辆位姿信息。
[0216]
2)实例地图具体是将第一次观测到车位的关键帧中顶视图坐标系下的增强特征车位投影到车辆坐标系下、再将车辆坐标系下的增强特征车位转换到世界坐标系下。
[0217]
补充说明：
[0218]
专利多处内容涉及由顶视图坐标转换到世界坐标系下(即世界坐标系)，包括步骤一过程4)、步骤二过程2)、步骤二过程3)。具体地，在顶视图坐标系中，以顶视图左上角为原点，x轴指向车辆后方，y轴指向车辆右方，z轴朝上。车辆坐标系以车辆后轴中心为原点，x轴指向车辆前方，y轴指向车辆左方，z轴朝上。两者之间的变换关系如下：
[0219][0220]
以上公式中，scale是顶视图坐标系与车辆坐标系之间的尺度因子，描述图片像素与真实世界长度之间的比值；x
p
、y
p
是顶视图坐标系中的点的位置，xb、yb是车辆坐标系中的点的位置，δx为顶视图中心点和顶视图中车辆后轴中心的距离差的绝对值。公式等号右边第一项可以将第二项表示的顶视图坐标系下的点的坐标信息转换到以车辆中心为原点的车辆坐标系下，再加上代表顶视图中心点到车辆后轴中心的距离乘以尺度因子的第三项，就可以得到以车辆后轴中心为原点的车辆坐标系下的坐标。
[0221]
进一步地，再将车辆坐标系下的增强特征车位转换到世界坐标系下，两者之间的转换关系如下所示：
[0222][0223]
以上公式中，xb、yb是车辆坐标系中的点的位置，xw、yw是世界坐标系下的坐标，x、y、θ是世界坐标系下中的车辆位姿。公式等号右边第一项代表由车辆位姿的航向角计算得到的旋转矩阵，第二项代表车辆坐标系下点的位置信息，通过第一项的旋转，再加上第三项，进行平移，最后就可以得到世界坐标系下的坐标。
[0224]
对顶视图的增强特征车位进行以上两步的坐标变换，可以将顶视图中的增强特征车位的两个角点、检测框中点(x1,y1；x2,y2；x,y)转换到世界坐标系下，从而将首次观测到的增强特征车位建入实例地图，完成初始建图。增强特征车位的检测框的长宽信息(w,h)仅进行尺度变换，即乘以公式中的尺度因子，则通过顶视图像素和尺度因子可以得到全局坐标中的长宽信息。增强特征车位的检测框的角度信息α加上车辆航向角θ得到全局坐标中的角度信息。增强特征车位的车位类别、车位号识别结果作为非空间位置信息则不进行变换。
[0225]
所述步骤二过程2)的用km算法对实例地图的实例车位和新的实时矫正后的顶视图的增强特征车位进行匹配，具体为：
[0226]
1)用当前新的实时矫正后的顶视图增强特征车位信息和实例地图的实例车位进行多维度信息匹配，具体为：利用以下5点信息进行匹配：所述新的实时矫正后的顶视图简称为新的顶视图，新的顶视图包含增强特征车位信息；
[0227]
a.利用车位位置信息进行匹配，得到fd，fd代表当前基于新的顶视图检测到的车位位置信息和实例地图的车位位置信息重合程度；fd也称为位置误差代价；
[0228]
b.利用车位类别信息进行匹配，得到f
t
,f
t
代表当前基于新的顶视图检测到的车位类别和实例地图的车位类别是否相同，f
t
也称为车位类别代价；
[0229]
c.利用车位号相似度进行匹配，得到fb,fb代表当前基于新的顶视图检测到的车位号和实例地图的车位号的相似度；fb也称为车位号相似代价；
[0230]
d.利用车位号检测框的重叠度进行匹配，得到fn，fn代表当前基于新的顶视图检测到的车位号检测框和实例地图的车位号检测框重叠程度；fn也称为检查框重叠代价；
[0231]
e.利用车位相对位置信息进行匹配，得到fr，fr代表当前基于顶视图检测到的车位相对位置信息和实例地图的车位相对位置信息的相似程度；fr也称为相对位置代价；
[0232]
所述的相邻车位指的是真实世界空间下，两个车位之间存在共用角点的情况，按顺时针方向，每个车位都可能存在上相邻车位和下相邻车位，相邻车位的具体判定公式如下所示：
[0233]
||p
a-pb||＜δs....(4)
[0234]
其中，pa代表a车位的某一角点位置，即pa＝[xa，ya]，pb代表b车位的某一角点位置，即pb＝[xb，yb]，δs代表两个车位相邻角点距离阈值；
[0235]
2)通过km算法进行最优匹配计算：综合以上5种信息，得出实例地图的增强特征车位和新的顶视图的增强特征车位之间进行匹配的总的关联代价函数，具体公式如下：(km(kuhn and munkres)算法，用于二部图的最优匹配)
[0236]
f＝ωdfd ω
tft
ωbfb ω
nfn
ω
rfr
....(5)
[0237]
其中，f为总代价函数，ωd、ω
t
、ωb、ωn、ωr分别为以上五种因素的权重系数；根据总代价函数f的公式，可以求出当前新的顶视图中所有潜在匹配关联的增强特征车位与地图实例的增强特征车位之间的代价，从而构造出相应的关联矩阵，最后带入km算法进行最优匹配的计算；所述潜在匹配关联既是：空间距离相近或者车位号相似则认为是潜在匹配关联；
[0238]
3)将当前新的顶视图的增强特征车位投影到世界坐标系下。
[0239]
所述fd、f
t
、fb、fn、fr的计算公式如下：
[0240][0241]
其中，xa、ya是实例地图车位两个角点的中点位置信息，xb、yb是新的顶视图中车位两角点的中点投影到世界坐标系下的位置信息；
[0242]
补充说明：
[0243]
例如车位n123的实例车位的两个角点坐标为[2，2]、[2，4.5]，xa＝(2 2)/2＝2，ya＝(2 4.5)/2＝3.25；车位n123在新的顶视图中转换到世界坐标系下的两个角点坐标为[1.8，2.1]、[1.8，4.6]，xb＝(1.8 1.8)/2＝1.8，yb＝(2.1 4.6)/2＝3.35；fd＝0.22；
[0244][0245]
其中，typea是实例地图车位的车位类型，typeb是新的顶视图中的车位类型；
[0246][0247]
其中，a是实例地图车位的车位号字符串，b是新的顶视图中车位的车位号字符串，表示异或，i是车位号字符串的索引；
[0248]
补充说明：
[0249]
例如实例地图车位的车位号字符串为n124，顶视图中识别结果为n123，其中n＝n，1＝1,2＝2，3≠4，对以上结果累加，则fn的结果则为3。即逐位比较实例地图中车位号和顶视图车位号识别结果的每一位的字母或者数字是否相同。
[0250][0251]
其中，a是实例地图中车位号检测框面积，b是新的顶视图中车位号检测框投影到世界坐标系下的面积；
[0252]
补充说明：
[0253]
如图3-1所示，a是实例地图中车位号检测框面积，b是新的顶视图中车位号检测框投影到世界坐标系下的面积，假设两个面积相等。图左边为a与b完全没有重叠面积，a∩b的意思是求两个检测框的重叠部分的面积，a∪b的意思是求两个检测框面积之和，重复部分只统计一次，由此代入公式(9)，a∩b＝0，a∪b＝2a＝2b，fb等于0；图中间为a与b有一半的重叠面积，因此a∩b等于检测框面积的一半，a∪b的意思是求两个检测框面积之和，重复部分只统计一次，由此代入公式(9)，a∩b＝a/2＝b/2，a∪b＝3a/2＝3b/2，fb等于1/3；图右边为a与b完全重叠，因此a∩b等于检测框面积，a∪b的意思是求两个检测框面积之和，重复部分只统计一次，由此代入公式(9)，a∩b＝a＝b，a∪b＝a＝b，fb等于1。
[0254]fr
＝ω
nlfnl
ω
nnfnn
ω
atfat
....(10)
[0255][0256][0257][0258]
其中，ω
nl
、ω
nn
、ω
at
分别为对应的权重系数，f
nl
是该车位上相邻车位的实例地图
车位与观测特征车位的车位号相似程度，f
nn
是该车位下相邻车位的实例地图车位与观测特征车位的车位号相似程度；f
at
表示该车位在滑动窗口内的车位空间分布类型与处于地图中的实例车位空间分布类型是否相同，type
ar
是实例地图车位的车位空间分布类型，type
br
是滑动窗口内的车位空间分布类型。
[0259]
补充说明：
[0260]
公式(10)由公式(11)、(12)、(13)构成，并按照一定权重分配系数。其中公式(11)、(12)分别代表该车位上、下相邻车位的实例车位与观测特征车位的车位号相似程度，具体计算参考公式(8)的补充说明。公式(13)的车位空间分布类型指的是车位是否处于路口交汇处，如图3-2所示，虚线框中是滑动窗口内的实例车位，实线框中是地图的实例车位，车位空间分布类型主要可以分为路口车位、非路口车位，其中n124、n125、n128、n111、n112、n115为路口车位，n123、n126、n127、n110、n113、n114为非路口车位。所述滑动窗口由最近连续几帧关键帧的观测车位的位置信息和关键帧位姿信息构成。
[0261]
所述步骤三过程1)的完成实例地图中新增车位和相邻车位之间的角点融合，具体过程如下：
[0262]
对相邻车位的公用角点进行融合：用新的顶视图中对相邻车位相对关系的观测结果，对实例地图车位之间的共用角点进行调整：若新的顶视图中相邻两个车位在滑动窗口内的多次观测中存在共用一个角点的情况，则在实例地图中，对相应的相邻两个车位存在误差的两个角点进行融合，使该两个车位都拥有该角点，以便后期优化过程中仅对一个角点位置信息进行优化；所述步骤三的过程2)的标记相邻车位之间角点共线的相对关系，
[0263]
根据新的顶视图中的车位之间的相对关系，标记实例地图中车位之间角点共线的相对关系，具体过程如下：
[0264]
所述新的顶视图中相邻地图车位，包括该车位在实例地图中已经匹配上和没有匹配上的车位，没有匹配上的车位既是地图中待建立的新的车位；所述相对关系指的是相近车位之间的角点是否共线；所述相邻车位指的是在顶视图中的两个车位存在共用的角点，按顺时针方向，每个车位都可能存在有上相邻车位和下相邻车位，具体定义如权利要求5的1)中的e步骤；
[0265]
相邻车位角点共线的具体判定公式如下所示：
[0266][0267]
其中，x
a1
、y
a1
和x
a2
、y
a2
是车位a的两个角点位置信息，x
b1
、y
b1
和x
b2
、y
b2
是车位b的两个角点位置信息，δω代表两个相邻车位各自两个角点连线的夹角阈值，若相邻车位各自两个角点连线的夹角绝对值小于夹角阈值，则认为两个车位的角点共线，把两个车位的角点标记为共线。
[0268]
所述步骤三过程3)的确认有车位的关键帧和没有车位的关键帧、剔除关键帧，用以后期优化地图所用，具体过程如下：
[0269]
1)确认关键帧
[0270]
a.根据是否有新建车位确定关键帧
[0271]
判断当前新的顶视图所对应的帧是否在实例地图中新建车位，若新建车位，则确认当前新的顶视图所对应的帧为关键帧、并加以保存；
[0272]
补充说明：
[0273]
如图2-1所示，假设车辆的行驶方向从左到右，当车辆“看到”图1有n123、n124两个新增车位时，因为“看到”有新增车位，所以，当前顶视图的帧被确认为关键帧；如图2-2所示，当车辆继续由左向右继续前行，除了看到原有的n123、n124车位，还看到新的n125车位，因此，确认当前帧为关键帧。
[0274]
b.根据距离确定关键帧
[0275]
如果当前新顶视图所对应的帧没有观测到车位或者所观测到的车位不是实例地图中新建的车位而是历史车位，则在当前帧与上一关键帧的距离或者航向角差值大于一定阈值时，确认关键帧，若该关键帧没有观测到车位信息则关键帧信息不包括图像数据仅包括当前车辆的位姿信息，该当前车辆的位姿即是当前车辆拍摄图像时的位姿；
[0276]
该根据距离插入关键帧的公式如下所示：
[0277]
‖p
k 1-pk‖＞δp....(15)
[0278]
其中，pk代表k时刻的车辆中心位置，即pk＝[xk，yk]，p
k 1
代表k 1时刻的车辆中心位置，即p
k 1
＝[x
k 1
，y
k 1
]，δp代表设定的车辆中心距离阈值，若k时刻与k 1时刻的车辆中心距离大于距离阈值，则确认新的关键帧；
[0279]
补充说明：
[0280]
如图2-3所示，当车辆继续前行，n125车位前方一定距离内没有新“看到”的车位了、并且从车辆越过图3的n125车位开始继续向前行驶的距离超过了一定阈值，通过公式(15)被确认为关键帧；但此时的关键帧只有“帧的位姿信息”而没有“车位位置”信息。没有“车位位置”信息的关键帧之所以被确认，是因为后续步骤三的过程5)是首先优化关键帧的位置，而不是优化车位。如果两帧之间距离过远，导致优化关键帧的位姿效果不佳，为了保持优化效果，按照距离确认关键帧，只要距离到了，就确认为关键帧。
[0281]
根据航向角差值插入关键帧的公式如下所示：
[0282]
‖θ
k 1-θk‖＞δθ....(16)
[0283]
其中，θk代表k时刻的车辆航向角，θ
k 1
代表k 1时刻的车辆航向角，δθ代表设定的航向角阈值，若k时刻与k 1时刻的航向角之差的绝对值大于航向角阈值，则确认新的关键帧；
[0284]
补充说明：
[0285]
如图2-4所示，车辆在不同时刻的航向角发生了变化，这个航向角的变化会影响到对于车位位置的观测，因此，这个时刻“帧”的位姿是重要的，标记为关键帧。航向角差值超过阈值的关键帧之所以被确认，是因为后续步骤三的过程5)是首先优化关键帧的车辆姿态，而不是优化车位。如果两帧之间航向角差值超过阈值，导致优化关键帧的姿态效果不佳，为了保持优化效果，按照航向角差值确认关键帧，只要当前帧与上一关键帧的航向角差值超过阈值，当前帧就确认为关键帧。
[0286]
2)剔除关键帧:
[0287]
a.计算当前新的顶视图所对应的帧对该同一个实例车位的观测分数：实例地图的每个车位记录了多个关键帧对同一个车位的观测结果：记录了其中最远观测和最近观测：根据以下公式记录最近观测和最远观测的观测数据，并计算每一关键帧对该实例车位的观测分数：
[0288]
l
min
＝min(l
min
,l)....(17)
[0289]
l
max
＝max(l
max
,l)....(18)
[0290][0291]
其中，l
min
为最近观测，即该顶视图中的车位角点或车位号检测框中点到顶视图中心的最小距离；l
max
为最远观测，即该顶视图中的车位角点或车位号检测框中点到顶视图中心的最大距离，l为当前关键帧的顶视图中的车位角点或车位号检测框中点到顶视图中心的距离，g是当前关键帧观测权重中的观测分数；
[0292]
补充说明：
[0293]
1)公式(17)、(18)、(19)的应用条件是当前帧为第2帧以上，如果只有第1帧还没有第2帧，则最小值l
min
、最大值l
max
、分数g默认分别为最大值(10、10、10)，默认为最大值的原因在于：当第一帧观测n123车位时，没有其它帧的比较，所以认为是最大值。例如车辆从距离车位n123较远的地方发现了该车位，虽然距离较远，但因为只有这个距离，所以默认是最大值。但是随着车辆的前行，会距离n123车位越来越近，距离越近则分值越高，所以第一帧的分数就会变化，在达到最短距离之前，第一帧的分数会越来越低。
[0294]
2)关于公式(19)的最小值l
min
、最大值l
max
：最小值l
min
、最大值l
max
是所有看到n123车位的多个关键帧的车位a类角点a、a
’……
的最小值和最大值、b类角点b、b
’……
的最小值和最大值、c类检测框中心点c、c
’……
的最小值和最大值(假如看到n123车位的关键帧有2个)；例如：假设n123被第一帧观测到时，该车位的两个角点、检测框中点的观测数值为{2.8，4.9，4.4}，n123第二帧被观测到时，该车位的两个角点、检测框中点的观测数值为{3.2，2，3.25}，计算最小值l
min
时，要用第一帧和第二帧的同类角点a、a’、b、b’进行对比、同类检测框中点c、c’进行对比，相对于a、a’的最小值是将3.2和2.8相比，为2.8，相对于b、b’的最小值,是4.9和2相比，最小值为2，相对于c、c’的最小值,是4.4和3.25相比为3.25；同理，相对于a、a’的最大值是将2.8和4.9相比，为4.9，相对于b、b’的最大值,是4.9和2相比，最大值为4.9，相对于c、c’的最小值,是4.4和3.25相比，最大值为4.4；如果看到n123车位的关键帧是6个，则a类角点最小值是6个a类角点的最小值，而不是上一帧和下一帧相比的最小值。
[0295]
3)公式(17)、(18)、(19)是针对每个角点、检测框中心点设计的，如果只有二帧数据，那会有3个最小值、3个最大值、3个l值，将它们分别代入公式(19)后，就会得到3个分数值；
[0296]
4)关于l：l为当前被计算分数的关键帧的顶视图中的车位角点或车位号检测框中点到顶视图中心的距离，意思是：如果当前被计算的关键帧分数是上一帧的分数，则l就是上一帧顶视图中的车位角点或车位号检测框中点到顶视图中心的距离，如果当前被计算的关键帧分数是最新发生的关键帧的分数，则l就是最新发生的顶视图中的车位角点或车位号检测框中点到顶视图中心的距离。
[0297]
5)重新计算上一个关键帧的分数的意义：用最新的关键帧的分数和重新计算以后的上一个关键帧的分数对比，假设检测车辆从a点发现n123车位到c点截止，经过了a、b、c三个点，其中，在b点车辆距离n123车位最近，虽然如此，但是车辆在到达b点时并不会停止，而是继续向前走，一直到达c点，当车辆到达c点时，只有用c点的关键帧分数和b点的关键帧分
数相比，才知道b点是距离最近的关键帧，这就需要在c点计算完关键帧分数后，还要重新计算上一帧b点的关键帧分数，虽然b点的关键帧分数已经计算过，但因为公式(19)最小值和最大值在车辆到达c点时发生了变化，所以重新计算b点的分数时，公式(19)里面的最小值、最大值、l值都发生了变化，所以重新计算b点关键帧分数时会和第一次计算的值不同，而如果不是重新计算b点关键帧分数，则有可能b点关键帧分数要低于c点关键帧分数，而事实上b点距离车位最近分数最高。原因在于：第一次计算b点关键帧分数时，车辆位于b点，只是用b点和a点相比确定最小值、最大值、l值，因此当时b点的分值可能会低于车辆到达c点时的分值，当车辆到达c点时，用新的最小值、最大值、l值重新计算b点的分值，因为c点和b点相比，b点的距离最短，所以用新的最小值、最大值、l值重新计算b点的分值时，b点关键帧分数高于c点关键帧分数。
[0298]
b.根据观测分数的计算结果，赋予不同的观测权重；
[0299]
c.若某关键帧的观测权重低于设定阈值，则该关键帧被剔除；所述观测权重的计算是基于特征车位的角点距图像中心的距离、车位号检测框中点距图像中心的距离，同时也受到观测时车辆的行驶状况、观测时光照情况的影；
[0300]
所述观测权重的计算主要分为两个部分：对顶视图车位角点观测、对顶视图车位号检测框的中点观测；以顶视图中心到图像特征车位角点的最短距离为角点最近观测，以顶视图中心到图像特征车位号检测框的中点最短距离为车位号最近观测，关键帧的观测权重的具体计算公式如下：
[0301]
f＝wbw
l
(∑(wcgc) ∑(w
ngn
))....(20)
[0302]
其中，f代表该关键帧的观测权重，wb、w
l
代表车辆颠簸、光照影响的权重系数，gc、gn代表角点、车位号观测分数，wc、wn代表角点、车位号是否为最近观测，为最近观测置为10，不为最近观测为1；
[0303]
当有其他关键帧被确认之后，按照以上的步骤计算关键帧对车位观测分数，更新关键帧的观测权重，当观测权重小于阈值则将该关键帧剔除。
[0304]
所述步骤三过程4)的利用有车位的关键帧进行局部地图优化，对地图中相邻车位的每个车位位置、以及相邻车位的三点共线、图像尺度因子进行优化；所述相邻车位的每个车位位置包括每个车位的车位角点、车位号检测框中点；
[0305]
具体表达式如下:
[0306][0307]
以上公式(21)是求实例地图第i个车位坐标信息与第i个车位在第j关键帧观测信息转换到世界坐标系车位坐标信息的差值,所述差值是世界坐标系下的差值,具体为：公式(21)等号右边第1项为实例地图中第i个车位的世界坐标信息，该值的初值为步骤八被保留的多个关键帧对同一个车位的观测结果中观测分数最高的观测信息转换到世界坐标系下的坐标信息；公式(21)等号右边第2项的大括号内的数据为将第j个关键帧观测到车辆坐标系下的第i个车位转换为世界坐标系下的坐标信息，用公式(21)等号右边第1项减去第2项，就能求出实例地图第i个车位与第i个车位在第j关键帧观测信息转换到世界坐标系车位坐
标信息的差值，具体如下：
[0308]
a.等号右边的第1项代表第i个实例地图车位中的两个角点或者车位号检测框中点在世界坐标系的坐标，l的取值范围是{a，b，c}，分别代表车位的两个角点及车位号检测框中点，w代表在世界坐标系；
[0309]
b.等号右边第2项大括号里面的sf代表图像尺度因子，由公式(25)求解得到sf值；
[0310]
c.等号右边第2项大括号里面的第1项、第3项矩阵代表车辆的旋转和平移的变化，其中，第1项为旋转的变化，第3项为平移的变化，矩阵[x
j y
j θj]是滑动窗口内第j关键帧车辆在世界坐标系中的位姿，所述滑动窗口既是指车辆在移动状态下连续数帧关键帧观测信息与关键帧位姿信息构成的组合；
[0311]
d.等号右边大括号里面的第2项矩阵代表第i个实例地图车位中的两个角点或者车位号检测框中点在滑动窗口内第j关键帧的车辆坐标系的坐标，l的取值范围是{a,b,c}，分别代表车位的两个角点及车位号检测框中点；代表车辆在不同位姿状态下观测到不同车位的坐标信息；
[0312][0313][0314][0315]
以上以上公式(23)、公式(24)分别代表相邻车位各自2个角点构成的向量，公式(22)表示公式(23)和公式(24)两个向量之间的叉乘；若向量叉乘结果趋近于零，则三点趋于共线；其中，i和i-1代表第i和车位和第i-1个车位；
[0316]
分别代表第i个实例车位的两个角点坐标，对于第i-1个车位，同样分别代表第i-1个实例车位的两个角点坐标；
[0317][0318]
以上公式(25)是总的优化函数，等号右边第一部分是对公式(21)不同关键帧对当前车位观测到的位置误差的平方再求和，然后再乘以λ
ijl
，λ
ijl
用于分配权重，λ
ijl
是对角矩阵，仅在对角线处存在元素，该矩阵的元素为对应公式(21)中使用的观测信息的观测权重；等号右边第二部分是对公式(22)不同关键帧对当前车位观测到三点共线误差的平方再求和、然后再乘以λk，λk仅对角线存在元素，且对角线元素均为1，对每一项三点共线误差平方进行权重分配。
[0319]
补充说明：
[0320]
公式(21)等号右边大括号的第2列放在第1列旋转矩阵的后面是想表达：该第2列
矩阵[x
j y
j θj]
t
的滑动窗口内第j关键帧车辆在世界坐标系中的位姿是在第1列旋转变化后“看到的”位姿，大括号的第3列的平移矩阵放在第2列的后面是想表达：将旋转后滑动窗口内第j关键帧车辆在世界坐标系中的位姿再进行平移。
[0321]
所述步骤三还包括过程5)：利用有车位信息的关键帧，进行回环检测，利用有车位信息的关键帧和没有车位的关键帧，对地图中关键帧的位姿进行优化，对地图中的车位角点、车位号检测框中点再次进行优化：所述回环检测包括：对滑动窗口内的车位与地图中车位号相似或者距离相近的车位进行搜索，使用km算法对滑动窗口内的车位与地图中的实例车位进行匹配关联，若存在一定数量的匹配，则认为发生回环，具体过程如下：
[0322]
补充说明：
[0323]
步骤三过程5)的应用前提是车辆行驶通过停车场的某一个区域a和某一个区域b，这两个区域的物理位置很相近，能够形成一个闭环。区域a和区域b的物理位置近，其包含的观测到的车位的物理位置很相近，才能将两个关键帧进行融合，融合后两个关键帧的位姿相同，达到误差为零，达到误差为零后，再从第一个车位的关键帧倒推回去逐个减少每个关键帧的误差，直到最后一个关键帧误差为0，所以，步骤三过程5)的设计前提是能够进行关键帧的位姿优化，位姿优化以后才能重复步骤三的过程4)。如果地下停车场的车位的物理位置不能形成闭环，而是一条直线的两个端点，一头一尾的关键帧就不能融合，也就没有位姿优化的前提条件，此时，重复步骤三的过程4)也就没有意义。
[0324]
1)对关键帧的位姿进行优化，对关键帧位姿优化的具体公式如下所示：
[0325][0326]
以上公式(26)中，t
ij
代表第i和第j帧关键帧之间的相对运动，ti和tj分别代表第i和第j帧关键帧的位姿；为ti的逆矩阵，两者相乘等于单位矩阵；
[0327][0328]
以上公式(27)中，e
ij
代表第i帧与第j帧关键帧之间的位姿变换构成的误差；为t
ij
的逆矩阵；ln表示对括号内的结果进行求对数操作；∨符号代表由李群形式变换为李代数形式；
[0329][0330]
以上公式(28)中，f为总的代价函数，代表位姿变换构成误差的平方和；e
ij
代表第i帧与第j帧关键帧之间的位姿变换构成的误差，为e
ij
对角转换矩阵；
[0331][0332][0333]
以上公式中，i、j均代表关键帧序号，ti和tj分别代表第i和第j帧关键帧的位姿；
[x
j y
j θj]是滑动窗口内第j关键帧车辆在世界坐标系中的位姿，通过形式变换，从而构造成tj的形式；
[0334]
补充说明：
[0335]
如图4所示，车辆在行驶过程中分别保留了9个关键帧的位姿，分别为t1、t2...t9。对应公式(26)，对应公式(26)，则可以通过t1求逆得到，tj＝t2，通过则可以求出t
ij
＝t
12
。在车辆行驶过程中，通过公式(26)可以得到t
12
、t
23
...t
89
。此时，可以通过t9直接推算出其与t1之间的相对位姿变换情况，这是因为t1可以推算出与t2的位姿变换情况，t2可以推算出与t3的位姿变换情况，综合就可以得到t1与t3的位姿变换情况，由此一步步则可以得到t1到t9之间的位姿变换情况。如图中虚线所示，t
91
既可以从多个位姿之间的相对关系递推得到，同时，t
91
也可以通过t1与t9都观察相同的车位n123直接求出相对位姿变换，此时需要考虑更相信哪一个位姿变换更加准确。相对来说，前者因为涉及到多个位姿之间的递推，累积的误差比较大，所以需要通过使用后者对前者调整，并由此将构成t
91
的t
12
、t
23
...t
89
均进行调整，从而构成公式(27)。公式(27)中代表由两个位姿和相对位姿构成的误差函数，因为是t
ij
的逆矩阵，由公式(26)可知由互为逆矩阵的乘积为单位矩阵，所以所以ln内部取对数之后应该趋于零。再对t1、t2...t9之间每两个位姿之间的位姿变换t
12
、t
23
...t
89
、t
91
构成的误差函数平方求和，则得到公式(28)中的总代价f。公式(29)、(30)则表示相邻两个关键帧位姿。
[0336]
2)返回步骤三的过程4)，重新进行实例地图车位位置信息优化，其中，尺度因子不作变化。
[0337]
以上所述并非是对本发明的限制，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明实质范围的前提下，还可以做出若干变化、改型、添加或替换，这些改进和润饰也应视为本发明的保护范围。