CN110908386A - 一种无人车分层路径规划方法 - Google Patents

一种无人车分层路径规划方法 Download PDF

Info

Publication number
CN110908386A
CN110908386A CN201911262897.4A CN201911262897A CN110908386A CN 110908386 A CN110908386 A CN 110908386A CN 201911262897 A CN201911262897 A CN 201911262897A CN 110908386 A CN110908386 A CN 110908386A
Authority
CN
China
Prior art keywords
path
section
point
interval
passable
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
CN201911262897.4A
Other languages
English (en)
Other versions
CN110908386B (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.)
National Defense Technology Innovation Institute PLA Academy of Military Science
Original Assignee
National Defense Technology Innovation Institute PLA Academy of Military Science
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 National Defense Technology Innovation Institute PLA Academy of Military Science filed Critical National Defense Technology Innovation Institute PLA Academy of Military Science
Priority to CN201911262897.4A priority Critical patent/CN110908386B/zh
Publication of CN110908386A publication Critical patent/CN110908386A/zh
Application granted granted Critical
Publication of CN110908386B publication Critical patent/CN110908386B/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/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria
    • 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)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种无人车分层路径规划方法，包括如下步骤：(1)输入无人车位姿，引导路径，障碍图等参数；(2)根据引导路径和障碍图求解可通行截面区间搜索树；(3)在引导路径终点对应的可通行截面区间集合中确定目标可通行截面区间，在可通行截面区间搜索树上搜索到达目标可通行截面区间的最优通道；(4)以最优通道中可通行截面区间为偏移约束，以路径平滑性、安全性为优化目标，求解最优偏移量；(5)将引导路径各点按照对应最优偏移量进行横向偏移，得到规划路径。本发明可以避免在整个局部规划范围内进行细粒度搜索，降低计算量，同时利用最优通道约束最终规划路径的求解空间，提高规划结果的稳定性和时空一致性。

Description

