CN111434550A - 一种基于仿真的泊车策略生成方法及系统 - Google Patents

Abstract

本发明涉及一种基于仿真的泊车策略生成方法及系统，属于智能驾驶技术领域。现有技术中各种停车地点情况不同，难以生成比较优越的泊车策略；本申请提供了基于仿真的泊车策略生成方法及系统，通过多个仿真器分别生成不同的泊车策略，再由路径评价单元通过特定的公式对每条泊车策略进行评价，选出最优的策略，使得该系统可适应不同的停车场地的各种情况，给出优越的泊车策略。

Description

一种基于仿真的泊车策略生成方法及系统

技术领域

本发明涉及一种智能驾驶技术领域，具体涉及一种基于仿真的泊车策略生成方法及系统。

背景技术

在实车运行时，在仿真环境中，使用泊车策略(planning policy)获得可以安全入库的规划路线(trajectory)，其中，泊车策略是指输入当前车辆的定位位姿和目标车位的定位位姿，输出控制车辆到达目标车位的车辆线速度和车辆转向角度，泊车策略输出的车辆线速度和车辆转向角度约束车辆只能在可行区域内行驶并最终驶入目标车位中。

具体的，在用户开启获取泊车策略的仿真软件程序时，仿真软件程序首先获取车辆当前所处环境的地图，根据地图、用户输入的目标车位信息以及车辆当前在地图中的坐标信息，获取车辆由当前位置驶入目标车位时可行驶的区域，即可行区域，获取侧面距离，侧面距离即车辆驶入该可行区域环境时，车辆相对靠近目标车位的车身侧面距离车位线的距离，而后进入训练泊车策略的过程。

例如，仿真环境可以如图1所示，矩形区域A为可行区域，可行区域的长度可以为8-10m，可行区域的宽可以为5-6m；矩形区域B为目标车位，目标车位的宽可以为2.6-3m；目标车位中箭头的指向为泊车时车头的朝向，即车辆必须按该朝向停到目标车位才视为任务成功；侧面距离的取值可以在0.5-2m之间，不同侧面距离对应不同泊车任务下的最优泊车策略，具体而言，侧面距离过小或过大都会加大寻找最优泊车策略的难度，例如，侧面距离为0.5时就很难泊车，侧面距离为2米时就相对容易一些。

发明内容

有鉴于此，本发明提供一种基于仿真的泊车策略生成方法，其特征在于：所述方法包括以下步骤：

步骤S1：启动基于仿真的泊车策略生成程序；

步骤S2：初始化用于所述仿真的仿真器；其中包括生成可行驶区域；

步骤S3：所述仿真器仿真生成多条候选泊车路径；

步骤S4：对所述候选路径进行评价，选择最优的候选路径。

优选地，所述仿真器仿真生成候选泊车路径即根据车辆观测信息和地图信息生成多条候选的自动泊车路径；所述车辆观测信息具体为七维向量o＝(x,y,yaw,s1,s2,s3,s4)；其中x，y分别表示车辆转向中心在可行区域的坐标系下的x坐标与y坐标，yaw为车辆当前姿态与x轴的角度，s1,s2,s3,s4四个参数主要用于在自动泊车过程中车辆边缘与其它物体的碰撞检测；车辆转向中心为两个后轮圆心连线的中点坐标。

优选地，所述仿真器可通过(1)一次性路径规划模式，或(2)实时泊车路径规划模式来仿真生成候选泊车路径；

其中，一次性路径规划模式为在控制车辆到达目标位置过程中只仿真规划一次，最后给出最优泊车路径；而多步仿真规划模式在每个车辆运行控制节点上都会重新进行规划，重新搜索路径。

优选地，在实时泊车路径规划模式下，候选泊车路径生成包括以下步骤：S31调用一次性规划生成单步最优路径节点；S32:基于强化学习模型搜索规划路径。

优选地，所述路径评价单元通过以下学习奖励公式来评价每一条可能的泊车路径的优越程度：

