CN110530390A - 一种在狭窄环境下的非质点车辆路径规划方法 - Google Patents

一种在狭窄环境下的非质点车辆路径规划方法 Download PDF

Info

Publication number
CN110530390A
CN110530390A CN201910869967.6A CN201910869967A CN110530390A CN 110530390 A CN110530390 A CN 110530390A CN 201910869967 A CN201910869967 A CN 201910869967A CN 110530390 A CN110530390 A CN 110530390A
Authority
CN
China
Prior art keywords
vehicle
point
follows
turning
path
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.)
Pending
Application number
CN201910869967.6A
Other languages
English (en)
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.)
Harbin Engineering University
Original Assignee
Harbin Engineering 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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201910869967.6A priority Critical patent/CN110530390A/zh
Publication of CN110530390A publication Critical patent/CN110530390A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明属于路径规划领域，特别涉及一种在狭窄环境下的非质点车辆路径规划方法。本发明方法包括：首先进行环境建模，建立二维平面直角坐标系并确定障碍物以及起始点和目标点；然后进行路径规划，包括膨胀障碍物、应用现有的三角剖分算法、A*算法以及路径平滑算法规划初始路径；然后对初始路径在转弯点进行车辆与障碍物碰撞检测。本方法在传统A*算法以及三角剖分算法和路径平滑的基础上，考虑了车辆的实际尺寸以及其转弯半径，提高了路径规划的成功率。本发明考虑到了车辆的尺寸以及车辆在转弯过程中的转弯半径问题，相比较于传统的路径规划方法，更符合现实需求，有效避免了规划处路径实用性差的缺点，提高了路径规划的成功率。

Description

