CN104354656A - 智能泊车系统的障碍物检测与库位判别方法及实现系统 - Google Patents

Abstract

一种智能泊车系统的障碍物检测与库位判别方法，该方法共包含六个部分：依靠雷达和定位系统完成数据采集；依靠坐标变换实现障碍物数据点的全局定位；依靠分析数据点间的几何关系实现数据点的分类；依靠最佳外接矩形拟合算法实现数据点的拟合；依靠分析矩形拟合结果间的几何关系实现库位判别；依靠库位信息协议实现数据发送。本发明还公开了实现所述方法的智能泊车系统的障碍物检测与库位判别系统，包括传感器单元和信号处理单元。本发明对于有前后固定障碍物的库位，能够实现车辆在一定范围内沿任意路径进行库位检测，所用到的库位检测算法更符合人类判别库位几何信息的规律，具有较高的检测精度、可靠性和实用价值。

Description

智能泊车系统的障碍物检测与库位判别方法及实现系统

技术领域

本发明属于汽车技术领域，涉及驾驶辅助技术，特别是智能泊车系统的障碍物检测方法。 

背景技术

随着汽车保有量的增大，停车位日益紧张，停车难这一问题变得愈加严重。对于没有经验的新手而言，将车辆安全泊入库位非常困难；即使对于有经验的驾驶员，将车辆停入狭小的库位也不是一件容易的事。开发智能泊车系统，有利于保证泊车过程的安全性，同时提高泊车过程的舒适性和便捷性。 

智能泊车系统一般包括以下几个模块：库位检测模块、路径规划模块、路径跟踪模块、执行模块和人机界面模块。库位检测模块是智能泊车系统的基础，很大程度上影响着整个泊车系统的精度和性能。 

目前，智能泊车系统的库位检测方法主要包含以下四类： 

1.基于超声波雷达的库位检测。由于车辆前端或后端存在圆角，受超声波测距传感器自身特性的局限，在前后端圆角处存在接收不到回波的现象，这将造成检测到的库位较实际偏大。2.基于毫米波雷达的库位检测。其优点在于对具有相对运动的物体检测精度较高，但是无法检测相对静止的物体。3.基于激光雷达的库位检测。其优点在于检测精度高，但是成本相对高昂。4.基于视觉技术的库位检测。其难点在于图像易受到阴影、光照等环境条件的影响。此类技术主要包括：(1)基于环视摄像头的图案识别；(2)基于单目摄像头的运动立体视觉；(3)基于双目摄像头的立体视觉；(4)基于双目摄像头的结构光分析。以上方案各有优点，不过仍然存在不足，客观上存在进一步改进的迫切需要。 

发明内容

本发明的目的在于提供一种智能泊车系统的障碍物检测与库位判别方法及实现系统，从而实现库位检测功能，对于有前后固定障碍物的平行、垂直、斜向库位等都能可靠、高效地检出并获得库位信息。 

为达到以上目的，本发明所采用的解决方案是： 

一种智能泊车系统的障碍物检测与库位判别方法，包括：步骤(a)进行数据采集；步骤(b)进行坐标变换；步骤(c)进行数据分类；步骤(d)进行数据拟合；步骤(e)进行库位判别。 

进一步：步骤(a)包括：(1)以一定的步长依次接收车辆定位数据与雷达数据；(2)根据雷达的特性与实际布置情况，筛选出有效的数据；(3)若有多个雷达参与环境感知，则进行数据融合。 

步骤(b)包括：(1)将雷达坐标系下的数据点坐标转换至车辆坐标系；(2)将车辆坐标系下的数据点坐标转换至定位系统坐标系；(3)将定位系统坐标系下的数据点坐标转换至全局坐标系。 

步骤(c)包括：(1)对于新扫描的数据点，求出该点至各已有障碍物的最短距离；(2)若该最小距离值在设定阈值内，则该点归属于对应物体，否则成立一个新物体。 

