[发明专利]一种用于多种泊车场景的路径规划方法、系统及介质

权利要求书

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.基于时间-空间解耦的初始化策略进行单次路径规划的迭代优化。