CN109814115A - 一种垂直泊车的角度识别校正方法 - Google Patents

Abstract

本发明公开了一种垂直泊车的角度识别校正方法，根据超声波雷达传感器检测两侧车辆与本车之间的距离信息，整车CAN信号传输回来的信息计算得到的本车车身的姿态角信息以及本车在由车位信息建立的虚拟坐标系下的位置信息来分别计算两侧车辆的角度以及在虚拟坐标系中的位置，然后对初始的车位信息进行校正，以避免在泊车过程中由于车身姿态角不对而出现碰撞事故。

Description

一种垂直泊车的角度识别校正方法

技术领域

本发明涉及智能驾驶技术领域，具体涉及智能停车技术。

背景技术

随着人们生活水平的不断提高和汽车产业不断的蓬勃发展，汽车数量随之逐年增加，而公路，街道及停车场显得越发拥挤狭窄，因车辆泊车引起的纠纷也逐年增加。泊车时驾驶员仅依靠后视镜与车窗玻璃观察周围车辆环境，视野受到较大限制，无法同时掌控其车辆后方的状况与兼顾周围车辆的状况，容易产生不安全的因素，而且泊车很大程度上依赖于驾驶员的驾驶经验与技巧，若是经验与技巧不够，又遇到较为狭窄的车位，泊车难度就会大大增加，因此才有了泊车辅助系统。

随着泊车辅助系统不断向着智能化，人性化的要求迈进，自动泊车系统应运而生。开发自动泊车系统的目的在于将驾驶员从复杂的泊车操作中解脱出来，提高驾驶的舒适性，缓解驾驶员泊车时的紧张程度，预防泊车事故的发生。自动泊车系统包含环境数据采集系统(超声波雷达系统，整车CAN信号采集)，车载ECU(进行数据处理，路径规划与控制命令输出)，车辆泊车操作系统(可主动控制的EPS)。

自动泊车分为侧方泊车与垂直泊车。对于垂直泊车，由于实际泊车过程中两侧车辆不可能完全的保持垂直，甚至会出现倾斜较大的情况，如果直接将自动泊车系统设成完全垂直的去泊车可能会出现撞到两侧车辆的情况。

发明内容

本发明所要解决的技术问题就是提供一种垂直泊车的角度识别校正方法，以避免在泊车过程中由于车身姿态角不对而出现碰撞事故。

为解决上述技术问题，本发明采用如下技术方案：一种垂直泊车的角度识别校正方法，包括如下步骤：

步骤S1，在本车的车尾进入车位后，安装在车尾侧面的两个雷达探头开始识别两侧障碍车；

步骤S2，对测到的雷达数据进行两次滤波得到比较连续的数据；

步骤S3，通过拟合直线的方式对数据进行处理计算得到两侧障碍车的车身角度；

步骤S4，根据两侧障碍车的车身角度来确定本车最终应停止的位置。

可选的，以车位中心和本车后轴位置相交处为原点建立虚拟坐标系，计算车身当前位置在虚拟坐标系中的值以及车身角度。

可选的，第一次滤波将偶尔出现的未测到数据的1～3个点通过插值法补全，第二次滤波通过插值法将突变部分的数据替换掉。

可选的，根据本车在虚拟坐标系中的位置、车身的姿态、雷达的数据，计算出每一个数据点对应两侧车辆边沿在虚拟坐标系中的点。

可选的，当经过滤波后计算得到的两侧车辆边沿在坐标系中的数据点的个数大于12个点时，即认为本组数据可以开始计算两侧车辆的姿态角。

可选的，根据计算得到的两侧车辆边沿在虚拟坐标系中的坐标值来进行拟合直线：假设车辆边沿所在直线的方程为y＝ax+b，那么根据最小二乘法有如下公式：

用虚拟坐标系中两侧车辆边沿的数据分别代入上式计算得到a_right、a_left、b_right、b_left，即两侧边沿所在直线方程为y_right＝a_rightx+b_right和y_left＝a_leftx+b_left，根据两直线方程的斜率a_right和a_left即可分别求得两侧车辆的姿态角θ_right＝arctan(a_right)和θ_left＝arctan(a_left)。

可选的，对于垂直车位，计算两侧车辆边沿在坐标系中y＝0时的x值x_right和x_left，当车位两侧均有车时，本车最终应停止的角度为(θ_right+θ_left)/2，最终应停止的位置则为(x_right+x_left)/2；当车位只有一侧有车时，以有车一侧计算得到的角度作为最终停止的角度，以设定值的车位大小来确定最终停止的x值。

可选的，对于倾斜车位，采用垂直车位相同的计算方法计算本车最终应停止的角度θ_end，当计算得到的最终停止角度θ_end大于95°或者小于85°时，根据θ_end的值对最终停止的y值进行如下修正用修正后的y值代入两侧边沿所在直线方程y_right＝a_rightx+b_right和y_left＝a_leftx+b_left中去计算x_right和x_left，取(x_right+x_left)/2作为最终应停止的位置。