Y＝w1*distance(car position,target position)+w2*abs(car yaw-targetyaw)+w3*target reached；

其中，Y表示泊车路径的优越程度；w1,w2表示控制任务完成度；w3表示任务完成的额外奖励；distance(car position,target position)表示车辆实时位置与目标车位之间的距离；abs(car yaw-target yaw)表示当前车辆的倾角与目标泊车时倾角差值的绝对值。

本发明还提供了一种基于仿真的泊车策略生成系统，其特征在于：所述系统包括启动单元、多个仿真器、路径评价单元；

所述启动单元：用于启动基于仿真的泊车策略生成程序；

多个仿真器：仿真生成多条候选泊车路径；其中包括生成可行驶区域；

路径评价单元：对所述候选路径进行评价，选择最优的候选路径。

优选地，所述仿真器仿真生成候选泊车路径即根据车辆观测信息和地图信息生成多条候选的自动泊车路径；所述车辆观测信息具体为七维向量o＝(x,y,yaw,s1,s2,s3,s4)；其中x，y分别表示车辆转向中心在可行区域的坐标系下的x坐标与y坐标，yaw为车辆当前姿态与x轴的角度，s1,s2,s3,s4四个参数主要用于在自动泊车过程中车辆边缘与其它物体的碰撞检测。

优选地，所述仿真器可通过(1)一次性路径规划模式，或(2)实时泊车路径规划模式来仿真生成候选泊车路径；

其中，一次性路径规划模式为在控制车辆到达目标位置过程中只仿真规划一次，最后给出最优泊车路径；而多步仿真规划模式在每个车辆运行控制节点上都会重新进行规划，重新搜索路径。

优选地，在实时泊车路径规划模式下，候选泊车路径生成包括以下步骤：S31调用一次性规划生成单步最优路径节点；S32:基于强化学习模型搜索规划路径。

优选地，在步骤S4中，通过以下学习奖励公式来评价每一条可能的泊车路径的优越程度：

Y＝w1*distance(car position,target position)+w2*abs(car yaw-targetyaw)+w3*target reached；

其中，Y表示泊车路径的优越程度；w1,w2表示控制任务完成度；w3表示任务完成的额外奖励；distance(car position,target position)表示车辆实时位置与目标车位之间的距离；abs(car yaw-target yaw)表示当前车辆的倾角与目标泊车时倾角差值的绝对值。

本发明的发明点在于以下几点，但不限于以下几点：

(1)本申请采用多个仿真器，进行不同路径的仿真，并可从中进行选择，仿真生成候选泊车路径以及最优泊车路径搜索的算法复杂度与待规划场景的复杂程度是紧密联系的，其复杂度往往与可行驶区域的自由度成指数关系；本申请中的仿真器能够根据泊车场景自适应的选择不同的路径规划方式；

(2)本申请仿真器可通过一次性路径规划模式，或实时泊车路径规划模式来仿真生成候选泊车路径；其中，一次性路径规划模式为在控制车辆到达目标位置过程中只仿真规划一次，最后给出最优泊车路径；而实时仿真规划模式在每个车辆运行控制节点上都会重新进行规划，重新搜索路径；

(3)本申请通过特别设定的学习奖励公式来评价每一条可能的泊车路径的优越程度：Y＝w1*distance(car position,target position)+w2*abs(car yaw-target yaw)+w3*target reached，有效的对多条路径进行了评价；

(4)所述仿真器仿真生成候选泊车路径即根据车辆观测信息和地图信息生成多条候选的自动泊车路径；所述车辆观测信息具体为七维向量o＝(x,y,yaw,s1,s2,s3,s4)，由此可见，本申请仿真器充分考虑了车辆的地理位姿信息，仿真结果具有可行性。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，并不构成对本发明的限定。在附图中：

图1是本发明实施例提供的一种环境设计的示意图；

图2是本发明实施例提供的一种并行仿真训练泊车策略的流程图。

具体实施例

