CN112414422A

CN112414422A - 一种自动泊车的路径规划方法、装置及存储介质

Info

Publication number: CN112414422A
Application number: CN202011199165.8A
Authority: CN
Inventors: 任秉韬; 王淅淅; 邓伟文; 南江峰; 赵蕊
Original assignee: Beihang University
Current assignee: Beihang University
Priority date: 2020-11-01
Filing date: 2020-11-01
Publication date: 2021-02-26

Abstract

本申请公开了一种自动泊车的路径规划方法、装置及存储介质，用于提升路径搜索的灵活性，搜索出更优的路径。本申请公开的自动泊车的路径规划方法包括：确定从起点S到泊车起始点P的第一路径；确定从所述泊车起始点P到目标点F的第二路径；对所述第一路径和所述第二路径进行平滑处理。本申请还提供了一种自动泊车的路径规划装置及存储介质。

Description

一种自动泊车的路径规划方法、装置及存储介质

技术领域

本申请涉及自动控制领域，尤其涉及一种自动泊车的路径规划方法、装置和存储介质。

背景技术

作为自动泊车***的重要组成部分，路径规划是确保行车安全、缩短行车距离和提高乘客行驶舒适性的关键。路径规划与轨迹跟随控制对于自动泊车***至关重要，是自动驾驶技术研究的热点问题。在现有技术中，规划的路径存在路程较长、频繁变换行驶方向、频繁泊车动作不合理等问题，并且路径不够平滑增加了汽车轮胎的磨损程度和轨迹跟随控制的难度。

发明内容

针对上述技术问题，本申请实施例提供了一种自动泊车的路径规划方法、装置及存储介质，用以提升路径搜索的灵活性，搜索出距离短且平滑的泊车路径。

第一方面，本申请实施例提供的一种自动泊车的路径规划方法，包括：

确定从起点S到泊车起始点P的第一路径；

确定从所述泊车起始点P到目标点F的第二路径；

对所述第一路径和所述第二路径进行平滑处理。

优选的，所述确定从起点S到泊车起始点P的第一路径包括：

使用混合A*搜索算法确定从所述起点S到所述泊车起始点P的第一路径。

优选的，所述混合A*搜索算法包括：

从所述起点S开始，以固定半径的RS曲线的混合A*算法作为成本进行搜索，若车身无碰撞，则保存搜索路径；

判断树枝节点是否到达所述泊车地点P，若已到达且无碰撞，则记录最后一个树枝节点为所述泊车地点P；

其中，所述RS曲线为Reeds-Shepp曲线。

优选的，所述以固定半径的RS曲线作为成本进行搜索包括：

将最小转弯半径作为所述A*搜索的约束；

每隔M个点使用一次所述RS曲线进行路径搜索；

每成功完成一次所述混合A*搜索进行一次碰撞检测；

其中，M是大于等于1的整数。

进一步的，所述确定从所述泊车起始点P到目标点F的第二路径包括：

使用可变半径的RS曲线搜索算法确定所述泊车起始点P到所述目标点F的第二路径；

其中，所述RS曲线为Reeds-Shepp曲线。

进一步的，所述对所述第一路径和所述第二路径进行平滑处理包括：

对所述第一路径使用贝塞尔曲线进行拟合，得到第三路径；

对所述第二路径使用贝塞尔曲线进行拟合，得到第四路径；

对所述第三路径和所述第四路径，使用共轭梯度下降方法进行路径优化。

优选的，所述使用共轭梯度下降方法进行路径优化包括：

根据下列公式对待优化的路径进行优化：

其中，第一项为边缘碰撞的惩罚项，第二项为曲率惩罚项；

x_i＝(x_i，y_i)，i∈[1，...，N]，x_i为使用贝塞尔曲线进行拟合后的待优化的路径；

o_i为车库边缘的坐标点；

Δx_i＝x_i-x_i-1是在点x_i处的位移量，

是所述待优化的路径在点x_i处的角度改变量，k_max是车辆转弯的最大曲率，ω_o、ω_k是权重因子，σ_o边缘碰撞的惩罚函数，σ_k为曲率惩罚函数。

所述边缘碰撞惩罚项的梯度为：

所述曲率惩罚的梯度为：

曲率k_i＝Δφ_i/(|Δx_i|)与x_i-1、x_i和x_i+1的偏导之和，

其中，

x_i是当前点，x_i-1是当前点的前一点，x_i+1是当前点的后一点。

