CN108121205B - 一种用于多种泊车场景的路径规划方法、系统及介质 - Google Patents

Abstract

本发明提供一种用于多种泊车场景的路径规划方法、系统及介质，采用环境感知模块、路径优化模块、单次路径规划模块、输出模块，所述单次路径规划模块包括依次连接路径规划建模模块、离散化模块、优化求解模块的结构和环境感知模块，基于视觉和超声波雷达进行车位和障碍物检测；路径优化模块，基于时间‑空间解耦的初始化策略进行单次路径规划的迭代优化；单次路径规划模块，基于一定的初始猜想，完成单次路径规划；输出模块，通过一定的筛选机制输出最优路径的规划方法，解决现有技术中路径规划方法不能适用于各种泊车场景的路径规划，未考虑离散点间约束违背的情况，缺乏一种更优的初始化方法的技术问题。

Description

一种用于多种泊车场景的路径规划方法、系统及介质

技术领域

本发明涉及路径规划技术领域，具体而言，涉及一种基于改进算法的用于多种泊车场景的路径规划方法、系统及介质。

背景技术

近年来，随着经济的高速发展，机动车数量迅速增加。但停车场数量有限，而且停车的规模越来越大，狭小的停车空间给驾驶员停车造成了严重的困难。泊车辅助系统可以有效解决“泊车难”的问题，而路径规划是自动泊车的重要步骤。

目前，路径规划的方法很多，大多仅针对特定泊车场景设计一段式或分段式泊车路径，同时存在停车转向、固定的泊车初始位置(固定点或区域)、无针对所有泊车位类型通用性的框架等问题。因此，一个适用于各种泊车场景的路径规划框架应运而生。然而，现有的通用性泊车路径规划方法未考虑离散点间约束违背的情况，同时缺乏一种更优的初始化方法，将导致更小泊车位的路径规划失败。

发明内容

本发明提供一种用于多种泊车场景的路径规划方法，采用环境感知模块、路径优化模块、单次路径规划模块、输出模块，环境感知模块，基于视觉和超声波雷达进行车位和障碍物检测；路径优化模块，基于时间-空间解耦的初始化策略进行单次路径规划的迭代优化；单次路径规划模块，基于一定的初始猜想，完成单次路径规划；输出模块，通过一定的筛选机制输出最优路径的规划方法，解决现有技术中路径规划方法不能适用于各种泊车场景的路径规划，未考虑离散点间约束违背的情况，缺乏一种更优的初始化方法的技术问题。

本发明为解决上述技术问题而提供一种用于多种泊车场景的路径规划方法包括环境感知模块、路径优化模块、单次路径规划模块、输出模块，所述环境感知模块包括车位检测模块和障碍物检测模块，所述车位检测模块判断所述车位的停车情况，所述障碍物检测模块判断检测周围障碍物情况，所述单次路径规划模块包括依次连接路径规划建模模块、离散化模块、优化求解模块。

一种用于多种泊车场景的路径规划方法，该方法包括以下步骤：

A.基于视觉和超声波雷达通过车位检测模块和障碍物检测模块判断所述车位的停车情况和周围障碍物情况，确定泊车场景；

B.基于车辆自身位置与待泊车车位和障碍物的相对距离建立二维平面坐标系,基于所建坐标系，将相关参数输入到单次路径规划模块中；

C.根据所述相关参数进行路径规划问题建模，得到路径规划模型；

D.将所建立的路径规划模型离散化为非线性规划问题NLP；

E.随机分形搜索SFS进行控制变量“v,w”的优化求解，所述的优化求解器随机分形搜索SFS以所有控制变量“v,w”的离散点“K*FE+1”以及t_f作为决策变量，状态变量

可通过数值积分获得，然后计算每个离散状态的约束违背代价，结合目标函数，构成最终的优化代价，通过优化求解，最终，我们可以得到控制变量“v,w”的离散化序列，可计算出相对应的车辆后轴中心的坐标和相车身航向角，将数据输送给路径优化模块；

F.路径优化模块将最优路径结果传输给输出模块，输出结果。

所述步骤C中所述路径规划建模还包括以下分步骤：

C1.基于前轮转向四轮车辆的阿克曼模型，建立车辆运动学微分方程：

其中，(x,y)代表车辆后轴中心的坐标，L代表前后轴中心轴距，v代表后轴中心的速度，