为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施方式和附图，对本发明做进一步详细说明。在此，本发明的示意性实施方式及其说明用于解释本发明，但并不作为对本发明的限定。

本发明的仿真环境如图1所示，矩形区域A为可行区域，可行区域的长度可以为8-10m，可行区域的宽可以为5-6m；矩形区域B为目标车位，目标车位的宽可以为2.6-3m；目标车位中箭头的指向为泊车时车头的朝向，即车辆必须按该朝向停到目标车位才视为任务成功；侧面距离的取值可以在0.5-2m之间，不同侧面距离对应不同泊车任务下的最优泊车策略，具体而言，侧面距离过小或过大都会加大寻找最优泊车策略的难度，例如，侧面距离为0.5时就很难泊车，侧面距离为2米时就相对容易一些。

本申请通过并行仿真的方法产生多条候选泊车路径，并通过对候选路径的评估给出泊车策略，即下一步控制车辆行驶的车辆线速度和车辆转向角度。仿真泊车策略流程图如图2所示，具体过程如下：

步骤1：启动基于实时仿真泊车策略

用户需要点击按键，启动仿真训练泊车策略。启动仿真泊车策略时需要用户选择确定泊车策略，即选择目标泊车位姿。启动仿真策略后，仿真系统即进行初始化获取地图信息。该地图信息主要用于输出泊车位置坐标信息、可行驶区域以及当前车辆在地图中的位置，为自动泊车策略提供导航信息。

步骤2：初始化或更新化仿真器

仿真器(simulator)需要根据当前车辆的观测信息以及地图导航生成候选泊车路径。需要获取的信息包括泊车目标在地图中的位置(xo,yo)，车辆转向角yaw，控制车辆当前在地图中的坐标信息(xs,ys),以及可行驶区域。可行驶区域是由多条线段围成的，是包含控制车辆起始点和终止点的闭合区域。假设{L1,L2,…,Ln}为组成可行驶区域的线段集合，这些线段首尾相接可以组成闭合的可行驶区域。可行驶区域信息的作用主要体现在两方面，一方面是为仿真器生成候选泊车路径提供生成空间和约束条件；另一方面为车辆碰撞检测提供依据。

步骤3：仿真生成候选路径(candidate trajectory)

在仿真环境中，车辆观测状态(observation)包括当前车辆坐标和传感器信息。根据可行区域的地图信息获得车辆在可行区域中的当前车辆坐标为(x,y,yaw)，其中，x，y分别表示车辆转向中心在可行区域的坐标系下的x坐标与y坐标，yaw为车辆当前姿态与x轴的角度。传感器信息(s1,s2,s3,s4)为车辆四个角点(例如，车辆最前端的两个角点和车辆最后端的两个角点，具体如图1中1、2、3、4所示的四个角点)处安装的声呐传感器测量得到的各角点到最近障碍物的距离。因此，车辆观测状态为七维向量o＝(x,y,yaw,s1,s2,s3,s4)。s1,s2,s3,s4四个参数主要用于在自动泊车过程中车辆边缘与其它物体的碰撞检测。仿真生成候选泊车路径即根据上述的车辆观测信息和地图信息生成多条候选的自动泊车路径。最优自动泊车路径的获取依赖两个因素，第一，仿真器在生成的候选泊车路径中包含了最优自动泊车路径；该因素与仿真器的性能相关。第二，泊车路径评价算法能够从候选泊车路径中将最优泊车路径挑选出来。

仿真生成候选泊车路径以及最优泊车路径搜索的算法复杂度与待规划场景的复杂程度是紧密联系的，其复杂度往往与可行驶区域的自由度成指数关系。本申请中的仿真器能够根据泊车场景自适应的选择不同的路径规划方式。在泊车场景及路径相对简单的情况下，仿真器进行一次性泊车路径规划。泊车场景及路径相对复杂的情况下，仿真器进行多步规划产生候选路径。泊车场景复杂程度的判断主要基于停车策略和地图中的可行驶区域进行度量。

