CN106740818A - 一种基于eps的自动泊车路线规划方法及系统 - Google Patents

Abstract

本发明公开了一种基于EPS的自动泊车路线规划方法，包括以下步骤：S1、进行车位探测以获取有效车位信息；S2、根据上述有效车位信息为车辆选取目标停车策略，并根据目标停车策略引导车辆自动进入上述有效车位泊车；S3、记录车辆自动泊车过程中方向盘上力矩介入力度以及力矩介入时间，并将上述力矩介入力度与预设力矩阈值、力矩介入时间与预设时间阈值进行比较，当力矩介入力度超过预设力矩阈值且力矩介入时间超过预设时间阈值时，进入EPS助力模式；本发明公开了一种基于EPS的自动泊车系统，泊车引导模块根据车位探测模块的结果选取目标停车策略，模式切换模块根据自动泊车过程中方向盘上力矩介入力度以及力矩介入时间切换泊车模式。

Description

一种基于EPS的自动泊车路线规划方法及系统

技术领域

本发明涉及汽车辅助驾驶技术领域，尤其涉及一种基于EPS的自动泊车路线规划方法及系统。

背景技术

现如今汽车辅助驾驶发展越来越迅猛，无论合资还是自主品牌车企都加大对其研发力度，汽车智能化发展是当前最热的趋势。而城市日益增长的汽车与可利用的泊车空间越来越少的矛盾日益突出，在城市中的停车难度大大增加。因此，自动泊车系统便应运而生。

自动泊车系统(Automatic Parking System,APS)主要由感知单元、中央控制器、转向执行机构、和人-机交互系统组成。以EPS作为APS执行机构，接收泊车系统中央控制器发出的转向指令后执行转向操作，可有效减少成本、节省布置空间。APS可以有效地帮助驾驶员，尤其是新手，快速、安全地将汽车泊入车位，极大地减轻驾驶员泊车负担。

当前自动泊车的研究重点在于环境感知、路径规划、路径跟踪，而采用摄像头、红外传感器探测环境的方案相对采用超声波传感器的方案而言无疑增加了开发成本。采用GPS定位的路径规划、路径跟踪同样也会增大开发成本。同时出于安全保障考虑，自动泊车控制器需能与EPS控制器实时通信，实现泊车模式与EPS模式安全切换。因此研发出一种低成本、能适应不同起始点完成平行或垂直泊车的自动泊车系统，能快速普及在众多中低端车型上，极大地减轻驾驶员尤其是新手在较小车位泊车的负担。

发明内容

基于背景技术存在的技术问题，本发明提出了一种基于EPS的自动泊车路线规划方法及系统。

本发明提出的基于EPS的自动泊车路线规划方法，包括以下步骤：

S1、进行车位探测以获取有效车位信息；

S2、根据上述有效车位信息为车辆选取目标停车策略，并根据目标停车策略引导车辆自动进入上述有效车位泊车；

S3、记录步骤S2中车辆自动泊车过程中方向盘上力矩介入力度以及力矩介入时间，并将上述力矩介入力度与预设力矩阈值、力矩介入时间与预设时间阈值进行比较，当力矩介入力度超过预设力矩阈值且力矩介入时间超过预设时间阈值时，退出自动泊车模式，进入EPS助力模式。

优选地，步骤S1具体包括在车辆进行车位探测过程中根据下降沿、上升沿突变计算车位大小L_p和车位深度S，并根据车位大小L_p和车位深度S判断当前车位信息是否为有效车位信息；

优选地，当L_v≤L_p<L_lim、S≥S_lim时，判断当前垂直车位有效；

当L_p<L_lim、S<S_lim时，继续进行车位探测；

当L_p≥L_lim、S≥S_lim2时，判断当前平行车位有效；

其中，L_v、L_lim、S_lim、S_lim2均为预设值；

优选地，车位探测过程中采集车辆右前轮与车辆周边障碍物的最小距离，并判断上述最小距离是否有突变，当上述最小距离突然减小即为下降沿，当上述最小距离突然增大即为上升沿。

优选地，步骤S2具体包括：

当前垂直车位判断为有效时，执行垂直泊车路径规划策略；

当前平行车位判断为有效时，对车位大小L_p作进一步判断：

当L_lim≤L_p<L₀时，执行带有库内调整的平行泊车路径规划策略；

当L_p≥L₀时，执行平行泊车路径规划策略；

其中，L₀为预设值；

优选地，执行垂直泊车路径规划策略具体包括：

根据相应法规：得到垂直泊车路径规划策略中起始预设区域的范围

结合公式计算出车辆与车位左侧边界发生碰撞的临界点的横坐标x_{r_lim}，所述公式为：

其中，x_p为停车时车辆后轴中心横坐标，y_p为停车时车辆后轴中心纵坐标，L_r为车辆后悬长度，L_p为车位大小，w为车辆宽度，R为车辆泊车过程中的转弯半径；

再结合公式计算出车辆与车位右侧边界发生碰撞的临界点的纵坐标y_{r_lim}，所述公式为(x_p-L_p/2)²+(R-y_p)²＝(R-w/2)²；

其中，x_p的值取值x_{r_lim}的大小；

优选地，以x_p＝2、y_p＝1.1、y_p＝y_{r_lim}＝1.8、(x_p-L_p/2)²+(R-y_p)²＝(R-w/2)²为边界，计算出垂直泊车区域；

优选地，当前平行车位判断为有效时，对车位大小L_p作进一步判断具体包括：

判断此时车辆右前轮与车辆周边障碍物的最小距离h，计算出上述距离h对应的最佳起始点坐标C；记录车辆右前轮与车辆周边障碍物的最小距离Y₁、车辆右后轮与车辆周边障碍物的最小距离Y₀，并对Y₁、Y₀进行分析，当|(Y₁-Y₀)|/l≥θ₀时，重新进行车位探测；

且在车辆移动过程中对车位大小L_p作进一步分析：

当L_lim≤L_p<L₀时，执行带有库内调整的平行泊车路径规划策略，所述带有库内调整的平行泊车路径规划策略具体包括：

车辆泊车过程中对车辆头部与车辆周边障碍物的最小距离、车辆尾部与车辆周边障碍物的最小距离进行检测，并根据上述检测结果执行前进式调整策略和后退式调整策略；

