一种基于环视视觉的全自动泊车定位与建图方法

本发明涉及车辆自动驾驶领域，尤其涉及一种全自动泊车定位与建图方法。

背景技术：

1、随着城市的快速发展和汽车保有量逐年增加，车位短缺以及复杂的泊车环境已经成为了驾驶者的一大困扰，浪费了大量的时间和精力。此外，由于停车操作需要同时兼顾车辆后方以及两侧的情况，且空间紧凑，过程中又无人指导，因此对驾驶员驾驶操作的熟练程度要求很高。即便是操作熟练的驾驶员也会被繁琐的泊车过程和找不到合适的车位等问题所困扰。基于以上需求，智能泊车技术应运而生，这项技术不仅可以解决驾驶员的烦恼，而且对于停车场空间的高效利用、泊车过程的安全性都有较大益处。

2、室内停车场自主泊车定位技术面临的主要问题包括两方面，一是受限于室内环境与gnss信号常被遮挡无法使用的问题；另一方面则是由于传感器成本较高而导致自主泊车技术普及较为困难的问题。虽然能够通过搭载如激光雷达等传感器增加精确环境感知的能力，但势必会带来成本的增加。在自动驾驶领域不断压低成本，提升产业竞争力的背景下，如何实现在有限成本下优化泊车效果，成为当前亟待解决的问题。

技术实现思路

1、本发明针对有限成本下泊车定位与建图优化问题，提供了一种基于环视视觉的泊车定位与建图方法，以期实现在不改变传感器配置的情况下，通过优化定位与建图来提高建图的精度，从而能保障车辆在地下停车场内安全行驶，并有助于提高泊车效率与准确度。

2、本发明为达到上述发明目的，采用如下技术方案：

3、本发明一种基于环视视觉的全自动泊车定位与建图方法的特点在于，包括以下步骤：

4、步骤1：车辆在移动过程中，通过安装在汽车前、后、左、右的鱼眼相机获取当前t时刻的环视图像后，经过相机畸变去除、逆透视变换与图像拼接的操作，得到一张t时刻的俯视图像it；

5、步骤2：采用卷积神经网络的deeplab v3+模型提取所述俯视图像it中的车道线以及障碍物信息并进行标注，得到带有车道线以及障碍物信息的图像i't；

6、步骤3：对提取的车道线以及障碍物信息进行点云转化；

7、步骤3.1、将i't中的车道线以及障碍物信息所对应的像素点转化为点云pt；

8、步骤3.2、根据t时刻的车辆imu传感器对点云pt进行水平校准，得到水平校准后的点云p′t；

9、步骤3.3、对水平校准后的点云p't进行半径滤波处理，得到精确点云p″t；

10、步骤4：融合imu传感器和里程计的数据，对t+1时刻的车辆位姿进行估算并生成全局停车场地图；

11、步骤4.1、利用式(1)计算t时刻imu估计的车辆加速度和车辆角速度

12、

13、式(1)中，i表示imu传感器坐标系，表示t时刻imu传感器输出的车辆加速度，表示t时刻车辆加速度的高斯白噪声，表示t时刻车辆加速度的随机游走噪声，表示t时刻imu传感器输出的车辆角速度；表示t时刻车辆角速度的高斯白噪声，表示t时刻车辆角速度的随机游走噪声；

14、步骤4.2、利用式(2)计算t+1时刻imu估计的车辆速度和车辆角度

15、

16、式(2)中，δt表示t时刻和t+1时刻之间的差值，δv表示速度的变化量，δθ表示角度的变化量；和分别表示t时刻imu估计的车辆速度和车辆角度；当t＝0时，令和均为零；

17、步骤4.3、利用式(3)计算t+1时刻里程计估计的车辆速度和车辆角度

18、

19、式(3)中，o表示里程计坐标系，vr,t+1和vl,t+1分别表示t+1时刻里程计输出的右轮速度和左轮速度，sr,t+1和sl,t+1分别表示δt内右轮行驶里程和左轮行驶里程，len是车身宽度，是t时刻里程计估计的车辆角度；当t＝0时，令为零；