进一步的，本发明提供的自动泊车的路径规划方法，还包括：

根据路径评价函数选出最优的路径，

所述路径评价函数为：

其中，S_i为第i条路径的评价函数，R_i为第i条路径的正负转换次数，L_i为第i条路径的路径总长度，ω_l为正负转换次数的权重系数，ω_p为路径总长度的权重系数；

评价函数值最小的路径为最优的路径。

所述正负转换为当前时刻与前一时刻的速度方向相反。

所述路径总长度通过下列公式确定：

L＝∑sqrt((x(i+1)-x(i))²+(y(i+1)-y(i))²)，

其中，(x_i，y_i)为所述路径的坐标，i为大于等于1小于等于N的整数，N为所述路径的计算点总数。

使用本发明提供的自动泊车的路径规划方法，首先利用混合A*进行泊车轨迹的全局搜索得到泊车起始点P，接着采用可变半径Reeds-Shepp曲线方法(VR-RS)规划泊车的倒车路径P-F，从而得到S-P-F路径；然后采用分段贝塞尔曲线和梯度下降路径进行搜索路径的平滑再优化；最后根据设计评价函数来选出最优的路径。本发明的方法可提升路径搜索的灵活性，搜索出距离短且平滑的泊车路径。

第二方面，本申请实施例还提供一种自动泊车的路径规划装置，包括：

第一规划模块，用于确定从起点S到泊车起始点P的第一路径；

第二规划模块，用于确定从所述泊车起始点P到目标点F的第二路径；

平滑处理模块，用于对所述第一路径和所述第二路径进行平滑处理。

优选的，本实施例的自动泊车的路径规划装置还可以包括路径评价模块，用于根据路径评价函数选出最优的路径，其中评价函数值最小的路径为最优的路径。

第三方面，本申请实施例还提供一种自动泊车的路径规划装置，包括：存储器、处理器和用户接口；

所述存储器，用于存储计算机程序；

所述用户接口，用于与用户实现交互；

所述处理器，用于读取所述存储器中的计算机程序，所述处理器执行所述计算机程序时，实现本发明提供的自动泊车的路径规划方法。

第四方面，本申请实施例还提供一种处理器可读存储介质，所述处理器可读存储介质存储有计算机程序，所述处理器执行所述计算机程序时实现本发明提供的自动泊车的路径规划方法。

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简要介绍，显而易见地，下面描述中的附图仅是本申请的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本申请实施例提供的自动泊车的路径组成示意图；

图2为本申请实施例提供的自动泊车的路径规划方法流程示意图；

图3为本申请实施例提供的路径平滑处理效果示意图；

图4为本申请实施例提供的自动泊车的路径规划装置组成示意图；

图5为本申请实施例提供的另一种自动泊车的路径规划装置结构示意图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部份实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

下面对文中出现的一些词语进行解释：

1、本发明实施例中术语“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。

2、本申请实施例中术语“多个”是指两个或两个以上，其它量词与之类似。

3、RS曲线，即Reeds-Shepp曲线的简称，假设车辆能以最小转弯半径转向，能够前进和后退，且行驶空间无约束，那么Reeds-Shepp曲线就是车辆在上述条件下从起点到终点的最短路径。该曲线不仅能保证车辆能够到达终点，而且能保证车辆的角度能在终点到达预期角度。

4、VR-RS曲线，即可变半径Reeds-Shepp曲线，取消RS曲线最小转弯半径的约束，改用不同大小的转弯半径进行路径搜索。其含不同半径的优势，在实际泊车场景中具有更好的避撞能力，搜到更优的路径。

5、混合A*搜索方法，根据A*算法通过修改子节点扩展方式和成本计算方式，进行改进的路径搜索算法，使搜索出的路径满足车辆动力学约束。

作为自动泊车***的重要组成部分，路径规划是确保行车安全、缩短行车距离和提高乘客行驶舒适性的关键。路径规划与轨迹跟随控制对于自动泊车***至关重要，是自动驾驶技术研究的热点问题。路径规划可描述为一个满足行驶安全性约束、车辆运动学约束的优化控制问题，以搜索和输出最短避撞和可行驶的路径。目前，规划的路径存在路程较长、频繁变换行驶方向、频繁泊车动作不合理等问题，并且路径不够平滑增加了汽车轮胎的磨损程度和轨迹跟随控制的难度。针对上述技术问题，本发明实施例提供了一种自动泊车的路径规划方法、装置和存储介质，本发明的方法基于VR-RS曲线与混合A*的路径优化，首先基于混合A*算法与VR-RS算法在障碍物地图中搜索出起始点到目标的路径，然后采用分段贝塞尔曲线和共轭梯度下降方法对路径进行平滑优化。