优选地，当车辆尾部与车辆周边障碍物的最小距离小于预设值S1时，检测车辆尾部右侧与车辆周边障碍物的最小距离L7、车辆尾部左侧与车辆周边障碍物的最小距离L6，并对L6、L7进行分析：

当L7-L6≥ΔL时，执行方向盘右打死前进式调整策略，

当L6-L7≥ΔL时，执行方向盘左打死前进式调整策略，

否则，执行方向盘0度角前进式调整策略；

其中，ΔL为预设值；

优选地，当车辆头部与车辆周边障碍物的最小距离预设值S1时，检测车辆头部右侧与车辆周边障碍物的最小距离L3、车辆头部左侧与车辆前侧障碍物的最小距离L2，并对L2、L3进行分析：

当L3-L2≥ΔD时，执行方向盘右打死后退式调整策略，

当L2-L3≥ΔD时，执行方向盘左打死后退式调整策略，

否则，执行方向盘0度角前进式调整策略；

其中，ΔD为预设值；

当L_p≥L₀时，执行平行泊车路径规划策略，所述平行泊车路径规划策略具体包括：

令车辆起始点根据下述公式计算每段路径长度，所述公式为：

(R₁+R₂)sinθ+L₂cosθ＝S₀

(R₁+R₂)(1-cosθ)+L₂sinθ＝H

L1:(x-S₀-ΔS)²+[y-(H-R_min)]²＝R_min ²

L2:y-ax-b＝0

L3:(x-ΔS)²+(y-R_min)²＝R_min ²；

其中，R₁为第一段路径的圆弧半径，R₂为第三段路径的圆弧半径，优选地，R₁＝R₂，且R₁、R₂为最小转弯半径；θ为车辆偏航角，S₀为车辆起始点与终点的横向距离，H为车辆起始点与终点的纵向距离，ΔS为车辆后轴与车辆后侧障碍物的安全距离，L₁表示第一段路径，L₂表示第二段路径，L₃表示第三段路径；

最佳起始点C的坐标与距离h的关系为：

x_c＝a₁+h/tanθ₁

y_c＝b₁+h；

其中，θ₁为规划定值，a₁、b₁为设定值，h∈[0.1n₁-0.05,0.1n₂+0.05]，其中，n₁、n₂为正整数且n₂>n₁，若n₃∈[n₁,n₂]，且0.1n₃-0.05≤h＜0.1n₃+0.05，则h＝0.1n₃；即规划后的三段路径长度为：

L₁＝π*R_min*θ₁

L₂＝0.1/sinθ₁*(h-0.4)。

L₃＝π*R_min*θ₁

优选地，步骤S3具体包括：在车辆自动泊车过程中检测方向盘上力矩介入力度B以及力矩介入时间T，且当B>B₀、T>T₀时，退出自动泊车模式，进入EPS助力模式；

优选地，所述EPS助力模式具体包括EPS助力电机驱动车辆前轮自主转向，方向盘跟踪目标转角，执行规划好的路径，泊车控制器向车速控制器发送指令进行刹车，泊车完成后方向盘回正；

其中，B₀、T₀均为预设值。

本发明提出的基于EPS的自动泊车系统，包括：

车位探测模块，用于进行车位探测以获取有效车位信息；

泊车引导模块，用于根据上述有效车位信息为车辆选取目标停车策略，并根据目标停车策略引导车辆自动进入上述有效车位泊车；

模式切换模块，用于在车辆自动泊车过程中检测方向盘上力矩介入力度以及力矩介入时间，且当力矩介入力度超过预设力矩阈值且力矩介入时间超过预设时间阈值时，退出自动泊车模式，切换至EPS助力模式。

优选地，车位探测模块具体用于在车辆进行车位探测过程中根据下降沿、上升沿突变计算车位大小L_p和车位深度S，并根据车位大小L_p和车位深度S判断当前车位信息是否为有效车位信息；

优选地，当L_v≤L_p<L_lim、S≥S_lim时，判断当前垂直车位有效；

当L_p<L_lim、S<S_lim时，继续进行车位探测；

当L_p≥L_lim、S≥S_lim2时，判断当前平行车位有效；

其中，L_v、L_lim、S_lim、S_lim2均为预设值；

优选地，车位探测模块在车辆进行车位探测过程中采集车辆右前轮与车辆周边障碍物的最小距离，并判断上述最小距离是否有突变，当上述最小距离突然减小即为下降沿，当上述最小距离突然增大即为上升沿。

优选地，车位探测模块采用超声波传感器进行车位检测；

优选地，车位探测模块包括十个超声波传感器；

第一超声波传感器安装于车辆头部左侧边角处，第二超声波传感器安装于车辆左前灯的右端点处，第三超声波传感器安装于车辆右前灯的左端点处，第四超声波传感器安装于车辆头部右侧边角处，第五超声波传感器安装于车辆尾部左侧边角处，第六超声波传感器安装于车辆左后灯的右端点处，第七超声波传感器安装于车辆右后灯的左端点处，第八超声波传感器安装于车辆尾部右侧边角处，第九超声波传感器安装于车辆右前轮处，第十超声波传感器安装于车辆右后轮处。

优选地，泊车引导模块具体用于：

当前垂直车位判断为有效时，执行垂直泊车路径规划策略；

当前平行车位判断为有效时，对车位大小L_p作进一步判断：

当L_lim≤L_p<L₀时，执行带有库内调整的平行泊车路径规划策略；

当L_p≥L₀时，执行平行泊车路径规划策略；

其中，L₀为预设值；

优选地，执行垂直泊车路径规划策略具体包括：

根据相应法规：得到垂直泊车路径规划策略中起始预设区域的范围

结合公式计算出车辆与车位左侧边界发生碰撞的临界点的横坐标x_{r_lim}，所述公式为：

其中，x_p为停车时车辆后轴中心横坐标，y_p为停车时车辆后轴中心纵坐标，L_r为车辆后悬长度，L_p为车位大小，w为车辆宽度，R为车辆泊车过程中的转弯半径；

再结合公式计算出车辆与车位右侧边界发生碰撞的临界点的纵坐标y_{r_lim}，所述公式为(x_p-L_p/2)²+(R-y_p)²＝(R-w/2)²；

其中，x_p的值取值x_{r_lim}的大小；

优选地，以x_p＝2、y_p＝1.1、y_p＝y_{r_lim}＝1.8、(x_p-L_p/2)²+(R-y_p)²＝(R-w/2)²为边界，计算出垂直泊车区域；