代表等效前轮转角，θ代表车辆的水平航向角，ω代表方向盘转速，本发明称为“v,w”为控制变量，称

为状态变量；

C2.车辆自身物理约束：

其中，v_max、

和ω_max分别代表泊车过程中最大速度，车辆自身最大阿克曼转角以及方向盘最大转速；

C3.避障约束：

其中，ABCD为矩形的四个端点，P为矩形内的一个点，ABCD代表车辆或者障碍物的四个坐标，P则代表对应的障碍物或者车辆的其中一个坐标，在平面中，当一个点P位于四边形外部，则表示未发生碰撞；

C4.活动范围约束：

其中，[x_lb，x_ub]代表车辆水平方向的活动范围，[y_lb，y_ub]代表车辆垂直方向的活动范围，(Cx_i,Cy_i),i＝1,2,3,4代表车体四个角的坐标，D_s代表车体与活动范围边界保持的安全距离；

C5.车辆初始状态的确定

其中，(x₀,y₀)代表车辆后轴中心的初始位置，v₀，θ₀，

分别代表车辆后轴中心的初始速度、车辆初始水平航向角以及车辆初始阿克曼转角，所述车辆初始水平航向角平行于车位；

C6.终止条件约束

基于环境感知建立的坐标系，其中，矩形ABCD代表车辆，矩形EOIH代表检测到的待泊车车位，SL代表车位长，SW代表车位宽，CL代表纵向距离范围，

对应的终止条件约束为：

其中，t_f代表终止时间，(P_x,P_y)代表坐标P的横纵坐标；

C7.确定待优化的目标函数

提供两种性能指标——时间最短和路径最短。

所述步骤D中所述离散化还包括以下分步骤：

D1.时间段t∈[0,t_f]被等分为FE段：{[t_i-1,t_i]|i＝1,2,...,FE}，其中，t_FE＝t_f,t₀＝0，并且每段时间长度为

D2.每段用(K+1)个插值点构成的Lagrange多项式进行离散插值，所有段上的分段多项式构成对真实连续控制或状态变量的离散化估计，通过(K+1)个插值点的Lagrange多项式{z_i0,z_i1,...,z_ik}来描述第ith个时间段[t_i-1,t_i]的控制变量v(t)：

其中，τ∈[0,1],τ₀＝0并且0＜τ_i≤1(j＝1,2,...,K).τ_i指高斯点，可以在指定K的条件下离线算出，因此，总共有FE*(K+1)个插值点用来描述控制变量v(t)，t∈[0,t_f]；

D3.考虑到控制变量应该物理连续，因而满足如下条件：

简写为z_iK＝z_(i+1)0,i＝1,2,...,FE-1，因此，多达N_sp＝(K*FE+1)个独立的插值点用来描述连续时间控制变量v(t)；

D4.在Lagrange多项式的基础上，在FE段的每一段划分为MMG个更小的时间段，考虑到物理连续性，因而共有N＝(FE-1)*(MMG-1)+MMG个离散点。

所述步骤E中所述优化求解还包括以下分步骤：

E1.单次路径规划中基于物理约束调整控制变量和状态变量的离散值；

E2.单次路径规划中约束违背代价的计算；

E3.目标函数的数值计算；

E4.基于时间-空间解耦的初始化策略进行单次路径规划的迭代优化。

所述基于物理约束进行控制变量和状态变量离散值调整为对于控制变量或者状态变量违背固有的物理约束时，进行数值调整，对于v(t)，

ω(t)满足如下物理约束：

以v(t)为例，其离散化为{v¹,v²,…,v^N}，其中N＝(FE-1)*(MMG-1)+MMG，检查是否违背其对应的物理约束，如果vⁱ>v_max，则vⁱ＝v_max；如果vⁱ<-v_max，则vⁱ＝-v_max，以此类推，进行的

ω(t)数值调整。

所述约束违背代价的计算包括碰撞避免约束、终止条件约束，通过计算准确的约束违背代价进行单次路径规划的优化求解，检查每个离散状态是否违背约束，并累加约束违背代价Ψ_collision，代的大小指引求解器进行优化，下面考虑在某个离散状态下的代价计算：

1.碰撞代价计算

点P为在矩形障碍物ABCD的点，考虑点P与矩形障碍物ABCD发生碰撞，则有

因此碰撞代价为

2.终止代价计算

类似碰撞代价计算，可以得到终止代价Ψ_terminal；

