CN104933228A - 基于速度障碍的无人车实时轨迹规划方法 - Google Patents

基于速度障碍的无人车实时轨迹规划方法 Download PDF

Info

Publication number
CN104933228A
CN104933228A CN201510278572.0A CN201510278572A CN104933228A CN 104933228 A CN104933228 A CN 104933228A CN 201510278572 A CN201510278572 A CN 201510278572A CN 104933228 A CN104933228 A CN 104933228A
Authority
CN
China
Prior art keywords
speed
rightarrow
node
moment
collision
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
CN201510278572.0A
Other languages
English (en)
Other versions
CN104933228B (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.)
Xian Jiaotong University
Original Assignee
Xian Jiaotong 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 Xian Jiaotong University filed Critical Xian Jiaotong University
Priority to CN201510278572.0A priority Critical patent/CN104933228B/zh
Publication of CN104933228A publication Critical patent/CN104933228A/zh
Application granted granted Critical
Publication of CN104933228B publication Critical patent/CN104933228B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • YGENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02TCLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00Road transport of goods or passengers
    • Y02T10/80Technologies aiming to reduce greenhouse gasses emissions common to all road transportation technologies
    • Y02T10/82Elements for improving aerodynamics

Abstract

本发明公开一种基于速度障碍的无人车实时轨迹规划方法,包括:1:基于无人车A的当前位置构造防碰机动搜索树的节点ni,j;2:基于无人车A及障碍物速度构造无人车A的可达防碰速度集合RAVj(ti);3:通过三次多项式光滑可控单元计算当前规划时刻的最佳速度;4:设置速度风险因子riski();5:基于可达速度集合RVj(ti)和riski()选择新的速度,即ti时刻操作符oi,j,l;6:基于节点ni,j和操作符oi,j,l构造树枝ej,k,获得时刻ti+1或ti+Ts时刻节点ni+1,k;7:如ni+1,k节点状态位于目标状态g的极小邻域内,则算法完成,否则返回第一步循环计算。本发明适用于无人驾驶车辆换道、交叉路口转弯、避障等各种机动情况下的实时轨迹规划,在多个动态障碍物存在条件下能够保证实时性和规划结果的高精度,并最大限度地满足规划速度及轨迹平滑性的要求。

Description