一种在狭窄环境下的非质点车辆路径规划方法
技术领域
本发明涉及路径规划领域，特别涉及一种在狭窄环境下的非质点车辆路径规划方法。
背景技术
近年来，随着定位导航技术的快速发展，路径规划已成为业内重点研究议题，其在车辆行驶过程中的应用尤为广泛。目前，A*算法和快速探索随机扩展树(RRT)算法等相关传统的路径规划算法都取得了一定的成功。A*算法是一种在网格化的环境建模基础上进行搜索的算法，其在启发式搜索提高效率的同时，找到一条基于评估函数的最优路径。RTT算法通过在状态空间内随机选择采样点把搜索导向空白区域，可以有效地搜索高维空间。但存在狭窄通道时，RRT算法由于在全局环境空间中普遍采用均匀随机取样，而狭窄通道占全局环境空间的比例比较小，以至其性能会发生大幅度下降。传统路径规划算法都有共性的问题：在路径规划过程中将车辆抽象成为一个质点，不考虑车辆的自身尺寸。在狭窄环境中，单纯考虑将车辆抽象为质点时搜索到的路径在实际环境中很大程度上不适用。因此，车辆本身的尺寸对路径规划的成功率有极大的影响。
综上所述，本发明在使用传统路径规划方法A*算法的基础上，考虑车辆自身尺寸以及在车辆转弯过程中的转弯半径，有效提高车辆在狭窄环境下的路径规划成功率。
发明内容
本发明的目的是为了提供一种在狭窄环境下的非质点车辆路径规划方法。
本发明的目的是这样实现的：
一种在狭窄环境下的非质点车辆路径规划方法，包括以下步骤：
步骤1：进行环境建模，包括建立平面直角坐标系、确定障碍物以及起始点和目标点；
步骤2：进行路径规划，包括膨胀障碍物、应用现有的三角剖分算法、A*算法以及路径平滑算法规划初始路径；
步骤3：对初始路径在转弯点进行车辆与障碍物碰撞检测，如果通过则结束，如果不通过则返回步骤2。
本发明还可以包括：
所述的步骤1中建立平面直角坐标系具体为：将车辆运动的环境规定为矩形，以矩形地理中的西南方向顶点作为平面直角坐标系原点O，单位长度为1米，向北方向为y轴正方向，向东方向为x轴正方向。
所述的步骤1中确定障碍物以及起始点和目标点具体为：
起始点坐标S(xs，ys)，目标点坐标G(xg，yg)；将障碍物用多边形表示OBJk＝{(xk1，yk1)，(xk2，yk2)，...，(xkn，ykn)}其中k代表第k个障碍物，n代表第k个障碍物中第n个顶点。
所述的步骤2中进行路径规划具体包括：
步骤2.1：进行外循环初始化，设定根据障碍物坐标计算障碍物质心坐标M(xm，ym)，计算质心到障碍物多边形各边距离Disk＝{d1，d2，...di，...dn}，膨胀障碍物多边形，使质心到障碍物各边的距离等于实际距离加车辆宽度w的1/2，即di+w/2；
步骤2.2：对车辆运动的环境采用现有的Bowyer-Watson三角剖分方法进行三角剖分，得到去除障碍物后的三角剖分三角形集合D；
步骤2.3：采用现有的A*算法进行寻路，找到一条从起始点到目标点的初步可行路径；
步骤2.4：避免转弯处过多，采用现有的路径平滑算法对找到的可行路径进行路径平滑；
步骤2.5：还原障碍物尺寸，根据步骤2.4中得到的路径规划轨迹，向轨迹两侧分别扩展w/2得到车辆初步可行空腔；
步骤2.6：在步骤2.5中路径转弯处，令原行驶方向与新的行驶方向夹角为α，汽车内侧前轮转角δ＝α；
步骤2.7：在转弯处计算内侧后轮转弯半径，检查内侧后轮轨迹与障碍物是否相交；若相交，返回步骤2.3，重新运用A*算法进行寻路，寻找次优解；
步骤2.8：找下一个转弯点进行检查，若检查通过了最后一个转弯点，则找到可行路径；否则，寻路失败。
所述的步骤2.7中转弯处车辆内侧后轮转弯半径r4为：
其中l为车辆轴距；δ∈(0°，90°)为车辆前轮转角；
所述的步骤2.7中检查内侧后轮轨迹与障碍物是否相交的具体方法为：判断障碍物多边形的任意顶点或任意一条边是否在由转弯前的路径l1内侧空腔边界、车辆转弯中心O′与车辆结束转弯时内侧后轮所处位置连线所在直线以及辆内侧后轮转弯过程中轨迹线所围成的封闭区域中；若是，则有碰撞发生，车辆不能通过该转弯点；否则没有碰撞发生，车辆顺利通过该转弯点；根据步骤2.4可知路径上三个转弯点坐标分别是A(xn-1，yn-1)，B(xn，yn)，C(xn+1，yn+1)；其中A点为车辆前轮内侧转弯点；B点为车辆内侧后轮位置；C点为车辆下一转弯点；则车辆转弯存在以下8种情况：
情况1：当xn-1＝xn且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程为：
车辆转弯中心坐标O′为：
情况2：当xn-1＝xn且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程为：
车辆转弯中心坐标O′为：
情况3：当xn+1＝xn且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程为：
车辆转弯中心坐标O′为：
情况4：当xn+1＝xn且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程为：
车辆转弯中心坐标O′为：
情况5：当xn-1≠xn≠xn+1且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程为：
车辆转弯中心坐标O′为：
情况6：当xn-1≠xn≠xn+1且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程为：
车辆转弯中心坐标O′为：
情况7：当xn-1≠xn≠xn+1且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程为：
车辆转弯中心坐标O′为：
情况8：当xn-1≠xn≠xn+1且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程：
车辆转弯中心坐标O′为：
对于每个障碍物多边形各点坐标(xki，yki)∈OBJk，当车辆转弯情况为情况1、情况4、情况6或情况7中任意一种情况时，则检测障碍物多边形所有顶点是否满足如下约束：
若有任意一个障碍物多边形的任意一个顶点满足上述约束，则车辆在该转弯点与障碍物发生碰撞，不能通过；
对于每个障碍物多边形各点坐标(xki，yki)∈OBJk，当车辆转弯情况为情况2、情况3、情况5或情况8中任意一种情况时，则检测障碍物多边形所有顶点是否满足如下约束：
若有任意一个障碍物多边形的任意一个顶点满足上述约束，则车辆在该转弯点与障碍物发生碰撞，不能通过。
所述的步骤2.7中寻找次优解的方法具体为：将步骤2.3中首次获得路径上的点(即使用A*算法获得的全局路径)记录下来，选择第一个点的次小代价点作为其下一步选择，然后运行步骤2.3至步骤2.8；若找不到路径，则迭代选择第二个点的次小代价点作为其下一步选择，然后运行步骤2.3至步骤2.8；结束条件：找到可行路径；迭代检查完路径上最后一个点的最大代价点。
本发明的有益效果在于：
本发明考虑到了车辆的尺寸以及车辆在转弯过程中的转弯半径问题，相比较于传统的路径规划方法，更符合现实需求，有效避免了规划处路径实用性差的缺点，提高了路径规划的成功率。
附图说明
图1为狭窄环境下非质点车辆的路径规划方法流程图；
图2为狭窄环境下非质点车辆的路径规划方法车辆转弯示意图；
图3为狭窄环境下非质点车辆的路径规划方法场景图；
具体实施方式
本发明提出一种在狭窄环境下的非质点车辆路径规划方法，旨在解决考虑车辆的实际尺寸和形状时在狭窄环境下进行路径规划的问题。在传统A*算法以及三角剖分算法和路径平滑的基础上，考虑车辆的实际尺寸以及其转弯半径，提高路径规划的成功率。本发明提出包括环境建模和路径规划两个步骤：
步骤1：前述的在狭窄环境下非质点车辆的路径规划方法环境建模具体还包括建立二维平面直角坐标系和确定障碍物以及起始点和目标点坐标两个步骤，其具体特征如下：
(1.1)建立二维空间平面直角坐标系
将车辆运动的环境规定为矩形，以矩形地理中的西南方向顶点作为平面直角坐标系原点O，单位长度为1米，向北方向为y轴正方向，向东方向为x轴正方向。
(1.2)确定障碍物以及起始点和目标点坐标
起始点坐标S(xs，ys)，目标点坐标G(xg，yg)。将障碍物用多边形表示OBJk＝{(xk1，yk1)，(xk２，yk２)，...，(xkn，ykn)}其中k代表第k个障碍物，n代表第k个障碍物中第n个顶点。
步骤2：前述的在狭窄环境下非质点车辆的路径规划方法路径规划具体还包括：
(2.1)根据障碍物坐标计算障碍物质心坐标M(xm，ym)，计算质心到障碍物多边形各边距离Disk＝{d1，d2，…di，...dn}，膨胀障碍物多边形，使质心到障碍物各边的距离等于实际距离加车辆宽度w的1/2，即di+w/2。
(2.2)对车辆运动的环境采用现有的Bowyer-Watson三角剖分方法进行三角剖分，得到去除障碍物后的三角剖分三角形集合D。
(2.3)采用现有的A*算法进行寻路，找到一条从起始点到目标点的初步可行路径。
(2.4)避免转弯处过多，采用现有的路径平滑算法对找到的可行路径进行路径平滑。
(2.5)还原障碍物尺寸，根据(2.4)中得到的路径规划轨迹，向轨迹两侧分别扩展w/2得到车辆初步可行空腔。
(2.6)在(2.5)中路径转弯处，令原行驶方向与新的行驶方向夹角为α，汽车内侧前轮转角δ＝α。
(2.7)在转弯处计算内侧后轮转弯半径，检查内侧后轮轨迹与障碍物是否相交。若相交，返回(2.3)，重新运用A*算法进行寻路，寻找次优解。
(2.8)找下一个转弯点进行检查，若检查通过了最后一个转弯点，则找到可行路径。否则，寻路失败。
假设车辆行驶在平坦道路的场景下，忽略与行驶动力学相关的垂向影响和耦合作用以及轮胎滑移角影响。车辆为刚性结构后驱车，前轮为转向轮。根据Ackerman转向运动学模型，低速行驶车辆转弯时，利用四连杆的相等曲柄，可以使内侧轮的转向角比外侧轮大大约2～4度，四个轮子绕一点作圆周运动，且四个轮子运动轨迹为四个同心圆。车辆转弯时，l为车辆轴距，w为车辆宽度，δ∈(0°，90°)为车辆前轮转角，r1为外侧前轮转弯半径，r2为内侧前轮转弯半径，r3为外侧后轮转弯半径，r4为内侧后轮转弯半径，O′为车辆转弯中心。
本发明中，步骤(2.1)中将障碍物各个方向尺寸均膨胀为车辆自身宽度的1/2，保证了在任意两个障碍物之间若存在质点可通过路径，则其间隔最小距离大于车辆自身宽度w，车辆在沿直线行驶过程中一定可以通过。转弯时，由于车辆四个轮子运动轨迹为四个同心圆，易知两个前轮转弯半径差和两个后轮转弯半径差均为w。前轮一定可以在初步可行空腔内行驶，而外侧后轮转弯半径r3∈(r2，r1)，内侧后轮转弯半径r4＜r2，所以步骤(2.7)中只考虑内侧后轮是否与障碍物相交即可。
前述的在狭窄环境下的非质点车辆路径规划方法路径规划部分步骤(2.7)还包括如下特征：
(2.7.1)转弯时车辆与障碍物碰撞检测
内侧后轮转弯半径为：
外侧前轮转弯半径为：
根据步骤(2.4)可知该路径三个转弯点坐标分别是A(xn-1，yn-1)，B(xn，yn)，C(xn+1，yn+1)，其中下述C(xn+1，yn+1)均为车辆下一转弯点。
转弯前的路径l1方程表示为：
a1x+b1y+c1＝0
转弯后的路径l2方程表示为：
a2x+b2y+c2＝0
由于车辆行驶方向以及在各个转弯点转弯方向不同，车辆内侧后轮就不同，则相对于车辆路径内侧空腔边界线就不相同。在该检验方法中根据车辆行驶方向以及在转弯点的转弯方向不同，步骤(2.7.1)具体还包括如下特征：
(2.7.1.1)当xn-1＝xn且时，则车辆前轮内侧转弯点A为
车辆内侧后轮位置
l1内侧空腔边界线方程：
l2内侧空腔边界线方程：
过内侧前轮当前轨迹线垂线方程：
b2x-a2y+a2yn-b2xn＝0 (2-1)
过内侧后轮当前轨迹线垂线方程：
由于四个车轮轨迹为同心圆，联立方程(2-1)、(2-2)，则可计算出车辆转弯中心坐标
(2.7.1.2)当xn-1＝xn且时，则车辆前轮内侧转弯点A为
车辆内侧后轮位置
l1内侧空腔边界线方程：
l2内侧空腔边界线方程：
过内侧前轮当前轨迹线垂线方程：
b2x-a2y+a2yn-b2xn＝0 (2-3)
过内侧后轮当前轨迹线垂线方程：
由于四个车轮轨迹为同心圆，联立方程(2-3)、(2-4)，则可计算出车辆转弯中心坐标
(2.7.1.3)当xn+1＝xn且时，则车辆前轮内侧转弯点A为
车辆内侧后轮位置
l1内侧空腔边界线方程：
l2内侧空腔边界线方程：
过内侧前轮当前轨迹线垂线方程：
b2x-a2y+a2yn-b2xn＝0 (2-5)
过内侧后轮当前轨迹线垂线方程：
由于四个车轮轨迹为同心圆，联立方程(2-5)、(2-6)，则可计算出车辆转弯中心坐 标。
(2.7.1.4)当xn+1＝xn且时，则车辆前轮内侧转弯点A为
车辆内侧后轮位置
l1内侧空腔边界线方程：
l2内侧空腔边界线方程：
过内侧前轮当前轨迹线垂线方程：
b2x-a2y+a2yn-b2xn＝0 (2-7)
过内侧后轮当前轨迹线垂线方程：
由于四个车轮轨迹为同心圆，联立方程(2-7)、(2-8)，则可计算出车辆转弯中心坐标
(2.7.1.5)当xn-1≠xn≠xn+1且时，则车辆前轮内侧转弯点A为
车辆内侧后轮位置
l1内侧空腔边界线方程：
l2内侧空腔边界线方程：
过内侧前轮当前轨迹线垂线方程：
b2x-a2y+a2yn-b2xn＝0 (2-9)
过内侧后轮当前轨迹线垂线方程：
由于四个车轮轨迹为同心圆，联立方程(2-9)、(2-10)，则可计算出车辆转弯中心坐标
(2.7.1.6)当xn-1≠xn≠xn+1且时，则车辆前轮内侧转弯点A为
车辆内侧后轮位置
l1内侧空腔边界线方程：
l2内侧空腔边界线方程：
过内侧前轮当前轨迹线垂线方程：
b2x-a2y+a2yn-b2xn＝0 (2-11)
过内侧后轮当前轨迹线垂线方程：
由于四个车轮轨迹为同心圆，联立方程(2-11)、(2-12)，则可计算出车辆转弯中心坐标
(2.7.1.7)当xn-1≠xn≠xn+1且时，则车辆前轮内侧转弯点A为
车辆内侧后轮位置
l1内侧空腔边界线方程：
l2内侧空腔边界线方程：
过内侧前轮当前轨迹线垂线方程：
b2x-a2y+a2yn-b2xn＝0 (2-13)
过内侧后轮当前轨迹线垂线方程：
由于四个车轮轨迹为同心圆，联立方程(2-13)、(2-14)，则可计算出车辆转弯中心坐标
(2.7.1.8)当xn-1≠xn≠xn+1且时，则车辆前轮内侧转弯点A为
车辆内侧后轮位置
l1内侧空腔边界线方程：
l2内侧空腔边界线方程：
过内侧前轮当前轨迹线垂线方程：
b2x-a2y+a2yn-b2xn＝0 (2-15)
过内侧后轮当前轨迹线垂线方程：
由于四个车轮轨迹为同心圆，联立方程(2-15)、(2-16)，则可计算出车辆转弯中心坐标
当车辆完成转弯时刻，近似令车辆在l2所在空腔运动，则此时车辆内侧后轮正处于转弯点处。在车辆转弯过程中，判断车辆内侧后轮是否会与障碍物发生碰撞问题则转化为判断障碍物多边形的任意顶点或任意一条边是否在由l1内侧空腔边界、车辆转弯中心O′与车辆结束转弯时内侧后轮所处位置连线所在直线以及辆内侧后轮转弯过程中轨迹线所围成的封闭区域中。若是，则有碰撞发生，车辆不能通过该转弯点；否则没有碰撞发生，车辆顺利通过该转弯点。
其具体实施方案为：
对于每个障碍物多边形各点坐标(xki，yki)∈OBJk，当车辆转弯情况为特征(2.7.1.1)、(2.7.1.4)、(2.7.1.6)(2.7.1.7)中任意一种情况时，则检测障碍物多边形所有顶点是否满足如下约束：
若有任意一个障碍物多边形的任意一个顶点满足上述约束，则车辆在该转弯点与障碍物发生碰撞，不能通过。
对于每个障碍物多边形各点坐标(xki，yki)∈OBJk，当车辆转弯情况为特征(2.7.1.2)、(2.7.1.3)、(2.7.1.5)(2.7.1.8)中任意一种情况时，则检测障碍物多边形所有顶点是否满足如下约束：
若有任意一个障碍物多边形的任意一个顶点满足上述约束，则车辆在该转弯点与障碍物发生碰撞，不能通过。
(2.7.2)寻找次优解方法
将步骤(2.3)中首次获得路径上的点(即使用A*算法获得的全局路径)记录下来，选择第一个点的次小代价点作为其下一步选择，然后运行步骤(2.3)-(2.8)；若找不到路径，则迭代选择第二个点的次小代价点作为其下一步选择，然后运行步骤(2.3)-(2.8)；结束条件：找到可行路径；迭代检查完路径上最后一个点的最大代价点。
本发明提出了一种在狭窄环境下非质点车辆的路径规划方法，其有益效果如下：
本发明考虑到了车辆的尺寸以及车辆在转弯过程中的转弯半径问题，相比较于传统的路径规划方法，更符合现实需求，有效避免了规划处路径实用性差的缺点，提高了路径规划的成功率。
实施例1：
本发明提出一种在狭窄环境下的非质点车辆路径规划方法，旨在解决考虑车辆的实际尺寸和形状在狭窄环境下进行路径规划。在传统A*算法以及三角剖分算法和路径平滑的基础上，考虑车辆的实际尺寸以及其转弯半径，提高路径规划的成功率。如图3，在该实施实例中，车辆长轴距为2米，宽度1.5米。
狭窄环境下非质点车辆的路径规划方法主要包括以下具体步骤：
(1)建立平面直角坐标系，以地理中的西南方向顶点作为平面直角坐标系原点O，单位长度为1米，向北方向为y轴正方向，向东方向为x轴正方向。
(2)读入场景中车辆起始点、目标点以及障碍物位置坐标，本实施实例中起始点坐标S(3，2)，目标点坐标G(10，5)，场景中有4个障碍物，每个障碍物的顶点坐标集合分别为：
OBJ1＝{(3，4)，(4，4)，(3，6)，(4，6)}，
OBJ2＝{(6，1)，(7，1)，(6，3)，(7，3)}，
OBJ3＝{(5，5)，(6，5)，(5，11)，(6，11)}，
OBJ4＝{(8，3)，(9，3)，(8，11)，(9，11)}.
(3)根据障碍物坐标计算各个障碍物质心坐标M1(3.5，5)，M2(6.5，2)，M3(5.5，8)，M4(8.5，7)，计算质心到障碍物多边形各边距离Dis1＝{0.5，1，0.5，1}，Dis2＝{0.5，1，0.5，1}，Dis3＝{0.5，3，0.5，3}，Dis4＝{0.5，4，0.5，4}，膨胀障碍物多边形，使质心到障碍物各边的距离等于实际距离加车辆宽度w的1/2，即0.75米。则膨胀后各个障碍物质心到各边距离为Dis1＝{1.25，1.75，1.25，1.75}，Dis2＝{1.25，1.75，1.25，1.75}，Dis3＝{1.25，3.75，1.25，3.75}，Dis4＝{1.25，4.75，1.25，4.75}。
(4)对车辆运动的环境采用现有的Bowyer-Watson三角剖分方法进行三角剖分，得到去除障碍物后的三角剖分三角形集合D。
(5)采用现有A*算法进行寻路，找到一条从起始点到目标点的初步可行路径。
(6)避免转弯处过多，采用现有的路径平滑算法对找到的可行路径进行路径平滑。
(7)还原障碍物尺寸，根据(2.4)中得到的路径规划轨迹，向轨迹两侧分别扩展0.75米得到车辆初步可行空腔。
(8)在(2.5)中路径转弯处，令原行驶方向与新的行驶方向夹角为α，汽车前轮转角δ＝α。
(9)在转弯处计算四个轮转弯半径，检查内侧后轮轨迹与障碍物是否相交。若相交，返回(2.3)，重新运用A*算法进行寻路，寻找次优解。
(10)找下一个转弯点进行检查，若检查通过了最后一个转弯点，则找到可行路径。否则，寻路失败。
在场景图3中，路径1为A*算法规划出来的第一条初步可行路径，经过车辆在转弯点碰撞检测，发现车辆会在转弯时与障碍物发生碰撞。因此，该路径车辆不能顺利通过。算法重新规划路径，寻找次优解，的到第二条初步可行路径，且该路径通过车辆在转弯点碰撞检测，找到实际可行路径。
综上所述，本发明属于路径规划领域，特别涉及一种在狭窄环境下的非质点车辆路径规划方法。通过环境建模建立二维平面直角坐标系和确定障碍物以及起始点和目标点，和路径规划在传统A*算法以及三角剖分算法和路径平滑的基础上，考虑车辆的实际尺寸以及其转弯半径，提高路径规划的成功率。本发明考虑到了车辆的尺寸以及车辆在转弯过程中的转弯半径问题，相比较于传统的路径规划方法，更符合现实需求，有效避免了规划处路径实用性差的缺点，提高了路径规划的成功率。

