一种基于车辆视觉和运动信息的停车场自主泊车定位方法技术

本发明专利技术涉及一种基于车辆视觉和运动信息的停车场自主泊车定位方法，包括以下步骤：1)位于地下车库的智能车通过车载双目摄像头采集图像数据，并采用基于双目视觉的同步定位与建图方法获取智能车在局部点云地图中的位置信息，完成一次定位；2)智能车在行驶过程中不断对车身周围的库位角点进行检测，判断是否经过库位；3)在检测到库位后，判断当前库位书否为可泊入库位；4)根据库位角点坐标以及车速和方向盘转角信息，采用扩展卡尔曼滤波进行融合，得到智能车相对于库位角点的精确相对位置，完成二次定位。与现有技术相比，本发明专利技术具有适应无GNSS信号环境、实时性和鲁棒性高、定位精度高、成本低、实用性强等优点。

【技术实现步骤摘要】
一种基于车辆视觉和运动信息的停车场自主泊车定位方法
本专利技术涉及车辆自主泊车
，尤其是涉及一种基于车辆视觉和运动信息的停车场自主泊车定位方法。
技术介绍
近年来，自动驾驶相关技术发展迅速，已经成为投资和科研的热点。而自主泊车技术由于可以消除繁琐的泊车操作、复杂的泊车环境对驾驶员所造成的困扰，已经成为自动驾驶技术落地和产业化的最佳途径之一，吸引了许多企业和高校投入大量资金进行研发。然而，为了空间的充分应用，越来越多的停车场建在地下。由于目前的智能汽车定位大都依赖GNSS，在GNSS信号缺失的地下停车场通常必须依靠自车传感器进行定位。现有的室内停车场定位方案主要依靠无线设备例如WiFi或UWB等，其一方面需要布设大量基站设备，成本高昂；另一方面定位精度(米级)难以满足无人驾驶需求。而基于激光雷达的室内SLAM方案成本太高，难以在量产车型中装备。此外，泊入库位过程中自车相对于库位的位置的确定一般仅依赖于航位推算，累计误差大，导致车辆最终姿态与理想姿态偏差较大。上述缺陷均是自主泊车系统普及过程中亟需解决的问题。
技术实现思路
本专利技术的目的就是为了克服上述现有技术存在的缺陷而提供一种基于车辆视觉和运动信息的停车场自主泊车定位方法。本专利技术的目的可以通过以下技术方案来实现：一种基于车辆视觉和运动信息的停车场自主泊车定位方法，用以在GNSS信号缺失的地下停车场通过前视、环视相机以及车辆底盘信息实现定位，包括以下步骤：1)位于地下车库的智能车通过车载双目摄像头采集图像数据，并采用基于双目视觉的同步定位与建图方法获取智能车在局部点云地图中的位置信息，完成一次定位；2)智能车在行驶过程中不断对车身周围的库位角点进行检测，判断是否经过库位；3)在检测到库位后，将车辆坐标系下的库位坐标转化到局部点云地图坐标系中，按照标准库位空间大小计算库位范围内地图点的数目，若该数目超过设定阈值，则表示当前库位已被占据，不可泊入，若该数目小于设定阈值，则表示当前库位为可泊入库位；4)在检测到可泊入库位后，智能车根据库位角点坐标以及车速和方向盘转角信息，采用扩展卡尔曼滤波进行融合，得到智能车相对于库位角点的精确相对位置，完成二次定位。所述的步骤1)具体包括以下步骤：11)通过设置在后视镜后不锈钢横梁上的前视双目相机获取车辆前方的双目图像信息，并采用棋盘格内参标定法对前视双目相机进行标定，获取两个前视单目相机的内参和双目相机的基线距离；12)在双目图像中提取候选点，完成双目匹配，建立视差图；13)在候选点中选取激活点，计算激活点的深度，从而获得深度的初步估计，将此深度作为先验信息，通过帧间匹配获取后续帧中激活点的深度信息，并利用像素坐标和深度值建立半稠密点云地图；14)将选取的激活点投影至相邻帧，通过非线性最小二乘法最小化光度误差获得对后续帧的位姿估计。所述的步骤13)中，激活点的深度z的计算式为：其中，f为相机焦距，b为双目基线距离，d为视差。所述的步骤2)具体包括以下步骤：21)通过鱼眼环视相机获取车辆周围的环视图像信息，并通过棋盘格内参标定法对鱼眼相机的内参数进行标定，通过地面标志点对鱼眼相机的外参数进行标定，所述的鱼眼环视相机设有4个，分别设置在车辆前后保险杠以及左右后视镜处；22)从环视图像中提取库位入口的两个角点，并获取库位角点在车辆坐标系中的位置坐标。所述的步骤3)具体包括以下步骤：31)利用车辆在局部点云地图中的当前位姿，将车辆坐标系下库位入口角点坐标转化到局部点云地图坐标系下；32)根据标准库位长宽高信息结合库位入口角点构建用以表示库位空间的长方体空间，统计局部点云地图中该库位空间中的点的数目，如果大于设定阈值则表示该库位被占据，否则，则发出可泊入信息。所述的步骤12)具体包括以下步骤：121)选择候选点：将双目图像分成多个32×32像素的块，并在每个块中选择像素梯度大于随区域变化的像素梯度阈值的所有像素点作为候选点；122)双目匹配建立视差图并计算视差d，具体为：计算左目图像I1待匹配像素和右目图像I2中匹配点的归一化互相关性值NCC，在右目图像中的水平极线上进行搜索，当NCC值最大时对应的点即为匹配点，并且获取视差d，所述的归一化互相关性值NCC的计算式为：其中，I1(x,y)为左目图像中(x,y)处的像素值，为左目图像待匹配像素(px,py)所在匹配窗口内所有像素的均值，(px,py)为左目图像待匹配像素坐标，(px+d,py)为右目图像匹配点坐标，I2(x+d,y)为右目图像中(x+d,y)处的像素值，为右目图像匹配点(px+d,py)所在匹配窗口内所有像素的均值，Wp为以待匹配像素坐标为中心的匹配窗口，通常为3×3匹配窗口。所述的步骤14)具体为：图像帧i遍历所有用于计算的帧集合F，像素点p遍历图像帧i中的点集合Pi，图像帧j遍历所有帧中能够观测到点p的对应点p′，总光度误差Ephoto为当前帧位姿的函数，通过非线性最小二乘法的数值计算方法高斯-牛顿法最小化总光度误差进行估计位姿，所述的总光度误差Ephoto的计算式为：Epj＝wpLγ(Ij[p′]-Ii[p])其中，Epj为像素点p与其在第j帧中对应投影点的光度误差，Ii,Ij分别表示第i帧图像和第j帧图像，wp为与强度梯度有关的加权系数，Lγ(I1,I2)为Huber函数，c为常数，为第i帧图像中像素点p处的像素梯度，Ij[p′]为第j帧图像中像素点p′处的像素强度，Ii[p]为第i帧图像中像素点p处的像素强度。所述的步骤31)中，库位角点在局部点云地图中的坐标(x′,y′)的转化式为：其中，θ为车辆坐标系下的库位观测角，(x,y)为车辆在局部点云地图中的坐标，为航向角，(Lot.x,Lot.y)为车辆坐标系下库位角点坐标，r为相对车辆距离。所述的步骤4)具体包括以下步骤：41)车辆开始泊入库位时，通过读取CAN总线上的车辆底盘信息并进行解析获取车辆的实时速度和方向盘转角信息；42)将实时库位角点位置信息以及车速和方向盘转角输入至扩展卡尔曼滤波器进行融合，通过计算欧式距离判断是否为同一库位，对重复观测的库位位置和车辆在局部坐标系下的位姿进行闭环优化，消除累积噪声，最终得到高精度的车辆位姿和库位位置。所述的步骤42)中，每一时刻获得的库位角点坐标和车辆底盘信息均含有测量误差，车辆和库位所处位置为待估计量，采用扩展卡尔曼滤波对进行描述，具体为：421)定义状态空间转移方程和状态空间的观测方程为：Xk＝f(Xk-1,Uk-1,Wk-1)Zk＝h(Xk,Vk)其中，k为当前时刻，k-1为上一时刻，X为包含车辆位姿及库位角点坐标的状态向量，U表示输入的车辆底盘数据，Z中包含了车辆对库位角点坐标的观测，W为系统噪声向量，f()为状态空间转移方程，h()为状态空间观测方程；422)扩展卡尔曼滤波的具体过程为：预测更新：Xk＝f(Xk-1,Uk-1,Wk-1)P(k)-＝F(k)P(k-1)F(k)T+Q测量更新：K(k)＝P(k)-H(k)T(H(k)P(k)-H(k)T+R)-1P(k)＝(I-K(k)H(k))P(k)-其中，P为误差协方差矩阵，K为卡尔曼增益矩阵，F为f()的雅可比矩阵，H为h()的雅可比矩阵，Q和R分别为系统噪声协方差阵和观测噪声矩阵协方差本文档来自技高网...