步骤(d)包括：(1)对于同一物体的数据点，令其外接矩形的一边即特征边与全局坐标系X轴的夹角从负45度至正45度遍历，对于该范围内任意的夹角，均可唯一确定外接矩形；(2)求出使外接矩形特征边两端点至物体的距离之和，在上述夹角范围内使该“距离和”最小的夹角值所对应的外接矩形即为所要拟合的结果。 

步骤(e)包括如下判别条件：(1)相邻两障碍物外接矩形的最短距离大于设定阈值；(2)前方障碍物已检出部分的外接矩形长边长度大于设定阈值；(3)相邻两障碍物外接矩形的长边夹角小于设定阈值。 

还包括进行数据发送步骤：(1)对符合判别条件的库位，提取前后障碍物的外接矩形，并将数据转换至库位坐标系；(2)以一定的协议将数据发送至智能泊车系统路径规划模块。 

实现上述方法的智能泊车系统的障碍物检测与库位判别系统，包括：(1)传感器单元：位于车辆车身的兼具角度和距离信号的二维雷达、具有相对定位功能的定位系统；(2)信号处理单元：雷达数据处理模块。 

所述信号处理单元包括数据采集模块、坐标变换模块、数据分类模块、数据拟合模块、库位判别模块与数据发送模块； 

优选的，数据采集模块用于二维雷达与定位系统数据的采集；坐标变换模块用于雷达数据点的全局定位；数据分类模块用于分析坐标变换后数据点之间的几何位置关系，从而确定每一个数据点的物体归属；数据拟合模块利用最佳外接矩形拟合算法实现数据点的拟合；库位判别模块用于分析矩形拟合结果间的几何关系，从而判断库位是否符合泊车路径规划的条件；数据发送模块用于整理库位信息并按协议发送至智能泊车系统的路径规划模块。 

所述传感器单元中的位于车辆车身的二维雷达，其有效感知角度范围不小于60度，有效感知距离不小于4米；或者，优选的，二维检测平面应与地面平行，高度介于0.5米至1米之间；或者，优选的， 

所述传感器单元中的具有相对定位功能的定位系统，选自以下一种或两种以上的组合：外置安装在车辆上的动态精度在10cm以内的GPS全球定位系统；通过内置传感器采集轮速与车辆转角等数据的车辆内部参数位姿估计系统；通过外置传感器采集外部参考数据，并附以SLAM即时定位与地图重构等技术的车辆外部参数位姿估计系统；或者，优选的，所述二维雷达采用激光雷达。 

由于采用了上述技术方案，本发明具有以下有益效果：通过融合雷达传感器得到的障碍物角度与距离信息以及定位系统得到的车辆位姿信息，可实现车辆在一定范围内沿任意路径进行库位检测，使低精度雷达产品获得更高精度的库位检测结果；依靠最佳外接矩形拟合算法，该技术方案相较传统库位检测方法能够获得更符合人类判断规律的结果，可靠性更强，并能以更少的运算量完成任务。 

附图说明

图1是本发明的二维雷达库位检测流程图。 

图2(a)是本发明的智能泊车系统的定位系统坐标系的定义示意图。 

图2(b)是本发明的智能泊车系统的全局坐标系的定义示意图。 

图2(c)是本发明的智能泊车系统的车辆坐标系与雷达坐标系的定义示意图。 

图2(d)是本发明的智能泊车系统的全局坐标系下各坐标系的关系示意图。 

图3是本发明的数据分类实施例示意图。 

图4(a)是本发明的数据拟合(最佳外接矩形拟合算法)实施例示意图(tanα＝-1)。 

图4(b)是本发明的数据拟合(最佳外接矩形拟合算法)实施例示意图(变量dis最小时的tanα)。 

图5是本发明的数据拟合实施例dis-α关系图。 

图6是本发明的库位判别实施例示意图。 

图7是本发明的库位检测结果示意图(全局坐标系G，单位：米)。 

