CN113753029A - 一种基于光流法的自动泊车方法及系统 - Google Patents

Abstract

本发明涉及自动泊车技术领域，提供一种基于光流法的自动泊车方法及系统，根据实际情况中产生泊车误差的车辆速度设置预设阈值，在车辆速度高于预设阈值时，采用常规的轮速传感器检测车辆的运动速度，可在全局坐标系中快速确定其运动位置；在车辆速度低于预设阈值时，则同时根据车载图像传感器和轮速传感器采集得到的数据，分别进行计算获取到的泊车速度(第一泊车速度、第二泊车速度)和位置坐标(第一位置坐标、第二位置坐标)，最后代入采用卡尔曼滤波算法，对低速情况下的车辆的泊车速度和位置坐标进行修正，将传感器累计误差降低至最小，使得自动泊车定位更加精准，泊车效果更优。

Description

一种基于光流法的自动泊车方法及系统

技术领域

本发明涉及自动泊车技术领域，尤其涉及一种基于光流法的自动泊车方法及系统。

背景技术

不同的自动泊车系统采用不同的方法来检测汽车周围的物体。有些在汽车前后保险杠四周装上了感应器，它们既可以充当发送器，也可以充当接收器。这些感应器会发送信号，当信号碰到车身周边的障碍物时会反射回来。然后，车上的计算机会利用其接收信号所需的时间来确定障碍物的位置。其他一些系统则使用安装在保险杠上的摄像头或雷达来检测障碍物。但都存在相同的最终目的：汽车根据检测到已停好的车辆、停车位的大小以及与路边的距离，规划泊车路线驶入停车位。

根据历史数据分析可知，在使用自动泊车系统进行泊车的实际过程中，泊车系统的车身定位信息的准确度会直接影响泊车效果。因此，在自动泊车系统中，车身定位精度举足轻重。但是，目前泊车系统使用的车身定位信息是由车身传感器获得，而在启动泊车到完成泊车的过程中，传感器累计误差会越来越大，从而导致自动泊车出现较大的误差，导致泊车失败并产生刮蹭或者碰撞等事故。

发明内容

本发明提供一种基于光流法的自动泊车方法及系统，解决了现有的自动泊车技术局限与传感器的累计误差，无法有效确定车身定位信息，导致的泊车失败并产生刮蹭或者碰撞等事故的技术问题。

为解决以上技术问题，本发明提供一种基于光流法的自动泊车方法，包括步骤：

S1、根据当前环境数据，建立全局坐标系；

S2、识别所述当前环境数据，确定目标车位；

S3、获取本车的当前行车状态，并根据预设规则计算对应的泊车速度和本车当前的位置坐标；

S4、根据所述目标车位、所述泊车速度和位置坐标，规划泊车路径并执行。

本基础方案基于自动泊车中车位静止的特点，根据当前环境数据，建立静止的全局坐标系，在识别并确定目标车位后，根据本车实际的行车状态(当前行车状态)，进入预先设置好的预设规则，计算出本车实际的泊车速度和位置坐标，从而提高车辆泊车精度，避免传感器误差导致的泊车失败。

在进一步的实施方案中，所述步骤S3包括：

S31、获取本车的当前运动车速，判断所述当前运动车速是否大于预设阈值，若是在进入步骤S32，若否则进入步骤S33；

S32、根据轮速传感器的轮速数据计算本车的泊车速度，并根据所述泊车速度计算本车当前的位置坐标；

S33、采用光流法检测车载图像传感器的瞬时速度，并根据所述瞬时速度计算本车的第一泊车速度，进而计算本车当前的第一位置坐标；根据轮速传感器的采集数据计算本车的第二泊车速度，进而计算本车当前的第二位置坐标；

S34、根据卡尔曼滤波算法，代入所述第一泊车速度、第二泊车速度、所述第一位置坐标、所述第二位置坐标，计算本车的泊车速度和本车当前的位置坐标。