优选地，当前平行车位判断为有效时，对车位大小L_p作进一步判断具体包括：

判断此时车辆右前轮与车辆周边障碍物的最小距离h，计算出上述距离h对应的最佳起始点坐标C；记录车辆右前轮与车辆周边障碍物的最小距离Y₁、车辆右后轮与车辆周边障碍物的最小距离Y₀，并对Y₁、Y₀进行分析，当|(Y₁-Y₀)|/l≥θ₀时，重新进行车位探测；

且在车辆移动过程中对车位大小L_p作进一步分析：

当L_lim≤L_p<L₀时，执行带有库内调整的平行泊车路径规划策略，所述带有库内调整的平行泊车路径规划策略具体包括：

车辆泊车过程中对车辆头部与车辆周边障碍物的最小距离、车辆尾部与车辆周边障碍物的最小距离进行检测，并根据上述检测结果执行前进式调整策略和后退式调整策略；

优选地，当车辆尾部与车辆周边障碍物的最小距离小于预设值S1时，检测车辆尾部右侧与车辆周边障碍物的最小距离L7、车辆尾部左侧与车辆周边障碍物的最小距离L6，并对L6、L7进行分析：

当L7-L6≥ΔL时，执行方向盘右打死前进式调整策略，

当L6-L7≥ΔL时，执行方向盘左打死前进式调整策略，

否则，执行方向盘0度角前进式调整策略；

其中，ΔL为预设值；

优选地，当车辆头部与车辆周边障碍物的最小距离预设值S1时，检测车辆头部右侧与车辆周边障碍物的最小距离L3、车辆头部左侧与车辆前侧障碍物的最小距离L2，并对L2、L3进行分析：

当L3-L2≥ΔD时，执行方向盘右打死后退式调整策略，

当L2-L3≥ΔD时，执行方向盘左打死后退式调整策略，

否则，执行方向盘0度角前进式调整策略；

其中，ΔD为预设值；

当L_p≥L₀时，执行平行泊车路径规划策略，所述平行泊车路径规划策略具体包括：

令车辆起始点根据下述公式计算每段路径长度，所述公式为：

(R₁+R₂)sinθ+L₂cosθ＝S₀

(R₁+R₂)(1-cosθ)+L₂sinθ＝H

L1:(x-S₀-ΔS)²+[y-(H-R_min)]²＝R_min ²

L2:y-ax-b＝0

L3:(x-ΔS)²+(y-R_min)²＝R_min ²；

其中，R₁为第一段路径的圆弧半径，R₂为第三段路径的圆弧半径，优选地，R₁＝R₂，且R₁、R₂为最小转弯半径；θ为车辆偏航角，S₀为车辆起始点与终点的横向距离，H为车辆起始点与终点的纵向距离，ΔS为车辆后轴与车辆后侧障碍物的安全距离，L₁表示第一段路径，L₂表示第二段路径，L₃表示第三段路径；

最佳起始点C的坐标与距离h的关系为：

x_c＝a₁+h/tanθ₁

y_c＝b₁+h；

其中，θ₁为规划定值，a₁、b₁为设定值，

h∈[0.1n₁-0.05,0.1n₂+0.05]，其中，n₁、n₂为正整数且n₂>n₁，若n₃∈[n₁,n₂]，且0.1n₃-0.05≤h＜0.1n₃+0.05，则h＝0.1n₃；即规划后的三段路径长度为：

L₁＝π*R_min*θ₁

L₂＝0.1/sinθ₁*(h-0.4)。

L₃＝π*R_min*θ₁

优选地，模式切换模块具体用于在车辆自动泊车过程中检测方向盘上力矩介入力度B以及力矩介入时间T，且当B>B₀、T>T₀时，退出自动泊车模式，进入EPS助力模式；

优选地，所述EPS助力模式具体包括EPS助力电机驱动车辆前轮自主转向，方向盘跟踪目标转角，执行规划好的路径，泊车控制器向车速控制器发送指令进行刹车，泊车完成后方向盘回正；

其中，B₀、T₀均为预设值。

本发明提出的一种基于EPS的自动泊车路线规划方法及系统，首先通过车身设置的检测模块进行车位检测以获取有效车位信息，进而根据上述有效车位信息为车辆选取目标停车策略，并根据目标停车策略引导车辆自动进入有效车位进行泊车，此过程中，通过检测模块对车辆与车辆周边的障碍物间的最小距离进行检测，并根据车辆与车辆周边的障碍物间的最小距离为车辆选取不同的前进和后退方案，保证车辆在车位不同状态下以及在车位不同大小的状态下进行移动，本发明给出的垂直泊车策略以及平行泊车策略能在车位不同状态以及不同大小的情况下进行泊车，实现车辆在不同起始点成功平行泊车和垂直泊车，进一步地，当平行车位较小时，则执行带有库内调整的平行泊车路径规划策略，使得车辆在车位较小的情况下依然可以顺利的停进车位；且本发明能够实现泊车模式与EPS助力模式安全切换，确保自动泊车过程的安全进行，在方便用户泊车的基础上保证了泊车过程的安全性。

附图说明

图1为一种基于EPS的自动泊车路线规划方法的步骤示意图；

图2为一种基于EPS的自动泊车系统的结构示意图；

图3为一种基于EPS的自动泊车路线规划方法及系统的垂直泊车区域划分示意图。

具体实施方式

如图1所示，图1、图2、图3为本发明提出的一种基于EPS的自动泊车路线规划方法及系统。

参照图1、图3，本发明提出的基于EPS的自动泊车路线规划方法，包括以下步骤：

S1、进行车位探测以获取有效车位信息；

具体包括在车辆进行车位探测过程中根据下降沿、上升沿突变计算车位大小L_p和车位深度S，并根据车位大小L_p和车位深度S判断当前车位信息是否为有效车位信息；

优选地，当L_v≤L_p<L_lim、S≥S_lim时，判断当前垂直车位有效；

当L_p<L_lim、S<S_lim时，继续进行车位探测；

当L_p≥L_lim、S≥S_lim2时，判断当前平行车位有效；

其中，L_v、L_lim、S_lim、S_lim2均为预设值；

优选地，车位探测过程中采集车辆右前轮与车辆周边障碍物的最小距离，并判断上述最小距离是否有突变，当上述最小距离突然减小即为下降沿，当上述最小距离突然增大即为上升沿。

