CN117782102A - 一种基于环视视觉的全自动泊车定位与建图方法 - Google Patents
一种基于环视视觉的全自动泊车定位与建图方法 Download PDFInfo
- Publication number
- CN117782102A CN117782102A CN202311830221.7A CN202311830221A CN117782102A CN 117782102 A CN117782102 A CN 117782102A CN 202311830221 A CN202311830221 A CN 202311830221A CN 117782102 A CN117782102 A CN 117782102A
- Authority
- CN
- China
- Prior art keywords
- time
- vehicle
- point cloud
- map
- imu
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Landscapes
- Navigation (AREA)
Abstract
本发明公开了一种基于环视视觉的全自动泊车定位与建图方法，包括：1.通过车辆安装的四个环视鱼眼相机获取车辆周围的环视图像信息；2.利用深度学习方法提取环视图像中的车道线以及障碍物信息；3.对提取的车道线以及障碍物信息进行点云转化处理；4.融合IMU和里程计数据，进行位姿估计并生成全局停车场地图；5.通过回环检测进行地图优化，生成栅格地图用于路径导航规划。本发明结合环视相机、IMU和里程计的各自优势，提升了停车场定位与建图的精度，有助于提高泊车效率与准确度。
Description
技术领域
本发明涉及车辆自动驾驶领域，尤其涉及一种全自动泊车定位与建图方法。
背景技术
随着城市的快速发展和汽车保有量逐年增加，车位短缺以及复杂的泊车环境已经成为了驾驶者的一大困扰，浪费了大量的时间和精力。此外，由于停车操作需要同时兼顾车辆后方以及两侧的情况，且空间紧凑，过程中又无人指导，因此对驾驶员驾驶操作的熟练程度要求很高。即便是操作熟练的驾驶员也会被繁琐的泊车过程和找不到合适的车位等问题所困扰。基于以上需求，智能泊车技术应运而生，这项技术不仅可以解决驾驶员的烦恼，而且对于停车场空间的高效利用、泊车过程的安全性都有较大益处。
室内停车场自主泊车定位技术面临的主要问题包括两方面，一是受限于室内环境与GNSS信号常被遮挡无法使用的问题；另一方面则是由于传感器成本较高而导致自主泊车技术普及较为困难的问题。虽然能够通过搭载如激光雷达等传感器增加精确环境感知的能力，但势必会带来成本的增加。在自动驾驶领域不断压低成本，提升产业竞争力的背景下，如何实现在有限成本下优化泊车效果，成为当前亟待解决的问题。
发明内容
本发明针对有限成本下泊车定位与建图优化问题，提供了一种基于环视视觉的泊车定位与建图方法，以期实现在不改变传感器配置的情况下，通过优化定位与建图来提高建图的精度，从而能保障车辆在地下停车场内安全行驶，并有助于提高泊车效率与准确度。
本发明为达到上述发明目的，采用如下技术方案：
本发明一种基于环视视觉的全自动泊车定位与建图方法的特点在于，包括以下步骤：
步骤1：车辆在移动过程中，通过安装在汽车前、后、左、右的鱼眼相机获取当前t时刻的环视图像后，经过相机畸变去除、逆透视变换与图像拼接的操作，得到一张t时刻的俯视图像It；
步骤2：采用卷积神经网络的DeepLab V3+模型提取所述俯视图像It中的车道线以及障碍物信息并进行标注，得到带有车道线以及障碍物信息的图像I't；
步骤3：对提取的车道线以及障碍物信息进行点云转化；
步骤3.1、将I't中的车道线以及障碍物信息所对应的像素点转化为点云Pt；
步骤3.2、根据t时刻的车辆IMU传感器对点云Pt进行水平校准，得到水平校准后的点云P′t；
步骤3.3、对水平校准后的点云P't进行半径滤波处理，得到精确点云P″t；
步骤4：融合IMU传感器和里程计的数据，对t+1时刻的车辆位姿进行估算并生成全局停车场地图；
步骤4.1、利用式(1)计算t时刻IMU估计的车辆加速度和车辆角速度
式(1)中，i表示IMU传感器坐标系，表示t时刻IMU传感器输出的车辆加速度，表示t时刻车辆加速度的高斯白噪声，表示t时刻车辆加速度的随机游走噪声，表示t时刻IMU传感器输出的车辆角速度；表示t时刻车辆角速度的高斯白噪声，表示t时刻车辆角速度的随机游走噪声；
步骤4.2、利用式(2)计算t+1时刻IMU估计的车辆速度和车辆角度
式(2)中，Δt表示t时刻和t+1时刻之间的差值，Δv表示速度的变化量，Δθ表示角度的变化量；和分别表示t时刻IMU估计的车辆速度和车辆角度；当t＝0时，令和均为零；
步骤4.3、利用式(3)计算t+1时刻里程计估计的车辆速度和车辆角度
式(3)中，o表示里程计坐标系，vr,t+1和vl,t+1分别表示t+1时刻里程计输出的右轮速度和左轮速度，sr,t+1和sl,t+1分别表示Δt内右轮行驶里程和左轮行驶里程，Len是车身宽度，是t时刻里程计估计的车辆角度；当t＝0时，令为零；
步骤4.3、利用式(4)估算t+1时刻融合后的车辆速度vt+1和车辆角度θt+1：
式(4)中，μ1表示IMU传感器对应的权重，μ2表示里程计对应的权重；当车辆直行时，设置μ1＜μ2；当车辆转弯时，设置μ1＞μ2；
步骤4.4、以车辆的初始位置为坐标原点，车辆朝向为Y轴正方向，车辆朝向沿车辆所在平面顺时针旋转90°为X轴正方向，建立二维地图坐标系；
定义车辆在t时刻的位姿Wt＝(Xt,Yt,Vt)，其中，Xt,Yt为车辆在t时刻的二维坐标，Vt为在t时刻车辆朝向与X轴正方向的夹角；当t＝0时，令W0＝(0,0,90°)；
步骤4.5、利用式(5)计算车辆在t+1时刻的位姿Wt+1＝(Xt+1,Yt+1,Vt+1)：
式(5)中，Xt+1和Yt+1表示车辆在t+1时刻的二维坐标，Vt+1表示车辆在t+1时刻的车辆朝向与X轴正方向的夹角；
步骤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：
Mp,t+1＝Rt+1*P″t+1+Tt+1 (6)
步骤5：通过回环检测对全局地图进行优化，生成栅格地图用于路径导航规划；
步骤5.1、获取初始时刻到t时刻的精确点云集合P”＝{P”m|m＝0,1,2,3,…t}，其中，P”m表示第m时刻的精确点云；
步骤5.2、在t+1时刻对精确点云集合P”进行回环检测，得到t+1时刻更新后的全局地图M′g,t+1：
步骤5.3、将t+1时刻更新后的全局地图M'h,t+1转化为t+1时刻的栅格地图Gt+1并保存后，用于车辆的全自动泊车。
本发明所述的基于环视视觉的全自动泊车定位与建图方法的特点也在于，步骤3.2中根据t时刻的车辆IMU传感器对点云Pt进行水平校准，得到水平校准后的点云P't。包括：
步骤3.2.1、定义车辆IMU传感器的平面方程为：ax+by+cz+d＝0，其中，x、y、z分别是车辆IMU传感器平面的x轴、y轴、z轴，a、b、c分别是x、y、z的法向量，d为所述车辆IMU传感器平面相对于自身坐标原点的偏移量；
步骤3.2.2、定义车辆坐标系中的竖直向量为L，将所述车辆IMU平面的法向量a、b、c通过所述竖直向量L进行旋转，得到旋转矩阵mr；
步骤3.2.3、根据所述旋转矩阵mr，利用点云旋转函数对Pt进行处理，得到旋转后的点云，将旋转后的点云的z轴高度值设置为0，从而得到水平校准后的点云P't。
步骤3.3中对水平校准后的点云P't进行半径滤波处理，得到精确点云P″t，包括：
步骤3.3.1、选取所述水平校准后的点云P't中的任意一个点记为点A，以点A作为圆心，以一个距离阈值作为半径，形成一个滤波圆，判断所述滤波圆内是否包含至少两个点，若包含，则将点A保留；否则，则将点A删除；
步骤3.3.2、重复执行步骤3.3.1，直至点云P't中的所有点均完成处理为止，从而将所有保留的点组成精确点云P″t。
步骤4.6中的正态分布变换算法是按如下步骤进行：
步骤4.6.1、将t时刻的局部点云地图Mp,t中的点云进行降采样处理后，得到t时刻处理后的局部点云地图M'p,t；
步骤4.6.2、将M'p,t离散化为三维的网格，并计算每个网格中点云的均值和协方差矩阵，从而得到一个高斯分布GD1；
步骤4.6.3、按照步骤4.6.1和步骤4.6.2对t+1时刻的精确点云信息P″t+1进行处理，得到另一个高斯分布GD2；
步骤4.6.4、计算GD1和GD2之间的KL散度，并通过梯度下降法最小化KL散度，从而得到最优的旋转矩阵Rt+1、平移矩阵Tt+1。
步骤5.2是按如下步骤进行：
步骤5.2.1、使用Kd-tree算法快速查找P”中与t+1时刻的精确点云P″t+1相似值最大的精确点云P″k，P″k∈P”,0≤k≤t；
步骤5.2.2、计算t+1时刻的精确点云P″t+1与P″k的相似性程度St+1；
步骤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；
步骤5.2.4、通过非线性优化算法对t+1时刻的位姿Wt+1进行优化，以最小化t+1时刻精确点云P″t+1与P″k之间的误差，从而得到t+1时刻优化后的位姿W't+1；
根据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。
步骤5.3是按如下步骤进行：
步骤5.3.1、对栅格地图Gt+1初始化，包括：设置t+1时刻的栅格地图Gt+1的分辨率，根据M'g,t+1的边界计算栅格地图Gt+1的边界框，设置Gt+1的原点也为车辆的初始位置；
步骤5.3.2、对M'g,t+1进行半径滤波、降采样处理后，得到t+1时刻轻量化的全局地图M″g,t+1；
步骤5.3.3、遍历M″g,t+1中的点云并逐一映射到栅格地图Gt+1中，并将栅格地图中被占据的位置的概率标记为100，未被占据的位置的概率标记为0，从而得到t+1时刻最终的栅格地图Gt+1。
本发明一种电子设备，包括存储器以及处理器的特点在于，所述存储器用于存储支持处理器执行所述全自动泊车定位与建图方法的程序，所述处理器被配置为用于执行所述存储器中存储的程序。
本发明一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序的特点在于，所述计算机程序被处理器运行时执行所述全自动泊车定位与建图方法的步骤。
与现有技术相比，本发明的有益效果在于：
1.本发明通过融合环视鱼眼相机、IMU和里程计信息，获取丰富的车位信息与障碍物信息，从而在不增加传感器成本的情况下提升泊车环境感知能力，克服了单独使用摄像头带来的定位不准确问题。
2.本发明提出了一种根据车辆姿态进行水平校准的方法，当车辆经过减速带和坑洼地带时，能够即使纠正车辆受到道路因素干扰时产生的偏差，利用车辆IMU传感器信息对当前获取的点云进行水平校准，提高了获取停车位和障碍物位置信息的准确性。
3.本发明提出了一种融合IMU传感器和里程计测量值的方法，根据车辆行驶状态和传感器的特性进行权重分配。有利于结合IMU传感器在车辆转弯、里程计在车辆直行时具有相对较高精度的优势，进一步提升了定位与建图的精度。
附图说明
图1为本发明提供的定位与建图方法流程图；
图2为本发明提供的回环检测优化后的全局点云地图示意图；
具体实施方式
下面结合说明书附图和具体实施例对本发明的技术方案做进一步详细说明。
本实施例中，如图1所示，一种基于环视视觉的全自动泊车定位与建图方法，包括以下步骤：
步骤1：车辆在移动过程中，通过安装在汽车前、后、左、右的鱼眼相机获取当前t时刻的环视图像后，经过相机畸变去除、逆透视变换与图像拼接的操作，得到一张t时刻的俯视图像It；实际应用中，鱼眼相机水平视场角为196°，垂直视场角为147°，检测距离＞10m，环视图像经过视频解码器输送至车辆搭载的全自动泊车控制器的视觉处理单元进行俯视图像的生成操作。
步骤2：采用卷积神经网络的DeepLab V3+模型提取俯视图像It中的车道线以及障碍物信息并进行标注，得到带有车道线以及障碍物信息的图像I't；
步骤2.1、在Ubuntu系统中安装Labelme软件，使用图像标注工具Labelme对采集的2000张停车场俯视图片进行标注，标注出车道线以及障碍物信息，得到标注好的停车场图片数据集D；
步骤2.2、将标注好的停车场图片数据集D输入DeepLab V3+模型进行训练，进行前向传播，计算损失函数，然后通过反向传播调整模型参数，最终得到优化后的模型M；
步骤2.3、使用优化后的模型M对俯视图像It进行处理，提取出车道线以及障碍物信息，不同信息类型会被标注为不同的颜色，例如车道线标注为红色，RGB值分别为255，0，0。障碍物标注为蓝色，RGB值分别为0，0，255；进而得到带有车道线以及障碍物信息的图像I't。
步骤3：对提取的车道线以及障碍物信息进行点云转化；
步骤3.1、将I't中的车道线以及障碍物信息所对应的像素点转化为点云Pt；默认设置点云格式为pcl::PointXYZRGB格式。
步骤3.2、根据t时刻的车辆IMU传感器对点云Pt进行水平校准，得到水平校准后的点云P′t：
步骤3.2.1、定义车辆IMU传感器的平面方程为：ax+by+cz+d＝0，其中，x、y、z分别是车辆IMU传感器平面的x轴、y轴、z轴，a、b、c分别是x、y、z的法向量，d为车辆IMU传感器平面相对于自身坐标原点的偏移量；
步骤3.2.2、定义车辆坐标系中的竖直向量为L，默认车辆坐标系中的竖直向量L为(0，0，1)，将车辆IMU平面的法向量a、b、c通过竖直向量L进行旋转，得到旋转矩阵mr；
步骤3.2.3、根据旋转矩阵mr，利用点云旋转函数对Pt进行处理，得到旋转后的点云，将旋转后的点云的z轴高度值设置为0，从而得到水平校准后的点云P't。
步骤3.3、对水平校准后的点云P't进行半径滤波处理，得到精确点云P″t：
步骤3.3.1、选取水平校准后的点云P't中的任意一个点记为点A，以点A作为圆心，以一个距离阈值作为半径，默认设置距离阈值为0.1m，形成一个滤波圆，判断滤波圆内是否包含至少两个点，若包含，则将点A保留；否则，则将点A删除；
步骤3.3.2、重复执行步骤3.3.1，直至点云P't中的所有点均完成处理为止，从而将所有保留的点组成精确点云P″t。
步骤4：融合IMU传感器和里程计的数据，对t+1时刻的车辆位姿进行估算并生成全局停车场地图；
步骤4.1、利用式(1)计算t时刻IMU估计的车辆加速度和车辆角速度
式(1)中，i表示IMU传感器坐标系，表示t时刻IMU传感器输出的车辆加速度，表示t时刻车辆加速度的高斯白噪声，表示t时刻车辆加速度的随机游走噪声，表示t时刻IMU传感器输出的车辆角速度；表示t时刻车辆角速度的高斯白噪声，表示t时刻车辆角速度的随机游走噪声。
步骤4.2、利用式(2)计算t+1时刻IMU估计的车辆速度和车辆角度
式(2)中，Δt表示t时刻和t+1时刻之间的差值，Δv表示速度的变化量，Δθ表示角度的变化量；和分别表示t时刻IMU估计的车辆速度和车辆角度；当t＝0时，令和均为零。
步骤4.3、利用式(3)计算t+1时刻里程计估计的车辆速度和车辆角度
式(3)中，o表示里程计坐标系，vr,t+1和vl,t+1分别表示t+1时刻里程计输出的右轮速度和左轮速度，sr,t+1和sl,t+1分别表示Δt内右轮行驶里程和左轮行驶里程，Len是车身宽度，是t时刻里程计估计的车辆角度；当t＝0时，令为零。
步骤4.3、利用式(4)估算t+1时刻融合后的车辆速度μ+1和车辆角度θt+1：
式(4)中，μ1表示IMU传感器对应的权重，μ2表示里程计对应的权重；当车辆直行时，设置μ1＜μ2；当车辆转弯时，设置μ1＞μ2；默认设置车辆直行时μ1为30％，μ2为70％。车辆转弯时μ1为70％，μ2为30％。
步骤4.4、以车辆的初始位置为坐标原点，车辆朝向为Y轴正方向，车辆朝向沿车辆所在平面顺时针旋转90°为X轴正方向，建立二维地图坐标系；
定义车辆在t时刻的位姿Wt＝(Xt,Yt,Vt)，其中，Xt,Yt为车辆在t时刻的二维坐标，Vt为在t时刻车辆朝向与X轴正方向的夹角；当t＝0时，令W0＝(0,0,90°)。
步骤4.5、利用式(5)计算车辆在t+1时刻的位姿Wt+1＝(Xt+1,Yt+1,Vt+1)：
式(5)中，Xt+1和Yt+1表示车辆在t+1时刻的二维坐标，Vt+1表示车辆在t+1时刻的车辆朝向与X轴正方向的夹角。
步骤4.6、在t时刻的位姿Wt上添加精确点云信息P″t，以构成t时刻的局部点云地图Mp,t；根据t+1时刻的位姿Wt+1，使用正态分布变换算法计算t+1时刻的精确点云信息P″t+1与Mp,t之间的旋转矩阵Rt+1、平移矩阵Tt+1：
步骤4.6.1、将t时刻的局部点云地图Mp,t中的点云使用pcl::VoxelGrid函数进行降采样处理后，得到t时刻处理后的局部点云地图M'p,t；
步骤4.6.2、将M'p,t离散化为三维的网格，默认设置网格的分辨率为1m。并计算每个网格中点云的均值和协方差矩阵，从而得到一个高斯分布GD1；
步骤4.6.3、按照步骤4.6.1和步骤4.6.2对t+1时刻的精确点云信息P″t+1进行处理，得到另一个高斯分布GD2；
步骤4.6.4、计算GD1和GD2之间的KL散度，并通过梯度下降法最小化KL散度，默认设置梯度下降法的迭代步长为0.01m，最大迭代次数为50次，从而得到最优的旋转矩阵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：
Mp,t+1＝Rt+1*P″t+1+Tt+1 (6)
步骤5：通过回环检测对全局地图进行优化，生成栅格地图用于路径导航规划；
步骤5.1、获取初始时刻到t时刻的精确点云集合P”＝{P”m|m＝0,1,2,3,…t}，其中，P”m表示第m时刻的精确点云。
步骤5.2、在t+1时刻对精确点云集合P”进行回环检测：
步骤5.2.1、使用Kd-tree算法快速查找P”中与t+1时刻的精确点云P″t+1相似值最大的精确点云P″k，P″k∈P”,0≤k≤t；
步骤5.2.2、计算t+1时刻的精确点云P″t+1与P″k的相似性程度St+1；
步骤5.2.3、使用迭代最近点算法计算t+1时刻的精确点云P″t+1与P″k的之间的欧氏距离dt+1；默认设置相似性程度阈值为90％，欧氏距离阈值为0.03m。当St+1大于设定的相似性程度阈值，且dt+1小于设定的欧氏距离阈值时，则表示车辆回到之前的历史位置，并执行步骤5.2.4，否则，表示车辆未处于回环状态，则得到t+1时刻更新后的全局地图M'g,t+1＝Mg,t+1。
步骤5.2.4、通过非线性优化算法对t+1时刻的位姿Wt+1进行优化，以最小化t+1时刻精确点云P″t+1与P″k之间的误差，从而得到t+1时刻优化后的位姿W't+1；
根据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，如图2所示。
步骤5.3、将t+1时刻更新后的全局地图M'g,t+1转化为t+1时刻的栅格地图Gt+1：
步骤5.3.1、对栅格地图Gt+1初始化，包括：设置t+1时刻的栅格地图Gt+1的分辨率，根据M'g,t+1的边界计算栅格地图Gt+1的边界框，设置Gt+1的原点也为车辆的初始位置；
步骤5.3.2、对M'g,t+1进行半径滤波、降采样处理后，得到t+1时刻轻量化的全局地图M″g,t+1；
步骤5.3.3、遍历M″g,t+1中的点云并逐一映射到栅格地图Gt+1中，并将栅格地图中被占据的位置的概率标记为100，未被占据的位置的概率标记为0，从而得到t+1时刻最终的栅格地图Gt+1并保存后，用于车辆的全自动泊车。
本实施例中，一种电子设备，包括存储器以及处理器，该存储器用于存储支持处理器执行上述方法的程序，该处理器被配置为用于执行该存储器中存储的程序。
本实施例中，一种计算机可读存储介质，是在计算机可读存储介质上存储有计算机程序，该计算机程序被处理器运行时执行上述方法的步骤。
Claims (8)
1.一种基于环视视觉的全自动泊车定位与建图方法，其特征在于，包括以下步骤：
步骤1：车辆在移动过程中，通过安装在汽车前、后、左、右的鱼眼相机获取当前t时刻的环视图像后，经过相机畸变去除、逆透视变换与图像拼接的操作，得到一张t时刻的俯视图像It；
步骤2：采用卷积神经网络的DeepLab V3+模型提取所述俯视图像It中的车道线以及障碍物信息并进行标注，得到带有车道线以及障碍物信息的图像I't；
步骤3：对提取的车道线以及障碍物信息进行点云转化；
步骤3.1、将I't中的车道线以及障碍物信息所对应的像素点转化为点云Pt；
步骤3.2、根据t时刻的车辆IMU传感器对点云Pt进行水平校准，得到水平校准后的点云P′t；
步骤3.3、对水平校准后的点云P't进行半径滤波处理，得到精确点云P″t；
步骤4：融合IMU传感器和里程计的数据，对t+1时刻的车辆位姿进行估算并生成全局停车场地图；
步骤4.1、利用式(1)计算t时刻IMU估计的车辆加速度和车辆角速度
式(1)中，i表示IMU传感器坐标系，表示t时刻IMU传感器输出的车辆加速度，表示t时刻车辆加速度的高斯白噪声，表示t时刻车辆加速度的随机游走噪声，表示t时刻IMU传感器输出的车辆角速度；表示t时刻车辆角速度的高斯白噪声，表示t时刻车辆角速度的随机游走噪声；
步骤4.2、利用式(2)计算t+1时刻IMU估计的车辆速度和车辆角度
式(2)中，Δt表示t时刻和t+1时刻之间的差值，Δv表示速度的变化量，Δθ表示角度的变化量；和分别表示t时刻IMU估计的车辆速度和车辆角度；当t＝0时，令和均为零；
步骤4.3、利用式(3)计算t+1时刻里程计估计的车辆速度和车辆角度
式(3)中，o表示里程计坐标系，vr,t+1和vl,t+1分别表示t+1时刻里程计输出的右轮速度和左轮速度，sr,t+1和sl,t+1分别表示Δt内右轮行驶里程和左轮行驶里程，Len是车身宽度，是t时刻里程计估计的车辆角度；当t＝0时，令为零；
步骤4.3、利用式(4)估算t+1时刻融合后的车辆速度vt+1和车辆角度θt+1：
式(4)中，μ1表示IMU传感器对应的权重，μ2表示里程计对应的权重；当车辆直行时，设置μ1＜μ2；当车辆转弯时，设置μ1＞μ2；
步骤4.4、以车辆的初始位置为坐标原点，车辆朝向为Y轴正方向，车辆朝向沿车辆所在平面顺时针旋转90°为X轴正方向，建立二维地图坐标系；
定义车辆在t时刻的位姿Wt＝(Xt,Yt,Vt)，其中，Xt,Yt为车辆在t时刻的二维坐标，Vt为在t时刻车辆朝向与X轴正方向的夹角；当t＝0时，令W0＝(0,0,90°)；
步骤4.5、利用式(5)计算车辆在t+1时刻的位姿Wt+1＝(Xt+1,Yt+1,Vt+1)：
式(5)中，Xt+1和Yt+1表示车辆在t+1时刻的二维坐标，Vt+1表示车辆在t+1时刻的车辆朝向与X轴正方向的夹角；
步骤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：
Mp,t+1＝Rt+1*P″t+1+Tt+1 (6)
步骤5：通过回环检测对全局地图进行优化，生成栅格地图用于路径导航规划；
步骤5.1、获取初始时刻到t时刻的精确点云集合P”＝{P”m|m＝0,1,2,3,…t}，其中，P”m表示第m时刻的精确点云；
步骤5.2、在t+1时刻对精确点云集合P”进行回环检测，得到t+1时刻更新后的全局地图M′g,t+1：
步骤5.3、将t+1时刻更新后的全局地图M'g,t+1转化为t+1时刻的栅格地图Gt+1并保存后，用于车辆的全自动泊车。
2.根据权利要求1所述的基于环视视觉的全自动泊车定位与建图方法，其特征在于，步骤3.2中根据t时刻的车辆IMU传感器对点云Pt进行水平校准，得到水平校准后的点云P't。包括：
步骤3.2.1、定义车辆IMU传感器的平面方程为：ax+by+cz+d＝0，其中，x、y、z分别是车辆IMU传感器平面的x轴、y轴、z轴，a、b、c分别是x、y、z的法向量，d为所述车辆IMU传感器平面相对于自身坐标原点的偏移量；
步骤3.2.2、定义车辆坐标系中的竖直向量为L，将所述车辆IMU平面的法向量a、b、c通过所述竖直向量L进行旋转，得到旋转矩阵mr；
步骤3.2.3、根据所述旋转矩阵mr，利用点云旋转函数对Pt进行处理，得到旋转后的点云，将旋转后的点云的z轴高度值设置为0，从而得到水平校准后的点云P't。
3.根据权利要求2所述的基于环视视觉的全自动泊车定位与建图方法，其特征在于，步骤3.3中对水平校准后的点云P't进行半径滤波处理，得到精确点云P″t，包括：
步骤3.3.1、选取所述水平校准后的点云P't中的任意一个点记为点A，以点A作为圆心，以一个距离阈值作为半径，形成一个滤波圆，判断所述滤波圆内是否包含至少两个点，若包含，则将点A保留；否则，则将点A删除；
步骤3.3.2、重复执行步骤3.3.1，直至点云P't中的所有点均完成处理为止，从而将所有保留的点组成精确点云P″t。
4.根据权利要求3所述的基于环视视觉的全自动泊车定位与建图方法，其特征在于，步骤4.6中的正态分布变换算法是按如下步骤进行：
步骤4.6.1、将t时刻的局部点云地图Mp,t中的点云进行降采样处理后，得到t时刻处理后的局部点云地图M'p,t；
步骤4.6.2、将M'p,t离散化为三维的网格，并计算每个网格中点云的均值和协方差矩阵，从而得到一个高斯分布GD1；
步骤4.6.3、按照步骤4.6.1和步骤4.6.2对t+1时刻的精确点云信息P″t+1进行处理，得到另一个高斯分布GD2；
步骤4.6.4、计算GD1和GD2之间的KL散度，并通过梯度下降法最小化KL散度，从而得到最优的旋转矩阵Rt+1、平移矩阵Tt+1。
5.根据权利要求4所述的基于环视视觉的全自动泊车定位与建图方法，其特征在于，步骤5.2是按如下步骤进行：
步骤5.2.1、使用Kd-tree算法快速查找P”中与t+1时刻的精确点云P″t+1相似值最大的精确点云P″k，P″k∈P”,0≤k≤t；
步骤5.2.2、计算t+1时刻的精确点云P″t+1与P″k的相似性程度St+1；
步骤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；
步骤5.2.4、通过非线性优化算法对t+1时刻的位姿Wt+1进行优化，以最小化t+1时刻精确点云P″t+1与P″k之间的误差，从而得到t+1时刻优化后的位姿W't+1；
根据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。
6.根据权利要求5所述的基于环视视觉的全自动泊车定位与建图方法，其特征在于，步骤5.3是按如下步骤进行：
步骤5.3.1、对栅格地图Gt+1初始化，包括：设置t+1时刻的栅格地图Gt+1的分辨率，根据M'g,t+1的边界计算栅格地图Gt+1的边界框，设置Gt+1的原点也为车辆的初始位置；
步骤5.3.2、对M'g,t+1进行半径滤波、降采样处理后，得到t+1时刻轻量化的全局地图M″g,t+1；
步骤5.3.3、遍历M″g,t+1中的点云并逐一映射到栅格地图Gt+1中，并将栅格地图中被占据的位置的概率标记为100，未被占据的位置的概率标记为0，从而得到t+1时刻最终的栅格地图Gt+1。
7.一种电子设备，包括存储器以及处理器，其特征在于，所述存储器用于存储支持处理器执行权利要求1-6中任一所述全自动泊车定位与建图方法的程序，所述处理器被配置为用于执行所述存储器中存储的程序。
8.一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，其特征在于，所述计算机程序被处理器运行时执行权利要求1-6中任一所述全自动泊车定位与建图方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311830221.7A CN117782102A (zh) | 2023-12-28 | 2023-12-28 | 一种基于环视视觉的全自动泊车定位与建图方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311830221.7A CN117782102A (zh) | 2023-12-28 | 2023-12-28 | 一种基于环视视觉的全自动泊车定位与建图方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117782102A true CN117782102A (zh) | 2024-03-29 |
Family
ID=90384762
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311830221.7A Pending CN117782102A (zh) | 2023-12-28 | 2023-12-28 | 一种基于环视视觉的全自动泊车定位与建图方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117782102A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118864303A (zh) * | 2024-09-26 | 2024-10-29 | 合肥工业大学 | 一种基于平均模糊的动态物体去除方法 |
| CN120368929A (zh) * | 2025-05-08 | 2025-07-25 | 浙江科维检测认证有限公司 | 一种桌面水平基准快测的测量装置 |
-
2023
- 2023-12-28 CN CN202311830221.7A patent/CN117782102A/zh active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118864303A (zh) * | 2024-09-26 | 2024-10-29 | 合肥工业大学 | 一种基于平均模糊的动态物体去除方法 |
| CN120368929A (zh) * | 2025-05-08 | 2025-07-25 | 浙江科维检测认证有限公司 | 一种桌面水平基准快测的测量装置 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| EP3842755B1 (en) | Lane line positioning method and device, storage medium and electronic device | |
| CN107246876B (zh) | 一种无人驾驶汽车自主定位与地图构建的方法及系统 | |
| CN111263960B (zh) | 用于更新高清晰度地图的设备和方法 | |
| CN109166140B (zh) | 一种基于多线激光雷达的车辆运动轨迹估计方法及系统 | |
| CN114677663B (zh) | 车辆定位方法、装置及电子设备、计算机可读存储介质 | |
| CN109443348B (zh) | 一种基于环视视觉和惯导融合的地下车库库位跟踪方法 | |
| US11158065B2 (en) | Localization of a mobile unit by means of a multi hypothesis kalman filter method | |
| CN111862673A (zh) | 基于顶视图的停车场车辆自定位及地图构建方法 | |
| CN113920198B (zh) | 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法 | |
| CN117782102A (zh) | 一种基于环视视觉的全自动泊车定位与建图方法 | |
| CN115683128B (zh) | 一种港口无人集卡视觉特征匹配定位方法 | |
| WO2023065342A1 (zh) | 车辆及其定位方法、装置、设备、计算机可读存储介质 | |
| CN113554705A (zh) | 一种变化场景下的激光雷达鲁棒定位方法 | |
| Mounier et al. | Lidar-based multisensor fusion with 3-d digital maps for high-precision positioning | |
| CN114427863A (zh) | 车辆定位方法及系统、自动泊车方法及系统、存储介质 | |
| CN118279468A (zh) | 车道线标注方法、电子设备及计算机存储介质 | |
| CN116858269A (zh) | 基于激光slam的烟草行业成品库平库盘点机器人路径优化方法 | |
| CN111521996A (zh) | 一种激光雷达的安装标定方法 | |
| CN118408545A (zh) | 一种空地协同智能化辅助行驶导航方法及协同系统 | |
| CN117705145A (zh) | 一种局部几何信息融合的激光惯性里程计方法 | |
| JP7337617B2 (ja) | 推定装置、推定方法及びプログラム | |
| CN117075158A (zh) | 基于激光雷达的无人变形运动平台的位姿估计方法及系统 | |
| CN114563006B (zh) | 基于参考线匹配的车辆全局定位方法及装置 | |
| CN113869203B (zh) | 车辆定位方法和系统 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |