CN114013428A - 一种基于分子间作用力的泊车路径动态规划方法 - Google Patents

Abstract

本发明属于泊车路径规划领域，尤其涉及一种基于分子间作用力的泊车路径动态规划方法，包括获取车位信息并识别车位类型；根据车位类型设置假想吸引点位置；车辆根据假想吸引点对车身的引力所产生的虚拟转矩计算目标方向盘转角；假想障碍物和车位顶点对车身产生斥力作用，假想引力和斥力共同产生总虚拟转矩并以此计算目标方向盘转角进行躲避；车位线作为约束条件，完成泊车等步骤。本发明降低了对泊车起始位姿的要求，实时测量与车位及附近障碍物的距离，通过计算车身受到引力斥力所产生的转矩，从而计算目标方向盘转角，控制方法简单，对跟踪精度要求不高，降低了误差修正的难度，提高了泊车效率。

Description

一种基于分子间作用力的泊车路径动态规划方法

技术领域

本发明属于泊车路径规划领域，尤其涉及一种基于分子间作用力的泊车路径动态规划方法。

背景技术

近年来，国内汽车保有量持续增长，城市中停车位资源日趋减少，停车空间也越来越小。许多没有充足驾驶经验的驾驶员在面对狭小空间的泊车时，往往需要多次尝试，甚至某些场景下仍无法顺利完成泊车，耗费大量时间。

自动泊车系统可以在一定程度上缓解停车难的问题。其通过车辆搭载的传感器获取车身周围环境信息，再考虑车辆转向、障碍物相对位置等约束条件，然后规划出一条可行路径，最后控制车辆跟随路径完成泊车。其中路径规划是自动泊车的关键环节，规划出的路径是否合理直接决定了泊车的效果。目前泊车路径规划的方法主要有，根据感知到的车位环境信息，采用圆弧-直线或双圆弧方法。

然而，由于泊车环境空间不足的问题，往往车辆很难按照规划出的路径一次完成泊车，尤其是垂直停车位在泊车开始前需要较大的侧方空间才能一次泊入。在车辆多次调整的过程中，由于传感器的探测范围等功能限制、车辆低速下的跟踪精度误差等问题，会使误差不断积累，可能会导致碰撞的发生。

参考分子间相互作用力，夸克粒子间的引力随着距离的增加而增加，当距离很小时引力为零；分子间的斥力随着距离的减小而增加，当距离很小时斥力无限大，近似为刚性。

因此，需要根据车辆与各要素间相对位置关系，在泊车过程中进行实时的动态规划，以避免碰撞发生的可能。

发明内容

本发明提供一种基于分子间作用力的泊车路径动态规划方法，以减少自动泊车过程中由于环境感知不充分、跟踪误差积累等原因导致的失败甚至是碰撞发生的可能性，在面对狭小车位时，可以根据斥力、引力作用通过多次调节顺利完成泊车。

为实现上述发明目的，本发明所采用的技术方案包括如下步骤：

1)利用车载超声波雷达、视觉传感器、毫米波雷达获取目标车位信息和周围障碍物信息，当获取到可泊车位且识别车位类型后开始泊车，并实时检测车位和障碍物与车辆之间的相对位置关系；

2)车辆前后轴分别受到假想吸引点的吸引作用，且距离越远引力越强，引力对车身产生虚拟转矩，有使车身绕质心发生转动的趋势，从作用于车身的虚拟转矩计算所需的目标方向盘转角α：

其中M为车身所受转矩，M₀为预设的转矩阈值，k_S为预设的方向盘转角跟随系数；

3)当检测到障碍物距离车辆小于1米时假想对车身产生斥力作用，且距离越近斥力越强；当障碍物距离车身小于20cm时，车辆停止并换挡向反方向行驶；根据假想的引力和斥力综合作用下的总虚拟转矩计算目标方向盘转角，控制车辆泊车方向；

4)当车辆未能一次完成泊车，车辆倒车时后方遇到车位边线，在距离车位线小于20cm时，车辆停止并换挡向前行驶，并根据所受转矩计算目标方向盘转角；车辆前进时前方遇到车位线，在距离小于20cm时车辆停止并换挡向后行驶，并根据所受转矩计算目标方向盘转角；