可选的，由于寻找车位时测到的车位值不能保证完全与实际车位大小一致，因此在计算得到x_right和x_left后，需要对车位的大小做修正，用x_right-x_left的值来代替初始测到的车位值。

可选的，对于只有一边停有障碍车的车位，则默认将车位大小固定为设定值。

可选的，对于只有一边停有障碍车的车位，则默认将车位大小固定为设定值。

本发明采用的技术方案，根据超声波雷达传感器检测两侧车辆与本车之间的距离信息，整车CAN信号传输回来的信息计算得到的本车车身的姿态角信息以及本车在由车位信息建立的虚拟坐标系下的位置信息来分别计算两侧车辆的角度以及在虚拟坐标系中的位置，然后对初始的车位信息进行校正，以避免在泊车过程中由于车身姿态角不对而出现碰撞事故。

本发明的具体技术方案及其有益效果将会在下面的具体实施方式中结合附图进行详细的说明。

附图说明

下面结合附图和具体实施方式对本发明作进一步描述：

图1为本发明流程图；

图2为建立虚拟坐标系示意图。

具体实施方式

如图1所示，一种垂直泊车的角度识别校正方法，包括如下步骤：

步骤S1，在本车的车尾进入车位后，安装在车尾侧面的两个雷达探头开始识别两侧障碍车；

步骤S2，对测到的雷达数据进行两次滤波得到比较连续的数据；

步骤S3，通过拟合直线的方式对数据进行处理计算得到两侧障碍车的车身角度；

步骤S4，根据两侧障碍车的车身角度来确定本车最终应停止的位置。

参考图2所示，以车位中心和本车后轴位置相交处为原点建立虚拟坐标系。泊车过程中，通过LIN通讯实时的采集在本车车身后方两侧的超声波雷达数据，通过CAN通讯实时收取车上CAN总线的数据，计算车身当前位置在虚拟坐标系中的值以及车身角度，当确定侧后方的两个雷达已经进入车位后，开始分别记录两侧雷达数据，并对记录的雷达数据进行初步的滤波处理。

其中，第一次滤波将偶尔出现的未测到数据的1～3个点通过插值法补全，，这样得到的数据就相对更连续一些。

考虑到汽车侧面比较平滑，雷达测到的数据应当为比较平滑且连续变化的数据点，而不应该有一些突变较大的数据点。针对这一情况，可以再一次对数据进行滤波处理，第二次滤波通过插值法将突变部分的数据替换掉。如果这部分数据中数据点个数足够多，那么便可以用于计算两侧障碍车的车身姿态角。若数据不够则继续重复上述记录数据及对数据滤波处理的步骤。

当雷达数据处理模块完成处理得到有效的数据点后，根据本车在虚拟坐标系中的位置、车身的姿态、雷达的数据，计算出每一个数据点对应两侧车辆边沿在坐标系中的点。计算公式如下：

x_target＝x-cos(θ)*dis±carwide/2*sin(θ)

y_target＝y-sin(θ)*dis±carwide/2*cos(θ)

其中，x和y为本车在虚拟坐标系中的位置，θ为当前车身的姿态角，dis为后侧方雷达与后轴之间的距离，carwide为车身宽度。

具体到本实施例，当计算得到的数据点的个数大于12个点时，即认为本组数据可以开始计算两侧车辆的姿态角。

姿态角的计算采用最小二乘法拟合直线的方法求得。根据计算得到的两侧车辆边沿在虚拟坐标系中的坐标值来进行拟合直线：假设车辆边沿所在直线的方程为y＝ax+b，那么根据最小二乘法有如下公式：

用虚拟坐标系中两侧车辆边沿的数据分别代入上式计算得到a_right、a_left、b_right、b_left，即两侧边沿所在直线方程为y_right＝a_rightx+b_right和y_left＝a_leftx+b_left，根据两直线方程的斜率α_right和a_left即可分别求得两侧车辆的姿态角θ_right＝arctan(a_right)和θ_left＝arctan(a_left)。

对于垂直车位，计算两侧车辆边沿在坐标系中y＝0时的x值x_right和x_left，当车位两侧均有车时，本车最终应停止的角度为(θ_right+θ_left)/2，最终应停止的位置则为(x_right+x_left)/2。当车位只有一侧有车时，以有车一侧计算得到的角度作为最终停止的角度，且默认将车位大小固定为设定值。例如以320cm的车位大小来确定最终停止的x值。

对于倾斜车位，采用垂直车位相同的计算方法计算本车最终应停止的角度θ_end，当计算得到的最终停止角度θ_end大于95°或者小于85°时，根据θ_end的值对最终停止的y值进行如下修正用修正后的y值代入两侧边沿所在直线方程y_right＝a_rightx+b_right和y_left＝a_leftx+b_left中去计算x_right和x_left，取(x_right+x_left)/2作为最终应停止的位置。