在得到了Ψ_collision和Ψ_terminal以后，我们可以定义总的代价为：

所述目标函数包括距离和时间，所述目标函数公式：

其中，t_f代表终止时间，N代表离散状态的数量，(xⁱ，yⁱ)代表后轴中心第i个离散状态的坐标。

所述的基于时间-空间解耦的初始化策略还包括以下分步骤：1.首先，定义一个“关键区域”,从“某个时刻”到t＝t_f，车辆应该在该“关键区域”进行前后局部微调，该“某个时刻”参数化为一个整数N_x∈[1,N_sp]，使得“关键区域”表示为：

其中，(P_x(t),P_y(t)),P＝A,B,C,D代表某一时刻车辆拐角的坐标；

车辆在“关键区域”内，即t∈[h_i·N_x,t_f]时间段内，不会与障碍物发生碰撞；

先在Nx＝1和没有初始化猜想的情况下求解NLP；若优化代价函数变小,则此时的求解结果作为Nx＝2时优化求解NLP的初始猜想，以此类推，直到N_x＝N_sp，最终选取优化代价最小的解作为最优路径。

一种用于多种泊车场景的路径规划系统，所述用于多种泊车场景的路径规划系统包括存储器、处理器以及存储在所述存储器上的计算机程序，所述计算机程序配置为由所述处理器调用时实现所述的方法的步骤。

一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现所述的方法的步骤。

本发明的有益效果：与现有技术相比，本发明的有益效果是解决离散点间约束违背的情况，同时引入迭代初始化策略，使得优化求解更易收敛，同时能够适用于待泊车位更小的情况，为进一步解决“泊车难”问题提供了借鉴意义。

附图说明

图1是本发明所述路径规划方法的结构示意图。

图2为本发明所述单次路径规划方法流程图。

图3为本发明所述基于环境感知建立的坐标系示意图。

图4为本发明所述基于分段拉格朗日插值进行离散化示意图。

图5为本发明所述碰撞代价计算示意图。

具体实施方式

结合上述附图说明本实用新型的具体实施例。

由图1可知，本发明提供用于多种泊车场景的路径规划方法，包括环境感知模块、路径优化模块、单次路径规划模块、输出模块，所述环境感知模块包括车位检测模块和障碍物检测模块，所述车位检测模块判断所述车位的停车情况，所述障碍物检测模块判断检测周围障碍物情况，所述单次路径规划模块包括依次连接路径规划建模模块、离散化模块、优化求解模块。

由图1和图2可知，本发明还提供一种多种泊车场景的路径规划方法，该方法包括以下步骤：

A.基于视觉和超声波雷达通过车位检测模块和障碍物检测模块判断所述车位的停车情况和周围障碍物情况，确定泊车场景；

B.基于车辆自身位置与待泊车车位和障碍物的相对距离建立二维平面坐标系,基于所建坐标系，将相关参数输入到单次路径规划模块中；

C.根据所述相关参数进行路径规划问题建模，得到路径规划模型；

D.将所建立的路径规划模型离散化为非线性规划问题NLP；

E.随机分形搜索SFS进行控制变量“v,w”的优化求解，所述的优化求解器随机分形搜索SFS以所有控制变量“v,w”的离散点“K*FE+1”以及t_f作为决策变量，状态变量

可通过数值积分获得，然后计算每个离散状态的约束违背代价，结合目标函数，构成最终的优化代价，通过优化求解，最终，我们可以得到控制变量“v,w”的离散化序列，可计算出相对应的车辆后轴中心的坐标和相车身航向角，将数据输送给路径优化模块；

F.路径优化模块将最优路径结果传输给输出模块，输出结果。

所述步骤C中所述路径规划建模还包括以下分步骤：

C1.基于前轮转向四轮车辆的阿克曼模型，建立车辆运动学微分方程：

其中，(x,y)代表车辆后轴中心的坐标，L代表前后轴中心轴距，v代表后轴中心的速度，

代表等效前轮转角，θ代表车辆的水平航向角，ω代表方向盘转速，本发明称为“v,w”为控制变量，称

为状态变量；

C2.车辆自身物理约束：

其中，v_max、

和ω_max分别代表泊车过程中最大速度，车辆自身最大阿克曼转角以及方向盘最大转速；

C3.避障约束：

