CN115092141B - 应用于自动驾驶车辆变道超车的轨迹规划方法及设备 - Google Patents
应用于自动驾驶车辆变道超车的轨迹规划方法及设备 Download PDFInfo
- Publication number
- CN115092141B CN115092141B CN202210723612.8A CN202210723612A CN115092141B CN 115092141 B CN115092141 B CN 115092141B CN 202210723612 A CN202210723612 A CN 202210723612A CN 115092141 B CN115092141 B CN 115092141B
- Authority
- CN
- China
- Prior art keywords
- node
- track
- path
- algorithm
- sampling
- 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
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units, or advanced driver assistance systems for ensuring comfort, stability and safety or drive control systems for propelling or retarding the vehicle
- B60W30/18—Propelling the vehicle
- B60W30/18009—Propelling the vehicle related to particular drive situations
- B60W30/18163—Lane change; Overtaking manoeuvres
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W60/00—Drive control systems specially adapted for autonomous road vehicles
- B60W60/001—Planning or execution of driving tasks
-
- Y—GENERAL 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
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
应用于自动驾驶车辆变道超车的轨迹规划方法及设备,属于机器人运动规划技术领域。为了解决现有的轨迹规划方法存在规划路径效果欠佳的问题,本发明方法在Informed RRT*算法基础上引入了偏置采样节点集合,当循环每进行第b次时,在偏置采样节点集合中随机挑选一个采样偏置节点,在以该节点为圆心,r为半径的圆内进行随机均匀采样;当循环每进行第b次以外的循环时,则在以起点和终点为焦点、cbest为长轴,构建的椭圆内进行随机均匀采样;利用改进的Informed RRT*算法对构型空间进行探索,快速生成可行的路径;然后利用Minimum Jerk算法对生成的路径进行优化处理,得到自动驾驶车辆行驶轨迹。用于自动驾驶车辆变道超车的轨迹规划。
Description
技术领域
本发明涉及一种应用于自动驾驶车辆变道超车的轨迹规划方法及设备,属于机器人运动规划技术领域。
背景技术
自动驾驶车辆是指带有先进的车载传感器、控制器、执行器等装置,具备复杂环境感知、智能决策、规划和控制等功能,且能达到人类驾驶员水平的车辆。自动驾驶技术有潜力大幅减少因驾驶员失误和疏忽而造成的车辆碰撞事故的发生,被认为是解决交通安全问题的有效手段之一。同时,自动驾驶技术的应用可以改善交通拥堵情况。由于人类驾驶员反应时间长,环境感知能力与驾驶决策能力有限,会导致交通堵塞情况的发生。而自动驾驶车辆凭借先进的车载感知设备,可以缩短车辆反应时间,减少车辆间隔距离,减少刹车次数,从而使交通情况得到良好的改善。
完整的自动驾驶系统可以分为以下四个方面:定位、环境感知、决策规划以及运动控制。决策规划模块具体可以分为全局路线规划层,行为决策层,以及运动规划层。首先根据用户目的地以及路网信息通过全局路径规划模块生成一系列在路网中连续的路标点,将其传给行为决策模块。行为决策模块需要依据这些路标点以及环境感知模块传入的行人、障碍物、信号灯等信息来决策合适的驾驶行为,诸如保持沿车道直行、换道、刹车等,并且将其传给局部运动规划模块。最后局部运动规划模块接收到相应的驾驶行为决策信息之后,会依据通过定位模块得到的自身位姿、相应的无碰撞构型空间以及决策模块选择的驾驶行为规划出相应的运动轨迹,并将其传给下层的运动控制模块进行轨迹的跟踪。运动规划作为决策规划模块的核心部分之一,主要功能是生成带有时间信息的轨迹,从而来决定自动驾驶车辆的行驶路线。由于自动驾驶技术对于时间的要求较高,如何在短时间内快速生成高质量的行车轨迹成为了主要的研究方向。
发明内容
本发明是为了解决现有基于RRT、Informed RRT*等算法的轨迹规划方法存在规划路径效果欠佳的问题。
一种应用于自动驾驶车辆变道超车的轨迹规划方法,包括以下步骤:
首先利用改进的Informed RRT*算法对构型空间进行探索,快速生成可行的路径;然后利用Minimum Jerk算法对生成的路径进行优化处理,得到自动驾驶车辆行驶轨迹;
所述利用改进的Informed RRT*算法对构型空间进行探索的过程,包括以下步骤:
步骤1.1、建立节点集合V与线段集合E,将自动驾驶车辆当前位置作为起点加入到节点集合V中,并将V与E联合起来构成搜索树T;建立可行解集合Xsoln并初始化;
步骤1.2、算法进入循环,遍历可行解集合Xsoln,并进行随机采样;当可行解集合Xsoln为空集时,在构型空间内进行随机均匀采样;
当可行解集合Xsoln不为空集时,找到可行解集合Xsoln中累计代价最小的节点,并将其代价记为cbest;每生成一次采样点作为一次循环;
当Informed RRT*算法循环每进行第b次时,在偏置采样节点集合中随机挑选一个采样偏置节点,在以该节点为圆心,r为半径的圆内进行随机均匀采样;其中b与r均为提前设计的参数;当Informed RRT*算法循环每进行第b次以外的循环时,则在以起点和终点为焦点、cbest为长轴,构建的椭圆内进行随机均匀采样;
步骤1.3、针对步骤1.2中采样得到的节点xrand,若采样节点xrand在障碍物内,则返回步骤1.2进行下一循环;若采样节点xrand不在障碍物内,则遍历搜索树T,寻找到距离采样节点xrand最近的节点xnearest,将xnearest向xrand延伸一定距离δ,得到新增节点xnew;若xnearest与xnew所连线段与障碍物不相交,则将xnew加入到搜索树T中;若xnearest与xnew所连线段与障碍物相交,则返回步骤1.2进行下一循环;
步骤1.4、遍历搜索树T,找到距离节点xnew最近的γ个节点;将寻找到的γ个节点共同构成临近节点集合Xnear,遍历临近节点集合Xnear,对于其中每一个节点xnear,若其与xnew所连线段与障碍物相交,则从临近节点集合Xnear中删除该节点;
步骤1.5、对于临近节点集合Xnear中的剩余节点,首先对其进行重连接,然后对集合Xnear中节点进行重布线;
步骤1.6、当找到初始可行路径后,每次循环针对当前路径进行路径优化处理:
以当前最优路径的最后一个节点,即位于终点目标区域的节点xsoln为起始节点,沿搜索树T进行反向追溯,对于任一个节点x,若其父节点的父节点Parent(xparent)与x所连线段不存在碰撞,则修改x的父节点为Parent(xparent),重复进行检测,直到节点x与x’所连线段不存在碰撞,与x’的父节点x”存在碰撞,则将x的父节点最终修改为x’,将x’加入到采样偏置节点集合中,再以x’为起始点重复进行该过程,直到到达起点xstart;
步骤1.7、当算法循环达到设定次数,或算法运行时间达到设定时间,算法跳出循环,输出当前最优路径。
进一步地,步骤1.4中遍历搜索树T,找到距离节点xnew最近的γ个节点,γ表示为:
其中η与τRRT*为提前设计的参数,card(V)为节点集合V的节点数目。
进一步地,步骤1.5所述进行重连接的过程包括以下步骤:
遍历Xnear中的所有节点,找到节点xmin,使得以xmin为xnew的父节点时,起点到达新增节点xnew的代价最小。
进一步地,步骤1.5所述进行重布线的过程包括以下步骤:
对于Xnear中的每一个节点xnear,若以xnew为xnear的父节点时,从起点到达xnear节点的代价变小,则将xnear节点的父节点修改为xnew。
进一步地,利用Minimum Jerk算法对生成的路径进行优化处理,得到自动驾驶车辆行驶轨迹的过程中,对于不符合最小转弯半径约束的轨迹部分,利用多项式螺旋线进行局部修改,最终生成可行的自动驾驶车辆行驶轨迹。
进一步地,利用Minimum Jerk算法对生成的路径进行优化处理的过程包括以下步骤:
步骤2.1、设路径规划算法产生的路径共有M+1个节点,则轨迹函数用M段五次多项式进行表示:
其中,pi,j表示第i段轨迹的第j个系数,时间节点T0,…,TM由后续步骤进行设计;
步骤2.2、构建Minimum Jerk算法中的二次规划问题:
构造目标函数为加速度变化率j(t)的平方项在整个时间范围内的积分;将轨迹函数的三阶导数代入并写为矩阵形式;
Minimum Jerk算法中约束条件分为导数约束与连续性约束;将两种约束写成矩阵形式;
步骤2.3、利用梯形速度曲线对时间进行分配:
设每段多项式轨迹的速度曲线均为梯形,以恒定加速度a从初始速度0加速到最大速度vmax,然后保持速度vmax,最后以恒定加速度a从vmax减速到0;当不存在匀速运动阶段时路径最大长度记为Dmax;
设第i段路径长度为Di,当Di≤Dmax时,第i段分配的时间ti为:
当Di>Dmax时,第i段分配的时间为:
求解每段分配时间后,即可求得各段首尾时间节点:
Ti+1=Ti+ti
其中T0为初始时刻0;
步骤2.4、利用步骤2.2与步骤2.3构建的二次规划问题,分别对路径规划算法生成路径节点序列的横坐标与纵坐标进行求解,得到分段五次多项式系数,并采用后验迭代法对轨迹进行循环迭代优化;
步骤2.5、当Minimum Jerk算法成功输出轨迹参数时,利用参数求解出轨迹的曲率曲线,若轨迹存在不符合最大曲率约束的部分,利用多项式螺旋线进行局部修改;
步骤2.6、针对多项式螺旋线轨迹部分进行速度规划,设该段的速度函数为:
v(t)=b0+b1t+b2t2+b3t3+b4t4+b5t5
其中,bi为待求解系数;
设二次规划目标函数为:
其约束条件为:
其中,v0、a0分别为多项式螺旋线轨迹部分起始点对应的速度和加速度约束;vf、af、sf分别为多项式螺旋线轨迹部分终点对应的速度、加速度和距离约束;
求解该二次规划问题即可得到多项式螺旋线参数;同样利用后验迭代法对该优化问题不断进行循环求解,向超出最大速度或最大加速度的时间节点施加额外的不等式约束,重新进行求解,直到得到符合约束的速度曲线以及加速度曲线;
步骤2.7、将步骤2.4、步骤2.5与步骤2.6中生成的多项式轨迹、多项式螺旋线路径以及速度曲线结合,构成自动驾驶车辆最终行驶轨迹。
进一步地,针对轨迹存在不符合最大曲率约束的部分,利用多项式螺旋线进行局部修改的过程博包括以下步骤:
不符合最大曲率约束部分的曲率曲线表达式为:
k(s)=a0+a1s+a2s2+a3s3
其中,ai为待求解系数,s为弧长;曲线与的切线与横轴的夹角θ写为:
利用辛普森积分公式求得曲线的横坐标与纵坐标;
构建目标函数:
其中,α,β,γ为提前设定好的权重系数,s'为积分符号,sf为多项式螺旋线最终弧长,k(s')为s'对应的曲率;
该优化问题的约束条件为:
其中k(sf)分别表示/>sf处对应的曲率;kmax表示最大曲率,kf表示sf处的曲率;
通过求解该优化问题,得到多项式螺旋线的系数。
进一步地,Minimum Jerk算法中的导数约束与连续性约束的矩阵形式如下:
其中,x0,v0,a0分别为初始位置,速度以及加速度;xM,vM,aM分别为最终位置,速度以及加速度;P为从第1段到第M段轨迹系数构成的向量;上述等式左侧的矩阵和等式右面的x0,v0,a0……xM,vM,aM是对应的,因此等式坐标对应每组xi,vi,ai的三行元素中,下一行是上一行的导数。
进一步地,步骤2.4所述分别对路径规划算法生成路径节点序列的横坐标与纵坐标进行求解,得到分段五次多项式系数,并采用后验迭代法对轨迹进行循环迭代优化的过程包括以下步骤:
利用多项式系数求解出每段多项式的位置、速度以及加速度曲线,进行可行性检测,当生成的曲线符合要求时,将这些参数输出,生成的轨迹即为最后的解;当生成的曲线不符合要求时,针对不符合要求的轨迹段,利用极值所在时间点构建新的不等式约束或进一步修改不符合要求的轨迹段的时间长度,重新进行二次规划问题的构建与求解,直到得到可行解或循环次数达到最大值。
一种应用于自动驾驶车辆变道超车的轨迹规划设备,所述设备包括处理器和存储器,所述存储器中存储有至少一条指令,所述至少一条指令由处理器加载并执行以实现所述的一种应用于自动驾驶车辆变道超车的轨迹规划方法。
有益效果:
1.本发明提出了改进的Informed RRT*算法,用来生成可行路径。相比于InformedRRT*算法添加了偏置采样环节与路径优化环节,本发明可以在相同时间内得到更短的路径,有效地加快了算法结果收敛与最优路径的速度。
2.本发明将无人机领域的Minimum Jerk算法拓展到自动驾驶车辆,可以快速生成符合速度约束与加速度约束的安全轨迹。
3.本发明考虑多项式曲线无法很好地约束轨迹的曲率,提出利用多项式螺旋线对轨迹中不符合最大曲率约束的部分进行局部修改,最终生成自动驾驶车辆可以执行的轨迹。
附图说明
图1为利用改进的Informed RRT*算法搜索路径示意图;
图2为当前规划周期变道超车最佳轨迹;
图3(a)为自动驾驶车辆的速度曲线,图3(b)为自动驾驶车辆的加速度曲线,图3(c)为自动驾驶车辆的曲率曲线;
图4为改进的Informed RRT*算法流程示意图;
图5为Minimum Jerk算法流程示意图;
图6为基于多项式螺旋线轨迹优化算法流程示意图;
图7(a)-图7(d)为基于RRT*算法的路径规划仿真实验结果,其中图7(a)为具有多障碍物的空间,图7(b)、图7(c)、图7(d)分别为循环1000次,循环2000次,循环3000次时的结果;
图8(a)-图8(d)为基于InformedRRT*算法的路径规划仿真实验结果,其中图8(a)为具有多障碍物的空间,图8(b)、图8(c)、图8(d)分别为循环1000次,循环2000次,循环3000次时的结果;
图9(a)-图9(d)为基于本发明算法的路径规划仿真实验结果,其中图9(a)为具有多障碍物的空间,图9(b)、图9(c)、图9(d)分别为循环1000次,循环2000次,循环3000次时的结果。
具体实施方式
本发明主要用于解决自动驾驶车辆变道超车的轨迹规划问题,主要包括基于改进的Informed RRT*算法的路径规划方法以及基于Minimum Jerk算法与多项式螺旋线拟合的轨迹优化方法两部分。下面结合具体实施方式对本发明进行详细说明。
具体实施方式一:
本实施方式所述的一种应用于自动驾驶车辆变道超车的轨迹规划方法,首先利用改进的Informed RRT*算法对构型空间进行探索,快速生成可行的路径;然后利用MinimumJerk算法对生成的路径进行优化处理,得到自动驾驶车辆行驶轨迹,对于不符合最小转弯半径约束的轨迹部分,利用多项式螺旋线进行局部修改,最终生成可行的自动驾驶车辆行驶轨迹。
(一)利用改进的Informed RRT*算法对构型空间进行探索的过程,如图4所示,包括以下步骤:
步骤1.1、进行算法的初始化:
建立节点集合V与线段集合E,将自动驾驶车辆当前位置作为起点加入到节点集合V中,并将V与E联合起来构成搜索树T。建立可行解集合Xsoln,将其初始化为空集。
步骤1.2、算法进入循环,遍历可行解集合Xsoln,并进行随机采样。当可行解集合Xsoln为空集时,在构型空间内进行随机均匀采样。
当可行解集合Xsoln不为空集时,找到可行解集合Xsoln中累计代价最小的节点,并将其代价记为cbest;每生成一次采样点作为一次循环。
当改进的Informed RRT*算法循环次数除以b余数为0时(即循环每进行第b次时),在步骤1.6生成的偏置采样节点集合中随机挑选一个采样偏置节点,在以该节点为圆心,r为半径的圆内进行随机均匀采样;其中b与r均为提前设计的参数。
当改进的Informed RRT*算法循环次数除以b余数不为0时(循环每进行第b次以外的循环),则在以起点和终点为焦点、cbest为长轴,构建的椭圆内进行随机均匀采样。
步骤1.3、针对步骤1.2中采样得到的节点xrand,若采样节点xrand在障碍物内,则返回步骤1.2进行下一循环。若采样节点xrand不在障碍物内,则遍历搜索树T,寻找到距离采样节点xrand最近的节点xnearest,将xnearest向xrand延伸一定距离δ,得到新增节点xnew。若xnearest与xnew所连线段与障碍物不相交,则将xnew加入到搜索树T中。若xnearest与xnew所连线段与障碍物相交,则返回步骤1.2进行下一循环。
步骤1.4、遍历搜索树T,找到距离节点xnew最近的γ个节点,γ可以表示为:
其中η与τRRT*为提前设计的参数,card(V)为节点集合V的节点数目。
将寻找到的γ个节点共同构成临近节点集合Xnear,遍历临近节点集合Xnear,对于其中每一个节点xnear,若其与xnew所连线段与障碍物相交,则从临近节点集合Xnear中删除该节点。
步骤1.5、对于临近节点集合Xnear中的剩余节点,首先对其进行重连接环节:遍历Xnear中的所有节点,找到节点xmin,使得以xmin为xnew的父节点时,起点到达新增节点xnew的代价最小;
然后对集合Xnear中节点进行重布线环节:对于Xnear中的每一个节点xnear,若以xnew为xnear的父节点时,从起点到达xnear节点的代价变小,则将xnear节点的父节点修改为xnew。
步骤1.6、当找到初始可行路径后,每次循环针对当前路径进行路径优化处理:
以当前最优路径的最后一个节点,即位于终点目标区域的节点xsoln为起始节点,沿搜索树T进行反向追溯,对于任一个节点x,若其父节点的父节点Parent(xparent)与x所连线段不存在碰撞,则修改x的父节点为Parent(xparent),重复进行检测,直到节点x与x’所连线段不存在碰撞,与x’的父节点x”存在碰撞,则将x的父节点最终修改为x’,将x’加入到采样偏置节点集合中,再以x’为起始点重复进行该过程,直到到达起点xstart。
步骤1.7、当算法循环达到指定次数,或算法运行时间达到指定时间,算法跳出循环,输出当前最优路径,算法结束。
(二)针对改进的Informed RRT*路径规划算法生成的可行路径,如图5所示,利用Minimum Jerk算法对该路径进行轨迹优化处理,通过二次规划的方式生成自动驾驶车辆可以执行的轨迹。针对该轨迹中不符合最大曲率约束的部分,利用多项式螺旋线对该部分进行局部修改,使之符合自动驾驶车辆的最大曲率约束,具体步骤包括以下步骤:
步骤2.1、设路径规划算法产生的路径共有M+1个节点,则轨迹函数用M段五次多项式进行表示:
其中,pi,j表示第i段轨迹的第j个系数,时间节点T0,…,TM由后续步骤进行设计。
步骤2.2、构建Minimum Jerk算法中的二次规划问题:
Minimum Jerk算法需要令加速度变化率在整段时间内积分最小,同时为避免加速度变化率存在正负,构造目标函数为加速度变化率j(t)的平方项在整个时间范围内的积分,即:
将轨迹函数的三阶导数代入,并写为矩阵形式,可以表示为:
其中Pi为第i段轨迹待求解系数构成的向量,Qi可以表示为:
Minimum Jerk算法中约束条件可以分为导数约束与连续性约束。导数约束指的是初始状态和终止状态的位置、速度、加速度以及中间路标点的位置是给定的;连续性约束指的是相邻段之间的路标点位置、速度、加速度连续。将两种约束可以写成矩阵形式,具体如下所示:
其中,x0,v0,a0分别为初始位置,速度以及加速度;xM,vM,aM分别为最终位置,速度以及加速度;P为从第1段到第M段轨迹系数构成的向量。上述等式左侧的矩阵和等式右面的x0,v0,a0……xM,vM,aM是对应的,因此等式坐标对应每组xi,vi,ai的三行元素中,下一行是上一行的导数;
步骤2.3、利用梯形速度曲线对时间进行分配:
设每段多项式轨迹的速度曲线均为梯形,以恒定加速度a从初始速度0加速到最大速度vmax,然后保持速度vmax,最后以恒定加速度a从vmax减速到0。当不存在匀速运动阶段时,路径最大长度Dmax为:
其中v表示速度;
设第i段路径长度为Di,当Di≤Dmax时,第i段分配的时间ti为:
当Di>Dmax时,第i段分配的时间为:
求解每段分配时间后,即可求得各段首尾时间节点:
Ti+1=Ti+ti
其中T0为初始时刻0。
步骤2.4、利用步骤2.2与步骤2.3构建的二次规划问题,分别对路径规划算法生成路径节点序列的横坐标与纵坐标进行求解,得到分段五次多项式系数。并采用后验迭代法对轨迹进行循环迭代优化。如图6所示,具体方式为利用多项式系数求解出每段多项式的位置、速度以及加速度曲线,进行可行性检测,当生成的曲线符合要求时,将这些参数输出,生成的轨迹即为最后的解。当生成的曲线不符合要求时,针对不符合要求的轨迹段,利用极值所在时间点构建新的不等式约束或进一步修改不符合要求的轨迹段的时间长度,重新进行二次规划问题的构建与求解,直到得到可行解或循环次数达到最大值,算法求解失败。
步骤2.5、当Minimum Jerk算法成功输出轨迹参数时,利用参数求解出轨迹的曲率曲线,若轨迹存在不符合最大曲率约束的部分,则对于该部分利用三次多项式螺旋线进行局部修改,不符合最大曲率约束部分的曲率曲线表达式为:
k(s)=a0+a1s+a2s2+a3s3
其中,ai为待求解系数,s为弧长。曲线与的切线与横轴的夹角θ可以写为:
利用辛普森积分公式可以求得曲线的横坐标与纵坐标分别为:
构建目标函数为:
其中,α,β,γ为提前设定好的权重系数,s'为积分符号,sf为多项式螺旋线最终弧长,k(s')为s'对应的曲率,
该优化问题的约束条件为:
其中k(sf)分别表示/>sf处对应的曲率;kmax表示最大曲率,kf表示sf处的曲率;/>
通过求解该优化问题,可以得到多项式螺旋线的系数。
步骤2.6、针对多项式螺旋线轨迹部分进行速度规划,设该段的速度函数为:
v(t)=b0+b1t+b2t2+b3t3+b4t4+b5t5
其中,bi为待求解系数。
设二次规划目标函数为:
其约束条件为:
求解该二次规划问题即可得到多项式螺旋线参数。同样利用后验迭代法对该优化问题不断进行循环求解,向超出最大速度或最大加速度的时间节点施加额外的不等式约束,重新进行求解,直到得到符合约束的速度曲线以及加速度曲线。
步骤2.7、将步骤2.4、步骤2.5与步骤2.6中生成的多项式轨迹、多项式螺旋线路径以及速度曲线结合,构成自动驾驶车辆最终行驶轨迹。
具体实施方式二:
本实施方式为一种应用于自动驾驶车辆变道超车的轨迹规划设备,所述设备包括处理器和存储器,应当理解,包括本发明描述的任何包括处理器和存储器的设备,设备还可以包括其他通过信号或指令进行显示、交互、处理、控制等以及其他功能的单元、模块;
所述存储器中存储有至少一条指令,所述至少一条指令由处理器加载并执行以实现所述的一种应用于自动驾驶车辆变道超车的轨迹规划方法。应当理解,包括本发明描述的任何方法对应的可以被提供为计算机程序产品、软件或计算机化方法,其可以包括其上存储有指令的非暂时性机器可读介质,所述指令可以用于编程计算机系统,或其他电子装置。存储介质可以包括但不限于磁存储介质,光存储介质;磁光存储介质包括:只读存储器ROM、随机存取存储器RAM、可擦除可编程存储器(例如,EPROM和EEPROM)以及闪存层;或者适合于存储电子指令的其他类型的介质。
实施例
利用本发明改进的Informed RRT*搜索方法进行轨迹规划。
步骤1:结合感知模块、预测模块、定位模块等模块传输的信息构建障碍物地图。
步骤2:进行算法的初始化。建立节点集合V与线段集合E,将自动驾驶车辆当前位置作为起点加入到节点集合V中,并将V与E联合起来构成搜索树T。建立可行解集合Xsoln,将其初始化为空集。
步骤3:算法进入循环,遍历可行解集合Xsoln,并进行随机采样。当可行解集合Xsoln为空集时,在构型空间内进行随机均匀采样。
当可行解集合Xsoln不为空集时,找到集合中累计代价最小的节点,并将其代价记为cbest。循环每进行5次,在偏置采样节点集合中随机挑选一个采样偏置节点,在以该节点为圆心,0.75m为半径的圆内进行随机均匀采样。其余循环则在以起点和终点为焦点,cbest为长轴,构建的椭圆内进行随机均匀采样。
步骤4:针对步骤3中采样得到的节点xrand,若采样节点xrand在障碍物内,则进行下一循环。若采样节点xrand不在障碍物内,则遍历搜索树T,寻找到距离xrand最近的节点xnearest,将xnearest向xrand延伸2m,得到新增节点xnew。若xnearest与xnew所连线段与障碍物不相交,则将xnew加入到节点集合V中。若xnearest与xnew所连线段与障碍物相交,则进行下一循环。
步骤5:遍历搜索树T,找到距离节点xnew为4m以内的所有节点,将寻找到的所有节点共同构成临近节点集合Xnear,遍历临近节点集合Xnear,对于其中每一个节点xnear,若其与xnew所连线段与障碍物相交,则从临近节点集合Xnear中删除该节点。
步骤6:对于临近节点集合Xnear中的剩余节点,首先对其进行重连接环节。遍历Xnear中的所有节点,找到节点xmin,使得以xmin为xnew的父节点时,起点到达新增节点xnew的代价最小。然后对集合Xnear中节点进行重布线环节,对于Xnear中的每一个节点xnear,若以xnew为xnear的父节点时,从起点到达xnear节点的代价变小,则将xnear节点的父节点修改为xnew。
步骤7:当找到初始可行路径后,每次循环针对当前路径进行路径优化处理。以当前最优路径的最后一个节点,即位于终点目标区域的节点xsoln为起始节点,沿搜索树T进行反向追溯,对于任一个节点x,若其父节点的父节点Parent(xparent)与x所连线段不存在碰撞,则修改x的父节点为Parent(xparent),重复进行循环检测,直到节点x与x’所连线段不存在碰撞,与x’的父节点x”存在碰撞,则将x的父节点最终修改为x’,将x’加入到采样偏置节点集合中,再以x’为起始点重复进行该过程,直到到达起点xstart。
步骤8:当算法循环达到指定次数后,算法跳出循环,输出当前最优路径,算法结束。
在每个规划周期内,由预测模块预测出目标车辆及周围车辆在一定时间内的行驶轨迹,并将其作为障碍物生成构型空间,利用改进的Informed RRT*算法生成可行路径。以第一个规划周期为例,如图1所示,蓝色方块(起始位置的深颜色方块)为自动驾驶车辆接收到决策模块变道超车指令时的位置,作为当前规划周期内采样搜索算法的起点,绿色方块(车道前进方向一直处于左侧区域的下方的方块)为变道超车目标车辆的当前位置,土黄色方块(车道前进方向一直处于左侧区域的上方的方块)为预测目标车辆最终位置,黄色长条(车道前进方向一直处于左侧区域的长条区域)为目标车辆预测行驶轨迹。图中红色线段(颜色深的细线)为初始连线,绿色线段(颜色浅的细线)为经过重布线环节后重新修改的连线,黄色线段为经过重布线环节修改的原线段,椭圆为以起点和终点为焦点,当前最佳路径为长轴所构成的当前采样空间,较粗的蓝色线条为以终点目标区域的圆心为终点的最短路径。
生成路径后,进行轨迹优化处理,得到的自动驾驶车辆可行轨迹如图2所示。对应的自动驾驶车辆的速度、加速度以及曲线曲率如图3(a)至图3(c)所示。
为了说明本发明的效果,本实施例还对RRT*、Informed RRT*进行了仿真,将本发明的方法中改进的Informed RRT*算法记为Informed RRT*-Smart算法,仿真如表1所示,
表1 RRT*、Informed RRT*、Informed RRT*-Smart算法仿真实验
针对如图7(a)所示的具有多障碍物的空间,左上角红点处为起始点,右下角绿点处为终点,设定步长δ为30,终点目标区域半径为50,设与新增节点距离在60以内的节点均为临近节点。其中一次实验结果如图7(b)、图7(c)、图7(d)所示,分别为循环1000次,循环2000次,循环3000次时的结果。
实验结果中红色线段(颜色深的细线)为初始连线,绿色线段(颜色浅的细线)为经过重布线环节后重新修改的连线,黄色线段为经过重布线环节修改的原线段。当循环1000次时,搜索树中节点数为559,最终路径长度为1095.5,路径节点数为27,用时2.769秒;循环2000次时,搜索树中节点数为1223,最终路径长度为1093.2,路径节点数为25,用时10.268秒;循环3000次时,搜索树中节点数为1924,最终路径长度为1047.4,路径节点数为24,用时21.131秒。
针对如图8(a)所示的具有多障碍物的空间,左上角红点处为起始点,右下角绿点处为终点,设定步长δ为30,终点目标区域半径为50,设与新增节点距离在60以内的节点均为临近节点。其中一次仿真实验的结果如图8(b)、图8(c)、图8(d)所示,分别为循环1000次,循环2000次,循环3000次时的结果。
实验结果中红色线段为初始连线,绿色线段为经过重布线环节后重新修改的连线,黄色线段为经过重布线环节修改的原线段,椭圆为以起点和终点为焦点,当前最优路径为长轴所构成的当前增强采样空间。当循环1000次时,搜索树中节点数为504,最终路径长度为1096.4,路径节点数为25,用时2.524秒;循环2000次时,搜索树中节点数为1116,最终路径长度为1074.8,路径节点数为25,用时9.78秒;循环3000次时,搜索树中节点数为1807,最终路径长度为1031.5,路径节点数为24,用时21.864秒。
针对如图9(a)所示的具有多障碍物的空间,左上角红点处为起始点,右下角绿点处为终点,设定步长δ为30,终点目标区域半径为50,设与新增节点距离在60以内的节点均为临近节点,偏置采样循环间隔次数b为5,偏置采样半径为20。其中一次仿真实验的结果如图9(b)、图9(c)、图9(d)所示,分别为循环1000次,循环2000次,循环3000次时的结果。
实验结果中红色线段为初始连线,绿色线段为经过重布线环节后重新修改的连线,黄色线段为经过重布线环节修改的原线段,椭圆为以起点和终点为焦点,当前最优路径为长轴所构成的当前增强采样空间。其中循环1000次时,搜索树中节点数为473,最终路径长度为1085.6,路径节点数为6,用时3.037秒;循环2000次时,搜索树中节点数为1125,最终路径长度为1041.0,路径节点数为6,用时12.259秒;循环3000次时,搜索树中节点数为1845,最终路径长度为1020.0,路径节点数为6,用时26.455秒。
本发明还可有其它多种实施例,在不背离本发明精神及其实质的情况下,本领域技术人员当可根据本发明作出各种相应的改变和变形,但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。
Claims (9)
1.一种应用于自动驾驶车辆变道超车的轨迹规划方法,其特征在于,包括以下步骤:
首先利用改进的Informed RRT*算法对构型空间进行探索,快速生成可行的路径;然后利用Minimum Jerk算法对生成的路径进行优化处理,得到自动驾驶车辆行驶轨迹;
所述利用改进的Informed RRT*算法对构型空间进行探索的过程,包括以下步骤:
步骤1.1、建立节点集合V与线段集合E,将自动驾驶车辆当前位置作为起点加入到节点集合V中,并将V与E联合起来构成搜索树T;建立可行解集合Xsoln并将Xsoln初始化为空集;
步骤1.2、算法进入循环,遍历可行解集合Xsoln,并进行随机采样;当可行解集合Xsoln为空集时,在构型空间内进行随机均匀采样;
当可行解集合Xsoln不为空集时,找到可行解集合Xsoln中累计代价最小的节点,并将其代价记为cbest;每生成一次采样点作为一次循环;
当改进的Informed RRT*算法循环每进行第b次时,在偏置采样节点集合中随机挑选一个采样偏置节点,在以该节点为圆心,r为半径的圆内进行随机均匀采样;其中b与r均为提前设计的参数;当改进的Informed RRT*算法循环每进行第b次以外的循环时,则在以起点和终点为焦点、cbest为长轴,构建的椭圆内进行随机均匀采样;
步骤1.3、针对步骤1.2中采样得到的节点xrand,若采样节点xrand在障碍物内,则返回步骤1.2进行下一循环;若采样节点xrand不在障碍物内,则遍历搜索树T,寻找到距离采样节点xrand最近的节点xnearest,将xnearest向xrand延伸一定距离δ,得到新增节点xnew;若xnearest与xnew所连线段与障碍物不相交,则将xnew加入到搜索树T中;若xnearest与xnew所连线段与障碍物相交,则返回步骤1.2进行下一循环;
步骤1.4、遍历搜索树T,找到距离节点xnew最近的γ个节点;将寻找到的γ个节点共同构成临近节点集合Xnear,遍历临近节点集合Xnear,对于其中每一个节点xnear,若其与xnew所连线段与障碍物相交,则从临近节点集合Xnear中删除该节点;
步骤1.5、对于临近节点集合Xnear中的剩余节点,首先对其进行重连接,然后对集合Xnear中节点进行重布线;
步骤1.6、当找到初始可行路径后,每次循环针对当前路径进行路径优化处理:
以当前最优路径的最后一个节点,即位于终点目标区域的节点xsoln为起始节点,沿搜索树T进行反向追溯,对于任一个节点x,若其父节点的父节点Parent(xparent)与x所连线段不存在碰撞,则修改x的父节点为Parent(xparent),重复进行检测,直到节点x与x’所连线段不存在碰撞,与x’的父节点x”存在碰撞,则将x的父节点最终修改为x’,将x’加入到采样偏置节点集合中,再以x’为起始点重复进行该过程,直到到达起点xstart;
步骤1.7、当算法循环达到设定次数,或算法运行时间达到设定时间,算法跳出循环,输出当前最优路径;
利用Minimum Jerk算法对生成的路径进行优化处理的过程包括以下步骤:
步骤2.1、设路径规划算法产生的路径共有M+1个节点,则轨迹函数用M段五次多项式进行表示:
其中,pi,j表示第i段轨迹的第j个系数,时间节点T0,…,TM由后续步骤进行设计;
步骤2.2、构建Minimum Jerk算法中的二次规划问题:
构造目标函数为加速度变化率j(t)的平方项在整个时间范围内的积分;将轨迹函数的三阶导数代入并写为矩阵形式;
Minimum Jerk算法中约束条件分为导数约束与连续性约束;将两种约束写成矩阵形式;
步骤2.3、利用梯形速度曲线对时间进行分配:
设每段多项式轨迹的速度曲线均为梯形,以恒定加速度a从初始速度0加速到最大速度vmax,然后保持速度vmax,最后以恒定加速度a从vmax减速到0;当不存在匀速运动阶段时路径最大长度记为Dmax;
设第i段路径长度为Di,当Di≤Dmax时,第i段分配的时间ti为:
当Di>Dmax时,第i段分配的时间为:
求解每段分配时间后,即可求得各段首尾时间节点:
Ti+1=Ti+ti
其中T0为初始时刻0;
步骤2.4、利用步骤2.2与步骤2.3构建的二次规划问题,分别对路径规划算法生成路径节点序列的横坐标与纵坐标进行求解,得到分段五次多项式系数,并采用后验迭代法对轨迹进行循环迭代优化;
步骤2.5、当Minimum Jerk算法成功输出轨迹参数时,利用参数求解出轨迹的曲率曲线,若轨迹存在不符合最大曲率约束的部分,利用多项式螺旋线进行局部修改;
步骤2.6、针对多项式螺旋线轨迹部分进行速度规划,设该段的速度函数为:
v(t)=b0+b1t+b2t2+b3t3+b4t4+b5t5
其中,bi为待求解系数;
设二次规划目标函数为:
其约束条件为:
其中,v0、a0分别为多项式螺旋线轨迹部分起始点对应的速度和加速度约束;vf、af、sf分别为多项式螺旋线轨迹部分终点对应的速度、加速度和距离约束;
求解该二次规划问题即可得到多项式螺旋线参数;同样利用后验迭代法对步骤2.6的二次规划问题不断进行循环求解,向超出最大速度或最大加速度的时间节点施加额外的不等式约束,重新进行求解,直到得到符合约束的速度曲线以及加速度曲线;
步骤2.7、将步骤2.4、步骤2.5与步骤2.6中生成的多项式轨迹、多项式螺旋线路径以及速度曲线结合,构成自动驾驶车辆最终行驶轨迹。
2.根据权利要求1所述的一种应用于自动驾驶车辆变道超车的轨迹规划方法,其特征在于,步骤1.4中遍历搜索树T,找到距离节点xnew最近的γ个节点,γ表示为:
其中η与τRRT*为提前设计的参数,card(V)为节点集合V的节点数目。
3.根据权利要求2所述的一种应用于自动驾驶车辆变道超车的轨迹规划方法,其特征在于,步骤1.5所述进行重连接的过程包括以下步骤:
遍历Xnear中的所有节点,找到节点xmin,使得以xmin为xnew的父节点时,起点到达新增节点xnew的代价最小。
4.根据权利要求3所述的一种应用于自动驾驶车辆变道超车的轨迹规划方法,其特征在于,步骤1.5所述进行重布线的过程包括以下步骤:
对于Xnear中的每一个节点xnear,若以xnew为xnear的父节点时,从起点到达xnear节点的代价变小,则将xnear节点的父节点修改为xnew。
5.根据权利要求1、2、3或4所述的一种应用于自动驾驶车辆变道超车的轨迹规划方法,其特征在于,利用Minimum Jerk算法对生成的路径进行优化处理,得到自动驾驶车辆行驶轨迹的过程中,对于不符合最小转弯半径约束的轨迹部分,利用多项式螺旋线进行局部修改,最终生成可行的自动驾驶车辆行驶轨迹。
6.根据权利要求5所述的一种应用于自动驾驶车辆变道超车的轨迹规划方法,其特征在于,针对轨迹存在不符合最大曲率约束的部分,利用多项式螺旋线进行局部修改的过程博包括以下步骤:
不符合最大曲率约束部分的曲率曲线表达式为:
k(s)=a0+a1s+a2s2+a3s3
其中,ai为待求解系数,s为弧长;曲线与的切线与横轴的夹角θ写为:
利用辛普森积分公式求得曲线的横坐标与纵坐标;
构建目标函数:
其中,α,β,γ为提前设定好的权重系数,s'为积分符号,sf为多项式螺旋线最终弧长,k(s')为s'对应的曲率;
该优化问题的约束条件为:
其中k(sf)分别表示/>sf处对应的曲率;kmax表示最大曲率,kf表示sf处的曲率;
通过求解该优化问题,得到多项式螺旋线的系数。
7.根据权利要求6所述的一种应用于自动驾驶车辆变道超车的轨迹规划方法,其特征在于,Minimum Jerk算法中的导数约束与连续性约束的矩阵形式如下:
其中,x0,v0,a0分别为初始位置,速度以及加速度;xM,vM,aM分别为最终位置,速度以及加速度;P为从第1段到第M段轨迹系数构成的向量;上述等式左侧的矩阵和等式右面的x0,v0,a0……xM,vM,aM是对应的,因此等式坐标对应每组xi,vi,ai的三行元素中,下一行是上一行的导数。
8.根据权利要求7所述的一种应用于自动驾驶车辆变道超车的轨迹规划方法,其特征在于,步骤2.4所述分别对路径规划算法生成路径节点序列的横坐标与纵坐标进行求解,得到分段五次多项式系数,并采用后验迭代法对轨迹进行循环迭代优化的过程包括以下步骤:
利用多项式系数求解出每段多项式的位置、速度以及加速度曲线,进行可行性检测,当生成的曲线符合要求时,将这些参数输出,生成的轨迹即为最后的解;当生成的曲线不符合要求时,针对不符合要求的轨迹段,利用极值所在时间点构建新的不等式约束或进一步修改不符合要求的轨迹段的时间长度,重新进行二次规划问题的构建与求解,直到得到可行解或循环次数达到最大值。
9.一种应用于自动驾驶车辆变道超车的轨迹规划设备,其特征在于,所述设备包括处理器和存储器,所述存储器中存储有至少一条指令,所述至少一条指令由处理器加载并执行以实现如权利要求1至8之一所述的一种应用于自动驾驶车辆变道超车的轨迹规划方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210723612.8A CN115092141B (zh) | 2022-06-23 | 2022-06-23 | 应用于自动驾驶车辆变道超车的轨迹规划方法及设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210723612.8A CN115092141B (zh) | 2022-06-23 | 2022-06-23 | 应用于自动驾驶车辆变道超车的轨迹规划方法及设备 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115092141A CN115092141A (zh) | 2022-09-23 |
CN115092141B true CN115092141B (zh) | 2023-07-21 |
Family
ID=83292472
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210723612.8A Active CN115092141B (zh) | 2022-06-23 | 2022-06-23 | 应用于自动驾驶车辆变道超车的轨迹规划方法及设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115092141B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115826591B (zh) * | 2023-02-23 | 2023-04-18 | 中国人民解放军海军工程大学 | 一种基于神经网络估计路径代价的多目标点路径规划方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9969386B1 (en) * | 2017-01-10 | 2018-05-15 | Mitsubishi Electric Research Laboratories, Inc. | Vehicle automated parking system and method |
CN109945882A (zh) * | 2019-03-27 | 2019-06-28 | 上海交通大学 | 一种无人驾驶车辆路径规划与控制系统及方法 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108983780A (zh) * | 2018-07-24 | 2018-12-11 | 武汉理工大学 | 一种基于改进rrt*算法的移动机器人路径规划方法 |
CN111752306B (zh) * | 2020-08-14 | 2022-07-12 | 西北工业大学 | 一种基于快速扩展随机树的无人机航路规划方法 |
CN112092810B (zh) * | 2020-09-24 | 2022-06-14 | 上海汽车集团股份有限公司 | 一种车辆泊出方法、装置及电子设备 |
CN113219998B (zh) * | 2021-06-15 | 2022-07-05 | 合肥工业大学 | 一种基于改进双向informed-RRT*的车辆路径规划方法 |
-
2022
- 2022-06-23 CN CN202210723612.8A patent/CN115092141B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9969386B1 (en) * | 2017-01-10 | 2018-05-15 | Mitsubishi Electric Research Laboratories, Inc. | Vehicle automated parking system and method |
CN109945882A (zh) * | 2019-03-27 | 2019-06-28 | 上海交通大学 | 一种无人驾驶车辆路径规划与控制系统及方法 |
Also Published As
Publication number | Publication date |
---|---|
CN115092141A (zh) | 2022-09-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112292646B (zh) | 车辆的控制系统、用于控制车辆的方法和非暂时性计算机可读存储器 | |
US11900797B2 (en) | Autonomous vehicle planning | |
CN110834644B (zh) | 一种车辆控制方法、装置、待控制车辆及存储介质 | |
US11783232B2 (en) | System and method for multi-agent reinforcement learning in a multi-agent environment | |
Ajanovic et al. | Search-based optimal motion planning for automated driving | |
Wei et al. | A behavioral planning framework for autonomous driving | |
Li et al. | Game theoretic modeling of vehicle interactions at unsignalized intersections and application to autonomous vehicle control | |
Lin et al. | Decision making through occluded intersections for autonomous driving | |
Li et al. | Planning and decision-making for connected autonomous vehicles at road intersections: A review | |
JP2020126619A (ja) | V2x通信及びイメージ処理を利用した情報融合によって自律走行の短期経路をプランニングするための方法及び装置{method and device for short−term path planning of autonomous driving through information fusion by using v2x communication and image processing} | |
CN111833597B (zh) | 具有规划控制的交通情形中的自主决策 | |
CN113682312B (zh) | 一种融合深度强化学习的自主换道方法及系统 | |
US20210271988A1 (en) | Reinforcement learning with iterative reasoning for merging in dense traffic | |
CN113581182B (zh) | 基于强化学习的自动驾驶车辆换道轨迹规划方法及系统 | |
Onishi et al. | End-to-end learning method for self-driving cars with trajectory recovery using a path-following function | |
Aradi et al. | Policy gradient based reinforcement learning approach for autonomous highway driving | |
Dong et al. | Interactive ramp merging planning in autonomous driving: Multi-merging leading PGM (MML-PGM) | |
CN115092141B (zh) | 应用于自动驾驶车辆变道超车的轨迹规划方法及设备 | |
CN114564016A (zh) | 一种结合路径规划和强化学习的导航避障控制方法、系统及模型 | |
Qiao et al. | Behavior planning at urban intersections through hierarchical reinforcement learning | |
WO2023097874A1 (zh) | 用于行驶轨迹规划的方法和装置 | |
Yalamanchi et al. | Long-term prediction of vehicle behavior using short-term uncertainty-aware trajectories and high-definition maps | |
Luo et al. | Real-time cooperative vehicle coordination at unsignalized road intersections | |
Gu et al. | Safe-state enhancement method for autonomous driving via direct hierarchical reinforcement learning | |
Guo et al. | Self-defensive coordinated maneuvering of an intelligent vehicle platoon in mixed traffic |
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 |