本方案根据实际情况中产生泊车误差的车辆速度设置预设阈值，在车辆速度高于预设阈值时，采用常规的轮速传感器检测车辆的运动速度，可在全局坐标系中快速确定其运动位置；在车辆速度低于预设阈值时，则同时根据车载图像传感器和轮速传感器采集得到的数据，分别进行计算获取到的泊车速度(第一泊车速度、第二泊车速度)和位置坐标(第一位置坐标、第二位置坐标)，最后代入采用卡尔曼滤波算法，对低速情况下的车辆的泊车速度和位置坐标进行修正，将传感器累计误差降低至最小，使得自动泊车定位更加精准，泊车效果更优。

在进一步的实施方案中，所述步骤S34包括：

A、根据所述第二泊车速度、所述第二位置坐标和初始参数，计算卡尔曼增益；

B、根据所述卡尔曼增益和第一泊车速度、第一位置坐标，结合第一算法计算本车的所述泊车速度和本车当前的所述位置坐标；

C、根据第二算法、所述卡尔曼增益，更新所述初始参数；

所述初始参数为，根据卡尔曼滤波计算的所述泊车速度、所述位置坐标的协方差矩阵。

本方案在根据计算出当前帧图像中本车的泊车速度、位置坐标后，根据本次计算得到的卡尔曼增益更新初始参数，由于前后帧图像的相似度较高，因此可直接将更新后的初始参数应用到下一帧图像的计算中，从而可降低计算误差。

在进一步的实施方案中，在所述步骤B中，所述第一算法如下：

Xk＝Xk’+K′(Zk-HkXk’)

其中，Xk表示当前帧的所述泊车速度、所述位置坐标；Xk’表示上一帧计算得到所述泊车速度、所述位置坐标；Zk表示所述第一泊车速度、第一位置坐标；HkXk’表示所述第二泊车速度、所述第二位置坐标；Zk-HkXk’表示测量残差。

在进一步的实施方案中，所述步骤S1包括：

S11、获取当前环境数据，确定车辆泊车起点；

S12、以在所述车辆泊车起点上本车的车身后轴中心为原点，结合所述当前环境数据建立全局坐标系。

在进一步的实施方案中，所述步骤S2包括：

S21、识别所述当前环境数据，建立车身坐标系，并获取周围的车位线数据、空间车位数据、障碍物信息；

S22、根据所述车位线数据、空间车位数据、障碍物信息，确定目标车位。

在进一步的实施方案中，所述步骤S22包括：

a、当所述车位线数据为无数据时，根据空间车位数据和所述障碍物信息确定目标车位；

b、当所述空间车位数据为无数据时，根据车位线数据和所述障碍物信息确定目标车位；

c、融合所述车位线数据、空间车位数据，将重叠的融合车位作为目标车位，并确定其车位坐标。

本方案考虑到泊车环境的多样性，在进行车位检测时，同时对车辆周围的车位线数据和空间车位数据进行获取，当无车位线数据时，判断为无有线车位，则根据空间车位数据和障碍物信息确定目标车位；当仅检测到有线车位时(当所述空间车位数据为无数据时)，进一步获取障碍物信息确定是否存在可泊车车位作为目标车位；当车位线数据和空间车位数据均存在时，进行数据融合，将重叠的融合车位作为目标车位，本方案通过情景预设覆盖用户可能遇到的所有泊车情况，从而给予用户良好的泊车体验。

在进一步的实施方案中，所述步骤S4包括：

S41、根据所述全局坐标系和所述车身坐标系的比例关系，将所述车位坐标转换为全局坐标；

S42、根据所述目标车位的全局坐标、所述泊车速度和所述位置坐标，规划泊车路径并执行。

本发明提供一种基于光流法的自动泊车系统，包括：路径规划模块和与其连接的车位检测模块、车身状态确定模块、泊车执行模块；所述车位检测模块包括线车位检测模块、空间车位检测模块；

所述线车位检测模块用于识别当前环境数据，获取车位线数据；

所述空间车位检测模块用于识别车身周围的障碍物，获取空间车位数据和障碍物信息；

所述车身状态确定模块用于获取本车的当前行车状态，并根据预设规则计算对应的泊车速度和本车当前的位置坐标；

所述路径规划模块用于，根据所述车位线数据、空间车位数据、障碍物信息确定目标车位，进而规划泊车路径；

所述泊车执行模块用于执行所述泊车路径。