如图1所示，自动泊车过程为从起点S开始到车辆进入车位F的过程，本申请实施例中，将泊车过程切分为两个阶段，从起点S前进到倒车起点P为第一阶段，从起点P到车入车位F为第二阶段。针对第一阶段和第二阶段，分别使用不同的搜索方法，从而搜索出更优的路径。

为了更清楚的描述本发明的技术方案，下面对A*算法、混合A*算法，RS曲线可变半径调节等进行说明。

A*算法：A*算法是一种状态空间启发式算法，能够在搜索过程中引入启发信息，对状态空间中的每一个搜索的位置进行评估，得到最好的位置，再从这个位置进行下一步的搜索直到目标。这样可以省略大量无效的搜索路径，提了效率。在图搜索过程中，每个节点包含一个评价函数：

f(n)＝g(n)+h(n) (1)

公式(1)中：f(n)是从初始状态经由状态n到目标状态的代价估计量；g(n)是在状态空间中从初始状态到状态n的实际代价；h(n)是从状态n到目标状态的最佳路径的代价估计。其中状态表示为搜索图中的节点，代价表示为距离。

混合A*算法：混合A*算法的搜索空间(x，y，θ)是离散的。但与A*算法将成本h(n)与单元格中心相关联并且仅访问与网格单元格中心相对应的状态的特点不同，混合A*算法是将车辆连续状态与每个单元格节点相关联，将汽车最小转弯半径作为约束，以其关联的连续状态的成本h(n)作为单元格的分数。因此混合A*算法不仅可以规划出从起点到终点的连续路径，还能保证车头方向与要求一致，满足车辆运动的非结构约束限制。

RS曲线可变半径调节：在开放环境下，RS曲线可规划出从起点到目标点的最短路径，该路径不仅能保证车辆能够到达终点，而且能保证车辆的角度能在终点到达预期角度。但是在实际汽车驾驶场景下，当以最小转弯半径进行路径规划时，由于障碍物的存在，固定的转弯半径可能会导致规划出的路径更加复杂，增加不必要的较大角度转向，从而加剧汽车轮胎的磨损，搜索出的路径在实际中不易跟踪，导致车辆实际转弯半径大于理论车辆最小转弯半径，引起跟踪误差累计，使车辆更加偏离规划的路径。而可调转弯半径的RS曲线具有更强的环境适应性，即在搜索过程中，转弯半径不是固定的，而是可调节的。

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，并不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

需要说明的是，本申请实施例的展示顺序仅代表实施例的先后顺序，并不代表实施例所提供的技术方案的优劣。

实施例一

参见图2，本申请实施例提供的一种自动泊车的路径规划方法示意图，如图所示，该方法包括步骤S201到S203：

S201、确定从起点S到泊车起始点P的第一路径；

S202、确定从所述泊车起始点P到目标点F的第二路径；

S203、对所述第一路径和所述第二路径进行平滑处理。

本实施例中，将泊车过程切分为两个阶段，第一段是从起点S开始，以前进方向到达倒车起点P。第二段是从起点P，倒入车位，到达停车点F。

作为一个优选示例，第一段是由混合A*从起点S开始，以基于固定半径RS曲线的混合A*算法作为成本进行的扩展式搜索，从S点到P点为混合A*扩展式搜索所得；第二段是采用VR-RS曲线方法进行入位搜索，完成泊车路径规划，即从P到F段。

在上述步骤S201中，使用混合A*搜索算法确定从所述起始点S到所述泊车起点P的第一路径，所述的混合A*搜索算法包括：从所述起始点S开始，以固定半径的RS曲线的混合A*作为成本进行搜索，若车身无碰撞，则保存搜索路径；

其中，所述RS曲线为Reeds-Shepp曲线。

优选的，所述以固定半径的RS曲线作为成本进行搜索包括：

将最小转弯半径作为所述A*搜索的约束；

每隔M个点使用一次所述RS曲线进行路径搜索；

每成功完成一次所述混合A*搜索进行一次碰撞检测；

其中，M是大于等于1的整数。

