CN116933961A - 一种基于teb优化框架的混合交通场景轨迹规划方法 - Google Patents
一种基于teb优化框架的混合交通场景轨迹规划方法 Download PDFInfo
- Publication number
- CN116933961A CN116933961A CN202310972401.2A CN202310972401A CN116933961A CN 116933961 A CN116933961 A CN 116933961A CN 202310972401 A CN202310972401 A CN 202310972401A CN 116933961 A CN116933961 A CN 116933961A
- Authority
- CN
- China
- Prior art keywords
- track
- speed
- vehicle
- constraint
- teb
- 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
Links
- 238000005457 optimization Methods 0.000 title claims abstract description 44
- 238000000034 method Methods 0.000 title claims abstract description 31
- 230000002452 interceptive effect Effects 0.000 claims abstract description 30
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 21
- 230000004888 barrier function Effects 0.000 claims description 9
- 230000007704 transition Effects 0.000 claims description 8
- 238000009826 distribution Methods 0.000 claims description 6
- 230000003068 static effect Effects 0.000 claims description 4
- 230000009471 action Effects 0.000 claims description 3
- 230000003993 interaction Effects 0.000 abstract description 16
- 230000001133 acceleration Effects 0.000 description 8
- 238000010586 diagram Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 238000004088 simulation Methods 0.000 description 5
- 238000004364 calculation method Methods 0.000 description 3
- 230000006399 behavior Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000001627 detrimental effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 238000011065 in-situ storage Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
- G06Q10/00—Administration; Management
- G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
- G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N7/00—Computing arrangements based on specific mathematical models
- G06N7/01—Probabilistic graphical models, e.g. probabilistic networks
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
- G06Q50/00—Information and communication technology [ICT] specially adapted for implementation of business processes of specific business sectors, e.g. utilities or tourism
- G06Q50/10—Services
- G06Q50/26—Government or public services
Landscapes
- Business, Economics & Management (AREA)
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Human Resources & Organizations (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Economics (AREA)
- Strategic Management (AREA)
- Tourism & Hospitality (AREA)
- Development Economics (AREA)
- Marketing (AREA)
- General Business, Economics & Management (AREA)
- Probability & Statistics with Applications (AREA)
- Mathematical Optimization (AREA)
- Operations Research (AREA)
- Entrepreneurship & Innovation (AREA)
- Game Theory and Decision Science (AREA)
- Algebra (AREA)
- Artificial Intelligence (AREA)
- Computational Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Evolutionary Computation (AREA)
- Mathematical Analysis (AREA)
- Quality & Reliability (AREA)
- Pure & Applied Mathematics (AREA)
- Computing Systems (AREA)
- General Engineering & Computer Science (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Educational Administration (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Primary Health Care (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明公开一种基于TEB优化框架的混合交通场景轨迹规划方法，考虑其他交通参与者的运动学约束、协同避障约束以及道路上下文约束，预测其他交通参与者交互式的未来轨迹；基于传统TEB算法的优化框架，考虑其他交通参与者的交互式预测轨迹、车辆的运动动力学约束以及避障约束，并以轨迹时间最短为优化目标生成本车的最优轨迹；将车辆运动学约束作为约束条件之一，生成一个时间最优的车辆运动轨迹，为了考虑其它交通参与者之间的运动交互，基于智能体的不可观测状态预测其与周围环境进行交互而产生的协同避障轨迹，考虑了其他交通参与者的意图和专注度来预测他们的交互轨迹，从而增强了混合交通场景中轨迹规划的安全性和鲁棒性。
Description
技术领域
本发明属于自动驾驶轨迹规划领域，具体涉及一种基于TEB优化框架的混合交通场景轨迹规划方法。
背景技术
近几年来无人驾驶技术得到了蓬勃的发展，然而大型混合交通场景中的运动规划仍然是自动驾驶领域亟待解决的难题，其中一个关键的问题是如何在考虑交互性的同时生成一条满足各种约束(运动动力学)的可行驶轨迹。密集的交通流使得难以为自动驾驶车辆规划最佳可行的轨迹.。另外，在混合交通场景中异构的代理表现出不同的运动特性并且相互之间存在复杂的交互。它们的运动轨迹会受到自己的状态以及周围环境变化的影响，这让它们变得难以准确预测。现有的方法难以同时考虑智能体之间的交互并生成可行的运动轨迹，因为这两者属于两个不同的范畴。有一些方法可以直接生成一条满足车辆运动动力学约束的轨迹。但是，由于没有考虑其它交通参与者之间的交互，无法准确推理出其未来的运动轨迹，这会造成潜在的危险。另外有一些方法考虑了智能体之间的交互式协同避障，能够对其他交通参与者未来的行为进行更准确地推理，从而做出更安全、合理的决策。然而，它们没有将考虑交互性的决策信息转化为车辆最终的运动轨迹，这不利于后续的优化与控制。
发明内容
为了解决现有技术中存在的问题，本发明提供了一个针对复杂混合交通场景的自动驾驶汽车轨迹规划方法，考虑了智能体之间的交互性，并最终优化生成一条运动轨迹，利用时间弹性带算法(TEB)的整体优化框架，将车辆运动学约束作为约束条件之一，生成一个时间最优的车辆运动轨迹，为了考虑其它交通参与者之间的运动交互，基于智能体的不可观测状态预测其与周围环境进行交互而产生的协同避障轨迹。
为了实现上述目的，本发明采用的技术方案是：一种基于TEB优化框架的混合交通场景轨迹规划方法，包括以下步骤：
步骤1，考虑其他交通参与者的运动学约束、协同避障约束以及道路上下文约束，预测其他交通参与者交互式的未来轨迹；
步骤2，基于传统TEB算法的优化框架，考虑其他交通参与者的交互式预测轨迹、车辆的运动动力学约束以及避障约束，并以轨迹时间最短为优化目标生成本车的最优轨迹；对于其他交通参与者的交互式预测轨迹，考虑其他交通参与者的意图和专注度进行预测。
步骤1所述的考虑其他交通参与者的运动学约束具体为：采用一种离散化的方式表示考虑运动学约束下智能体可行速度空间，给定一系列速度朝向角，并计算所述朝向角下的最大可行速度vmax，将所有速度朝向角下的智能体最大速度作为一个速度区域的边界，生成考虑运动约束的可行速度区域Kτ。
步骤1所述的考虑其他交通参与者的协同避障约束具体为：先计算本车的速度障碍物，随后通过给本车和障碍物车辆施加偏置速度使二者的相对速度位于速度障碍物之外，最后做垂直于偏置速度的半平面构建出可行速度半平面；针对每个邻居智能体，获得一个可行速度区域半平面，获得智能体A考虑交互式协同避障的可行速度区域。
将最小偏置速度voff按设定比例分配给本车A和邻居智能体B，并分别表示为uA和uB，最小偏置速度voff的分配由智能体的避碰责任γ来决定，避碰责任γ越大，为其分配更高的偏置速度，使其做出更大的避让，避碰责任γ越小，为其分配相对更小的偏置速度，使其做出更小的避让动作。
步骤1所述的考虑道路上下文约束具体为：当智能体距离道路边界d米处，提供另一个速度约束半平面Cτ对车辆的横向速度进行约束，位于速度平面Cτ中的速度满足vx＜d/τ，τ为规划时间间隔。
步骤1所述的预测其他交通参与者交互式未来轨迹具体为：
基于可行速度空间生成最优预测速度，最优预测速度满足：位于可行速度空间内并且与智能体偏爱速度最接近；在获取最优预测速度之后，基于智能体的运动模型进行状态转移，获取其下一个预测轨迹点，进行最优速度预测以及智能体的状态转移，获取一系列预测轨迹点，最终得到预测轨迹。
步骤2所述的考虑其他交通参与者的交互式预测轨迹具体为：计算本车轨迹与其他交通参与者预测轨迹的时空距离，并将时空距离的大小作为动态障碍物避障约束添加到TEB轨迹优化框架中，对本车轨迹进行优化。
对于静态障碍物，时空距离即为障碍物当前位置与轨迹点的距离；对于动态障碍物，首先从动态障碍物预测轨迹中获取时间T＝T1时的预测位置Pk，随后在统一时间的基础上计算Pk与轨迹点ck的欧氏距离，从而得到时空距离，采用轨迹点与障碍物的时空距离大于最小距离作为轨迹规划的避障约束。
车辆的轨迹由一系列离散点以及离散点之间的时间间隔组成，优化的对象为离散点与时间间隔的集合，轨迹规划问题转化为以轨迹点时间间隔的平方和最小为优化目标、以车辆运动动力学和车辆与障碍物距离为约束函数的有约束非线性优化问题。
运动学具体为：在两个连续的轨迹点之间采用恒定控制，车辆在两个连续的离散点之间做匀速直线运动或匀速圆周运动，vk和vk+1分别表示车辆在轨迹点sk与sk+1时朝向角与ek的夹角，ek表示轨迹点sk与sk+1的连线，两个离散点同时位于一段恒定曲率的圆弧或直线上，当且仅当vk＝vk+1时成立。
与现有技术相比，本发明至少具有以下有益效果：
本发明方法考虑智能体之间的交互性，并最终优化生成一条运动轨迹，利用时间弹性带算法(TEB)的整体优化框架，将车辆运动学约束作为约束条件之一，生成一个时间最优的车辆运动轨迹，为了考虑其它交通参与者之间的运动交互，基于智能体的不可观测状态预测其与周围环境进行交互而产生的协同避障轨迹；本发明在生成符合运动动力学约束轨迹的同时考虑了其它交通参与者之间的交互，考虑了其他交通参与者的意图和专注度来预测他们的交互轨迹，从而增强了混合交通场景中轨迹规划的安全性和鲁棒性，在仿真平台summit上设置了不同道路结构与交通流密度的场景，同时测试本发明在该场景中的性能，经验证，基于本发明所述方法规划路径的安全、舒适和有效。
进一步的，基于可靠的交互式轨迹计算本车轨迹与动态障碍物的时空距离，并作为动态避障约束加入到轨迹优化的过程中，大大提高了自动驾驶车辆在复杂交互式环境中的动态避障能力。
进一步的，考虑其它交通参与者状态的不确定性，本发明针对智能体的意图与专注度两种不可观测状态建立概率模型，设计了一个隐状态推理机对不可观测状态的概率分布进行更新。
附图说明
图1为本发明的系统框架图。
图2为基于车辆运动学约束生成可行速度半平面的示意图。
图3为基于交互式协同避障约束生成可行速度半平面的示意图。
图4为基于道路上下文约束生成可行速度半平面的示意图。
图5为时空距离示意图。
具体实施方式
下面结合附图和具体实施对本申请的示范性实例进行详细阐明，其中包括本申请实施例的各种细节以助于理解。应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。
图1表示本发明的整个系统框架，其输入信息为道路结构、全局参考路径以及周围的动静态障碍物信息，输出一条考虑交互性以及车辆运动动力学约束的轨迹。整个框架可以分为两个部分，第一部分为交互式轨迹预测。首先构建每个交通代理的可行速度区域，随后根据交通代理的状态转移信息对其不确定性状态进行推理，最后根据不确定性状态优化得到交通代理的最优预测速度，迭代进行交通代理的最优速度预测以及状态转移，本发明获得每个其它交通参与者的交互式预测轨迹，并输入到本车的轨迹规划模块中。本发明的第二部分为轨迹优化，将其它交通参与者的预测轨迹用于计算本车与动态障碍物的时空距离，从而计算避障损失。整个轨迹优化模块以轨迹总时间最短为优化目标，考虑车辆的运动动力学约束以及避障约束，并采用G2O方法优化得到本车的最优运动轨迹。本发明的细节如下：
步骤1：预测其他交通参与者的轨迹，并同时考虑其运动学约束、交互式协同避障约束以及道路上下文约束，流程如算法1所示。
具体细节如下：
(1)考虑运动学约束
对于完整约束机器人，其运动不受约束，因此其在下一时刻的速度可以朝向任意方向。而对于非完整约束的机器人，其下一时刻速度受车辆当前位姿的影响。为了降低计算量，本发明采用一种离散化的方式表示考虑运动学约束下智能体可行速度空间，如图2所示。给定一系列速度朝向角，并计算该朝向角下的最大可行速度vmax，最大可行速度满足：在时间TAO内，智能体可以通过控制器对该速度进行追踪，并且保持追踪误差始终低于最大误差ε。将所有速度朝向角下的智能体最大速度作为一个速度区域的边界，生成考虑运动约束的可行速度区域Kτ。容易知道的是，考虑运动学约束的可行速度空间可以依据不同运动模型进行离线计算，在轨迹预测时直接调用即可，能大大降低轨迹预测的运算量。
(2)考虑交互式协同避障约束
图3描述考虑交互式协同避障约束的可行速度半平面生成过程。智能体的运动速度常常会受到周围其它交通参与者运动的影响。专注的智能体会主动避让周围其它交通参与者，生成无碰撞的轨迹。为了更加准确地表示智能体的几何形状，本发明使用多边形来描述智能体的外形。为了进一步简化推理，本发明参照本车A对障碍物B进行膨胀，从而将本车A转为为一个质点进行处理，将障碍物B膨胀后的区域表示为OB|A，随后，可以计算速度障碍物(图4阴影区域)，表示一个在时间τ内会导致碰撞的相对速度集。
其中，vA|B表示本车A相对于邻居智能体B的相对速度，表示速度障碍物。
如果相对速度vA|B位于速度障碍物之外，则表示在时间τ内二者不会相撞，这时认为二者按照原来的速度行驶。否则，给相对速度vA|B施加一个最小偏置速度voff，使相对速度位于速度障碍物/>之外。为了更有效地实现避障，将最小偏置速度voff按设定比例分配给A和B，并分别表示为uA和uB，最小偏置速度voff的分配由智能体的避碰责任γ来决定。避碰责任γ越大(例如专注的、或者自由度更高的智能体)，为其分配更高的偏置速度，使其做出更大的避让，避碰责任γ越小(如分心的、或者运动约束更多的智能体)，为其分配相对更小的偏置速度，使其做出更小的避让动作。最终，添加偏置速度后的相对速度v'A|B由下式计算：
其中，vA和vB分别智能体A和智能体B的初始速度。
以垂直于最小偏置速度voff的分割线为边界，可以获取本车的一个可行速度区域半平面代表考虑与B进行协同避障时A的可行速度区域。同理，针对每个邻居智能体，都可以获得这样一个可行速度区域半平面。最终，获得智能体A考虑交互式协同避障的可行速度区域/>
(3)考虑道路上下文约束
需要保证智能体在时间τ内不违反道路上下文的约束，即道路结构与交通规则。如图4所示，当智能体距离道路边界d米处，需要另一个速度约束半平面Cτ对车辆的横向速度进行约束。位于速度约束半平面Cτ中的速度满足vx＜d/τ，τ为规划时间间隔。
(4)优化目标与迭代预测
所有速度约束半平面的交集构成智能体的可行速度空间，需要在可行速度空间中找出最接近智能体的偏好速度的速度作为最优预测速度/>
智能体的偏好速度受智能体的不可观测状态影响，这将在第(5)部分进行介绍。根据最优预测速度和交通代理的运动学模型进行智能体状态转移，可以获得其下一个预测轨迹点。通过迭代地执行最优速度预测以及状态转移，获得一系列预测轨迹点，进而获得代理的最终预测轨迹。
(5)智能体不可观测状态推理
智能体的偏好速度取决于智能体的意图与专注度。本发明将智能体可能的初始路径作为智能体的意图。在交叉路口，智能体的意图将智能体引导至某条道路；行人的意图通常将行人引导至道路边上的非机动车道。每个智能体可能有多个意图路径，多个意图路径存在一定的概率分布。为了衡量智能体的专注程度，本发明将智能体分为专注和分心两种情况。本发明认为专注的智能体会主动避让周围车辆，即按照所预测的轨迹行驶。专注智能体的偏好速度指向其概率最大的意图路径。本发明认为分心的智能体不对周围环境进行响应，沿着当前速度与方向做匀速直线运动。容易知道的是智能体的意图与专注度是两种不可观测的状态，将其表示为sh，其在时间t时的联合概率分布为pt(sh)。
pt(sh)＝η*G(||Pt'-Pt|||0,σ)*pt-1(sh) (4)
其中，G(·|0,σ)表示均值为0，标准差为σ的高斯概率模型。η为归一化系数，Pt'表示智能体在状态sh下的预测轨迹点，Pt为实际轨迹点。
步骤2：在获取其他交通参与者的交互式预测轨迹之后，本发明利用传统TEB算法的优化框架及其他交通参与者的交互式预测轨迹，生成考虑交互性的本车最优轨迹，具体细节如下：
(1)目标函数
车辆的轨迹由一系列离散点(ck)k＝1,2,...,n以及离散点之间的时间间隔(ΔTk)k＝1,2,...,n-1组成，其中ck＝(xk,yk,θk)代表车辆在第k个轨迹点的位置与朝向角。优化的对象为离散点与时间间隔的集合Θ，Θ:＝{c1,ΔT1,c2,ΔT2,...,cn-1,ΔTn-1,cn}。最终轨迹规划问题转化为以轨迹点时间间隔的平方和最小为优化目标，以车辆运动动力学和车辆与障碍物距离为约束函数的有约束非线性优化问题。
其中，cstart和cend分别为轨迹的起点和终点位姿，由初始轨迹给定，且在优化过程中保持不变。轨迹点间的时间间隔ΔTk受车辆速度的影响，并限制在事先定义的可行区间[0,ΔTmax]。mk(ck+1,ck)为车辆运动学约束，ρk(ck+1,ck)为最小转弯半径约束，ok(ck)为障碍物约束，vk(ck+1,ck,ΔTk)为速度约束，αk(ck+2,ck+1,ck,ΔTk,ΔTk+1)为加速度约束。
(2)运动学约束
本发明使用的车辆模型是经典的阿克曼模型，假设在两个连续的轨迹点之间采用恒定控制uk＝(vk,φk)，因此车辆在两个连续的离散点之间做匀速直线运动(φk＝0)或匀速圆周运动(φk≠0)。令ek＝[xk+1-xk,yk+1-yk,0]T，vk和vk+1分别表示车辆在轨迹点sk与sk+1时朝向角与ek的夹角。为了保证车辆做直线运动或圆周运动，我们需要保证两个离散点同时位于一段恒定曲率的圆弧(或直线)上，当且仅当vk＝vk+1时成立，采用一个更通用的形式来表示车辆的运动学约束，如式6所示。
式(6)对于车量原地打方向盘的情形(dk＝0)同样适用。
最小转弯半径同样是衡量车辆机动性的重要参数，需要根据实际车辆的机械结构对轨迹的最小转弯半径进行约束。当车辆的角度变化Δθk很小时，车辆转弯半径近似如下：
转弯半径约束为ρk(ck+1,ck)＝rk-rmin＞＝0，其中，rmin为车辆最小转弯半径，是车辆的固有参数，提前给定。
(3)动力学约束
考虑到道路限速以及车辆的加速性能，对轨迹的动力学进行约束。车辆在第k个轨迹点的速度与加速度由下式计算。
其中，D(ck+1,ck)表示两个轨迹点之间的欧式距离。本发明不考虑倒车情形，有vk≥0。要求车辆满足最大速度与最大加速度限制，如下式所示：
其中，vmax表示最大线速度，wmax表示最大角速度，amax表示最大加速度。
(4)基于交互式轨迹预测的避障约束
传统的TEB算法在进行动态避障时，将动态障碍物假设为简单的匀速直线运动。然而，现实中这种假设是不符合实际情况。在真实的混合交通场景中，交通参与者的意图会随时间不断变化。同时，为了躲避当前路径上的其他交通代理，交通参与者也需要适当调整自己的运动速度与方向。传统的TEB算法无法对这种意图变化以及代理间的交互进行建模，因此在复杂混合交通场景中表现出的性能难以令人满意。本发明结合其它交通参与者的交互式预测轨迹来计算本车的避障约束ok(ck)，从而进一步提高算法在真实场景的动态避障性能。本发明提出时空距离的概念用来描述障碍物与轨迹点ck的实际距离。如图5所示，对于静态障碍物，时空距离即为障碍物当前位置与轨迹点ck的距离。而对于动态障碍物，首先从其预测轨迹中获取时间T＝T1时(T1为轨迹点ck所对应的时间)的预测位置Pk，随后在统一时间的基础上计算Pk与轨迹点ck的欧氏距离，从而得到时空距离。
采用轨迹点与障碍物的时空距离大于最小距离ζmin作为轨迹规划的避障约束，如式(11)所示。
其中，表示本车轨迹点ck与障碍物O1的时空距离，/>为车辆与障碍物的最小距离，分析可知，动态障碍物与车辆时空距离计算的准确性取决于动态障碍物预测轨迹的准确性，因此将智能体的交互式预测轨迹替换初始匀速直线运动轨迹是非常有意义的。
为了进一步提高轨迹优化的求解效率，本发明将约束作为附加惩罚项并入目标函数中，从而将有约束的非线性优化问题转化为近似非线性平方优化问题。
为了进一步验证本发明的有效性，在summit仿真器中构建若干不同道路结构以及交通流密度的混合交通场景，测试并评估了本发明在这些场景的性能，具体细节如下：
设置3种不同道路结构的仿真场景，分别为交叉路口，高速公路以及换道。同时，基于每种不同的道路结构设置三种不同程度拥挤的交通流，如表1所示。通过对比算法在不同交通流密度下的性能指标，进一步验证算法的鲁棒性。
表1不同拥挤程度场景设置
选择轨迹的安全性、舒适性、有效性来评估本发明的性能。对于安全性，计算车辆平均每走过1m所发生的碰撞次数，并用M.C.(m-1·10-4)表示。对于舒适性，计算车辆平均每米内的停车次数，用M.S.(m-1·10-1)表示。停车次数越多，认为乘客乘车的舒适性越差。对于有效性，计算车辆在混合交通场景中的平均速度，并用M.V.(m/s)表示。平均速度越快，车辆通过混合交通区域时间越短，认为此时算法有效性更高。将三种常用的轨迹规划算法与本发明进行对比，即Roll-out、context-POMDP和TEB。Roll-out算法是一种利用默认策略来确定车辆加速度的反应式决策算法，Context-POMDP算法是指一种使用基于上下文的部分可观测马尔可夫决策过程对运动规划问题进行建模并根据当前车辆状态输出最佳加速度的方法。此外，TEB是一种基于约束非线性优化的轨迹规划方法。
自车从一个固定的起点开始，当发生碰撞或车辆超出场景边界时，认为一个仿真回合结束。为了确保更普遍的比较，本发明在每个仿真环境中测试100个仿真回合，并计算每个度量指标的平均值。结果列在表2中。如表2所示，本发明在大多数场景中表现优于其他基线算法。就安全性而言，与TEB相比，本发明展现出更低的碰撞率，这归因于其考虑了与其他交通参与者的互动。然而，在高速公路场景中，与Roll-out和context-POMDP相比，本发明在碰撞率方面表现不佳。原因是本发明生成更灵活的轨迹，涉及频繁的换道操作，从而增加了与其他车道上的车辆发生碰撞的风险。就舒适性而言，与Roll-out和context-POMDP相比，本发明所述方法表现出较低的停车率。原因在于本发明可以生成更灵活的轨迹，有效地避开障碍物。相比之下，Roll-out和context-POMDP只提供沿着固定路径的加速度信息，导致更频繁的停车和礼让行为。在效率方面，与Roll-out和context-POMDP相比，本发明在大多数场景中有着更高的平均速度。这可以归因于两个因素。首先，本发明减少了停车等待时间。其次，本发明以轨迹时间最短为优化目标，从而可以生成更快和更高效的轨迹。
表2算法性能对比
综上所述，本发明提供了一个针对复杂混合交通场景的自动驾驶汽车轨迹规划算法，该算法考虑了智能体之间的交互性，并最终优化生成一条运动轨迹，利用时间弹性带算法(TEB)的整体优化框架，将车辆运动学约束作为约束条件之一，生成一个时间最优的车辆运动轨迹，为了考虑其它交通参与者之间的运动交互，基于智能体的不可观测状态预测其与周围环境进行交互而产生的协同避障轨迹，基于可靠的交互式轨迹计算本车轨迹与动态障碍物的时空距离，并作为动态避障约束加入到轨迹优化的过程中，大大提高了自动驾驶车辆在复杂交互式环境中的动态避障能力。另外，考虑其它交通参与者状态的不确定性，本发明针对智能体的意图与专注度两种不可观测状态建立概率模型，设计了一个隐状态推理机对不可观测状态的概率分布进行更新。
Claims (10)
1.一种基于TEB优化框架的混合交通场景轨迹规划方法，其特征在于，包括以下步骤：
步骤1，考虑其他交通参与者的运动学约束、协同避障约束以及道路上下文约束，预测其他交通参与者交互式的未来轨迹；
步骤2，基于传统TEB算法的优化框架，考虑其他交通参与者的交互式预测轨迹、车辆的运动动力学约束以及避障约束，并以轨迹时间最短为优化目标生成本车的最优轨迹；对于其他交通参与者的交互式预测轨迹，考虑其他交通参与者的意图和专注度进行预测。
2.根据权利要求1所述的一种基于TEB优化框架的混合交通场景轨迹规划方法，其特征在于，步骤1所述的考虑其他交通参与者的运动学约束具体为：采用一种离散化的方式表示考虑运动学约束下智能体可行速度空间，给定一系列速度朝向角，并计算所述朝向角下的最大可行速度vmax，将所有速度朝向角下的智能体最大速度作为一个速度区域的边界，生成考虑运动约束的可行速度区域Kτ。
3.根据权利要求1所述的一种基于TEB优化框架的混合交通场景轨迹规划方法，其特征在于，步骤1所述的考虑其他交通参与者的协同避障约束具体为：先计算本车的速度障碍物，随后通过给本车和障碍物车辆施加偏置速度使二者的相对速度位于速度障碍物之外，最后做垂直于偏置速度的半平面构建出可行速度半平面；针对每个邻居智能体，获得一个可行速度区域半平面，获得智能体A考虑交互式协同避障的可行速度区域。
4.根据权利要求3所述的一种基于TEB优化框架的混合交通场景轨迹规划方法，其特征在于，将最小偏置速度voff按设定比例分配给本车A和邻居智能体B，并分别表示为uA和uB，最小偏置速度voff的分配由智能体的避碰责任γ来决定，避碰责任γ越大，为其分配更高的偏置速度，使其做出更大的避让，避碰责任γ越小，为其分配相对更小的偏置速度，使其做出更小的避让动作。
5.根据权利要求1所述的一种基于TEB优化框架的混合交通场景轨迹规划方法，其特征在于，步骤1所述的考虑道路上下文约束具体为：当智能体距离道路边界d米处，提供另一个速度约束半平面Cτ对车辆的横向速度进行约束，位于速度平面Cτ中的速度满足vx＜d/τ，τ为规划时间间隔。
6.根据权利要求1所述的一种基于TEB优化框架的混合交通场景轨迹规划方法，其特征在于，步骤1所述的预测其他交通参与者交互式未来轨迹具体为：
基于可行速度空间生成最优预测速度，最优预测速度满足：位于可行速度空间内并且与智能体偏爱速度最接近；在获取最优预测速度之后，基于智能体的运动模型进行状态转移，获取其下一个预测轨迹点，进行最优速度预测以及智能体的状态转移，获取一系列预测轨迹点，最终得到预测轨迹。
7.根据权利要求1所述的一种基于TEB优化框架的混合交通场景轨迹规划方法，其特征在于，步骤2所述的考虑其他交通参与者的交互式预测轨迹具体为：计算本车轨迹与其他交通参与者预测轨迹的时空距离，并将时空距离的大小作为动态障碍物避障约束添加到TEB轨迹优化框架中，对本车轨迹进行优化。
8.根据权利要求7所述的一种基于TEB优化框架的混合交通场景轨迹规划方法，其特征在于，对于静态障碍物，时空距离即为障碍物当前位置与轨迹点的距离；对于动态障碍物，首先从动态障碍物预测轨迹中获取时间T＝T1时的预测位置Pk，随后在统一时间的基础上计算Pk与轨迹点ck的欧氏距离，从而得到时空距离，采用轨迹点与障碍物的时空距离大于最小距离作为轨迹规划的避障约束。
9.根据权利要求1所述的一种基于TEB优化框架的混合交通场景轨迹规划方法，其特征在于，车辆的轨迹由一系列离散点以及离散点之间的时间间隔组成，优化的对象为离散点与时间间隔的集合，轨迹规划问题转化为以轨迹点时间间隔的平方和最小为优化目标、以车辆运动动力学和车辆与障碍物距离为约束函数的有约束非线性优化问题。
10.根据权利要求1所述的一种基于TEB优化框架的混合交通场景轨迹规划方法，其特征在于，运动学具体为：在两个连续的轨迹点之间采用恒定控制，车辆在两个连续的离散点之间做匀速直线运动或匀速圆周运动，vk和vk+1分别表示车辆在轨迹点sk与sk+1时朝向角与ek的夹角，ek表示轨迹点sk与sk+1的连线，两个离散点同时位于一段恒定曲率的圆弧或直线上，当且仅当vk＝vk+1时成立。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310972401.2A CN116933961A (zh) | 2023-08-03 | 2023-08-03 | 一种基于teb优化框架的混合交通场景轨迹规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310972401.2A CN116933961A (zh) | 2023-08-03 | 2023-08-03 | 一种基于teb优化框架的混合交通场景轨迹规划方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116933961A true CN116933961A (zh) | 2023-10-24 |
Family
ID=88382544
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310972401.2A Pending CN116933961A (zh) | 2023-08-03 | 2023-08-03 | 一种基于teb优化框架的混合交通场景轨迹规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116933961A (zh) |
-
2023
- 2023-08-03 CN CN202310972401.2A patent/CN116933961A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113359757B (zh) | 一种无人驾驶车辆路径规划与轨迹跟踪方法 | |
| CN112292646B (zh) | 车辆的控制系统、用于控制车辆的方法和非暂时性计算机可读存储器 | |
| CN108919795B (zh) | 一种自动驾驶汽车换道决策方法及装置 | |
| Wei et al. | A behavioral planning framework for autonomous driving | |
| Cao et al. | Highway exiting planner for automated vehicles using reinforcement learning | |
| WO2019042295A1 (zh) | 一种无人驾驶路径规划方法、系统和装置 | |
| CN112046484B (zh) | 一种基于q学习的车辆变道超车路径规划方法 | |
| CN109324620A (zh) | 基于车道线平行偏移进行避障及超车的动态轨迹规划方法 | |
| CN112965476B (zh) | 一种基于多窗口模型的高速无人车轨迹规划系统及方法 | |
| CN108256233A (zh) | 基于驾驶员风格的智能车轨迹规划及跟踪方法和系统 | |
| CN114312830B (zh) | 一种考虑危险驾驶工况的智能车耦合决策模型及方法 | |
| CN114564016A (zh) | 一种结合路径规划和强化学习的导航避障控制方法、系统及模型 | |
| CN110928297A (zh) | 基于多目标动态粒子群优化的智能公交车辆路径规划方法 | |
| CN114771563A (zh) | 一种自动驾驶车辆轨迹规划控制实现方法 | |
| CN115683145A (zh) | 一种基于轨迹预测的自动驾驶安全避障方法 | |
| Goulet et al. | Distributed maneuver planning with connected and automated vehicles for boosting traffic efficiency | |
| CN114543827A (zh) | 一种路径规划方法及装置 | |
| CN115257746A (zh) | 一种考虑不确定性的自动驾驶汽车换道决策控制方法 | |
| CN113255998A (zh) | 基于多智能体强化学习的高速道路无人驾驶车辆编队方法 | |
| Liu et al. | Impact of sharing driving attitude information: A quantitative study on lane changing | |
| CN112236352A (zh) | 用于确定车辆的经更新的轨迹的方法 | |
| CN117141489B (zh) | 一种基于最小作用量原理的智能车辆轨迹分层规划方法 | |
| CN117864135A (zh) | 基于驾驶员操纵舒适性的车辆变道转向辅助方法 | |
| Park et al. | Path generation algorithm based on crash point prediction for lane changing of autonomous vehicles | |
| CN111121804B (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 |