一种无人车启发式自动泊车方法及装置

本发明涉及自动驾驶技术领域，特别涉及一种无人车启发式自动泊车方法及装置。

背景技术：

近年来，随着汽车工业的飞速发展和人们生活质量的不断提高，拥有私家车对多数家庭来说已不再是一种奢想，与此同时，也突显出一系列交通安全问题。一方面，汽车拥有量急剧增加，导致城市可利用的泊车空间越来越少；另一方面，新手逐年增多，由于不熟练导致的各种事故频发，泊车碰撞便是常见事故之一。如何提升汽车的操控性，降低新手泊车过程中存在的不便利，消除安全隐患，迅速准确安全地将汽车停靠到合适的位置逐渐引起了人们的关注。因此，设计有效安全、快速准确的自动泊车技术是当今社会的研究热点之一。

技术实现要素：

本发明提供了一种无人车启发式自动泊车方法及装置，以解决现有技术所存在的无法适应车辆任意初始位姿的自动泊车问题。

为解决上述技术问题，本发明提供了如下技术方案：

一方面，本发明提供了一种无人车启发式自动泊车方法，包括：

构建车辆位姿状态集；

构建多分辨率的状态转换规则集；其中，所述状态转换规则集为从任意位姿状态发散出来的、方向不同的、可以连接相邻两个不同位姿状态的所有分支集合；所述状态转换规则集包括在车辆起始位置和车辆目标泊车点预设范围内使用的第一规则集，以及在除车辆起始位置及车辆目标泊车点预设范围外的其它位置使用的第二规则集，所述第一规则集的精度大于所述第二规则集的精度；

构建用于泊车路径搜索的启发函数和估计函数；

基于构建的启发函数和估计函数，以及所述状态转换规则集，进行基于a^*算法的启发式自动泊车路径搜索，为车辆找出符合预设要求的自动泊车路径。

进一步地，所述构建车辆位姿状态集，包括：

定义车辆位姿状态，采用三维状态空间点(x,y,θ)来表征车辆特定位姿；其中，(x,y)表示车辆中心在世界坐标系中的位置，θ表示车身朝向角度；

通过离散化方法得到表征车辆位姿的所有状态的集合。

进一步地，所述构建多分辨率的状态转换规则集，包括：

定义状态转换规则集为从任意位姿状态发散出来的、方向不同的、可连接相邻两个不同位姿状态的所有分支集合，用一系列分支将起始状态、终止状态及两者间多个过渡状态按一定顺序连接起来，便得到泊车过程中的整条路径；

定义第一规则集和第二规则集，使两者位于同一个维度的网络空间中；

使所述第二规则集成为所述第一规则集的一个真子集，以确保所述第一规则集中的路径被充分利用，同时减少图搜索过程中的分支数量，并使得车辆在不同区域拥有不同的决策空间。

进一步地，所述构建用于泊车路径搜索的启发函数和估计函数，包括：

将车辆当前位置到终点的距离和车辆当前朝向与目标朝向之间的角度差分别进行归一化处理，表达式如下：

l′＝l/lt

α′＝α/360

其中，lt表示车辆起点到终点的欧氏距离，l表示车辆当前位置到终点的欧氏距离，l′表示归一化后的距离，α表示车辆当前朝向与目标朝向之间的角度差，且α的单位为度，α′表示归一化后的角度差；

基于归一化的距离和角度差，定义启发函数：

h^*(n)＝w1·l′+w2·α′

其中，w1为距离权值，w2为角度权值，且w1和w2相加为1；

构建状态节点的估计函数f^*(n)：

f^*(n)＝g^*(n)+h^*(n)

其中，g^*(n)为起始点到当前点路径的实际代价值。

进一步地，g^*(n)采用车辆实际已走过的路径长度及车身转过角度的加权和表示；其中，车辆实际已走过的路径长度及车身转过角度分别进行归一化处理。

进一步地，所述基于构建的启发函数和估计函数，根据所述状态转换规则集，进行基于a^*算法的启发式自动泊车路径搜索，为车辆找出符合预设要求的自动泊车路径，包括：

对启发函数进行膨胀，即给启发函数乘上一个大于1的膨胀因子ε，相应地，将估计函数修正为：f^*(n)＝g^*(n)+ε·h^*(n)；

构建open表和closed表；其中，open表用来存储待扩展节点，closed表用来存储已扩展节点；open表中的每一个节点n都有一个估价值g(start,n)+ε·h(n,goal)，其中，g(start,n)为车辆初始位置到车辆当前位置的实际路径代价值，h(n,goal)为启发函数，为当前点到目标点的路径估计代价；ε为大于1的膨胀因子；