具体来说，在第一段搜索过程中，用混合A*从车辆起点S到泊车起始点P，以固定半径的RS或A*作为成本进行扩展式搜索，若车身无碰撞，便从节点反向从树枝到树根保留路径；之后判断树枝节点是否到达泊车起始点(即从此节点是否能搜到到达目标点F的无碰撞可变RS曲线)，若已到达，记此时最后一个节点为P；此过程第一次使用RS曲线，每隔M个点使用一次。M是预先设定的，大于等于1的整数。第一阶段搜索过程中，采用RS曲线保证了路径的可行使性，每隔M个点使用一次RS曲线，保证了路径搜索速度，每成功使用一次混合A*即进行碰撞检测，保证了路径的安全性。即：使用混合A*搜索算法确定从所述起始点S到所述泊车起点P的第一路径。

在上述步骤S202中，使用可变半径的RS曲线搜索(即通过将不同大小转弯半径代替汽车最小转弯半径应用于搜索，得到不同半径大小的多条路径)算法确定所述泊车起点P到所述目标点F的第二路径(其中P点会随着第二路径搜索结果而发生变化)；其中，所述RS曲线为Reeds-Shepp曲线。

具体来说，在第二段搜索过程中，采用可变半径的RS曲线，由扩展式搜索的终点P和目标点位置F计算规划出能满足车辆泊车运动约束的无碰撞路径。在第二阶段使用可变半径的RS曲线(即VR-RS曲线)，VR-RS相比RS曲线具有更高的灵活性，含不同半径的优势，具有较高的避撞能力，在相同的环境下更易搜到从泊车起始点P到目标点F的无碰撞路径，减少了固定半径时搜索出的路径转向冗余和频繁倒车的不良现象。

作为另一种优选示例，上述步骤S203中，所述对所述第一路径和所述第二路径进行平滑处理包括：

对所述第一路径使用贝塞尔曲线进行拟合，得到第三路径；

对所述第二路径使用贝塞尔曲线进行拟合，得到第四路径；

即，首先依据车辆运动方向，将路径于车辆车速正负转换处(即泊车起始点P处)断开，即将路径分为所述第一路径和第二路径；然后对第一路径和第二路径分别使用贝塞尔曲线进行优化；最后对优化后没有折点的第三路径和第四路径使用共轭梯度下降的方法来对路径的曲率和与障碍物的间距加以限制。

优选的，本实施例提供的分段贝塞尔曲线包括：

根据(2)和(3)，给定一个坐标序列x_i＝(x_i，y_i)，i∈[1，N]，根据车辆的运动方向判断是否出现车速正负转换行为(即本实施例中的P点)，若有则将此位置记录下来，作为路径分段的依据，可得

通过检测车辆的前进距离D来发现车辆行进方向的改变，用符号sig来记录。将整条曲线分段得到各个路径片段[P_seg](例如分为第一路径和第二路径)，利用贝塞尔曲线对其分段优化，对优化得到的各段路径[P_{seg_bezier}]拼接得到最终的优化路径P_bezier。其中，N是大于等于1的整数，N个坐标组成待拟合的路径，例如第一路径或者第二路径。

优选的，本实施例提供的共轭梯度下降法的路径优化包括：

对于贝塞尔曲线优化后的路径(例如第三路径或者第四路径)：x_i＝(x_i，y_i)，i∈[1，...，N]和车库边缘的坐标点o_i，使用如下目标函数进行再优化：

其中第一项为车辆与停车位边缘碰撞的惩罚项；第二项为曲率惩罚项，使规划的路径曲率不大于汽车最大转弯曲率。Δx_i＝x_i-x_i-1是在点x_i处的位移量，

是路径在点x_i处的角度改变量，k_max是车辆转弯的最大曲率。ω_o、ω_k分别是这第一项和第二项的权重因子。σ_o和σ_k为惩罚函数。边缘碰撞的惩罚方程为：

σ_o＝-log(|x_i-o_i|+λ) (6)

其中，参数λ是防止因车辆距离停车位边缘过近，即当x接近o时，式(6)式计算结果趋近负无穷而导致梯度***。

优选的，边缘碰撞惩罚项的梯度为：

对于曲率惩罚项，因为曲率k_i＝Δφ_i/(|Δx_i|)与路径中的三个点x_i-1，x_i和x_i+1有关，其中

因此曲率惩罚项需要分别对这三个点求偏导：

将(8)，(9)，(10)三项加权求和即可得到曲率惩罚项的梯度，其中，

即曲率惩罚项的梯度为曲率k_i＝Δφ_i/(|Δx_i|)对xi-1、xi和xi+1的偏导数加权之和，其中xi是当前点，xi-1是当前点的前一点，xi+1是当前点的后一点。