一次性规划模式计算简单，针对简单自动泊车场景能够快速生成规划路径。在简单场景中一次性规划生成的最优规划路径往往能够满足实际泊车需求，即经过一次性规划后控制车辆按照泊车路径自动行驶即可达到泊车的目的。但此类方法难以适应复杂以及多变的泊车场景，当一次性规划在遇到位置变动的障碍物以及可行驶区域形状复杂时，生成的泊车路径未必是最优行驶路径。这要求仿真器能够实时进行多步规划，即行驶过程中需要实时更新仿真器信息并重新规划泊车路径。因此，为了更加高效的针对不同场景进行自动泊车规划，本仿真系统支持两种泊车路径规划模式，即一次性泊车路径规划和实时泊车路径规划模式。这两种模式在应用时由系统根据地图和停车策略自动判断、调用。

1)、一次性泊车路径规划模式。在单次规划的算法搜索过程中，通过调用仿真环境来约束搜索状态，本申请中主要利用地图中的可行驶区域以及车辆侧边与可行驶区域边界的距离进行约束。约束条件为：首先，所有规划路径节点都包含在可行驶区域内；其次，要求车辆侧面距离可行驶区域边界大于0.5m，因此实际中当该距离小于0.5m时，会出现泊车困难的情形。穷举搜索，A*，RRT(Rapidly-exploring random tree)*等算法均可用于一次性泊车路径规划中的路径搜索，这里以RRT*算法为例，给出在约束条件下进行一次性泊车路径规划方法，其它如A*,RRT*算法照此类推：

首先，基于可行驶区域以及泊车的起始和终止点坐标随机生成路径节点；当路径节点到可行驶区域边界的距离小于0.5m时，删除当前路径节点。生成随机节点时，生成的随机节点可约束为到当前节点的欧式距离不小于d1且生成的同一深度的随机节点之间的欧式距离不小于d2。通过上述的约束可以避免生成大量的无效泊车路径，可提高仿真器的运行速度。假设每次生成的候选路径节点数为K，路径长度为M，即每条泊车路径上有M个节点；欧式距离为欧几里得距离，指欧几里得空间中两点间“普通”(即直线)距离。

然后，基于候选路径节点集合中任何两个节点之间的连接生成所有可能的泊车路径，生成路径的数量为K^M-1；这些路径基本覆盖了几乎所有可能的行驶路径，但绝大部分都不是理想的形式路径。

最后，根据步骤4对所有的泊车路径进行评价打分,给出当前仿真器规划产生的最优泊车路径。

2)、实时泊车路径规划模式，如强化学习，MDP(Markov Decision Process)模型。在一次性泊车路径规划中，算法每步迭代一次生成一个路径节点。通过多步反复调用一次性规划模型，在模拟器中可以生产一系列的路径点。

多步规划模型中以强化学习模型为例进行示范，其它类似MDP模型依次类推：

Step1:调用一次性规划生成单步最优路径节点；

Step2:基于强化学习搜索规划路径:

深度强化学习来训练泊车策略，只有在探索到目标车位或者发生碰撞才会停止规划，并根据奖励函数获得相应奖励。具体的，在深度强化学习过程中,本申请从探索序列中学习泊车策略，探索序列[o0,a0,r0,o1,a1,r1,o2,a2,r2,...]中的每个元组(oi,ai,ri)由三个元素组成：车辆观测状态o，车辆在该观测状态下执行的预测动作a，任务反馈奖励值r，探索目标argmax(a0,a1,a2,...)(r0+r1+r2+...)，i＝0,1,2,....等自然数，i表示元组更新的序数。