5)当车辆前后轴中点分别在对应的理想停车范围内时，完成泊车；前后轴理想停车范围为前后轴理想停车点附近10cm区域，两个理想停车点分别为车辆位于车位正中间时的前后轴中点位置。

进一步地，上述步骤2)中，所述假想吸引点的设置为：

1)针对侧方停车位，吸引点为侧方停车位中前后轴理想停车点，当吸引力作用线不被预设的引力边界遮挡时，车辆前后轴中点分别受到对应吸引点的引力作用；

2)针对垂直停车位，吸引点为车辆根据阿克曼转向几何得到的可以一次进入垂直库位时的前后轴中点对应位置，且后轴中点不被预设的引力边界遮挡时，车辆开始受到引力作用。

进一步地，上述步骤2)中，所述引力大小和虚拟转矩的计算方式为：

1)车辆后轴中点所受引力的大小F_Y1＝k₁d₁，其中k₁为后轴中点所受到的引力的系数，d₁为后轴中点到对应吸引点的距离；在引力F_Y1的作用下，对车身产生的转矩大小为M₁＝F_Y1l₁，其中l₁为引力F_Y1所在直线到车辆质心的距离；

2)车辆前轴中点所受引力大小为F_Y2＝k₂d₂，其中k₂为前轴中点所受到引力的系数，d₂为前轴中点到对应吸引点之间的距离；引力F_Y2产生的转矩为M₂＝-F_Y2l₂，其中l₂为引力F_Y2所在直线到车辆质心的距离。

进一步地，上述步骤3)中，所述的障碍物与车辆相对位置关系、斥力和转矩的计算方式为：

1)第一类障碍物为出现在车辆正后方时，此时车辆受到斥力大小为

其中k_Z1是第一类斥力系数，d_Z1是障碍物与车辆后轴中点之间的距离。当障碍物在车身后方左半区域时，对车身的转矩为M_Z1＝F_Z1l_Z1；当障碍物在车身后方右半区域时，对车身的转矩为M_Z1＝-F_Z1l_Z1,其中l_Z1为车辆质心到障碍物与车辆后轴中点连线的距离；

2)第二类障碍物为出现在车辆后轴后方的两侧区域内，斥力的大小为

其中k_Z2为第二类斥力系数；当障碍物在车身左侧区域时，对车身的转矩大小为M_Z2＝F_Z2l_Z1；当障碍物在车身右侧区域时，对车身的转矩大小为M_Z2＝-F_Z2l_Z1；

3)第三类障碍物为出现在车辆前方区域内，此时车辆与障碍物之间斥力大小为

其中k_Z3是第三类斥力系数，d_Z2是障碍物与车辆前轴中点之间的距离；当障碍物在车身左半区域时，转矩为M_Z3＝F_Z3l_Z2；当障碍物在车身右半区域时，转矩为M_Z3＝-F_Z3l_Z2，其中l_Z2为车辆质心到障碍物与车辆后轴中点连线的距离；

4)第四类障碍物为出现在车辆后轴之前的区域内，斥力大小为

其中k_Z4为此时的斥力系数；当障碍物出现在车身左半侧区域时，转矩为M_Z4＝-F_Z4l_Z2；当障碍物出现在车身右半侧区域时，转矩为M_Z4＝F_Z4l_Z2；

障碍物对车辆产生的斥力转矩为M_Z＝M_Z1+M_Z2+M_Z3+M_Z4。

进一步地，上述步骤3)中车身所受的总转矩为前后中点所受引力和障碍物对车身的斥力共同产生的转矩；当车身所受转矩变化时，目标方向盘转角跟随转矩的增加而向某一方向转动，转矩减小时向另一方向转动，当转矩大于预设的最大转矩值M₀或小于-M₀时，目标方向盘转角保持最大转角不变，直到转矩值减小。

进一步地，上述步骤4)中，车位线作为约束条件，针对垂直停车位，若车辆未能一次泊入理想停车点，且车辆前进时若前方没有障碍物约束，则当车身所受转矩为零时停止并换挡后退。

进一步地，上述步骤5)中，泊车完成的条件为：