路径搜索过程中，每次均选取open表中估价值最小的节点，判断当前选取的节点是否为目标节点，若是，则最优路径搜索过程结束，若不是，则按照所述状态转换规则集对当前选取的节点进行扩展，并将扩展出的且不属于预设禁止区域的新节点放入open表，同时设置这些新节点至其父节点的指针，并将父节点放入closed表，迭代这一过程，直至最优路径搜索过程结束；

在最优路径搜索过程结束后，根据所设的父节点指针从目标节点追溯到起始节点，形成最终的泊车路径，并输出最终的泊车路径。

进一步地，所述基于构建的启发函数和估计函数，根据所述状态转换规则集，进行基于a^*算法的启发式自动泊车路径搜索，为车辆找出符合预设要求的自动泊车路径，还包括：

逐步减小膨胀因子ε的值，从而将生成的次优路径不断优化，若膨胀因子ε减小至1，此时生成的路径为最优。

另一方面，本发明还提供了一种无人车启发式自动泊车装置，包括：

状态集构建模块，用于获取车辆位姿状态，构建并存储车辆位姿状态集；

规则集构建模块，用于构建并存储多分辨率的状态转换规则集；其中，所述状态转换规则集为从任意位姿状态发散出来的、方向不同的、可以连接相邻两个不同位姿状态的所有分支集合；所述状态转换规则集包括在车辆起始位置和车辆目标泊车点预设范围内使用的第一规则集，以及在除车辆起始位置及车辆目标泊车点预设范围外的其它位置使用的第二规则集，所述第一规则集的精度大于所述第二规则集的精度；

函数构建模块，用于构建并存储用于泊车路径搜索的启发函数和估计函数；

路径规划模块，用于基于所述函数构建模块所构建的启发函数和估计函数，以及所述规则集构建模块所构建的状态转换规则集，进行基于a^*算法的启发式自动泊车路径搜索，为车辆找出符合预设要求的自动泊车路径。

再一方面，本发明还提供了一种电子设备，其包括处理器和存储器；其中，存储器中存储有至少一条指令，所述指令由处理器加载并执行以实现上述方法。

又一方面，本发明还提供了一种计算机可读存储介质，所述存储介质中存储有至少一条指令，所述指令由处理器加载并执行以实现上述方法。

本发明提供的技术方案带来的有益效果至少包括：

本发明的上述方案通过构建车辆位姿状态集；构建多分辨率的状态转换规则集；构建用于泊车路径搜索的启发函数和估计函数；基于构建的启发函数和估计函数，根据状态转换规则集，进行基于a^*算法的启发式自动泊车路径搜索，为车辆找出符合预设要求的自动泊车路径。可以有效地改善汽车的操控性，显著减少泊车过程中的不便利，消除安全隐患，迅速、准确、安全地将汽车停靠到合适的位置，并且适合车辆任意初始位姿及边界约束条件下的自动泊车场景。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的无人车启发式自动泊车方法的流程示意图；

图2为本发明实施例提供的车辆位姿(x,y,θ)的三维空间网格结构图；

图3为本发明实施例提供的启发式自动泊车算法的高精度规则集示意图；

图4为本发明实施例提供的启发式自动泊车算法的低精度规则集示意图；

图5为本发明实施例提供的基于a*算法的自动泊车路径规划算法流程图；

图6为本发明实施例提供的启发式自动泊车效果示意图；

图7为本发明实施例提供的无人车启发式自动泊车装置的结构示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。

第一实施例

针对现有技术很少解决车辆任意初始状态和边界约束条件下的自动泊车问题，本实施例提供了一种无人车启发式自动泊车方法，该方法可以由电子设备实现，该电子设备可以是终端或者服务器。该方法执行流程如图1所示，包括：

s101，构建车辆位姿状态集；

s102，构建多分辨率的状态转换规则集；

s103，构建用于泊车路径搜索的启发函数和估计函数；

s104，基于构建的启发函数和估计函数，以及状态转换规则集，进行基于a^*算法的启发式自动泊车路径搜索，为车辆找出符合预设要求的自动泊车路径。

进一步地，所述构建车辆位姿状态集，包括：

定义车辆位姿状态，采用三维状态空间点(x,y,θ)来表征车辆特定位姿；其中，(x,y)表示车辆中心在世界坐标系中的位置，θ表示车身朝向角度；(x,y,θ)对确保车辆在世界坐标系中位姿的合法性起着很重要的作用，比如说能确保规划出的路径不会使车身朝向发生剧变，即保证路径的平滑性。

最后通过离散化方法得到表征车辆位姿的所有状态的集合。

进一步地，所述构建多分辨率的状态转换规则集，包括：