S2、根据上述有效车位信息为车辆选取目标停车策略，并根据目标停车策略引导车辆自动进入上述有效车位泊车；

平行泊车过程中，所述路径“圆弧-直线-圆弧”中圆弧半径皆为最小转弯半径，每一个侧向距离h对应着不同的“圆弧-直线-圆弧”路径。在圆弧-直线-圆弧切换点处，车速控制器降低车速，减小方向盘在换向过程时车辆偏离规划路径的偏差；垂直泊车中，所述三段圆弧半径也皆为最小转弯半径，圆弧之间切换点需停车换挡；

具体包括：

当前垂直车位判断为有效时，执行垂直泊车路径规划策略；

当前平行车位判断为有效时，对车位大小L_p作进一步判断：

当L_lim≤L_p<L₀时，执行带有库内调整的平行泊车路径规划策略；

当L_p≥L₀时，执行平行泊车路径规划策略；

其中，L₀为预设值；

优选地，执行垂直泊车路径规划策略具体包括：

根据相应法规：得到垂直泊车路径规划策略中起始预设区域的范围

结合公式计算出车辆与车位左侧边界发生碰撞的临界点的横坐标x_{r_lim}，所述公式为：

其中，x_p为停车时车辆后轴中心横坐标，y_p为停车时车辆后轴中心纵坐标，L_r为车辆后悬长度，L_p为车位大小，w为车辆宽度，R为车辆泊车过程中的转弯半径；

再结合公式计算出车辆与车位右侧边界发生碰撞的临界点的纵坐标y_{r_lim}，所述公式为(x_p-L_p/2)²+(R-y_p)²＝(R-w/2)²；

其中，x_p的值取值x_{r_lim}的大小；

优选地，以x_p＝2、y_p＝1.1、y_p＝y_{r_lim}＝1.8、(x_p-L_p/2)²+(R-y_p)²＝(R-w/2)²为边界，计算出垂直泊车区域；

优选地，当前平行车位判断为有效时，对车位大小L_p作进一步判断具体包括：

判断此时车辆右前轮与车辆周边障碍物的最小距离h，计算出上述距离h对应的最佳起始点坐标C；记录车辆右前轮与车辆周边障碍物的最小距离Y₁、车辆右后轮与车辆周边障碍物的最小距离Y₀，并对Y₁、Y₀进行分析，当|(Y₁-Y₀)|/l≥θ₀时，重新进行车位探测；

且在车辆移动过程中对车位大小L_p作进一步分析：

当L_lim≤L_p<L₀时，执行带有库内调整的平行泊车路径规划策略，所述带有库内调整的平行泊车路径规划策略具体包括：

车辆泊车过程中对车辆头部与车辆周边障碍物的最小距离、车辆尾部与车辆周边障碍物的最小距离进行检测，并根据上述检测结果执行前进式调整策略和后退式调整策略；

优选地，当车辆尾部与车辆周边障碍物的最小距离小于预设值S1时，检测车辆尾部右侧与车辆周边障碍物的最小距离L7、车辆尾部左侧与车辆周边障碍物的最小距离L6，并对L6、L7进行分析：

当L7-L6≥ΔL时，执行方向盘右打死前进式调整策略，

当L6-L7≥ΔL时，执行方向盘左打死前进式调整策略，

否则，执行方向盘0度角前进式调整策略；

其中，ΔL为预设值；

优选地，当车辆头部与车辆周边障碍物的最小距离预设值S1时，检测车辆头部右侧与车辆周边障碍物的最小距离L3、车辆头部左侧与车辆前侧障碍物的最小距离L2，并对L2、L3进行分析：

当L3-L2≥ΔD时，执行方向盘右打死后退式调整策略，

当L2-L3≥ΔD时，执行方向盘左打死后退式调整策略，

否则，执行方向盘0度角前进式调整策略；

其中，ΔD为预设值；

当L_p≥L₀时，执行平行泊车路径规划策略，所述平行泊车路径规划策略具体包括：

令车辆起始点根据下述公式计算每段路径长度，所述公式为：

(R₁+R₂)sinθ+L₂cosθ＝S₀

(R₁+R₂)(1-cosθ)+L₂sinθ＝H

L1:(x-S₀-ΔS)²+[y-(H-R_min)]²＝R_min ²

L2:y-ax-b＝0

L3:(x-ΔS)²+(y-R_min)²＝R_min ²；

其中，R₁为第一段路径的圆弧半径，R₂为第三段路径的圆弧半径，优选地，R₁＝R₂，且R₁、R₂为最小转弯半径；θ为车辆偏航角，S₀为车辆起始点与终点的横向距离，H为车辆起始点与终点的纵向距离，ΔS为车辆后轴与车辆后侧障碍物的安全距离，L₁表示第一段路径，L₂表示第二段路径，L₃表示第三段路径；

最佳起始点C的坐标与距离h的关系为：

x_c＝a₁+h/tanθ₁

y_c＝b₁+h；

其中，θ₁为规划定值，a₁、b₁为设定值，h∈[0.1n₁-0.05,0.1n₂+0.05]，其中，n₁、n₂为正整数且n₂>n₁，若n₃∈[n₁,n₂]，且0.1n₃-0.05≤h＜0.1n₃+0.05，则h＝0.1n₃；即规划后的三段路径长度为：

L₁＝π*R_min*θ₁

L₂＝0.1/sinθ₁*(h-0.4)。

L₃＝π*R_min*θ₁

S3、记录步骤S2中车辆自动泊车过程中方向盘上力矩介入力度以及力矩介入时间，并将上述力矩介入力度与预设力矩阈值、力矩介入时间与预设时间阈值进行比较，当力矩介入力度超过预设力矩阈值且力矩介入时间超过预设时间阈值时，退出自动泊车模式，进入EPS助力模式；

具体包括：在车辆自动泊车过程中检测方向盘上力矩介入力度B以及力矩介入时间T，且当B>B₀、T>T₀时，退出自动泊车模式，进入EPS助力模式；

优选地，所述EPS助力模式具体包括EPS助力电机驱动车辆前轮自主转向，方向盘跟踪目标转角，执行规划好的路径，泊车控制器向车速控制器发送指令进行刹车，泊车完成后方向盘回正；其中，B₀、T₀均为预设值。