基于速度障碍的无人车实时轨迹规划方法 【技术领域】
[0001] 本发明涉及智能交通系统技术领域,特别涉及无人驾驶系统中的实时轨迹规划方 法。 【背景技术】
[0002] 无人驾驶车辆不但是智能交通系统的重要组成部分,也是移动机器人研宄领域的 研宄热点。无人驾驶车辆在静态环境条件下的轨迹规划方法已取得大量的成果,而动态环 境的轨迹规划要比静态环境的轨迹规划问题复杂的多,所以还没有统一而有效的规划方 法。
[0003] 目前解决动态环境中的轨迹规划问题的方法主要分为四类。第一类方法同静态环 境轨迹规划第一类方法类似,只是将搜索空间增加了一维时间轴,在位形-时间空间中通 过穷举法或智能搜索方法直接进行轨迹规划。这种策略的优点是能够获得完备解,但是,该 方法的搜索空间维数较高,使计算量急剧上升,高效的图搜索技术是该算法的难点。第二 类方法是路径-速度分离算法。第一步先假定工作空间中的障碍物静止,规划一条静态环 境中的无碰有效路径;第二步再沿着这条规划好的路径选择速度的分布以避开运动的障碍 物。由于该方法在进行速度规划时路径不再改变,当动态障碍物较多时,速度的优化策略是 方法的难点。第三类方法是将静态环境轨迹规划方法中通过连续优化过程生成一个单一的 高阶参数化的光滑路径的策略同碰撞检测技术相结合。首先生成一条参数化表达的平滑轨 迹,然后进行碰撞检测,若有碰撞产生,则重新生成一条参数化表达的平滑轨迹,直到生成 的轨迹同所有障碍物无碰撞。有效的参数修正策略是该方法的难点。第四类方法是利用速 度信息直接决定潜在的碰撞可能,在位形-速度空间通过穷举法或智能搜索方法直接进行 规划,这种策略的优点是没有增加搜索空间的维数,但是,当动态障碍物较多时候,该方法 中涉及的计算几何方法的计算量较大,寻找高效的智能搜索方法是该类方法的关键。在多 个动态障碍物的条件下,现有大多数无人驾驶车辆在线轨迹规划方法难于兼顾规划结果 的精度和规划过程的运算效率。
[0004] 以下给出检索的相关文献:
[0005] [1] Ferguson D, Howard TM, Likhachev M. Motion Planning in Urban Environments:Part I [C].Proceedings of the 2008IEEE/RSJ International Conference on Intelligent Robots and Systems, Acropolis Convention Center, Nice, France, Sep. 2008:1063-1069.
[0006] [2]Godbole DN, Hagenmeyer V, Sengupta R and Swaroop D. Design of Emergency Maneuvers for Automated Highway System:Obstacle Avoidance Problem[C]. Proceedings of the 36th Conference on Decision and Control, San Diego, California USA, Dec. 1997:4774-4779.
[0007] [3]Kanaris A, Kosmatopoulos EB and Ioannou PA. Strategies and Spacing Requirements for Lane Changing and Merging in Automated Highway Systems[J]. IEEE Transactions on Vehicular Techinology, Nov. 2001, 50(6):1568-1581.
[0008] [4]Karatas T and Bullo F. Randomized Searches and Nonlinear Programming in Trajectory Planning[C]. Proceedings of the Conference on Decision and Contro 1, Orlando, FL, Dec. 2001:5032-5037.
[0009] [5]Gomez M,Martinez-marin T, Sanchez S and Meziat D. Optimal Control for Wheeled Mobile Vehicles based on Cell Mapping Techniques[C]. Proceedings of the 2008IEEE Intelligent Vehicles Symposium,Eindhoven University of Technology, Eindhoven, The Netherlands, Jun. 2008:1009-1014.
[0010] [6]Autere A. Hierarchical A^Based Path Planning-A Case Study [J]. Knowledge-Based System,2002, 15(1-2):53-66
[0011] [7]Ghosh S and Mount D. An Output-sensitive Algorithm for Computing Visibility Graphs[J]. SIAM Journal on Computing, 1991, 20(5):888 - 910.
[0012] [8]Choset and Burdiek J. Sensor Based Planning I:The Generalized Voronoi Graph[C]. Proceedings of the IEEE international Conference on Robotics and Auto mation,1995, Vol. 2:1643-1648.
[0013] [9 ] Saha M and La tomb e J. Finding Narrow Passages with Probabilistic Roadmaps:The Small Step Retraction Method[J].Autonomous Robots,Dec. 2005, 19 (3) : 301-319.
[0014] [10] Sdnchez G and Latombe J. A Single Query Bi-directional Probabilistic Roadmap Planner with Lazy Collision Checking[J] • Robotics Research,2003, 6:403-417.
[0015] [11]Baker D. Exact Solutions to Some Minimum-time Problems and Their Behavior near Inequality State Constraints[J]. IEEE Transactions on Automatic Control,1999, 34 (1) : 103-106.
[0016] [12] Jackson JW and Crouch PE. Curved Path Approaches and Dynamic Interpo1 ation [J]. IEEE Aerospace and Electronic Systems Magazine,1991,6 (2) :8_13.
[0017] [13]Kalm6r-Nagy T,D' Andrea R and Ganguly P.Near-optimal Dynamic Trajectory Generation and Control of an Omnidirectional Vehicle[J]. Robotics and Autonomous Systems,2004, 46:47-64.
[0018] [14]Ferguson D, Kalra N and Stenz A. Replanning with RRT[C]. Proceedings of the IEEE International Conference on Robotics and Automation, Piscataway,U SA,2006:1-6.
[0019] [15]Kant K and Zueker SW. Toward Efficient Trajectory Planning:The Path-velocity Decomposition [J]• International Journal of Robotics Research,fall,1986, 5 (3) : 72-89.
[0020] [16]Iakovos Papadimitriou and Masayoshi Tomizuka. Fast Lane Changing Computations using Polynomials[C]. Proceedings of the American Control Conferen ce, Denver, Colorado, Jun. 2003:48-53. 【发明内容】
[0021] 本发明的目的在于提供一种基于速度障碍的无人车实时轨迹规划方法,解决上述 现有理论与技术上存在的缺陷或不足。本发明可在车辆三维的位置及航向的位形空间中合 理而有效的进行车辆的实时轨迹规划,将符合车辆动力学约束的光滑可控基元与速度障碍 概念相结合,通过设置最优速度的方法进行启发式搜索,求解无人车任意两个位形间的无 碰轨迹。
[0022] 为了实现上述目的,本发明采用如下技术方案:
[0023] 1、基于速度障碍的无人车实时轨迹规划方法,其特征在于,包括以下步骤:
[0024] 步骤一,基于无人车A的当前位置构造防碰机动搜索树的节点nu,表示时刻心无 人车A的位置和速度:
[0025] 节点:k.., = (1,(,,.)々',(,,))尤⑴
[0026] 其中叫,』是ti时刻第j个节点,xi;j=(Xj(ti),Yj(ti))是A的位置信息, 匕,=(U.是A的速度信息;
[0027] 步骤二,基于无人车A的速度及障碍物速度获得无人车A的可达防碰速度集合 RA' (h),构造防碰机动搜索树的操作符〇i,j;1:
[0028] 操作符:尻 16e = (U,t (2)
[0029] 可达防碰速度集合RAVjUi)中的速度作为防碰机动搜索树中的操作符,通过其扩 展心时刻的节点到ti+1时刻的后继节点,且ti+1=ti+Ts;其中,RAVj(ti)是为节点nu计算 的可达避障速度,AXY为无人车A在二维平面的可行加速度区域,o是节点nu上第1个 操作符;
[0030] 步骤三,通过三次多项式光滑可控单元计算当前规划时刻的最佳速度;
[0031] 步骤四,设置速度风险因子速度风险因子是所选择的速度同所有障碍物 发生碰撞的最短预测时间fc,(vV)以及同v^CT:/,之间距离的函数:
Figure CN104933228AD00061
[0033] 其中pJPp"分别是碰撞时间项和速度偏差项的加权因子,pt根据米样间隔T為 取,p"根据最大加速度和采样间隔Ts选取;
[0034] 步骤五,基于可达速度集合RV和r/i,.(v~V)选择新的速度卜即心时刻操作符〇u1;
[0035] 步骤六,基于节点ni;』和操作符〇 i;j;1构造树枝ej;k,获得时刻ti+1或ti+Ts时刻节 点ni+1,k:
[0036] 树枝:| «/+u =l+((〇,." +《几 /(4)
[0037] ej,k是防碰机动搜索树中t#寸刻节点nu同ti+1时刻节点ni+1,k之间的边;
[0038] 步骤七,如ni+1,k节点状态为目标状态g,则算法完成,否则返回第一步循环计算。
[0039] 优选的,可达防碰速度集合RAVj(ti)包含可达速度集合RVj(ti)和速度障碍 OV^ti);
Figure CN104933228AD00071
[0041] 其中:m是障碍物的个数,此时的VO称为联合速度障碍VO, ti时刻第j个节点的 V0 记为 OVjUi);
Figure CN104933228AD00072
[0043] 其中:TS为采样间隔;t i时刻第j个节点的狀的)i己为RV^ti);
Figure CN104933228AD00073
[0045] 其中0是集合差集的运算符。
[0046] 优选的,步骤五中新的速度R为:
Figure CN104933228AD00074
[0048] 新的速度弋为通过在RV^ti)中进行N次采样来逼近速度风险性因子最小的速度。
[0049] 与现有技术相比,本发明具有以下技术效果:
[0050] 本发明利用速度障碍概念直接将动态环境中的运动障碍物投影到车辆的速度空 间,并将三次多项式函数光滑可控基元中的速度变量设置为最优速度,从而构造搜索三维 位形空间的启发式函数,进而在车辆的位形空间及速度空间进行有效地轨迹规划。该方法 可适用于换道、交叉路口转弯、避障等各种机动情况下的实时轨迹规划。
[0051] 本发明方法适用于无人驾驶车辆换道、交叉路口转弯、避障等各种机动情况下的 实时轨迹规划。计算机仿真结果表明,本方法在多个动态障碍物存在条件下能够保证实时 性和规划结果的高精度,并最大限度地满足轨迹及规划速度平滑性的要求。 【附图说明】
[0052] 图1是本发明基于速度障碍的无人车实时轨迹规划方法流程图;
[0053] 图2是全局搜索的防碰机动子树示意图;
[0054] 图3是无人车A和运动障碍物B1当前状态示意图;
[0055] 图4是相对速度圮』,和碰撞锥示意图;
[0056] 图5是联合速度障碍V0外部的^组成的防碰速度示意图;
[0057] 图6是无人车A可达速度集合示意图;
[0058] 图7是多车辆及行人的广场场景仿真示意图;
[0059] 图8是无人车多车辆及行人的广场场景轨迹规划示意图;
[0060] 图9是无人车A多车辆及行人的广场场景规划机动序列的状态变量示意图:其中, 图9 (a)为规划机动序列的X坐标和Y坐标;图9 (b)为规划机动序列的偏航角图9 (c)为 规划机动序列的X方向的运动速度ux;图9(d)为规划机动序列的Y方向的运动速度u y; 图9(e)为规划机动序列的纵向速度u ;
[0061] 图10是无人车A多车辆及行人的广场场景规划机动序列的控制量示意图:其中, 图10(a)为规划机动序列的横摆角速率r;图10(b)为规划机动序列的纵向加速度6。 【具体实施方式】
[0062] 无人驾驶车辆在真实环境行驶过程中,往往需要在多个同向行驶车辆及各向行人 干扰的动态环境中进行换道、避障或转弯等复杂的机动行为。合理而有效的实时轨迹规划 方法是无人驾驶车辆能够在动态环境中安全行驶的前提及基础。
[0063] 下面对本发明做进一步的详细描述。
[0064]参见图1,本发明一种基于速度障碍的无人车实时轨迹规划方法分为以下七个步 骤,每个步骤具体如下:
[0065] 1)基于无人车A的当前位置构造防碰机动搜索树的节点ni;j:
[0066] 参见图2,这里用离散时刻的防碰机动搜索树来表示无人车的状态空间,树上的节 点表示时刻h无人车A的位置和速度。
[0067] 节点:= .!.A-/../ = (\((. ),[(O),1^/../ = (1仏(0,1
[0068]其中叫,』是ti时刻第j个节点,xi,j=(X j(ti),Yj(ti))是A的位置信息, V7,., 是 A 的速度信息。
[0069] 2)基于无人车A的速度及障碍物速度获得无人车A的可达防碰速度集合RAVj (tj (包含可达速度集合R' (^)和速度障碍〇' (tj),构造防碰机动搜索树的操作符〇i;j;1:
[0070] 操作符:= {引 6e似匕也认=叫及),%((.)) =t
[0071] 参见图2,可达防碰速度集合RAVjUi)(图中RAVi;j)中的速度将作为防碰机动搜 索树中的操作符,通过其扩展h时刻的节点到t i+1时刻的后继节点,且t i+1= t i+Ts。其中, RAVj (tj是为节点nu计算的可达避障速度,Am为无人车A在二维平面的可行加速度区域, 是节点n 上第1个操作符。
[0072] (2a)计算速度障碍0V」(tj
[0073]参见图3,圆A代表无人车而圆&代表运动的障碍物。A和B :在ti时刻分别具有 速度A和\。
[0074] 参见图4,将圆A缩小为点2,将圆&的半径扩大为B i的半径加上A的半径形成 圆或,则&位于A的位形空间中,即圆A的工作空间同位形空间一致。分别在车辆和运动 障碍物的位置J和或处附加他们的速度向量来表示他们的状态。这种表示称为基本位形空 间中的速度空间,允许在动态环境中直接应用计算几何工具来计算防碰机动。计算』和爲 的相对速度
[0075]
[0076] 从A向圆或引出两条切线Af和人r,和人r之间的相对速度都会导致)和或的 碰撞。因此,这里定义碰撞锥^^^的概念,J和成之间相对速度的集合:
[0077]
Figure CN104933228AD00091
[0078] 其中,是相对速度引出的同圆或相交的射线。明显地,只要障碍物或保持 其当前的形状和运动速度,任何位于碰撞锥CC4,V之外的相对速度都不会导致2和或的碰 撞。这样,碰撞锥就将相对速度空间分割成了两个部分:碰撞速度和防碰速度。
[0079] 通过给碰撞锥中的每一个速度加上Bi的速度以,获得基于A的绝对速度建 立同碰撞锥等价的集合,即速度障碍。速度障碍V0定义为:
[0080] ro = o:邙㊉ \
[0081] 其中@表示Minkowski向量和操作符。这样,对象A的绝对速度空间就被速度障 碍V0分割为碰撞速度和防碰速度两部分。在速度障碍V0外部选择速度^就可以避免同障 碍物的碰撞。反之,如果A和Bi保持当前的速度不变,在速度障碍V0内部选择速度匕: 最终会导致A和&在未来某一时刻发生碰撞。即:
[0082]
Figure CN104933228AD00092
[0083] 参见图5,对于多个障碍物,可以将多个速度障碍V0联合起来组成一个单一的速 度障碍V0 :
[0084] FO=U7.lVOi
[0085] 其中m是障碍物的个数,此时的V0称为联合速度障碍V0,心时刻第j个节点的V0 记为OVj(ti)。
[0086] (2b)计算无人车A可达速度集合RVj(tj
[0087] 在二维平面运动的无人车受到纵向力和侧向力fn=mur的作用具有行 驶速度u和横摆角速率r,m为车辆的质量。车辆的运动学方程为:
Figure CN104933228AD00093
[0091] 其中X和Y是车辆在笛卡尔坐标系的位置坐标,t是速度矢量的偏航角,t)和r分 别是速度和转向的控制输入。有效的控制输入U(动力学约束)为:
Figure CN104933228AD00094
[0093] 有效控制输入U的范围是以t)和r为变量的椭圆,称为摩擦圆。进一步可以获得:
[0094]
Figure CN104933228AD00101
[0095] 无人车在二维平面的可行加速度区域仍然是一个椭圆,可以通过将摩擦圆旋转角 度供(0获得,即二维平面的可行加速度区域为:
Figure CN104933228AD00102
[0097] 另外,车辆的行驶速度具有上界和下界:
[0098]
[0099] 参见图6,自主驾驶车辆的规划器必须服从车辆运动学和动力学约束,这些约束限 定了基于当前的速度矢量f可选择的下一时刻新的行驶速度0的范围,也即可到达的新速 度范围。我们用狀沢)来表示基于当前速度•^可到达的新速度A的集合:
Figure CN104933228AD00103
[0101] 其中Ts为采样间隔。ti时刻第j个节点的/^灰)记为RV^ti)。
[0102] (2c)计算无人车A可达防碰速度集合RAVj(h)
[0103] 可达防碰速度集合RAV定义为可达速度集合〃以6)同速度障碍集合V0的差集:
[0104] RAV{v,) =RV(v,)&V〇
[0105] 其中0是集合差集的运算符。此时,同障碍物的防碰机动就可以通过在RAV中选 择速度来进行计算。h时刻第j个节点的RAV记为RAV^心)。
[0106] 3)通过三次多项式光滑可控单元计算当前规划时刻的最佳速度;自主驾驶车辆 的轨迹规划是在线规划,所以防碰机动的搜索采用启发式搜索,三次多项式函数可以保证 位置、速度以及加速度的连续性变化,有利于执行器平滑动作,并且易于满足车辆的运动学 及动力学约束。
[0107] (3a)构造当前状态和目标状态之间生成轨迹的函数族
[0108] 在X向、Y向分别选用三次多项式函数生成位置函数族,如下式所示:
Figure CN104933228AD00104
[0111] 对上式位置函数族进行一阶求导,获得X、Y两个方向上的速度函数族,如下式所 示:
Figure CN104933228AD00105
Figure CN104933228AD00111
[0114] 其中t为时间,是多项式的变量,i表示各阶幂指数,aJPb 多项式各项的系数。 位置函数族和速度函数族组成无人车的轨迹函数族。
[0115] (3b)确定无人车规划初始时刻当前状态Sstart和规划终止时刻目标状态SgMl的轨 迹函数族表达形式:
[0116] 规划初始时刻车辆的状态为,Fs,ur,,^Psku.t,UstUrt') 'Xsturt、Ysku.t、、Ustm.t 分别是车辆当前在X和Y方向的位置,偏航角及行驶速度,目标状态为
Figure CN104933228AD00112
分别是车辆的目标状态在X和Y方向的 位置,偏航角及行驶速度。
[0117] 根据(3a)中轨迹函数族确定规划初始时刻%_和规划终止时刻tgMl的当前状态 Sstart和目标状态sgMl:
Figure CN104933228AD00113
[0126] 其中邱sta, > = (匕cos邱,,)=X-及u' K(UJ=~劇,叫_ ) = ,及 (k") = sin 。 可得tstart时刻和tgMl时刻当前状态sstart和目标状态s8。31的轨迹函数族表达形式:
[0127]
[0128]
Figure CN104933228AD00114
Figure CN104933228AD00121
[0129] 其中
Figure CN104933228AD00122
,pA=[a3a2a!aJT和pB=[b3b2b!b0]T。
[0130](3c)确定规划初始时刻%_和规划终止时刻tgMi
[0131] 通常tstart取为0,而t_可以根据系统的实际情况确定其取值。这里根据下式确 tg〇ai:
[0132] tgoal= 2a11IPgoal-PstartII/ (Ustart+Ugoal)
[0133]其中| |PgMl_Pstart| |是起始状态位置同目标状态位置之间的直线距离,Pstart表示 起始点,PgMl表示目标点,at为系数,与P^和P_的位置及方向状态相关,可根据规划策 略进行设置。通常,ate[0.8, 1.2],如果规划策略选择为尽快到达目标状态,就选择at接 近〇. 8,如果规划策略为尽量平滑的到达目标状态,就选择at接近1. 2。
[0134] (3d)计算下一采样时刻的最佳速度
[0135] 根据(3b)中当前状态Sstart和目标状态SgMl的轨迹函数族表达形式以及(3c)中 时刻tstart和时刻tgMl,即可求出pjPpB,则从tstart开始,下一米样时刻的最佳速度为:
Figure CN104933228AD00123
[0137] 4)设置速度风险因子r/.s人.仍)
[0138] (4a)计算最短预测时间
[0139] 假设< 为选择的一个新速度,它同所有障碍物发生碰撞的最短预测时间记为 h(v〇,可通过求解如下所示方程获得:
Figure CN104933228AD00124
[0141] 其中是从P点出发方向同速度f一致的射线。
[0142] 对于联合速度障碍0'(&),化的_)为车辆A同所有障碍物的碰撞预测时间中最小 的那个。
[0143] (4b)计算速度风险因子為(<)
[0144] 速度风险性因子是所选择的速度< 同(4a)中的最短预测时间岣(<)以 及同(3d)中的之间距离的函数:
Figure CN104933228AD00125
[0146] 其中pt*p"分别是碰撞时间项和速度偏差项的加权因子,pt可以根据米样间隔 Ts选取pt= 1000Ts,p"可以根据最大加速度和采样间隔Ts选取凡=5〇〇max少丨厂。
[0147] 5)基于RV^ti)和/vi,(v〇选择新的速度,即心时刻操作符om
[0148] 为车辆A选择的时刻的速度A为(2b)中RVj (tD里r/_為(v7,)最小的< :
Figure CN104933228AD00131
[0150] 可以通过在RV^ti)中进行N次采样来逼近(4b)中风险性因子最小的速度。
[0151] 6)参见图2,基于1)中节点叫,』和5)中操作符〇 ^卩构造树枝e j,k,获得时刻ti+1 或心+八时刻节点n i+1,k;
[0152] 7)如ni+1,k节点状态位于目标状态g的0. 0001m的邻域内,则轨迹规划方法完成, 否则返回步骤1)循环计算。
[0153] 本发明中给出了一种基于速度障碍的无人车实时轨迹规划方法,并利用MATLAB 对该规划方法进行了仿真验证。从图7-图10和表1-表2的仿真结果中可以看到,本发明 的规划结果误差可以达到毫米级,而航向的规划结果误差均小于l〇_ 7rad,完全能够满足无 人车轨迹规划任务的精度要求。而当关注区域内运动障碍物数量多达5个时,本章算法的 运行时间仅为〇. 172s,完全能够满足无人车实时轨迹规划方法运算效率的要求。仿真实验 证明基于本发明方法实现的无人车在线轨迹规划器在多个动态障碍物存在条件下能够同 时保证实时性和规划结果的高精度,并最大限度地满足规划速度及轨迹平滑性的要求。
[0154] 表1实时轨迹规划方法规划结果绝对误差
Figure CN104933228AD00132