定义规则集为从任意状态发散出来的、方向不同的、可以连接相邻两个不同状态的所有分支集合，如图2所示，可通过一系列分支将起始状态、终止状态及两者间多个过渡状态按一定顺序连接起来，便得到泊车过程中的整条路径。

定义第一规则集和第二规则集，如图3和图4所示，使两者位于同一个维度的网络空间(x,y,θ)中；其中，第一规则集为在车辆起始位置和车辆目标泊车点预设范围内使用的高精度规则集ah，第二规则集为在除车辆起始位置及车辆目标泊车点预设范围外的其它位置使用的低精度规则集al；此处，需要说明的是，在当前车辆所在位置处以及车辆目标泊车点的附近采用高精度的规则集，而在其他地方采用较低精度的规则集。这种方法可以发挥高精度规则集的优势，即囊括尽可能多的可行路径，且无需过多的计算和存储开销。

使低精度规则集al成为高精度规则集ah的一个真子集，即确保低精度决策空间中的路径被充分利用，且能有效减少图搜索过程中的分支数量，并且可使车辆在不同的区域有不同的路径决策空间，具有很好的灵活性和时效性。

进一步地，构建泊车路径规划启发函数目的在于将搜索往有利的方向上引导，从而提高搜索效率。启发函数的选择恰当与否直接关系到搜索性能的好坏，本实施例在设计启发函数时引入距离和角度两个因素。启发函数构建过程如下：

首先，将车辆当前位置到终点的距离和车辆当前朝向与目标朝向之间的角度差分别进行归一化处理，以消除两者量纲不统一造成的问题：

l′＝l/lt

α′＝α/360

其中，lt表示车辆起点到终点的欧氏距离，l表示车辆当前位置到终点的欧氏距离，l′表示归一化后的距离，α表示车辆当前朝向与目标朝向之间的角度差，且α的单位为度，α′表示归一化后的角度差；

然后，基于归一化的距离和角度差，定义启发函数：

h^*(n)＝w1·l′+w2·α′

其中，w1为距离权值，w2为角度权值，可将两者的数值均设为0.5左右，且w1和w2相加为1；

此处，需要说明的是，归一化后仅考虑距离和角度的贡献度，而非距离和角度的具体数值，因此避免了两者量纲不统一带来的问题。

最后，构建状态节点的估计函数f^*(n)：

f^*(n)＝g^*(n)+h^*(n)

其中，g^*(n)为起始点到当前点路径的实际代价值，可用车辆实际已走过的路径长度及车身转过角度的加权和表示，当然，此处也需对车辆实际已走过的路径长度及车身转过角度进行归一化处理。

进一步地，如图5所示，所述基于构建的启发函数和估计函数，根据所述状态转换规则集，进行基于a^*算法的启发式自动泊车路径搜索，为车辆找出符合预设要求的自动泊车路径，包括：

首先，为了提升路径规划的实时性，对启发函数进行膨胀，即给启发函数h乘上一个大于1的膨胀因子ε，相应地，将估计函数修正为：f^*(n)＝g^*(n)+ε·h^*(n)；

此处，需要说明的是，启发信息的膨胀可使算法搜索速度明显提高。然而，搜索速度的提升是以牺牲路径的最优性为代价的，我们得到的路径可能是次优的。a^*算法还有另一个重要特性，即假如启发函数在整个搜索过程中保持不变，那么启发函数膨胀ε倍之后，生成的路径代价也绝不会超过最优路径代价的ε倍。若路径搜索时间充裕，我们可逐步减小膨胀因子ε的值，从而将生成的次优路径不断优化，若膨胀因子ε减小至1，此时生成的路径为最优。

然后，构建open表和closed表；其中，open表用来存储待扩展节点，closed表用来存储已扩展节点；open表中的每一个节点n都有一个估价值g(start,n)+ε·h(n,goal)，其中，g(start,n)为车辆初始位置到车辆当前位置的实际路径代价值，h(n,goal)为启发函数，为当前点到目标点的路径估计代价；ε为大于1的膨胀因子；

路径搜索过程中，每次均选取open表中估价值最小的节点，判断当前选取的节点是否为目标节点，若是，则最优路径搜索过程结束，若不是，则按照所述状态转换规则集对当前选取的节点进行扩展，并将扩展出的且不属于预设禁止区域的新节点放入open表，同时设置这些新节点至其父节点的指针，并将父节点放入closed表，迭代这一过程，直至最优路径搜索过程结束；

在最优路径搜索过程结束后，根据所设的父节点指针从目标节点追溯到起始节点，形成最终的泊车路径，并输出最终的泊车路径。

图6展示了在垂直泊车和水平泊车两种场景下的自动泊车路径规划效果。从图6中可以看出，本实施例的无人车启发式自动泊车算法能针对车辆任意初始位姿及边界约束条件，准确快速地规划出接近最优的泊车路径。