20、步骤4.3、利用式(4)估算t+1时刻融合后的车辆速度vt+1和车辆角度θt+1：

21、

22、式(4)中，μ1表示imu传感器对应的权重，μ2表示里程计对应的权重；当车辆直行时，设置μ1＜μ2；当车辆转弯时，设置μ1＞μ2；

23、步骤4.4、以车辆的初始位置为坐标原点，车辆朝向为y轴正方向，车辆朝向沿车辆所在平面顺时针旋转90°为x轴正方向，建立二维地图坐标系；

24、定义车辆在t时刻的位姿wt＝(xt,yt,vt)，其中，xt,yt为车辆在t时刻的二维坐标，vt为在t时刻车辆朝向与x轴正方向的夹角；当t＝0时，令w0＝(0,0,90°)；

25、步骤4.5、利用式(5)计算车辆在t+1时刻的位姿wt+1＝(xt+1,yt+1,vt+1)：

26、

27、式(5)中，xt+1和yt+1表示车辆在t+1时刻的二维坐标，vt+1表示车辆在t+1时刻的车辆朝向与x轴正方向的夹角；

28、步骤4.6、在t时刻的位姿wt上添加精确点云信息p″t，以构成t时刻的局部点云地图mp,t；根据t+1时刻的位姿wt+1，使用正态分布变换算法计算t+1时刻的精确点云信息p″t+1与mp,t之间的旋转矩阵rt+1、平移矩阵tt+1，从而利用式(6)对t+1时刻的点云p″t+1进行变换操作，得到t+1时刻的局部点云地图mp,t+1，进而得到t+1时刻的全局地图mg,t+1＝mp,t+mp,t+1：

29、mp,t+1＝rt+1*p″t+1+tt+1  (6)

30、步骤5：通过回环检测对全局地图进行优化，生成栅格地图用于路径导航规划；

31、步骤5.1、获取初始时刻到t时刻的精确点云集合p”＝{p”m|m＝0,1,2,3,…t}，其中，p”m表示第m时刻的精确点云；

32、步骤5.2、在t+1时刻对精确点云集合p”进行回环检测，得到t+1时刻更新后的全局地图m′g,t+1：

33、步骤5.3、将t+1时刻更新后的全局地图m'h,t+1转化为t+1时刻的栅格地图gt+1并保存后，用于车辆的全自动泊车。

34、本发明所述的基于环视视觉的全自动泊车定位与建图方法的特点也在于，步骤3.2中根据t时刻的车辆imu传感器对点云pt进行水平校准，得到水平校准后的点云p't。包括：

35、步骤3.2.1、定义车辆imu传感器的平面方程为：ax+by+cz+d＝0，其中，x、y、z分别是车辆imu传感器平面的x轴、y轴、z轴，a、b、c分别是x、y、z的法向量，d为所述车辆imu传感器平面相对于自身坐标原点的偏移量；

36、步骤3.2.2、定义车辆坐标系中的竖直向量为l，将所述车辆imu平面的法向量a、b、c通过所述竖直向量l进行旋转，得到旋转矩阵mr；

37、步骤3.2.3、根据所述旋转矩阵mr，利用点云旋转函数对pt进行处理，得到旋转后的点云，将旋转后的点云的z轴高度值设置为0，从而得到水平校准后的点云p't。

38、步骤3.3中对水平校准后的点云p't进行半径滤波处理，得到精确点云p″t，包括：

39、步骤3.3.1、选取所述水平校准后的点云p't中的任意一个点记为点a，以点a作为圆心，以一个距离阈值作为半径，形成一个滤波圆，判断所述滤波圆内是否包含至少两个点，若包含，则将点a保留；否则，则将点a删除；

40、步骤3.3.2、重复执行步骤3.3.1，直至点云p't中的所有点均完成处理为止，从而将所有保留的点组成精确点云p″t。