上述步骤S203中，先使用贝塞尔曲线进行拟合，再使用共轭梯度下降方法进行路径优化，过程如图3所示，图3中图a表示采用贝塞尔曲线的路径平滑处理后的路径，在局部位置曲率较大，超出汽车的转弯半径约束；而通过梯度下降方法对曲率进行修改后的图b则曲率明显变小，满足汽车的最小转弯半径能力约束。对于任意给定一段路径，由于存在折点，车辆无法对其跟随(如图a的橘线)，先对其用贝塞尔曲线进行平滑处理得到如图b的结果，平滑后的曲线在尖点处过渡圆滑；再采用共轭梯度下降方法对路径曲率加以限制，进一步得到如图c的汽车可行驶的平滑曲线。

作为另一种优选示例，在步骤S203处理完成后，根据路径评价函数从生成的多条路径中筛选出最优的路径。路径评价函数可以是：

评价函数值最小的路径为最优的路径。

所述正负转换为当前时刻与前一时刻的速度方向相反。

所述路径总长度通过下列公式确定：

L＝∑sqrt((x(i+1)-x(i))²+(y(i+1)-y(i))²) (13)

若第i+1点和第i点之间的距离固定为D(例如D＝0.1m)，则(13)可以简化为：

L＝D*(N-1) (14)

通过本实施例的方法，首先，将路径切分为第一段和第二段；第一段中，使用混合A*搜索算法确定从所述起始点S到所述泊车起点P的第一路径；第二段中，使用可变半径的RS曲线搜索算法确定所述泊车起点P到所述目标点F的第二路径，其中通过使用可变半径的RS曲线可获得多条路径。其次，对所述第一路径使用贝塞尔曲线进行拟合，得到第三路径，对所述第二路径使用贝塞尔曲线进行拟合，得到第四路径。然后，对所述第三路径和所述第四路径，使用共轭梯度下降方法进行路径优化。最后，使用根据路径评价函数从生成的多条路径中选出最优的路径。从而有效地提升路径的灵活性，减小轨迹跟随的难度，能够适应于多种不同类型的泊车环境。

实施例二

基于同一个发明构思，本发明实施例还提供了一种自动泊车的路径规划装置，如图4所示，该装置包括：

第一规划模块401，用于确定从起始点S到泊车起点P的第一路径；

第二规划模块402，用于确定从所述泊车起点P到目标点F的第二路径；

平滑处理模块403，用于对所述第一路径和所述第二路径进行平滑处理。

优选的，本实施例提供的自动泊车的路径规划装置还可以包括路径评价模块，用于根据路径评价函数选出最优的路径，其中评价函数值最小的路径为最优的路径。

需要说明的是，本实施例提供的第一规划模块401，能实现实施例一中步骤S201包含的全部功能，解决相同技术问题，达到相同技术效果，在此不再赘述；

相应的，本实施例提供的第二规划模块402，能实现实施例一中步骤S202包含的全部功能，解决相同技术问题，达到相同技术效果，在此不再赘述。

相应的，本实施例提供的平滑处理模块403，能实现实施例一中步骤S203包含的全部功能，解决相同技术问题，达到相同技术效果，在此不再赘述。

相应的，本实施例提供的路径评价模块，能实现实施例一中路径评价函数包含的全部功能，解决相同技术问题，达到相同技术效果，在此不再赘述

需要说明的是，实施例二提供的装置与实施例一提供的方法属于同一个发明构思，解决相同的技术问题，达到相同的技术效果，实施例二提供的装置能实现实施例一的所有方法，相同之处不再赘述。

实施例三

基于同一个发明构思，本发明实施例还提供了一种自动泊车的路径规划装置，如图5所示，该装置包括：

包括存储器502、处理器501和用户接口503；

所述存储器502，用于存储计算机程序；

所述用户接口503，用于与用户实现交互；

所述处理器501，用于读取所述存储器502中的计算机程序，所述处理器501执行所述计算机程序时，实现：

确定从起始点S到泊车起点P的第一路径；

确定从所述泊车起点P到目标点F的第二路径；

对所述第一路径和所述第二路径进行平滑处理。

其中，在图5中，总线架构可以包括任意数量的互联的总线和桥，具体由处理器501代表的一个或多个处理器和存储器502代表的存储器的各种电路链接在一起。总线架构还可以将诸如***设备、稳压器和功率管理电路等之类的各种其他电路链接在一起，这些都是本领域所公知的，因此，本文不再对其进行进一步描述。总线接口提供接口。处理器501负责管理总线架构和通常的处理，存储器502可以存储处理器501在执行操作时所使用的数据。