其中，ABCD为矩形的四个端点，P为矩形内的一个点，ABCD代表车辆或者障碍物的四个坐标，P则代表对应的障碍物或者车辆的其中一个坐标，在平面中，当一个点P位于四边形外部，则表示未发生碰撞；

C4.活动范围约束：

其中，[x_lb，x_ub]代表车辆水平方向的活动范围，[y_lb，y_ub]代表车辆垂直方向的活动范围，(Cx_i,Cy_i),i＝1,2,3,4代表车体四个角的坐标，D_s代表车体与活动范围边界保持的安全距离；

C5.车辆初始状态的确定

其中，(x₀,y₀)代表车辆后轴中心的初始位置，v₀，θ₀，

分别代表车辆后轴中心的初始速度、车辆初始水平航向角以及车辆初始阿克曼转角，所述车辆初始水平航向角平行于车位；

C6.终止条件约束

基于环境感知建立的坐标系，其中，矩形ABCD代表车辆，矩形EOIH代表检测到的待泊车车位，SL代表车位长，SW代表车位宽，CL代表纵向距离范围，

对应的终止条件约束为：

其中，t_f代表终止时间，(P_x,P_y)代表坐标P的横纵坐标；

C7.确定待优化的目标函数

提供两种性能指标——时间最短和路径最短。

所述步骤D中所述离散化还包括以下分步骤：

D1.时间段t∈[0,t_f]被等分为FE段：{[t_i-1,t_i]|i＝1,2,...,FE}，其中，t_FE＝t_f,t₀＝0，并且每段时间长度为

D2.每段用(K+1)个插值点构成的Lagrange多项式进行离散插值，所有段上的分段多项式构成对真实连续控制或状态变量的离散化估计，通过(K+1)个插值点的Lagrange多项式{z_i0,z_i1,...,z_ik}来描述第ith个时间段[t_i-1,t_i]的控制变量v(t)：

其中，τ∈[0,1],τ₀＝0并且0＜τ_i≤1(j＝1,2,...,K).τ_i指高斯点，可以在指定K的条件下离线算出，因此，总共有FE*(K+1)个插值点用来描述控制变量v(t)，t∈[0,t_f]；

D3.考虑到控制变量应该物理连续，因而满足如下条件：

简写为z_iK＝z_(i+1)0,i＝1,2,...,FE-1，因此，多达N_sp＝(K*FE+1)个独立的插值点用来描述连续时间控制变量v(t)；

D4.在Lagrange多项式的基础上，在FE段的每一段划分为MMG个更小的时间段，考虑到物理连续性，因而共有N＝(FE-1)*(MMG-1)+MMG个离散点。

所述步骤E中所述优化求解还包括以下分步骤：

E1.单次路径规划中基于物理约束调整控制变量和状态变量的离散值；

E2.单次路径规划中约束违背代价的计算；

E3.目标函数的数值计算；

E4.基于时间-空间解耦的初始化策略进行单次路径规划的迭代优化。

所述基于物理约束进行控制变量和状态变量离散值调整为对于控制变量或者状态变量违背固有的物理约束时，进行数值调整，对于v(t)，

ω(t)满足如下物理约束：

以v(t)为例，其离散化为{v¹,v²,…,v^N}，其中N＝(FE-1)*(MMG-1)+MMG，检查是否违背其对应的物理约束，如果vⁱ>v_max，则vⁱ＝v_max；如果vⁱ<-v_max，则vⁱ＝-v_max，以此类推，进行的

ω(t)数值调整。

所述约束违背代价的计算包括碰撞避免约束、终止条件约束，通过计算准确的约束违背代价进行单次路径规划的优化求解，检查每个离散状态是否违背约束，并累加约束违背代价Ψ_collision，代的大小指引求解器进行优化，下面考虑在某个离散状态下的代价计算：

1.碰撞代价计算

点P为在矩形障碍物ABCD的点，考虑点P与矩形障碍物ABCD发生碰撞，则有

因此碰撞代价为

2.终止代价计算

类似碰撞代价计算，可以得到终止代价Ψ_terminal；

在得到了Ψ_collision和Ψ_terminal以后，我们可以定义总的代价为：

所述目标函数包括距离和时间，所述目标函数公式：

其中，t_f代表终止时间，N代表离散状态的数量，(xⁱ，yⁱ)代表后轴中心第i个离散状态的坐标。