图8是本发明的库位检测术语示意图(右侧平行泊车例)。 

具体实施方式

以下结合附图所示实施例(右侧平行泊车)对本发明作进一步的说明。如未特殊说明，本说明书中的库位检测相关术语参照图8；如未特殊说明，本说明书中的“航向角”在笛卡尔坐标系下以X轴正方向为0，逆时针为正。 

本发明一种智能泊车系统的障碍物检测与库位判别方法，包括：依靠雷达和定位系统完成数据采集；依靠坐标变换实现障碍物数据点的全局定位；依靠分析数据点间的几何关系实现数据点的分类；依靠最佳外接矩形拟合算法实现数据点的拟合；依靠分析矩形拟合结果间的几何关系实现库位判别；依靠库位信息协议实现数据发送。 

一种智能泊车系统的障碍物检测与库位判别系统，包括：传感器单元：位于车辆车身的兼具角度和距离信号的二维雷达(如激光雷达等)、具有相对定位功能的定位系统(如GPS全球定位系统、车辆本地位姿估计系统等)；(2)信号处理单元：雷达数据处理模块。 

所述信号处理单元包括数据采集模块、坐标变换模块、数据分类模块、数据拟合模块、库位判别模块与数据发送模块。数据采集模块用于二维雷达与定位系统数据的采集；坐标变换模块用于雷达数据点的全局定位；数据分类模块用于分析坐标变换后数据点之间的几何位置关系，从而确定每一个数据点的物体归属；数据拟合模块利用最佳外接矩形拟合算法实现数据点的拟合；库位判别模块用于分析矩形拟合结果间的几何关系，从而判断库位是否符合泊车路径规划的条件；数据发送模块用于整理库位信息并按协议发送至智能泊车系统的路径规划模块。 

所述传感器单元中的位于车辆车身的二维雷达，其有效感知角度范围不小于60度，有效感知距离不小于4米，二维检测平面应与地面平行，高度介于0.5米至1米之间。 

所述传感器单元中的具有相对定位功能的定位系统，可以是外置安装在车辆上的动态精度在10cm以内的GPS全球定位系统；也可以是通过内置传感器采集轮速与车辆转角等数据的车辆内部参数位姿估计系统；也可以是通过外置传感器采集外部参考数据，并附以SLAM即时定位与地图重构等技术的车辆外部参数位姿估计系统；也可以是以上系统的组合。 

所述信号处理单元中的数据采集模块进行数据采集的方法，包括如下步骤：(1)以一定的步长依次接收车辆定位数据与雷达数据；(2)根据雷达的特性与实际布置情况，筛选出有效的数据；(3)若有多个雷达参与环境感知，则需进行数据融合。 

所述信号处理单元中的坐标变换模块进行坐标变换的方法，包括如下步骤：(1)将雷达坐标系下的数据点坐标转换至车辆坐标系；(2)将车辆坐标系下的数据点坐标转换至定位系统坐标系；(3)将定位系统坐标系下的数据点坐标转换至全局坐标系。 

所述信号处理单元中的数据分类模块进行数据分类的方法，包括如下步骤：(1)对于新扫描的数据点，求出该点至各已有障碍物的最短距离；(2)若该最小距离值在设定阈值内，则该点归属于对应物体，否则成立一个新物体。 

所述信号处理单元中的数据拟合模块进行数据拟合的方法，包括如下步骤：(1)对于同一物体的数据点，令其外接矩形的一边(称为特征边)与全局坐标系X轴的夹角从负45度至正45度遍历，对于该范围内任意的夹角，均可唯一确定外接矩形；(2)求出使外接矩形特征边两端点至物体的距离之和，在上述夹角范围内使该“距离和”最小的夹角值所对应的外接矩形即为所要拟合的结果。 

所述信号处理单元中的库位判别模块进行库位判别的方法，包括如下判别条件：(1)相邻两障碍物外接矩形的最短距离大于设定阈值；(2)前方障碍物已检出部分的外接矩形长边 长度大于设定阈值；(3)相邻两障碍物外接矩形的长边夹角小于设定阈值。 

