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

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

Path planning method, system and medium for multiple parking scenarios

The invention provides a path planning method, system and medium for multiple parking scenes. It adopts the environment perception module, path optimization module, single path planning module and output module. The single path planning module includes the structure of the path planning module, the dispersion module and the optimization solution module. And the environment perception module, based on the vision and ultrasonic radar for vehicle location and obstacle detection; path optimization module, the iterative optimization of single path planning based on the initialization strategy of time decoupling space; the single path planning module, based on certain initial conjectures, completed a single path planning; output module, through After a certain selection mechanism, the optimal path planning method is used to solve the path planning method in the existing technology. The path planning method can not be applied to the path planning of various parking scenes, without considering the constraint violation between discrete points, and the technical problem of a better initialization method is lack.

【技术实现步骤摘要】
一种用于多种泊车场景的路径规划方法、系统及介质
本专利技术涉及路径规划
，具体而言，涉及一种基于改进算法的用于多种泊车场景的路径规划方法、系统及介质。
技术介绍
近年来，随着经济的高速发展，机动车数量迅速增加。但停车场数量有限，而且停车的规模越来越大，狭小的停车空间给驾驶员停车造成了严重的困难。泊车辅助系统可以有效解决“泊车难”的问题，而路径规划是自动泊车的重要步骤。目前，路径规划的方法很多，大多仅针对特定泊车场景设计一段式或分段式泊车路径，同时存在停车转向、固定的泊车初始位置(固定点或区域)、无针对所有泊车位类型通用性的框架等问题。因此，一个适用于各种泊车场景的路径规划框架应运而生。然而，现有的通用性泊车路径规划方法未考虑离散点间约束违背的情况，同时缺乏一种更优的初始化方法，将导致更小泊车位的路径规划失败。
技术实现思路
本专利技术提供一种用于多种泊车场景的路径规划方法，采用环境感知模块、路径优化模块、单次路径规划模块、输出模块，环境感知模块，基于视觉和超声波雷达进行车位和障碍物检测；路径优化模块，基于时间-空间解耦的初始化策略进行单次路径规划的迭代优化；单次路径规划模块，基于一定的初始猜想，完成单次路径规划；输出模块，通过一定的筛选机制输出最优路径的规划方法，解决现有技术中路径规划方法不能适用于各种泊车场景的路径规划，未考虑离散点间约束违背的情况，缺乏一种更优的初始化方法的技术问题。本专利技术为解决上述技术问题而提供一种用于多种泊车场景的路径规划方法包括环境感知模块、路径优化模块、单次路径规划模块、输出模块，所述环境感知模块包括车位检测模块和障碍物检测模块，所述车位检测模块判断所述车位的停车情况，所述障碍物检测模块判断检测周围障碍物情况，所述单次路径规划模块包括依次连接路径规划建模模块、离散化模块、优化求解模块。一种用于多种泊车场景的路径规划方法，该方法包括以下步骤：A.基于视觉和超声波雷达通过所述车位检测模块和障碍物检测模块判断所述车位的停车情况和周围障碍物情况，确定泊车场景；B.基于车辆自身位置与待泊车车位和障碍物的相对距离建立二维平面坐标系,基于所建坐标系，将相关参数输入到单次路径规划模块中；C.根据所述相关参数进行路径规划问题建模，得到路径规划模型；D.将所建立的路径规划模型离散化为非线性规划问题NLP；E.随机分形搜索SFS进行控制变量“v,w”的优化求解，所述的优化求解器随机分形搜索SFS以所有控制变量“v,w”的离散点“K*FE+1”以及tf作为决策变量，状态变量可通过数值积分获得，然后计算每个离散状态的约束违背代价，结合目标函数，构成最终的优化代价，通过优化求解，最终，我们可以得到控制变量“v,w”的离散化序列，可计算出相对应的车辆后轴中心的坐标和相车身航向角，将数据输送给路径优化模块；F.路径优化模块将最优路径结果传输给输出模块，输出结果。所述步骤C中所述路径规划建模还包括以下分步骤：C1.基于前轮转向四轮车辆的阿克曼模型，建立车辆运动学微分方程：其中，(x,y)代表车辆后轴中心的坐标，L代表前后轴中心间距(轴距)，v代表后轴中心的速度，代表等效前轮转角，θ代表车辆的水平航向角，ω代表方向盘转速，本专利技术称为“v,w”为控制变量，称为状态变量；C2.车辆自身物理约束：其中，vmax、和wmax分别代表泊车过程中最大速度，车辆自身最大阿克曼转角以及方向盘最大转速；C3.避障约束：其中，ABCD为矩形的四个端点，P为矩形内的一个点，ABCD代表车辆或者障碍物的四个坐标，P则代表对应的障碍物或者车辆的其中一个坐标，在平面中，当一个点P位于四边形外部，则表示未发生碰撞；C4.活动范围约束：其中，[xlb,xub]代表车辆水平方向的活动范围，[ylb,yub]代表车辆垂直方向的活动范围，(Cxi,Cyi),i＝1,2,3,4代表车体四个角的坐标，Ds代表车体与活动范围边界保持的安全距离；C5.车辆初始状态的确定其中，(x0,y0)代表车辆后轴中心的初始位置，v0,θ0,分别代表车辆后轴中心的初始速度、车辆初始水平航向角以及车辆初始阿克曼转角，所述车辆初始水平航向角平行于车位；C6.终止条件约束基于环境感知建立的坐标系，其中，矩形ABCD代表车辆，矩形EOIH代表检测到的待泊车车位，SL代表车位长，SW代表车位宽，CL代表纵向距离范围，对应的终止条件约束为：其中，tf代表终止时间，(Px,Py)代表坐标P的横纵坐标；C7.确定待优化的目标函数提供两种性能指标——时间最短和路径最短。所述步骤D中所述离散化还包括以下分步骤：D1.时间段t∈[0,tf]被等分为FE段：{[ti-1,ti]|i＝1,2,...,FE}，其中，tFE＝tf,tO＝O，并且每段时间长度为D2.每段用(K+1)个插值点构成的Lagrange多项式进行离散插值，所有段上的分段多项式构成对真实连续控制或状态变量的离散化估计，通过(K+1)个插值点的Lagrange多项式{zi0,zi1,...,zik}来描述第ith个时间段[ti-1,ti]的控制变量v(t)：其中，τ∈[0,1],τ0＝0并且0＜τi≤1(j＝1,2,...,K).τi指高斯点，可以在指定K的条件下离线算出，因此，总共有FE*(K+1)个插值点(离散点)用来描述控制变量v(t),t∈[0,tf]；D3.考虑到控制变量应该物理连续，因而满足如下条件：简写为ziK＝z(i+1)0,i＝1,2,...,FE-1，因此，多达Nsp＝(K*FE+1)个独立的插值点(离散点)用来描述连续时间控制变量v(t)；D4.在Lagrange多项式的基础上，在FE段的每一段划分为MMG个更小的时间段，考虑到物理连续性，因而共有N＝(FE-1)*(MMG-1)+MMG个离散点。所述步骤E中所述优化求解还包括以下分步骤：E1.单次路径规划中基于物理约束调整控制变量和状态变量的离散值；E2.单次路径规划中约束违背代价的计算；E3.目标函数的数值计算；E4.基于时间-空间解耦的初始化策略进行单次路径规划的迭代优化。所述基于物理约束进行控制变量和状态变量离散值调整为对于控制变量或者状态变量违背固有的物理(最大值)约束时，进行数值调整，对于v(t),w(t)满足如下物理约束：以v(t)为例，其离散化为{v1,v2,…,vN}，其中N＝(FE-1)*(MMG-1)+MMG，检查是否违背其对应的物理约束，如果vi>vmax，则vi＝vmax；如果vi<-vmax，则vi＝-vmax，以此类推，进行w(t)的数值调整。所述约束违背代价的计算包括碰撞避免约束、终止条件约束，通过计算准确的约束违背代价进行单次路径规划的优化求解，检查每个离散状态是否违背约束，并累加约束违背代价Ψcollision，代的大小指引求解器进行优化，下面考虑在某个离散状态下的代价计算：1.碰撞代价计算点P为在矩形障碍物ABCD的点，考虑点P与矩形障碍物ABCD发生碰撞，则有因此碰撞代价为2.终止代价计算类似碰撞代价计算，可以得到终止代价Ψterminal；在得到了Ψcollision和Ψterminal以后，我们可以定义总的代价为：所述目标函数包括距离和时间，所述目标函数公式：其中，tf代表终止本文档来自技高网...