在进一步的实施方案中，所述车身状态确定模块包括主控模块，和与所述主控模块连接的方向盘传感器、轮速传感器、光流检测模块；

所述方向盘传感器用于根据方向盘转动角度确定本车车轮的转角度数；

所述轮速传感器用于获取本车车轮的轮速数据；

所述光流检测模块用于，根据L-K光流法和从车载图像传感器中获取的运动图像，计算对应的瞬时速度；

所述主控模块用于在判断到当前运动车速大于预设阈值时，根据所述轮速数据计算本车的泊车速度，并根据所述泊车速度和所述转角度数计算本车当前的位置坐标；否则，根据卡尔曼滤波算法以及所述轮速数据、所述瞬时速度、所述转角度数，计算本车的泊车速度和当前的位置坐标。

本方案在现有的车载设备基础上，增加光流检测模块，在车辆低速时，根据L-K光流法和从车载图像传感器中获取的运动图像，计算对应的瞬时速度；从而与原来的轮速传感器获取的轮速数据集合，根据卡尔曼滤波算法计算本车的泊车速度和当前的位置坐标，从而降低自动泊车过程中的累计误差，避免泊车目标位置和停车位置的偏差，提高自动泊车准确率。

附图说明

图1是本发明实施例1提供的一种基于光流法的自动泊车方法的工作流程图图；

图2是本发明实施例1提供的全局坐标系的示意图；

图3是本发明实施例1提供的空间车位的识别示意图；

图4是本发明实施例1提供的有线车位的识别示意图；

图5是本发明实施例2提供的一种基于光流法的自动泊车系统的系统框架图。

具体实施方式

下面结合附图具体阐明本发明的实施方式，实施例的给出仅仅是为了说明目的，并不能理解为对本发明的限定，包括附图仅供参考和说明使用，不构成对本发明专利保护范围的限制，因为在不脱离本发明精神和范围基础上，可以对本发明进行许多改变。

实施例1

本发明实施例提供的一种基于光流法的自动泊车方法，如图1所示，在本实施例中，包括步骤S1～S4：

S1、根据当前环境数据，建立全局坐标系，包括步骤S11～S12：

S11、获取当前环境数据，确定车辆泊车起点；

S12、以在车辆泊车起点上本车的车身后轴中心为原点，结合当前环境数据建立全局坐标系，例如以有线车位的长、宽方向为X轴、Y轴方向，或者以车道径向、横向为X轴、Y轴方向，参见图2，其中S为车辆历史行驶轨迹，F为为车辆前进方向，车辆后轴中心坐标为(x_R,y_R)。

S2、识别所述当前环境数据，确定目标车位，包括步骤S21～S22：

S21、识别当前环境数据，建立车身坐标系，并获取周围的车位线数据、空间车位数据、障碍物信息；

S22、根据车位线数据、空间车位数据、障碍物信息，确定目标车位，包括步骤a～c：

a、当车位线数据为无数据、只检测到空间车位数据时，根据空间车位数据和障碍物信息确定目标车位；

具体地，参见图3，当仅检测到空间车位(即检测到空间车位数据)时，且判断到检测到的空间车位的空间大于车身的长宽、该空间车位内没有障碍物，判断为满足泊车要求，则将对应空间车位作为目标车位，以空间车位4个角点的坐标为目标车位坐标。

b、当空间车位数据为无数据、只检测到车位线数据时，根据车位线数据和障碍物信息确定目标车位。

具体地，参见图4，当检测到有线车位(即检测到车位线数据)，进一步根据障碍物信息判断线车位里有没有障碍物，如果没有障碍物，则以有线车位的4个角点坐标为目标车位坐标，如果有障碍物，则判断该线车位为无效车位。

c、融合车位线数据、空间车位数据，将重叠的融合车位作为目标车位，并确定其车位坐标。

具体地，融合车位线数据、空间车位数据，将车身坐标系下的目标车位的4个车位坐标进行数据融合，判断有线车位和空间车位的车位坐标是否重叠，如果重叠则判断该车位为目标车位，否则判断该车位为无效车位。