此外，由于寻找车位时测到的车位值不能保证完全与实际车位大小一致，因此在计算得到x_right和x_left后，需要对车位的大小做修正，用x_right-x_left的值来代替初始测到的车位值(即在寻找车位过程中测到的车位长度值)。

考虑到车辆在两侧都有车的车位中时，若对x值修正过大容易导致试验车在调整车身在车位中的位置时与两侧车辆发生碰撞，因此针对两侧都有车的车位，需要对计算得到的x值的大小做限制以避免发生车辆碰撞。对x值的限制方式如下条件：

本领域技术人员可以理解是，本发明中本车在虚拟坐标系中的位置，以车后轴中心在虚拟坐标系中的位置为准。另外，本发明涉及的长度单位均为厘米。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，熟悉该本领域的技术人员应该明白本发明包括但不限于上面具体实施方式中描述的内容。任何不偏离本发明的功能和结构原理的修改都将包括在权利要求书的范围中。

Claims (10)

1.一种垂直泊车的角度识别校正方法，其特征在于包括如下步骤：

步骤S1，在本车的车尾进入车位后，安装在车尾侧面的两个雷达探头开始识别两侧障碍车；

步骤S2，对测到的雷达数据进行两次滤波得到比较连续的数据；

步骤S3，通过拟合直线的方式对数据进行处理计算得到两侧障碍车的车身角度；

步骤S4，根据两侧障碍车的车身角度来确定本车最终应停止的位置。

2.根据权利要求1所述的一种垂直泊车的角度识别校正方法，其特征在于：以车位中心和本车后轴位置相交处为原点建立虚拟坐标系，计算车身当前位置在虚拟坐标系中的值以及车身角度。

3.根据权利要求2所述的一种垂直泊车的角度识别校正方法，其特征在于：第一次滤波将偶尔出现的未测到数据的1～3个点通过插值法补全，第二次滤波通过插值法将突变部分的数据替换掉。

4.根据权利要求3所述的一种垂直泊车的角度识别校正方法，其特征在于：根据本车在虚拟坐标系中的位置、车身的姿态、雷达的数据，计算出每一个数据点对应两侧车辆边沿在虚拟坐标系中的点。

5.根据权利要求4所述的一种垂直泊车的角度识别校正方法，其特征在于：当经过滤波后计算得到的两侧车辆边沿在坐标系中的数据点的个数大于12个点时，即认为本组数据可以开始计算两侧车辆的姿态角。

6.根据权利要求5所述的一种垂直泊车的角度识别校正方法，其特征在于：根据计算得到的两侧车辆边沿在虚拟坐标系中的坐标值来进行拟合直线：假设车辆边沿所在直线的方程为y＝ax+b，那么根据最小二乘法有如下公式：

用虚拟坐标系中两侧车辆边沿的数据分别代入上式计算得到a_right、a_left、b_right、b_left，即两侧边沿所在直线方程为y_right＝a_rightx+b_right和y_left＝a_leftx+b_left，根据两直线方程的斜率a_right和a_left即可分别求得两侧车辆的姿态角θ_right＝arctan(a_right)和θ_left＝arctan(a_left)。

7.根据权利要求6所述的一种垂直泊车的角度识别校正方法，其特征在于：对于垂直车位，计算两侧车辆边沿在坐标系中y＝0时的x值x_right和x_left，当车位两侧均有车时，本车最终应停止的角度为(θ_right+θ_left)/2，最终应停止的位置则为(x_right+x_left)/2；当车位只有一侧有车时，以有车一侧计算得到的角度作为最终停止的角度，以设定值的车位大小来确定最终停止的x值。

8.根据权利要求7所述的一种垂直泊车的角度识别校正方法，其特征在于：对于倾斜车位，采用垂直车位相同的计算方法计算本车最终应停止的角度θ_end，当计算得到的最终停止角度θ_end大于95°或者小于85°时，根据θ_end的值对最终停止的y值进行如下修正用修正后的y值代入两侧边沿所在直线方程y_right＝a_rightx+b_right和y_left＝a_leftx+b_left中去计算x_right和x_left，取(x_right+x_left)/2作为最终应停止的位置。

9.根据权利要求7或者8所述的一种垂直泊车的角度识别校正方法，其特征在于：由于寻找车位时测到的车位值不能保证完全与实际车位大小一致，因此在计算得到x_right和x_left后，需要对车位的大小做修正，用x_right-x_left的值来代替初始测到的车位值。

10.根据权利要求7或者8所述的一种垂直泊车的角度识别校正方法，其特征在于：对于只有一边停有障碍车的车位，则默认将车位大小固定为设定值。