所述的基于时间-空间解耦的初始化策略还包括以下分步骤：1.首先，定义一个“关键区域”,从“某个时刻”到t＝t_f，车辆应该在该“关键区域”进行前后局部微调，该“某个时刻”参数化为一个整数Nx∈[1,Nsp]，使得“关键区域”表示为：

其中，(P_x(t),P_y(t)),P＝A,B,C,D代表某一时刻车辆拐角的坐标；

车辆在“关键区域”内，即t∈[h_i·N_x,t_f]时间段内，不会与障碍物发生碰撞；

先在N_x＝1和没有初始化猜想的情况下求解NLP；若优化代价函数变小,则此时的求解结果作为N_x＝2时优化求解NLP的初始猜想，以此类推，直到N_x＝N_sp，最终选取优化代价最小的解作为最优路径。

一种用于多种泊车场景的路径规划系统，所述用于多种泊车场景的路径规划系统包括存储器、处理器以及存储在所述存储器上的计算机程序，所述计算机程序配置为由所述处理器调用时实现所述的方法的步骤。

一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现所述的方法的步骤。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明具体实施只局限于这些说明，对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于发明的保护范围。

Claims (7)

1.一种用于多种泊车场景的路径规划方法，其特征在于：该方法包括以下步骤：

A.基于视觉和超声波雷达通过车位检测模块和障碍物检测模块判断所述车位的停车情况和周围障碍物情况，确定泊车场景；

B.基于车辆自身位置与待泊车车位和障碍物的相对距离建立二维平面坐标系，基于所建坐标系，将相关参数输入到单次路径规划模块中；

C.根据所述相关参数进行路径规划问题建模，得到路径规划模型；

D.将所建立的路径规划模型离散化为非线性规划问题NLP；

E.随机分形搜索SFS进行控制变量“v，w”的优化求解，所述的优化求解器随机分形搜索SFS以所有控制变量“v，w”的离散点“K*FE+1”以及t_f作为决策变量，状态变量

可通过数值积分获得，然后计算每个离散状态的约束违背代价，结合目标函数，构成最终的优化代价，通过优化求解，最终，得到控制变量“v，w”的离散化序列，可计算出相对应的车辆后轴中心的坐标和相车身航向角，将数据输送给路径优化模块；

F.路径优化模块将最优路径结果传输给输出模块，输出结果；

其中，所述步骤C中所述路径规划建模还包括以下分步骤：

C1.基于前轮转向四轮车辆的阿克曼模型，建立车辆运动学微分方程：

其中，(x，y)代表车辆后轴中心的坐标，L代表前后轴中心轴距，v代表后轴中心的速度，

代表等效前轮转角，θ代表车辆的水平航向角，ω代表方向盘转速，称“v，w”为控制变量，称

为状态变量；

C2.车辆自身物理约束：

其中，v_max、

和ω_max分别代表泊车过程中最大速度，车辆自身最大阿克曼转角以及方向盘最大转速；

C3.避障约束：

其中，ABCD代表车辆或者障碍物的四个坐标，P则代表对应的障碍物或者车辆的其中一个坐标，在平面中，当一个点P位于四边形外部，则表示未发生碰撞；

C4.活动范围约束：

其中，[x_lb，x_ub]代表车辆水平方向的活动范围，[y_lb，y_ub]代表车辆垂直方向的活动范围，(Cx_i，Cy_i)，i＝1，2，3，4代表车体四个角的坐标，D_s代表车体与活动范围边界保持的安全距离；

C5.车辆初始状态的确定

其中，(x₀，y₀)代表车辆后轴中心的初始位置，v₀，θ₀，

分别代表车辆后轴中心的初始速度、车辆初始水平航向角以及车辆初始阿克曼转角，所述车辆初始水平航向角平行于车位；

C6.终止条件约束

基于环境感知建立的坐标系，其中，矩形EOIH代表检测到的待泊车车位，SL代表车位长，SW代表车位宽，CL代表纵向距离范围，

对应的终止条件约束为：

其中，t_f代表终止时间，(P_x，P_y)代表坐标P的横纵坐标；

C7.确定待优化的目标函数

提供两种性能指标——时间最短和路径最短；

所述步骤D中所述离散化还包括以下分步骤：

D1.时间段t∈[0，t_f]被等分为FE段：{[t_i-1，t_i]|i＝1，2，...，FE}，其中，t_FE＝t_f，

t₀＝0，并且每段时间长度为