【技术保护点】
1.一种基于车辆视觉和运动信息的停车场自主泊车定位方法，用以在GNSS信号缺失的地下停车场通过前视、环视相机以及车辆底盘信息实现定位，其特征在于，包括以下步骤：1)位于地下车库的智能车通过车载双目摄像头采集图像数据，并采用基于双目视觉的同步定位与建图方法获取智能车在局部点云地图中的位置信息，完成一次定位；2)智能车在行驶过程中不断对车身周围的库位角点进行检测，判断是否经过库位；3)在检测到库位后，将车辆坐标系下的库位坐标转化到局部点云地图坐标系中，按照标准库位空间大小计算库位范围内地图点的数目，若该数目超过设定阈值，则表示当前库位已被占据，不可泊入，若该数目小于设定阈值，则表示当前库位为可泊入库位；4)在检测到可泊入库位后，智能车根据库位角点坐标以及车速和方向盘转角信息，采用扩展卡尔曼滤波进行融合，得到智能车相对于库位角点的精确相对位置，完成二次定位。

【技术特征摘要】
1.一种基于车辆视觉和运动信息的停车场自主泊车定位方法，用以在GNSS信号缺失的地下停车场通过前视、环视相机以及车辆底盘信息实现定位，其特征在于，包括以下步骤：1)位于地下车库的智能车通过车载双目摄像头采集图像数据，并采用基于双目视觉的同步定位与建图方法获取智能车在局部点云地图中的位置信息，完成一次定位；2)智能车在行驶过程中不断对车身周围的库位角点进行检测，判断是否经过库位；3)在检测到库位后，将车辆坐标系下的库位坐标转化到局部点云地图坐标系中，按照标准库位空间大小计算库位范围内地图点的数目，若该数目超过设定阈值，则表示当前库位已被占据，不可泊入，若该数目小于设定阈值，则表示当前库位为可泊入库位；4)在检测到可泊入库位后，智能车根据库位角点坐标以及车速和方向盘转角信息，采用扩展卡尔曼滤波进行融合，得到智能车相对于库位角点的精确相对位置，完成二次定位。2.根据权利要求1所述的一种基于车辆视觉和运动信息的停车场自主泊车定位方法，其特征在于，所述的步骤1)具体包括以下步骤：11)通过设置在后视镜后不锈钢横梁上的前视双目相机获取车辆前方的双目图像信息，并采用棋盘格内参标定法对前视双目相机进行标定，获取两个前视单目相机的内参和双目相机的基线距离；12)在双目图像中提取候选点，完成双目匹配，建立视差图；13)在候选点中选取激活点，计算激活点的深度，从而获得深度的初步估计，将此深度作为先验信息，通过帧间匹配获取后续帧中激活点的深度信息，并利用像素坐标和深度值建立半稠密点云地图；14)将选取的激活点投影至相邻帧，通过非线性最小二乘法最小化光度误差获得对后续帧的位姿估计。3.根据权利要求2所述的一种基于车辆视觉和运动信息的停车场自主泊车定位方法，其特征在于，所述的步骤13)中，激活点的深度z的计算式为：其中，f为相机焦距，b为双目基线距离，d为视差。4.根据权利要求1所述的一种基于车辆视觉和运动信息的停车场自主泊车定位方法，其特征在于，所述的步骤2)具体包括以下步骤：21)通过鱼眼环视相机获取车辆周围的环视图像信息，并通过棋盘格内参标定法对鱼眼相机的内参数进行标定，通过地面标志点对鱼眼相机的外参数进行标定，所述的鱼眼环视相机设有4个，分别设置在车辆前后保险杠以及左右后视镜处；22)从环视图像中提取库位入口的两个角点，并获取库位角点在车辆坐标系中的位置坐标。5.根据权利要求1所述的一种基于车辆视觉和运动信息的停车场自主泊车定位方法，其特征在于，所述的步骤3)具体包括以下步骤：31)利用车辆在局部点云地图中的当前位姿，将车辆坐标系下库位入口角点坐标转化到局部点云地图坐标系下；32)根据标准库位长宽高信息结合库位入口角点构建用以表示库位空间的长方体空间，统计局部点云地图中该库位空间中的点的数目，如果大于设定阈值则表示该库位被占据，否则，则发出可泊入信息。6.根据权利要求2所述的一种基于车辆视觉和运动信息的停车场自主泊车定位方法，其特征在于，所述的步骤12)具体包括以下步骤：121)选择候选点：将双目图像分成多个32×32像素的块，并在每个块中选择像素梯度大于随区域变化的像素梯度阈值的所有像素点作为候选点；122)双目匹配建立视差图并计算视差d，具体为：计算左目图像I1待匹配像素和右目图像I2中匹配点的归一化互相关性值NCC，在右目图像中的水平极线上进行搜索，当NCC值最大时对应的点即为匹配点，并且获取视差d，所述的归一化互相关性值NCC的计算式为：其中，I1(x,y)为左...

【专利技术属性】
技术研发人员：熊璐，康戎，张培志，许明煜，严森炜，李志强，
申请(专利权)人：同济大学，
类型：发明
国别省市：上海,31