本实施例考虑到泊车环境的多样性，在进行车位检测时，同时对车辆周围的车位线数据和空间车位数据进行获取，当无车位线数据时，判断为无有线车位，则根据空间车位数据和障碍物信息确定目标车位；当仅检测到有线车位时(当空间车位数据为无数据时)，进一步获取障碍物信息确定是否存在可泊车车位作为目标车位；当车位线数据和空间车位数据均存在时，进行数据融合，将重叠的融合车位作为目标车位，本实施例通过情景预设覆盖用户可能遇到的所有泊车情况，从而给予用户良好的泊车体验。

S3、获取本车的当前行车状态，并根据预设规则计算对应的泊车速度和本车当前的位置坐标，包括步骤S31～S34：

S31、获取本车的当前运动车速，判断当前运动车速是否大于预设阈值，若是在进入步骤S32，若否则进入步骤S33；

在本实施例中，预设阈值优选为4km/h。

S32、根据轮速传感器的轮速数据计算本车的泊车速度，并根据泊车速度计算本车当前的位置坐标；

S33、采用光流法检测车载图像传感器的瞬时速度，并根据瞬时速度计算本车的第一泊车速度V₁₁，进而计算本车当前的第一位置坐标P₁₁；根据轮速传感器的采集数据计算本车的第二泊车速度V₁₂，进而计算本车当前的第二位置坐标P₁₂；

S34、根据卡尔曼滤波算法，代入第一泊车速度V₁₁、第二泊车速度V₁₂、第一位置坐标P₁₁、第二位置坐标P₁₂，计算本车的泊车速度和本车当前的位置坐标，包括步骤A～C：

A、根据第二泊车速度V₁₂、第二位置坐标P₁₂和初始参数，计算卡尔曼增益；

在本实施例中，初始参数为，根据卡尔曼滤波计算的泊车速度、位置坐标的协方差矩阵。卡尔曼增益的计算公式如下：

K′＝PkHkT(HkPkHkT+Rk)-1

其中，Pk表示通过卡尔曼滤波计算出的泊车速度、位置坐标的协方差矩阵，Rk表示根据第二泊车速度V₁₂、第二位置坐标P₁₂计算出的协方差矩阵，Hk表示协方差矩阵。

B、根据卡尔曼增益和第一泊车速度V₁₁、第一位置坐标P₁₁，结合第一算法计算本车的泊车速度和本车当前的位置坐标。

第一算法如下：

Xk＝Xk’+K′(Zk-HkXk’)

其中，Xk表示当前帧的泊车速度、位置坐标[P₁V₁]^T；Xk’表示上一帧计算得到泊车速度、位置坐标；Zk表示第一泊车速度V₁₁、第一位置坐标P₁₁(即[P₁₂V₁₂]^T)；HkXk’表示第二泊车速度V₁₂、第二位置坐标P₁₂(即[P₁₁V₁₁]^T)；Zk-HkXk’表示测量残差。

C、根据第二算法、卡尔曼增益，更新初始参数。

第二算法如下：

P_k’＝P_k-K′H_kP_k

其中，Pk’为更新后的初始参数；K′为卡尔曼增益，Hk表示协方差矩阵；Pk为更新前的初始参数。

D、重复步骤a～c，计算下一帧图像中本车的泊车速度和位置坐标。

本实施例在根据计算出当前帧图像中本车的泊车速度、位置坐标后，根据本次计算得到的卡尔曼增益更新初始参数，由于前后帧图像的相似度较高，因此可直接将更新后的初始参数应用到下一帧图像的计算中，从而可降低计算误差。

本实施例根据实际情况中产生泊车误差的车辆速度设置预设阈值，在车辆速度高于预设阈值时，采用常规的轮速传感器检测车辆的运动速度，可在全局坐标系中快速确定其运动位置；在车辆速度低于预设阈值时，则同时根据车载图像传感器和轮速传感器采集得到的数据，分别进行计算获取到的泊车速度(第一泊车速度V₁₁、第二泊车速度V₁₂)和位置坐标(第一位置坐标、第二位置坐标P₁₂)，最后代入采用卡尔曼滤波算法，对低速情况下的车辆的泊车速度和位置坐标进行修正，将传感器累计误差降低至最小，使得自动泊车定位更加精准，泊车效果更优。