Claims (6)

1.一种在狭窄环境下的非质点车辆路径规划方法，其特征在于，包括以下步骤：
步骤1：进行环境建模，包括建立平面直角坐标系、确定障碍物以及起始点和目标点；
步骤2：进行路径规划，包括膨胀障碍物、应用现有的三角剖分算法、A*算法以及路径平滑算法规划初始路径；
步骤3：对初始路径在转弯点进行车辆与障碍物碰撞检测，如果通过则结束，如果不通过则返回步骤2。
2.根据权利要求1所述的一种在狭窄环境下的非质点车辆路径规划方法，其特征在于：所述的步骤1中建立平面直角坐标系具体为：将车辆运动的环境规定为矩形，以矩形地理中的西南方向顶点作为平面直角坐标系原点O，单位长度为1米，向北方向为y轴正方向，向东方向为x轴正方向。
3.根据权利要求1或2所述的一种在狭窄环境下的非质点车辆路径规划方法，其特征在于：所述的步骤1中确定障碍物以及起始点和目标点具体为：
起始点坐标S(xs，ys)，目标点坐标G(xg，yg)；将障碍物用多边形表示OBJk＝{(xk1,yk1),(xk2,yk2),...,(xkn,ykn)}其中k代表第k个障碍物，n代表第k个障碍物中第n个顶点。
4.根据权利要求3所述的一种在狭窄环境下的非质点车辆路径规划方法，其特征在于：所述的步骤2中进行路径规划具体包括：
步骤2.1：进行外循环初始化，设定根据障碍物坐标计算障碍物质心坐标M(xm,ym)，计算质心到障碍物多边形各边距离Disk＝{d1,d2,…di,…dn}，膨胀障碍物多边形，使质心到障碍物各边的距离等于实际距离加车辆宽度w的1/2，即di+w/2；
步骤2.2：对车辆运动的环境采用现有的Bowyer-Watson三角剖分方法进行三角剖分，得到去除障碍物后的三角剖分三角形集合D；
步骤2.3：采用现有的A*算法进行寻路，找到一条从起始点到目标点的初步可行路径；
步骤2.4：避免转弯处过多，采用现有的路径平滑算法对找到的可行路径进行路径平滑；
步骤2.5：还原障碍物尺寸，根据步骤2.4中得到的路径规划轨迹，向轨迹两侧分别扩展w/2得到车辆初步可行空腔；
步骤2.6：在步骤2.5中路径转弯处，令原行驶方向与新的行驶方向夹角为α，汽车内侧前轮转角δ＝α；
步骤2.7：在转弯处计算内侧后轮转弯半径，检查内侧后轮轨迹与障碍物是否相交；若相交，返回步骤2.3，重新运用A*算法进行寻路，寻找次优解；
步骤2.8：找下一个转弯点进行检查，若检查通过了最后一个转弯点，则找到可行路径；否则，寻路失败。
5.根据权利要求4所述的一种在狭窄环境下的非质点车辆路径规划方法，其特征在于：所述的步骤2.7中转弯处车辆内侧后轮转弯半径r4为：
其中l为车辆轴距；δ∈(0°,90°)为车辆前轮转角；
所述的步骤2.7中检查内侧后轮轨迹与障碍物是否相交的具体方法为：判断障碍物多边形的任意顶点或任意一条边是否在由转弯前的路径l1内侧空腔边界、车辆转弯中心O′与车辆结束转弯时内侧后轮所处位置连线所在直线以及辆内侧后轮转弯过程中轨迹线所围成的封闭区域中；若是，则有碰撞发生，车辆不能通过该转弯点；否则没有碰撞发生，车辆顺利通过该转弯点；根据步骤2.4可知路径上三个转弯点坐标分别是A(xn-1,yn-1)，B(xn,yn)，C(xn+1,yn+1)；其中A点为车辆前轮内侧转弯点；B点为车辆内侧后轮位置；C点为车辆下一转弯点；则车辆转弯存在以下8种情况：
情况1：当xn-1＝xn且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程为：
车辆转弯中心坐标O′为：
情况2：当xn-1＝xn且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程为：
车辆转弯中心坐标O′为：
情况3：当xn+1＝xn且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程为：
车辆转弯中心坐标O′为：
情况4：当xn+1＝xn且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程为：
车辆转弯中心坐标O′为：
情况5：当xn-1≠xn≠xn+1且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程为：
车辆转弯中心坐标O′为：
情况6：当xn-1≠xn≠xn+1且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程为：
车辆转弯中心坐标O′为：
情况7：当xn-1≠xn≠xn+1且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程为：
车辆转弯中心坐标O′为：
情况8：当xn-1≠xn≠xn+1且时：
车辆前轮内侧转弯点A为：
车辆内侧后轮位置B为：
l1内侧空腔边界线方程：
车辆转弯中心坐标O′为：
对于每个障碍物多边形各点坐标(xki,yki)∈OBJk，当车辆转弯情况为情况1、情况4、情况6或情况7中任意一种情况时，则检测障碍物多边形所有顶点是否满足如下约束：
若有任意一个障碍物多边形的任意一个顶点满足上述约束，则车辆在该转弯点与障碍物发生碰撞，不能通过；
对于每个障碍物多边形各点坐标(xki,yki)∈OBJk，当车辆转弯情况为情况2、情况3、情况5或情况8中任意一种情况时，则检测障碍物多边形所有顶点是否满足如下约束：
若有任意一个障碍物多边形的任意一个顶点满足上述约束，则车辆在该转弯点与障碍物发生碰撞，不能通过。
6.根据权利要求4所述的一种在狭窄环境下的非质点车辆路径规划方法，其特征在于：所述的步骤2.7中寻找次优解的方法具体为：将步骤2.3中首次获得路径上的点(即使用A*算法获得的全局路径)记录下来，选择第一个点的次小代价点作为其下一步选择，然后运行步骤2.3至步骤2.8；若找不到路径，则迭代选择第二个点的次小代价点作为其下一步选择，然后运行步骤2.3至步骤2.8；结束条件：找到可行路径；迭代检查完路径上最后一个点的最大代价点。
CN201910869967.6A 2019-09-16 2019-09-16 一种在狭窄环境下的非质点车辆路径规划方法 Pending CN110530390A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910869967.6A CN110530390A (zh) 2019-09-16 2019-09-16 一种在狭窄环境下的非质点车辆路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910869967.6A CN110530390A (zh) 2019-09-16 2019-09-16 一种在狭窄环境下的非质点车辆路径规划方法