所述信号处理单元中的数据发送模块进行数据发送的方法，包括如下步骤：(1)对符合判别条件的库位，提取前后障碍物的外接矩形，并将数据转换至库位坐标系；(2)以一定的协议将数据发送至智能泊车系统路径规划模块。 

具体而言，传感器单元包括：位于车辆车身的兼具角度和距离信号的二维雷达(如激光雷达等)、具有相对定位功能的定位系统(如GPS全球定位系统、车辆本地位姿估计系统等)。 

(1)二维雷达 

位于车辆车身的二维雷达，其有效感知角度范围不小于60度，有效感知距离不小于4米，二维检测平面应与地面平行，高度介于0.5米至1米之间。雷达的布置方式为：对于泊车位在车辆所在道路右侧的情况(右侧方案)，可以在车辆右侧车身靠前的位置正向车辆右侧安装雷达，也可以在车辆右前转向灯位置附近斜向安装雷达，无论采用何种安装方式，车辆坐标系V(如图2(c))的第三、四象限应各至少涵盖雷达10度的检测角度范围，以保证正向、背向行车方向一侧的数据点都能被准确扫描；对于泊车位在车辆所在道路左侧的情况(左侧方案)，布置方式为右侧方案基于车辆坐标系X轴的镜像翻转；考虑到智能泊车系统功能的完整性，车辆应同时配备右侧方案与左侧方案两套雷达设备。 

(2)定位系统 

具有相对定位功能的定位系统，可以是外置安装在车辆上的动态精度在10cm以内的GPS全球定位系统，GPS的定位参考点水平位置可位于车辆后轴中点；也可以是基于内置传感器采集轮速与车辆转角等数据及车辆内部参数的位姿估计系统，轮速信号可由能反映车轮运动的传感器信号(如ABS制动防抱死系统的轮速传感器信号等)分析获得，车辆转角信号可由能反映转向轮转角的传感器信号(如安装在转向柱上的转向盘转角传感器信号等)分析获得；也可以是通过外置传感器采集外部参考数据，并附以SLAM即时定位与地图重构等技术的车辆外部参数位姿估计系统，外置传感器可以是用于检测的雷达本身，也可以是摄像头等其它传感设备；具有相对定位功能的定位系统也可以是以上各类系统的组合。 

信号处理单元包括数据采集模块、坐标变换模块、数据分类模块、数据拟合模块、库位判别模块与数据发送模块，各模块组成的工作流程如图1所示。 

(1)数据采集模块 

数据采集模块用于二维雷达与定位系统数据的采集。具体步骤为： 

1)以一定的步长依次接收车辆定位数据与雷达数据。数据接收步长由传感器数据刷新频率决定。安排雷达数据处理顺序时，对于右侧泊车工况应按雷达平面逆时针方向依次处理数据点，左侧泊车工况应按雷达平面顺时针方向依次处理数据点。 

2)根据雷达的特性与实际布置情况，筛选出有效的数据。不同雷达处理超出检测范围的数据点的方式不同；而由于车身嵌入式安装的需要，并非雷达全部角度范围的数据都能被使用。此时需根据实际情况对接收的雷达数据进行筛选，以减少后续数据处理量。 

3)若有多个雷达参与环境感知，则需进行数据融合。数据融合的内容包括：数据点的叠加、重叠部分数据的冗余算法等。 

(2)坐标变换模块 