S4、根据目标车位、泊车速度和位置坐标，规划泊车路径并执行，包括步骤S41～S42：

S41、根据全局坐标系和车身坐标系的比例关系，将车位坐标转换为全局坐标；

S42、根据目标车位的全局坐标、泊车速度和位置坐标，规划泊车路径并执行。

本发明实施例基于自动泊车中车位静止的特点，根据当前环境数据，建立静止的全局坐标系，在识别并确定目标车位后，根据本车实际的行车状态(当前行车状态)，进入预先设置好的预设规则，计算出本车实际的泊车速度和位置坐标，从而提高车辆泊车精度，避免传感器误差导致的泊车失败。

实施例2

在本实施例附图中的附图标记包括：路径规划模块1；车位检测模块2，线车位检测模块21、空间车位检测模22；车身状态确定模块3，主控模块31、方向盘传感器32、轮速传感器33、光流检测模块34；泊车执行模块4。

本发明实施例提供一种基于光流法的自动泊车系统，参见图5，包括：路径规划模块1和与其连接的车位检测模块2、车身状态确定模块3、泊车执行模块4；车位检测模块2包括线车位检测模块21、空间车位检测模22块2；

线车位检测模块21用于识别当前环境数据，获取车位线数据；

空间车位检测模22块2用于识别车身周围的障碍物，获取空间车位数据和障碍物信息；

车身状态确定模块3用于获取本车的当前行车状态，并根据预设规则计算对应的泊车速度和本车当前的位置坐标；

路径规划模块1用于，根据车位线数据、空间车位数据、障碍物信息确定目标车位，进而规划泊车路径；

泊车执行模块4用于执行泊车路径。

在本实施例中，车身状态确定模块3包括主控模块31，和与主控模块31连接的方向盘传感器32、轮速传感器33、光流检测模块34；

方向盘传感器32用于根据方向盘转动角度确定本车车轮的转角度数；

轮速传感器33用于获取本车车轮的轮速数据；

光流检测模块34用于，根据L-K光流法和从车载图像传感器中获取的运动图像，计算对应的瞬时速度；

主控模块31用于在判断到当前运动车速大于预设阈值时，根据轮速数据计算本车的泊车速度，并根据泊车速度和转角度数计算本车当前的位置坐标；否则，根据卡尔曼滤波算法以及轮速数据、瞬时速度、转角度数，计算本车的泊车速度和当前的位置坐标。

本实施例在现有的车载设备基础上，增加光流检测模块34，在车辆低速时，根据L-K光流法和从车载图像传感器中获取的运动图像，计算对应的瞬时速度；从而与原来的轮速传感器33获取的轮速数据集合，根据卡尔曼滤波算法计算本车的泊车速度和当前的位置坐标，从而降低自动泊车过程中的累计误差，避免泊车目标位置和停车位置的偏差，提高自动泊车准确率。

本实施例所提供的自动泊车系统采用各个模块实现自动泊车方法中的各个步骤，为自动泊车方法提供硬件基础，便于方法实施。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.一种基于光流法的自动泊车方法，其特征在于，包括步骤：

S1、根据当前环境数据，建立全局坐标系；

S2、识别所述当前环境数据，确定目标车位；

S3、获取本车的当前行车状态，并根据预设规则计算对应的泊车速度和本车当前的位置坐标；

S4、根据所述目标车位、所述泊车速度和所述位置坐标，规划泊车路径并执行。

2.如权利要求1所述的一种基于光流法的自动泊车方法，其特征在于，所述步骤S3包括：

S31、获取本车的当前运动车速，判断所述当前运动车速是否大于预设阈值，若是在进入步骤S32，若否则进入步骤S33；

S32、根据轮速传感器的轮速数据计算本车的泊车速度，并根据所述泊车速度计算本车当前的位置坐标；

S33、采用光流法检测车载图像传感器的瞬时速度，并根据所述瞬时速度计算本车的第一泊车速度，进而计算本车当前的第一位置坐标；根据轮速传感器的采集数据计算本车的第二泊车速度，进而计算本车当前的第二位置坐标；

S34、根据卡尔曼滤波算法，代入所述第一泊车速度、第二泊车速度、所述第一位置坐标、所述第二位置坐标，计算本车的泊车速度和本车当前的位置坐标。