在深度强化学习过程中,元组(即观测状态o、预测动作a和奖励值r)每0.1s更新一次。也就是说,实时根据当前观测状态o,输出预测动作a和奖励值r进行路线规划,例如,基于初始的当前观测状态o0输出预测动作a0,车辆执行预测动作a0后得到更新的当前观测状态o1,奖励函数基于更新的当前观测状态o1和目标车位输出奖励值r0，得到原始元组(当前观测状态o0、预测动作a0、奖励值r0)；将更新后的当前观测状态o1视为当前观测状态,再基于当前观测状态o1输出预测动作a1,车辆再执行预测动作a1得到更新的当前观测状态o2,奖励函数再基于更新的当前观测状态o2和目标车位得到奖励值r1,得到更新一次的元组(当前观测状态o1、预测动作a1、奖励值r1)；以此类推,直至车辆到达目标车位，由各次输出的预测动作a组成一条由初始位置到目标车位的完成路线。

以上两种模式的区别在于一次性泊车路径规划模式在控制车辆到达目标位置过程中只仿真规划一次，最后给出最优泊车路径；而多步仿真规划模式在每个车辆运行控制节点上都会重新进行规划，重新搜索路径，如此能够适应多变、复杂的泊车场景。

步骤4：对候选路径进行评价

在训练泊车策略得到多条泊车路径之后，本申请还可以通过以下学习奖励公式来评价每一条可能的泊车路径的优越程度(结果数值越大表示泊车路径越优)：

Y＝w1*distance(car position,target position)+w2*abs(car yaw-targetyaw)+w3*target reached

其中,Y表示泊车路径的优越程度；w1,w2表示控制任务完成度；w3表示任务完成的额外奖励；yaw表示的是车头的角度，abs(car yaw-target yaw)表示当前车辆的倾角与目标泊车时倾角差值的绝对值。

假设规划任务的空间是由{L1,L2,…,Ln}组合而成，则w1＝1/Lm；Lm是{L1,L2,…,Ln}中所有线段长度的均值。w2＝1/2π；w3＝1，distance()函数返回车辆转向中心距离目标车位点的距离，abs()函数为取括号内数的绝对值，target reached表明车辆是否到达目标车位，如果车辆到达目标车位，则target reach＝1，否则，target reach＝0。

针对路径进行评价打分后，每一条候选泊车路径都得到一个评价分数，一个仿真器输出的最优路径通过下式选择

Ys＝argmax(Y1,Y2,…,Yn)

其中，n＝KM为候选泊车路径的数量，Y1,Y2,…,Yn代表不同的泊车路径。Ys为评价分数最高的泊车路径。

无论是一次性规划还是多步规划模式，本申请都通过并行计算的模式进行多次规划，最终通过求路径分数最大的方式，获得多次规划中的最优路径。通过并发的多个仿真器进行多次规划可以有效的避免单仿真器一次性规划有可能出现的不收敛问题。多仿真器规划最终输出路径为：

Yo＝argmax(Ys1,Ys2,…,Ysk)

其中，Ys1是仿真器1输出的最优路径对应的评价分数，Ys2是仿真器2输出的最优路径对应的评价分数，依次的Ysk是仿真器k输出的最优路径对应的评价分数。实际应用时，可以取一个仿真器也可以视计算平台的计算能力取多个仿真器，仿真器越多泊车路径仿真规划效果越好。

Yo为本申请仿真系统输出的最优规划路径。

步骤5：控制车辆

车辆的动作空间(action)为能够控制车辆运动的输出，即预测动作a。在该仿真环境中预测动作a包括车辆线速度linear_v和车辆转向角度angular_z，即a＝(linear_v,angular_z)。通过车辆控制系统可以按照量化的预测结果控制车辆进行动作并达到相应的路径节点。完成车辆控制后系统进入步骤2并不断循环至车辆到达目标地点为止。

显然，本领域的技术人员应该明白，上述的本发明实施例的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明实施例不限制于任何特定的硬件和软件结合。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明实施例可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于仿真的泊车策略生成方法，其特征在于：所述方法包括以下步骤：

步骤S1：启动基于仿真的泊车策略生成程序；

步骤S2：初始化用于所述仿真的仿真器；其中包括生成可行驶区域；

步骤S3：所述仿真器仿真生成多条候选泊车路径；