坐标变换模块用以实现障碍物数据点的全局定位，即通过处理采集得到的雷达数据与车辆定位数据，将所有的有效数据点转换至全局坐标系。各坐标系定义如图2(a)～(d)所示。其中，以笛卡尔坐标系下定位系统原点作为定位系统坐标系E的原点，以定位系统0航向角方向作为定位系统坐标系E的X轴正方向；以笛卡尔坐标系下库位检测起始点处车辆后轴中点作为全局坐标系G的原点，以此时车辆车头方向作为全局坐标系G的X轴正方向；以笛卡 尔坐标系下任意时刻车辆后轴中点作为车辆坐标系V的原点，以车辆车头方向作为车辆坐标系V的X轴正方向；以笛卡尔坐标系下任意时刻雷达检测介质发射中心点为雷达坐标系L的原点，以雷达0角度方向作为雷达坐标系L的X轴正方向；以笛卡尔坐标系下检出库位时前方障碍物外接矩形左后角点作为库位坐标系P的原点，以此时前方障碍物外接矩形沿道路方向的边(即长边)所在方向作为库位坐标系P的X轴正方向。 

雷达坐标系L下的数据点坐标可根据下式转换至全局坐标系G： 

point_G＝point_LH_LVH_VEH_EG    (式2-1) 

式中，point_L为雷达坐标系L下雷达数据点的坐标，H_LV为雷达坐标系L至车辆坐标系V的坐标变换矩阵，其它依此类推。 

具体地，以上坐标变换过程可分解为三步(按图2(a)～(d)所示的坐标定义)： 

1)将雷达坐标系L下的数据点坐标转换至车辆坐标系V。 

point_VX＝cosθ_LV*point_LX-sinθ_LV*point_LY+X_LV    (式2-2) 

point_VY＝sinθ_LV*point_LX+cosθ_LV*point_LY+Y_LV    (式2-3) 

其中，point_LX、point_LY分别为雷达坐标系L下雷达数据点的横纵坐标，X_LV、Y_LV分别为雷达坐标系L原点在车辆坐标系V下的横纵坐标，θ_LV为雷达坐标系L的X轴正方向在车辆坐标系V中的航向角，其它依此类推。 

2)将车辆坐标系V下的数据点坐标转换至定位系统坐标系E。 

point_EX＝cosθ_VE*point_VX-sinθ_VE*point_VY+X_VE    (式2-4) 

point_EY＝sinθ_VE*point_VX+cosθ_VE*point_VY+Y_VE    (式2-5) 

3)将定位系统坐标系E下的数据点坐标转换至全局坐标系G。 

point_GX＝(point_EX-X_GE)*cosθ_GE+(point_EY-Y_GE)*sinθ_GE    (式2-6) 

point_GY＝(point_EY-Y_GE)*cosθ_GE-(point_EX-X_GE)*sinθ_GE    (式2-7) 

(3)数据分类模块 

数据分类模块用于分析坐标变换后数据点之间的几何位置关系，从而确定每一个数据点的物体归属。不同时刻扫描得到的障碍物数据点经过坐标变换，于全局坐标系G不断叠加，第一次采集得到的数据及之后每次新叠加的数据点即是需要分类的数据点。具体步骤为： 

1)对于新扫描的数据点，求出该点至各已有障碍物的最短距离。即对于每个新叠加的数据点point，令 

$d=\underset{j=1}{\overset{N}{\min }}\left|\stackrel{\→}{\mathrm{point},\mathrm{Object}\left(j\right)}\right|$     (式2-8) 

式中，Object(j)表示归于第j个已分类物体的所有数据点，N为已分类物体个数。该式可以理解为：针对第j个已分类物体，可以得到新扫描数据点到该物体各点间距离的最小值，该值即为新扫描数据点到第j个已分类物体的距离；对于N个已分类物体，共可得到N个这样的距离；d即为这些距离的最小值。 

2)若该最小距离值在设定阈值内(阈值由二维雷达与车辆定位系统的精度、数据刷新频率等综合确定，该阈值是动态的，对于雷达返回距离越远的点，阈值应越大)，则该点归属于对应物体，否则成立一个新物体。即当d取到最小值时，令j₀＝j，于是 