41、步骤4.6中的正态分布变换算法是按如下步骤进行：

42、步骤4.6.1、将t时刻的局部点云地图mp,t中的点云进行降采样处理后，得到t时刻处理后的局部点云地图m'p,t；

43、步骤4.6.2、将m'p,t离散化为三维的网格，并计算每个网格中点云的均值和协方差矩阵，从而得到一个高斯分布gd1；

44、步骤4.6.3、按照步骤4.6.1和步骤4.6.2对t+1时刻的精确点云信息p″t+1进行处理，得到另一个高斯分布gd2；

45、步骤4.6.4、计算gd1和gd2之间的kl散度，并通过梯度下降法最小化kl散度，从而得到最优的旋转矩阵rt+1、平移矩阵tt+1。

46、步骤5.2是按如下步骤进行：

47、步骤5.2.1、使用kd-tree算法快速查找p”中与t+1时刻的精确点云p″t+1相似值最大的精确点云p″k，p″k∈p”,0≤k≤t；

48、步骤5.2.2、计算t+1时刻的精确点云p″t+1与p″k的相似性程度st+1；

49、步骤5.2.3、使用迭代最近点算法计算t+1时刻的精确点云p″t+1与p″k的之间的欧氏距离dt+1；当st+1大于设定的相似性程度阈值，且dt+1小于设定的欧氏距离阈值时，则表示车辆回到之前的历史位置，并执行步骤5.2.4，否则，表示车辆未处于回环状态，则得到t+1时刻更新后的全局地图m'g,t+1＝mg,t+1；

50、步骤5.2.4、通过非线性优化算法对t+1时刻的位姿wt+1进行优化，以最小化t+1时刻精确点云p″t+1与p″k之间的误差，从而得到t+1时刻优化后的位姿w't+1；

51、根据w't+1，对t+1时刻的局部点云地图mp,t+1进行更新，得到t+1时刻更新后的局部点云地图m'p,t+1，从而得到t+1时刻更新后的全局地图m'g,t+1＝mp,t+m'p,t+1。

52、步骤5.3是按如下步骤进行：

53、步骤5.3.1、对栅格地图gt+1初始化，包括：设置t+1时刻的栅格地图gt+1的分辨率，根据m'g,t+1的边界计算栅格地图gt+1的边界框，设置gt+1的原点也为车辆的初始位置；

54、步骤5.3.2、对m'g,t+1进行半径滤波、降采样处理后，得到t+1时刻轻量化的全局地图m″g,t+1；

55、步骤5.3.3、遍历m″g,t+1中的点云并逐一映射到栅格地图gt+1中，并将栅格地图中被占据的位置的概率标记为100，未被占据的位置的概率标记为0，从而得到t+1时刻最终的栅格地图gt+1。

56、本发明一种电子设备，包括存储器以及处理器的特点在于，所述存储器用于存储支持处理器执行所述全自动泊车定位与建图方法的程序，所述处理器被配置为用于执行所述存储器中存储的程序。

57、本发明一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序的特点在于，所述计算机程序被处理器运行时执行所述全自动泊车定位与建图方法的步骤。

58、与现有技术相比，本发明的有益效果在于：

59、1.本发明通过融合环视鱼眼相机、imu和里程计信息，获取丰富的车位信息与障碍物信息，从而在不增加传感器成本的情况下提升泊车环境感知能力，克服了单独使用摄像头带来的定位不准确问题。

60、2.本发明提出了一种根据车辆姿态进行水平校准的方法，当车辆经过减速带和坑洼地带时，能够即使纠正车辆受到道路因素干扰时产生的偏差，利用车辆imu传感器信息对当前获取的点云进行水平校准，提高了获取停车位和障碍物位置信息的准确性。

61、3.本发明提出了一种融合imu传感器和里程计测量值的方法，根据车辆行驶状态和传感器的特性进行权重分配。有利于结合imu传感器在车辆转弯、里程计在车辆直行时具有相对较高精度的优势，进一步提升了定位与建图的精度。