1)针对侧方停车位，当车辆前后轴中点分别位于对应理想停车范围内时，即完成泊车。

2)针对垂直停车位，当车辆前后轴中点分别到达吸引点后，只需直线后退即可到达理想停车范围内。若车辆前后轴中点已经在吸引点后方且车身未平行于车位，则车辆继续后退并根据车身受到的转矩进行转向，直到前后轴中点进入理想停车点。

与现有技术相比，本发明降低了对车辆泊车前的起始位姿要求，通过动态规划方法提高了对不同泊车环境的适应性；通过实时测量与车位及附近障碍物的距离，以计算车身所受转矩的方式调节目标方向盘转角，控制方法简单；对跟踪精度要求不高，降低了误差修正的难度，提高了泊车效率。

附图说明

图1是本发明一种基于分子间作用力的泊车路径动态规划方法的流程图。

图2是侧方停车位假想吸引点及引力边界示意图。

图3是垂直停车位假想吸引点示意图。

图4是垂直停车位的引力边界示意图。

图5是车辆后轴受引力示意图。

图6是车辆前后轴受引力示意图。

图7是车辆正后方障碍物斥力示意图。

图8是车辆后轴后方障碍物斥力示意图。

图9是车辆正前方障碍物斥力示意图。

图10是车辆后轴前方障碍物斥力示意图。

图11是侧方停车位泊车完成示意图。

图12是垂直停车位泊车完成示意图。

图13是垂直停车位的车辆方向修正示意图。

图14是斥力和引力共同作用示意图。

图15是侧方停车位的前后轴轨迹仿真示意图。

图16是垂直停车位的前后轴轨迹仿真示意图。

具体实施方式：

下面结合附图和具体实施例对本发明作进一步的说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

如图1所示为本发明的方法流程图，包括以下步骤：

1)利用车载超声波雷达、视觉传感器、毫米波雷达获取目标车位信息和周围障碍物信息，当获取到可泊车位且识别车位类型后开始泊车，并实时检测车位和障碍物与车辆之间的相对位置关系；

2)车辆前后轴分别受到假想吸引点的吸引作用，且距离越远引力越强，引力对车身产生虚拟转矩，有使车身绕质心发生转动的趋势，从作用于车身的虚拟转矩计算所需的目标方向盘转角α：

其中M为车身所受转矩，M₀为预设的转矩阈值，k_S为预设的方向盘转角跟随系数；

3)当检测到障碍物距离车辆小于1米时假想对车身产生斥力作用，且距离越近斥力越强；当障碍物距离车身小于20cm时，车辆停止并换挡向反方向行驶；根据假想的引力和斥力综合作用下的总虚拟转矩计算目标方向盘转角，控制车辆泊车方向；

4)当车辆未能一次完成泊车，车辆倒车时后方遇到车位边线，在距离车位线小于20cm时，车辆停止并换挡向前行驶，并根据所受转矩计算目标方向盘转角；车辆前进时前方遇到车位线，在距离小于20cm时车辆停止并换挡向后行驶，并根据所受转矩计算目标方向盘转角；

5)当车辆前后轴中点分别在对应的理想停车范围内时，完成泊车；前后轴理想停车范围为前后轴理想停车点附近10cm区域，两个理想停车点分别为车辆位于车位正中间时的前后轴中点位置。

作为本发明的优选实施例，上述步骤2)中关于假想吸引点的设置包括以下内容：

针对侧方停车位，假想吸引点a’、b’为侧方车位中前后轴理想停车点，预设引力边界为车位前方边线向外扩大1.2m处，如图2所示，可以使车辆入库时车身与库位角保持一定距离。当假想前轴中点吸引力作用线a、a’点连线不被预设的引力边界遮挡时，首先车辆前轴中点a点受到a’点的假想吸引力作用，如图2所示。同理，当假想后轴中点吸引力作用线b、b’点连线不被预设引力边界遮挡时，后轴中点b点受到b’点的假想吸引力作用。针对垂直停车位，假想吸引点a’、b’为车辆根据阿克曼转向几何得到的可以一次进入垂直库位时的前后轴中点对应位置，如图3所示。当b、b’点连线不被预设的引力边线，即不被车位边线遮挡时，车辆开始受到引力作用，如图4所示。在引力作用下产生转矩使车身绕车辆质心发生转动，质心运动坐标为