综上，本实施例通过构建车辆位姿状态集；构建多分辨率的状态转换规则集；构建用于泊车路径搜索的启发函数和估计函数；基于构建的启发函数和估计函数，根据状态转换规则集，进行基于a^*算法的启发式自动泊车路径搜索，为车辆找出符合预设要求的自动泊车路径。可以有效地改善汽车的操控性，显著减少泊车过程中的不便利，消除安全隐患，迅速、准确、安全地将汽车停靠到合适的位置，并且适合车辆任意初始位姿及边界约束条件下的自动泊车场景。

第二实施例

本实施例提供了一种无人车启发式自动泊车装置的具体实施方式，由于本实施例提供的无人车启发式自动泊车装置与第一实施例的无人车启发式自动泊车方法的具体实施方式相对应，该无人车启发式自动泊车装置可通过执行上述方法具体实施方式中的流程步骤来实现本发明的目的，因此上述无人车启发式自动泊车方法具体实施方式中的解释说明，也适用于本发明提供的无人车启发式自动泊车装置的具体实施方式，在本发明以下的具体实施方式中将不再赘述。

如图7所示，该本实施例的无人车启发式自动泊车装置包括以下模块：

状态集构建模块11，用于获取车辆位姿状态，构建并存储车辆位姿状态集；

规则集构建模块12，用于构建并存储多分辨率的状态转换规则集；其中，所述状态转换规则集为从任意位姿状态发散出来的、方向不同的、可以连接相邻两个不同位姿状态的所有分支集合；所述状态转换规则集包括在车辆起始位置和车辆目标泊车点预设范围内使用的第一规则集，以及在除车辆起始位置及车辆目标泊车点预设范围外的其它位置使用的第二规则集，所述第一规则集的精度大于所述第二规则集的精度；

函数构建模块13，用于构建并存储用于泊车路径搜索的启发函数和估计函数；

路径规划模块14，用于基于所述函数构建模块13所构建的启发函数和估计函数，以及所述规则集构建模块12所构建的状态转换规则集，进行基于a^*算法的启发式自动泊车路径搜索，为车辆找出符合预设要求的自动泊车路径。

本实施例的无人车启发式自动泊车装置可以获取车辆初始位姿状态，包括车辆中心坐标(x,y)及车辆朝向角度θ；并存储自动泊车过程中涉及到的车辆位姿状态，主要包括分别存储待扩展状态和已扩展状态的open表和closed表；通过构建多分辨率的状态转换规则集并进行存储；构建泊车路径搜索的启发函数和估价函数；并在此基础上进行基于a^*算法的启发式自动泊车路径搜索，即为open表中的所有状态节点计算估价函数值，每次取出估价函数值最小的状态，若为终点，路径规划结束，否则驱动车辆至该状态，并根据状态转移规则从该状态扩展出一系列新状态，将不属于禁止区域的新状态放至open表，并创建新状态指向其父节点的指针，最后将该父节点放至closed表，迭代这一过程，直至路径规划结束。从而可以在车辆任意初始位姿状态、目标位姿状态及任意边界约束条件下，均能快速准确地规划出接近最优的泊车路径。

第三实施例

本实施例提供一种电子设备，其包括处理器和存储器；其中，存储器中存储有至少一条指令，所述指令由处理器加载并执行，以实现第一实施例的方法。

该电子设备可因配置或性能不同而产生比较大的差异，可以包括一个或一个以上处理器(centralprocessingunits，cpu)和一个或一个以上的存储器，其中，存储器中存储有至少一条指令，所述指令由处理器加载并执行上述方法。

第四实施例

本实施例提供一种计算机可读存储介质，该存储介质中存储有至少一条指令，所述指令由处理器加载并执行，以实现上述第一实施例的方法。其中，该计算机可读存储介质可以是rom、随机存取存储器、cd-rom、磁带、软盘和光盘数据存储设备等。其内所存储的指令可以由终端中的处理器加载并执行。

此外，需要说明的是，本发明可提供为方法、装置或计算机程序产品。因此，本发明实施例可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本发明实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质上实施的计算机程序产品的形式。

本发明实施例是参照根据本发明实施例的方法、终端设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、嵌入式处理机或其他可编程数据处理终端设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理终端设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理终端设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理终端设备上，使得在计算机或其他可编程终端设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程终端设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的相同要素。

最后需要说明的是，以上所述是本发明优选实施方式，应当指出，尽管已描述了本发明优选实施例，但对于本技术领域的技术人员来说，一旦得知了本发明的基本创造性概念，在不脱离本发明所述原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明实施例范围的所有变更和修改。