$\mathrm{point}\∈\left\{\begin{array}{c}\mathrm{Object}\left(j_0\right),d\≤\mathrm{threshold};\\ \mathrm{Object}(N+1),d>\mathrm{threshold}.\end{array}\right.$     (式2-9) 

式中，Object(j)表示归于第j个已分类物体的所有数据点，N为已分类物体个数。 

以上分类算法基于如下假设： 

1)驾驶员驾驶车辆检测库位时车辆距离障碍物较近(平行库位泊车为2米以内)； 

2)距离相隔很近的两障碍物(间距远小于有效库位)可算作一个障碍物。 

基于以上假设是由于：限制车辆与障碍物间距可大幅降低数据点处理量；当两障碍物距离很近时，合并为一个物体对最终库位检测结果的影响很小。 

以下通过一实施例来说明数据分类算法。 

如图3所示，某一时刻雷达扫描到共计三组有效数据点。根据上述算法，第组数据点距离障碍物①较近，且距离值在阈值范围内，故归属于障碍物①；第组数据点距离障碍物②较近，且距离值在阈值范围内，故归属于障碍物②；第组数据点中，部分距离障碍物①较近，部分距离障碍物②较近，但距离值均超出阈值，故考虑新建立障碍物③。 

(4)数据拟合模块 

数据拟合模块利用最佳外接矩形拟合算法实现数据点的拟合。同一障碍物的数据点拟合为一个外接矩形，多个障碍物对应相等数量的矩形拟合结果。对于某一障碍物的数据点，具体拟合步骤为： 

1)由于这些数据点对应的外接矩形有无穷多个，故令其外接矩形的一边(称为特征边)与全局坐标系X轴的夹角从负45度至正45度遍历(遍历的步长决定特征边的角度精度，如以1度步长进行遍历，则特征边的角度精度为1度)。如图4(a)，令l₁的斜率为tanα，且有α∈[-45°，45°)。对于每一个α均能唯一确定外接矩形P₁P₂P₃P₄。 

2)求出使外接矩形特征边两端点至物体的距离之和，在上述夹角范围内使该“距离和”最小的夹角值所对应的外接矩形即为所要拟合的结果。即令 

$\mathrm{dis}=\left|\stackrel{\→}{P_1,\mathrm{Object}}\right|+\left|\stackrel{\→}{P_4,\mathrm{Object}}\right|$     (式2-10) 

求出使dis最小的斜率tanα，其对应的物体外接矩形即为结果(如图4(b))。 

以上最佳外接矩形拟合算法基于如下假设： 

1)人类对于车辆轮廓的判别逻辑基于数据点的几何特征，而非代数特征； 

2)检测车辆轮廓的关键是获得车辆停放的方向，即获得车辆侧面轮廓线的斜率； 

3)每一时刻数据点采集时的车辆角度、车速各不相同，造成数据点的分布不均匀； 

4)车辆外观的复杂性使得部分凹陷处的数据点并不完全表征车辆的轮廓。 

基于以上假设设计的最佳外接矩形拟合算法具有如下优势：鲁棒性好：对于各类常见的车身及其它障碍物轮廓均能获得较好的拟合结果；可靠性好：出现预想之外结果的可能性低；算法简单：相较直角边拟合等算法，其逻辑更为简洁。 

最佳外接矩形拟合算法还具有以下特点： 

1)变量α∈[-45°，45°)时，该算法可涵盖任一角度的障碍物； 

2)若仅获得障碍物两边的数据点，该算法依然能够准确拟合； 

3)对于已知的数据点，障碍物长边斜率的精度与运算量是一对矛盾。根据图4所示障碍物数据点绘制的dis-α关系图如图5所示，由于理论上变量dis在谷值两侧区间分别单调，且收敛性好，故对于变量α可先选取较大的步长，在所得结果α_result的某一邻域内，令α取一较小的步长重复以上算法，即可在保证精度的同时进一步降低运算量。 