3.如权利要求2所述的一种基于光流法的自动泊车方法，其特征在于，所述步骤S34包括：

A、根据对应于当前帧图像的所述第二泊车速度、所述第二位置坐标和初始参数，计算卡尔曼增益；

B、根据所述卡尔曼增益和第一泊车速度、第一位置坐标，结合第一算法计算本车的所述泊车速度和本车当前的所述位置坐标；

C、根据第二算法、所述卡尔曼增益，更新所述初始参数；

所述初始参数为，根据卡尔曼滤波计算的所述泊车速度、所述位置坐标的协方差矩阵。

4.如权利要求3所述的一种基于光流法的自动泊车方法，其特征在于，在所述步骤B中，所述第一算法如下：

Xk＝Xk’+K′(Zk-HkXk’)

其中，Xk表示当前帧的所述泊车速度、所述位置坐标；Xk’表示上一帧计算得到所述泊车速度、所述位置坐标；Zk表示所述第一泊车速度、第一位置坐标；HkXk’表示所述第二泊车速度、所述第二位置坐标；Zk-HkXk’表示测量残差。

5.如权利要求1所述的一种基于光流法的自动泊车方法，其特征在于，所述步骤S1包括：

S11、获取当前环境数据，确定车辆泊车起点；

S12、以在所述车辆泊车起点上本车的车身后轴中心为原点，结合所述当前环境数据建立全局坐标系。

6.如权利要求1所述的一种基于光流法的自动泊车方法，其特征在于，所述步骤S2包括：

S21、识别所述当前环境数据，建立车身坐标系，并获取周围的车位线数据、空间车位数据、障碍物信息；

S22、根据所述车位线数据、空间车位数据、障碍物信息，确定目标车位。

7.如权利要求6所述的一种基于光流法的自动泊车方法，其特征在于，所述步骤S22包括：

a、当所述车位线数据为无数据时，根据空间车位数据和所述障碍物信息确定目标车位；

b、当所述空间车位数据为无数据时，根据车位线数据和所述障碍物信息确定目标车位；

c、融合所述车位线数据、空间车位数据，将重叠的融合车位作为目标车位，并确定其车位坐标。

8.如权利要求7所述的一种基于光流法的自动泊车方法，其特征在于，所述步骤S4包括：

S41、根据所述全局坐标系和所述车身坐标系的比例关系，将所述车位坐标转换为全局坐标；

S42、根据所述目标车位的全局坐标、所述泊车速度和所述位置坐标，规划泊车路径并执行。

9.一种基于光流法的自动泊车系统，其特征在于，包括：路径规划模块和与其连接的车位检测模块、车身状态确定模块、泊车执行模块；所述车位检测模块包括线车位检测模块、空间车位检测模块；

所述线车位检测模块用于识别当前环境数据，获取车位线数据；

所述空间车位检测模块用于识别车身周围的障碍物，获取空间车位数据和障碍物信息；

所述车身状态确定模块用于获取本车的当前行车状态，并根据预设规则计算对应的泊车速度和本车当前的位置坐标；

所述路径规划模块用于，根据所述车位线数据、空间车位数据、障碍物信息确定目标车位，进而规划泊车路径；

所述泊车执行模块用于执行所述泊车路径。

10.如权利要求9所述的一种基于光流法的自动泊车系统，其特征在于：所述车身状态确定模块包括主控模块，和与所述主控模块连接的方向盘传感器、轮速传感器、光流检测模块；

所述方向盘传感器用于根据方向盘转动角度确定本车车轮的转角度数；

所述轮速传感器用于获取本车车轮的轮速数据；

所述光流检测模块用于，根据L-K光流法和从车载图像传感器中获取的运动图像，计算对应的瞬时速度；

所述主控模块用于在判断到当前运动车速大于预设阈值时，根据所述轮速数据计算本车的泊车速度，并根据所述泊车速度和所述转角度数计算本车当前的位置坐标；否则，根据卡尔曼滤波算法以及所述轮速数据、所述瞬时速度、所述转角度数，计算本车的泊车速度和当前的位置坐标。

Images