D2.每段用(K+1)个插值点构成的Lagrange多项式进行离散插值，所有段上的分段多项式构成对真实连续控制或状态变量的离散化估计，通过(K+1)个插值点的Lagrange多项式{z_i0，z_i1，...，z_ik}来描述第ith个时间段[t_i-1，t_i]的控制变量v(t)：

其中，τ∈[0，1]，τ₀＝0并且0＜τ_i≤1，j＝1，2，...，K，τ_i指高斯点，可以在指定K的条件下离线算出，因此，总共有FE*(K+1)个插值点用来描述控制变量v(t)，t∈[0，t_f]；

D3.考虑到控制变量应该物理连续，因而满足如下条件：

简写为z_iK＝z_(i+1)0，i＝1，2，...，FE-1，因此，多达N_sp＝(K*FE+1)个独立的插值点用来描述连续时间控制变量v(t)；

D4.在Lagrange多项式的基础上，在FE段的每一段划分为MMG个更小的时间段，考虑到物理连续性，因而共有N＝(FE-1)*(MMG-1)+MMG个离散点；

所述步骤E中所述优化求解还包括以下分步骤：

E1.单次路径规划中基于物理约束调整控制变量和状态变量的离散值；

E2.单次路径规划中约束违背代价的计算；

E3.目标函数的数值计算；

E4.基于时间-空间解耦的初始化策略进行单次路径规划的迭代优化。

2.根据权利要求1所述的用于多种泊车场景的路径规划方法，其特征在于：所述基于物理约束进行控制变量和状态变量离散值调整为对于控制变量或者状态变量违背固有的物理约束时，进行数值调整，对于v(t)，

ω(t)满足如下物理约束：

以v(t)为例，其离散化为{v¹，v²，…，v^N}，其中N＝(FE-1)*(MMG-1)+MMG，检查是否违背其对应的物理约束，如果vⁱ>v_max，则vⁱ＝v_max；如果vⁱ<-v_max，则vⁱ＝-v_max，以此类推，进行的

ω(t)数值调整。

3.根据权利要求1所述的用于多种泊车场景的路径规划方法，其特征在于：所述约束违背代价的计算包括碰撞避免约束、终止条件约束，通过计算准确的约束违背代价进行单次路径规划的优化求解，检查每个离散状态是否违背约束，并累加约束违背代价Ψ_collision，代的大小指引求解器进行优化，下面考虑在某个离散状态下的代价计算：

1.碰撞代价计算

点P为代表车辆或者障碍物的四个坐标ABCD内的点，考虑点P与矩形障碍物ABCD发生碰撞，则有

因此碰撞代价为

2.终止代价计算

类似碰撞代价计算，可以得到终止代价Ψ_terminal；

在得到了Ψ_collision和Ψ_terminal以后，我们可以定义总的代价为：

4.根据权利要求1所述的用于多种泊车场景的路径规划方法，其特征在于：所述目标函数包括距离和时间，所述目标函数公式：

其中，t_f代表终止时间，N代表离散状态的数量，(xⁱ，yⁱ)代表后轴中心第i个离散状态的坐标。

5.根据权利要求1所述的用于多种泊车场景的路径规划方法，其特征在于：所述的基于时间-空间解耦的初始化策略还包括以下分步骤：1.首先，定义一个“关键区域”，从“某个时刻”到t＝t_f，车辆应该在该“关键区域”进行前后局部微调，该“某个时刻”参数化为一个整数N_x∈[1，N_sp]，使得“关键区域”表示为：

其中，(P_x(t)，P_y(t))，P为代表车辆或者障碍物的四个坐标ABCD内某一时刻车辆拐角的坐标；

车辆在“关键区域”内，即t∈[h_i·N_x，t_f]时间段内，不会与障碍物发生碰撞；

先在N_x＝1和没有初始化猜想的情况下求解NLP；若优化代价函数变小，则此时的求解结果作为N_x＝2时优化求解NLP的初始猜想，以此类推，直到N_x＝N_sp，最终选取优化代价最小的解作为最优路径。

6.一种用于多种泊车场景的路径规划系统，其特征在于:所述用于多种泊车场景的路径规划系统包括存储器、处理器以及存储在所述存储器上的计算机程序，所述计算机程序配置为由所述处理器调用时实现权利要求1-5中任一项所述的方法的步骤。

7.一种计算机可读存储介质，其特征在于:所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现权利要求1-5中任一项所述的方法的步骤。

Images