4)由于采用了数据点的几何特征，该算法在雷达数据点很少时亦可获得较为理想的拟合结果，故该算法适合于采样频率较低、感知角度较小、角度分辨率不高的二维雷达。 

(5)库位判别模块 

库位判别模块用于分析矩形拟合结果间的几何关系，从而判断库位是否符合泊车路径规划的条件，即判断是否存在可执行智能泊车的库位。如图6，判定条件主要有： 

1)相邻两障碍物外接矩形的最短距离(图中尺寸)大于设定阈值。该条件用于确保有足够的空间实现泊车入库。即 

$\left|\stackrel{\→}{\mathrm{Object}\left(1\right),\mathrm{Object}\left(2\right)}\right|\≥\mathrm{threshold};$     (式2-11) 

2)前方障碍物已检出部分的外接矩形长边长度(图中尺寸)大于设定阈值。该条件用于确保前方障碍物的长边L₁航向角达到足够的精度，并使泊车起始位姿有利于进行后续路径规划。即 

length(2)≥threshold；    (式2-12) 

其中，length(2)表示当前时刻前方障碍物②已检出的长边长度。 

3)相邻两障碍物外接矩形的长边夹角(图中尺寸)小于设定阈值。该条件用于库位的择优，放弃具有碰撞风险的不规则库位。即 

θ(L₁，L₃)≤threshold.    (式2-13) 

其中，θ(L₁，L₃)表示L₁与L₃所在直线的夹角(如图2(d)所示)。 

以上判定条件中的阈值需根据库位类型(平行、垂直、斜向库位)、车辆最小转弯半径、车身长度、库位检测时及入库时的车速以及智能泊车系统路径规划模块的具体需求进行标定。 

(6)数据发送模块 

数据发送模块用于整理库位信息并按协议发送至智能泊车系统的路径规划模块。具体步骤为： 

1)对符合判别条件的库位，提取前后障碍物的外接矩形，并将数据转换至库位坐标系P。根据库位坐标系的定义，仅采用以下数据即可描述本次泊车的关键信息，包括：泊车起始位姿(后轴中点坐标与车辆航向角)、后方障碍物外接矩形的左前角点A坐标与长边L₃斜率。 

参照图2定义，可依据下式将上述数据由全局坐标系G转换至库位坐标系P： 

X_VP＝(X_VG-X_BG)*cosθ_1G+(Y_VG-Y_BG)*sinθ_1G    (式2-14) 

Y_VP＝(Y_VG-Y_BG)*cosθ_1G-(X_VG-X_BG)*sinθ_1G    (式2-15) 

θ_VP＝θ_VG-θ_1G    (式2-16) 

X_AP＝(X_AG-X_BG)*cosθ_1G+(Y_AG-Y_BG)*sinθ_1G    (式2-17) 

Y_AP＝(Y_AG-Y_BG)*cosθ_1G-(X_AG-X_BG)*sinθ_1G    (式2-18) 

θ_3P＝θ_3G-θ_1G-180°    (式2-19) 

其中，X_BG、Y_BG分别为前方障碍物外接矩形左后角点B在全局坐标系G下的横纵坐标，θ_1G为前方障碍物外接矩形长边L₁在全局坐标系G中的航向角，其它依此类推。 

2)以一定的协议将数据发送至智能泊车系统路径规划模块，该模块根据本方法获得的抽象化的库位信息进行泊车路径规划。 

图7显示了全局坐标系下基于激光雷达的库位检测结果。 

上述的对实施例的描述是为便于该技术领域的普通技术人员能理解和应用本发明。熟悉本领域技术的人员显然可以容易地对这些实施例做出各种修改，并把在此说明的一般原理应用到其它实施例中而不必经过创造性劳动。因此，本发明不限于上述实施例，本领域技术人员根据本发明的揭示，不脱离本发明的范畴所做出的改进和修改都应该在本发明的保护范围之内。 

Claims (10)