参照图2、图3，本发明提出的基于EPS的自动泊车系统，包括：

车位探测模块，用于进行车位探测以获取有效车位信息；

车位探测模块具体用于在车辆进行车位探测过程中根据下降沿、上升沿突变计算车位大小L_p和车位深度S，并根据车位大小L_p和车位深度S判断当前车位信息是否为有效车位信息；

优选地，当L_v≤L_p<L_lim、S≥S_lim时，判断当前垂直车位有效；

当L_p<L_lim、S<S_lim时，继续进行车位探测；

当L_p≥L_lim、S≥S_lim2时，判断当前平行车位有效；

其中，L_v、L_lim、S_lim、S_lim2均为预设值；

优选地，车位探测模块在车辆进行车位探测过程中采集车辆右前轮与车辆周边障碍物的最小距离，并判断上述最小距离是否有突变，当上述最小距离突然减小即为下降沿，当上述最小距离突然增大即为上升沿。

车位探测模块采用超声波传感器进行车位检测；

优选地，车位探测模块包括十个超声波传感器；

第一超声波传感器安装于车辆头部左侧边角处，第二超声波传感器安装于车辆左前灯的右端点处，第三超声波传感器安装于车辆右前灯的左端点处，第四超声波传感器安装于车辆头部右侧边角处，第五超声波传感器安装于车辆尾部左侧边角处，第六超声波传感器安装于车辆左后灯的右端点处，第七超声波传感器安装于车辆右后灯的左端点处，第八超声波传感器安装于车辆尾部右侧边角处，第九超声波传感器安装于车辆右前轮处，第十超声波传感器安装于车辆右后轮处。

泊车引导模块，用于根据上述有效车位信息为车辆选取目标停车策略，并根据目标停车策略引导车辆自动进入上述有效车位泊车；

泊车引导模块具体用于：

当前垂直车位判断为有效时，执行垂直泊车路径规划策略；

当前平行车位判断为有效时，对车位大小L_p作进一步判断：

当L_lim≤L_p<L₀时，执行带有库内调整的平行泊车路径规划策略；

当L_p≥L₀时，执行平行泊车路径规划策略；

其中，L₀为预设值；

优选地，执行垂直泊车路径规划策略具体包括：

根据相应法规：得到垂直泊车路径规划策略中起始预设区域的范围

结合公式计算出车辆与车位左侧边界发生碰撞的临界点的横坐标x_{r_lim}，所述公式为：

其中，x_p为停车时车辆后轴中心横坐标，y_p为停车时车辆后轴中心纵坐标，L_r为车辆后悬长度，L_p为车位大小，w为车辆宽度，R为车辆泊车过程中的转弯半径；

再结合公式计算出车辆与车位右侧边界发生碰撞的临界点的纵坐标y_{r_lim}，所述公式为(x_p-L_p/2)²+(R-y_p)²＝(R-w/2)²；

其中，x_p的值取值x_{r_lim}的大小；

优选地，以x_p＝2、y_p＝1.1、y_p＝y_{r_lim}＝1.8、(x_p-L_p/2)²+(R-y_p)²＝(R-w/2)²为边界，计算出垂直泊车区域；

优选地，当前平行车位判断为有效时，对车位大小L_p作进一步判断具体包括：

判断此时车辆右前轮与车辆周边障碍物的最小距离h，计算出上述距离h对应的最佳起始点坐标C；记录车辆右前轮与车辆周边障碍物的最小距离Y₁、车辆右后轮与车辆周边障碍物的最小距离Y₀，并对Y₁、Y₀进行分析，当|(Y₁-Y₀)|/l≥θ₀时，重新进行车位探测；

且在车辆移动过程中对车位大小L_p作进一步分析：

当L_lim≤L_p<L₀时，执行带有库内调整的平行泊车路径规划策略，所述带有库内调整的平行泊车路径规划策略具体包括：

车辆泊车过程中对车辆头部与车辆周边障碍物的最小距离、车辆尾部与车辆周边障碍物的最小距离进行检测，并根据上述检测结果执行前进式调整策略和后退式调整策略；

优选地，当车辆尾部与车辆周边障碍物的最小距离小于预设值S1时，检测车辆尾部右侧与车辆周边障碍物的最小距离L7、车辆尾部左侧与车辆周边障碍物的最小距离L6，并对L6、L7进行分析：

当L7-L6≥ΔL时，执行方向盘右打死前进式调整策略，

当L6-L7≥ΔL时，执行方向盘左打死前进式调整策略，

否则，执行方向盘0度角前进式调整策略；

其中，ΔL为预设值；

优选地，当车辆头部与车辆周边障碍物的最小距离预设值S1时，检测车辆头部右侧与车辆周边障碍物的最小距离L3、车辆头部左侧与车辆前侧障碍物的最小距离L2，并对L2、L3进行分析：

当L3-L2≥ΔD时，执行方向盘右打死后退式调整策略，

当L2-L3≥ΔD时，执行方向盘左打死后退式调整策略，

否则，执行方向盘0度角前进式调整策略；

其中，ΔD为预设值；

当L_p≥L₀时，执行平行泊车路径规划策略，所述平行泊车路径规划策略具体包括：

令车辆起始点根据下述公式计算每段路径长度，所述公式为：

(R₁+R₂)sinθ+L₂cosθ＝S₀

(R₁+R₂)(1-cosθ)+L₂sinθ＝H

L1:(x-S₀-ΔS)²+[y-(H-R_min)]²＝R_min ²

L2:y-ax-b＝0

L3:(x-ΔS)²+(y-R_min)²＝R_min ²；

其中，R₁为第一段路径的圆弧半径，R₂为第三段路径的圆弧半径，优选地，R₁＝R₂，且R₁、R₂为最小转弯半径；θ为车辆偏航角，S₀为车辆起始点与终点的横向距离，H为车辆起始点与终点的纵向距离，ΔS为车辆后轴与车辆后侧障碍物的安全距离，L₁表示第一段路径，L₂表示第二段路径，L₃表示第三段路径；

最佳起始点C的坐标与距离h的关系为：

x_c＝a₁+h/tanθ₁

y_c＝b₁+h；

其中，θ₁为规划定值，a₁、b₁为设定值，h∈[0.1n₁-0.05,0.1n₂+0.05]，其中，n₁、n₂为正整数且n₂>n₁，若n₃∈[n₁,n₂]，且0.1n₃-0.05≤h＜0.1n₃+0.05，则h＝0.1n₃；即规划后的三段路径长度为：