步骤S4：对所述候选路径进行评价，选择最优的候选路径。

2.根据权利要求1所述的方法，其特征在于：所述仿真器仿真生成候选泊车路径即根据车辆观测信息和地图信息生成多条候选的自动泊车路径；所述车辆观测信息具体为七维向量o＝(x,y,yaw,s1,s2,s3,s4)；其中x，y分别表示车辆转向中心在可行区域的坐标系下的x坐标与y坐标，yaw为车辆当前姿态与x轴的角度，s1,s2,s3,s4四个参数主要用于在自动泊车过程中车辆边缘与其它物体的碰撞检测。

3.根据权利要求1所述的方法，其特征在于：所述仿真器可通过(1)一次性路径规划模式，或(2)实时泊车路径规划模式来仿真生成候选泊车路径；

其中，一次性路径规划模式为在控制车辆到达目标位置过程中只仿真规划一次，最后给出最优泊车路径；而实时仿真规划模式在每个车辆运行控制节点上都会重新进行规划，重新搜索路径。

4.根据权利要求3所述的方法，其特征在于：在实时泊车路径规划模式下，候选泊车路径生成包括以下步骤：S31：调用一次性规划生成单步最优路径节点；S32:基于强化学习模型搜索规划路径。

5.根据权利要求2所述的方法，其特征在于：在步骤S4中，通过以下学习奖励公式来评价每一条可能的泊车路径的优越程度：

Y＝w1*distance(car position,target position)+w2*abs(car yaw-target yaw)+w3*target reached；

其中,Y表示泊车路径的优越程度；w1,w2表示控制任务完成度；w3表示任务完成的额外奖励；distance(car position,target position)表示车辆实时位置与目标车位之间的距离；abs(car yaw-target yaw)表示当前车辆的倾角与目标泊车时倾角差值的绝对值。

6.一种基于仿真的泊车策略生成系统，其特征在于：所述系统包括启动单元、多个仿真器、路径评价单元；

所述启动单元：用于启动基于仿真的泊车策略生成程序；

多个仿真器：仿真生成多条候选泊车路径；其中包括生成可行驶区域；

路径评价单元：对所述候选路径进行评价，选择最优的候选路径。

7.根据权利要求6所述的系统，其特征在于：所述仿真器仿真生成候选泊车路径即根据车辆观测信息和地图信息生成多条候选的自动泊车路径；所述车辆观测信息具体为七维向量o＝(x,y,yaw,s1,s2,s3,s4)；其中x，y分别表示车辆转向中心在可行区域的坐标系下的x坐标与y坐标，yaw为车辆当前姿态与x轴的角度，s1,s2,s3,s4四个参数主要用于在自动泊车过程中车辆边缘与其它物体的碰撞检测。

8.根据权利要求6所述的系统，其特征在于：所述仿真器可通过(1)一次性路径规划模式，或(2)实时泊车路径规划模式来仿真生成候选泊车路径；

其中，一次性路径规划模式为在控制车辆到达目标位置过程中只仿真规划一次，最后给出最优泊车路径；而多步仿真规划模式在每个车辆运行控制节点上都会重新进行规划，重新搜索路径。

9.根据权利要求6所述的系统，其特征在于：在实时泊车路径规划模式下，候选泊车路径生成包括以下步骤：S31：调用一次性规划生成单步最优路径节点；S32：基于强化学习模型搜索规划路径。

10.根据权利要求7所述的系统，其特征在于：所述路径评价单元通过以下学习奖励公式来评价每一条可能的泊车路径的优越程度：

Y＝w1*distance(car position,target position)+w2*abs(car yaw-target yaw)+w3*target reached；

其中，Y表示泊车路径的优越程度；w1,w2表示控制任务完成度；w3表示任务完成的额外奖励；distance(car position,target position)表示车辆实时位置与目标车位之间的距离；abs(car yaw-target yaw)表示当前车辆的倾角与目标泊车时倾角差值的绝对值。

Images