【技术保护点】
一种用于多种泊车场景的路径规划方法，其特征在于：该方法包括以下步骤：A.基于视觉和超声波雷达通过所述车位检测模块和障碍物检测模块判断所述车位的停车情况和周围障碍物情况，确定泊车场景；B.基于车辆自身位置与待泊车车位和障碍物的相对距离建立二维平面坐标系,基于所建坐标系，将相关参数输入到单次路径规划模块中；C.根据所述相关参数进行路径规划问题建模，得到路径规划模型；D.将所建立的路径规划模型离散化为非线性规划问题NLP；E.随机分形搜索SFS进行控制变量“v,w”的优化求解，所述的优化求解器随机分形搜索SFS以所有控制变量“v,w”的离散点“K*FE+1”以及tf作为决策变量，状态变量

【技术特征摘要】
1.一种用于多种泊车场景的路径规划方法，其特征在于：该方法包括以下步骤：A.基于视觉和超声波雷达通过所述车位检测模块和障碍物检测模块判断所述车位的停车情况和周围障碍物情况，确定泊车场景；B.基于车辆自身位置与待泊车车位和障碍物的相对距离建立二维平面坐标系,基于所建坐标系，将相关参数输入到单次路径规划模块中；C.根据所述相关参数进行路径规划问题建模，得到路径规划模型；D.将所建立的路径规划模型离散化为非线性规划问题NLP；E.随机分形搜索SFS进行控制变量“v,w”的优化求解，所述的优化求解器随机分形搜索SFS以所有控制变量“v,w”的离散点“K*FE+1”以及tf作为决策变量，状态变量可通过数值积分获得，然后计算每个离散状态的约束违背代价，结合目标函数，构成最终的优化代价，通过优化求解，最终，我们可以得到控制变量“v,w”的离散化序列，可计算出相对应的车辆后轴中心的坐标和相车身航向角，将数据输送给路径优化模块；F.路径优化模块将最优路径结果传输给输出模块，输出结果。2.根据权利要求1所述用于多种泊车场景的路径规划方法，其特征在于：所述步骤C中所述路径规划建模还包括以下分步骤：C1.基于前轮转向四轮车辆的阿克曼模型，建立车辆运动学微分方程：其中，(x,y)代表车辆后轴中心的坐标，L代表前后轴中心轴距，v代表后轴中心的速度，代表等效前轮转角，θ代表车辆的水平航向角，ω代表方向盘转速，本发明称为“v,w”为控制变量，称为状态变量；C2.车辆自身物理约束：其中，vmax、和wmax分别代表泊车过程中最大速度，车辆自身最大阿克曼转角以及方向盘最大转速；C3.避障约束：其中，ABCD为矩形的四个端点，P为矩形内的一个点，ABCD代表车辆或者障碍物的四个坐标，P则代表对应的障碍物或者车辆的其中一个坐标，在平面中，当一个点P位于四边形外部，则表示未发生碰撞；C4.活动范围约束：其中，[xlb,xub]代表车辆水平方向的活动范围，[ylb,yub]代表车辆垂直方向的活动范围，(Cxi,Cyi),i＝1,2,3,4代表车体四个角的坐标，Ds代表车体与活动范围边界保持的安全距离；C5.车辆初始状态的确定其中，(x0,y0)代表车辆后轴中心的初始位置，v0,θ0,分别代表车辆后轴中心的初始速度、车辆初始水平航向角以及车辆初始阿克曼转角，所述车辆初始水平航向角平行于车位；C6.终止条件约束基于环境感知建立的坐标系，其中，矩形ABCD代表车辆，矩形EOIH代表检测到的待泊车车位，SL代表车位长，SW代表车位宽，CL代表纵向距离范围，对应的终止条件约束为：其中，tf代表终止时间，(Px,Py)代表坐标P的横纵坐标；C7.确定待优化的目标函数提供两种性能指标——时间最短和路径最短。3.根据权利要求1所述的用于多种泊车场景的路径规划方法，其特征在于：所述步骤D中所述离散化还包括以下分步骤：D1.时间段t∈[0,tf]被等分为FE段：{[ti-1,ti]|i＝1,2,...,FE}，其中，tFE＝tf,t0＝0，并且每段时间长度为D2.每段用(K+1)个插值点构成的Lagrange多项式进行离散插值，所有段上的分段多项式构成对真实连续控制或状态变量的离散化估计，通过(K+1)个插值点的Lagrange多项式{zi0,zi1,...,zik}来描述第ith个时间段[ti-1,ti]的控制变量v(t)：其中，τ∈[0,1],τ0＝0并且0＜τi≤1(j＝1,2,...,K).τi指高斯点，可以在指定K的条件下离线算出，因此，总共有FE*(K+1)个插值点(离散点)用来描述控制变量v(t),t∈[...

【专利技术属性】
技术研发人员：孙兆忙，孙立华，高忠，
申请(专利权)人：深圳市航盛电子股份有限公司，
类型：发明
国别省市：广东,44