其中(x₀，y₀)为车辆后轴中点坐标，(x₁，y₁)为车辆前轴中点坐标，L_W为车辆轴距，a₁为前轴中点到车辆质心的距离，a₂为后轴中点到车辆质心的距离。

作为本发明的优选实施例，上述步骤2)中关于引力和转矩的计算方式为：

车辆后轴中点b点所受引力的大小F_Y1＝k₁d₁，其中k₁为后轴中点所受到的引力的系数，d₁为后轴中点到对应吸引点的距离，即b点到b’点的距离。在引力F_Y1的作用下，对车身产生的转矩大小为M₁＝F_Y1l₁，其中l₁为引力F_Y1所在直线到车辆质心的距离，即为b、b’点连线所在直线到车辆质心c点的距离，如图5所示。前轴中点a点所受引力大小为F_Y2＝k₂d₂，其中k₂为前轴中点所受到引力的系数，d₂为前轴中点到对应吸引点直接的距离，即a点到a’点的距离。引力F_Y2产生的转矩为M₂＝-F_Y2l₂，其中l₂为引力F_Y2所在直线到车辆质心的距离，即为a、a’点连线所在直线到车辆质心c点的距离。车辆前后轴同时受到引力如图6所示。

作为本发明的优选实施例，上述步骤3)中的障碍物分为4类：

第一类障碍物为出现在车辆正后方时，如图7所示，此时车辆受到斥力大小为

其中k_Z1是斥力系数，d_Z1是障碍物与车辆后轴中点之间的距离。当障碍物在车身后方左半区域时，对车身的转矩为M_Z1＝F_Z1l_Z1；当障碍物在车身后方右半区域时，对车身的转矩为M_Z1＝-F_Z1l_Z1，其中l_Z1为车辆质心到障碍物与车辆后轴中点连线的距离。

第二类障碍物为出现在车辆后轴后方的两侧区域内，如图8所示，斥力的大小为

其中k_Z2是这个区域内的斥力系数。当障碍物在车身左侧区域时，对车身的转矩大小为M_Z2＝F_Z2l_Z1；当障碍物在车身右侧区域时，对车身的转矩大小为M_Z2＝-F_Z2l_Z1。

第三类障碍物为出现在车辆前方区域内，如图9所示，此时车辆与障碍物之间斥力大小为

其中k_Z3是斥力系数，d_Z2是障碍物与车辆前轴中点之间的距离。当障碍物在车身左半区域时，转矩为M_Z3＝F_Z3l_Z2；当障碍物在车身右半区域时，转矩为M_Z3＝-F_Z3l_Z2，其中l_Z2为车辆质心到障碍物与车辆后轴中点连线的距离。

第四类障碍物为出现在车辆后轴之前的区域内，如图10所示，斥力大小为

其中k_Z4为此时的斥力系数。当障碍物出现在车身左半侧区域时，转矩为M_Z4＝-F_Z4l_Z2；当障碍物出现在车身右半侧区域时，转矩为M_Z4＝F_Z4l_Z2。

步骤3)中车位顶点也作为需要躲避的障碍物考虑。车身所受的总转矩为前后中点所受引力和障碍物对车身的斥力共同产生的转矩。当车身所受转矩变化时，目标方向盘转角跟随转矩的增加而向某一方向转动，转矩减小时向另一方向转动，当转矩大于预设的最大转矩值M₀或小于-M₀时，目标方向盘转角保持最大转角不变，直到转矩值减小。障碍物对车辆产生的斥力转矩为M_Z＝M_Z1+M_Z2+M_Z3+M_Z4。

作为本发明的优选实施例，上述步骤4)中车位线作为约束条件，针对垂直停车位，若车辆未能一次泊入理想停车点，且车辆前进时若前方没有障碍物约束，则当车身所受转矩为零时停止并换挡后退。

作为本发明的优选实施例，上述步骤5)中泊车完成的条件为：