一种无人车分层路径规划方法
技术领域
本发明属于无人车路径规划领域，具体涉及一种无人车分层路径规划方法。
背景技术
无人车在减少交通事故、节约能源、提升运输效率等方面有着巨大潜力，已经成为各国政府、科研机构的研究热点。无人车的路径规划是实现其自动驾驶的关键技术之一，其目的是要为无人车在所处环境中找到一条逐步通向目标位置的安全路径。
现有技术公开了一种用于无人驾驶汽车局部路径规划的装置及方法(申请号为201110007154.X)，该装置包括环境感知装置、斥力计算装置、引力计算装置、合力方向角度计算装置和方向盘转角计算装置，其通过环境感知装置探测障碍物，建立道路边界模型和道路中心线模型；斥力计算装置建立斥力点函数和计算斥力；引力计算装置建立引力点函数和计算引力；合力方向角度计算装置计算斥力和引力的合力的方向角度；方向盘转角计算装置根据合力的方向角度和转向系统传动比确定方向盘转角。
现有技术又公开了一种车辆运动规划方法(申请号为201510750059.7)，其根据行走路径更新经验区域的概率分布，在车辆运动过程中，根据更新后的经验区域的概率分布结合车辆状态转换的概率计算转移到相邻栅格的概率；根据转移到相邻栅格的概率预扩展随机树，并作碰撞检验，若通过碰撞检测，则用对应的栅格扩展随机树，并更新当前状态；直至尝试所有的栅格都无法通过碰撞检测或已到达目的栅格。
现有技术还公开了一种用于自主驾驶的车辆躲避移动障碍物的统一的运动规划(申请号为201510859205.X)，提供了用于碰撞避免目的的转向校正。该方法包括检测车辆沿其行驶的道路车道的车道中心并且确定将车辆从其当前位置引向车道中心的车道定中路径。该方法还包括检测车辆前方的移动物体并且确定如果车辆沿着车道定中路径以当前车辆速度行驶，在车辆和物体之间是否将要发生碰撞。该方法求解五阶多项式方程来限定从当前车辆位置到距物体一定安全距离的路点的碰撞避免路径以及车辆自动沿其转向的从该路点到车道中心的返回路径。
现有技术又公开了一种基于虚拟约束的自动驾驶车辆路径规划方法(申请号为201710161509.8)，包括以下步骤：根据车道线检测结果获取车体坐标系下的车道中心线航向；利用卡尔曼滤波器和获取的车道中心线航向对车辆的航向进行修正；利用修正后的车辆航向对原始路径中的路径点进行校正；基于三维激光雷达栅格地图，获取自动驾驶车辆与道路的相对横向位置关系；生成虚拟约束；根据自动驾驶系统的决策指令，生成期望路径。
上述的路径规划方法通常在给定的局部环境中进行直接求解，往往带来一些问题：一方面求解空间较大，容易导致较大的计算量，给系统运行造成较大的负担；另一方面，由于局部环境障碍分布未知，在局部环境的整体求解空间上进行路径规划，数学上无法保证是一个全局凸优化问题，其本身就可能存在多个极值，当环境感知结果发生扰动时，路径规划结果就可能在多个极值上跳转，导致规划结果不稳定，降低无人车行驶的安全性。
发明内容
本发明的目的是提供一种无人车分层路径规划方法，它能够克服现有技术的不足，并用于无人驾驶车辆的自主导航。
本发明的技术方案如下：一种无人车分层路径规划方法，它包括如下步骤：
步骤一：输入无人车位姿V，引导路径T、障碍图D、引导路径横向采样范围qmax，横向采样步长qstep，无人车安全半径dsafe。
所述无人车位姿V包括坐标、航向角度，记为V＝(xv，yv，θv)，xv，yv，θv分别为无人车的x坐标、y坐标和航向角；引导路径T为一平滑路径点序列，每个路径点的信息包括坐标、方位角，引导路径可表示为
Figure BDA0002309766790000031
Figure BDA0002309766790000032
分别为路径点i的横纵坐标、方位角，n为路径点个数；所述引导路径横向采样范围qmax表示引导路径左右两侧的最大采样范围；所述横向采样步长qstep为引导路径左右两侧进行采样的步长参数；所述无人车安全半径dsafe指当无人车所在位置与障碍物的距离大于dsafe时，无人车是安全的。
步骤二：结合障碍图D、引导路径横向采样范围qmax，横向采样步长qstep，无人车安全半径dsafe，求解引导路径T上各路径点
Figure BDA0002309766790000033
对应的可通行截面区间集合Si，所有路径点对应的可通行截面区间集合Si依次级联起来组成可通行截面区间搜索树Q。
步骤三：将无人车所处位置设为起始可通行截面区间，在引导路径终点对应的可通行截面区间集合中确定目标可通行截面区间O，在可通行截面区间搜索树Q上搜索到达目标可通行截面区间O的最优通道L。
步骤四：将规划路径视为引导路径上各点横向偏移后形成的路径，以引导路径上各点的横向偏移q＝[q0，...，qn-1]为优化变量，以最优通道L对应的可通行截面区间为各点偏移约束，以衡量路径平滑性、安全性的评价函数J为优化目标，求解最优偏移量
Figure BDA0002309766790000034
步骤五：将引导路径各点
Figure BDA0002309766790000035
按照对应最优偏移量
Figure BDA0002309766790000036
进行横向偏移，得到规划路径。
优选地，所述步骤一中障碍图为二值栅格地图，栅格点为0表示对应位置无障碍物，栅格点为1表示对应位置有障碍物。
进一步的，所述步骤二中引导路径T上各路径点
Figure BDA0002309766790000041
对应的可通行截面区间是指在该点处、垂直于引导路径的横截面上的可通行区间，可通行截面区间集合Si求解方式如下：
(1)在引导路径横向采样范围qmax内按照横向采样步长qstep均匀采样，计算路径点
Figure BDA0002309766790000042
左、右侧采样点个数
Figure BDA0002309766790000043
路径点
Figure BDA0002309766790000044
亦作为采样点，因此采样点合计点数为mi+1，其中
Figure BDA0002309766790000045
(2)计算左侧采样点：
Figure BDA0002309766790000046
计算右侧采样点：
Figure BDA0002309766790000047
左右采样点以及引导路径点统一表示为：
Figure BDA0002309766790000048
(3)初始化j＝0、当前考察的可通行采样点集合w为空；
(4)结合障碍图D判断采样点
Figure BDA0002309766790000049
的可通行性，若
Figure BDA00023097667900000410
可通行，则将
Figure BDA00023097667900000411
加入到w中；若
Figure BDA00023097667900000412
不可通行，则取当前可通行截面区间为[wmin，wmax]，其中wmin表示w中的最小值，wmax表示w中的最大值，[]表示实数区间集合，若w为空集，则当前可通行截面区间为空集，若wmin＝wmax，则当前可通行截面区间为{wmin)，将当前可通行截面区间加入Si，并将w清空；
(5)j＝j+1，若j＞mi则退出，否则转(4)。
更进一步的，结合所述障碍图D判断采样点
Figure BDA0002309766790000051
的可通行性的方法如下：(1)将障碍图进行距离变换，得到距离图，距离图中各点取值为该点到障碍的最近距离；(2)从上述距离图中查询采样点
Figure BDA0002309766790000052
到障碍的最近距离；(3)若采样点
Figure BDA0002309766790000053
到障碍的最近距离大于无人车安全半径dsafe，则
Figure BDA0002309766790000054
是可通行的，否则，
Figure BDA0002309766790000055
不可通行。
进一步的，所述步骤三中在引导路径终点对应的可通行截面区间集合中确定目标区间O的方法如下：(1)若引导路径终点是可通行的，则其所在的可通行截面区间为目标区间；(2)若引导路径终点不可通行，则选择离终点最近的可通行截面区间作为目标区间。
更进一步的，所述步骤三中在可通行截面区间搜索树Q上搜索达到目标区间O的最优通道L中采用的搜索方法为A*方法，方法如下：
(1)建立OPEN空表和CLOSE空表，分别用于存放待扩展截面区间节点和已扩展截面区间节点，将起始可通行截面区间加入到OPEN表中；
(2)若OPEN表不为空，则弹出OPEN表中第一个截面区间w；否则，搜索失败；
(3)判断截面区间w是否为目标截面区间，若是，则搜索结束，输出截面区间w及其所有父截面区间节点作为搜索结果；
(4)将截面区间w对应的引导路径点的下一个引导路径点对应的所有可通行截面区间作为w的拟扩展子区间节点；
(5)判断各拟扩展子区间节点是否已在CLOSE表中，若是，则舍弃该子区间节点；
(6)计算拟扩展子区间节点的代价函数F(i)，代价函数F(i)＝G(i)+H(i)，
其中G(i)表示从起始截面区间到截面区间i的代价，H(i)表示从截面区间节点i到目标截面区间代价的估计值；
(7)将拟扩展子区间节点加入到OPEN表中，并将OPEN表中所有截面区间节点按照对应代价F(i)从小到大排序，将当前区间节点加入CLOSE表中，转(2)。
优选的，所述G(i)和H(i)计算方法为：
G(i)＝a1G1(i)+a2G2(i)+a3G3(i)，H(i)＝b1H1(i)+b2H2(i)
其中G1(i)表示从起始截面区间节点到截面区间节点i各区间中点与对应引导路径点横向偏移之和，G2(i)表示从起始截面区间节点到截面区间节点i各区间中点与对应引导路径点横向偏移增量之和，G3(i)表示从起始截面区间节点到截面区间节点i沿引导路径的纵向路程，a1、a2、a3为加权参数；H1(i)表示截面区间节点i与目标截面区间中点与对应引导路径点横向偏移之差，H2(i)表示从截面区间节点i到目标截面区间沿引导路径的纵向路程，b1、b2为加权参数；
进一步的，所述步骤四中优化目标J的定义如下：J＝c1J1+c2J2
其中
Figure BDA0002309766790000061
Figure BDA0002309766790000062
为规划路径上点pi指向Pi+1的向量，pi的坐标
Figure BDA0002309766790000063
Figure BDA0002309766790000064
Figure BDA0002309766790000065
为最优通道L中引导路径点
Figure BDA0002309766790000066
对应的可通行截面区间；c1、c2为加权参数。
优选的，所述最优偏移量
Figure BDA0002309766790000071
采用梯度下降法求解，其步骤为：
(1)设置初始值
Figure BDA0002309766790000072
最大迭代次数M，梯度阈值ε，梯度方向搜索参数η，已迭代次数m＝0；
(2)根据J的定义求解J关于q的梯度
Figure BDA0002309766790000073
若
Figure BDA0002309766790000074
则转(4)；
(3)按照梯度下降方向进行迭代
Figure BDA0002309766790000075
已迭代次数m＝m+1，若m≥M，则转(4)，否则转(2)；
(4)输出当前迭代变量为最优值q*＝q。
进一步的，所述步骤五中将引导路径各点按照对应最优偏移量进行横向偏移得到规划路径的方法如下：对于引导路径上任一引导路径点
Figure BDA0002309766790000076
按照最优横向偏移量
Figure BDA0002309766790000077
进行横向偏移得到点
Figure BDA0002309766790000078
所有
Figure BDA0002309766790000079
形成的点列即为规划路径。
本发明的有益效果在于：(1)采用一种分层路径规划策略，先以引导路径为基准，对无人车前方区域进行拓扑分析，在此基础上求取粗粒度的最优通道，再在最优通道内求取细粒度的最优路径，可以避免在整个无人车前方区域内进行细粒度的搜索，有效降低计算量；(2)当障碍图中存在检测扰动时，利用最优通道约束最终规划路径的求解空间，避免规划路径发生大范围的变化，提高规划结果的稳定性和时空一致性。本发明提出的方法可直接应用于无人车的局部路径规划，可有效提升无人车行驶的稳定性和安全性。
附图说明
图1为本发明的无人车分层路径规划方法整体流程图；
图2为本发明的引导路径点对应的可通行截面区间集合求解流程图；
图3为本发明的场景实例；
图4为本发明的场景实例中引导路径两侧采样点示意图；
图5为本发明的场景实例中引导路径两侧可通行截面区间示意图；
图6为本发明的场景实例中从起始截面区间到目标可通行截面区间最优通道示意图；
图7为本发明的场景实例中最终规划路径示意图。
具体实施方式
下面结合附图及具体实施例对本发明作进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本发明提出一种无人车分层路径规划方法，结合附图3所示的具体实例，按照附图1所示的流程，具体步骤如下：
步骤一：输入无人车位姿V，引导路径T、障碍图D、引导路径横向采样范围qmax，横向采样步长qstep，无人车安全半径dsafe。所述无人车位姿V包括坐标、航向角，记为V＝(xv，yv，θv)，其中xv，yv，θv分别为无人车的x坐标、y坐标和航向角，通常通过定位定向系统实际测量得到；所述的引导路径T为预先给定的无人车期望行驶路径，通常可设置为一平滑路径点序列，每个路径点的信息包括坐标、方位角，引导路径可表示为
Figure BDA0002309766790000081
其中
Figure BDA0002309766790000082
分别为路径点i的横纵坐标、方位角，n为路径点个数；无人车位姿和引导路径如图3所示，路径上黑色圆点表示引导路径点，无人车当前位置与引导路径T存在一定偏差。
所述障碍图描述了无人车周围环境一定范围内的障碍物分布情况，通常通过无人车环境感知系统实时获得，可设置为二值栅格地图，0表示对应位置无障碍物、1表示对应位置有障碍物。障碍图如图3所示，外围虚线框表示障碍图的范围，虚线框内深色区域表示对应位置有障碍物，不可行驶；白色区域无障碍物分布，可以行驶。
所述引导路径横向采样范围qmax为算法参数，用于在步骤二中限制引导路径左右两侧的最大采样范围，通常可设置为无人车宽度的2倍；所述横向采样步长qstep为步骤二中在引导路径左右两侧进行采样的步长参数，通常可设置为无人车宽度的0.5倍；所述无人车安全半径dsafe指当无人车所在位置与障碍物的距离大于dsafe时，无人车是安全的，通常可以设置为适当大于无人车对角线长度的0.5倍。
步骤二：结合障碍图D、引导路径横向采样范围qmax，横向采样步长qstep，无人车安全半径dsafe，求解引导路径T上各路径点
Figure BDA0002309766790000091
对应的可通行截面区间集合Si，所有路径点对应的可通行截面区间集合依次级联起来组成可通行截面区间搜索树Q，引导路径两侧可通行截面区间如附图5所示。所述的引导路径T上各路径点
Figure BDA0002309766790000092
对应的可通行截面区间是指在该点处、垂直于引导路径的横截面上的可通行区间，由于障碍的存在，可通行区间可能为零个、一个或多个，对照附图2所示的引导路径点对应的可通行截面区间集合求解流程图，引导路径点
Figure BDA0002309766790000093
对应的可通行截面区间集合Si求解方式如下：
(1)计算路径点
Figure BDA0002309766790000094
左、右侧采样点个数
Figure BDA0002309766790000095
[]表示取整；
(2)计算左侧采样点：
Figure BDA0002309766790000096
计算右侧采样点：
Figure BDA0002309766790000097
左右采样点以及引导路径点统一表示为：
Figure BDA0002309766790000101
引导路径两侧采样点分布示意图如附图4所示。
(3)设j＝0，当前可通行区间w为空；
(4)结合障碍图D判断采样点
Figure BDA0002309766790000102
的可通行性，若
Figure BDA0002309766790000103
可通行，则将
Figure BDA0002309766790000104
加入到w中；若
Figure BDA0002309766790000105
不可通行，则取当前可通行截面区间为[wmin，wmax]，其中wmin表示w中的最小值，wmax表示w中的最大值，[]表示实数区间集合，若w为空集，则当前可通行截面区间为空集，若wmin＝wmax，则当前可通行截面区间为{wmin}，将w加入Si，并将w清空；
(5)j＝j+1，若j≥mi则退出，否则转(4)。
所述结合障碍图D判断采样点
Figure BDA0002309766790000106
的可通行性的方法如下：
(1)将障碍图进行距离变换，得到距离图，距离图中各点取值为该点到障碍的最近距离；
(2)从上述距离图中查询采样点
Figure BDA0002309766790000107
到障碍的最近距离；
(3)若采样点
Figure BDA0002309766790000108
到障碍的最近距离大于无人车安全半径dsafe，则
Figure BDA0002309766790000109
是可通行的，否则，
Figure BDA00023097667900001010
不可通行。
步骤三：将无人车所处位置设为起始可通行截面区间，在引导路径终点对应的可通行截面区间集合中确定目标可通行截面区间O，在可通行截面区间搜索树Q上搜索到达目标可通行截面区间O的最优通道L。
所述在引导路径终点对应的可通行截面区间集合中确定目标区间O的方法如下：
(1)若引导路径终点是可通行的，则其所在的可通行截面区间为目标区间；
(2)若引导路径终点不可通行，则选择离终点最近的可通行截面区间作为目标区间。
所述在可通行截面区间搜索树Q上搜索达到目标区间O的最优通道L中采用的搜索方法为A*算法，搜索结果如附图6所示。其过程为：
1)建立OPEN空表和CLOSE空表，分别用于存放待扩展截面区间节点和已扩展截面区间节点，将起始可通行截面区间加入到OPEN表中；
2)若OPEN表不为空，则弹出OPEN表中第一个截面区间w；否则，搜索失败；
3)判断截面区间w是否为目标截面区间，若是，则搜索结束，输出截面区间w及其所有父截面区间节点作为搜索结果；
4)将截面区间w对应的引导路径点的下一个引导路径点对应的所有可通行截面区间作为w的拟扩展子区间节点；
5)判断各拟扩展子区间节点是否已在CLOSE表中，若是，则舍弃该子区间节点；
6)计算拟扩展子区间节点的代价函数F(i)，代价函数F(i)＝G(i)+H(i)，其中G(i)表示从起始截面区间到截面区间i的代价，G(i)＝a1G1(i)+a2G2(i)+a3G3(i)，其中G1(i)表示从起始截面区间节点到截面区间节点i各区间中点与对应引导路径点横向偏移之和，G2(i)表示从起始截面区间节点到截面区间节点i各区间中点与对应引导路径点横向偏移增量之和，G3(i)表示从起始截面区间节点到截面区间节点i沿引导路径的纵向路程，a1、a2、a3为加权参数；H(i)表示从截面区间节点i到目标截面区间代价的估计值，H(i)＝b1H1(i)+b2H2(i)，H1(i)表示截面区间节点i与目标截面区间中点与对应引导路径点横向偏移之差，H2(i)表示从截面区间节点i到目标截面区间沿引导路径的纵向路程，b1、b2为加权参数；
7)将拟扩展子区间节点加入到OPEN表中，并将OPEN表中所有截面区间节点按照代价F(i)从小到大排序，将当前区间节点加入CLOSE表中，转(2)。
步骤四：将规划路径视为引导路径上各点横向偏移后形成的路径，以引导路径上各点的横向偏移q＝[q0，...，qn-1]为优化变量，以最优通道L对应的可通行截面区间为各点偏移约束，以衡量路径平滑性、安全性的评价函数J为优化目标，求解最优偏移量
Figure BDA0002309766790000121
所述求解最优偏移量的优化目标J的定义如下：J＝c1J1+c2J2；其中
Figure BDA0002309766790000122
Figure BDA0002309766790000123
为规划路径上点pi指向pi+1的向量，pi的坐标
Figure BDA0002309766790000124
Figure BDA0002309766790000125
Figure BDA0002309766790000126
Figure BDA0002309766790000127
为最优通道L中引导路径点
Figure BDA0002309766790000128
对应的可通行截面区间；c1、c2为加权参数。
上述最优偏移量
Figure BDA0002309766790000129
可采用梯度下降法求解，其步骤为：
(1)设置初始值
Figure BDA00023097667900001210
最大迭代次数M，梯度阈值ε，梯度方向搜索参数η，已迭代次数m＝0；
(2)根据J的定义求解J关于q的梯度
Figure BDA00023097667900001211
若
Figure BDA00023097667900001212
则转4)；
(3)按照梯度下降方向进行迭代
Figure BDA00023097667900001213
已迭代次数m＝m+1，若m≥M，则转(4)，否则转(2)；
(4)输出当前迭代变量为最优值q*＝q。
步骤五：将引导路径各点按照对应最优偏移量
Figure BDA00023097667900001214
进行横向偏移，得到规划路径。对于引导路径上任一引导路径点
Figure BDA00023097667900001215
按照最优横向偏移量
Figure BDA0002309766790000131
进行横向偏移得到点
Figure BDA0002309766790000132
所有
Figure BDA0002309766790000133
形成的点列即为规划路径，如附图7所示。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种无人车分层路径规划方法，其特征在于，包括如下步骤：
步骤一：输入无人车位姿V，引导路径T，障碍图D，引导路径横向采样范围qmax，引导路径横向采样步长qstep，无人车安全半径dsafe；
其中V＝(xv，yv，θv)，xv，yv，θv分别为无人车的x坐标、y坐标和航向角；
Figure FDA0002309766780000011
为一平滑路径点序列，
Figure FDA0002309766780000012
分别为路径点i的横纵坐标、方位角，n为路径点个数；qmax为引导路径左右两侧的最大采样范围；qstep为在引导路径左右两侧进行采样的步长参数；dsafe指当无人车所在位置与障碍物的距离大于dsafe时无人车为安全的距离；
步骤二：结合障碍图D、引导路径横向采样范围qmax，横向采样步长qstep和无人车安全半径dsafe，求解引导路径T上各路径点
Figure FDA0002309766780000013
对应的可通行截面区间集合Si，所有路径点对应的可通行截面区间集合Si依次级联起来组成可通行截面区间搜索树Q；
步骤三：将无人车所处位置设为起始可通行截面区间，在引导路径终点对应的可通行截面区间集合中确定目标可通行截面区间O，在可通行截面区间搜索树Q上搜索到达目标可通行截面区间O的最优通道L；
步骤四：将规划路径视为引导路径上各点横向偏移后形成的路径，以引导路径上各点的横向偏移q＝[q0，...，qn-1]为优化变量，以最优通道上对应的可通行截面区间为各点偏移约束，以衡量路径平滑性、安全性的评价函数J为优化目标，求解最优偏移量
Figure FDA0002309766780000014
步骤五：将引导路径各点
Figure FDA0002309766780000015
按照对应最优偏移量
Figure FDA0002309766780000016
进行横向偏移，得到规划路径。
2.根据权利要求1所述的无人车分层路径规划方法，其特征在于，所述步骤一中障碍图为二值栅格地图，栅格点为0表示对应位置无障碍物，栅格点为1表示对应位置有障碍物。
3.根据权利要求1所述的无人车分层路径规划方法，其特征在于，所述步骤二中各路径点
Figure FDA0002309766780000021
对应的可通行截面区间是指在该点处、垂直于引导路径的横截面上的可通行区间，求解可通行截面区间集合Si方法为：
(1)计算路径点
Figure FDA0002309766780000022
左、右侧采样点个数
Figure FDA0002309766780000023
[]表示取整；
(2)计算左侧采样点：
Figure FDA0002309766780000024
计算右侧采样点：
Figure FDA0002309766780000025
左右采样点以及引导路径点统一表示为：
Figure FDA0002309766780000026
(3)设j＝0，当前可通行区间w为空；
(4)结合障碍图D判断采样点
Figure FDA0002309766780000027
的可通行性，若
Figure FDA0002309766780000028
可通行，则将
Figure FDA0002309766780000029
加入到w中；若
Figure FDA00023097667800000210
不可通行，则取当前可通行截面区间为[wmin，wmax]，其中wmin表示w中的最小值，wmax表示w中的最大值，[]表示实数区间集合，若w为空集，则当前可通行截面区间为空集，若wmin＝wmax，则当前可通行截面区间为{wmin}，将w加入Si，并将w清空；
(5)j＝j+1，若j≥mi则退出，否则转(4)。
4.根据权利要求3所述的无人车分层路径规划方法，其特征在于，所述结合障碍图D判断采样点
Figure FDA0002309766780000031
可通行性的方法为：
(1)将障碍图进行距离变换，得到距离图，距离图中各点取值为该点到障碍的最近距离；
(2)从距离图中查询采样点
Figure FDA0002309766780000032
到障碍的最近距离；
(3)若采样点
Figure FDA0002309766780000033
到障碍的最近距离大于无人车安全半径dsafe，则
Figure FDA0002309766780000034
是可通行的，否则，
Figure FDA0002309766780000035
不可通行。
5.根据权利要求1所述的无人车分层路径规划方法，其特征在于，所述步骤三中引导路径终点对应的可通行截面区间集合中确定目标区间O的方法为：判断引导路径终点是否可通行，可通行，则其所在的可通行截面区间为目标区间；不可通行，则选择离终点最近的可通行截面区间作为目标区间。
6.根据权利要求5所述的无人车分层路径规划方法，其特征在于，所述在可通行截面区间搜索树Q上搜索达到目标区间O的最优通道L采用A*方法，方法如下：
(1)建立OPEN空表和CLOSE空表，分别用于存放待扩展截面区间节点和已扩展截面区间节点，将起始可通行截面区间加入OPEN表中；
(2)若OPEN表不为空，则取出OPEN表中第一个截面区间w；否则，搜索失败；
(3)判断截面区间w是否为目标截面区间，若是，则搜索结束，输出截面区间w及其所有父区间节点作为搜索结果；
(4)将截面区间w对应的引导路径点的下一个引导路径点对应的所有可通行截面区间作为w的拟扩展子区间节点；
(5)判断各拟扩展子区间节点是否已在CLOSE表中，若是，则舍弃该子区间节点；
(6)计算拟扩展子区间节点的代价函数F(i)
F(i)＝G(i)+H(i)，
其中G(i)表示从起始截面区间到截面区间i的代价，H(i)表示从截面区间节点i到目标截面区间代价的估计值；
(7)将拟扩展子区间节点加入到OPEN表中，并将OPEN表中所有截面区间节点按照对应代价F(i)从小到大排序，将当前区间节点加入CLOSE表中，转(2)。
7.根据权利要求6所述的无人车分层路径规划方法，其特征在于，所述G(i)和H(i)计算方法为：
G(i)＝a1G1(i)+a2G2(i)+a3G3(j)
H(i)＝b1H1(i)+b2H2(i)
其中G1(i)表示从起始截面区间节点到截面区间节点i各区间中点与对应引导路径点横向偏移之和，G2(i)表示从起始截面区间节点到截面区间节点i各区间中点与对应引导路径点横向偏移增量之和，G3(i)表示从起始截面区间节点到截面区间节点i沿引导路径的纵向路程，a1、a2、a3为加权参数；H1(i)表示截面区间节点i与目标截面区间中点与对应引导路径点横向偏移之差，H2(i)表示从截面区间节点i到目标截面区间沿引导路径的纵向路程，b1、b2为加权参数。
8.根据权利要求1所述的无人车分层路径规划方法，其特征在于，所述步骤四中优化目标J的定义为：
J＝c1J1+c2J2
其中
Figure FDA0002309766780000051
Figure FDA0002309766780000052
为规划路径上点pi指向pi+1的向量，pi的坐标
Figure FDA0002309766780000053
Figure FDA0002309766780000054
为最优通道L中引导路径点
Figure FDA00023097667800000515
对应的可通行截面区间，c1、c2为加权参数。
9.根据权利要求8所述的无人车分层路径规划方法，其特征在于，所述最优偏移量
Figure FDA0002309766780000055
采用梯度下降法求解，其步骤为：
(1)设置初始值
Figure FDA0002309766780000056
最大迭代次数M，梯度阈值ε，梯度方向搜索参数η，已迭代次数m＝0；
(2)根据J的定义求解J关于q的梯度
Figure FDA0002309766780000057
若
Figure FDA0002309766780000058
则转(4)；
(3)按照梯度下降方向进行迭代
Figure FDA0002309766780000059
已迭代次数m＝m+1，若m≥M，则转(4)，否则转(2)；
(4)输出当前迭代变量为最优值q*＝q。
10.根据权利要求1所述的无人车分层路径规划方法，其特征在于，所述步骤五中引导路径各点
Figure FDA00023097667800000510
按照对应最优偏移量进行横向偏移得到规划路径的求解方法为：对于引导路径上任一引导路径点
Figure FDA00023097667800000511
按照最优横向偏移量
Figure FDA00023097667800000512
进行横向偏移得到点
Figure FDA00023097667800000513
所有
Figure FDA00023097667800000514
形成的点列即为规划路径。
CN201911262897.4A 2019-12-09 2019-12-09 一种无人车分层路径规划方法 Active CN110908386B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911262897.4A CN110908386B (zh) 2019-12-09 2019-12-09 一种无人车分层路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911262897.4A CN110908386B (zh) 2019-12-09 2019-12-09 一种无人车分层路径规划方法