Publications (1)

Publication Number Publication Date
CN110530390A true CN110530390A (zh) 2019-12-03

Family

ID=68668614

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910869967.6A Pending CN110530390A (zh) 2019-09-16 2019-09-16 一种在狭窄环境下的非质点车辆路径规划方法

Country Status (1)

Country Link
CN (1) CN110530390A (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111896004A (zh) * 2020-07-28 2020-11-06 湖南大学 一种狭窄通道车辆轨迹规划方法及系统
CN112747760A (zh) * 2020-12-16 2021-05-04 中国船舶重工集团有限公司第七一0研究所 一种狭水道水面无人平台自主航行航路规划方法及装置
CN112857374A (zh) * 2021-01-07 2021-05-28 华南理工大学 基于当量膨胀的狭窄环境空间的移动机器人可通行性方法
CN113591198A (zh) * 2020-03-11 2021-11-02 黑芝麻智能科技(重庆)有限公司 路径规划方法及计算机可读介质
CN114063618A (zh) * 2021-11-15 2022-02-18 绍兴市上虞区武汉理工大学高等研究院 考虑车辆形状约束的平滑曲线路径路线点生成方法及系统
CN116570439A (zh) * 2023-07-13 2023-08-11 小舟科技有限公司 智能轮椅控制模式切换方法及装置、设备、存储介质

Citations (29)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1470967A1 (en) * 2002-01-28 2004-10-27 Matsushita Electric Works, Ltd. Obstacle detecting/alarming system for vehicle
KR20130037968A (ko) * 2011-10-07 2013-04-17 엘지이노텍 주식회사 주차 보조 장치 및 방법
CN103335658A (zh) * 2013-06-19 2013-10-02 华南农业大学 一种基于弧线路径生成的自主车辆避障方法
CN103697891A (zh) * 2013-12-13 2014-04-02 中联重科股份有限公司 一种工程机械及其入场路径规划装置、系统及方法
CN104411558A (zh) * 2012-07-06 2015-03-11 丰田自动车株式会社 车辆的行驶控制装置
CN105760954A (zh) * 2016-02-15 2016-07-13 南通大学 一种基于改进蚁群算法的泊车系统路径规划方法
CN106043126A (zh) * 2016-07-28 2016-10-26 江苏罗思韦尔电气有限公司 一种重型卡车转向预警辅助装置及预警方法
CN106774310A (zh) * 2016-12-01 2017-05-31 中科金睛视觉科技（北京）有限公司 一种机器人导航方法
CN106843282A (zh) * 2017-03-17 2017-06-13 东南大学 基于m100开发平台的区域完全搜索与避障系统及方法
CN106985815A (zh) * 2017-03-14 2017-07-28 重庆长安汽车股份有限公司 遥控泊车系统及方法
CN107015563A (zh) * 2016-12-29 2017-08-04 北京航空航天大学 移动机器人路径规划方法及装置
CN107145147A (zh) * 2017-04-10 2017-09-08 广州小鹏汽车科技有限公司 一种车辆低速自动驾驶避碰方法及系统
US9764472B1 (en) * 2014-07-18 2017-09-19 Bobsweep Inc. Methods and systems for automated robotic movement
CN107472127A (zh) * 2016-06-07 2017-12-15 重庆工程职业技术学院 基于双目测距的汽车侧面图像检测方法及系统
CN107917711A (zh) * 2017-11-14 2018-04-17 重庆邮电大学 一种基于优化混合蚁群算法的机器人路径规划算法
CN107990903A (zh) * 2017-12-29 2018-05-04 东南大学 一种基于改进a*算法的室内agv路径规划方法
CN108422996A (zh) * 2018-05-10 2018-08-21 江铃汽车股份有限公司 一种集成bsd、sdg功能的泊车系统及自动泊车控制方法
CN108764579A (zh) * 2018-06-01 2018-11-06 成都交大光芒科技股份有限公司 一种基于拥塞控制的仓储多机器人任务调度方法
CN108775902A (zh) * 2018-07-25 2018-11-09 齐鲁工业大学 基于障碍物虚拟膨胀的伴随机器人路径规划方法及系统
CN108955695A (zh) * 2018-08-22 2018-12-07 洛阳中科龙网创新科技有限公司 一种用于农田机器人的全局路径规划方法
CN109059924A (zh) * 2018-07-25 2018-12-21 齐鲁工业大学 基于a*算法的伴随机器人增量路径规划方法及系统
CN109115226A (zh) * 2018-09-01 2019-01-01 哈尔滨工程大学 基于跳点搜索的多机器人冲突避免的路径规划方法
CN109540159A (zh) * 2018-10-11 2019-03-29 同济大学 一种快速完备的自动驾驶轨迹规划方法
CN109552308A (zh) * 2017-09-27 2019-04-02 奥迪股份公司 用于车辆的驾驶辅助设备和方法
CN109668561A (zh) * 2017-10-13 2019-04-23 中兴通讯股份有限公司 一种室内路径规划方法、终端及可读存储介质
CN109720342A (zh) * 2019-01-16 2019-05-07 杭州湘滨电子科技有限公司 一种垂直泊车的路径规划设计方法
CN109798909A (zh) * 2019-02-01 2019-05-24 安徽达特智能科技有限公司 一种全局路径规划的方法
CN109952518A (zh) * 2016-09-09 2019-06-28 克诺尔商用车制动系统有限公司 用于对车辆的车辆驾驶员进行物体的警示的设备及具有这种设备的车辆
CN109945873A (zh) * 2019-04-04 2019-06-28 东南大学 一种用于室内移动机器人运动控制的混合路径规划方法

Patent Citations (29)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1470967A1 (en) * 2002-01-28 2004-10-27 Matsushita Electric Works, Ltd. Obstacle detecting/alarming system for vehicle
KR20130037968A (ko) * 2011-10-07 2013-04-17 엘지이노텍 주식회사 주차 보조 장치 및 방법
CN104411558A (zh) * 2012-07-06 2015-03-11 丰田自动车株式会社 车辆的行驶控制装置
CN103335658A (zh) * 2013-06-19 2013-10-02 华南农业大学 一种基于弧线路径生成的自主车辆避障方法
CN103697891A (zh) * 2013-12-13 2014-04-02 中联重科股份有限公司 一种工程机械及其入场路径规划装置、系统及方法
US9764472B1 (en) * 2014-07-18 2017-09-19 Bobsweep Inc. Methods and systems for automated robotic movement
CN105760954A (zh) * 2016-02-15 2016-07-13 南通大学 一种基于改进蚁群算法的泊车系统路径规划方法
CN107472127A (zh) * 2016-06-07 2017-12-15 重庆工程职业技术学院 基于双目测距的汽车侧面图像检测方法及系统
CN106043126A (zh) * 2016-07-28 2016-10-26 江苏罗思韦尔电气有限公司 一种重型卡车转向预警辅助装置及预警方法
CN109952518A (zh) * 2016-09-09 2019-06-28 克诺尔商用车制动系统有限公司 用于对车辆的车辆驾驶员进行物体的警示的设备及具有这种设备的车辆
CN106774310A (zh) * 2016-12-01 2017-05-31 中科金睛视觉科技（北京）有限公司 一种机器人导航方法
CN107015563A (zh) * 2016-12-29 2017-08-04 北京航空航天大学 移动机器人路径规划方法及装置
CN106985815A (zh) * 2017-03-14 2017-07-28 重庆长安汽车股份有限公司 遥控泊车系统及方法
CN106843282A (zh) * 2017-03-17 2017-06-13 东南大学 基于m100开发平台的区域完全搜索与避障系统及方法
CN107145147A (zh) * 2017-04-10 2017-09-08 广州小鹏汽车科技有限公司 一种车辆低速自动驾驶避碰方法及系统
CN109552308A (zh) * 2017-09-27 2019-04-02 奥迪股份公司 用于车辆的驾驶辅助设备和方法
CN109668561A (zh) * 2017-10-13 2019-04-23 中兴通讯股份有限公司 一种室内路径规划方法、终端及可读存储介质
CN107917711A (zh) * 2017-11-14 2018-04-17 重庆邮电大学 一种基于优化混合蚁群算法的机器人路径规划算法
CN107990903A (zh) * 2017-12-29 2018-05-04 东南大学 一种基于改进a*算法的室内agv路径规划方法
CN108422996A (zh) * 2018-05-10 2018-08-21 江铃汽车股份有限公司 一种集成bsd、sdg功能的泊车系统及自动泊车控制方法
CN108764579A (zh) * 2018-06-01 2018-11-06 成都交大光芒科技股份有限公司 一种基于拥塞控制的仓储多机器人任务调度方法
CN108775902A (zh) * 2018-07-25 2018-11-09 齐鲁工业大学 基于障碍物虚拟膨胀的伴随机器人路径规划方法及系统
CN109059924A (zh) * 2018-07-25 2018-12-21 齐鲁工业大学 基于a*算法的伴随机器人增量路径规划方法及系统
CN108955695A (zh) * 2018-08-22 2018-12-07 洛阳中科龙网创新科技有限公司 一种用于农田机器人的全局路径规划方法
CN109115226A (zh) * 2018-09-01 2019-01-01 哈尔滨工程大学 基于跳点搜索的多机器人冲突避免的路径规划方法
CN109540159A (zh) * 2018-10-11 2019-03-29 同济大学 一种快速完备的自动驾驶轨迹规划方法
CN109720342A (zh) * 2019-01-16 2019-05-07 杭州湘滨电子科技有限公司 一种垂直泊车的路径规划设计方法
CN109798909A (zh) * 2019-02-01 2019-05-24 安徽达特智能科技有限公司 一种全局路径规划的方法
CN109945873A (zh) * 2019-04-04 2019-06-28 东南大学 一种用于室内移动机器人运动控制的混合路径规划方法

Non-Patent Citations (11)

* Cited by examiner, † Cited by third party
Title
HUIQIANG WANG等: "A Dependable Service Path Searching Method in Distributed Virtualized Environment Using Adaptive Bonus-Penalty Micro-Canonical Annealing", 《2015 IEEE 2ND INTERNATIONAL CONFERENCE ON CYBER SECURITY AND CLOUD COMPUTING》 *
MOHAMMED AYOUB JUMAN等: "An integrated path planning system for a robot designed for oil palm plantations", 《TENCON 2017 - 2017 IEEE REGION 10 CONFERENCE》 *
PAUL SCERRI等: "A decentralized approach to space deconfliction", 《2007 10TH INTERNATIONAL CONFERENCE ON INFORMATION FUSION》 *
SANGYOL YOON等: "Recursive Path Planning Using Reduced States for Car-Like Vehicles on Grid Maps", 《IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS》 *
SANGYOL YOON等: "SLPA*: Shape-Aware Lifelong Planning A* for Differential Wheeled Vehicles", 《IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS》 *
刘亚杰等: "狭窄环境中基于几何法的全局路径规划新方法", 《海军工程大学学报》 *
张海鹰等: "超声波自主探路车的转弯办法", 《信息技术与信息化》 *
杨淮清等: "基于动作基元的行走轨迹规划及其在原地转向型机器人中的应用", 《2006中国控制与决策学术年会论文集》 *
胡志远: "基于2D激光雷达的移动机器人关键技术研究", 《中国优秀硕士学位论文全文数据库 (信息科技辑)》 *
郭芙等: "基于鼠标绘制的起重机入场路径规划", 《建设机械技术与管理》 *
黄诚杰等: "改进型A*算法在智能AGV路径规划中的应用", 《莆田学院学报》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113591198A (zh) * 2020-03-11 2021-11-02 黑芝麻智能科技(重庆)有限公司 路径规划方法及计算机可读介质
CN113591198B (zh) * 2020-03-11 2023-09-22 黑芝麻智能科技(重庆)有限公司 路径规划方法及计算机可读介质
CN111896004A (zh) * 2020-07-28 2020-11-06 湖南大学 一种狭窄通道车辆轨迹规划方法及系统
CN112747760A (zh) * 2020-12-16 2021-05-04 中国船舶重工集团有限公司第七一0研究所 一种狭水道水面无人平台自主航行航路规划方法及装置
CN112747760B (zh) * 2020-12-16 2022-10-28 中国船舶重工集团有限公司第七一0研究所 一种狭水道水面无人平台自主航行航路规划方法及装置
CN112857374A (zh) * 2021-01-07 2021-05-28 华南理工大学 基于当量膨胀的狭窄环境空间的移动机器人可通行性方法
CN114063618A (zh) * 2021-11-15 2022-02-18 绍兴市上虞区武汉理工大学高等研究院 考虑车辆形状约束的平滑曲线路径路线点生成方法及系统
CN116570439A (zh) * 2023-07-13 2023-08-11 小舟科技有限公司 智能轮椅控制模式切换方法及装置、设备、存储介质
CN116570439B (zh) * 2023-07-13 2023-09-22 小舟科技有限公司 智能轮椅控制模式切换方法及装置、设备、存储介质

Similar Documents

Publication Publication Date Title
CN110530390A (zh) 一种在狭窄环境下的非质点车辆路径规划方法
Banzhaf et al. The future of parking: A survey on automated valet parking with an outlook on high density parking
CN110018689B (zh) 一种基于动态窗口的多虚拟目标点全局动态路径规划算法
CN109927716B (zh) 基于高精度地图的自主垂直泊车方法
CN105539430B (zh) 一种基于手持终端的人车交互智能泊车方法
CN106541945B (zh) 一种基于icp算法的无人车自动泊车方法
CN110307850A (zh) 航迹推算定位方法及自动泊车系统
CN108088456A (zh) 一种具有时间一致性的无人驾驶车辆局部路径规划方法
CN104819724B (zh) 一种基于gis的无人地面车辆自主行驶辅助系统
US9633143B2 (en) Process and apparatus for generating a three-dimensional swept envelope of a vehicle
CN108597251A (zh) 一种基于车联网的交通路口分布式车辆碰撞预警方法
CN106017486B (zh) 一种面向无人车导航的基于轨迹拐点滤波的地图定位方法
CN105159291B (zh) 一种基于信息物理网的车队智能避障装置及避障方法
CN105043376B (zh) 一种适用于非全向移动车辆的智能导航方法及系统
CN103390355B (zh) 一种基于a-smgcs系统的滑行道冲突检测方法
CN104881025A (zh) 一种地下矿用车辆的反应式导航控制方法
CN113619603B (zh) 一种双阶段自动驾驶车辆调头轨迹规划方法
Zheng et al. RRT based path planning for autonomous parking of vehicle
CN108091176A (zh) 一种汽车主动防碰撞虚拟斑马线控制系统
US20220101534A1 (en) Sidewalk edge finder device, system and method
CN105182971A (zh) 一种室内环境下服务机器人的避障方法
CN113895463A (zh) 一种适用于自动驾驶车辆掉头的路径规划方法
Lee et al. Collision risk assessment of occluded vehicle based on the motion predictions using the precise road map
CN113589685B (zh) 一种基于深度神经网络的挪车机器人控制系统及其方法
Liu et al. Development, testing, and assessment of a kinematic path-following model for towing vehicle systems

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20191203

RJ01 Rejection of invention patent application after publication