Vehicle lateral and longitudinal control based on road boundary constraints
-
摘要:
为提高自动驾驶车辆对不同道路的适应性和跟踪稳定性,提出一种基于道路边界约束和双比例−积分−微分(PID)的横纵向轨迹跟踪方法. 基于道路边界约束,求出使车辆安全行驶的转向曲率,结合二自由度动力学模型计算安全行驶转向角. 该方法计算简单,且最远预瞄点由道路的宽度及曲率信息自主确定. 通过仿真试验,制作车辆加速度、速度、油门和刹车的标定关系表,并基于标定表设计双PID速度跟踪控制器. 最后通过Carsim−Simulink联合仿真,证明横纵向控制器可以安全地行驶在道路范围内,并有良好的跟踪精度和稳定性.
Abstract:In order to improve the adaptability and tracking stability of autonomous vehicles to different roads, a lateral and longitudinal trajectory tracking method based on road boundary constraints and dual proportional-integral-derivative (PID) was proposed. Based on the road boundary constraints, the steering curvature for safe driving was obtained, and then the safe driving steering angle was calculated by combining the two-degree-of-freedom dynamic model. The method is simple to calculate, and the farthest preview point is determined autonomously by the width and curvature information of the road. Through the simulation test, the calibration relationship table of vehicle acceleration, speed, accelerator and brake were made, and a dual-PID speed tracking controller was designed based on the calibration table. Finally, through the Carsim-Simulink co-simulation, it is proved that the lateral and longitudinal controller can safely drive within the road range, and has good tracking accuracy and stability.
-
自动驾驶已经成为汽车未来发展的主要方向[1-2],其在提高驾驶舒适度和安全性方面有巨大的潜力. 轨迹跟踪是实现自动驾驶的关键技术,对此研究具有很强的工程应用价值和理论研究意义. 通常将车辆的轨迹跟踪分为纵向跟踪和横向跟踪,纵向跟踪称为速度跟踪,横向跟踪称为转向控制,目的是控制转向使车辆行驶在指定轨迹上. 在过去半个多世纪里,学者们根据车辆的行驶特点,研究了各式各样的控制器,有基于车辆运动学的纯跟踪法和斯坦利法[3],基于车辆动力学的PID控制[4]、线性二次型调节器(LQR)[5]、模糊逻辑控制[6]、滑模控制(SMC)[7]和模型预测控制(MPC)[8].
谢辉等[9]使用上下位控制器实现车辆的纵向控制,其中上位控制器将车辆加速模型简化为一阶惯性环节,求取车辆的期望加速度,下位控制器根据发动机特性求得节气门开度,但此法过分依赖模型的准确性. 陈无畏等[10]提出一类预瞄控制算法,预瞄具有对汽车运动的前瞻性,但不同的预瞄距离在面对不用的道路时跟踪精度不同,固定的预瞄距离效果并不好. 李红志等[11]提出预瞄距离自适应变化的预瞄控制,但该算法涉及积分运算,计算量较大,无法很好地保证跟踪的实时性.
本研究提出一种基于道路边界约束的横向控制器和基于车辆标定表的双PID纵向控制器,结合车辆的二自由度模型计算根据道路边界约束求解得到的转向曲率,最后使用Carsim和Simulink软件对控制器进行仿真验证,并设计不同的车速和路径对控制器进行验证.
1. 横向控制
1.1 二自由度动力学模型
汽车的数学模型是一个复杂的非线性模型,动力学模型可以较为准确地反映汽车的运动状态. 在汽车动力学模型研究中,二自由度动力学模型最为经典,其可以较好地表现汽车的横向转向控制. 为便于分析,将车辆模型简化为单轨道模型. 如图1所示,坐标系OXY为惯性坐标系,
${\alpha _{\rm{f}}}$ 与${\alpha _{\rm{r}}}$ 为前后轮的侧偏角,侧偏角为车轮的速度方向与车轮轴线的夹角,${F_{y{\rm{f}}}}$ 和${F_{y{\rm{r}}}}$ 为前后轮的侧向力,$\delta $ 为前轮转角,$ a $ 与$ b $ 为前后车桥到质心处的距离,${v_{\rm{f}}}$ 与${v_{\rm{r}}}$ 为前后轮的速度. 由于侧偏角的存在,车轮的速度方向与车轮方向不相同.$ v $ 为质心处的速度,一般情况下不与车身坐标系的$ x $ 轴重合,$ {v_x} $ 和$ {v_y} $ 为车辆质心速度在车辆坐标系下的纵向速度分量和横向速度分量,$ \beta $ 为质心侧偏角,即质心速度与车辆坐标系$ x $ 轴的夹角,$ \varphi $ 为汽车的航向角,即车辆坐标系$ x $ 轴与惯性坐标轴$ X $ 轴的夹角. 二自由度动力学模型体现车辆的横向运动与航向角运动,认为车辆的纵向速度$ {v_x} $ 为常数. 沿车辆坐标系$ y $ 轴和车辆的航向变化方向的牛顿欧拉方程为{∑Fy=may=Fyfcosδ+Fyr∑Mz=Iz¨φ=aFyfcosδ−bFyr (1) 考虑高速行驶时汽车的转向角通常较小,设
${\text{cos }}\delta$ $ \approx $ 1. 由于车辆的侧偏角较小,车辆的侧偏力与侧偏角的关系还处于线性段,式(1)可改写为{may=Fyr+Fyf=Cαfαf+CαrαrIz¨φ=aFyf−bFyr=aCαfαf−bCαrαr (2) 式中:
$ m $ 为汽车质量;${I_z}$ 为汽车绕地面垂直方向$ z $ 轴的转动惯量;$ {a_y} $ 为汽车$ y $ 轴方向的加速度;$ \ddot \varphi $ 为汽车的角加速度;${C_{\alpha {\rm{f}}}}$ 和${C_{\alpha {\rm{r}}}}$ 为前后轮的侧偏刚度. 借助刚体运动学公式,根据质心的速度及车辆横摆角速度计算得到车轮处的速度,得到前后轮侧偏角与质心速度和角速度的关系为{tan(−αr)=˙φb−vsinβvcosβ=˙φb−vyvx≈−αrtan(δ+αf)=˙φa+vsinβvcosβ=˙φa+vyvx≈δ+αf (3) 式中:
$ \dot \varphi $ 为横摆角速度. 考虑到车轮的侧偏角通常较小,所以使$ \tan (x) \approx x $ . 同时因为$ {a_y} = {\dot v_y} + {v_x}\dot \varphi $ ,将式(3)代入式(2)中,得到二自由度车辆动力学模型的微分方程为{˙vy=Cαf+Cαrmvxvy+(aCαf−bCαrmvx−vx)˙φ−Cαfmδ¨φ=aCαf−bCαrIzvxvy+a2Cαf+b2CαrIzvx˙φ−aCαfIzδ (4) 1.2 转向模型
假设车辆的前轮转角固定,车辆处于稳态进行阶段,令车辆的横向加速度
$ {\dot v_y} $ 和横摆角加速度$ \ddot \varphi $ 等于0,将其代入到式(4)中. 联立两个公式消去共同变量$ {v_y} $ ,得到稳态横摆角速度的增益为{˙φδ=vx(a+b)(1+Kvx2)K = m(a+b)2(aCαr−bCαf) (5) 式中:
$ K $ 为稳定性因数,表征车辆稳态转向响应能力. 根据运动学知识,车辆的行驶曲率$ \kappa $ 与横摆角速度以及车速的关系为κ = ˙φv (6) 因为侧偏角
$ \beta $ 较小,设$ \cos \beta \approx 1 $ ,得到$ v \approx {v_x} $ . 结合式(6)和式(5)消去车辆的横摆角速度,得到行驶曲率与前轮转向角的关系式为κ = δ(a+b)(1+Kvx2) (7) 2. 横向跟踪控制
2.1 车辆道路边界限制
在轨迹跟踪中,控制器的目的是控制车辆行驶在道路范围内. 设计时常把车辆简化为一个质点,道路也可简化为一条曲线,控制器的目的就变成使车辆质点保持在道路曲线上. 但这与驾驶员操作汽车的思路和习惯不太符合,驾驶员驾驶汽车时,主要思路是控制车辆保持在道路范围内的前提下,尽可能地行驶在道路中央.
控制器的控制思路与驾驶员的操作思路略有区别,前者将道路简化为一条曲线,忽略了道路的宽度信息. 与Cui等[12]研究类似,本研究中车辆道路边界限制概念考虑道路宽度和车辆宽度信息,道路约束条件如图2所示.
$ W $ 为车辆宽度,${f_{{\rm{road}},{\rm{l}}}}(x)$ 和${f_{{\rm{road}},{\rm{r}}}}(x)$ 为车辆左右两侧的道路边界,为方便起见,将车辆进一步简化为一条刚性杆. 车辆左侧和右侧的边界限制${f_{\rm{l}}}\left( x \right)$ 和${f_{\rm{r}}}\left( x \right)$ 为车辆的横向位置应在每个时刻的道路约束内进行限制,定义为{fr(x)=froad,r(x)+W2+dsfl(x)=froad,l(x)−W2−ds (8) 式中:
${d_{\rm{s}}}$ 为车辆边界限制的附加距离. 需要注意的是,使用式(8)定义边界限制的先决条件是道路边界几乎与X轴平行. 当这一条件不能很好地满足时,道路宽度的压缩应尽可能垂直于道路的主要推进方向(图2). 根据式(8),车辆的行驶轨迹$ f\left( x \right) $ 应满足fr(x)⩽f(x)⩽fl(x) (9) 为保证车辆行驶在安全道路边界范围内,车辆的转向曲率需限制在一定范围. 将式(9)中关于车辆行驶轨迹的限制转换为转向曲率限制,道路边界限制转换为转向曲率限制,方法如图3所示.
$ f({x_{{\rm{l}}i}}) $ 和$ f({x_{{\rm{r}}i}}) $ 为位于左右侧道路边界限制上的左预瞄点和右预瞄点,从车辆当前位置开始,沿车辆的速度方向移动,车辆在预测范围内的轨迹被约束在边界限制范围内,通过计算车辆到左右边界预瞄点得到的转向曲率作为车辆安全行驶限制. 此外,考虑较短的预测时间,假设车辆稳态行驶,横摆角速度和横向速度对应的车辆状态在一段时间内保持不变. 因此,车辆以匀速圆周运动向前移动,并以期望的圆形轨迹到达预瞄点. 图中:$ M $ 为车辆的转向中心;$ \kappa $ 为车辆质心处的转向曲率;$ {l_{\rm{p}}} $ 为质心到预瞄点处的距离;$ e $ 为预瞄点到车辆速度方向的距离;$ \theta $ 为车辆速度方向与${l_{\rm{p}}}$ 的夹角. 根据几何关系,弦切角是圆心角的一半,通过中间变量$ \theta $ 得到质心处转向曲率$ \kappa $ ,表达式为κ=2el2p (10) $ e $ 表示的距离偏差有正负之分,当预瞄点位于车辆速度方向左侧时为正,位于右侧为负.$ e $ 可看作车辆质心到预瞄点向量在车辆速度垂直向量上的投影,记车辆质心处的坐标为$\left( {{x_{\rm{h}}},{y_{\rm{h}}}} \right)$ ,预瞄点的坐标为$\left( {{x_{\rm{p}}},{y_{\rm{p}}}} \right)$ ,得到$ e $ 的计算公式为e=cos(φ+β)(yp−yh)−sin(φ+β)(xp−xh) (11) 式中:横摆角、侧偏角及质心位置都可以通过传感器获得,通过式(10)和式(11)将道路边界限制转换为转向曲率约束,从而避免车辆碰撞道路边界. 将根据预瞄点
$f\left( {{x_{{\rm{l}}i}}} \right)$ 计算得到的转向曲率记为${\kappa _{{\rm{l}}i}}$ ,则车辆的安全行驶转向曲率应满足κri⩽κ⩽κli (12) 2.2 期望转向角
由式(12)可以得到期望转向曲率的约束范围,但这个范围很广,无法获得一个确切的最优转向曲率,同时如何选择预瞄点也成为问题. 当车辆直线行驶时预瞄点应尽量选择远距离的,当路径曲率较大时预瞄点应选择较近的. 从驾驶员的操作经验来看,最优的转向曲率由驾驶员的视距范围决定. 根据这种思想,提出一种求解期望转向曲率的方法,如图4所示. 该方法不再单独计算一组预瞄点得到的曲率范围,而是充分利用保持在道路范围内这个思想,由近到远根据预瞄点计算转向角曲率约束,将每次根据更远预瞄点计算得到的曲率约束与当前车辆的曲率约束取交集,得到新的车辆曲率约束. 以此类推,不断地向远处选取预瞄点计算曲率,然后取交集,直到新的预瞄点与车辆的曲率约束交集为空.
根据预瞄点计算转向曲率的过程记为
$ F $ ,车辆位置信息为$\left( {{x_{\rm{h}}},{y_{\rm{h}}},{\varphi _{\rm{h}}},{\beta _{\rm{h}}}} \right)$ ,计算得到预瞄点${f_{\rm{l}}}\left( {{x_{{\rm{l}}i}}} \right)$ 所需的转向曲率为$ F\left( {\left( {{x_{\rm{h}}},{y_{\rm{h}}},{\varphi _{\rm{h}}},{\beta _{\rm{h}}}} \right),{f_{\rm{l}}}\left( {{x_{{\rm{l}}i}}} \right)} \right) $ . 期望转向曲率的计算过程如图5所示. 最后求得的转向曲率约束$ \left( {{\kappa _{\min }},{\kappa _{\max }}} \right) $ 是指在这个曲率范围内稳态行驶时,车辆可以行驶最长的距离. 由于$ {\kappa _{\max }} $ 和$ {\kappa _{\min }} $ 数值很接近,取其中值作为期望转向曲率${\kappa _{\rm{d}}}$ .假设车辆处于稳态行驶状态,根据式(7)中转向角和转向曲率的关系,得到期望前轮转角为
δd = κd(a+b)(1+Kvx2) (13) 式中:
${\delta _{\rm{d}}}$ 为横向控制最后得到的期望转向角. 该法避免了预瞄距离的选择问题,在期望转向曲率计算中,道路信息从近到远逐渐使用,直至到达最远的点. 最远预瞄点由全局道路信息的宽度、曲率等参数自主确定. 该方法使用较为简单的线性二自由度动力学模型,避免了复杂的非线性汽车模型.3. 纵向控制
纵向控制指控制车辆的加、减速使车辆按规划速度行驶. 从驾驶员角度来看,车辆的加速由油门开度控制,减速由制动压力控制. 从反馈控制角度来看,纵向控制是简单的单输入单输出系统,将车辆速度与期望速度作差后经过PID控制即可作为期望加速度来控制. 但是车辆的加速度很难直接进行控制,需要通过制动压力和油门开度来实现,所以纵向控制问题变为如何通过油门开度和制动压力使车辆的加速度为期望加速度.
车辆加速度与制动压力和油门开度并没有明确的解析式关系,且受到车辆速度的影响,不同车辆之间也有所不同. 以油门与加速度的关系为例,受发动机功率的限制,在相同油门开度下,不同速度油门产生的加速度也是不同的,相同制动压力产生的加速度也与车辆的当前速度有关.
针对这种情况,借鉴文献[13]的方法,通过试验制作标定表描述车辆加速度、速度及油门开度和制动压力的关系. 使用Carsim和Simulink联合仿真,由于油门和刹车很少同时使用,分别设置油门开度为
$\left[ {0:0.05:1} \right]$ ,车辆从0开始加速,设置刹车制动压力为$ \left[ {0:0.1:80} \right] $ MPa,车辆从50 m/s开始减速. 记录每时刻汽车的纵向速度、加速度及油门开度或制动压力数据,使用scatteredInterpolant函数对记录数据进行插值计算,得到函数$ F $ 关系式为y=F(v,a) (14) 式中:
$ v $ 为车辆速度;$ a $ 为车辆加速度;$ y $ 为给到车辆的油门开度或制动压力,当$ y $ 大于0时为油门开度,当$ y $ 小于0时为刹车制动压力,经过仿真后函数$ F $ 的图像如图6所示.根据标定表得到不同速度下加速度与油门开度及制动压力的关系. 根据反馈PID控制得到期望加速度,代入标定表得相应的油门开度或制动压力. 进行纵向控制时不应只考虑速度误差,还需考虑位置误差,因为速度相等不代表车辆处于期望位置,所以针对位置误差还要进行反馈控制补偿,设计两个PID控制器,分别对位置误差和速度误差进行消除. 双PID纵向控制器如图7所示. 图中:
${s_{\rm{d}}}$ ,${v_{\rm{d}}}$ 和${a_{\rm{d}}}$ 为规划位置、规划速度和规划加速度;$ s $ ,$ v $ 和$ a $ 为车辆当前时刻的位置、速度以及加速度;${v_{{\rm{d}}s}}$ 为由位置误差经PID产生得到的期望速度;${a_{{\rm{d}}v}}$ 为由速度误差经PID产生得到的期望加速度. 其中${v_{{\rm{d}}s}}$ 与规划速度${v_{\rm{d}}}$ 一并作为规划速度对速度进行跟踪,速度误差经PID后得到${a_{{\rm{d}}v}}$ ,将其与${a_{\rm{d}}}$ 相加,得到车辆所需的加速度并代入标定表,根据标定表可得所需的油门开度或制动压力.4. 仿真验证
使用Carsim和Simulink软件联合仿真验证控制算法,Simulink编写控制算法,Carsim提供更精确的汽车动力学模型. 模型中汽车参数见表1.
表 1 仿真参数Table 1. Simulation parameters定义 符号 值 整车质量/kg $ m $ 1412 质心到前轴距离/m $ a $ 1.015 质心到后轴距离/m $ b $ 1.895 前轮侧偏刚度/$({\text{N} }·{\text{ra} }{ {\text{d} }^{ { { - 1} } } })$ ${C_{\alpha {\rm{f}}} }$ −148970 后轮侧偏刚度/$({\text{N} }·{\text{ra} }{ {\text{d} }^{ { { - 1} } } })$ ${C_{\alpha {\rm{r}}} }$ −82204 为测试速度跟踪器的加减速性能,使车辆在直线段行驶. 在
$ t $ =0时,车辆的位置、速度和加速度均为0;在$ t $ =20 s时,车辆的位置为300 m,速度和加速度均为0. 使用五次多项式绘制位置和时间曲线$ s\left( t \right) $ ,如图8所示. 位置误差的PID比例系数为0.5,速度误差的PID比例系数为1.8. 跟踪结果非常好,在指定时间均能很好地到达规划位置,速度也基本吻合,证明了纵向控制的有效性.图9为车辆跟踪圆形道路的仿真结果. 道路宽度设置为2 m,这里的宽度不是真实的道路宽度,而是相对质心缩减后的道路宽度. 从图中可以看出,车辆一直稳定行驶在圆形道路范围内,表明控制器在定曲率路径下有稳定的跟踪效果. 图10为车辆跟踪正弦道路的仿真结果. 道路宽度为2 m,因为初始方向与道路方向有所区别,所以在开始行驶阶段车辆行驶出现误差,但车辆还是稳定行驶在道路范围内,证明控制器在连续转向道路内同样有稳定的跟踪效果. 从跟踪结果可以看出,控制器并不以跟踪道路中心线为目的,而是以安全行驶在道路范围内为目标.
为更好地测试控制算法,以国际标准ISO3888—1:1999中的双移线作为测试路径,速度为80 km/h,并与线性二次调节器(LQR)控制算法对比. 以双移线为基准,上下各移动0.2 m形成道路边界,跟踪结果如图11所示. 从图中可以看出,LQR控制器的跟踪精度明显更好,基于道路边界约束的控制器虽然不如LQR控制器跟踪得好,但也能很好地保持在道路范围内,且误差不超过0.2 m. 使用Matlab中tic和toc函数计算两种控制算法在单个周期内的计算时间,比较两种算法在实时性上的差异,如图12所示. 从图中可以看出,LQR控制算法计算时间更长,基于道路边界约束的控制器计算时间更短. 这可能是因为LQR控制器在求解时要求解ricatti迭代方程从而增大了计算复杂性. 从结果来看,虽然LQR控制器在跟踪精度上表现更好,但基于道路边界约束的控制算法在实时性上表现更好,且有不错的跟踪精度.
5. 结 语
针对车辆跟踪控制设计复杂、计算难度大等问题,设计基于道路边界约束的横向控制器,解决预瞄距离的选择困难,使用二自由度线性动力学模型简化计算. 制作车辆标定表更精确地体现油门刹车与车辆的速度关系,设计双PID控制器消除车辆的位置误差和速度误差,实现了纵向控制.
仿真结果表明,无论是连续变化的曲率路径还是定曲率路径,车辆都可以稳定行驶在道路范围内,且实时性表现较好,证明车辆的横纵向控制器在面对不同的路径时均有良好的跟踪效果.
-
表 1 仿真参数
Table 1. Simulation parameters
定义 符号 值 整车质量/kg $ m $ 1412 质心到前轴距离/m $ a $ 1.015 质心到后轴距离/m $ b $ 1.895 前轮侧偏刚度/$({\text{N} }·{\text{ra} }{ {\text{d} }^{ { { - 1} } } })$ ${C_{\alpha {\rm{f}}} }$ −148970 后轮侧偏刚度/$({\text{N} }·{\text{ra} }{ {\text{d} }^{ { { - 1} } } })$ ${C_{\alpha {\rm{r}}} }$ −82204 -
[1] CHEN Y M, HU C, WANG J M. Human-centered trajectory tracking control for autonomous vehicles with driver cut-in behavior prediction[J] . IEEE Transactions on Vehicular Technology,2019,68(9):8461 − 8471. doi: 10.1109/TVT.2019.2927242 [2] HUANG Y J, DING H T, ZHANG Y B, et al. A motion planning and tracking framework for autonomous vehicles based on artificial potential field elaborated resistance network approach[J] . IEEE Transactions on Industrial Electronics,2020,67(2):1376 − 1386. doi: 10.1109/TIE.2019.2898599 [3] THRUN S, MONTEMERLO M, DAHLKAMP H, et al. Stanley: The robot that won the darpa grand challenge[J] . Journal of field Robotics,2006,23(9):661 − 692. doi: 10.1002/rob.20147 [4] NORMEY-RICO J E, ALCALÁ I, GÓMEZ-ORTEGA J, et al. Mobile robot path tracking using a robust pid controller[J] . Control Engineering Practice,2001,9(11):1209 − 1214. doi: 10.1016/S0967-0661(01)00066-1 [5] 彭传颂, 赖杰, 潘鲁彬. 基于lqr与前向增益的无人车辆轨迹跟踪控制[J] . 农业装备与车辆工程,2021,59(4):40 − 43. [6] HU C, CHEN Y M, WANG J M. Fuzzy observer-based transitional path-tracking control for autonomous vehicles[J] . IEEE Transactions on Intelligent Transportation Systems,2021,22(5):3078 − 3088. doi: 10.1109/TITS.2020.2979431 [7] HU C, WANG Z F, QIN Y C, et al. Lane keeping control of autonomous vehicles with prescribed performance considering the rollover prevention and input saturation[J] . IEEE Transactions on Intelligent Transportation Systems,2020,21(7):3091 − 3103. doi: 10.1109/TITS.2019.2924937 [8] GUO N Y, LENZO B, ZHANG X D, et al. A real-time nonlinear model predictive controller for yaw motion optimization of distributed drive electric vehicles[J] . IEEE Transactions on Vehicular Technology,2020,69(5):4935 − 4946. doi: 10.1109/TVT.2020.2980169 [9] 谢辉, 刘爽爽. 基于模型预测控制的无人驾驶汽车横纵向运动控制[J] . 汽车安全与节能学报,2019,10(3):326 − 333. doi: 10.3969/j.issn.1674-8484.2019.03.008 [10] 陈无畏, 谈东奎, 汪洪波, 等. 一类基于轨迹预测的驾驶员方向控制模型[J] . 机械工程学报,2016,52(14):106 − 115. [11] 李红志, 李亮, 宋健, 等. 预瞄时间自适应的最优预瞄驾驶员模型[J] . 机械工程学报,2010,46(20):106 − 111. [12] CUI Q J, DING R J, WEI C F, et al. Path-tracking and lateral stabilisation for autonomous vehicles by using the steering angle envelope[J] . Vehicle System Dynamics,2021,59(11):1672 − 1696. doi: 10.1080/00423114.2020.1776344 [13] 张栩源, 李军. 基于lqr双pid的智能电动汽车轨迹跟踪横纵向协同控制[J] . 汽车安全与节能学报,2021,12(3):346 − 354. doi: 10.3969/j.issn.1674-8484.2021.03.009 期刊类型引用(0)
其他类型引用(1)
-