CN109917794B - 全局路径规划方法及装置 - Google Patents
全局路径规划方法及装置 Download PDFInfo
- Publication number
- CN109917794B CN109917794B CN201910314226.1A CN201910314226A CN109917794B CN 109917794 B CN109917794 B CN 109917794B CN 201910314226 A CN201910314226 A CN 201910314226A CN 109917794 B CN109917794 B CN 109917794B
- Authority
- CN
- China
- Prior art keywords
- point
- optimal
- edge
- cost
- queue
- 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.)
- Active
Links
Images
Abstract
本发明提供了一种全局路径规划方法,包括:得到多个包括起点和终点的采样点并从中顶点;当顶点队列中的最优点的期望总代价小于边队列中最优边的期望总代价时,对最优点进行拓展,直到拓展后的顶点队列中的最优点的期望总代价不小于边队列中的最优边的期望总代价;确定起点至终点的连通路径;计算连通路径的已付代价并作为椭圆的长轴;以起点和终点的距离作为椭圆的焦距;根据长轴和焦距,确定短轴;根据长轴、焦距和短轴,确定搜索空间;计算搜索空间的位置参数;根据位置参数,对搜索空间进行位置变换;在进行位置变换后的搜索空间内重新获取采样点,迭代的在目标搜索空间内得到目标路径。由此,提高了计算速度,且满足运动学约束。
Description
技术领域
本发明涉及数据处理技术领域,尤其涉及一种全局路径规划方法及装置。
背景技术
随着人工智能技术的发展,自动驾驶技术成为汽车工业发展的新方向。自动驾驶车辆能够提升交通环境安全、充分利用交通资源,减少排放。自动驾驶车辆的发展将全面带动社会生产力的发展。
现有的全局路径规划算法一般分为两大类:分辨率完备型算法和概率完备型算法。
分辨率完备型算法是将状态空间进行离散化处理,在离散处理结果的基础上进行路径搜索,如果状态空间中存在解,一般情况下分辨率完备法能够在一定时间限制内搜索到一个比较接近最优解的结果。搜索结果与最优解的接近程度取决于离散化预处理过程中所取的分辨率。一般情况下,分辨率越高,越接近最优解,但是提高分辨率会明显降低搜索速度,然而过低的分辨率有可能搜不到解。对于分辨率完备的局部路径规划算法,分辨率是一个非常重要的调节参数。典型的分辨率完备法有:单元分解法,迪杰斯特拉(Dijkstra)算法,A*,D*,泰森多边形(Voronoi)图法,人工势场法等。
概率完备型算法是基于某个启发函数随机的在状态空间进行采样,如果状态空间中存在解,那么此类方法可以在有限时间内搜索到解。但是这个解不一定是最优解,而且结果不具备一致性。搜索速度的快慢与状态空间的复杂度有关,状态空间越复杂,搜索花费的时间越长。一般情况下,搜索得到的解与最优解相差较大,通过此类方法得到解的局部路径规划算法都要设计优化器,使结果靠近最优解。典型的概率完备型算法有:概率路线图法(Probabilistic Roadmap Method,PRM),快速随机树法(Rapid-exploration RandomTree,RRT)。
现有技术的路径规划方法存在的问题是:
(1)分辨率完备法一般会遵循“路径最短原则”,生成的路径不一定满足无人驾驶车辆的运动学约束。
(2)现有路径搜索算法中,大多不对路径点的朝向进行约束。
(3)基于分辨率完备性的算法计算速度还有一定的提升空间。
发明内容
本发明实施例的目的是提供一种全局路径规划方法及装置,以解决现有计算中存在的问题。
为解决上述问题,第一方面,本发明提供了一种全局路径规划方法,所述方法包括:
根据车辆行驶的起点和终点,得到多个采样点;所述采样点包括起点和终点;
基于距离变换地图,从所述采样点中选取采样点作为顶点;
将能够连接到起点的顶点添加至顶点队列中;
当所述顶点队列中的最优点的期望总代价小于边队列中最优边的期望总代价时,对所述最优点进行拓展,直到拓展后的顶点队列中的最优点的期望总代价不小于边队列中的最优边的期望总代价;所述最优点为期望总代价最小的点,所述最优边为期望总代价最小的边;
根据拓展的结果,得到从所述起点至所述终点的连通路径;
计算所述连通路径的已付代价,并将所述已付代价确定为椭圆的长轴;
以所述起点和终点的距离作为椭圆的焦距;
根据所述椭圆的长轴和焦距,确定所述椭圆的短轴;
根据所述长轴、焦距和短轴,确定搜索空间;
计算所述搜索空间的位置参数;
根据所述位置参数,对所述搜索空间进行位置变换;
在进行位置变换后的搜索空间内重新获取采样点,迭代的在目标搜索空间内得到目标路径。
在一种可能的实现方式中,所述将能够连接到起点的顶点添加至顶点队列中,具体包括:
将所述起点加入到搜索树的顶点中;
将所述搜索树中的顶点添加到顶点队列中。
在一种可能的实现方式中,所述方法还包括:
在一种可能的实现方式中,所述当所述顶点队列中的最优点的期望总代价小于边队列中最优边的期望总代价时,对所述最优点进行拓展之后,还包括:
在一种可能的实现方式中,所述计算所述搜索空间的位置参数,具体包括:
其中,xmid为所述椭圆的中心,x′为状态x变换后的状态,θ为搜索空间的旋转角度。
第二方面,本发明提供了一种全局路径规划装置,所述装置包括:
采样单元,所述采样单元用于根据车辆行驶的起点和终点,得到多个采样点;所述采样点包括起点和终点;
选取单元,所述选取单元用于基于距离变换地图,从所述采样点中选取采样点作为顶点;
添加单元,所述添加单元用于将能够连接到起点的顶点添加至顶点队列中;
拓展单元,所述拓展单元用于当所述顶点队列中的最优点的期望总代价小于边队列中最优边的期望总代价时,对所述最优点进行拓展,直到拓展后的顶点队列中的最优点的期望总代价不小于边队列中的最优边的期望总代价;所述最优点为期望总代价最小的点,所述最优边为期望总代价最小的边;
确定单元,所述确定单元用于根据拓展的结果,得到从所述起点至所述终点的连通路径;
计算单元,所述计算单元用于计算所述连通路径的已付代价,并将所述已付代价确定为椭圆的长轴;
所述确定单元还用于,以所述起点和终点的距离作为椭圆的焦距;
所述确定单元还用于,根据所述椭圆的长轴和焦距,确定所述椭圆的短轴;
所述确定单元还用于,根据所述长轴、焦距和短轴,确定搜索空间;
所述计算单元还用于,计算所述搜索空间的位置参数;
位置变换单元,所述位置变换单元用于根据所述位置参数,对所述搜索空间进行位置变换;
迭代单元,所述迭代单元用于在进行位置变换后的搜索空间内重新获取采样点,迭代的在目标搜索空间内得到目标路径。
在一种可能的实现方式中,所述添加单元具体用于:
将所述起点加入到树顶点;
将所述树顶点中的顶点添加到顶点队列中。
在一种可能的实现方式中,所述计算单元具体用于:
在一种可能的实现方式中,所述装置还包括评价单元;
在一种可能的实现方式中,所述计算单元具体用于:
其中,xmid为所述椭圆的中心,x′为状态x变换后的状态,θ为搜索空间的旋转角度。
第三方面,本发明提供了一种设备,包括存储器和处理器,所述存储器用于存储程序,所述处理器用于执行第一方面任一所述的方法。
第四方面,本发明提供了一种包含指令的计算机程序产品,当所述计算机程序产品在计算机上运行时,使得所述计算机执行如第一方面任一所述的方法。
第五方面,本发明提供了一种计算机可读存储介质,所述计算机可读存储介质上存储有计算机程序,所述计算机程序被处理器执行时实现如第一方面任一所述的方法。
通过应用本发明提供的全局路径规划方法及装置,具有以下技术效果:
(1)提升概率完备型全局路径规划算法的计算速度;
(2)计算过程中进行一定的约束,使结果尽可能的满足无人驾驶车辆的运动学约束;
(3)逻辑清晰,便于计算机实现。
附图说明
图1为本发明实施例一提供的全局路径规划方法流程示意图;
图2A为距离变换前的原始地图;
图2B为距离变换后的结果;
图3为低密度采样下的路径搜索;
图4为使用启发函数搜索路径;
图5为缩小搜索空间;
图6为路径优化处理;
图7为搜索空间示意图;
图8为本发明实施例二提供的全局路径规划装置结构示意图。
具体实施方式
下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是,此处所描述的具体实施例仅仅用于解释相关发明,而非对该发明的限定。另外还需要说明的是,为便于描述,附图中仅示出了与有关发明相关的部分。
需要说明的是,在不冲突的情况下,本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。
图1为本发明实施例一提供的全局路径规划方法流程示意图。该方法应用在自动驾驶领域中,该方法的执行主体为车辆的计算处理单元,该计算处理单元可以是车辆控制单元,如图1所示,该方法包括以下步骤:
步骤101,根据车辆行驶的起点、终点,得到多个采样点;采样点包括起点和终点。
其中,服务器可以接收用户终端发送的行程的终点,然后将终点发送给车辆控制单元。起点可以是车辆的当前位置,可以通过全球定位系统(Global Positioning System,GPS)得到车辆的当前位置。
可以从起点至终点范围内,随机采样多个点,得到多个采样点。
步骤102,基于距离变换地图,从采样点中选取采样点作为顶点。
图2A和2B为距离变换过程。图2A为距离变换前的原始地图,该地图是一个二值图,其中黑色圆位障碍物,白色为可通行区域。图2B为距离变换后的结果,通过距离变换,将二值图转换为了灰度图,即距离变换地图,在2B中,某处的灰度值表示了该位置距离不可同行区域的最短距离。若某像素的灰度等于0,即表示当前位置所表示的位置为不可通行区域。
步骤103,将能够连接到起点的顶点添加至顶点队列中。
其中,能够连接到起点的顶点,即已经存入到搜索树V中的顶点,搜索树由树顶点和顶点关系组成,在初始状态下,搜索树中的顶点只有起点状态。搜索树顶点队列中的顶点以及顶点与顶点关系构成的边E是已经经过碰撞检测的。
其中,可以通过以下方法进行碰撞检测:
(1)从待检测路径的起点开始到终点为止,每隔2.5米进行一次采样;
(2)获取采样点的距离变换值(如上文所属,距离变换值表示当前位置距离不可同行区域的最短距离;
(3)如果任一采样点的距离变换值小于2.5(当前采样距离不可同行区域的最短距离小于2.5米)则认为该路径将使车辆发生碰撞;
(4)如果所有采样点的距离变换值均大于2.5(当前路径上所有采样点距离不可同行区域的最小距离都大于2.5米)则认为该路径将不会使车辆发生碰撞;
(5)上述2.5米并非固定值,可根据需求自行修改。
步骤103具体包括:将起点加入到搜索树的顶点中,将搜索树中的顶点添加到顶点队列中。
步骤104,当所述顶点队列中的最优点的期望总代价小于边队列中最优边的期望总代价时,对所述最优点进行拓展,直到拓展后的顶点队列中的最优点的期望总代价不小于边队列中的最优边的期望总代价;所述最优点为期望总代价最小的点,所述最优边为期望总代价最小的边。
进一步的,当顶点队列中的最优点的期望总代价不小于边队列中的最优边的期望总代价时,即跳出算法1的行7、8。
具体的,对于距离变换地图中某个节点(即顶点)与以该节点为圆心,取r为半径的圆内的其他节点与之建立连接关系。可以以给定的初始状态xstart作为空间树的根节点进行搜索。目标状态xgoal作为采样点的某个常采样点存入变量Xsamples。RGG图形的边由边队列QE进行维护,顶点由顶点队列QV进行维护。算法1如表1所示:
表1
其中,表1中的地图信息,为距离变换地图。
具体的,批处理过程包括搜索空间修整、采样、初始化运算队列三个操作(算法1,行4~6)。
修整算法如算法2所示。搜索空间的修整指的是采样点Xsamples和空间树将根据的值进行的修整,剔除那些不会使结果向最优解收敛的采样点和边的过程。如果搜索树还未搜索到目标位置xgoal那么此时不需要对Xsamples和进行任何修整。
表2
采样操作根据起点和终点的相对位置以及目标点的已付代价进行。伪代码如算法3所示。如果空间树还未找到从起始点到终点的任何一条路径,那么传入的已付代价(算法1,行5)。如图3、4所示,此时采样为全地图范围采样(算法3,行1~6),其中rand(map)表示在地图范围map内随机进行采样得到状态x(算法3,行3)。如果x通过了基于距离变换的碰撞检测,即x∈Xfree,并且在黑域内以x为圆心,半径r′以内的点的集合为空,那么将状态x添加到采样序列Xsamples中,并且将该点存入k-d树内,本文取r′=0.5m(算法3,行4,5),其终止条件为在原Xsamples的基础上又采样了若干个采样点,本申请可以将终止条件定为100个点。
表3
其中,代表从x1到x2的所需代价,即估计代价,也可称为直接距离。c(x1,x2)代表从x1到x2的真实代价,需要在x1到x2之间每隔1m采一个点进行碰撞检测,如果从x1到x2的连线上任一采样点发生了碰撞c(x1,x2)返回正无穷,这是唯一碰撞检测的地方。
PE值越小则认为该边越优。相同的,QV也按照一定的规则从优到劣进行排序,不同于QE的评价,QV的评价依据每个节点的代价:
PV值越小则认为该节点越优。函数BestQueueValue(QE)和BestQueueValue(QV)(算法1,行7)分别表示的边队列中的最优评价值和顶点队列里的最优评价值。函数BestInQueue(QE)和BestInQueue(QV)分别返回评价最优的边和评价最优的点。对边顶点队列QV中的顶点,如果存在某个顶点的期望总代价低于边队列中最优的边的期望总代价,即过该顶点有望获得更优解,那么将该对QV中的最优顶点进行拓展,得到若干条边,将这些边存入边队列中以供备用(算法1,行7,8)。顶点拓展过程如算法4所示。
表4
函数ExpandVertex(v)将从顶点队列QV中移除一个顶点(算法4,行1)。Xsamples以“r-临近”的方式构建随机几何图形(RGG)。取以状态v为圆心,半径r范围内的所有状态Xnear(算法4,行2)分别与状态v共同构成新的边,如果过新生成的边的路径有望获得更优解,则将该边加入到边队列QE中(算法4,行3)。如果传入的顶点为当前批次新生成的顶点(算法4,行4),那么就取以传入点v为圆心,r为半径范围内的所有顶点为临近顶点Vnear。如果传入状态与临近顶点组成的边有望使全局结果更优并且使到达临近顶点的局部解更优,则将该边加入到边队列QE中(算法4,行6)。
更新完两个队列后,为了尽快找到当前批次的最优解,根据贪婪原则应选取边队列中最优的边进行后继评估(算法1,行11~21),并将要评估的边从边队列中删除,以防止重复评估同一条边(算法1,行9,10)。需要说明的是,在边选择的过程中,每个批次一个节点只能被拓展一次,一条边也只能被评估一次。
边的处理过程包括边的评估过程(算法1,行11~14)和边的更新过程(算法1,行15~21)。通过评估能够通过低代价的计算过程评判某条边的优劣,之后有必要才进行下一步的操作,省去了大量复杂的操作,使结果快速收敛到最优解。每次评估的对象都是边队列中最优的边(x1,x2)。评估过程分三步进行。
第一步对边进行粗评,判断该边是否有望使路径更优,粗评代价为当前边起点的已付代价、当前边的期望代价与当前边到终点的期望未付代价的和(算法1,行11),当未找到任何一条路径的时候此时粗评总是能通过的。如果找到了某条可通行路径,粗评代价如果低于当前终点的已付代价则进行进一步的评估。否则,在QE和QV中都不会再存在有望使结果更优的元素,此时将QE和QV清空,进行下一次批处理过程(算法1,行21)。
第二步对边进行约束评估,判断当前边是否满足约束条件,评估代价为当前边起点的已付代价的估计、当前边的约束代价与当前边终点的期望未付代价的和(算法1,行12)。约束代价包含两部分:运动约束与碰撞约束。运动约束为了限制空间树中边生长的方向,防止出现过小转向半径甚至折返路径的情况。令当前边起点在空间树内的父节点为x0,那么可以获得两个向量v1(x0,x1)和v2(x1,x2),如果v1和v2两向量构成的夹角<v1,v2>大于某个限定角度δ,则认为转向角度过大,当前边的约束代价直接置为正无穷。其中δ是一个可调整的值,由于最终结果还会经过两次优化,并且在路径跟踪的过程中参考路径仅作为参考,车辆会根据自身状态与周围环境进行动态局部路径规划,所以δ的约束可以是一个松弛的约束,本文取δ=45°;碰撞约束,顾名思义,需要对当前边是否会与不可通行区域发生发生干涉进行检测。同样的,由于全局路径仅作为参考路径,不必也不值得进行非常详细的碰撞检测,只需在该边上每隔1米取一个点,对该点进行粗检即可。粗检用到了距离变换地图,如果距离变换值小于某个阈值则认为该点会发生碰撞,此时该边的约束代价为正无穷。如果检测的边即满足运动约束又满足碰撞约束,则约束代价值为该边的长度。
第三步对待检测边终点的已付代价进行评估,判断当前边是否能使空间树更新节点关系。评估代价为当前边起点的已付代价与当前边的期望代价的和(算法1,行13)。如果需要进行第三步的检测,证明该边已经通过了前两步的检测,该边即满足运动约束又满足碰撞约束,此时当前边的期望代价即可认为是真实代价。如果代价值低于空间树中当前边终点状态x2的已付代价则表明通过状态x1到达状态x2为更优的路径,可以对空间树进行更新。
当三次检测都通过后,将x2从采样集合中移除(算法1,行15)并将x2归入搜索树中(算法1,行16,17,18)。
其中,归入搜索树包括三部分:
首先,将x2归入搜索树顶点V;
然后,将搜索树边E中原能连接到x2的边删除;
最后,将边,即连接关系(x1,x2)归入搜索树边E,同时,将x2归入顶点队列Qv中(算法1,行16),并将搜索树中所有有望通过x2得到更优解的边归入边队列QE中(算法1,行19)。
边的更新过程即为空间树的更新过程(算法1,行14~19)。如果当前边的终点状态x2不在空间树内,表明该点是通过拓展采样点获得的(算法4,行2,3),此时需要将状态x2从采样序列Xsamples中剔除,并将该状态存入顶点队列QV和空间树中,其父节点为x1(算法1,行15,16)。无论状态x2是通过拓展采样点获得或是通过拓展树节点获得,只要通过三步评估,都将该边存入空间树即对空间树状态的更新(算法1,行17,18)。之后将边队列QE中到达状态x2期望代价高于状态x2更新后的已付代价的边进行过滤,减少不必要的操作(算法1,行19)。通过不断的迭代对空间树的状态进行更新,直至边队列QE中不再存在有望使结果更优的边,则一个处理批次完成。步骤105,根据拓展的结果,得到从所述起点至所述终点的连通路径。
具体的,经过多次拓展后,通过搜索树的顶点以及边可以得到从起点至终点的连通路径。
步骤106,计算连通路径的已付代价,并将已付代价确定为椭圆的长轴。
步骤107,以起点和终点的距离作为椭圆的焦距。
步骤108,根据椭圆的长轴和焦距,确定椭圆的短轴。
步骤109,根据长轴、焦距和短轴,确定搜索空间。
步骤110,计算搜索空间的位置参数。
如果空间树已经找到了从起点到终点的某条路径,那么传入的已付代价(算法1,行5)。此时采样过程会收缩采样空间以期在更小的范围内找到更优的解(算法3,行7~15)。如图7所示为搜索空间示意图,假设已找到的路径已付代价为2a∈R<∞。路径过状态x,那么从起点状态到终点状态的当前最短路径为如果取状态xstart和状态xgoal为椭圆的焦点,同时长轴取短轴取那么可以轻易得知,如果当前解不是最优解,最优解必然落在该椭圆内(算法3,行9)。确定了搜索空间的几何参数并不足以确定搜索范围,还需确定搜索空间的位置参数,即该椭圆在空间中的位置。令θ表示搜索空间的旋转角度,xmid为起始状态xstart和终止状态xgoal的中点(算法3,行10)。取单位乐贝格度量球ζn内的任意状态为x(算法3,行11)。
步骤111,根据位置参数,对搜索空间进行位置变换。
具体的,对状态x进行位置变换操作,变换操作如下(算法3,行12):
通过该变换即可将单位随机状态x变换到收缩采样空间内,如图5和图6所示。如果状态x′通过了基于距离变换的碰撞检测,且在黑域内以x′为圆心,半径r′以内的点的集合为空,则将状态x′添加到采样序列Xsamples内,内(算法3,行13,14)。
步骤112,在进行位置变换后的搜索空间内重新获取采样点,迭代的在目标搜索空间内得到目标路径。
在缩小搜索空间后,继续执行步骤102至步骤111,以进一步的缩小搜索空间,从而在缩小的搜索空间内,重新采样进行迭代计算,得到目标路径。
通过应用本发明提供的全局路径规划方法及装置,具有以下技术效果:
(1)提升概率完备型全局路径规划算法的计算速度;
(2)计算过程中进行一定的约束,使结果尽可能的满足无人驾驶车辆的运动学约束;
(3)逻辑清晰,便于计算机实现。
图8为本发明实施例二提供的全局路径规划装置结构示意图,该全局路径规划装置应用在全局路径规划方法中,如图8所示,该全局路径规划装置800包括:采样单元801、选取单元802、添加单元803、拓展单元804、确定单元805、计算单元806、位置变换单元807、迭代单元808和评价单元809。
采样单元801用于根据车辆行驶的起点和终点,得到多个采样点;采样点包括起点和终点;
选取单元802用于基于距离变换地图,从采样点中选取采样点作为顶点;
添加单元803用于将能够连接到起点的顶点添加至顶点队列中;
拓展单元804用于当所述顶点队列中的最优点的期望总代价小于边队列中最优边的期望总代价时,对所述最优点进行拓展,直到拓展后的顶点队列中的最优点的期望总代价不小于边队列中的最优边的期望总代价;所述最优点为期望总代价最小的点,所述最优边为期望总代价最小的边;
确定单元805用于根据拓展的结果,得到从所述起点至所述终点的连通路径;
计算单元806用于计算最短路径的已付代价,并将已付代价确定为椭圆的长轴;
确定单元805还用于,以起点和终点的距离作为椭圆的焦距;
确定单元805还用于,根据椭圆的长轴和焦距,确定椭圆的短轴;
确定单元805还用于,根据长轴、焦距和短轴,确定搜索空间;
计算单元806还用于,计算搜索空间的位置参数;
位置变换单元807用于根据位置参数,对搜索空间进行位置变换;
迭代单元808用于在进行位置变换后的搜索空间内重新获取采样点,迭代的在目标搜索空间内得到目标路径。
进一步的,所述添加单元803还用于,将所述起点加入到搜索树的顶点中;将所述搜索树中的顶点添加到顶点队列中。
进一步的,计算单元806具体用于:
进一步的,计算单元806具体用于:
其中,xmid为椭圆的中心,x′为状态x变换后的状态,θ为搜索空间的旋转角度。
本发明实施例三提供了一种设备,包括存储器和处理器,存储器用于存储程序,存储器可通过总线与处理器连接。存储器可以是非易失存储器,例如硬盘驱动器和闪存,存储器中存储有软件程序和设备驱动程序。软件程序能够执行本发明实施例提供的上述方法的各种功能;设备驱动程序可以是网络和接口驱动程序。处理器用于执行软件程序,该软件程序被执行时,能够实现本发明实施例一提供的方法。
本发明实施例四提供了一种包含指令的计算机程序产品,当计算机程序产品在计算机上运行时,使得计算机执行本发明实施例一提供的方法。
本发明实施例五提供了一种计算机可读存储介质,计算机可读存储介质上存储有计算机程序,计算机程序被处理器执行时实现本发明实施例一提供的方法。
专业人员应该还可以进一步意识到,结合本文中所公开的实施例描述的各示例的单元及算法步骤,能够以电子硬件、计算机软件或者二者的结合来实现,为了清楚地说明硬件和软件的可互换性,在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行,取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能,但是这种实现不应认为超出本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块,或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
以上的具体实施方式,对本发明的目的、技术方案和有益效果进行了进一步详细说明,所应理解的是,以上仅为本发明的具体实施方式而已,并不用于限定本发明的保护范围,凡在本发明的精神和原则之内,所做的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (8)
1.一种全局路径规划方法,其特征在于,所述方法包括:
根据车辆行驶的起点和终点,得到多个采样点;所述采样点包括起点和终点;
基于距离变换地图,从所述采样点中选取采样点作为顶点;
将能够连接到起点的顶点添加至顶点队列中;
当所述顶点队列中的最优点的期望总代价小于边队列中最优边的期望总代价时,对所述最优点进行拓展,直到拓展后的顶点队列中的最优点的期望总代价不小于边队列中的最优边的期望总代价;所述最优点为期望总代价最小的点,所述最优边为期望总代价最小的边;
根据拓展的结果,得到从所述起点至所述终点的连通路径;
计算所述连通路径的已付代价,并将所述已付代价确定为椭圆的长轴;
以所述起点和终点的距离作为椭圆的焦距;
根据所述椭圆的长轴和焦距,确定所述椭圆的短轴;
根据所述长轴、焦距和短轴,确定搜索空间;
计算所述搜索空间的位置参数;
根据所述位置参数,对所述搜索空间进行位置变换;
在进行位置变换后的搜索空间内重新获取采样点,迭代的在目标搜索空间内得到目标路径;
其中,所述计算所述搜索空间的位置参数,具体包括:
其中,xmid为所述椭圆的中心,x′为状态x变换后的状态,θ为搜索空间的旋转角度,a为所述椭圆的长轴,b为所述椭圆的短轴。
2.根据权利要求1所述的方法,其特征在于,所述将能够连接到起点的顶点添加至顶点队列中,具体包括:
将所述起点加入到搜索树的顶点中;
将所述搜索树中的顶点添加到顶点队列中。
5.一种全局路径规划装置,其特征在于,所述装置包括:
采样单元,所述采样单元用于根据车辆行驶的起点和终点,得到多个采样点;所述采样点包括起点和终点;
选取单元,所述选取单元用于基于距离变换地图,从所述采样点中选取采样点作为顶点;
添加单元,所述添加单元用于将能够连接到起点的顶点添加至顶点队列中;
拓展单元,所述拓展单元用于当所述顶点队列中的最优点的期望总代价小于边队列中最优边的期望总代价时,对所述最优点进行拓展,直到拓展后的顶点队列中的最优点的期望总代价不小于边队列中的最优边的期望总代价;所述最优点为期望总代价最小的点,所述最优边为期望总代价最小的边;
确定单元,所述确定单元用于根据拓展的结果,得到从所述起点至所述终点的连通路径;
计算单元,所述计算单元用于计算所述连通路径的已付代价,并将所述已付代价确定为椭圆的长轴;
所述确定单元还用于,以所述起点和终点的距离作为椭圆的焦距;
所述确定单元还用于,根据所述椭圆的长轴和焦距,确定所述椭圆的短轴;
所述确定单元还用于,根据所述长轴、焦距和短轴,确定搜索空间;
所述计算单元还用于,计算所述搜索空间的位置参数;
位置变换单元,所述位置变换单元用于根据所述位置参数,对所述搜索空间进行位置变换;
迭代单元,所述迭代单元用于在进行位置变换后的搜索空间内重新获取采样点,迭代的在目标搜索空间内得到目标路径;
其中,计算单元具体用于:
其中,xmid为椭圆的中心,x′为状态x变换后的状态,θ为搜索空间的旋转角度,a为所述椭圆的长轴,b为所述椭圆的短轴。
6.根据权利要求5所述的装置,其特征在于,所述添加单元具体用于:
将所述起点加入到树顶点;
将所述树顶点中的顶点添加到顶点队列中。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910314226.1A CN109917794B (zh) | 2019-04-18 | 2019-04-18 | 全局路径规划方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910314226.1A CN109917794B (zh) | 2019-04-18 | 2019-04-18 | 全局路径规划方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109917794A CN109917794A (zh) | 2019-06-21 |
CN109917794B true CN109917794B (zh) | 2022-02-18 |
Family
ID=66977804
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910314226.1A Active CN109917794B (zh) | 2019-04-18 | 2019-04-18 | 全局路径规划方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109917794B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112650252A (zh) * | 2020-12-25 | 2021-04-13 | 珠海市一微半导体有限公司 | 用于搜索起始清扫位置的寻径代价获取方法及芯片 |
Citations (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5911767A (en) * | 1994-10-04 | 1999-06-15 | Garibotto; Giovanni | Navigation system for an autonomous mobile robot |
EP2338029A1 (en) * | 2008-10-24 | 2011-06-29 | Gray & Company, Inc. | Control and systems for autonomously driven vehicles |
CN102420392A (zh) * | 2011-07-30 | 2012-04-18 | 山东鲁能智能技术有限公司 | 基于磁导航的变电站巡检机器人全局路径规划方法 |
CN103605368A (zh) * | 2013-12-04 | 2014-02-26 | 苏州大学张家港工业技术研究院 | 一种动态未知环境中路径规划方法及装置 |
CN106292673A (zh) * | 2016-09-29 | 2017-01-04 | 深圳大学 | 一种路径优化方法及系统 |
CN106441303A (zh) * | 2016-09-30 | 2017-02-22 | 哈尔滨工程大学 | 一种基于可搜索连续邻域a*算法的路径规划方法 |
CN106598055A (zh) * | 2017-01-19 | 2017-04-26 | 北京智行者科技有限公司 | 一种智能车局部路径规划方法及其装置、车辆 |
CN107702716A (zh) * | 2017-08-31 | 2018-02-16 | 广州小鹏汽车科技有限公司 | 一种无人驾驶路径规划方法、系统和装置 |
CN108875998A (zh) * | 2018-04-20 | 2018-11-23 | 北京智行者科技有限公司 | 一种自动驾驶车辆规划方法和系统 |
CN108955695A (zh) * | 2018-08-22 | 2018-12-07 | 洛阳中科龙网创新科技有限公司 | 一种用于农田机器人的全局路径规划方法 |
CN109002041A (zh) * | 2018-08-09 | 2018-12-14 | 北京智行者科技有限公司 | 一种车辆避障方法 |
CN109508016A (zh) * | 2018-12-26 | 2019-03-22 | 北京工商大学 | 水质采样巡航船路径规划最优化方法 |
CN109542117A (zh) * | 2018-10-19 | 2019-03-29 | 哈尔滨工业大学(威海) | 基于改进rrt的水下航行器滚动规划算法 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP4661838B2 (ja) * | 2007-07-18 | 2011-03-30 | トヨタ自動車株式会社 | 経路計画装置及び方法、コスト評価装置、並びに移動体 |
WO2015008032A1 (en) * | 2013-07-15 | 2015-01-22 | Bae Systems Plc | Route planning |
US9524647B2 (en) * | 2015-01-19 | 2016-12-20 | The Aerospace Corporation | Autonomous Nap-Of-the-Earth (ANOE) flight path planning for manned and unmanned rotorcraft |
-
2019
- 2019-04-18 CN CN201910314226.1A patent/CN109917794B/zh active Active
Patent Citations (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5911767A (en) * | 1994-10-04 | 1999-06-15 | Garibotto; Giovanni | Navigation system for an autonomous mobile robot |
EP2338029A1 (en) * | 2008-10-24 | 2011-06-29 | Gray & Company, Inc. | Control and systems for autonomously driven vehicles |
CN102420392A (zh) * | 2011-07-30 | 2012-04-18 | 山东鲁能智能技术有限公司 | 基于磁导航的变电站巡检机器人全局路径规划方法 |
CN103605368A (zh) * | 2013-12-04 | 2014-02-26 | 苏州大学张家港工业技术研究院 | 一种动态未知环境中路径规划方法及装置 |
CN106292673A (zh) * | 2016-09-29 | 2017-01-04 | 深圳大学 | 一种路径优化方法及系统 |
CN106441303A (zh) * | 2016-09-30 | 2017-02-22 | 哈尔滨工程大学 | 一种基于可搜索连续邻域a*算法的路径规划方法 |
CN106598055A (zh) * | 2017-01-19 | 2017-04-26 | 北京智行者科技有限公司 | 一种智能车局部路径规划方法及其装置、车辆 |
CN107702716A (zh) * | 2017-08-31 | 2018-02-16 | 广州小鹏汽车科技有限公司 | 一种无人驾驶路径规划方法、系统和装置 |
CN108875998A (zh) * | 2018-04-20 | 2018-11-23 | 北京智行者科技有限公司 | 一种自动驾驶车辆规划方法和系统 |
CN109002041A (zh) * | 2018-08-09 | 2018-12-14 | 北京智行者科技有限公司 | 一种车辆避障方法 |
CN108955695A (zh) * | 2018-08-22 | 2018-12-07 | 洛阳中科龙网创新科技有限公司 | 一种用于农田机器人的全局路径规划方法 |
CN109542117A (zh) * | 2018-10-19 | 2019-03-29 | 哈尔滨工业大学(威海) | 基于改进rrt的水下航行器滚动规划算法 |
CN109508016A (zh) * | 2018-12-26 | 2019-03-22 | 北京工商大学 | 水质采样巡航船路径规划最优化方法 |
Non-Patent Citations (1)
Title |
---|
基于功能区域的反舰导弹逆向航路规划;刘钢,等;《系统工程与电子技术》;20110430;第799-805页 * |
Also Published As
Publication number | Publication date |
---|---|
CN109917794A (zh) | 2019-06-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109540142B (zh) | 一种机器人定位导航的方法、装置、计算设备 | |
CN109764886B (zh) | 一种路径规划方法 | |
US10466058B2 (en) | Navigation for vehicles | |
CN108519094B (zh) | 局部路径规划方法及云处理端 | |
WO2022056770A1 (zh) | 一种路径规划方法和路径规划装置 | |
CN108332758B (zh) | 一种移动机器人的走廊识别方法及装置 | |
JP7316310B2 (ja) | 測位方法、装置、コンピューティングデバイス、およびコンピュータ可読記憶媒体 | |
JP7297172B2 (ja) | 物体の拡大状態を追跡するためのシステムおよび方法 | |
CN109916421B (zh) | 路径规划方法及装置 | |
CN111680747B (zh) | 用于占据栅格子图的闭环检测的方法和装置 | |
CN111121812B (zh) | 一种路径优化方法、电子设备及存储介质 | |
CN109341698B (zh) | 一种移动机器人的路径选择方法及装置 | |
CN114111774B (zh) | 车辆的定位方法、系统、设备及计算机可读存储介质 | |
JP2015179514A (ja) | ターゲットオブジェクトの動きパラメーターを予測する方法及び装置 | |
KR20180092960A (ko) | 고속 탐색 랜덤화 피드백 기반의 모션 계획 | |
Rhinehart et al. | Contingencies from observations: Tractable contingency planning with learned behavior models | |
CN109917794B (zh) | 全局路径规划方法及装置 | |
US11914023B2 (en) | System and method for tracking an expanded state of a moving object using a compound measurement model | |
CN113139696B (zh) | 一种轨迹预测模型构建方法及轨迹预测方法、装置 | |
US11662731B2 (en) | Systems and methods for controlling a robot | |
CN112577500A (zh) | 定位与地图构建方法、装置、机器人及计算机存储介质 | |
CN116679698A (zh) | 用于车辆的自动驾驶方法、装置、车辆、设备和介质 | |
CN115562290A (zh) | 一种基于a星惩罚控制优化算法的机器人路径规划方法 | |
Xu et al. | TrafficEKF: A learning based traffic aware extended Kalman filter | |
CN110728359A (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 | ||
CP01 | Change in the name or title of a patent holder |
Address after: B4-006, maker Plaza, 338 East Street, Huilongguan town, Changping District, Beijing 100096 Patentee after: Beijing Idriverplus Technology Co.,Ltd. Address before: B4-006, maker Plaza, 338 East Street, Huilongguan town, Changping District, Beijing 100096 Patentee before: Beijing Idriverplus Technology Co.,Ltd. |
|
CP01 | Change in the name or title of a patent holder |