处理器501可以是CPU、ASIC、FPGA或CPLD，处理器501也可以采用多核架构。

处理器501执行存储器502存储的计算机程序时，实现实施例一中的任一自动泊车的路径规划方法。

需要说明的是，实施例三提供的装置与实施例一提供的方法属于同一个发明构思，解决相同的技术问题，达到相同的技术效果，实施例三提供的装置能实现实施例一的所有方法，相同之处不再赘述。

本申请还提出一种处理器可读存储介质。其中，该处理器可读存储介质存储有计算机程序，所述处理器执行所述计算机程序时实现实施例一中的任一自动泊车的路径规划方法。

需要说明的是，本申请实施例中对单元的划分是示意性的，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

本领域内的技术人员应明白，本申请的实施例可提供为方法、***、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

Claims

1.一种自动泊车的路径规划方法，其特征在于，包括：

确定从起点S到泊车起始点P的第一路径；

确定从所述泊车起始点P到目标点F的第二路径；

对所述第一路径和所述第二路径进行平滑处理。

2.根据权利要求1所述的方法，其特征在于，所述确定从起点S到泊车起始点P的第一路径包括：

3.根据权利要求2所述的方法，其特征在于，所述混合A*搜索算法包括：

其中，所述RS曲线为Reeds-Shepp曲线。

4.根据权利要求3所述的方法，其特征在于，所述以固定半径的RS曲线作为成本进行搜索包括：

将最小转弯半径作为所述混合A*算法搜索的约束；

每隔M个点使用一次所述RS曲线进行路径搜索；

每成功完成一次所述混合A*搜索进行一次碰撞检测；

其中，M是大于等于1的整数。

5.根据权利要求1所述的方法，其特征在于，所述确定从所述泊车起始点P到目标点F的第二路径包括：

其中，所述RS曲线为Reeds-Shepp曲线。

6.根据权利要求1所述的方法，其特征在于，所述对所述第一路径和所述第二路径进行平滑处理包括：

对所述第一路径使用贝塞尔曲线进行拟合，得到第三路径；

对所述第二路径使用贝塞尔曲线进行拟合，得到第四路径；

7.根据权利要求6所述的方法，其特征在于，所述使用共轭梯度下降方法进行路径优化包括：

根据下列公式对待优化的路径进行优化：

其中，第一项为边缘碰撞的惩罚项，第二项为曲率惩罚项；

o_i为车库边缘的坐标点；

Δx_i＝x_i-x_i-1是在点x_i处的位移量，

8.根据权利要求7所述的方法，其特征在于，所述边缘碰撞惩罚项的梯度为：

9.根据权利要求7所述的方法，其特征在于，所述曲率惩罚的梯度为：

曲率k_i＝Δφ_i/(|Δx_i|)，其与x_i-1、x_i和x_i+1的偏导加权之和，

其中，

10.根据权利要求1所述的方法，其特征在于，还包括：

根据路径评价函数选出最优的路径，

所述路径评价函数为：

评价函数值最小的路径为最优的路径。

11.根据权利要求10所述的方法，其特征在于，包括：

所述正负转换为当前时刻的速度与前一时刻的速度方向相反。

12.根据权利要求10所述的方法，其特征在于，所述路径总长度通过下列公式确定：

L＝∑sqrt((x(i+1)-x(i))²+(y(i+1)-y(i))²)，

其中，(xi，yi)为所述路径的坐标，i为大于等于1小于等于N的整数，N为所述路径的计算点总数。

13.一种自动泊车的路径规划装置，其特征在于，包括：

14.根据权利要求13所述的装置，其特征在于，还包括：

路径评价模块，用于根据路径评价函数选出最优的路径，其中评价函数值最小的路径为最优的路径。

15.一种自动泊车的路径规划装置，其特征在于，包括存储器、处理器和用户接口；

所述存储器，用于存储计算机程序；

所述用户接口，用于与用户实现交互；

所述处理器，用于读取所述存储器中的计算机程序，所述处理器执行所述计算机程序时，实现如权利要求1到12之一所述的自动泊车的路径规划方法。

16.一种处理器可读存储介质，其特征在于，所述处理器可读存储介质存储有计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至12之一所述的自动泊车的路径规划方法。