Claims (3)

1. 基于速度障碍的无人车实时轨迹规划方法,其特征在于,包括w下步骤: 步骤一,基于无人车A的当前位置构造防碰机动捜索树的节点nu,表示时刻ti无人车A的位置和速度; 节点;";,=托V=(却典))而.=的,.片的,.似)} ( 1 ) 其中nu是t府刻第j个节点,Xu=(Xj.(ti),Yj.(ti))是A的位置信息, 巧,=的,x似,似)是A的速度f目息; 步骤二,基于无人车A的速度及障碍物速度获得无人车A的可达防碰速度集合RAV^ti),构造防碰机动捜索树的操作符〇u,i; 操作符;=例巧E化),巧=(巧、.似,巧,.似)=巧j+心I:} (2) 可达防碰速度集合RAVj. (ti)中的速度作为防碰机动捜索树中的操作符,通过其扩展ti时刻的节点到时刻的后继节点,且tw=ti+L;其中,RAVj.(ti)是为节点nu计算的可 达避障速度,Axy为无人车A在二维平面的可行加速度区域,0 ,11是节点nU上第1个操作 符; 步骤=,通过=次多项式光滑可控单元计算当前规划时刻的最佳速度 步骤四,设置速度风险因子/'/心,.仍');速度风险因子是所选择的速度同所有障碍物发生 碰撞的最短预测时间fc,.巧')W及同vV""'之间距离的函数:
Figure CN104933228AC00021
(3) 其中Pt和P。分别是碰撞时间项和速度偏差项的加权因子,Pt根据采样间隔T,选取,P。根据最大加速度和采样间隔选取; 步骤五,基于可达速度集合RV和n'.v/:,.仍')选择新的速度巧,即ti时刻操作符0u,i; 步骤六,基于节点nu和操作符0u,i构造树枝eik,获得时刻tw或ti+T拥刻节点 Di+i'k: 树枝;=俯,尸",.+1,*-)1",.+1,*-=",v+ (的如 + 巧,/2)} (4) 6j,k是防碰机动捜索树中ti时刻节点nU同tW时刻节点nw,k之间的边; 步骤走,如niAk节点状态为目标状态g,则算法完成,否则返回第一步循环计算。
2. 根据权利要求书1所述的基于速度障碍的无人车实时轨迹规划方法,其特征在于: 可达防碰速度集合RAVj.(ti)包含可达速度集合RVj.(ti)和速度障碍OVj.(ti); F〇=ur月 其中;m是障碍物的个数,此时的VO称为联合速度障碍V0,ti时刻第j个节点的VO记 为OVj(ti);
Figure CN104933228AC00022
其中;Ts为采样间隔;t府刻第j个节点的W识)记为RVj.(ti);
Figure CN104933228AC00023
其中0是集合差集的运算符。
3.根据权利要求书1所述的基于速度障碍的无人车实时轨迹规划算法,其特征在于: 步骤五中新的速度巧为:
Figure CN104933228AC00031
新的速度术,为通过在RV^ti)中进行N次采样来逼近速度风险性因子最小的速度。
CN201510278572.0A 2015-05-27 2015-05-27 基于速度障碍的无人车实时轨迹规划方法 Active CN104933228B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510278572.0A CN104933228B (zh) 2015-05-27 2015-05-27 基于速度障碍的无人车实时轨迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510278572.0A CN104933228B (zh) 2015-05-27 2015-05-27 基于速度障碍的无人车实时轨迹规划方法