L₁＝π*R_min*θ₁

L₂＝0.1/sinθ₁*(h-0.4)。

L₃＝π*R_min*θ₁

模式切换模块，用于在车辆自动泊车过程中检测方向盘上力矩介入力度以及力矩介入时间，且当力矩介入力度超过预设力矩阈值且力矩介入时间超过预设时间阈值时，退出自动泊车模式，切换至EPS助力模式。

模式切换模块具体用于在车辆自动泊车过程中检测方向盘上力矩介入力度B以及力矩介入时间T，且当B>B₀、T>T₀时，退出自动泊车模式，进入EPS助力模式；

优选地，所述EPS助力模式具体包括EPS助力电机驱动车辆前轮自主转向，方向盘跟踪目标转角，执行规划好的路径，泊车控制器向车速控制器发送指令进行刹车，泊车完成后方向盘回正；其中，B₀、T₀均为预设值。

本实施方式中，所述超声波传感器为KS103型号，探测范围1cm～500cm或1cm～1100cm，具有实时的温度补偿功能，用于实时探测周边环境、车位测量、泊车过程防碰撞预警。

本实施方式中还设有轮速传感器、语音模块、车速控制器、转角传感器，所述轮速传感器为增量式光电旋转编码器，旋转一周共发出1440个脉冲信号，安装在两后轮，随后轮同轴转动，用于实时计算轮速、测量移动距离；所述语音模块为XFS5152CE系统，所述小音箱通过3.5mm耳机插口与其连接，用于语音提示驾驶员执行换挡和刹车操作；所述的车速控制模块包括车速控制器和真空助力泵，车速控制模块可对真空助力泵进行压力调节实现对车速在0～10km/h之间的速度控制；所述转角传感器为博世公司产品，用于采集方向盘实时转角信号；所述超声波传感器通过I2C通信与泊车控制器连接；所述轮速传感器通过CAN通信与泊车控制器连接；所述语音模块通过UART接口与泊车控制器通信连接；所述车速控制器通过CAN通信与泊车控制器连接；所述转角传感器通过CAN总线与泊车控制器、EPS控制器实时通信。

本实施方式提出的一种基于EPS的自动泊车路线规划方法及系统，首先通过车身设置的检测模块进行车位检测以获取有效车位信息，进而根据上述有效车位信息为车辆选取目标停车策略，并根据目标停车策略引导车辆自动进入有效车位进行泊车，此过程中，通过检测模块对车辆与车辆周边的障碍物间的最小距离进行检测，并根据车辆与车辆周边的障碍物间的最小距离为车辆选取不同的前进和后退方案，保证车辆在车位不同状态下以及在车位不同大小的状态下进行移动，给出的垂直泊车策略以及平行泊车策略能在车位不同状态以及不同大小的情况下进行泊车，实现车辆在不同起始点成功平行泊车和垂直泊车，进一步地，当平行车位较小时，则执行带有库内调整的平行泊车路径规划策略，使得车辆在车位较小的情况下依然可以顺利的停进车位；且能够实现泊车模式与EPS助力模式安全切换，确保自动泊车过程的安全进行，在方便用户泊车的基础上保证了泊车过程的安全性。

本实施方式提供一种基于EPS的自动泊车系统，与现有技术方案相比，开发成本低，能够实现泊车与EPS模式安全切换，确保自动泊车过程的安全进行；本实施方式所提供的一种自动泊车路径规划方法，简单且实现，经过简单的参数改变设置，能移植到不同车型上，能实现在不同的起始点成功平行或者泊车。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (9)

1.一种基于EPS的自动泊车路线规划方法，其特征在于，包括以下步骤：

S1、进行车位探测以获取有效车位信息；

S2、根据上述有效车位信息为车辆选取目标停车策略，并根据目标停车策略引导车辆自动进入上述有效车位泊车；

S3、记录步骤S2中车辆自动泊车过程中方向盘上力矩介入力度以及力矩介入时间，并将上述力矩介入力度与预设力矩阈值、力矩介入时间与预设时间阈值进行比较，当力矩介入力度超过预设力矩阈值且力矩介入时间超过预设时间阈值时，退出自动泊车模式，进入EPS助力模式。

2.根据权利要求1所述的基于EPS的自动泊车路线规划方法，其特征在于，步骤S1具体包括在车辆进行车位探测过程中根据下降沿、上升沿突变计算车位大小L_p和车位深度S，并根据车位大小L_p和车位深度S判断当前车位信息是否为有效车位信息；

优选地，当L_v≤L_p<L_lim、S≥S_lim时，判断当前垂直车位有效；

当L_p<L_lim、S<S_lim时，继续进行车位探测；

当L_p≥L_lim、S≥S_lim2时，判断当前平行车位有效；

其中，L_v、L_lim、S_lim、S_lim2均为预设值；

优选地，车位探测过程中采集车辆右前轮与车辆周边障碍物的最小距离，并判断上述最小距离是否有突变，当上述最小距离突然减小即为下降沿，当上述最小距离突然增大即为上升沿。

3.根据权利要求2所述的基于EPS的自动泊车路线规划方法，其特征在于，步骤S2具体包括：

当前垂直车位判断为有效时，执行垂直泊车路径规划策略；

当前平行车位判断为有效时，对车位大小L_p作进一步判断：

当L_lim≤L_p<L₀时，执行带有库内调整的平行泊车路径规划策略；

当L_p≥L₀时，执行平行泊车路径规划策略；

其中，L₀为预设值；

优选地，执行垂直泊车路径规划策略具体包括：

根据相应法规：L_p/2≤(x_p-L_r)≤2L_p、ε＝0.2m，得到垂直泊车路径规划策略中起始预设区域的范围

结合公式计算出车辆与车位左侧边界发生碰撞的临界点的横坐标x_{r_lim}，所述公式为：

其中，x_p为停车时车辆后轴中心横坐标，y_p为停车时车辆后轴中心纵坐标，L_r为车辆后悬长度，L_p为车位大小，w为车辆宽度，R为车辆泊车过程中的转弯半径；

再结合公式计算出车辆与车位右侧边界发生碰撞的临界点的纵坐标y_{r_lim}，所述公式为(x_p-L_p/2)²+(R-y_p)²＝(R-w/2)²；