针对侧方停车位，当车辆前后轴中点分别位于理想停车点时，如图11所示，即完成泊车。针对垂直停车位，车辆前后轴中点分别到达吸引点后，只需直线后退即可到达理想停车范围内，如图12所示。若车辆前后轴中点已经在吸引点后方，且车身未平行于车位，则车辆继续后退并根据车身受到的转矩进行转向，如图13所示，当进入停车点完成泊车。

在本实施例中k₁＝300，k₂＝600，k₃＝200，k₄＝200，k_Z1＝500，k_Z2＝k_Z3＝k_Z4＝300，车辆前轴中点到质心的距离a₁＝1.1，车辆后轴中点到质心的距离a₂＝1.65，k_S＝2.1，M₀＝240N*m，车辆长度L为4.65米，宽度为1.9米，轴距L_W为2.8米，前悬为0.96米，后悬为0.94米。

车辆在搜寻到库位信息后即可进入泊车状态，车辆首先直线后退，当受到假想吸引点的引力作用时，开始计算对车身的虚拟转矩，根据车身所受虚拟转矩从而实时计算所需的目标方向盘转角，车辆只需使方向盘跟随此目标方向盘转角即可。若在这个过程中有障碍物距离车身小于1米，则假想对车身产生斥力作用，且距离越近，斥力越大。根据引力和斥力综合作用下产生的虚拟转矩计算目标方向盘转角，如图14所示，直到完成泊车。下面分别对狭小的侧方停车位和垂直停车位进行仿真分析，验证了方法的有效性，可以对无法一次泊入的库位通过动态规划多次调节完成泊入。

针对侧方停车位进行仿真，设置车位尺寸为6×2.5米，车辆初始侧方距离1米，泊车轨迹仿真结果如图15所示。在面对狭小侧方停车位时，经过三次调整之后成功泊入理想停车点。

针对垂直停车位进行仿真，设置车位尺寸为6×2.7米，车辆初始侧方距离0.5米，车位对侧2.9米处存在障碍物约束，泊车轨迹仿真结果如图16所示。在经过两次调整后，成功泊入狭小空间的垂直停车位理想停车点。

Claims (7)

1.一种基于分子间作用力的泊车路径动态规划方法，其特征在于，包括如下步骤：

1)利用车载超声波雷达、视觉传感器、毫米波雷达获取目标车位信息和周围障碍物信息，当获取到可泊车位且识别车位类型后开始泊车，并实时检测车位和障碍物与车辆之间的相对位置关系；

2)车辆前后轴分别受到假想吸引点的吸引作用，且距离越远引力越强，引力对车身产生虚拟转矩，有使车身绕质心发生转动的趋势，从作用于车身的虚拟转矩计算所需的目标方向盘转角α：

其中M为车身所受转矩，M₀为预设的转矩阈值，k_S为预设的方向盘转角跟随系数；

3)当检测到障碍物距离车辆小于1米时假想对车身产生斥力作用，且距离越近斥力越强；当障碍物距离车身小于20cm时，车辆停止并换挡向反方向行驶；根据假想的引力和斥力综合作用下的总虚拟转矩计算目标方向盘转角，控制车辆泊车方向；

4)当车辆未能一次完成泊车，车辆倒车时后方遇到车位边线，在距离车位线小于20cm时，车辆停止并换挡向前行驶，并根据所受转矩计算目标方向盘转角；车辆前进时前方遇到车位线，在距离小于20cm时车辆停止并换挡向后行驶，并根据所受转矩计算目标方向盘转角；

5)当车辆前后轴中点分别在对应的理想停车范围内时，完成泊车；前后轴理想停车范围为前后轴理想停车点附近10cm区域，两个理想停车点分别为车辆位于车位正中间时的前后轴中点位置。

2.根据权利要求1所述的基于分子间作用力的泊车路径动态规划方法，其特征在于，所述步骤2)中，所述假想吸引点的设置为：

1)针对侧方停车位，吸引点为侧方停车位中前后轴理想停车点，当吸引力作用线不被预设的引力边界遮挡时，车辆前后轴中点分别受到对应吸引点的引力作用；

2)针对垂直停车位，吸引点为车辆根据阿克曼转向几何得到的可以一次进入垂直库位时的前后轴中点对应位置，且后轴中点不被预设的引力边界遮挡时，车辆开始受到引力作用。