1.一种智能泊车系统的障碍物检测与库位判别方法，其特征在于：包括：步骤(a)进行数据采集；步骤(b)进行坐标变换；步骤(c)进行数据分类；步骤(d)进行数据拟合；步骤(e)进行库位判别。

2.根据权利要求1所述的方法，其特征在于：步骤(a)包括：(1)以一定的步长依次接收车辆定位数据与雷达数据；(2)根据雷达的特性与实际布置情况，筛选出有效的数据；(3)若有多个雷达参与环境感知，则进行数据融合。

3.根据权利要求1所述的方法，其特征在于：步骤(b)包括：(1)将雷达坐标系下的数据点坐标转换至车辆坐标系；(2)将车辆坐标系下的数据点坐标转换至定位系统坐标系；

(3)将定位系统坐标系下的数据点坐标转换至全局坐标系。

4.根据权利要求1所述的方法，其特征在于：步骤(c)包括：(1)对于新扫描的数据点，求出该点至各已有障碍物的最短距离；(2)若该最小距离值在设定阈值内，则该点归属于对应物体，否则成立一个新物体。

5.根据权利要求1所述的方法，其特征在于：步骤(d)包括：(1)对于同一物体的数据点，令其外接矩形的一边即特征边与全局坐标系X轴的夹角从负45度至正45度遍历，对于该范围内任意的夹角，均可唯一确定外接矩形；(2)求出使外接矩形特征边两端点至物体的距离之和，在上述夹角范围内使该“距离和”最小的夹角值所对应的外接矩形即为所要拟合的结果。

6.根据权利要求1所述的方法，其特征在于：步骤(e)包括如下判别条件：(1)相邻两障碍物外接矩形的最短距离大于设定阈值；(2)前方障碍物已检出部分的外接矩形长边长度大于设定阈值；(3)相邻两障碍物外接矩形的长边夹角小于设定阈值。

7.根据权利要求1所述的方法，其特征在于：还包括进行数据发送步骤：(1)对符合判别条件的库位，提取前后障碍物的外接矩形，并将数据转换至库位坐标系；(2)以一定的协议将数据发送至智能泊车系统路径规划模块。

8.实现权利要求1至7中任一所述方法的智能泊车系统的障碍物检测与库位判别系统，其特征在于：包括：(1)传感器单元：位于车辆车身的兼具角度和距离信号的二维雷达、具有相对定位功能的定位系统；(2)信号处理单元：雷达数据处理模块。

9.根据权利要求8所述的智能泊车系统的障碍物检测与库位判别系统，其特征在于：

所述信号处理单元包括数据采集模块、坐标变换模块、数据分类模块、数据拟合模块、库位判别模块与数据发送模块；

优选的，数据采集模块用于二维雷达与定位系统数据的采集；坐标变换模块用于雷达数据点的全局定位；数据分类模块用于分析坐标变换后数据点之间的几何位置关系，从而确定每一个数据点的物体归属；数据拟合模块利用最佳外接矩形拟合算法实现数据点的拟合；库位判别模块用于分析矩形拟合结果间的几何关系，从而判断库位是否符合泊车路径规划的条件；数据发送模块用于整理库位信息并按协议发送至智能泊车系统的路径规划模块。

10.根据权利要求8所述的智能泊车系统的障碍物检测与库位判别系统，其特征在于：

所述传感器单元中的位于车辆车身的二维雷达，其有效感知角度范围不小于60度，有效感知距离不小于4米；或者，优选的，二维检测平面应与地面平行，高度介于0.5米至1米之间；或者，优选的，

所述传感器单元中的具有相对定位功能的定位系统，选自以下一种或两种以上的组合：外置安装在车辆上的动态精度在10cm以内的GPS全球定位系统；通过内置传感器采集轮速与车辆转角等数据的车辆内部参数位姿估计系统；通过外置传感器采集外部参考数据，并附以SLAM即时定位与地图重构等技术的车辆外部参数位姿估计系统；或者，优选的，

所述二维雷达采用激光雷达。