Publications (2)

Publication Number Publication Date
CN104933228A true CN104933228A (zh) 2015-09-23
CN104933228B CN104933228B (zh) 2018-03-02

Family

ID=54120394

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510278572.0A Active CN104933228B (zh) 2015-05-27 2015-05-27 基于速度障碍的无人车实时轨迹规划方法

Country Status (1)

Country Link
CN (1) CN104933228B (zh)

Cited By (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105223956A (zh) * 2015-11-09 2016-01-06 中山大学 一种全向移动机器人的动态避障方法
CN105955253A (zh) * 2016-04-20 2016-09-21 郑州宇通客车股份有限公司 自主驾驶车辆的转向灯以及车门的控制方法和控制系统
CN106446430A (zh) * 2016-09-30 2017-02-22 长安大学 一种半挂车弯道超车风险分析方法
CN107885209A (zh) * 2017-11-13 2018-04-06 浙江工业大学 一种基于动态窗口与虚拟目标点的避障方法
CN108595823A (zh) * 2018-04-20 2018-09-28 大连理工大学 一种联合驾驶风格和博弈理论的自主车换道策略的计算方法
CN108701362A (zh) * 2016-02-29 2018-10-23 深圳市大疆创新科技有限公司 目标跟踪期间的障碍避免
CN108693878A (zh) * 2017-04-06 2018-10-23 丰田自动车株式会社 前进路线设定装置以及前进路线设定方法
CN109141911A (zh) * 2018-06-26 2019-01-04 百度在线网络技术(北京)有限公司 无人车性能测试的控制量的获取方法和装置
CN109318890A (zh) * 2018-06-29 2019-02-12 北京理工大学 一种基于动态窗口及障碍物势能场的无人车动态避障方法
CN109334661A (zh) * 2018-09-06 2019-02-15 上海工程技术大学 基于速度障碍模型/碰撞概率密度模型的避障预判方法
CN109491377A (zh) * 2017-09-11 2019-03-19 百度(美国)有限责任公司 用于自动驾驶车辆的基于dp和qp的决策和规划
CN109521761A (zh) * 2017-09-18 2019-03-26 百度(美国)有限责任公司 用于自动驾驶车辆的基于约束平滑样条的速度优化
CN109521763A (zh) * 2017-09-18 2019-03-26 百度(美国)有限责任公司 用于自动驾驶车辆的基于约束平滑样条的路径优化
CN109521762A (zh) * 2017-09-18 2019-03-26 百度(美国)有限责任公司 用于自动驾驶车辆的基于2d约束平滑样条的平滑道路参考线路
CN109709949A (zh) * 2017-10-25 2019-05-03 保时捷股份公司 用于运行自动驾驶的机动车辆的方法以及自动驾驶的机动车辆
CN112455440A (zh) * 2020-11-30 2021-03-09 北京易控智驾科技有限公司 自动驾驶车辆编组的协同避让方法、装置、设备及介质
CN108595823B (zh) * 2018-04-20 2021-10-12 大连理工大学 一种联合驾驶风格和博弈理论的自主车换道策略计算方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102037422B (zh) * 2008-05-22 2016-08-03 村田机械株式会社 行驶车系统及行驶车系统中的行驶控制方法
CN101883326B (zh) * 2010-05-31 2012-12-05 西安电子科技大学 基于无人驾驶车辆监测的无线传感器网络数据传输方法
CN101996516A (zh) * 2010-11-22 2011-03-30 南京信息工程大学 基于矢量法的路径规划预处理方法
CN102495631B (zh) * 2011-12-09 2013-08-21 中国科学院合肥物质科学研究院 一种无人驾驶车辆跟踪预定轨迹的智能控制方法
CN102632891B (zh) * 2012-04-06 2014-09-17 中国人民解放军军事交通学院 无人驾驶车辆实时跟踪行驶轨迹的计算方法
CN103150786B (zh) * 2013-04-09 2015-04-22 北京理工大学 一种非接触式无人驾驶车辆行驶状态测量系统及测量方法

Cited By (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105223956B (zh) * 2015-11-09 2018-02-27 中山大学 一种全向移动机器人的动态避障方法
CN105223956A (zh) * 2015-11-09 2016-01-06 中山大学 一种全向移动机器人的动态避障方法
CN108701362A (zh) * 2016-02-29 2018-10-23 深圳市大疆创新科技有限公司 目标跟踪期间的障碍避免
CN105955253A (zh) * 2016-04-20 2016-09-21 郑州宇通客车股份有限公司 自主驾驶车辆的转向灯以及车门的控制方法和控制系统
CN106446430A (zh) * 2016-09-30 2017-02-22 长安大学 一种半挂车弯道超车风险分析方法
CN106446430B (zh) * 2016-09-30 2019-06-04 长安大学 一种半挂车弯道超车风险分析方法
CN108693878A (zh) * 2017-04-06 2018-10-23 丰田自动车株式会社 前进路线设定装置以及前进路线设定方法
CN109491377A (zh) * 2017-09-11 2019-03-19 百度(美国)有限责任公司 用于自动驾驶车辆的基于dp和qp的决策和规划
CN109521762A (zh) * 2017-09-18 2019-03-26 百度(美国)有限责任公司 用于自动驾驶车辆的基于2d约束平滑样条的平滑道路参考线路
CN109521761A (zh) * 2017-09-18 2019-03-26 百度(美国)有限责任公司 用于自动驾驶车辆的基于约束平滑样条的速度优化
CN109521763A (zh) * 2017-09-18 2019-03-26 百度(美国)有限责任公司 用于自动驾驶车辆的基于约束平滑样条的路径优化
CN109709949A (zh) * 2017-10-25 2019-05-03 保时捷股份公司 用于运行自动驾驶的机动车辆的方法以及自动驾驶的机动车辆
CN107885209A (zh) * 2017-11-13 2018-04-06 浙江工业大学 一种基于动态窗口与虚拟目标点的避障方法
CN108595823A (zh) * 2018-04-20 2018-09-28 大连理工大学 一种联合驾驶风格和博弈理论的自主车换道策略的计算方法
CN108595823B (zh) * 2018-04-20 2021-10-12 大连理工大学 一种联合驾驶风格和博弈理论的自主车换道策略计算方法
CN109141911A (zh) * 2018-06-26 2019-01-04 百度在线网络技术(北京)有限公司 无人车性能测试的控制量的获取方法和装置
CN109318890A (zh) * 2018-06-29 2019-02-12 北京理工大学 一种基于动态窗口及障碍物势能场的无人车动态避障方法
CN109334661B (zh) * 2018-09-06 2020-05-29 上海工程技术大学 基于速度障碍模型和碰撞概率密度模型的避障预判方法
CN109334661A (zh) * 2018-09-06 2019-02-15 上海工程技术大学 基于速度障碍模型/碰撞概率密度模型的避障预判方法
CN112455440A (zh) * 2020-11-30 2021-03-09 北京易控智驾科技有限公司 自动驾驶车辆编组的协同避让方法、装置、设备及介质

Also Published As

Publication number Publication date
CN104933228B (zh) 2018-03-02

Similar Documents

Publication Publication Date Title
CN104933228B (zh) 基于速度障碍的无人车实时轨迹规划方法
CN107168305B (zh) 路口场景下基于Bezier和VFH的无人车轨迹规划方法
Fahimi et al. Real-time obstacle avoidance for multiple mobile robots
Zhang et al. Hybrid trajectory planning for autonomous driving in highly constrained environments
Alonso-Mora et al. Reciprocal collision avoidance for multiple car-like robots
Xu et al. Behavior-based formation control of swarm robots
Zips et al. Optimisation based path planning for car parking in narrow environments
Koyuncu et al. A probabilistic B-spline motion planning algorithm for unmanned helicopters flying in dense 3D environments
JP6610915B2 (ja) 車両を制御するコントローラー及び方法並びに非一時的コンピューター可読メモリ
Shiller et al. Online obstacle avoidance at high speeds
Chang et al. Path planning of wheeled mobile robot with simultaneous free space locating capability
Li et al. A model based path planning algorithm for self-driving cars in dynamic environment
Mouhagir et al. A markov decision process-based approach for trajectory planning with clothoid tentacles
Guo et al. Global trajectory generation for nonholonomic robots in dynamic environments
Dong et al. An optimal curvature smoothing method and the associated real-time interpolation for the trajectory generation of flying robots
Fu et al. Collision-free and kinematically feasible path planning along a reference path for autonomous vehicle
Vasseur et al. Navigation of car-like mobile robots in obstructed environments using convex polygonal cells
Lin et al. Indoor robot navigation based on DWA*: Velocity space approach with region analysis
Liu et al. Trajectory Planning for Ground Service Robot
Wang et al. Motion control for intelligent ground vehicles based on the selection of paths using fuzzy inference
Sinodkin et al. A method for constructing a global motion path and planning a route for a self-driving vehicle
Li et al. DF-FS based path planning algorithm with sparse waypoints in unstructured terrain
Song et al. A TC-RRT-based path planning algorithm for the nonholonomic mobile robots
EP3623759A1 (en) A computer-implemented method and a system for defining a path for a vehicle within an environment with obstacles
Dang et al. A path planning method for indoor robots based on partial & global A-star algorithm

Legal Events

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