Publications (2)

Publication Number Publication Date
CN110908386A true CN110908386A (zh) 2020-03-24
CN110908386B CN110908386B (zh) 2020-09-01

Family

ID=69824236

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911262897.4A Active CN110908386B (zh) 2019-12-09 2019-12-09 一种无人车分层路径规划方法

Country Status (1)

Country Link
CN (1) CN110908386B (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111506063A (zh) * 2020-04-13 2020-08-07 中国科学技术大学 一种基于分层强化学习框架的移动机器人无图导航方法
CN111552284A (zh) * 2020-04-20 2020-08-18 宁波吉利汽车研究开发有限公司 无人驾驶车辆的局部路径规划方法、装置、设备及介质
CN112099504A (zh) * 2020-09-16 2020-12-18 深圳优地科技有限公司 机器人移动方法、装置、设备及存储介质
CN112130574A (zh) * 2020-09-30 2020-12-25 拉扎斯网络科技（上海）有限公司 机器人的控制方法、装置、电子设备及计算机存储介质
CN113050627A (zh) * 2021-03-02 2021-06-29 北京旷视机器人技术有限公司 路径规划方法、装置、移动机器人及计算机存储介质
CN113618276A (zh) * 2021-07-27 2021-11-09 华南理工大学 基于分层搜索树实现工件自动布置的变位机路径规划方法
WO2023221537A1 (zh) * 2022-05-16 2023-11-23 北京京东乾石科技有限公司 路径规划方法及装置、无人车

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2008234447A (ja) * 2007-03-22 2008-10-02 Yaskawa Electric Corp 移動体の制御装置及びそれを備えた移動体
CN102591332A (zh) * 2011-01-13 2012-07-18 同济大学 用于无人驾驶汽车局部路径规划的装置及方法
CN106708059A (zh) * 2017-01-24 2017-05-24 厦门万久科技股份有限公司 一种基于通道选择的移动机器人实时运动规划方法
CN108106623A (zh) * 2017-09-08 2018-06-01 同济大学 一种基于流场的无人车路径规划方法
CN108444488A (zh) * 2018-02-05 2018-08-24 天津大学 基于等步采样a*算法的无人驾驶局部路径规划方法
CN110333659A (zh) * 2019-07-15 2019-10-15 中国人民解放军军事科学院国防科技创新研究院 一种基于改进a星搜索的无人驾驶汽车局部路径规划方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2008234447A (ja) * 2007-03-22 2008-10-02 Yaskawa Electric Corp 移動体の制御装置及びそれを備えた移動体
CN102591332A (zh) * 2011-01-13 2012-07-18 同济大学 用于无人驾驶汽车局部路径规划的装置及方法
CN106708059A (zh) * 2017-01-24 2017-05-24 厦门万久科技股份有限公司 一种基于通道选择的移动机器人实时运动规划方法
CN108106623A (zh) * 2017-09-08 2018-06-01 同济大学 一种基于流场的无人车路径规划方法
CN108444488A (zh) * 2018-02-05 2018-08-24 天津大学 基于等步采样a*算法的无人驾驶局部路径规划方法
CN110333659A (zh) * 2019-07-15 2019-10-15 中国人民解放军军事科学院国防科技创新研究院 一种基于改进a星搜索的无人驾驶汽车局部路径规划方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
MOHAMMAD POURMAHMOOD AGHABABA: "3D path planning for underwater vehicles using five evolutionary optimization algorithms avoiding static and energetic obstacles", 《APPLIED OCEAN RESEARCH》 *
宋金泽: "一种改进的RRT路径规划算法", 《电子学报》 *
朱琪 等: "基于减法聚类的合并最优路径层次聚类算法", 《计算机工程》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111506063A (zh) * 2020-04-13 2020-08-07 中国科学技术大学 一种基于分层强化学习框架的移动机器人无图导航方法
CN111506063B (zh) * 2020-04-13 2021-08-13 中国科学技术大学 一种基于分层强化学习框架的移动机器人无图导航方法
CN111552284A (zh) * 2020-04-20 2020-08-18 宁波吉利汽车研究开发有限公司 无人驾驶车辆的局部路径规划方法、装置、设备及介质
CN112099504A (zh) * 2020-09-16 2020-12-18 深圳优地科技有限公司 机器人移动方法、装置、设备及存储介质
CN112130574A (zh) * 2020-09-30 2020-12-25 拉扎斯网络科技（上海）有限公司 机器人的控制方法、装置、电子设备及计算机存储介质
CN113050627A (zh) * 2021-03-02 2021-06-29 北京旷视机器人技术有限公司 路径规划方法、装置、移动机器人及计算机存储介质
CN113618276A (zh) * 2021-07-27 2021-11-09 华南理工大学 基于分层搜索树实现工件自动布置的变位机路径规划方法
CN113618276B (zh) * 2021-07-27 2022-04-26 华南理工大学 基于分层搜索树实现工件自动布置的变位机路径规划方法
WO2023221537A1 (zh) * 2022-05-16 2023-11-23 北京京东乾石科技有限公司 路径规划方法及装置、无人车

Also Published As

Publication number Publication date
CN110908386B (zh) 2020-09-01

Similar Documents

Publication Publication Date Title
CN110908386B (zh) 一种无人车分层路径规划方法
CN107702716B (zh) 一种无人驾驶路径规划方法、系统和装置
CN106647769B (zh) 基于a*提取引导点的agv路径跟踪与避障协调方法
CN110609552B (zh) 一种水下无人航行器编队平面航迹规划方法
CN107168305B (zh) 路口场景下基于Bezier和VFH的无人车轨迹规划方法
CN109506655B (zh) 基于非均匀建模的改进蚁群路径规划算法
CN108958285B (zh) 一种基于分解思想的高效多无人机协同航迹规划方法
CN112378408A (zh) 一种实现轮式移动机器人实时避障的路径规划方法
KR101339480B1 (ko) Ｒｒｔ 기반의 듀얼 트리 구조를 이용한 이동 로봇의 궤적 계획 방법
CN108073176A (zh) 一种改进型D*Lite车辆动态路径规划方法
Lan et al. Continuous curvature path planning for semi-autonomous vehicle maneuvers using RRT
CN109579854B (zh) 基于快速扩展随机树的无人车避障方法
CN113204236A (zh) 一种智能体路径跟踪控制方法
Ben-Messaoud et al. Smooth obstacle avoidance path planning for autonomous vehicles
CN111998858B (zh) 一种基于改进a*算法的无人机航路规划方法
CN116331264A (zh) 一种未知障碍物分布的避障路径鲁棒规划方法及系统
Gao et al. Multi-lane convoy control for autonomous vehicles based on distributed graph and potential field
CN114706400A (zh) 一种越野环境下基于改进的a*算法的路径规划方法
López et al. A new approach to local navigation for autonomous driving vehicles based on the curvature velocity method
CN113219990B (zh) 基于自适应邻域与转向代价的机器人路径规划方法
Nagasaka et al. Towards safe, smooth, and stable path planning for on-road autonomous driving under uncertainty
CN112706760B (zh) 一种用于特殊道路场景的无人驾驶泊车路径规划方法
Bi et al. CURE: A hierarchical framework for multi-robot autonomous exploration inspired by centroids of unknown regions
CN112947491A (zh) 一种无人车快速轨迹规划方法
Smit et al. Informed sampling-based trajectory planner for automated driving in dynamic urban environments

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