CN111857148A - 一种非结构化道路车辆路径规划方法 - Google Patents

一种非结构化道路车辆路径规划方法 Download PDF

Info

Publication number
CN111857148A
CN111857148A CN202010736989.8A CN202010736989A CN111857148A CN 111857148 A CN111857148 A CN 111857148A CN 202010736989 A CN202010736989 A CN 202010736989A CN 111857148 A CN111857148 A CN 111857148A
Authority
CN
China
Prior art keywords
node
path
parking
angle
straight line
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010736989.8A
Other languages
English (en)
Other versions
CN111857148B (zh
Inventor
徐彪
杨帆
秦兆博
边有钢
胡满江
王晓伟
秦晓辉
谢国涛
秦洪懋
陈鑫鹏
丁荣军
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan University
Original Assignee
Hunan University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hunan University filed Critical Hunan University
Priority to CN202010736989.8A priority Critical patent/CN111857148B/zh
Publication of CN111857148A publication Critical patent/CN111857148A/zh
Application granted granted Critical
Publication of CN111857148B publication Critical patent/CN111857148B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种非结构化道路车辆路径规划方法，包括如下步骤：步骤1，输入包含障碍物及道路边界的地图，设置起始构型Ns、目标构型Ng以及目标区域Ag；步骤2，设置初始open集仅包含初始构型Ns；步骤3，从open集中选出代价最小的节点；步骤4，判断节点Ni是否已经存在于open集中；步骤5，判断节点Ni是否搜索至目标区域Ag；步骤6，采用“直线‑圆弧‑直线”停车模型生成精确到达目标构型的停车路径；步骤7，判断所述步骤6生成的各停车路径是否与障碍物发生碰撞；步骤8，路径回溯。本发明的非结构化道路车辆路径规划方法，通过步骤1至步骤8的设置，便可有效的实现车辆路径的规划了。

Description

一种非结构化道路车辆路径规划方法
技术领域
本发明涉及自动驾驶领域，尤其是一种非结构化道路车辆路径规划方法。
背景技术
近年来随着人工智能技术的发展和汽车工业的不断进步，智能车辆技术成为了学术界和工业界研究的热点。路径规划作为汽车无人驾驶技术的研究重点之一，其任务是根据环境感知模块接收到的实时环境信息以及数字地图提供的静态环境信息规划出一条从起始点至目标点的安全、可行驶路径。目前，针对智能车辆的路径规划技术可做以下分类：基于图搜索的方法、基于采样的方法、基于插值曲线的方法以及基于数值优化的方法等，每种方法都各有其优缺点及适用场景。其中，非结构化道路是智能车辆面对的主要场景之一，因此非结构化道路的路径规划方法及系统是汽车无人驾驶技术的重要研究内容。
目前针对非结构化道路场景下的路径规划方法主要是采用基于图搜索的方法和基于采样的方法来搜索得到起始点到目标点的全局路径。其中，基于图搜索的混合A*算法由于其具有较快的搜索速度、考虑车辆运动学约束以及生成的路径具有最优性(近似最优)等特性，是智能车辆路径规划方法中应用最广泛的方法。目前对于混合A*算法的研究和改进有以下几个方面：(1)改进A*算法中的启发函数，如专利文件CN 109443364 A提出的方法中，通过在启发函数中加入安全代价估计，使得规划出的路径远离障碍物，保证所规划路径的安全性；专利文件CN 110333659 A提出的方法中，通过引入引导线来修正A*的启发函数，使其更符合人类驾驶员的驾驶习惯。(2)考虑车辆实际运动学模型，如专利文件CN108444488 A提出的基于等步采样的路径规划方法中，通过考虑车辆实际运动学模型，得到带有转向约束的A*算法；专利文件CN 110207716 A中也用到混合A*算法生成参考行驶线，通过使用车辆动力学约束来计算A*算法的可扩展区域来生成光滑的掉头路径。此外还有基于双向搜索、基于变步长等改进型的A*路径搜索算法。
以上针对混合A*算法提出的各改进方法虽然在一定程度上能改善路径的搜索效率以及提高所规划路径在车辆模型上的适用性，但作为图搜索算法的一种典型代表，其仍然存在固有的缺点，即搜索得到的路径只能到达目标构型附近而始终无法精确地到达指定的目标位置和方向。其次，各方法为了保证搜索路径的距离最短，生成的路径往往贴近障碍物或者道路边缘，未考虑路径的光滑性。
发明内容
针对现有技术存在的不足，本发明的目的在于提供一种非结构化道路车辆路径规划方法，采用改进启发函数的混合A*算法与“直线-圆弧-直线”停车模型相结合的路径规划方法来克服或减轻现有技术的上述缺陷。
为实现上述目的，本发明提供了如下技术方案：一种非结构化道路车辆路径规划方法，其特征在于：包括如下步骤：
步骤1，输入包含障碍物及道路边界的地图，设置起始构型Ns、目标构型Ng以及目标区域Ag，设置搜索空间的栅格位置分辨率ξ、角度分辨率γ，所述起始构型Ns、目标构型Ng包含后轴中心位置(x,y)及横摆角信息
Figure BDA0002605442670000021
Figure BDA0002605442670000022
所述目标区域Ag是以目标构型Ng为中心，以距离r为半径的圆形区域；
步骤2，设置初始open集仅包含初始构型Ns，初始close集为空，所述初始构型Ns的启发值hs设置为1，代价值gs为0，Ns的总代价值fs＝hs+gs；
步骤3，从open集中选出代价最小的节点，将其置于close集中，并将该节点设为父节点Np，根据车辆运动学约束基于父节点生成离散子节点Ni的位置信息(xi,yi)以及横摆角度
Figure BDA0002605442670000031
采用改进型启发函数计算节点总代价值fi，所述fi＝α1·hi+α2·gi，其中启发值hi为改进的启发函数计算得到的归一化后的启发值，代价值gi为根据当前节点与起始节点之间路径累积距离及起始节点与目标节点之间的曼哈顿距离计算得到的归一化后的代价值，根据节点Ni的位置信息(xi,yi)、横摆角度
Figure BDA0002605442670000032
栅格位置分辨率ξ以及角度分辨率γ分别计算节点Ni所在的位置栅格
Figure BDA0002605442670000033
以及角度栅格
Figure BDA0002605442670000034
步骤4，判断节点Ni是否已经存在于open集中，如果是，则进行节点的代价比较与更新处理，否则将节点Ni加入open集中，并直接进行下一步骤，所述判断方法为：对比当前节点Ni与open集中所有节点所在的位置栅格、角度栅格，若存在节点Nτ与节点Ni的位置栅格以及角度栅格分别相等，则说明节点Ni已经存在open集中，所述节点的代价比较与更新方法为：比较节点Ni与节点Nτ的代价值g，当gi<gτ时，则将节点Nτ从open集中删除，并将节点Ni加入open集中，进行下一步骤；当gi>gτ时，则跳回步骤3；
步骤5，判断节点Ni是否搜索至目标区域Ag，如果是，则进行下一步骤，否则跳回步骤3；
步骤6，采用“直线-圆弧-直线”停车模型生成精确到达目标构型的停车路径；
步骤7，判断所述步骤6生成的各停车路径是否与障碍物发生碰撞，如果都与障碍物发生碰撞，则删除步骤6所生成的各停车路径，跳回步骤3继续拓展离散子节点，若存在不与障碍物发生碰撞的停车路径，则选择无碰撞的路径中转弯半径最大的“直线—圆弧—直线”停车路径作为停车路径，并进行下一步骤；步骤8，路径回溯，得到从起始构型精确到达目标构型的全局路径。
作为本发明的进一步改进，所述步骤3中，所述节点包含位置信息(x,y)及角度信息
Figure BDA0002605442670000041
考虑到车辆的前轮转角约束θmax，将车辆的前轮转角范围(-θmax,θmax)以固定的角度差Δθ＝(θmax-(-θmax))/2n离散为2n等份，根据离散的前轮转角生成2k+1个离散子节点Ni：
Figure BDA0002605442670000042
其中k＝-n,-n+1,…,-1,0,1,…,n-1,n；
Figure BDA0002605442670000043
为各离散子节点的父节点，一个父节点最多可拓展生成2k+1个离散子节点
Figure BDA0002605442670000044
l表示为进行节点拓展的步长；L表示为车辆的轴距。
作为本发明的进一步改进，所述步骤3中，混合A*算法代价函数的具体计算公式如下：
Figure BDA0002605442670000045
其中，fi表示子节点Ni的总代价值，α1、α2为正的权重系数；gi表示子节点Ni到起始构型的代价函数，da为节点Ni与起始构型之间的实际累积距离代价，dm为起始构型与目标构型之间的曼哈顿预估距离代价；hi表示改进后的启发函数计算公式，定义为各启发式的累加和，w1、w2、w3为各启发式的权重系数。
作为本发明的进一步改进，所述
Figure BDA0002605442670000046
为节点Ni关于横摆角变化率的启发式，计算公式为：
Figure BDA0002605442670000047
其中
Figure BDA0002605442670000048
为节点Ni的横摆角，
Figure BDA0002605442670000049
为其父节点Np对应的横摆角，
Figure BDA00026054426700000410
Figure BDA00026054426700000411
为当车辆以最大前轮转角θmax生成子节点时，子节点与父节点之间横摆角的最大差值。
Figure BDA0002605442670000051
为节点Ni关于路径搜索方向的启发式，计算公式为：
Figure BDA0002605442670000052
其中启发式
Figure BDA0002605442670000053
具体定义为：当前节点Ni、目标构型Ng之间连线的角度β与当前节点的横摆角方向
Figure BDA0002605442670000054
之间的角度差值，其中β＝tan-1((yg-yi)/(xg-xi))。
Figure BDA0002605442670000055
为节点Ni关于与目标构型Ng之间相对曼哈顿距离的启发式，计算公式为：
Figure BDA0002605442670000056
通过2、3、4、5公式计算得到各离散子节点的代价值fi。
作为本发明的进一步改进，所述步骤6生成精确到达目标构型的“直线—圆弧—直线”停车路径具体包括：
步骤6.1，以所述步骤5中搜索至目标区域Ag的节点为停车起始点
Figure BDA0002605442670000057
Figure BDA0002605442670000058
根据目标构型Ng的位置和角度将坐标系旋转、平移至以目标构型的位置为坐标原点且目标构型的方向为x轴正方向的新坐标系下，得到新的目标构型为N′(g)＝(0,0,0)、停车起始点为
Figure BDA0002605442670000059
步骤6.2，在步骤5.1旋转、平移后的新坐标系下，根据车辆最大前轮转角θmax求得与目标构型N′g＝(0,0,0)相切的上、下两个以车辆最小转弯半径rmin为半径的圆，分别为圆1和圆2；
步骤6.3，根据新坐标系下停车起始点N′a的角度
Figure BDA00026054426700000510
以及最小转弯半径圆生成停车路径可拟合区域S(x,y)，所述停车路径可拟合区域S(x,y)为过切点N′t且角度为
Figure BDA00026054426700000511
的切线与过切点的水平线之间所夹的区域；
步骤6.4，判断停车起始点
Figure BDA00026054426700000512
是否位于所述步骤6.3求得的停车路径可拟合区域内，若是，则生成“直线—圆弧—直线”停车路径，反之则返回步骤3继续拓展离散子节点，直到生成的子节点满足位于其所对应的停车路径可拟合区域内，则生成“直线—圆弧—直线”停车路径。
作为本发明的进一步改进，所述步骤6.3中停车路径可拟合区域S(x,y)的生成步骤如下：
步骤6.31，根据停车起始点角度
Figure BDA00026054426700000513
确定所对应相切的最小转弯半径圆，根据角度
Figure BDA0002605442670000061
的大小以及对应的最小转弯半径圆求得最小转弯半径圆上的切点
Figure BDA0002605442670000062
步骤6.32，根据步骤(1)中求得的切点N′t以及停车起始点角度
Figure BDA0002605442670000063
计算停车路径可拟合区域S(x,y)，根据
Figure BDA0002605442670000064
的大小可分为以下四种情况：
情况一：
Figure BDA0002605442670000065
时，停车路径可拟合区域S(x,y)解析式表示为：
Figure BDA0002605442670000066
情况二：
Figure BDA0002605442670000067
时，停车路径可拟合区域S(x,y)，解析式表示为：
Figure BDA0002605442670000068
情况三：
Figure BDA0002605442670000069
时，停车路径可拟合区域S(x,y)解析式表示为：
Figure BDA00026054426700000610
情况四：
Figure BDA00026054426700000611
时，停车路径可拟合区域S(x,y)解析式表示为：
Figure BDA00026054426700000612
作为本发明的进一步改进，所述步骤6.4中生成停车路径的具体步骤如下：
步骤6.41，根据停车起始点N′a和目标构型N′g，计算同时与x轴、与过停车起始点且角度为
Figure BDA00026054426700000613
的直线相切的转弯圆；
步骤6.42，所述步骤6.41中转弯圆的半径范围为[rmin,rmax]，其中rmin为车辆最小转弯半径，rmax为当所述步骤6.41中的转弯圆增大至与停车起始点或目标构型相切时对应的转弯圆半径大小，通过将转弯圆的半径范围[rmin,rmax]离散成k-1等份，得到k个不同半径、同时与x轴以及与过停车起始点且角度为
Figure BDA00026054426700000614
的直线相切的转弯圆弧；
步骤6.43，通过计算过起点、终点的直线与步骤(2)中得到的k个转弯圆的切点，从而进一步生成k条“直线—圆弧—直线”停车路径，具体生成方式为：转弯圆上两切点之间的最短圆弧即为停车路径中的圆弧路径，停车起始点、目标构型与各自切点连成的直线段即为停车路径的直线路径。将所述直线路径与圆弧路径拼接得到停车起始点精确到达目标构型的“直线—圆弧—直线”停车路径；
步骤6.44，将步骤6.43中生成的“直线—圆弧—直线”停车路径通过坐标轴旋转、平移还原至原坐标系下。
本发明的有益效果，本发明提出的改进启发函数的混合A*算法，通过在启发函数中加入考虑相邻节点横摆角变化率的启发式，可以有效避免相邻路径点之间出现较大的角度变化，提高所搜索路径的光滑性；通过在启发函数中加入考虑搜索方向的启发式，使得算法能更倾向于朝着目标构型方向进行搜索，能有效地减少节点拓展的数量，提高搜索的效率。本发明提出的“直线—圆弧—直线”停车模型，根据停车起始点与目标构型生成的停车路径可拟合区域能快速判断是否能生成满足车辆前轮最大转角约束的最短停车路径，并生成能精确到达目标构型的“直线—圆弧—直线”停车路径。本发明通过将改进启发函数的混合A*与“直线—圆弧—直线”停车模型相结合，可在非结构化道路下生成精确到达目标构型的全局路径。
附图说明
图1是本发明一种车辆全局路径规划方法的流程图；
图2是本发明使用的车辆运动学模型；
图3是本发明改进型启发函数的计算示意图；
图4是本发明“直线-圆弧-直线”停车路径可拟合区生成示意图；
图5是本发明“直线-圆弧-直线”停车路径生成示意图；
图6是本发明精确到达目标点的全局路径效果图。
具体实施方式
下面将结合附图所给出的实施例对本发明做进一步的详述。
如图1所示，为本实施列所提供的车辆全局路径规划方法的主要步骤，具体步骤如下：
步骤1，输入包含障碍物及道路边界的地图，设置起始构型、目标构型以及目标区域，设置搜索空间的栅格位置分辨率ξ、角度分辨率γ。所述起始构型Ns、目标构型Ng包含后轴中心位置(x,y)及横摆角信息
Figure BDA0002605442670000081
Figure BDA0002605442670000082
所述目标区域Ag是以目标构型Ng为中心，以距离r为半径的圆形区域；
步骤2，设置初始open集仅包含初始构型Ns，初始close集为空。所述初始构型Ns的启发值hs设置为1，代价值gs为0，Ns的总代价值fs＝hs+gs；
步骤3，从open集中选出代价最小的节点，将其置于close集中，并将该节点设为父节点Np。根据车辆运动学约束基于父节点生成离散子节点Ni的位置信息(xi,yi)以及横摆角度
Figure BDA0002605442670000083
采用改进型启发函数计算节点总代价值fi。所述fi＝α1·hi+α2·gi，其中启发值hi为改进的启发函数计算得到的归一化后的启发值，代价值gi为根据当前节点与起始节点之间路径累积距离及起始节点与目标节点之间的曼哈顿距离计算得到的归一化后的代价值。根据节点Ni的位置信息(xi,yi)、横摆角度
Figure BDA0002605442670000084
栅格位置分辨率ξ以及角度分辨率γ分别计算节点Ni所在的位置栅格
Figure BDA0002605442670000085
以及角度栅格
Figure BDA0002605442670000086
步骤4，判断节点Ni是否已经存在于open集中，如果是，则进行节点的代价比较与更新处理，否则将节点Ni加入open集中，并直接进行步骤5。所述判断方法为：对比当前节点Ni与open集中所有节点所在的位置栅格、角度栅格，若存在节点Nτ与节点Ni的位置栅格以及角度栅格分别相等，则说明节点Ni已经存在open集中。所述节点的代价比较与更新方法为：比较节点Ni与节点Nτ的代价值g，当gi<gτ时，则将节点Nτ从open集中删除，并将节点Ni加入open集中，进行步骤5；当gi>gτ时，则跳回步骤3。
步骤5，判断节点Ni是否搜索至目标区域Ag，如果是，则进行步骤6，否则跳回步骤3；
步骤6，采用“直线-圆弧-直线”停车模型生成精确到达目标构型的停车路径；
步骤7，判断所述步骤6生成的各停车路径是否与障碍物发生碰撞，如果都与障碍物发生碰撞，则删除步骤6所生成的各停车路径，跳回步骤3继续拓展离散子节点，若存在不与障碍物发生碰撞的停车路径，则选择无碰撞的路径中转弯半径最大的“直线—圆弧—直线”停车路径作为停车路径，并进行步骤8；
步骤8，路径回溯，得到从起始构型精确到达目标构型的全局路径。
结合附图2，本实施例中，步骤3满足车辆运动学约束的离散子节点生成过程为：
步骤3.1，考虑车辆的前轮最大转角θmax的约束，将车辆的转角范围(-θmax,θmax)以固定的角度差Δθ＝(θmax-(-θmax))/2k离散为2k等份，得到2k+1个等分的离散转向角；
步骤3.2，分别以2k+1个等分的离散转向角为控制量分别代入如下的车辆运动学微分约束公式中，生成2k+1个离散子节点
Figure BDA0002605442670000091
Figure BDA0002605442670000092
其中k＝-N,-N+1,…,-1,0,1,…,N-1,N；
Figure BDA0002605442670000093
为各离散子节点的父节点，一个父节点拓展生成2k+1个离散子节点
Figure BDA0002605442670000094
l表示为进行节点拓展的步长；L表示为车辆的轴距。
结合附图3，本实施例中，步骤3中改进型启发函数hi的具体计算包含以下步骤：
步骤3.3，计算节点Ni关于子、父节点之间横摆角变化率的启发式
Figure BDA0002605442670000095
Figure BDA0002605442670000096
其中
Figure BDA0002605442670000097
为节点Ni的横摆角，
Figure BDA0002605442670000098
为其父节点对应的横摆角，
Figure BDA0002605442670000099
Figure BDA00026054426700000910
为当车辆以前轮最大转角生成子节点时，子节点与父节点之间横摆角的最大变化量。通过启发式
Figure BDA0002605442670000101
引导混合A*在搜索过程中优先的选择角度变化小的节点，因而提高所搜索路径的光滑性。
步骤3.4，计算节点Ni关于路径搜索方向的启发式
Figure BDA0002605442670000102
Figure BDA0002605442670000103
其中启发式
Figure BDA0002605442670000104
具体定义为：当前节点Ni、目标构型Ng之间的连线方向β与当前节点的横摆角方向
Figure BDA0002605442670000105
之间的角度差值，其中β＝tan-1((yg-yi)/(xg-xi))。通过启发式h2i引导混合A*在搜索的过程中优先的朝目标位置方向进行搜索，避免算法在不必要的方向进行探索。
步骤3.5，计算节点Ni关于与目标构型之间相对曼哈顿距离的启发式
Figure BDA0002605442670000106
Figure BDA0002605442670000107
其中(xi,yi)为节点Ni的坐标；(xs,ys)为起始构型的坐标；(xg,yg)为目标构型的坐标。启发式
Figure BDA0002605442670000108
能有效引导算法搜索至目标点附近。
步骤3.6，通过公式(3)、(4)、(5)计算得到改进的启发函数hi：
hi＝(w1·h1i+w2·h2i+w3·h3i)/3 (2)
其中w1、w2、w3为各启发式的权重系数。
步骤3.7，节点Ni的代价函数为：
fi＝α1*hi+α2*gi
其中fi表示子节点Ni的总代价值，α1、α2为两个正的权重系数；gi表示子节点Ni与起始构型之间的实际距离代价函数；hi表示子节点Ni的启发函数。由改进启发函数的混合A*算法引导路径朝目标区域Ag搜索。
步骤5，判断节点
Figure BDA0002605442670000109
是否搜索至目标区域Ag，具体判断方式为：计算节点Ni与目标构型Ng之间的欧式距离d，当距离d小于目标区域Ag的半径r时，则已经搜索至目标区域。
如图4所示，在本实施列中，步骤6“直线—圆弧—直线”停车路径的生成包括以下步骤：
步骤6.1，以所述步骤5中搜索至目标区域Ag的节点为停车起始点
Figure BDA0002605442670000111
根据目标构型Ng的位置和角度将坐标系旋转、平移至以目标构型的位置为坐标原点且目标构型的方向为x轴正方向的新坐标系下，得到新的目标构型为N′g＝(0,0,0)、停车起始点为
Figure BDA0002605442670000112
步骤6.2，在步骤6.1旋转、平移后的新坐标系下，根据车辆最大前轮转角θmax求得与目标构型N′g＝(0,0,0)相切的上、下两个以车辆最小转弯半径rmin为半径的圆(圆1、圆2)；
步骤6.3，根据新坐标系下的停车起始点N′a的角度
Figure BDA0002605442670000113
以及最小转弯半径圆生成停车路径可拟合区域S(x,y)。
优选地，步骤6.3具体包括：
情况一：停车起始点角度
Figure BDA0002605442670000114
对应的相切的最小转弯圆为圆2。根据角度
Figure BDA0002605442670000115
的大小以及对应的最小转弯半径圆求得最小转弯半径圆上的切点N′(t)＝(x′t,y′t,θ′t)；根据切点N′(t)以及停车起始点角度
Figure BDA0002605442670000116
计算停车路径可拟合区域，所述停车路径可拟合区域S(x,y)为过切点N′t且角度为
Figure BDA0002605442670000117
的切线与过切点的水平线之间所夹的区域，解析式表示为：
Figure BDA0002605442670000118
情况二：停车起始点角度
Figure BDA0002605442670000119
对应的相切的最小转弯圆为圆2。根据角度
Figure BDA00026054426700001110
的大小以及对应的最小转弯半径圆求得最小转弯半径圆上的切点N′t＝(x′t,y′t,θ′t)；根据切点N′t以及停车起始点角度
Figure BDA00026054426700001111
计算停车路径可拟合区域S(x,y)，解析式表示为：
Figure BDA00026054426700001112
情况三：停车起始点角度
Figure BDA00026054426700001113
对应的相切的最小转弯圆为圆1。根据角度
Figure BDA00026054426700001114
的大小以及对应的最小转弯半径圆求得最小转弯半径圆上的切点N′(t)＝(x′t,y′t,θ′t)；根据切点N′t以及停车起始点角度
Figure BDA00026054426700001115
计算停车路径可拟合区域S(x,y)，解析式表示为：
Figure BDA0002605442670000121
情况四：停车起始点角度
Figure BDA0002605442670000122
对应的相切的最小转弯圆为圆1。根据角度
Figure BDA0002605442670000123
的大小以及对应的最小转弯半径圆求得最小转弯半径圆上的切点N′(t)＝(x′t,y′t,θ′t)；根据切点N′t以及停车起始点角度
Figure BDA0002605442670000124
计算停车路径可拟合区域S(x,y)，解析式表示为：
Figure BDA0002605442670000125
步骤6.4，判断停车起始点
Figure BDA0002605442670000126
是否位于所述步骤6.3求得的停车路径可拟合区域S(x,y)内，若是，则生成“直线—圆弧—直线”停车路径，反之则返回步骤3继续拓展离散子节点，直到生成的子节点满足位于其所对应的停车路径可拟合区域内，则生成“直线—圆弧—直线”停车路径。结合图5，所述停车路径生成具体方式如下：
(1)根据停车起始点N′a和目标构型N′g计算同时与x轴、过停车起始点且方向角为
Figure BDA0002605442670000127
的直线相切的转弯圆，切点分别为
Figure BDA0002605442670000128
Figure BDA0002605442670000129
(2)所述(1)中转弯圆的半径范围为[rmin,rmax]，其中rmin为车辆最小转弯半径，rmax为当所述(1)中的转弯圆增大至与停车起始点或目标构型相切时对应的最大转弯圆半径大小，通过将转弯圆的半径范围[rmin,rmax]离散成k-1等份，得到k个不同半径、同时与x轴以及与过停车起始点且角度为
Figure BDA00026054426700001210
的直线相切的转弯圆弧；
需要说明的是，最大转弯半径圆与停车起始点还是目标构型相切，决定于停车起始点N′a与切点
Figure BDA00026054426700001211
的直线距离、目标构型N′g与切点
Figure BDA00026054426700001212
的直线距离的相对长度大小，最大转弯半径圆与长度小者相切。
(3)通过计算过起点、终点的直线与步骤(2)中得到的k个转弯圆的切点，从而进一步生成k条“直线—圆弧—直线”停车路径，具体生成方式为：转弯圆上两切点
Figure BDA0002605442670000131
之间的最短圆弧即为停车路径中的转弯圆弧路径，停车起始点、目标构型与各自切点连成的直线段即为停车路径的直线路径。将所述直线路径与圆弧路径拼接得到停车起始点精确到达目标构型的“直线—圆弧—直线”停车路径；
(4)将步骤(3)中生成的“直线—圆弧—直线”停车路径通过坐标轴旋转、平移还原至原坐标系下。
如图6所示，为精确到达目标点的全局路径效果图，图中的1表示起始构型，2表示为目标构型，3表示为Hybrid A*搜索得到的起始构型至目标构型附近的全局路径，4表示为精确到达目标点的“S-C-S”停车路径，5表示为道路边界，6表示为障碍物。
综上所述，上述各实施例的方法能够有效改善混合A*搜索得到的路径的光滑性，提高路径搜索的效率。另外，由于在最终停车阶段采用“直线—圆弧—直线”停车模型，可以生成精确到达目标点的全局路径。
以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (7)

1.一种非结构化道路车辆路径规划方法，其特征在于：包括如下步骤：
步骤1，输入包含障碍物及道路边界的地图，设置起始构型Ns、目标构型Ng以及目标区域Ag，设置搜索空间的栅格位置分辨率ξ、角度分辨率γ，所述起始构型Ns、目标构型Ng包含后轴中心位置(x,y)及横摆角信息
Figure FDA0002605442660000013
Figure FDA0002605442660000014
所述目标区域Ag是以目标构型Ng为中心，以距离r为半径的圆形区域；
步骤2，设置初始open集仅包含初始构型Ns，初始close集为空，所述初始构型Ns的启发值hs设置为1，代价值gs为0，Ns的总代价值fs＝hs+gs；
步骤3，从open集中选出代价最小的节点，将其置于close集中，并将该节点设为父节点Np，根据车辆运动学约束基于父节点生成离散子节点Ni的位置信息(xi,yi)以及横摆角度
Figure FDA0002605442660000015
采用改进型启发函数计算节点总代价值fi，所述fi＝α1·hi+α2·gi，其中启发值hi为改进的启发函数计算得到的归一化后的启发值，代价值gi为根据当前节点与起始节点之间路径累积距离及起始节点与目标节点之间的曼哈顿距离计算得到的归一化后的代价值，根据节点Ni的位置信息(xi,yi)、横摆角度
Figure FDA0002605442660000016
栅格位置分辨率ξ以及角度分辨率γ分别计算节点Ni所在的位置栅格
Figure FDA0002605442660000011
以及角度栅格
Figure FDA0002605442660000012
步骤4，判断节点Ni是否已经存在于open集中，如果是，则进行节点的代价比较与更新处理，否则将节点Ni加入open集中，并直接进行下一步骤，所述判断方法为：对比当前节点Ni与open集中所有节点所在的位置栅格、角度栅格，若存在节点Nτ与节点Ni的位置栅格以及角度栅格分别相等，则说明节点Ni已经存在open集中，所述节点的代价比较与更新方法为：比较节点Ni与节点Nτ的代价值g，当gi<gτ时，则将节点Nτ从open集中删除，并将节点Ni加入open集中，进行下一步骤；当gi>gτ时，则跳回步骤3；
步骤5，判断节点Ni是否搜索至目标区域Ag，如果是，则进行下一步骤，否则跳回步骤3；
步骤6，采用“直线-圆弧-直线”停车模型生成精确到达目标构型的停车路径；
步骤7，判断所述步骤6生成的各停车路径是否与障碍物发生碰撞，如果都与障碍物发生碰撞，则删除步骤6所生成的各停车路径，跳回步骤3继续拓展离散子节点，若存在不与障碍物发生碰撞的停车路径，则选择无碰撞的路径中转弯半径最大的“直线—圆弧—直线”停车路径作为停车路径，并进行下一步骤；
步骤8，路径回溯，得到从起始构型精确到达目标构型的全局路径。
2.根据权利要求1所述的非结构化道路车辆路径规划方法，其特征在于：所述步骤3中，所述节点包含位置信息(x,y)及角度信息
Figure FDA0002605442660000021
考虑到车辆的前轮转角约束θmax，将车辆的前轮转角范围(-θmax,θmax)以固定的角度差Δθ＝(θmax-(-θmax))/2n离散为2n等份，根据离散的前轮转角生成2k+1个离散子节点Ni：
Figure FDA0002605442660000022
其中k＝-n,-n+1,…,-1,0,1,…,n-1,n；
Figure FDA0002605442660000023
为各离散子节点的父节点，一个父节点最多可拓展生成2k+1个离散子节点
Figure FDA0002605442660000024
l表示为进行节点拓展的步长；L表示为车辆的轴距。
3.根据权利要求2所述的非结构化道路车辆路径规划方法，其特征在于：所述步骤3中，混合A*算法代价函数的具体计算公式如下：
Figure FDA0002605442660000025
其中，fi表示子节点Ni的总代价值，α1、α2为正的权重系数；gi表示子节点Ni到起始构型的代价函数，da为节点Ni与起始构型之间的实际累积距离代价，dm为起始构型与目标构型之间的曼哈顿预估距离代价；hi表示改进后的启发函数计算公式，定义为各启发式的累加和，w1、w2、w3为各启发式的权重系数。
4.根据权利要求3所述的非结构化道路车辆路径规划方法，其特征在于：所述
Figure FDA0002605442660000026
为节点Ni关于横摆角变化率的启发式，计算公式为：
Figure FDA0002605442660000027
其中
Figure FDA0002605442660000028
为节点Ni的横摆角，
Figure FDA0002605442660000029
为其父节点Np对应的横摆角，
Figure FDA00026054426600000210
Figure FDA0002605442660000031
为当车辆以最大前轮转角θmax生成子节点时，子节点与父节点之间横摆角的最大差值。
Figure FDA0002605442660000032
为节点Ni关于路径搜索方向的启发式，计算公式为：
Figure FDA0002605442660000033
其中启发式
Figure FDA0002605442660000034
具体定义为：当前节点Ni、目标构型Ng之间连线的角度β与当前节点的横摆角方向
Figure FDA0002605442660000035
之间的角度差值，其中β＝tan-1((yg-yi)/(xg-xi))。
Figure FDA0002605442660000036
为节点Ni关于与目标构型Ng之间相对曼哈顿距离的启发式，计算公式为：
Figure FDA0002605442660000037
通过2、3、4、5公式计算得到各离散子节点的代价值fi。
5.根据权利要求1至4任意一项所述的非结构化道路车辆路径规划方法，其特征在于：所述步骤6生成精确到达目标构型的“直线—圆弧—直线”停车路径具体包括：
步骤6.1，以所述步骤5中搜索至目标区域Ag的节点为停车起始点
Figure FDA0002605442660000038
Figure FDA0002605442660000039
根据目标构型Ng的位置和角度将坐标系旋转、平移至以目标构型的位置为坐标原点且目标构型的方向为x轴正方向的新坐标系下，得到新的目标构型为N′(g)＝(0,0,0)、停车起始点为
Figure FDA00026054426600000310
步骤6.2，在步骤5.1旋转、平移后的新坐标系下，根据车辆最大前轮转角θmax求得与目标构型N′g＝(0,0,0)相切的上、下两个以车辆最小转弯半径rmin为半径的圆，分别为圆1和圆2；
步骤6.3，根据新坐标系下停车起始点N′a的角度
Figure FDA00026054426600000311
以及最小转弯半径圆生成停车路径可拟合区域S(x,y)，所述停车路径可拟合区域S(x,y)为过切点N′t且角度为
Figure FDA00026054426600000312
的切线与过切点的水平线之间所夹的区域；
步骤6.4，判断停车起始点
Figure FDA00026054426600000313
是否位于所述步骤6.3求得的停车路径可拟合区域内，若是，则生成“直线—圆弧—直线”停车路径，反之则返回步骤3继续拓展离散子节点，直到生成的子节点满足位于其所对应的停车路径可拟合区域内，则生成“直线—圆弧—直线”停车路径。
6.根据权利要求5所述的非结构化道路车辆路径规划方法，其特征在于：所述步骤6.3中停车路径可拟合区域S(x,y)的生成步骤如下：
步骤6.31，根据停车起始点角度
Figure FDA0002605442660000041
确定所对应相切的最小转弯半径圆，根据角度
Figure FDA0002605442660000042
的大小以及对应的最小转弯半径圆求得最小转弯半径圆上的切点N′t＝(x′t,y′t,θ′t)；
步骤6.32，根据步骤(1)中求得的切点N′t以及停车起始点角度
Figure FDA0002605442660000043
计算停车路径可拟合区域S(x,y)，根据
Figure FDA0002605442660000044
的大小可分为以下四种情况：
情况一：
Figure FDA0002605442660000045
时，停车路径可拟合区域S(x,y)解析式表示为：
Figure FDA0002605442660000046
情况二：
Figure FDA0002605442660000047
时，停车路径可拟合区域S(x,y)，解析式表示为：
Figure FDA0002605442660000048
情况三：
Figure FDA0002605442660000049
时，停车路径可拟合区域S(x,y)解析式表示为：
Figure FDA00026054426600000410
情况四：
Figure FDA00026054426600000411
时，停车路径可拟合区域S(x,y)解析式表示为：
Figure FDA00026054426600000412
7.根据权利要求6所述的非结构化道路车辆路径规划方法，其特征在于：所述步骤6.4中生成停车路径的具体步骤如下：
步骤6.41，根据停车起始点N′a和目标构型N′g，计算同时与x轴、与过停车起始点且角度为
Figure FDA00026054426600000413
的直线相切的转弯圆；
步骤6.42，所述步骤6.41中转弯圆的半径范围为[rmin,rmax]，其中rmin为车辆最小转弯半径，rmax为当所述步骤6.41中的转弯圆增大至与停车起始点或目标构型相切时对应的转弯圆半径大小，通过将转弯圆的半径范围[rmin,rmax]离散成k-1等份，得到k个不同半径、同时与x轴以及与过停车起始点且角度为
Figure FDA00026054426600000414
的直线相切的转弯圆弧；
步骤6.43，通过计算过起点、终点的直线与步骤(2)中得到的k个转弯圆的切点，从而进一步生成k条“直线—圆弧—直线”停车路径，具体生成方式为：转弯圆上两切点之间的最短圆弧即为停车路径中的圆弧路径，停车起始点、目标构型与各自切点连成的直线段即为停车路径的直线路径。将所述直线路径与圆弧路径拼接得到停车起始点精确到达目标构型的“直线—圆弧—直线”停车路径；
步骤6.44，将步骤6.43中生成的“直线—圆弧—直线”停车路径通过坐标轴旋转、平移还原至原坐标系下。
CN202010736989.8A 2020-07-28 2020-07-28 一种非结构化道路车辆路径规划方法 Active CN111857148B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010736989.8A CN111857148B (zh) 2020-07-28 2020-07-28 一种非结构化道路车辆路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010736989.8A CN111857148B (zh) 2020-07-28 2020-07-28 一种非结构化道路车辆路径规划方法

Publications (2)

Publication Number Publication Date
CN111857148A true CN111857148A (zh) 2020-10-30
CN111857148B CN111857148B (zh) 2022-04-29

Family

ID=72948469

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010736989.8A Active CN111857148B (zh) 2020-07-28 2020-07-28 一种非结构化道路车辆路径规划方法

Country Status (1)

Country Link
CN (1) CN111857148B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112612266A (zh) * 2020-12-04 2021-04-06 湖南大学 一种非结构化道路全局路径规划方法与系统
CN113119995A (zh) * 2021-03-11 2021-07-16 京东鲲鹏(江苏)科技有限公司 一种路径搜索方法及装置、设备、存储介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102963355A (zh) * 2012-11-01 2013-03-13 同济大学 一种智能辅助泊车方法及其实现系统
CN106945662A (zh) * 2017-03-30 2017-07-14 重庆大学 一种垂直自动泊车路径规划方法及系统
CN107672585A (zh) * 2017-08-29 2018-02-09 广州小鹏汽车科技有限公司 一种自动泊车路径规划方法及系统
CN108444488A (zh) * 2018-02-05 2018-08-24 天津大学 基于等步采样a*算法的无人驾驶局部路径规划方法
CN110440824A (zh) * 2019-08-27 2019-11-12 广州小鹏汽车科技有限公司 一种路径规划方法及路径规划系统
US20200149906A1 (en) * 2017-08-31 2020-05-14 Guangzhou Xiaopeng Motors Technology Co., Ltd. Path planning method, system and device for autonomous driving
CN111158366A (zh) * 2019-12-31 2020-05-15 湖南大学 基于图搜索和几何曲线融合的路径规划方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102963355A (zh) * 2012-11-01 2013-03-13 同济大学 一种智能辅助泊车方法及其实现系统
CN106945662A (zh) * 2017-03-30 2017-07-14 重庆大学 一种垂直自动泊车路径规划方法及系统
CN107672585A (zh) * 2017-08-29 2018-02-09 广州小鹏汽车科技有限公司 一种自动泊车路径规划方法及系统
US20200149906A1 (en) * 2017-08-31 2020-05-14 Guangzhou Xiaopeng Motors Technology Co., Ltd. Path planning method, system and device for autonomous driving
CN108444488A (zh) * 2018-02-05 2018-08-24 天津大学 基于等步采样a*算法的无人驾驶局部路径规划方法
CN110440824A (zh) * 2019-08-27 2019-11-12 广州小鹏汽车科技有限公司 一种路径规划方法及路径规划系统
CN111158366A (zh) * 2019-12-31 2020-05-15 湖南大学 基于图搜索和几何曲线融合的路径规划方法

Non-Patent Citations (9)

* Cited by examiner, † Cited by third party
Title
ALEXEJJANZ；JOHANNA SCHRAMM: "Model Based Path Optimization for Valet Parking with Trailer", 《IFAC-PAPERSONLINE》 *
JIE SONG;WEIWEI ZHANG;XUNCHENG WU;QIAOMING GAO: "Automatic Perpendicular Parking Trajectory Planning and Following for Vehicle", 《2019 3RD INTERNATIONAL CONFERENCE ON ELECTRONIC INFORMATION TECHNOLOGY AND COMPUTER ENGINEERING (EITCE)》 *
KANGBIN TU; SHUAISHUAI YANG; HAO ZHANG; ZHUPING WANG: "Hybrid A∗ Based Motion Planning for Autonomous Vehicles in Unstructured Environment", 《2019 IEEE INTERNATIONAL SYMPOSIUM ON CIRCUITS AND SYSTEMS (ISCAS)》 *
SAEID SEDIGHI; DUONG-VAN NGUYEN; KLAUS-DIETER KUHNERT: "Guided Hybrid A-star Path Planning Algorithm for Valet Parking Applications", 《2019 5TH INTERNATIONAL CONFERENCE ON CONTROL, AUTOMATION AND ROBOTICS (ICCAR)》 *
TSUNG-HUA HSU;JING-FU LIU;PEN-NING YU;WANG-SHUAN LEE;JIA-SING HS: "Development of an automatic parking system for vehicle", 《2008 IEEE VEHICLE POWER AND PROPULSION CONFERENCE》 *
ZHENJI LV; LINHUI ZHAO; ZHIYUAN LIU: "A Path-Planning Algorithm for Automatic Parallel Parking", 《2013 THIRD INTERNATIONAL CONFERENCE ON INSTRUMENTATION, MEASUREMENT, COMPUTER, COMMUNICATION AND CONTROL》 *
宋英博: "混合导引小车控制策略研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
毕野虹天: "自主代客泊车系统决策与规划方法研究", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 *
陈宇民: "特定场景下的智能车辆路径规划研究", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112612266A (zh) * 2020-12-04 2021-04-06 湖南大学 一种非结构化道路全局路径规划方法与系统
CN112612266B (zh) * 2020-12-04 2022-04-01 湖南大学 一种非结构化道路全局路径规划方法与系统
CN113119995A (zh) * 2021-03-11 2021-07-16 京东鲲鹏(江苏)科技有限公司 一种路径搜索方法及装置、设备、存储介质

Also Published As

Publication number Publication date
CN111857148B (zh) 2022-04-29

Similar Documents

Publication Publication Date Title
CN112964271B (zh) 一种面向多场景的自动驾驶规划方法及系统
CN107702716B (zh) 一种无人驾驶路径规划方法、系统和装置
CN110361013B (zh) 一种用于车辆模型的路径规划系统及方法
CN109799828B (zh) 自动驾驶车辆横纵向协调控制方法
CN112020014B (zh) 一种换道轨迹规划方法、装置、服务器及存储介质
CN111089594B (zh) 一种适用于多场景的自主泊车轨迹规划方法
CN113044029B (zh) 一种保证无人车在三维地形上安全行驶的运动规划方法
CN109764886A (zh) 一种路径规划方法
CN111806467A (zh) 一种基于车辆行驶规律的变速动态换道轨迹规划方法
CN111857148B (zh) 一种非结构化道路车辆路径规划方法
CN113448335A (zh) 路径规划方法和装置、车辆和可读存储介质
CN116185014A (zh) 一种基于动态规划的智能车全局最优轨迹规划方法与系统
CN111896004A (zh) 一种狭窄通道车辆轨迹规划方法及系统
CN113515111B (zh) 一种车辆避障路径规划方法及装置
CN114940163B (zh) 一种后轮转向车辆的横向运动控制方法、后轮转向车辆及电子系统
CN115077553A (zh) 基于栅格搜索轨迹规划方法、系统、汽车、设备及介质
CN114527761A (zh) 一种基于融合算法的智能汽车局部路径规划方法
CN113124875A (zh) 一种路径导航方法
CN114763133A (zh) 车辆泊车规划方法、装置、设备及计算机存储介质
CN114115268B (zh) 路径生成方法、装置、交通工具及存储介质
CN116009558A (zh) 一种结合运动学约束的移动机器人路径规划方法
CN116494976A (zh) 用于控制车辆变道的方法、装置及存储介质
CN115542899A (zh) 车辆路径跟踪的方法、装置、车辆、电子设备及介质
Daniel et al. Energy constrained trajectory generation for ADAS
CN114137972B (zh) 路径生成方法、装置、交通工具及存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant