CN114693787A - 停车库建图与定位方法、系统及车辆 - Google Patents
停车库建图与定位方法、系统及车辆 Download PDFInfo
- Publication number
- CN114693787A CN114693787A CN202210269955.1A CN202210269955A CN114693787A CN 114693787 A CN114693787 A CN 114693787A CN 202210269955 A CN202210269955 A CN 202210269955A CN 114693787 A CN114693787 A CN 114693787A
- Authority
- CN
- China
- Prior art keywords
- pose
- vehicle
- map
- semantic
- information
- 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
- 238000000034 method Methods 0.000 title claims abstract description 29
- 238000000605 extraction Methods 0.000 claims abstract description 32
- 238000013507 mapping Methods 0.000 claims abstract description 32
- 238000004364 calculation method Methods 0.000 claims abstract description 14
- 238000001914 filtration Methods 0.000 claims abstract description 13
- 230000004927 fusion Effects 0.000 claims abstract description 11
- 238000005457 optimization Methods 0.000 claims description 25
- 239000011159 matrix material Substances 0.000 claims description 21
- 230000009466 transformation Effects 0.000 claims description 16
- 238000012545 processing Methods 0.000 claims description 15
- 230000008859 change Effects 0.000 claims description 14
- 230000006870 function Effects 0.000 claims description 10
- 230000008447 perception Effects 0.000 claims description 10
- 230000008569 process Effects 0.000 claims description 10
- 230000011218 segmentation Effects 0.000 claims description 7
- 230000004807 localization Effects 0.000 claims description 6
- 230000003044 adaptive effect Effects 0.000 claims description 5
- 238000001514 detection method Methods 0.000 claims description 5
- 238000013519 translation Methods 0.000 claims description 5
- 238000012937 correction Methods 0.000 claims description 4
- 238000013135 deep learning Methods 0.000 claims description 4
- 230000001131 transforming effect Effects 0.000 claims description 4
- 238000004519 manufacturing process Methods 0.000 abstract description 4
- 238000005516 engineering process Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013136 deep learning model Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T5/00—Image enhancement or restoration
- G06T5/40—Image enhancement or restoration using histogram techniques
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T5/00—Image enhancement or restoration
- G06T5/70—Denoising; Smoothing
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T5/00—Image enhancement or restoration
- G06T5/80—Geometric correction
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/215—Motion-based segmentation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20004—Adaptive image processing
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20112—Image segmentation details
- G06T2207/20164—Salient point detection; Corner detection
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30241—Trajectory
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30244—Camera pose
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30248—Vehicle exterior or interior
- G06T2207/30252—Vehicle exterior; Vicinity of vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Image Processing (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种停车库建图与定位方法、系统及车辆，该系统包括：环视标定拼接模块、语义提取模块、特征点提取模块、车轮编码器、融合里程计、语义地图模块、语义特征融合定位模块、滤波输出位姿模块；对环视图像进行标定拼接，然后在拼接图上提取语义信息；车轮编码器输出的里程计信息与环视图像提取的特征点信息融合输出更加精准的动态航迹位姿，在动态航迹位姿下构建语义地图；利用语义地图，结合环视语义信息和里程计信息，在停车场内高精度定位。本发明提供一种新型建图和定位框架，不额外增加整车传感器，有效利用摄像头及自车车辆状态里程计计算，实现低成本室内停车库建图与定位，可以实现量产。
Description
技术领域
本发明属于建图与定位技术领域，具体涉及一种停车库建图与定位方法、系统及车辆。
背景技术
随着智能驾驶系统的升级，为了解决车辆在无源环境下认识到自身在哪里、周围环境是怎么样的问题，并为后续的规划决策做准备，引入了SLAM实时定位与建图的技术。SLAM(simultaneous localization and mapping)，也称为CML(Concurrent Mapping andLocalization)，即时定位与地图构建或并发建图与定位。在智能驾驶场景中运用描述为：智能驾驶车辆进入未知环境中的未知位置，一边移动一边逐步描绘出此环境的地图和确认自车的位置。
传统的SLAM多是基于视觉特征点的方案进行建图与定位，特征点是一种具有尺度不变性、旋转不变性、光照不变性、速度快的一种特征信息，视觉寻找特征点对周边环境的纹理信息要求很高，特征点匹配鲁棒性不够好，难以进行车辆量产；另一种方法是单独采用鲁棒的语义信息进行建图与定位，图像语义信息表示颜色、纹理和形状等特征信息，但是采用语义信息需要有足够的语义信息来完成局部地图到全局地图的匹配。真实的停车场能够提供语义信息的主要是车库位、地面箭头、车轮档等信息，不足以支持该匹配。
发明内容
针对现有技术中存在的问题，本发明提供一种停车库建图与定位方法、系统及车辆，解决停车库的建图与定位问题。
本发明提供了一种停车库建图与定位方法，包括以下步骤：
利用车辆上的环视摄像头采集停车库图像，并进行摄像头标定和环视图像拼接；
对环视拼接图像进行语义分割提取语义信息，语义信息包括车库位线及库位箭头信息；
对环视摄像头原始图像进行直方图处理，调用ORB特征提取库进行特征提取，并进行畸变矫正，对特征点深度进行评估然后放入追踪函数进行追踪；对环视摄像头输入的每一帧提取的特征点信息进行追踪输出相机轨迹位姿点信息；
左右车轮编码器输出左右车轮脉冲信息，根据阿克曼转向原理，后轴两轮的速度瞬心相同，建立运动模型推算出车辆相对于起始位置的位姿，实现车辆位姿里程推算；
将相机轨迹位姿点信息与车辆位姿里程推算进行加权融合，输出动态航迹位姿；
在动态航迹位姿下实时构建语义地图：以车辆坐标系初始点的位置作为原点进行局部建图并进行定位初始化，特定距离建立的局部图像进行语义特征的回环检测，局部图像进行拼接成为全局地图；
利用全局地图，结合环视语义信息和动态航迹位姿，在停车库进行高精度定位：先使用动态航迹位姿得出车辆当前位置的粗定位，然后在地图中车辆一定半径范围内搜索所有语义结果，并与车辆实时感知语义结果做匹配，得到车辆周围每一个感知语义和地图语义信息的位姿差，该位姿差包含角度旋转值的差δR和距离差δt，则对于t时刻的位姿可以表示为X*t＝Xt+CRΣδR+CtΣδt；其中，CR和Ct为系数，X*t是融合定位后t时刻的位姿，Xt是粗定位的位姿；
滤波输出位姿，滤波分为预测和更新：预测是从上一个地图匹配的时间t-1得到的地图匹配位姿结果[x，y，head]，结合t时刻的运动模型，得到t时刻的运动模型的预测结果；更新是根据粗定位结果和环境观测值，得到精确定位。
进一步，摄像头标定包括：内参标定、角点提取、世界坐标系获取、计算RT变化矩阵；
内参标定采用输入基准内参的方式，在后续外参标定中结合棋盘格的角点对内参进行矫正；角点提取会先对图像进行透视变换，对透视变换后的图像进行白边处理和自适应阈值调整找到黑白分界线和角点，然后再反透视变换角点，返回角点信息；世界坐标系获取是将角点坐标转换进实际标定场景地图并获得在场景地图坐标；计算RT变化矩阵是通过PnP匹配求解出角点在图像坐标系到世界坐标系的变化矩阵，其中R是旋转，T是平移；
环视图像拼接是对同一时刻采集到的环视图像进行权重处理，以保证拼接缝处的平滑。
进一步，建立运动模型推算车辆相对于起始位置的位姿，包括位置x和y，以及航向角heading；运动模型如下：
Xi＝Xi-1+△Si*sin(heading)；
Yi＝Yi-1+△Si*cos(heading)；
heading＝Σ△θ；
其中，Xi是i时刻的X坐标，Yi是i时刻的Y坐标，Xi-1是i-1时刻的X坐标，Yi-1是i-1时刻的Y坐标，heading是航向角，△Si是脉冲区间的行驶距离增量，△θ是脉冲区间的航向角增量。
进一步，在建图过程中，表征车辆位姿变换的控制向量u1:t和对环境观测向量z1:t已知，需要求解准确的地图信息m和准确的车辆位姿信息xt；
建立后验概率P(xt，m|z1:t，u1:t)，该后验概率表示在t时刻车辆当前位姿xt和地图m的最大概率；
对该概率采用图优化的理论进行解答：车辆位姿x1：t和局部地图m被描述成节点优化变量vi，vi∈V，V表示节点优化变量集合；
将环境观测向量和辆位姿变换的控制向量用约束边表示，则构成了一张优化变量与约束的图，用公式J(V)＝Σe[zij-z*ij(vi，vj)]TΩij e[zij-z*ij(vi，vj)]表示；其中，e[zij-z*ij(vi，vj)]是损失函数，描述观测值zij与预测值z*ij的差异性，此差异性用信息矩阵Ωij来衡量；
该图优化问题用高斯牛顿法来求解，最优的解V*＝argmin J(V)，即求得的V*为最终的估计环境局部地图和车辆位姿。
进一步，利用深度学习网络uNet对拼接后的环视图像进行语义分割。
本发明还提供一种停车库建图与定位系统，包括：
环视标定拼接模块，用于利用车辆上的环视摄像头采集停车库图像，并进行摄像头标定和环视图像拼接；
语义提取模块，用于对环视拼接图像进行语义分割提取语义信息，语义信息包括车库位线及库位箭头信息；
特征点提取模块，用于对环视摄像头原始图像进行直方图处理，调用ORB特征提取库进行特征提取，并进行畸变矫正，对特征点深度进行评估然后放入追踪函数进行追踪；对环视摄像头输入的每一帧提取的特征点信息进行追踪输出相机轨迹位姿点信息；
车轮编码器，用于输出左右车轮脉冲信息，根据阿克曼转向原理，后轴两轮的速度瞬心相同，建立运动模型推算出车辆相对于起始位置的位姿，实现车辆位姿里程推算；
融合里程计，用于将相机轨迹位姿点信息与车辆位姿里程推算进行加权融合，输出动态航迹位姿；
语义地图模块，用于在动态航迹位姿下实时构建语义地图：以车辆坐标系初始点的位置作为原点进行局部建图并进行定位初始化，特定距离建立的局部图像进行语义特征的回环检测，局部图像进行拼接成为全局地图；
语义特征融合定位模块，用于利用全局地图，结合环视语义信息和动态航迹位姿，在停车库进行高精度定位：先使用动态航迹位姿得出车辆当前位置的粗定位，然后在地图中车辆一定半径范围内搜索所有语义结果，并与车辆实时感知语义结果做匹配，得到车辆周围每一个感知语义和地图语义信息的位姿差，该位姿差包含角度旋转值的差δR和距离差δt，则对于t时刻的位姿可以表示为X*t＝Xt+CRΣδR+CtΣδt；其中，CR和Ct为系数，X*t是融合定位后t时刻的位姿，Xt是粗定位的位姿；
滤波输出位姿模块，用于滤波输出位姿，滤波分为预测和更新：预测是从上一个地图匹配的时间t-1得到的地图匹配位姿结果[x，y，head]，结合t时刻的运动模型，得到t时刻的运动模型的预测结果；更新是根据粗定位结果和环境观测值，得到精确定位。
进一步，摄像头标定包括：内参标定、角点提取、世界坐标系获取、计算RT变化矩阵；
内参标定采用输入基准内参的方式，在后续外参标定中结合棋盘格的角点对内参进行矫正；角点提取会先对图像进行透视变换，对透视变换后的图像进行白边处理和自适应阈值调整找到黑白分界线和角点，然后再反透视变换角点，返回角点信息；世界坐标系获取是将角点坐标转换进实际标定场景地图并获得在场景地图坐标；计算RT变化矩阵是通过PnP匹配求解出角点在图像坐标系到世界坐标系的变化矩阵，其中R是旋转，T是平移；
环视图像拼接是对同一时刻采集到的环视图像进行权重处理，以保证拼接缝处的平滑。
进一步，建立运动模型推算车辆相对于起始位置的位姿，包括位置x和y，以及航向角heading；运动模型如下：
Xi＝Xi-1+△Si*sin(heading)；
Yi＝Yi-1+△Si*cos(heading)；
heading＝Σ△θ；
其中，Xi是i时刻的X坐标，Yi是i时刻的Y坐标，Xi-1是i-1时刻的X坐标，Yi-1是i-1时刻的Y坐标，heading是航向角，△Si是脉冲区间的行驶距离增量，△θ是脉冲区间的航向角增量。
进一步，在建图过程中，表征车辆位姿变换的控制向量u1:t和对环境观测向量z1:t已知，需要求解准确的地图信息m和准确的车辆位姿信息xt；
建立后验概率P(xt，m|z1:t，u1:t)，该后验概率表示在t时刻车辆当前位姿xt和地图m的最大概率；
对该概率采用图优化的理论进行解答：车辆位姿x1：t和局部地图m被描述成节点优化变量vi，vi∈V，V表示节点优化变量集合；
将环境观测向量和辆位姿变换的控制向量用约束边表示，则构成了一张优化变量与约束的图，用公式J(V)＝Σe[zij-z*ij(vi，vj)]TΩij e[zij-z*ij(vi，vj)]表示；其中，e[zij-z*ij(vi，vj)]是损失函数，描述观测值zij与预测值z*ij的差异性，此差异性用信息矩阵Ωij来衡量；
该图优化问题用高斯牛顿法来求解，最优的解V*＝argmin J(V)，即求得的V*为最终的估计环境局部地图和车辆位姿。
本发明还提供了一种车辆，该车辆采用上述的停车库建图与定位方法。
本发明与现有技术相比，具有以下优点及有益效果：
本发明提供一种新型建图和定位框架，不额外增加整车传感器，有效利用摄像头及自车车辆状态里程计计算，实现低成本室内停车库建图与定位，可以实现量产。
附图说明
图1为本发明实施例的停车库建图与定位系统示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
本发明采用的传感器主要有：四个环视摄像头、车轮编码器；四个环视摄像头输出的图像进行标定拼接，然后在拼接图上提取语义信息；车轮编码器输出的里程计信息与环视图像提取的特征点信息融合输出更加精准的动态航迹位姿，在动态航迹位姿下构建语义地图；利用语义地图，结合环视语义信息和里程计信息，在停车场内高精度定位。
如图1所示，本发明实施例的新型建图与定位框架包括：
环视标定拼接模块：从布置在车辆前、后及左右外后视镜上的四个鱼眼摄像头采集的数据进行标定和拼接。摄像头标定的主要步骤为内参标定、角点提取、世界坐标系获取、计算RT变化矩阵。内参标定主要采用输入基准内参的方式，在后续外参标定中结合棋盘格的角点对内参进行矫正，缓解相机工艺导致的不同批次相机内参不一致问题。角点提取会先对图像进行透视变换，对透视变换后的图像进行白边处理和自适应阈值调整找到黑白分界线和角点，然后再反透视变换角点，返回角点信息。世界坐标系获取是将角点坐标转换进实际标定场景地图并获得在场景地图坐标。而RT矩阵的获取是通过PnP匹配求解出该角点在图像坐标系到世界坐标系的变化矩阵，其中R是旋转，T是平移关系。对于拼接环节，为提高拼接效果，需要对同一时刻的环视图像进行权重处理，以保证拼接缝处的平滑。
对于环视拼接图像模块出来的拼接图像分两路，一路传给深度学习模型做语义提取使用，一路做特征点提取使用。
深度学习语义提取模块：采用深度学习网络uNet对环视图像进行语义分割提取车库位线及库位箭头信息，用于后续建图与定位模块。
融合里程计：左右车轮编码器输出左右车轮脉冲信息，根据阿克曼转向原理，后轴两轮的速度瞬心相同，则可以建立运动模型推算车辆相对于起始位置的位姿，包括了x，y以及航向角heading。对以下三个公式进行解算Xi＝Xi-1+△Si*sin(heading)；Yi＝Yi-1+△*cos(heading)；heading＝Σ△θ；其中Xi是i时刻的X坐标，Yi是i时刻的Y坐标，heading是航向角信息；通过车辆编码器输出的左右车辆脉冲信息可以实现车辆里程计，进行初步定位。其中，△Si是脉冲区间的行驶距离增量、△θ是脉冲区间的航向角增量。
特征点提取模块：对环视原始图进行预处理-直方图处理，调用ORB特征提取库进行特征提取，并进行畸变矫正，对特征点深度进行评估然后放入追踪函数进行追踪。相机输入的每一帧提取的特征点信息进行追踪输出相机轨迹位姿点；该位姿点信息与上述车辆位姿里程推算进行加权融合输出更加精准的动态航迹位姿。
语义地图模块：在里程计的动态航迹位姿轨迹下实时构建地图(里程计应当连接语义地图)，即得到精确位姿下的建图。通过环视摄像头输入的拼接图像进行语义分割信息提取，语义信息包含车库位线和箭头信息，按照车辆坐标系初始点的位置作为原点进行局部建图并进行定位初始化。每5米建立的局部图像进行语义特征的回环检测，图像进行拼接成为全局地图。建图算法把每个语义位姿作为一系列的随机变量表示地图，算法可以被建模成一个贝叶斯问题，然后使用贝叶斯相关理论，进行近似和后验估计，求得最优全局地图。在建图过程中，表征车辆位姿变换的控制向量u1:t和对环境观测向量z1:t已知，u1:t表示1至t时刻的控制向量，z1:t表示1至t时刻的环境观测向量，而对准确的地图信息m和准确的车辆位姿信息xt是需要求解的。于是建立后验概率来描述P(xt，m|z1:t，u1:t)，表示在t时刻车辆当前位姿xt和m地图最大概率是怎样，对这个概率采用图优化的理论进行解答。车辆位姿x1：t和局部地图m被描述成节点优化变量vi，vi表示i位置的优化变量，vi∈V，V表示节点优化变量的集合；将测量和控制的向量用约束边表示，则构成了一张优化变量与约束的图，用公式J(V)＝Σe[zij-z*ij(vi，vj)]TΩij e[zij-z*ij(vi，vj)]表示，其中e[zij-z*ij(vi，vj)]是损失函数，它描述了观测值zij与预测值z*ij的差异性，而这个差异性可以用信息矩阵Ωij来衡量，z*ij(vi，vj)表示从i位置到j位置的优化变量预测值；这个图优化问题可以用高斯牛顿法来求解，最优的解V*＝argminJ(V)，即求得的V*为最终的估计环境局部地图和车辆位姿。
语义特征融合定位模块：该模块利用语义地图模块生成的全局地图，结合环视语义信息和里程计信息，在停车场内高精度定位的过程。整个模块需要维护的矩阵是E＝[X，Y，θ]，表示车辆在全局地图中的实时位置和航向角。先使用里程计的航迹推算出当前位置的粗定位。然后在地图中车辆半径3米范围内搜索所有语义结果，并与车辆实时感知语义结果做匹配，得到车辆周围每一个感知语义和地图语义信息的位姿差，这个位姿差包含了角度旋转值的差δR和距离差δt，则对于t时刻的位姿可以表示为X*t＝Xt+CRΣδR+CtΣδt，其中CR和Ct为系数，需要实车测量综合得出，X*t是融合定位后t时刻的位姿，Xt是单独里程计粗定位的位姿。
滤波输出三自由度位姿：滤波分为两个步骤，预测和更新；预测过程是从上一个地图匹配的时间t-1得到的地图匹配位姿结果[x，y，head]，结合t时刻的运动模型，得到t时刻的运动模型的预测结果，主要是以上运动状态转移描述过程。更新步骤是根据里程计粗定位结果和环境观测值z1:t，得到精确定位的过程。是以上视觉观测描述转移过程。
本发明对记忆泊车过程中需要使用的SLAM技术进行了算力优化以达到可以量产的可能性。
本领域的技术人员容易理解，以上仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
Claims (10)
1.一种停车库建图与定位方法，其特征在于，包括以下步骤：
利用车辆上的环视摄像头采集停车库图像，并进行摄像头标定和环视图像拼接；
对环视拼接图像进行语义分割提取语义信息，语义信息包括车库位线及库位箭头信息；
对环视摄像头原始图像进行直方图处理，调用ORB特征提取库进行特征提取，并进行畸变矫正，对特征点深度进行评估然后放入追踪函数进行追踪；对环视摄像头输入的每一帧提取的特征点信息进行追踪输出相机轨迹位姿点信息；
左右车轮编码器输出左右车轮脉冲信息，根据阿克曼转向原理，后轴两轮的速度瞬心相同，建立运动模型推算出车辆相对于起始位置的位姿，实现车辆位姿里程推算；
将相机轨迹位姿点信息与车辆位姿里程推算进行加权融合，输出动态航迹位姿；
在动态航迹位姿下实时构建语义地图：以车辆坐标系初始点的位置作为原点进行局部建图并进行定位初始化，特定距离建立的局部图像进行语义特征的回环检测，局部图像进行拼接成为全局地图；
利用全局地图，结合环视语义信息和动态航迹位姿，在停车库进行高精度定位：先使用动态航迹位姿得出车辆当前位置的粗定位，然后在地图中车辆一定半径范围内搜索所有语义结果，并与车辆实时感知语义结果做匹配，得到车辆周围每一个感知语义和地图语义信息的位姿差，该位姿差包含角度旋转值的差δR和距离差δt，则对于t时刻的位姿可以表示为X*t＝Xt+CRΣδR+CtΣδt；其中，CR和Ct为系数，X*t是融合定位后t时刻的位姿，Xt是粗定位的位姿；
滤波输出位姿，滤波分为预测和更新：预测是从上一个地图匹配的时间t-1得到的地图匹配位姿结果[x，y，head]，结合t时刻的运动模型，得到t时刻的运动模型的预测结果；更新是根据粗定位结果和环境观测值，得到精确定位。
2.根据权利要求1所述的停车库建图与定位方法，其特征在于，摄像头标定包括：内参标定、角点提取、世界坐标系获取、计算RT变化矩阵；
内参标定采用输入基准内参的方式，在后续外参标定中结合棋盘格的角点对内参进行矫正；角点提取会先对图像进行透视变换，对透视变换后的图像进行白边处理和自适应阈值调整找到黑白分界线和角点，然后再反透视变换角点，返回角点信息；世界坐标系获取是将角点坐标转换进实际标定场景地图并获得在场景地图坐标；计算RT变化矩阵是通过PnP匹配求解出角点在图像坐标系到世界坐标系的变化矩阵，其中R是旋转，T是平移；
环视图像拼接是对同一时刻采集到的环视图像进行权重处理，以保证拼接缝处的平滑。
3.根据权利要求1所述的停车库建图与定位方法，其特征在于，建立运动模型推算车辆相对于起始位置的位姿，包括位置x和y，以及航向角heading；运动模型如下：
Xi＝Xi-1+△Si*sin(heading)；
Yi＝Yi-1+△Si*cos(heading)；
heading＝Σ△θ；
其中，Xi是i时刻的X坐标，Yi是i时刻的Y坐标，Xi-1是i-1时刻的X坐标，Yi-1是i-1时刻的Y坐标，heading是航向角，△Si是脉冲区间的行驶距离增量，△θ是脉冲区间的航向角增量。
4.根据权利要求1所述的停车库建图与定位方法，其特征在于，在建图过程中，表征车辆位姿变换的控制向量u1:t和对环境观测向量z1:t已知，需要求解准确的地图信息m和准确的车辆位姿信息xt；
建立后验概率P(xt，m|z1:t，u1:t)，该后验概率表示在t时刻车辆当前位姿xt和地图m的最大概率；
对该概率采用图优化的理论进行解答：车辆位姿x1：t和局部地图m被描述成节点优化变量vi，vi∈V，V表示节点优化变量集合；
将环境观测向量和辆位姿变换的控制向量用约束边表示，则构成了一张优化变量与约束的图，用公式J(V)＝Σe[zij-z*ij(vi，vj)]TΩije[zij-z*ij(vi，vj)]表示；其中，e[zij-z*ij(vi，vj)]是损失函数，描述观测值zij与预测值z*ij的差异性，此差异性用信息矩阵Ωij来衡量；
该图优化问题用高斯牛顿法来求解，最优的解V*＝argminJ(V)，即求得的V*为最终的估计环境局部地图和车辆位姿。
5.根据权利要求1所述的停车库建图与定位方法，其特征在于，利用深度学习网络uNet对拼接后的环视图像进行语义分割。
6.一种停车库建图与定位系统，其特征在于，包括：
环视标定拼接模块，用于利用车辆上的环视摄像头采集停车库图像，并进行摄像头标定和环视图像拼接；
语义提取模块，用于对环视拼接图像进行语义分割提取语义信息，语义信息包括车库位线及库位箭头信息；
特征点提取模块，用于对环视摄像头原始图像进行直方图处理，调用ORB特征提取库进行特征提取，并进行畸变矫正，对特征点深度进行评估然后放入追踪函数进行追踪；对环视摄像头输入的每一帧提取的特征点信息进行追踪输出相机轨迹位姿点信息；
车轮编码器，用于输出左右车轮脉冲信息，根据阿克曼转向原理，后轴两轮的速度瞬心相同，建立运动模型推算出车辆相对于起始位置的位姿，实现车辆位姿里程推算；
融合里程计，用于将相机轨迹位姿点信息与车辆位姿里程推算进行加权融合，输出动态航迹位姿；
语义地图模块，用于在动态航迹位姿下实时构建语义地图：以车辆坐标系初始点的位置作为原点进行局部建图并进行定位初始化，特定距离建立的局部图像进行语义特征的回环检测，局部图像进行拼接成为全局地图；
语义特征融合定位模块，用于利用全局地图，结合环视语义信息和动态航迹位姿，在停车库进行高精度定位：先使用动态航迹位姿得出车辆当前位置的粗定位，然后在地图中车辆一定半径范围内搜索所有语义结果，并与车辆实时感知语义结果做匹配，得到车辆周围每一个感知语义和地图语义信息的位姿差，该位姿差包含角度旋转值的差δR和距离差δt，则对于t时刻的位姿可以表示为X*t＝Xt+CRΣδR+CtΣδt；其中，CR和Ct为系数，X*t是融合定位后t时刻的位姿，Xt是粗定位的位姿；
滤波输出位姿模块，用于滤波输出位姿，滤波分为预测和更新：预测是从上一个地图匹配的时间t-1得到的地图匹配位姿结果[x，y，head]，结合t时刻的运动模型，得到t时刻的运动模型的预测结果；更新是根据粗定位结果和环境观测值，得到精确定位。
7.根据权利要求6所述的停车库建图与定位系统，其特征在于，摄像头标定包括：内参标定、角点提取、世界坐标系获取、计算RT变化矩阵；
内参标定采用输入基准内参的方式，在后续外参标定中结合棋盘格的角点对内参进行矫正；角点提取会先对图像进行透视变换，对透视变换后的图像进行白边处理和自适应阈值调整找到黑白分界线和角点，然后再反透视变换角点，返回角点信息；世界坐标系获取是将角点坐标转换进实际标定场景地图并获得在场景地图坐标；计算RT变化矩阵是通过PnP匹配求解出角点在图像坐标系到世界坐标系的变化矩阵，其中R是旋转，T是平移；
环视图像拼接是对同一时刻采集到的环视图像进行权重处理，以保证拼接缝处的平滑。
8.根据权利要求6所述的停车库建图与定位系统，其特征在于，建立运动模型推算车辆相对于起始位置的位姿，包括位置x和y，以及航向角heading；运动模型如下：
Xi＝Xi-1+△Si*sin(heading)；
Yi＝Yi-1+△Si*cos(heading)；
heading＝Σ△θ；
其中，Xi是i时刻的X坐标，Yi是i时刻的Y坐标，Xi-1是i-1时刻的X坐标，Yi-1是i-1时刻的Y坐标，heading是航向角，△Si是脉冲区间的行驶距离增量，△θ是脉冲区间的航向角增量。
9.根据权利要求6所述的停车库建图与定位系统，其特征在于，在建图过程中，表征车辆位姿变换的控制向量u1:t和对环境观测向量z1:t已知，需要求解准确的地图信息m和准确的车辆位姿信息xt；
建立后验概率P(xt，m|z1:t，u1:t)，该后验概率表示在t时刻车辆当前位姿xt和地图m的最大概率；
对该概率采用图优化的理论进行解答：车辆位姿x1：t和局部地图m被描述成节点优化变量vi，vi∈V，V表示节点优化变量集合；
将环境观测向量和辆位姿变换的控制向量用约束边表示，则构成了一张优化变量与约束的图，用公式J(V)＝Σe[zij-z*ij(vi，vj)]TΩije[zij-z*ij(vi，vj)]表示；其中，e[zij-z*ij(vi，vj)]是损失函数，描述观测值zij与预测值z*ij的差异性，此差异性用信息矩阵Ωij来衡量；
该图优化问题用高斯牛顿法来求解，最优的解V*＝argminJ(V)，即求得的V*为最终的估计环境局部地图和车辆位姿。
10.一种车辆，其特征在于，该车辆采用权利要求1至5中任意一项所述的停车库建图与定位方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210269955.1A CN114693787A (zh) | 2022-03-18 | 2022-03-18 | 停车库建图与定位方法、系统及车辆 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210269955.1A CN114693787A (zh) | 2022-03-18 | 2022-03-18 | 停车库建图与定位方法、系统及车辆 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114693787A true CN114693787A (zh) | 2022-07-01 |
Family
ID=82139759
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210269955.1A Pending CN114693787A (zh) | 2022-03-18 | 2022-03-18 | 停车库建图与定位方法、系统及车辆 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114693787A (zh) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115297308A (zh) * | 2022-07-29 | 2022-11-04 | 东风汽车集团股份有限公司 | 基于无人机的环绕ar-hud投影系统和方法 |
| CN115388880A (zh) * | 2022-10-27 | 2022-11-25 | 联友智连科技有限公司 | 一种低成本记忆泊车建图与定位方法、装置及电子设备 |
| CN115435795A (zh) * | 2022-09-21 | 2022-12-06 | 苏州轻棹科技有限公司 | 一种基于环视图像和语义地图的车辆定位方法 |
| CN115601271A (zh) * | 2022-11-29 | 2023-01-13 | 上海仙工智能科技有限公司（Cn） | 一种视觉信息防抖方法、仓储库位状态管理方法及系统 |
-
2022
- 2022-03-18 CN CN202210269955.1A patent/CN114693787A/zh active Pending
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115297308A (zh) * | 2022-07-29 | 2022-11-04 | 东风汽车集团股份有限公司 | 基于无人机的环绕ar-hud投影系统和方法 |
| CN115297308B (zh) * | 2022-07-29 | 2023-05-26 | 东风汽车集团股份有限公司 | 基于无人机的环绕ar-hud投影系统和方法 |
| CN115435795A (zh) * | 2022-09-21 | 2022-12-06 | 苏州轻棹科技有限公司 | 一种基于环视图像和语义地图的车辆定位方法 |
| CN115435795B (zh) * | 2022-09-21 | 2024-05-03 | 苏州轻棹科技有限公司 | 一种基于环视图像和语义地图的车辆定位方法 |
| CN115388880A (zh) * | 2022-10-27 | 2022-11-25 | 联友智连科技有限公司 | 一种低成本记忆泊车建图与定位方法、装置及电子设备 |
| CN115601271A (zh) * | 2022-11-29 | 2023-01-13 | 上海仙工智能科技有限公司（Cn） | 一种视觉信息防抖方法、仓储库位状态管理方法及系统 |
| CN115601271B (zh) * | 2022-11-29 | 2023-03-24 | 上海仙工智能科技有限公司 | 一种视觉信息防抖方法、仓储库位状态管理方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111595333B (zh) | 视觉惯性激光数据融合的模块化无人车定位方法及系统 | |
| CN109945858B (zh) | 用于低速泊车驾驶场景的多传感融合定位方法 | |
| CN114693787A (zh) | 停车库建图与定位方法、系统及车辆 | |
| CN106679648B (zh) | 一种基于遗传算法的视觉惯性组合的slam方法 | |
| CN108534782B (zh) | 一种基于双目视觉系统的地标地图车辆即时定位方法 | |
| CN113269837B (zh) | 一种适用于复杂三维环境的定位导航方法 | |
| CN110842940A (zh) | 一种建筑测量机器人多传感器融合三维建模方法及系统 | |
| CN111060924B (zh) | 一种slam与目标跟踪方法 | |
| CN110569704A (zh) | 一种基于立体视觉的多策略自适应车道线检测方法 | |
| CN109523589B (zh) | 一种更鲁棒的视觉里程计的设计方法 | |
| Berrio et al. | Camera-LIDAR integration: Probabilistic sensor fusion for semantic mapping | |
| CN112734841B (zh) | 一种用轮式里程计-imu和单目相机实现定位的方法 | |
| CN112734765B (zh) | 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质 | |
| CN113220818B (zh) | 一种停车场自动建图与高精度定位方法 | |
| CN113903011A (zh) | 一种适用于室内停车场的语义地图构建及定位方法 | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| CN116222543B (zh) | 用于机器人环境感知的多传感器融合地图构建方法及系统 | |
| CN112669354A (zh) | 一种基于车辆非完整约束的多相机运动状态估计方法 | |
| CN112767546B (zh) | 移动机器人基于双目图像的视觉地图生成方法 | |
| CN111998862A (zh) | 一种基于bnn的稠密双目slam方法 | |
| CN116518984B (zh) | 一种煤矿井下辅助运输机器人车路协同定位系统及方法 | |
| CN112136021A (zh) | 用于构建基于地标的高清地图的系统和方法 | |
| CN114325634A (zh) | 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法 | |
| CN113781562A (zh) | 一种基于道路模型的车道线虚实配准和自车定位方法 | |
| CN117367427A (zh) | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 |
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 |