其中，x_p的值取值x_{r_lim}的大小；

优选地，以x_p＝2、y_p＝1.1、y_p＝y_{r_lim}＝1.8、(x_p-L_p/2)²+(R-y_p)²＝(R-w/2)²为边界，计算出垂直泊车区域；

优选地，当前平行车位判断为有效时，对车位大小L_p作进一步判断具体包括：

判断此时车辆右前轮与车辆周边障碍物的最小距离h，计算出上述距离h对应的最佳起始点坐标C；记录车辆右前轮与车辆周边障碍物的最小距离Y₁、车辆右后轮与车辆周边障碍物的最小距离Y₀，并对Y₁、Y₀进行分析，当|(Y₁-Y₀)|/l≥θ₀时，重新进行车位探测；

且在车辆移动过程中对车位大小L_p作进一步分析：

当L_lim≤L_p<L₀时，执行带有库内调整的平行泊车路径规划策略，所述带有库内调整的平行泊车路径规划策略具体包括：

车辆泊车过程中对车辆头部与车辆周边障碍物的最小距离、车辆尾部与车辆周边障碍物的最小距离进行检测，并根据上述检测结果执行前进式调整策略和后退式调整策略；

优选地，当车辆尾部与车辆周边障碍物的最小距离小于预设值S1时，检测车辆尾部右侧与车辆周边障碍物的最小距离L7、车辆尾部左侧与车辆周边障碍物的最小距离L6，并对L6、L7进行分析：

当L7-L6≥ΔL时，执行方向盘右打死前进式调整策略，

当L6-L7≥ΔL时，执行方向盘左打死前进式调整策略，

否则，执行方向盘0度角前进式调整策略；

其中，ΔL为预设值；

优选地，当车辆头部与车辆周边障碍物的最小距离预设值S1时，检测车辆头部右侧与车辆周边障碍物的最小距离L3、车辆头部左侧与车辆前侧障碍物的最小距离L2，并对L2、L3进行分析：

当L3-L2≥ΔD时，执行方向盘右打死后退式调整策略，

当L2-L3≥ΔD时，执行方向盘左打死后退式调整策略，

否则，执行方向盘0度角前进式调整策略；

其中，ΔD为预设值；

当L_p≥L₀时，执行平行泊车路径规划策略，所述平行泊车路径规划策略具体包括：

令车辆起始点根据下述公式计算每段路径长度，所述公式为：

(R₁+R₂)sinθ+L₂cosθ＝S₀

(R₁+R₂)(1-cosθ)+L₂sinθ＝H

L1:(x-S₀-ΔS)²+[y-(H-R_min)]²＝R_min ²

L2:y-ax-b＝0

L3:(x-ΔS)²+(y-R_min)²＝R_min ²；

其中，R₁为第一段路径的圆弧半径，R₂为第三段路径的圆弧半径，优选地，R₁＝R₂，且R₁、R₂为最小转弯半径；θ为车辆偏航角，S₀为车辆起始点与终点的横向距离，H为车辆起始点与终点的纵向距离，ΔS为车辆后轴与车辆后侧障碍物的安全距离，L₁表示第一段路径，L₂表示第二段路径，L₃表示第三段路径；

最佳起始点C的坐标与距离h的关系为：

$\begin{array}{c}{x}_c=a_1+h/{\mathrm{tan\&theta;}}_1\\ y_c=b_1+h\end{array};$

其中，θ₁为规划定值，a₁、b₁为设定值，h∈[0.1n₁-0.05,0.1n₂+0.05]，其中，n₁、n₂为正整数且n₂>n₁，若n₃∈[n₁,n₂]，且0.1n₃-0.05≤h＜0.1n₃+0.05，则h＝0.1n₃；即规划后的三段路径长度为：

$\begin{array}{c}{L}_1=\mathrm{\&pi;}*R_{\min }*{\mathrm{\&theta;}}_1\\ L_2=0.1/{\mathrm{sin\&theta;}}_1*\left(h-0.4\right)\\ L_3=\mathrm{\&pi;}*R_{\min }*{\mathrm{\&theta;}}_1\end{array}.$

4.根据权利要求1所述的基于EPS的自动泊车路线规划方法，其特征在于，步骤S3具体包括：在车辆自动泊车过程中检测方向盘上力矩介入力度B以及力矩介入时间T，且当B>B₀、T>T₀时，退出自动泊车模式，进入EPS助力模式；

优选地，所述EPS助力模式具体包括EPS助力电机驱动车辆前轮自主转向，方向盘跟踪目标转角，执行规划好的路径，泊车控制器向车速控制器发送指令进行刹车，泊车完成后方向盘回正；

其中，B₀、T₀均为预设值。

5.一种基于EPS的自动泊车系统，其特征在于，包括：

车位探测模块，用于进行车位探测以获取有效车位信息；

泊车引导模块，用于根据上述有效车位信息为车辆选取目标停车策略，并根据目标停车策略引导车辆自动进入上述有效车位泊车；

模式切换模块，用于在车辆自动泊车过程中检测方向盘上力矩介入力度以及力矩介入时间，且当力矩介入力度超过预设力矩阈值且力矩介入时间超过预设时间阈值时，退出自动泊车模式，切换至EPS助力模式。

6.根据权利要求5所述的基于EPS的自动泊车系统，其特征在于，车位探测模块具体用于在车辆进行车位探测过程中根据下降沿、上升沿突变计算车位大小L_p和车位深度S，并根据车位大小L_p和车位深度S判断当前车位信息是否为有效车位信息；

优选地，当L_v≤L_p<L_lim、S≥S_lim时，判断当前垂直车位有效；

当L_p<L_lim、S<S_lim时，继续进行车位探测；

当L_p≥L_lim、S≥S_lim2时，判断当前平行车位有效；

其中，L_v、L_lim、S_lim、S_lim2均为预设值；

优选地，车位探测模块在车辆进行车位探测过程中采集车辆右前轮与车辆周边障碍物的最小距离，并判断上述最小距离是否有突变，当上述最小距离突然减小即为下降沿，当上述最小距离突然增大即为上升沿。

7.根据权利要求5所述的基于EPS的自动泊车系统，其特征在于，车位探测模块采用超声波传感器进行车位检测；

优选地，车位探测模块包括十个超声波传感器；