3.根据权利要求1所述的基于分子间作用力的泊车路径动态规划方法，其特征在于，所述步骤2)中，所述引力大小和虚拟转矩的计算方式为：

1)车辆后轴中点所受引力的大小F_Y1＝k₁d₁，其中k₁为后轴中点所受到的引力的系数，d₁为后轴中点到对应吸引点的距离；在引力F_Y1的作用下，对车身产生的转矩大小为M₁＝F_Y1l₁，其中l₁为引力F_Y1所在直线到车辆质心的距离；

2)车辆前轴中点所受引力大小为F_Y2＝k₂d₂，其中k₂为前轴中点所受到引力的系数，d₂为前轴中点到对应吸引点之间的距离；引力F_Y2产生的转矩为M₂＝-F_Y2l₂，其中l₂为引力F_Y2所在直线到车辆质心的距离。

4.根据权利要求1所述的基于分子间作用力的泊车路径动态规划方法，其特征在于所述步骤3)中，所述的障碍物与车辆相对位置关系、斥力和转矩的计算方式为：

1)第一类障碍物为出现在车辆正后方时，此时车辆受到斥力大小为

其中k_Z1是第一类斥力系数，d_Z1是障碍物与车辆后轴中点之间的距离；当障碍物在车身后方左半区域时，对车身的转矩为M_Z1＝F_Z1l_Z1；当障碍物在车身后方右半区域时，对车身的转矩为M_Z1＝-F_Z1l_Z1，其中l_Z1为车辆质心到障碍物与车辆后轴中点连线的距离；

2)第二类障碍物为出现在车辆后轴后方的两侧区域内，斥力的大小为

其中k_Z2为第二类斥力系数；当障碍物在车身左侧区域时，对车身的转矩大小为M_Z2＝F_Z2l₂₁；当障碍物在车身右侧区域时，对车身的转矩大小为M_Z2＝-F_Z2l_Z1；

3)第三类障碍物为出现在车辆前方区域内，此时车辆与障碍物之间斥力大小为

其中k_Z3是第三类斥力系数，d_Z2是障碍物与车辆前轴中点之间的距离；当障碍物在车身左半区域时，转矩为M_Z3＝F_Z3l_Z2；当障碍物在车身右半区域时，转矩为M_Z3＝-F_Z3l_Z2，其中l_Z2为车辆质心到障碍物与车辆后轴中点连线的距离；

4)第四类障碍物为出现在车辆后轴之前的区域内，斥力大小为

其中k_Z4为此时的斥力系数；当障碍物出现在车身左半侧区域时，转矩为M_Z4＝-F_Z4l_Z2；当障碍物出现在车身右半侧区域时，转矩为M_Z4＝F_Z4l_Z2；

障碍物对车辆产生的斥力转矩为M_Z＝M_Z1+M_Z2+M_Z3+M_Z4。

5.根据权利要求1所述的基于分子间作用力的泊车路径动态规划方法，其特征在于所述步骤3)中车身所受的总转矩为前后中点所受引力和障碍物对车身的斥力共同产生的转矩；当车身所受转矩变化时，目标方向盘转角跟随转矩的增加而向某一方向转动，转矩减小时向另一方向转动，当转矩大于预设的最大转矩值M₀或小于-M₀时，目标方向盘转角保持最大转角不变，直到转矩值减小。

6.根据权利要求1所述的基于分子间作用力的泊车路径动态规划方法，其特征在于所述步骤4)中，车位线作为约束条件，针对垂直停车位，若车辆未能一次泊入理想停车点，且车辆前进时若前方没有障碍物约束，则当车身所受转矩为零时停止并换挡后退。

7.根据权利要求1所述的基于分子间作用力的泊车路径动态规划方法，其特征在于所述步骤5)中，泊车完成的条件为：

1)针对侧方停车位，当车辆前后轴中点分别位于对应理想停车范围内时，即完成泊车；

2)针对垂直停车位，当车辆前后轴中点分别到达吸引点后，只需直线后退即可到达理想停车范围内；若车辆前后轴中点已经在吸引点后方且车身未平行于车位，则车辆继续后退并根据车身受到的转矩进行转向，直到前后轴中点进入理想停车点。

Images