CN113465625B - 局部路径规划方法和装置 - Google Patents
局部路径规划方法和装置 Download PDFInfo
- Publication number
- CN113465625B CN113465625B CN202110943501.3A CN202110943501A CN113465625B CN 113465625 B CN113465625 B CN 113465625B CN 202110943501 A CN202110943501 A CN 202110943501A CN 113465625 B CN113465625 B CN 113465625B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- preset
- collision risk
- surrounding
- model
- 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
- 238000000034 method Methods 0.000 title claims abstract description 44
- 238000011156 evaluation Methods 0.000 claims abstract description 50
- 239000011159 matrix material Substances 0.000 claims description 40
- 238000012502 risk assessment Methods 0.000 claims description 31
- 230000006870 function Effects 0.000 claims description 30
- 238000004422 calculation algorithm Methods 0.000 claims description 11
- 230000008859 change Effects 0.000 claims description 7
- 238000010586 diagram Methods 0.000 description 7
- 230000033001 locomotion Effects 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 238000004891 communication Methods 0.000 description 4
- 238000005457 optimization Methods 0.000 description 3
- 230000004044 response Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 206010039203 Road traffic accident Diseases 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 230000004913 activation Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 125000004435 hydrogen atom Chemical group [H]* 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 239000004576 sand Substances 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000007723 transport mechanism Effects 0.000 description 1
Images
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
-
- 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/3453—Special cost functions, i.e. other than distance or default speed limit of road segments
- G01C21/3492—Special cost functions, i.e. other than distance or default speed limit of road segments employing speed data or traffic data, e.g. real-time or historical
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
Abstract
本申请公开了一种局部路径规划方法及装置,该方法包括:预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列;其中,周围车辆是指距离第一车辆在预设范围内的车辆,所述周围车辆集合是指全部周围车辆的集合;根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型;根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角,以实现最优化的局部路径规划。本申请公开的方法和装置提高了驾驶效率和安全性能。
Description
技术领域
本申请涉及车辆智能驾驶领域,尤指一种局部路径规划方法和装置。
背景技术
随着社会经济和汽车工业的发展和进步,人们越来越注重汽车的行驶安全。无人驾驶汽车作为减少交通事故的有力手段吸引了众多学者的研究。无人驾驶汽车包括环境感知,全局路径规划,局部路径规划及轨迹跟踪等环节,其中局部路径规划需要输出合适的纵向力和转向角等确保无人驾驶汽车可以安全到达目的地。
目前已有一些学者对局部路径规划进行了研究。郭洪艳(申请号CN202010647239.3)以区域虚拟力场对道路环境进行建模,利用模型预测控制对车辆未来状态进行预测,并基于此完成横纵向耦合路径规划的任务,但该发明未考虑周围障碍物的运动变化,实时性不足;余伶俐(申请号CN201911081063.3)建立无人驾驶车辆多目标路径规划问题的数学模型;利用线性动态规划方法进行路径规划,解决了基于随机撒点的路径规划方法得到的路径难以符合车辆运动学约束的问题,但该发明没有对自车与周围障碍物的碰撞风险进行准确评估;江昆提出一种基于学习采样式的汽车自动驾驶运动规划方法及系统,通过计算每条前向仿真轨迹的评价值,选取评价值最高的轨迹作为规则最优轨迹,但该发明计算量大,算法实时性难以保证。
发明内容
本申请提供了一种局部路径规划方法及装置,解决了以往的局部路径规划算法不够准确,实时性差,对于突发的碰撞风险应对不足的问题,提高了驾驶效率和安全性能。
本申请提供的局部路径规划方法,包括:
预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列;其中,周围车辆是指距离第一车辆在预设范围内的车辆,所述周围车辆集合是指全部周围车辆的集合;
根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型;根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角,以实现最优化的局部路径规划。
一种示例性实施例中,所述预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列,包括:
根据所述周围车辆集合中的每个周围车辆在当前时刻之前预设第二时间段内的第二横向速度序列和第二纵向速度序列通过预先建立的LSTM网络预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列。
一种示例性实施例中,所述根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型包括:
根据所述第一横向速度序列、第一纵向速度序列建立第一车辆与周围车辆集合的碰撞风险评估系数模型;
根据预设的道路边界建立第一车辆与道路边界的碰撞风险评估系数模型;
根据所述第一车辆与周围车辆集合的碰撞风险评估系数模型与所述第一车辆与道路边界的碰撞风险评估系数模型建立碰撞风险评估系数模型。
一种示例性实施例中,所述碰撞风险评估系数模型为:
其中,kNC为RAINC的权重,kR为RAIR的权重,RAINC,i表示第一车辆与周围车辆集合中的第i个周围车辆的碰撞风险评估系数;RAIR,k表示第一车辆与第k条道路的道路边界的碰撞风险评估系数。
一种示例性实施例中,所述预设的第一车辆的动力学模型为:
其中,vx,vy和ω分别表示第一车辆的纵向速度,侧向速度和横摆角速度,m表示第一车辆的总质量,Fx表示驱动轮纵向合力,Fy,f和Fy,r表示前轮和后轮的侧向力,Lf和Lr分别表示前轮和后轮的轮距,Iz表示第一车辆的横摆转动惯量,δ表示前轮转向角。
一种示例性实施例中,根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角,包括:
将所述第一车辆的动力学模型进行线性化和离散化,得到线性化和离散化后的第一车辆的动力学模型;
根据所述碰撞风险评估系数模型、预设的全局轨迹跟踪项建立MPC代价函数;
通过模型预测控制算法求解满足所述MPC代价函数以及预设的车辆动力学和运动学约束的所述线性化和离散化后的第一车辆的动力学模型,得到第一车辆跟踪全局路径的当前时刻的纵向力和转向角。
一种示例性实施例中,线性化和离散化后的第一车辆的动力学模型为:
其中,Ad,Bd和Cd表示离散化后的状态空间矩阵,x(k)和u(k)表示离散化后的状态变量和控制变量,Ns,Nu和Ny分别表示状态量的个数、控制量的个数和模型输出量的个数。
一种示例性实施例中,所述预设的车辆动力学和运动学约束包括道路规则约束、控制量约束、输出量约束。
一种示例性实施例中,所述预设的全局轨迹跟踪项用标准二次式表示为:
其中,
S1=Np-1,S2=Np-2,SN=Np-Nc-2,
H1和g1表示代价函数项J1的hessian矩阵和梯度矩阵,A~和B~表示预测时域内的预测矩阵,Q和R表示状态变量的权重矩阵;Ypre为预测时域内第一车辆的动力学模型的输出状态量,Yref为第一车辆的动力学模型的输出状态量的期望值,反映对控制量平稳变化的要求;为Q,R系数矩阵,Q、R是MPC代价函数中固有的系数矩阵,Np为预测时域,Nc为控制时域,Ny为第一车辆的动力学模型输出状态量的个数;
其中,εk表示k步处的松弛变量。
本申请还提供了一种局部路径规划装置,包括存储器和处理器,
所述存储器,用于保存用于局部路径规划的程序;
所述处理器,用于读取执行所述用于局部路径规划的程序,执行如下操作:
预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列;其中,周围车辆是指距离第一车辆在预设范围内的车辆,所述周围车辆集合是指全部周围车辆的集合;
根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型;根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角,以实现最优化的局部路径规划。
一种示例性实施例中,所述预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列,包括:
根据所述周围车辆集合中的每个周围车辆在当前时刻之前预设第二时间段内的第二横向速度序列和第二纵向速度序列通过预先建立的LSTM网络预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列。
一种示例性实施例中,所述根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型包括:
根据所述第一横向速度序列、第一纵向速度序列建立第一车辆与周围车辆集合的碰撞风险评估系数模型;
根据预设的道路边界建立第一车辆与道路边界的碰撞风险评估系数模型;
根据所述第一车辆与周围车辆集合的碰撞风险评估系数模型与所述第一车辆与道路边界的碰撞风险评估系数模型建立碰撞风险评估系数模型。
一种示例性实施例中,所述碰撞风险评估系数模型为:
其中,kNC为RAINC的权重,kR为RAIR的权重,RAINC,i表示第一车辆与周围车辆集合中的第i个周围车辆的碰撞风险评估系数;RAIR,k表示第一车辆与第k条道路的道路边界的碰撞风险评估系数。
一种示例性实施例中,所述预设的第一车辆的动力学模型为:
其中,vx,vy和ω分别表示第一车辆的纵向速度,侧向速度和横摆角速度,m表示第一车辆的总质量,Fx表示驱动轮纵向合力,Fy,f和Fy,r表示前轮和后轮的侧向力,Lf和Lr分别表示前轮和后轮的轮距,Iz表示第一车辆的横摆转动惯量,δ表示前轮转向角。
一种示例性实施例中,根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角,包括:
将所述第一车辆的动力学模型进行线性化和离散化,得到线性化和离散化后的第一车辆的动力学模型;
根据所述碰撞风险评估系数模型、预设的全局轨迹跟踪项建立MPC代价函数;
通过模型预测控制算法求解满足所述MPC代价函数以及预设的车辆动力学和运动学约束的所述线性化和离散化后的第一车辆的动力学模型,得到第一车辆跟踪全局路径的当前时刻的纵向力和转向角。
一种示例性实施例中,线性化和离散化后的第一车辆的动力学模型为:
其中,Ad,Bd和Cd表示离散化后的状态空间矩阵,x(k)和u(k)表示离散化后的状态变量和控制变量,Ns,Nu和Ny分别表示状态量的个数、控制量的个数和模型输出量的个数。
一种示例性实施例中,所述预设的车辆动力学和运动学约束包括道路规则约束、控制量约束、输出量约束。
一种示例性实施例中,所述预设的全局轨迹跟踪项用标准二次式表示为:
其中,
S1=Np-1,S2=Np-2,SN=Np-Nc-2,
H1和g1表示代价函数项J1的hessian矩阵和梯度矩阵,A~和B~表示预测时域内的预测矩阵,Q和R表示状态变量的权重矩阵;Ypre为预测时域内第一车辆的动力学模型的输出状态量,Yref为第一车辆的动力学模型的输出状态量的期望值,反映对控制量平稳变化的要求;为Q,R系数矩阵,Q、R是MPC代价函数中固有的系数矩阵,Np为预测时域,Nc为控制时域,Ny为第一车辆的动力学模型输出状态量的个数;
其中,εk表示k步处的松弛变量。
本申请的其它特征和优点将在随后的说明书中阐述,并且,部分地从说明书中变得显而易见,或者通过实施本申请而了解。本申请的其他优点可通过在说明书以及附图中所描述的方案来实现和获得。
附图说明
附图用来提供对本申请技术方案的理解,并且构成说明书的一部分,与本申请的实施例一起用于解释本申请的技术方案,并不构成对本申请技术方案的限制。
图1为本申请实施例的局部路径规划方法的原理图;
图2为本申请实施例的局部路径规划方法的流程图;
图3为本申请实施例的LSTM层的原理图。
图4为本申请实施例的自车的三自由度动力学模型。
图5a为本申请实施例的紧急换道场景示意图;
图5b为本申请实施例的基于LSTM预测的局部路径规划方法输出的轨迹图;
图6为本申请实施例的局部路径规划装置的示意图。
具体实施方式
图1为本申请实施例的局部路径规划方法的原理图,如图1所示,本实施例的局部路径规划方法,包括S11-S13步骤:
S11、预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列;
S12、根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型;
S13、根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角,以实现最优化的局部路径规划。
其中,周围车辆是指距离第一车辆在预设范围内的车辆,所述周围车辆集合是指全部周围车辆的集合。
一种示例性的实施例中,所述预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列,包括:
根据所述周围车辆集合中的每个周围车辆在当前时刻之前预设第二时间段内的第二横向速度序列和第二纵向速度序列通过预先建立的LSTM网络预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列。
一种示例性的实施例中,基于highD数据集构建和设计LSTM网络,实现对周围车辆横纵向速度的预测。其中,highD数据集是德国高速公路的大型自然车辆轨迹数据集。LSTM网络的输入是周围车辆过去一段时间的横纵向速度(通过V2V通信获取),输出为预测时域内的横纵向速度序列。
一种示例性的实施例中,所述根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型包括:
根据所述第一横向速度序列、第一纵向速度序列建立第一车辆与周围车辆集合的碰撞风险评估系数模型;
根据预设的道路边界建立第一车辆与道路边界的碰撞风险评估系数模型;
根据所述第一车辆与周围车辆集合的碰撞风险评估系数模型与所述第一车辆与道路边界的碰撞风险评估系数模型建立碰撞风险评估系数模型。
一种示例性的实施例中,所述碰撞风险评估系数模型为:
其中,kNC为RAINC的权重,kR为RAIR的权重,RAINC,i表示第一车辆与周围车辆集合中的第i个周围车辆的碰撞风险评估系数;RAIR,k表示第一车辆与第k条道路的道路边界的碰撞风险评估系数。
一种示例性的实施例中,所述预设的第一车辆的动力学模型为:
其中,vx,vy和ω分别表示第一车辆的纵向速度,侧向速度和横摆角速度,m表示第一车辆的总质量,Fx表示驱动轮纵向合力,Fy,f和Fy,r表示前轮和后轮的侧向力,Lf和Lr分别表示前轮和后轮的轮距,Iz表示第一车辆的横摆转动惯量,δ表示前轮转向角。
一种示例性的实施例中,根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角,包括:
将所述第一车辆的动力学模型进行线性化和离散化,得到线性化和离散化后的第一车辆的动力学模型;
根据所述碰撞风险评估系数模型、预设的全局轨迹跟踪项建立MPC代价函数;
通过模型预测控制算法求解满足所述MPC代价函数以及预设的车辆动力学和运动学约束的所述线性化和离散化后的第一车辆的动力学模型,得到第一车辆跟踪全局路径的当前时刻的纵向力和转向角。
一种示例性的实施例中,线性化和离散化后的第一车辆的动力学模型为:
其中,Ad,Bd和Cd表示离散化后的状态空间矩阵,x(k)和u(k)表示离散化后的状态变量和控制变量,Ns,Nu和Ny分别表示状态量的个数、控制量的个数和模型输出量的个数。
一种示例性的实施例中,所述预设的车辆动力学和运动学约束包括道路规则约束、控制量约束、输出量约束。
一种示例性的实施例中,所述预设的全局轨迹跟踪项用标准二次式表示为:
其中,
S1=Np-1,S2=Np-2,SN=Np-Nc-2,
H1和g1表示代价函数项J1的hessian矩阵和梯度矩阵,和表示预测时域内的预测矩阵,Q和R表示状态变量的权重矩阵;Ypre为预测时域内第一车辆的动力学模型的输出状态量,Yref为第一车辆的动力学模型的输出状态量的期望值,反映对控制量平稳变化的要求;为Q,R系数矩阵,Q、R是MPC代价函数中固有的系数矩阵,Np为预测时域,Nc为控制时域,Ny为第一车辆的动力学模型输出状态量的个数;
其中,εk表示k步处的松弛变量。
本申请实施例的局部路径规划方法通过预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列;根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型;根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角,从而实现了最优化的局部路径规划,解决了以往的局部路径规划算法不够准确,实时性差,对于突发的碰撞风险应对不足的问题,提高了驾驶效率和安全性能,有利于在实车中应用。
图2为本申请实施例的局部路径规划方法的流程图,主要分为3个部分:
第一部分:基于highD数据集构建和设计LSTM网络,实现对周围车辆横纵向速度的预测。LSTM网络的输入是周围车辆过去一段时间的横纵向速度,输出为预测时域内的横纵向速度序列。LSTM网络的预测时域NP=20,由输入层,LSTM层,全连接层以及输出层组成,其中LSTM层的原理如图3所示,包括:
遗忘门,以上一个单元的输出和当前单元的输入组合为输入的sigmoid函数,产生一个在[0,1]内的值,来控制上一单元状态被遗忘的程度,如式(1)所示:
输入门,和一个tanh函数配合控制新信息的通过,产生一个新的候选向量。输入门产生一个在[0,1]内的值,控制新信息被通过的多少,如式(2)所示:
输出门,用来控制当前的单元状态有多少被过滤掉。先将单元状态激活,输出门为其中每一项产生一个在[0,1]内的值,控制单元状态被过滤的程度,如式(3)所示:
结合遗忘门的输出f、输入门的输入i以及tanh激活函数可实现存储信息的更新,如式(4)所示:
LSTM层的输出可表示为:
其中,Wx,i、Wx,f、Wx,o、Wx,g(维度均为R100×1),以及Wh,i、Wh,f、Wh,o、Wh,g(维度均为R100 ×100)表示LSTM的线性变换矩阵,bi、bf、bo、bg表示偏差向量。
第二部分:结合第一部分所预测的周围车辆的横纵向速度,以及道路边界约束,设计碰撞风险评估系数RAI。碰撞风险评估系数的值越大,表示自车越接近周围车辆或道路边界,越容易发生碰撞;
碰撞风险评估系数RAI中考虑周围车辆及道路边界对自车行驶安全的影响,如式(6)所示:
RAINC表示自车与周围车辆的碰撞风险评估系数,RAIR表示自车与道路边界的碰撞风险评估系数,kNC和kR分别为RAINC和kR的权重。
RAI具体计算方法包括以下步骤:
(2.1)自车与周围车辆的碰撞风险评估系数RAINC,如式(7)所示:
si表示第i个周围车辆与自车之间的安全距离,ai和bi表示RAINC的形状和强度参数(ai和bi表示两个系数,形状和强度参数仅表示名字,无特殊含义),dX和dY表示自车与周围车辆纵向和横向距离,X0和Y0表示最小纵向距离和最小横向距离,T0表示安全时间间隔,θ表示自车与周围车辆航向角的夹角,uo,i表示第i个周围车辆的纵向速度,u表示自车(即上文的第一车辆)的纵向速度,an表示舒适系数,Δua,i表示自车和周围车辆的纵向相对速度,Δva,i表示自车和周围车辆的横向相对速度,W、W0、ku和kw表示权重系数,i为正整数。
(2.2)自车与道路边界的碰撞风险评估系数RAIR,如式(8)所示:
ak表示RAIR的形状参数,Da表示道路边界的允许距离,SR,k表示第k条道路边界与自车之间的安全距离。
第三部分:应用模型预测控制算法(MPC)计算实现碰撞风险评估系数最小化以及在车辆动力学和运动学约束内跟踪全局路径的纵向力和转向角,避免自车与周围车辆发生碰撞。MPC算法设计包括以下步骤
(3.1)预测模型:
如图4,考虑自车的纵向运动,侧向运动及横摆运动建立三自由度动力学模型,如式(9)所示:
自车的运动学公式为:
其中,vx、vy和ω分别表示自车纵向速度,侧向速度和横摆角速度,X、Y和表示自车的纵向位置,横向位置和航向角。m表示自车总质量,Fx表示驱动轮纵向合力,Fy,f和Fy,r表示前轮和后轮的侧向力。Lf和Lr分别表示前轮和后轮的轮距,Iz表示自车横摆转动惯量。
基于线性轮胎模型计算车轮侧偏力,
其中,Cf和Cr表示前轮和后轮的侧偏刚度,δ表示前轮转向角。
分别采用泰勒多项式和欧拉方法对上述非线性车辆动力学模型进行线性化和离散化,线性化和离散化后的模型如式(12)所示:
(3.2)车辆动力学和运动学约束:
MPC算法中考虑了道路规则,控制量和输出量的约束。道路规则约束了行驶车速,
控制量约束包括边界限制和增量限制,如下所示
轮胎的侧向和纵向力受摩擦椭圆限制,
Fx_max表示轮胎纵向力的最大值,Fy_max表示轮胎侧向力的最大值,μ表示路面附着系数。
同时,摩擦椭圆随垂向载荷的转移而变化。考虑纵向载荷转移,前后轮的垂向载荷如下所示:
其中,g和h表示重力加速度和质心高度。
因此,轮胎力的椭圆约束如下:
(3.3)最优求解:
将RAI项、全局轨迹跟踪项以及为扩展可行域而设计的松弛变量引入MPC代价函数。全局轨迹跟踪项可以转化为标准二次式,如式(18)所示
其中,
S1=Np-1,S2=Np-2,SN=Np-Nc-2,
H1和g1表示代价函数项J1的hessian矩阵和梯度矩阵,A~和B~表示预测时域内的预测矩阵,Q和R表示状态变量的权重矩阵。
碰撞风险评估系数RAI是一个非线性非凸函数,传统的QP求解器无法直接求解。通常,这类问题可以用序列二次规划(SQP)方法求解,其主要思想是将原非线性非凸问题近似为一系列凸子问题,RAI函数采用式(19)中的泰勒近似法离线逼近,
其中,
其中,X(k)表示k步长的纵向位置,Xobs(k)表示障碍物的纵向位置。
最终,优化问题描述如下:
s.t.(k=1,…Np),
其中,s.t表示受限制于,t+k表示t之后k步处的预测值;表示在t步之后k-1步的控制量,xt+k,t表示在t步之后k步的状态量,xt+k-1,t表示在t步之后k-1步的状态量,x为预测模型的状态量,X为纵向位置;表示代价函数中状态量的等式约束,即上述摩擦椭圆约束的抽象形式;εk表示k步处的松弛变量,Cs和Ds分别表示约束变量的输出矩阵和前馈矩阵。
将上述优化问题转化成近似二次凸优化问题,应用序列二次规划(SQP)求解器得到所需的车轮纵向力和转向角,完成局部路径规划,确保自车安全到达目的地。
图5a为本申请实施例的紧急换道场景示意图;图5b为本申请实施例的基于LSTM预测的局部路径规划方法输出的轨迹图。从图5b中可以看出自车在无碰撞的情况下完成了换道操作,验证了本申请的有效性。
图6为本申请实施例的局部路径规划装置的示意图,如图6所示,局部路径规划装置包括存储器和处理器,其特征在于:
所述存储器,用于保存用于局部路径规划的程序;
所述处理器,用于读取执行所述用于局部路径规划的程序,执行如下方法:
预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列;其中,周围车辆是指距离第一车辆在预设范围内的车辆,所述周围车辆集合是指全部周围车辆的集合;
根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型;根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角,以实现最优化的局部路径规划。
其中,周围车辆是指距离第一车辆在预设范围内的车辆,所述周围车辆集合是指全部周围车辆的集合。
一种示例性的实施例中,所述预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列,包括:
根据所述周围车辆集合中的每个周围车辆在当前时刻之前预设第二时间段内的第二横向速度序列和第二纵向速度序列通过预先建立的LSTM网络预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列。
一种示例性的实施例中,基于highD数据集构建和设计LSTM网络,实现对周围车辆横纵向速度的预测。其中,highD数据集是德国高速公路的大型自然车辆轨迹数据集。LSTM网络的输入是周围车辆过去一段时间的横纵向速度(通过V2V通信获取),输出为预测时域内的横纵向速度序列。
一种示例性的实施例中,所述根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型包括:
根据所述第一横向速度序列、第一纵向速度序列建立第一车辆与周围车辆集合的碰撞风险评估系数模型;
根据预设的道路边界建立第一车辆与道路边界的碰撞风险评估系数模型;
根据所述第一车辆与周围车辆集合的碰撞风险评估系数模型与所述第一车辆与道路边界的碰撞风险评估系数模型建立碰撞风险评估系数模型。
一种示例性的实施例中,所述碰撞风险评估系数模型为:
其中,kNC为RAINC的权重,kR为RAIR的权重,RAINC,i表示第一车辆与周围车辆集合中的第i个周围车辆的碰撞风险评估系数;RAIR,k表示第一车辆与第k条道路的道路边界的碰撞风险评估系数。
一种示例性的实施例中,所述预设的第一车辆的动力学模型为:
其中,vx,vy和ω分别表示第一车辆的纵向速度,侧向速度和横摆角速度,m表示第一车辆的总质量,Fx表示驱动轮纵向合力,Fy,f和Fy,r表示前轮和后轮的侧向力,Lf和Lr分别表示前轮和后轮的轮距,Iz表示第一车辆的横摆转动惯量,δ表示前轮转向角。
一种示例性的实施例中,根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角,包括:
将所述第一车辆的动力学模型进行线性化和离散化,得到线性化和离散化后的第一车辆的动力学模型;
根据所述碰撞风险评估系数模型、预设的全局轨迹跟踪项建立MPC代价函数;
通过模型预测控制算法求解满足所述MPC代价函数以及预设的车辆动力学和运动学约束的所述线性化和离散化后的第一车辆的动力学模型,得到第一车辆跟踪全局路径的当前时刻的纵向力和转向角。
一种示例性的实施例中,线性化和离散化后的第一车辆的动力学模型为:
其中,Ad,Bd和Cd表示离散化后的状态空间矩阵,x(k)和u(k)表示离散化后的状态变量和控制变量,Ns,Nu和Ny分别表示状态量的个数、控制量的个数和模型输出量的个数。
一种示例性的实施例中,所述预设的车辆动力学和运动学约束包括道路规则约束、控制量约束、输出量约束。
一种示例性的实施例中,所述预设的全局轨迹跟踪项用标准二次式表示为:
其中,
S1=Np-1,S2=Np-2,SN=Np-Nc-2,
H1和g1表示代价函数项J1的hessian矩阵和梯度矩阵,A~和B~表示预测时域内的预测矩阵,Q和R表示状态变量的权重矩阵;Ypre为预测时域内第一车辆的动力学模型的输出状态量,Yref为第一车辆的动力学模型的输出状态量的期望值,反映对控制量平稳变化的要求;为Q,R系数矩阵,Q、R是MPC代价函数中固有的系数矩阵,Np为预测时域,Nc为控制时域,Ny为第一车辆的动力学模型输出状态量的个数;
其中,εk表示k步处的松弛变量。
本申请实施例的局部路径规划装置通过预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列;根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型;根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角,从而实现了最优化的局部路径规划,解决了以往的局部路径规划算法不够准确,实时性差,对于突发的碰撞风险应对不足的问题,提高了驾驶效率和安全性能,有利于在实车中应用。
本申请描述了多个实施例,但是该描述是示例性的,而不是限制性的,并且对于本领域的普通技术人员来说显而易见的是,在本申请所描述的实施例包含的范围内可以有更多的实施例和实现方案。尽管在附图中示出了许多可能的特征组合,并在具体实施方式中进行了讨论,但是所公开的特征的许多其它组合方式也是可能的。除非特意加以限制的情况以外,任何实施例的任何特征或元件可以与任何其它实施例中的任何其他特征或元件结合使用,或可以替代任何其它实施例中的任何其他特征或元件。
本申请包括并设想了与本领域普通技术人员已知的特征和元件的组合。本申请已经公开的实施例、特征和元件也可以与任何常规特征或元件组合,以形成由权利要求限定的独特的发明方案。任何实施例的任何特征或元件也可以与来自其它发明方案的特征或元件组合,以形成另一个由权利要求限定的独特的发明方案。因此,应当理解,在本申请中示出和/或讨论的任何特征可以单独地或以任何适当的组合来实现。因此,除了根据所附权利要求及其等同替换所做的限制以外,实施例不受其它限制。此外,可以在所附权利要求的保护范围内进行各种修改和改变。
此外,在描述具有代表性的实施例时,说明书可能已经将方法和/或过程呈现为特定的步骤序列。然而,在该方法或过程不依赖于本文所述步骤的特定顺序的程度上,该方法或过程不应限于所述的特定顺序的步骤。如本领域普通技术人员将理解的,其它的步骤顺序也是可能的。因此,说明书中阐述的步骤的特定顺序不应被解释为对权利要求的限制。此外,针对该方法和/或过程的权利要求不应限于按照所写顺序执行它们的步骤,本领域技术人员可以容易地理解,这些顺序可以变化,并且仍然保持在本申请实施例的精神和范围内。
本领域普通技术人员可以理解,上文中所公开方法中的全部或某些步骤、系统、装置中的功能模块/单元可以被实施为软件、固件、硬件及其适当的组合。在硬件实施方式中,在以上描述中提及的功能模块/单元之间的划分不一定对应于物理组件的划分;例如,一个物理组件可以具有多个功能,或者一个功能或步骤可以由若干物理组件合作执行。某些组件或所有组件可以被实施为由处理器,如数字信号处理器或微处理器执行的软件,或者被实施为硬件,或者被实施为集成电路,如专用集成电路。这样的软件可以分布在计算机可读介质上,计算机可读介质可以包括计算机存储介质(或非暂时性介质)和通信介质(或暂时性介质)。如本领域普通技术人员公知的,术语计算机存储介质包括在用于存储信息(诸如计算机可读指令、数据结构、程序模块或其他数据)的任何方法或技术中实施的易失性和非易失性、可移除和不可移除介质。计算机存储介质包括但不限于RAM、ROM、EEPROM、闪存或其他存储器技术、CD-ROM、数字多功能盘(DVD)或其他光盘存储、磁盒、磁带、磁盘存储或其他磁存储装置、或者可以用于存储期望的信息并且可以被计算机访问的任何其他的介质。此外,本领域普通技术人员公知的是,通信介质通常包含计算机可读指令、数据结构、程序模块或者诸如载波或其他传输机制之类的调制数据信号中的其他数据,并且可包括任何信息递送介质。
Claims (8)
1.一种局部路径规划方法,其特征在于,
预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列;其中,周围车辆是指距离第一车辆在预设范围内的车辆,所述周围车辆集合是指全部周围车辆的集合;
根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型;根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角,以实现最优化的局部路径规划;
根据所述碰撞风险评估系数模型、预设的第一车辆的动力学模型、以及预设的全局轨迹跟踪项计算第一车辆跟踪全局路径的当前时刻的纵向力和转向角,包括:
将所述第一车辆的动力学模型进行线性化和离散化,得到线性化和离散化后的第一车辆的动力学模型;
根据所述碰撞风险评估系数模型、预设的全局轨迹跟踪项建立MPC代价函数;
通过模型预测控制算法求解满足所述MPC代价函数以及预设的车辆动力学和运动学约束的所述线性化和离散化后的第一车辆的动力学模型,得到第一车辆跟踪全局路径的当前时刻的纵向力和转向角;
所述预设的全局轨迹跟踪项用标准二次式表示为:
其中,
S1=Np-1,S2=Np-2,SN=Np-Nc-2,H1和g1表示代价函数项J1的hessian矩阵和梯度矩阵,和表示预测时域内的预测矩阵,Q和R表示状态变量的权重矩阵;Ypre为预测时域内第一车辆的动力学模型的输出状态量,Yref为第一车辆的动力学模型的输出状态量的期望值,反映对控制量平稳变化的要求;为Q,R系数矩阵,Q、R是MPC代价函数中固有的系数矩阵,Np为预测时域,Nc为控制时域,Ny为第一车辆的动力学模型输出状态量的个数;
其中,ε表示松弛变量,εk表示第k步的松弛变量;
Ad,Bd和Cd表示离散化后的状态空间矩阵,Nu表示控制量的个数。
2.如权利要求1所述的局部路径规划方法,所述预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列,包括:
根据所述周围车辆集合中的每个周围车辆在当前时刻之前预设第二时间段内的第二横向速度序列和第二纵向速度序列通过预先建立的LSTM网络预测周围车辆集合中的每个周围车辆在当前时刻之后预设第一时间段内的第一横向速度序列和第一纵向速度序列。
3.如权利要求1所述的局部路径规划方法,所述根据所述第一横向速度序列、第一纵向速度序列建立碰撞风险评估系数模型包括:
根据所述第一横向速度序列、第一纵向速度序列建立第一车辆与周围车辆集合的碰撞风险评估系数模型;
根据预设的道路边界建立第一车辆与道路边界的碰撞风险评估系数模型;
根据所述第一车辆与周围车辆集合的碰撞风险评估系数模型与所述第一车辆与道路边界的碰撞风险评估系数模型建立碰撞风险评估系数模型。
7.如权利要求1所述的局部路径规划方法,包括:
所述预设的车辆动力学和运动学约束包括道路规则约束、控制量约束、输出量约束。
8.一种局部路径规划装置,包括存储器和处理器,其特征在于:
所述存储器,用于保存用于局部路径规划的程序;
所述处理器,用于读取执行所述用于局部路径规划的程序,执行如权利要求1-7任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110943501.3A CN113465625B (zh) | 2021-08-17 | 2021-08-17 | 局部路径规划方法和装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110943501.3A CN113465625B (zh) | 2021-08-17 | 2021-08-17 | 局部路径规划方法和装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113465625A CN113465625A (zh) | 2021-10-01 |
CN113465625B true CN113465625B (zh) | 2022-06-03 |
Family
ID=77866696
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110943501.3A Active CN113465625B (zh) | 2021-08-17 | 2021-08-17 | 局部路径规划方法和装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113465625B (zh) |
Families Citing this family (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114564012B (zh) * | 2022-02-16 | 2024-06-07 | 华南理工大学 | 一种无人赛车路径规划方法、系统、装置及存储介质 |
CN115432010B (zh) * | 2022-10-10 | 2024-06-14 | 南京林业大学 | 一种考虑邻车切入风险的轨迹规划方法 |
CN116909268B (zh) * | 2023-06-30 | 2024-05-28 | 广东省机场管理集团有限公司工程建设指挥部 | 基于5g的代步机器人路径规划方法、装置、设备及介质 |
CN117091618B (zh) * | 2023-10-18 | 2024-01-26 | 理工雷科智途(北京)科技有限公司 | 一种无人驾驶车辆路径规划方法、装置及电子设备 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108674414A (zh) * | 2018-07-02 | 2018-10-19 | 清华大学 | 一种极限工况的智能汽车轨迹跟踪控制方法 |
KR20190012851A (ko) * | 2017-07-28 | 2019-02-11 | 주식회사 만도 | 차량 충돌 방지 장치 및 방법 |
CN109375632A (zh) * | 2018-12-17 | 2019-02-22 | 清华大学 | 自动驾驶车辆实时轨迹规划方法 |
CN111260956A (zh) * | 2020-01-15 | 2020-06-09 | 清华大学 | 一种基于模型预测控制的车辆自动换道规划与控制方法 |
CN111845738A (zh) * | 2020-06-22 | 2020-10-30 | 江苏大学 | 基于双模型复合的车辆路径跟踪控制方法 |
CN112109704A (zh) * | 2020-09-22 | 2020-12-22 | 同济大学 | 一种基于精准轨迹预测的车辆避撞动态安全路径规划方法 |
-
2021
- 2021-08-17 CN CN202110943501.3A patent/CN113465625B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20190012851A (ko) * | 2017-07-28 | 2019-02-11 | 주식회사 만도 | 차량 충돌 방지 장치 및 방법 |
CN108674414A (zh) * | 2018-07-02 | 2018-10-19 | 清华大学 | 一种极限工况的智能汽车轨迹跟踪控制方法 |
CN109375632A (zh) * | 2018-12-17 | 2019-02-22 | 清华大学 | 自动驾驶车辆实时轨迹规划方法 |
CN111260956A (zh) * | 2020-01-15 | 2020-06-09 | 清华大学 | 一种基于模型预测控制的车辆自动换道规划与控制方法 |
CN111845738A (zh) * | 2020-06-22 | 2020-10-30 | 江苏大学 | 基于双模型复合的车辆路径跟踪控制方法 |
CN112109704A (zh) * | 2020-09-22 | 2020-12-22 | 同济大学 | 一种基于精准轨迹预测的车辆避撞动态安全路径规划方法 |
Non-Patent Citations (2)
Title |
---|
Crash Mitigation in Motion Planning for Autonomous Vehicles;王红等;《IEEE Transactions on Intelligent Transportation Systems》;20190117;第20卷(第9期);第3313-3323页 * |
基于并线行为识别的自适应巡航控制方法;蔡英凤等;《汽车工程》;20210730;第43卷(第7期);第1077-1087、1095页 * |
Also Published As
Publication number | Publication date |
---|---|
CN113465625A (zh) | 2021-10-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113465625B (zh) | 局部路径规划方法和装置 | |
Funke et al. | Collision avoidance and stabilization for autonomous vehicles in emergency scenarios | |
Chowdhri et al. | Integrated nonlinear model predictive control for automated driving | |
Lin et al. | Trajectory tracking of autonomous vehicle with the fusion of DYC and longitudinal–lateral control | |
Wang et al. | A framework of vehicle trajectory replanning in lane exchanging with considerations of driver characteristics | |
CN110539752B (zh) | 一种智能汽车多预测范围模型预测轨迹跟踪控制方法及系统 | |
Son et al. | Simulation-based testing framework for autonomous driving development | |
Kang et al. | Observer-based backstepping control method using reduced lateral dynamics for autonomous lane-keeping system | |
Micheli et al. | NMPC trajectory planner for urban autonomous driving | |
Wang et al. | Post-impact motion planning and tracking control for autonomous vehicles | |
Koga et al. | Realization of different driving characteristics for autonomous vehicle by using model predictive control | |
Hima et al. | Controller design for trajectory tracking of autonomous passenger vehicles | |
Ercan et al. | An adaptive and predictive controller design for lateral control of an autonomous vehicle | |
Liu et al. | Moving horizon shared steering strategy for intelligent vehicle based on potential‐hazard analysis | |
Wang et al. | Optimal control-based highway pilot motion planner with stochastic traffic consideration | |
Guo et al. | Adaptive non‐linear coordinated optimal dynamic platoon control of connected autonomous distributed electric vehicles on curved roads | |
Lima et al. | Progress maximization model predictive controller | |
Ko et al. | Integrated path planning and tracking control of autonomous vehicle for collision avoidance based on model predictive control and potential field | |
Liu et al. | Comprehensive predictive control method for automated vehicles in dynamic traffic circumstances | |
CN114779641A (zh) | 基于新航向误差定义的环境自适应mpc路径追踪控制方法 | |
Ghandriz et al. | Trajectory-following and off-tracking minimisation of long combination vehicles: A comparison between nonlinear and linear model predictive control | |
Yue et al. | Automated hazard escaping trajectory planning/tracking control framework for vehicles subject to tire blowout on expressway | |
Elmi et al. | Path planning using model predictive controller based on potential field for autonomous vehicles | |
Fehér et al. | Proving ground test of a ddpg-based vehicle trajectory planner | |
van Lookeren Campagne et al. | A nonlinear model predictive control based evasive manoeuvre assist function |
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 |