第一超声波传感器安装于车辆头部左侧边角处，第二超声波传感器安装于车辆左前灯的右端点处，第三超声波传感器安装于车辆右前灯的左端点处，第四超声波传感器安装于车辆头部右侧边角处，第五超声波传感器安装于车辆尾部左侧边角处，第六超声波传感器安装于车辆左后灯的右端点处，第七超声波传感器安装于车辆右后灯的左端点处，第八超声波传感器安装于车辆尾部右侧边角处，第九超声波传感器安装于车辆右前轮处，第十超声波传感器安装于车辆右后轮处。

8.根据权利要求6所述的基于EPS的自动泊车系统，其特征在于，泊车引导模块具体用于：

当前垂直车位判断为有效时，执行垂直泊车路径规划策略；

当前平行车位判断为有效时，对车位大小L_p作进一步判断：

当L_lim≤L_p<L₀时，执行带有库内调整的平行泊车路径规划策略；

当L_p≥L₀时，执行平行泊车路径规划策略；

其中，L₀为预设值；

优选地，执行垂直泊车路径规划策略具体包括：

根据相应法规：L_p/2≤(x_p-L_r)≤2L_p、ε＝0.2m，得到垂直泊车路径规划策略中起始预设区域的范围

结合公式计算出车辆与车位左侧边界发生碰撞的临界点的横坐标x_{r_lim}，所述公式为：

其中，x_p为停车时车辆后轴中心横坐标，y_p为停车时车辆后轴中心纵坐标，L_r为车辆后悬长度，L_p为车位大小，w为车辆宽度，R为车辆泊车过程中的转弯半径；

再结合公式计算出车辆与车位右侧边界发生碰撞的临界点的纵坐标y_{r_lim}，所述公式为(x_p-L_p/2)²+(R-y_p)²＝(R-w/2)²；

其中，x_p的值取值x_{r_lim}的大小；

优选地，以x_p＝2、y_p＝1.1、y_p＝y_{r_lim}＝1.8、(x_p-L_p/2)²+(R-y_p)²＝(R-w/2)²为边界，计算出垂直泊车区域；

优选地，当前平行车位判断为有效时，对车位大小L_p作进一步判断具体包括：

判断此时车辆右前轮与车辆周边障碍物的最小距离h，计算出上述距离h对应的最佳起始点坐标C；记录车辆右前轮与车辆周边障碍物的最小距离Y₁、车辆右后轮与车辆周边障碍物的最小距离Y₀，并对Y₁、Y₀进行分析，当|(Y₁-Y₀)|/l≥θ₀时，重新进行车位探测；

且在车辆移动过程中对车位大小L_p作进一步分析：

当L_lim≤L_p<L₀时，执行带有库内调整的平行泊车路径规划策略，所述带有库内调整的平行泊车路径规划策略具体包括：

车辆泊车过程中对车辆头部与车辆周边障碍物的最小距离、车辆尾部与车辆周边障碍物的最小距离进行检测，并根据上述检测结果执行前进式调整策略和后退式调整策略；

优选地，当车辆尾部与车辆周边障碍物的最小距离小于预设值S1时，检测车辆尾部右侧与车辆周边障碍物的最小距离L7、车辆尾部左侧与车辆周边障碍物的最小距离L6，并对L6、L7进行分析：

当L7-L6≥ΔL时，执行方向盘右打死前进式调整策略，

当L6-L7≥ΔL时，执行方向盘左打死前进式调整策略，

否则，执行方向盘0度角前进式调整策略；

其中，ΔL为预设值；

优选地，当车辆头部与车辆周边障碍物的最小距离预设值S1时，检测车辆头部右侧与车辆周边障碍物的最小距离L3、车辆头部左侧与车辆前侧障碍物的最小距离L2，并对L2、L3进行分析：

当L3-L2≥ΔD时，执行方向盘右打死后退式调整策略，

当L2-L3≥ΔD时，执行方向盘左打死后退式调整策略，

否则，执行方向盘0度角前进式调整策略；

其中，ΔD为预设值；

当L_p≥L₀时，执行平行泊车路径规划策略，所述平行泊车路径规划策略具体包括：

令车辆起始点根据下述公式计算每段路径长度，所述公式为：

(R₁+R₂)sinθ+L₂cosθ＝S₀

(R₁+R₂)(1-cosθ)+L₂sinθ＝H

L1:(x-S₀-ΔS)²+[y-(H-R_min)]²＝R_min ²

L2:y-ax-b＝0

L3:(x-ΔS)²+(y-R_min)²＝R_min ²；

其中，R₁为第一段路径的圆弧半径，R₂为第三段路径的圆弧半径，优选地，R₁＝R₂，且R₁、R₂为最小转弯半径；θ为车辆偏航角，S₀为车辆起始点与终点的横向距离，H为车辆起始点与终点的纵向距离，ΔS为车辆后轴与车辆后侧障碍物的安全距离，L₁表示第一段路径，L₂表示第二段路径，L₃表示第三段路径；

最佳起始点C的坐标与距离h的关系为：

$\begin{array}{c}{x}_c=a_1+h/{\mathrm{tan\&theta;}}_1\\ y_c=b_1+h\end{array};$

其中，θ₁为规划定值，a₁、b₁为设定值，h∈[0.1n₁-0.05,0.1n₂+0.05]，其中，n₁、n₂为正整数且n₂>n₁，若n₃∈[n₁,n₂]，且0.1n₃-0.05≤h＜0.1n₃+0.05，则h＝0.1n₃；即规划后的三段路径长度为：

$\begin{array}{c}{L}_1=\mathrm{\&pi;}*R_{\min }*{\mathrm{\&theta;}}_1\\ L_2=0.1/{\mathrm{sin\&theta;}}_1*\left(h-0.4\right)\\ L_3=\mathrm{\&pi;}*R_{\min }*{\mathrm{\&theta;}}_1\end{array}.$

9.根据权利要求5所述的基于EPS的自动泊车系统，其特征在于，模式切换模块具体用于在车辆自动泊车过程中检测方向盘上力矩介入力度B以及力矩介入时间T，且当B>B₀、T>T₀时，退出自动泊车模式，进入EPS助力模式；

优选地，所述EPS助力模式具体包括EPS助力电机驱动车辆前轮自主转向，方向盘跟踪目标转角，执行规划好的路径，泊车控制器向车速控制器发送指令进行刹车，泊车完成后方向盘回正；

其中，B₀、T₀均为预设值。