CN116048081A - 一种考虑安全边界约束的自动驾驶车辆决策与规控方法 - Google Patents
一种考虑安全边界约束的自动驾驶车辆决策与规控方法 Download PDFInfo
- Publication number
- CN116048081A CN116048081A CN202310035839.8A CN202310035839A CN116048081A CN 116048081 A CN116048081 A CN 116048081A CN 202310035839 A CN202310035839 A CN 202310035839A CN 116048081 A CN116048081 A CN 116048081A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- time
- decision
- path
- speed
- 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
- 238000000034 method Methods 0.000 title claims abstract description 61
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 17
- 230000008878 coupling Effects 0.000 claims abstract description 16
- 238000010168 coupling process Methods 0.000 claims abstract description 16
- 238000005859 coupling reaction Methods 0.000 claims abstract description 16
- 230000000694 effects Effects 0.000 claims abstract description 10
- 238000005070 sampling Methods 0.000 claims description 39
- 230000033001 locomotion Effects 0.000 claims description 30
- 238000004364 calculation method Methods 0.000 claims description 18
- 230000008569 process Effects 0.000 claims description 15
- 238000005457 optimization Methods 0.000 claims description 13
- 230000008859 change Effects 0.000 claims description 11
- 238000010586 diagram Methods 0.000 claims description 8
- 238000006073 displacement reaction Methods 0.000 claims description 7
- 230000003247 decreasing effect Effects 0.000 claims description 3
- 238000005315 distribution function Methods 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- NRNCYVBFPDDJNE-UHFFFAOYSA-N pemoline Chemical compound O1C(N)=NC(=O)C1C1=CC=CC=C1 NRNCYVBFPDDJNE-UHFFFAOYSA-N 0.000 claims description 3
- 230000003068 static effect Effects 0.000 description 8
- 238000013461 design Methods 0.000 description 6
- 230000007613 environmental effect Effects 0.000 description 3
- 238000002474 experimental method Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000003542 behavioural effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 125000004122 cyclic group Chemical group 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 239000000725 suspension Substances 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
-
- 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)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明适用于自动驾驶汽车控制技术领域，提供了一种考虑安全边界约束的自动驾驶车辆决策与规控方法，包括以下步骤：首先搭建行为决策模块，将路径及速度信息作用于规划控制器；然后由车辆三自由度耦合模型得到非线性预测模型，考虑车辆物理约束和安全边界约束，利用模型预测控制算法构造代价函数，并对其进行求解，完成轨迹规划任务；最后，将获得的控制输入作用于车辆系统，实现对车辆轨迹跟踪的控制效果。本发明在考虑车辆行驶安全边界的基础上对决策、规划和控制模块进行集成化控制，从而有效提升行车安全性，减少决策与规控的信息偏差对系统所带来的干扰，并有效应对系统的强非线性、强耦合性等特点，满足系统对控制精度及安全性的要求。
Description
技术领域
本发明属于自动驾驶汽车控制技术领域，尤其涉及一种考虑安全边界约束的自动驾驶车辆决策与规控方法。
背景技术
自动驾驶系统主要包括以下四个模块：环境感知、行为决策、轨迹规划和跟踪控制。行为决策模块根据环境感知所提供的道路及障碍物信息对车辆行为进行决策；轨迹规划模块在接收相应的决策信息后对车辆前方路径及本车速度进行规划，从而生成一条具有路径和速度信息的轨迹；跟踪控制模块根据轨迹规划模块给定的期望行驶轨迹和车辆实时反馈的状态信息，通过控制相应的底盘执行器，实现对期望轨迹的精确跟踪。对于自动驾驶车辆，安全行驶始终是第一要务，而考虑安全边界约束的自动驾驶车辆能够大大提升系统的安全性。安全边界约束包括车辆行驶位置安全约束和车辆运动状态安全约束，建立安全边界并应用于自动驾驶汽车是提高行车安全性的有效方法。因此，在尽可能保证安全的前提下使系统每个模块能够实时精确的运转对自动驾驶车辆具有极其重要的意义。
当前，针对自动驾驶车辆的安全性，主要问题如下：1.由于车辆周围环境是一个高动态环境，当采用行为决策、轨迹规划与路径跟踪分层控制方法时，在决策规划信息与实际信息发生偏差的情况下，控制器若按照既定路线跟踪可能会导致车辆跟踪效果较差，甚至出现碰撞的可能，这些因素都对车辆的行驶安全不利；2.针对车辆规划和控制模块，实时的车辆行驶位置和运动状态信息对行车安全起到至关重要的作用，而在传统的车辆系统常规安全约束中却并未充分考虑车辆行驶位置安全约束和车辆运动状态安全约束这些关键指标，导致其并不能充分保证行车安全性。同时，由于车辆系统的结构复杂，其表现出强非线性、多维运动高度耦合等特点，基于车辆线性模型建立的控制器忽略了很多动态特性，难以满足系统的控制精度需求，因此车辆安全性也难以得到充分保障。为此本申请提出一种考虑安全边界约束的自动驾驶车辆决策与规控方法。
发明内容
本发明的目的在于提供一种考虑安全边界约束的自动驾驶车辆决策与规控方法，旨在解决上述背景技术中提出的问题。
为实现上述目的，本发明提供如下技术方案：
一种考虑安全边界约束的自动驾驶车辆决策与规控方法，包括以下步骤：
步骤S1：搭建行为决策模块，基于动态规划算法的行为决策模块对车辆前方道路进行采样，并将采样点映射到S-L和S-T图上，以五次多项式对采样点进行连接，构建相应的成本函数，以动态规划算法对成本函数进行求解，将得到的路径决策信息及速度决策信息作用于规划控制器；
步骤S2：由车辆三自由度耦合模型得到横纵耦合的非线性预测模型，选择前轮转向角与前后轮驱动力作为控制量，考虑车辆物理约束和安全边界约束，利用模型预测控制算法构造代价函数，并对其进行求解，得到控制信号，完成轨迹规划任务；
步骤S3：将获得的控制信号作用于车辆系统，实现车辆轨迹规划及路径跟踪的控制效果。
进一步的，所述步骤S1中，路径决策的具体方法如下：
对自车前方道路横纵向分别进行等间隔路径点的采样，相邻列的采样点之间通过五次多项式曲线平滑连接；在Frenet坐标系下基于当前车道中心线对车辆行驶路径进行决策，以使车辆沿无碰撞路径行驶；
基于纵向路径采样的五次多项式有下列等式成立：
构造五次多项式曲线后，通过成本函数的求和来评估路径优劣，总成本函数是平滑度、避障和参考线成本函数的线性组合，总成本函数的公式如下：
Ct(f)＝Cs(f)+Co(f)+Cg(f)
其中，Ct(f)为总成本函数，其在总体上衡量路径的优劣程度；Cs(f)为平滑度成本函数，其作用为衡量路径的平滑程度；Co(f)为避障成本函数，其作用为衡量路径避障效果的优劣；Cg(f)为参考线成本函数，其作用为衡量车辆跟随车道参考线的能力。
进一步的，路径的所述平滑度成本函数通过下式衡量：
Cs(f)＝w1∫(f′(s))2ds+w2∫(f″(s))2ds
其中，w1，w2分别为路径五次多项式的一阶导和二阶导的代价权重系数；
路径的避障成本函数基于障碍物和汽车之间的距离进行设置，将其距离表示为d，具体表达式如下：
其中，Cn为单调递减函数，Cc为碰撞成本，ds为安全距离，dc为危险距离；
路径的参考线成本函数设置如下：当路径周围没有障碍物时，参考线为路径的中心线，将其功能定义为g(s)，参考线成本函数通过下式衡量：
Cg(f)＝∫(f(s)-g(s))2ds。
进一步的，所述步骤S1中，利用动态规划算法将多阶段决策问题转化为一系列单阶段最优化问题，逐步求解以完成决策过程，求解待定系数，从而得到路径决策信息，具体方法如下：
将车辆前方路径采样点作为研究对象；
因为从起点到第i列采样点的成本的计算结果，是基于从起点到i-1列所有采样点的总成本计算之和，所以将从起点到每一列路径采样点的总成本作为一个阶段；
将每个阶段决策问题转化为单阶段最优化问题，得到成本最小的路径信息。
进一步的，所述步骤S1中，速度决策的具体方法如下：
将障碍物信息离散为S-T图上的矩形边框，将(t0，t1，…，tn)表示为时间轴上间隔为dt的等距点；分段线性速度分布函数表示为S＝(s0，s1，…，sn)；在Frenet坐标系下基于当前车道中心线对车辆的速度进行决策，利用动态规划算法逐步求解以完成决策过程，求出待定系数，得到速度决策信息；
基于横向速度采样的五次多项式有下列等式成立：
构造五次多项式曲线后，通过成本函数的求和来评估速度大小，总成本函数是平滑度、避障和参考速度成本函数的线性组合，总成本函数的公式如下：
Ct(S)＝Cs(S)+Co(S)+Cg(S)
其中，Ct(S)为总成本函数，其在总体上衡量当前速度的合理性；Cs(S)为速度平滑度成本函数，其作用为衡量速度变化的平滑程度；Co(S)为避障成本函数，其用来表征避障时速度的变化过程；Cg(S)为参考速度成本函数，其用来衡量车辆跟随参考速度的能力。
进一步的，速度的所述平滑度成本函数通过下式衡量：
速度的避障成本函数是基于S-T图中障碍物和车辆之间的距离，其表达式与路径的避障成本函数相同；
参考速度成本函数表达式如下：
参考速度成本函数表示当没有障碍物或红绿灯限制时，车辆应遵循指定速度，Vref描述了由道路速度限制、曲率和其他交通法规确定的参考速度。
进一步的，所述步骤S2中，将车辆平面运动动力学模型的两个前轮和两个后轮分别一个等效的前轮与后轮在车辆的轴线方向进行代替，得到车辆三自由度耦合模型；纵向车速、侧向车速和横摆运动方程如下：
其中，vx为纵向速度，vy为横向速度，ωr为横摆角速度，δ为前轮转角，Fxr为后轮纵向力，Fxf为前轮纵向力，Fyr为后轮侧向力，Fyf为前轮侧向力，m为车身质量，lf为前轴距，lr为后轴距，Iz为转动惯量；
车辆在大地坐标系下的运动学方程如下：
其中，X、Y分别为纵向和横向位置，ψ为横摆角；
采用轮胎刷子模型计算轮胎侧向力，其计算公式如下所示：
其中，Fy为轮胎侧向力，Ca为侧偏刚度，α为轮胎侧偏角，μ为路面附着系数，Fz为车辆轮胎垂向载荷；
前轮侧偏角及后轮侧偏角计算公式如下所示：
其中，αf和αr分别为前轮侧偏角和后轮侧偏角。
进一步的，所述步骤S2中，考虑安全边界约束的非线性模型预测规划控制器设计步骤如下：
建立车辆控制器预测模型，通过车辆三自由度自行车模型得到车辆的控制器预测模型如下：
选取纵向速度vx、侧向速度vy、横摆角ψ、横摆角速度ωr、质心侧偏角β、大地坐标系下横向位移X和大地坐标系下纵向位移Y作为状态量，即：
假设状态量全部可测，车辆前轮转角δ，以及车辆纵向力Fx为控制量，即：
u＝[δ,Fx]
采用欧拉方法对控制器预测模型进行离散，Ts为采样时间，在时刻k，离散后的预测模型为：
记Np，Nc分别为预测时域和控制时域，并且满足Nc≤Np；则在k时刻，有如下序列：
其中，U(k)为系统控制序列，为状态序列；u(k|k)、u(k+1|k)、…、u(k+Nc-1|k)分别为在k时刻，对k时刻、k+1时刻、…、k+Nc-1时刻控制量u的预测； 分别为在k时刻，对k时刻、k+1时刻、…、k+Np-1时刻状态量的预测；
控制器的目标函数为：
其中，Y(k+i|k)为在k时刻，对k+i时刻纵向位置Y的预测；Yref(k+i)为k+i时刻的参考纵向位置；vx(k+i|k)为在k时刻，对k+i时刻纵向速度vx的预测；vxref(k+i)为k+i时刻的参考纵向速度；ψ(k+i|k)为在k时刻，对k+i时刻横摆角ψ的预测；ψref(k+i)为k+i时刻的参考横摆角；Δu(k+i|k)为在k时刻，对k+i时刻控制量u的预测增量；X(k+i|k)为在k时刻，对k+i时刻横向位置X的预测；Xobs(k+i)为k+i时刻的障碍物横向位置；Yobs(k+i)为k+i时刻的障碍物纵向位置；目标函数的前三项表征系统对于决策模块的给出的参考路径和速度信息的跟踪能力，第四项表征系统的平稳性，第五项为障碍物的碰撞函数。
进一步的，车辆行驶位置的所述安全边界约束为：
其中，dc为车身宽度，Y(k+i|k)为在k时刻，对k+i时刻纵向位置Y的预测；Yref(k+i)为k+i时刻的参考纵向位置；Yl(k+i|k)为在k时刻，对k+i时刻的左侧安全位置边界线的预测；Yr(k+i|k)为在k时刻，对k+i时刻的右侧安全位置边界线的预测；在车辆行驶过程中，车辆位置相对变化的位置边界通过实时计算得到；
由车辆动力学可知，横摆角速度最大值和质心侧偏角的最大值由下式得到：
其中，μ为路面附着系数，αrs为后轮侧偏角饱和值；ωrmax(k+i|k)为在k时刻，对k+i时刻横摆角速度的预测最大值；βmax(k+i|k)为在k时刻，对k+i时刻质心侧偏角的预测最大值；vx(k+i|k)为在k时刻，对k+i时刻纵向速度的预测；ωr(k+i|k)为在k时刻，对k+i时刻横摆角速度ωr的预测；
车辆运动状态的安全边界约束如下：
假设μ、ωr和vx可实时估计，则车辆运动状态安全边界的边界线可通过实时计算得到；其中，ωr(k+i|k)为在k时刻，对k+i时刻横摆角速度ωr的预测；β(k+i|k)为在k时刻，对k+i时刻质心侧偏角β的预测；前轮转角δ、车辆驱动力Fx的变化值均在允许的最小值与最大值之间；
umin≤u(k+i|k)≤umax,i＝0,1,...,Nc-1
其中，u(k+i|k)为在k时刻，对k+i时刻控制量u的预测；umin和umax的定义分别为：
其中，δmin和δmax分别为前轮转角最小值和最大值，Fxmin和Fxmax分别为车辆驱动力的最小值和最大值。
进一步的，所述车辆轨迹规划和路径跟踪控制问题描述为如下形式的优化问题：
在每个采样时刻在线进行数值优化求解，实现规划控制系统的控制目标。
与现有技术相比，本发明的有益效果是：
1、本发明所采用的决策、规划与控制一体化方法可在有效提升车辆行车安全的基础上大大减小决策、规划与控制模块间信息偏差所造成的干扰，从而进一步提高系统安全性。
2、本发明在考虑车辆行驶位置安全约束和运动状态安全约束的前提下，对规划和控制模块进行集成的非线性模型预测规划控制器设计，从而有效应对系统的强非线性、强耦合性等特点，满足系统对控制精度的要求。
附图说明
图1是本发明中车辆总体控制框图。
图2是车辆三自由度耦合模型，作为控制器设计模型。
图3是Prescan中预设的车辆与动态及静态障碍物的位置关系图。
图4是车辆对静态障碍物的路径决策结果图。
图5是车辆对静态和动态障碍物的速度决策结果图。
图6是车辆轨迹跟踪曲线。
图7是车辆实际速度曲线。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
以下结合具体实施例对本发明的具体实现进行详细描述。
如图1和图2所示，为本发明一个实施例提供的一种考虑安全边界约束的自动驾驶车辆决策与规控方法，包括以下步骤：
步骤S1：搭建行为决策模块，基于动态规划算法的行为决策模块对车辆前方道路进行采样，并将采样点映射到S-L和S-T图上，以五次多项式对采样点进行连接，构建相应的成本函数，以动态规划算法对成本函数进行求解，将得到的路径决策信息及速度决策信息作用于规划控制器；
步骤S2：由车辆三自由度耦合模型得到横纵耦合的非线性预测模型，选择前轮转向角与前后轮驱动力作为控制量，考虑车辆物理约束和安全边界约束，利用模型预测控制算法构造代价函数，并对其进行求解，得到控制信号，完成轨迹规划任务；
步骤S3：将获得的控制信号作用于车辆系统，实现车辆轨迹规划及路径跟踪的控制效果。
作为本发明的一种优选实施例，所述步骤S1中，路径决策的具体方法如下：
对自车前方道路横纵向分别进行等间隔路径点的采样，相邻列的采样点之间通过五次多项式曲线平滑连接；在Frenet坐标系下基于当前车道中心线对车辆行驶路径进行决策，以使车辆沿无碰撞路径行驶；
基于纵向路径采样的五次多项式有下列等式成立：
构造五次多项式曲线后，通过成本函数的求和来评估路径优劣，总成本函数是平滑度、避障和参考线成本函数的线性组合，总成本函数的公式如下：
Ct(f)＝Cs(f)+Co(f)+Cg(f) (2)
其中，Ct(f)为总成本函数，其在总体上衡量路径的优劣程度；Cs(f)为平滑度成本函数，其作用为衡量路径的平滑程度；Co(f)为避障成本函数，其作用为衡量路径避障效果的优劣；Cg(f)为参考线成本函数，其作用为衡量车辆跟随车道参考线的能力。
作为本发明的一种优选实施例，路径的所述平滑度成本函数通过下式衡量：
Cs(f)＝w1∫(f′(s))2ds+w2∫(f″(s))2ds (3)
其中，w1，w2分别为路径五次多项式的一阶导和二阶导的代价权重系数；
路径的避障成本函数基于障碍物和汽车之间的距离进行设置，将其距离表示为d，具体表达式如下：
其中，Cn被定义为单调递减函数，Cc为碰撞成本，其具有很大的值，有助于检测车辆的不可行路径，ds为安全距离，dc为危险距离；
路径的参考线成本函数设置如下：当路径周围没有障碍物时，参考线被定义为理想的驾驶路径，该线通常提取为路径的中心线，将其功能定义为g(s)，参考线成本函数通过下式衡量：
Cg(f)＝∫(f(s)-g(s))2ds (5)。
作为本发明的一种优选实施例，所述步骤S1中，利用动态规划算法将多阶段决策问题转化为一系列单阶段最优化问题，逐步求解以完成决策过程，求解待定系数，从而得到路径决策信息，具体方法如下：
将车辆前方路径采样点作为研究对象；
因为从起点到第i列采样点的成本的计算结果，是基于从起点到i-1列所有采样点的总成本计算之和，所以将从起点到每一列路径采样点的总成本作为一个阶段；
将每个阶段决策问题转化为单阶段最优化问题，得到成本最小的路径信息。
作为本发明的一种优选实施例，所述步骤S1中，速度决策的具体方法如下：
将障碍物信息离散为S-T图上的矩形边框，将(t0，t1，…，tn)表示为时间轴上间隔为dt的等距点；分段线性速度分布函数表示为S＝(s0，s1，…，sn)；在Frenet坐标系下基于当前车道中心线对车辆的速度进行决策，利用动态规划算法逐步求解以完成决策过程，求出待定系数，得到速度决策信息；
基于横向速度采样的五次多项式有下列等式成立：
构造五次多项式曲线后，通过成本函数的求和来评估速度大小，总成本函数是平滑度、避障和参考速度成本函数的线性组合，总成本函数的公式如下：
Ct(S)＝Cs(S)+Co(S)+Cg(S) (7)
其中，Ct(S)为总成本函数，其在总体上衡量当前速度的合理性；Cs(S)为速度平滑度成本函数，其作用为衡量速度变化的平滑程度；Co(S)为避障成本函数，其用来表征避障时速度的变化过程；Cg(S)为参考速度成本函数，其用来衡量车辆跟随参考速度的能力。
作为本发明的一种优选实施例，速度的所述平滑度成本函数通过下式衡量：
速度的避障成本函数是基于S-T图中障碍物和车辆之间的距离，其表达式同公式(4)相同；
参考速度成本函数表达式如下：
参考速度成本函数表示当没有障碍物或红绿灯限制时，车辆应遵循指定速度，Vref描述了由道路速度限制、曲率和其他交通法规确定的参考速度。
利用动态规划算法逐步求解以完成决策过程，求出待定系数，得到速度决策信息。由此，行为决策模块得到横纵向路径和速度信息，对接下来的轨迹规划和跟踪控制模块给出参考量。
作为本发明的一种优选实施例，所述步骤S2中，将车辆平面运动动力学模型的两个前轮和两个后轮分别一个等效的前轮与后轮在车辆的轴线方向进行代替，得到车辆三自由度耦合模型；纵向车速、侧向车速和横摆运动方程如下：
其中，vx为纵向速度，vy为横向速度，ωr为横摆角速度，δ为前轮转角，Fxr为后轮纵向力，Fxf为前轮纵向力，Fyr为后轮侧向力，Fyf为前轮侧向力，m为车身质量，lf为前轴距，lr为后轴距，Iz为转动惯量；
车辆在大地坐标系下的运动学方程如下：
其中，X、Y分别为纵向和横向位置，ψ为横摆角；车辆的运动最终依赖于轮胎受力，因而如何有效的对轮胎进行建模具有重要的意义。典型的轮胎模型包括统一指数模型(Uni-tire)、魔术公式(Magic formula)模型以及轮胎刷子模型(Tire Brush Models)。由于在运动规划控制中，需要较高的车辆动态，因此采用轮胎刷子模型计算轮胎侧向力，其计算公式如下所示：
其中，Fy为轮胎侧向力，Ca为侧偏刚度，α为轮胎侧偏角，μ为路面附着系数，Fz为车辆轮胎垂向载荷；
前轮侧偏角及后轮侧偏角计算公式如下所示：
其中，αf和αr分别为前轮侧偏角和后轮侧偏角。
在本发明实施例中，考虑到本申请需要对车辆路径实现跟踪以及车辆横纵向之间的耦合关系，建立控制器模型时，重点考虑汽车纵向运动，汽车横向运动和横摆运动这三个自由度，并做一定的假设。在此模型中我们认为车辆的车身与底盘为刚性连接，车身在悬架上方没有侧倾和俯仰运动，同时也忽略由于轮胎纵向形变位移所带来的车身侧倾和俯仰运动。在受力分析中忽略横向和纵向空气动力学对于车辆横摆特性的影响。
由于车辆在x轴向上具有对称性，因此我们可以对平面动力学模型进行简化。将车辆平面运动动力学模型的两个前轮和两个后轮分别一个等效的前轮与后轮在车辆的轴线方向进行代替。
在轮胎刷子模型中，车辆轮胎垂向载荷Fz可以通过下式计算得到：
其中，Fzf和Fzr分别为前后轮垂向荷载，g为重力加速度，hcg为车辆质心高度。由此可以根据该模型进行非线性模型预测规划控制器的设计。
作为本发明的一种优选实施例，所述步骤S2中，考虑安全边界约束的非线性模型预测规划控制器设计步骤如下：
首先建立车辆控制器预测模型，通过车辆三自由度自行车模型得到车辆的控制器预测模型如下：
选取纵向速度vx、侧向速度vy、横摆角ψ、横摆角速度ωr、质心侧偏角β、大地坐标系下横向位移X和大地坐标系下纵向位移Y作为状态量，即：
假设状态量全部可测，车辆前轮转角δ，以及车辆纵向力Fx为控制量，即：
u＝[δ,Fx] (17)
采用欧拉方法对控制器预测模型进行离散，Ts为采样时间，在时刻k，离散后的预测模型为：
记Np，Nc分别为预测时域和控制时域，并且满足Nc≤Np；则在k时刻，有如下序列：
其中，U(k)为系统控制序列，为状态序列；u(k|k)、u(k+1|k)、…、u(k+Nc-1|k)分别为在k时刻，对k时刻、k+1时刻、…、k+Nc-1时刻控制量u的预测； 分别为在k时刻，对k时刻、k+1时刻、…、k+Np-1时刻状态量的预测；
因为控制器的控制目标分别为车辆跟踪决策的路径及速度信息、保持车辆平稳性和保证车辆能够有效避障，因此控制器的目标函数设置为：
其中，Y(k+i|k)为在k时刻，对k+i时刻纵向位置Y的预测；Yref(k+i)为k+i时刻的参考纵向位置；vx(k+i|k)为在k时刻，对k+i时刻纵向速度vx的预测；vxref(k+i)为k+i时刻的参考纵向速度；ψ(k+i|k)为在k时刻，对k+i时刻横摆角ψ的预测；ψref(k+i)为k+i时刻的参考横摆角；Δu(k+i|k)为在k时刻，对k+i时刻控制量u的预测增量；X(k+i|k)为在k时刻，对k+i时刻横向位置X的预测；Xobs(k+i)为k+i时刻的障碍物横向位置；Yobs(k+i)为k+i时刻的障碍物纵向位置；目标函数的前三项表征系统对于决策模块的给出的参考路径和速度信息的跟踪能力，第四项表征系统的平稳性，第五项为障碍物的碰撞函数。
在本发明实施例中，在得到行为决策模块给出的路径及速度决策信息后，系统的轨迹规划和跟踪控制模块需要考虑车辆位置安全约束和车辆运动状态安全约束，并处理系统多变量问题。由于模型预测控制是依赖模型的设计方法，可以有效地处理含有硬约束、多变量的系统。考虑到车辆横纵向间的非线性耦合特性，本申请应用非线性模型预测控制器处理车辆轨迹规划和路径跟踪控制问题。
作为本发明的一种优选实施例，车辆行驶位置的所述安全边界约束为：
其中，dc为车身宽度，Y(k+i|k)为在k时刻，对k+i时刻纵向位置Y的预测；Yref(k+i)为k+i时刻的参考纵向位置；Yl(k+i|k)为在k时刻，对k+i时刻的左侧安全位置边界线的预测；Yr(k+i|k)为在k时刻，对k+i时刻的右侧安全位置边界线的预测；在车辆行驶过程中，车辆位置相对变化的位置边界通过实时计算得到；
由车辆动力学可知，横摆角速度最大值和质心侧偏角的最大值由下式得到：
其中，μ为路面附着系数，αrs为后轮侧偏角饱和值；ωrmax(k+i|k)为在k时刻，对k+i时刻横摆角速度的预测最大值；βmax(k+i|k)为在k时刻，对k+i时刻质心侧偏角的预测最大值；vx(k+i|k)为在k时刻，对k+i时刻纵向速度的预测；ωr(k+i|k)为在k时刻，对k+i时刻横摆角速度ωr的预测；
根据式(22)可得车辆运动状态的安全边界约束如下：
假设μ、ωr和vx可实时估计，则车辆运动状态安全边界的边界线可通过实时计算得到；其中，ωr(k+i|k)为在k时刻，对k+i时刻横摆角速度ωr的预测；β(k+i|k)为在k时刻，对k+i时刻质心侧偏角β的预测；据实际系统特性，控制量还需满足如式(24)所示的常规安全约束，即前轮转角δ、车辆驱动力Fx的变化值均在允许的最小值与最大值之间；
umin≤u(k+i|k)≤umax,i＝0,1,...,Nc-1 (24)
其中，u(k+i|k)为在k时刻，对k+i时刻控制量u的预测；umin和umax的定义分别为：
其中，δmin和δmax分别为前轮转角最小值和最大值，Fxmin和Fxmax分别为车辆驱动力的最小值和最大值。
在本发明实施例中，车辆在行驶过程中都具有一个相对确定的位置安全边界或安全区域，在不同的行驶环境下，车辆行驶稳定性的运动状态参数也具有一个合理的安全边界。车辆安全边界形成的位置安全区域和车辆运动状态安全区域为车辆运动状态的改变提供了较大的调整空间。明确车辆行驶位置安全边界和运动状态安全边界，设计合理的换道控制方法，可使车辆安全、稳定和舒适的应对不同行驶环境，进一步提升应对时变环境的鲁棒性。
作为本发明的一种优选实施例，所述车辆轨迹规划和路径跟踪控制问题描述为如下形式的优化问题：
在每个采样时刻在线进行数值优化求解，实现规划控制系统的控制目标。
实验验证
为了验证本发明的有效性，设计了如下仿真实验：
首先，在Prescan中预设如图3所示的驾驶场景，即车辆在平直公路上行驶，前方道路有两个蓝色的静态障碍物及三个红色的动态障碍物，这三个动态障碍物分别以3m/s、5m/s和7m/s的速度在公路车道间做循环往复的横向运动，两个静态障碍物分列在最下方车道内，车身各传感器对路况进行实时检测并将相应环境信息发送给行为决策模块以应对突发情况；
然后，行为决策模块做出相应的路径及速度决策。图4为车辆对静态障碍物的路径决策结果图。图5为车辆对静态和动态障碍物的速度决策结果图。
最后，车辆对前方轨迹进行规划及实时跟踪控制，图6为车辆轨迹跟踪曲线。图7为车辆实际速度曲线。在静态及动态障碍物的避障实验中，车辆保持着较好的决策和规控能力，并实时保证了车辆的行车安全。
以上仅是本发明的优选实施方式，应当指出，对于本领域的技术人员来说，在不脱离本发明构思的前提下，还可以作出若干变形和改进，这些也应该视为本发明的保护范围，这些均不会影响本发明实施的效果和专利的实用性。
Claims (10)
1.一种考虑安全边界约束的自动驾驶车辆决策与规控方法，其特征在于，包括以下步骤：
步骤S1：搭建行为决策模块，基于动态规划算法的行为决策模块对车辆前方道路进行采样，并将采样点映射到S-L和S-T图上，以五次多项式对采样点进行连接，构建相应的成本函数，以动态规划算法对成本函数进行求解，将得到的路径决策信息及速度决策信息作用于规划控制器；
步骤S2：由车辆三自由度耦合模型得到横纵耦合的非线性预测模型，选择前轮转向角与前后轮驱动力作为控制量，考虑车辆物理约束和安全边界约束，利用模型预测控制算法构造代价函数，并对其进行求解，得到控制信号，完成轨迹规划任务；
步骤S3：将获得的控制信号作用于车辆系统，实现车辆轨迹规划及路径跟踪的控制效果。
2.根据权利要求1所述的考虑安全边界约束的自动驾驶车辆决策与规控方法，其特征在于，所述步骤S1中，路径决策的具体方法如下：
对自车前方道路横纵向分别进行等间隔路径点的采样，相邻列的采样点之间通过五次多项式曲线平滑连接；在Frenet坐标系下基于当前车道中心线对车辆行驶路径进行决策，以使车辆沿无碰撞路径行驶；
基于纵向路径采样的五次多项式有下列等式成立：
构造五次多项式曲线后，通过成本函数的求和来评估路径优劣，总成本函数是平滑度、避障和参考线成本函数的线性组合，总成本函数的公式如下：
Ct(f)＝Cs(f)+Co(f)+Cg(f)
其中，Ct(f)为总成本函数，其在总体上衡量路径的优劣程度；Cs(f)为平滑度成本函数，其作用为衡量路径的平滑程度；Co(f)为避障成本函数，其作用为衡量路径避障效果的优劣；Cg(f)为参考线成本函数，其作用为衡量车辆跟随车道参考线的能力。
3.根据权利要求2所述的考虑安全边界约束的自动驾驶车辆决策与规控方法，其特征在于，路径的所述平滑度成本函数通过下式衡量：
Cs(f)＝w1∫(f′(s))2ds+w2∫(f″(s))2ds
其中，w1，w2分别为路径五次多项式的一阶导和二阶导的代价权重系数；
路径的避障成本函数基于障碍物和汽车之间的距离进行设置，将其距离表示为d，具体表达式如下：
其中，Cn为单调递减函数，Cc为碰撞成本，ds为安全距离，dc为危险距离；
路径的参考线成本函数设置如下：当路径周围没有障碍物时，参考线为路径的中心线，将其功能定义为g(s)，参考线成本函数通过下式衡量：
Cg(f)＝∫(f(s)-g(s))2ds。
4.根据权利要求1所述的考虑安全边界约束的自动驾驶车辆决策与规控方法，其特征在于，所述步骤S1中，利用动态规划算法将多阶段决策问题转化为一系列单阶段最优化问题，逐步求解以完成决策过程，求解待定系数，从而得到路径决策信息，具体方法如下：
将车辆前方路径采样点作为研究对象；
因为从起点到第i列采样点的成本的计算结果，是基于从起点到i-1列所有采样点的总成本计算之和，所以将从起点到每一列路径采样点的总成本作为一个阶段；
将每个阶段决策问题转化为单阶段最优化问题，得到成本最小的路径信息。
5.根据权利要求3所述的考虑安全边界约束的自动驾驶车辆决策与规控方法，其特征在于，所述步骤S1中，速度决策的具体方法如下：
将障碍物信息离散为S-T图上的矩形边框，将(t0，t1，…，tn)表示为时间轴上间隔为dt的等距点；分段线性速度分布函数表示为S＝(s0，s1，…，sn)；在Frenet坐标系下基于当前车道中心线对车辆的速度进行决策，利用动态规划算法逐步求解以完成决策过程，求出待定系数，得到速度决策信息；
基于横向速度采样的五次多项式有下列等式成立：
构造五次多项式曲线后，通过成本函数的求和来评估速度大小，总成本函数是平滑度、避障和参考速度成本函数的线性组合，总成本函数的公式如下：
Ct(S)＝Cs(S)+Co(S)+Cg(S)
其中，Ct(S)为总成本函数，其在总体上衡量当前速度的合理性；Cs(S)为速度平滑度成本函数，其作用为衡量速度变化的平滑程度；Co(S)为避障成本函数，其用来表征避障时速度的变化过程；Cg(S)为参考速度成本函数，其用来衡量车辆跟随参考速度的能力。
6.根据权利要求5所述的考虑安全边界约束的自动驾驶车辆决策与规控方法，其特征在于，速度的所述平滑度成本函数通过下式衡量：
速度的避障成本函数是基于S-T图中障碍物和车辆之间的距离，其表达式与路径的避障成本函数相同；
参考速度成本函数表达式如下：
参考速度成本函数表示当没有障碍物或红绿灯限制时，车辆应遵循指定速度，Vref描述了由道路速度限制、曲率和其他交通法规确定的参考速度。
7.根据权利要求1所述的考虑安全边界约束的自动驾驶车辆决策与规控方法，其特征在于，所述步骤S2中，将车辆平面运动动力学模型的两个前轮和两个后轮分别一个等效的前轮与后轮在车辆的轴线方向进行代替，得到车辆三自由度耦合模型；纵向车速、侧向车速和横摆运动方程如下：
其中，vx为纵向速度，vy为横向速度，ωr为横摆角速度，δ为前轮转角，Fxr为后轮纵向力，Fxf为前轮纵向力，Fyr为后轮侧向力，Fyf为前轮侧向力，m为车身质量，lf为前轴距，lr为后轴距，Iz为转动惯量；
车辆在大地坐标系下的运动学方程如下：
其中，X、Y分别为纵向和横向位置，ψ为横摆角；
采用轮胎刷子模型计算轮胎侧向力，其计算公式如下所示：
其中，Fy为轮胎侧向力，Ca为侧偏刚度，α为轮胎侧偏角，μ为路面附着系数，Fz为车辆轮胎垂向载荷；
前轮侧偏角及后轮侧偏角计算公式如下所示：
其中，αf和αr分别为前轮侧偏角和后轮侧偏角。
8.根据权利要求7所述的考虑安全边界约束的自动驾驶车辆决策与规控方法，其特征在于，所述步骤S2中，考虑安全边界约束的非线性模型预测规划控制器设计步骤如下：
建立车辆控制器预测模型，通过车辆三自由度自行车模型得到车辆的控制器预测模型如下：
选取纵向速度vx、侧向速度vy、横摆角ψ、横摆角速度ωr、质心侧偏角β、大地坐标系下横向位移X和大地坐标系下纵向位移Y作为状态量，即：
假设状态量全部可测，车辆前轮转角δ，以及车辆纵向力Fx为控制量，即：
u＝[δ,Fx]
采用欧拉方法对控制器预测模型进行离散，Ts为采样时间，在时刻k，离散后的预测模型为：
记Np，Nc分别为预测时域和控制时域，并且满足Nc≤Np；则在k时刻，有如下序列：
其中，U(k)为系统控制序列，为状态序列；u(k|k)、u(k+1|k)、…、u(k+Nc-1|k)分别为在k时刻，对k时刻、k+1时刻、…、k+Nc-1时刻控制量u的预测； 分别为在k时刻，对k时刻、k+1时刻、…、k+Np-1时刻状态量的预测；
控制器的目标函数为：
其中，Y(k+i|k)为在k时刻，对k+i时刻纵向位置Y的预测；Yref(k+i)为k+i时刻的参考纵向位置；vx(k+i|k)为在k时刻，对k+i时刻纵向速度vx的预测；vxref(k+i)为k+i时刻的参考纵向速度；ψ(k+i|k)为在k时刻，对k+i时刻横摆角ψ的预测；ψref(k+i)为k+i时刻的参考横摆角；Δu(k+i|k)为在k时刻，对k+i时刻控制量u的预测增量；X(k+i|k)为在k时刻，对k+i时刻横向位置X的预测；Xobs(k+i)为k+i时刻的障碍物横向位置；Yobs(k+i)为k+i时刻的障碍物纵向位置；目标函数的前三项表征系统对于决策模块的给出的参考路径和速度信息的跟踪能力，第四项表征系统的平稳性，第五项为障碍物的碰撞函数。
9.根据权利要求8所述的考虑安全边界约束的自动驾驶车辆决策与规控方法，其特征在于，车辆行驶位置的所述安全边界约束为：
其中，dc为车身宽度，Y(k+i|k)为在k时刻，对k+i时刻纵向位置Y的预测；Yref(k+i)为k+i时刻的参考纵向位置；Yl(k+i|k)为在k时刻，对k+i时刻的左侧安全位置边界线的预测；Yr(k+i|k)为在k时刻，对k+i时刻的右侧安全位置边界线的预测；在车辆行驶过程中，车辆位置相对变化的位置边界通过实时计算得到；
由车辆动力学可知，横摆角速度最大值和质心侧偏角的最大值由下式得到：
其中，μ为路面附着系数，αrs为后轮侧偏角饱和值；ωrmax(k+i|k)为在k时刻，对k+i时刻横摆角速度的预测最大值；βmax(k+i|k)为在k时刻，对k+i时刻质心侧偏角的预测最大值；vx(k+i|k)为在k时刻，对k+i时刻纵向速度的预测；ωr(k+i|k)为在k时刻，对k+i时刻横摆角速度ωr的预测；
车辆运动状态的安全边界约束如下：
假设μ、ωr和vx可实时估计，则车辆运动状态安全边界的边界线可通过实时计算得到；其中，ωr(k+i|k)为在k时刻，对k+i时刻横摆角速度ωr的预测；β(k+i|k)为在k时刻，对k+i时刻质心侧偏角β的预测；前轮转角δ、车辆驱动力Fx的变化值均在允许的最小值与最大值之间；
umin≤u(k+i|k)≤umax,i＝0,1,...,Nc-1
其中，u(k+i|k)为在k时刻，对k+i时刻控制量u的预测；umin和umax的定义分别为：
其中，δmin和δmax分别为前轮转角最小值和最大值，Fxmin和Fxmax分别为车辆驱动力的最小值和最大值。
10.根据权利要求1所述的考虑安全边界约束的自动驾驶车辆决策与规控方法，其特征在于，所述车辆轨迹规划和路径跟踪控制问题描述为如下形式的优化问题：
在每个采样时刻在线进行数值优化求解，实现规划控制系统的控制目标。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310035839.8A CN116048081A (zh) | 2023-01-10 | 2023-01-10 | 一种考虑安全边界约束的自动驾驶车辆决策与规控方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310035839.8A CN116048081A (zh) | 2023-01-10 | 2023-01-10 | 一种考虑安全边界约束的自动驾驶车辆决策与规控方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116048081A true CN116048081A (zh) | 2023-05-02 |
Family
ID=86132849
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310035839.8A Pending CN116048081A (zh) | 2023-01-10 | 2023-01-10 | 一种考虑安全边界约束的自动驾驶车辆决策与规控方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116048081A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116907532A (zh) * | 2023-09-12 | 2023-10-20 | 城市之光(深圳)无人驾驶有限公司 | 无人驾驶车辆窄路三段调头路径规划方法、装置及设备 |
| CN117742316A (zh) * | 2023-11-28 | 2024-03-22 | 上海友道智途科技有限公司 | 一种基于带挂车模型的最优轨迹规划方法 |
-
2023
- 2023-01-10 CN CN202310035839.8A patent/CN116048081A/zh active Pending
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116907532A (zh) * | 2023-09-12 | 2023-10-20 | 城市之光(深圳)无人驾驶有限公司 | 无人驾驶车辆窄路三段调头路径规划方法、装置及设备 |
| CN116907532B (zh) * | 2023-09-12 | 2023-11-21 | 城市之光(深圳)无人驾驶有限公司 | 无人驾驶车辆窄路三段调头路径规划方法、装置及设备 |
| CN117742316A (zh) * | 2023-11-28 | 2024-03-22 | 上海友道智途科技有限公司 | 一种基于带挂车模型的最优轨迹规划方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111845774B (zh) | 一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法 | |
| Yao et al. | Control strategies on path tracking for autonomous vehicle: State of the art and future challenges | |
| CN108482363B (zh) | 车辆横摆稳定性预测模型控制方法 | |
| CN110356404B (zh) | 一种具有自主换道功能并提高侧向安全性的智能驾驶系统 | |
| CN107161207B (zh) | 一种基于主动安全的智能汽车轨迹跟踪控制系统及控制方法 | |
| CN108227491B (zh) | 一种基于滑模神经网络的智能车轨迹跟踪控制方法 | |
| CN113276848B (zh) | 一种智能驾驶换道避障轨迹规划、跟踪控制方法及系统 | |
| Chen et al. | Path tracking and handling stability control strategy with collision avoidance for the autonomous vehicle under extreme conditions | |
| CN116048081A (zh) | 一种考虑安全边界约束的自动驾驶车辆决策与规控方法 | |
| Qin et al. | A lateral and longitudinal dynamics control framework of autonomous vehicles based on multi-parameter joint estimation | |
| CN112578672B (zh) | 基于底盘非线性的无人驾驶汽车轨迹控制系统及其轨迹控制方法 | |
| CN113064344B (zh) | 一种多轴无人重载车辆的轨迹跟踪控制方法 | |
| CN113581201B (zh) | 一种基于势场模型的无人汽车避撞控制方法及系统 | |
| CN116552550A (zh) | 基于参数不确定性和横摆稳定性的车辆轨迹跟踪控制系统 | |
| CN114942642A (zh) | 一种无人驾驶汽车轨迹规划方法 | |
| Khan et al. | A model predictive control strategy for lateral and longitudinal dynamics in autonomous driving | |
| Xu et al. | Linear time-varying MPC vehicle trajectory tracking controller considering driving road surface factors | |
| Li et al. | Path tracking control based on the prediction of tire state stiffness using the optimized steering sequence | |
| Lin et al. | A novel coordinated algorithm for vehicle stability based on optimal guaranteed cost control theory | |
| CN112835292B (zh) | 一种自动驾驶电动车在崎岖路面的横向控制方法 | |
| Wu et al. | Trajectory planning and tracking for four-wheel independent drive intelligent vehicle based on model predictive control | |
| CN114179818A (zh) | 基于自适应预瞄时间和滑模控制的智能汽车横向控制方法 | |
| Jing et al. | A tracking control method for collision avoidance trajectory of autonomous vehicle based on multi-constraint MPC | |
| CN113008240B (zh) | 基于稳定域的四轮独立驱动智能电动汽车路径规划方法 | |
| Wang et al. | Cooperative Control of Two-Vehicle Transportation Based on Time-Varying Distance Between Vehicles |
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 |