江苏省工程建设标准站网站,百度网站外链发布平台,武清网站开发,三明百度seo在上一讲《Coursera自动驾驶课程第6讲#xff1a;Vehicle Longitudinal Control》中我们了解了如何使用PID算法进行汽车纵向控制。
本讲我们继续学习新的模块#xff1a;汽车横向控制。具体地#xff0c;我们将学习三种控制算法#xff1a;Pure pursuit#xff0c;Stanle…在上一讲《Coursera自动驾驶课程第6讲Vehicle Longitudinal Control》中我们了解了如何使用PID算法进行汽车纵向控制。
本讲我们继续学习新的模块汽车横向控制。具体地我们将学习三种控制算法Pure pursuitStanleyMPC。
B站视频链接https://www.bilibili.com/video/BV1WE411D74g?p1 文章目录1. Introduction to Lateral Control1.1 Overview1.2 Two Types of Control Design1.3 Driving Controller2. Geometric Steering Control - Pure Pursuit2.1 Geometric path tracking2.2 Pure pursuit3. Geometric Steering Control - Stanley Controller3.1 Stanley Controller Approach3.2 Heading control Combined steering law3.3 Case Study4. Advanced Vehicle Control Methods 难点4.1 Overview4.2 Receding Horizon Control4.3 MPC Structure4.4 Vehicle Lateral Control1. Introduction to Lateral Control
1.1 Overview
在设计汽车横向控制器时通常需要进行以下三步 定义车辆所处位置和预期路径之间的偏差。 选择一种控制设计策略将偏差降至零同时仍满足输入限制如输入转向角限制等。 考虑车辆本身的动态限制和所需的行驶特性。 下面介绍三种参考路径定义
直线段序列。一系列紧密间隔的航点。一系列连续的参数化曲线来定义路径。
在上述三种路径跟踪情况下控制器都试图消除车辆与预期路径之间的偏差并使车辆航向与路径方向对齐。 1.2 Two Types of Control Design
下面介绍两种类型控制器 第一类控制器是几何控制器Geometric Controllers常见的有Pure pursuit和Stanley。另一类控制器称为动态控制器Dynamic Controllers常见的有MPC 控制。 后面我们以自行车模型为基础来讨论汽车横向控制。
如下图所示我们将使用线段作为参考路径在图中以实心黑线显示。还可以看到一条平行于路径但穿过前轴中心的黑色虚线。为了进行横向控制我们重新定义了相对于当前路径线段的航向δ\deltaδ和ψ\psiψ。
请注意根据我们的控制器设计我们可以在后轴中心前轴中心或重心处放置车辆参考系。 1.3 Driving Controller
如上一节所述我们将介绍两种类型误差航向误差和轨迹偏移误差。
航向误差为沿路径参考点的路径方向与车辆实际航向之差。 航向误差变化率有助于我们了解航向误差如何随时间变化并且可以从自行车模型方程中进行计算。
这里我们介绍相对于前轴的航向误差率。对于直线段所需的航向变化率为零可以将其删除。 这是因为参考航向对于一条直线不是随时间变化的并且实际上等于零。最终我们得到航向误差变化率为 ψ˙(t)−vf(t)sinδ(t)L\dot{\psi}(t)\frac{-v_{f}(t) \sin \delta(t)}{L}ψ˙(t)L−vf(t)sinδ(t)
另一种类型的误差是轨迹偏移误差。定义为车辆上的参考点与所需路径上的最接近点之间的距离。其公式为 e˙(t)vf(t)sin(ψ(t)−δ(t))\dot{e}(t)v_{f}(t) \sin (\psi(t)-\delta(t))e˙(t)vf(t)sin(ψ(t)−δ(t))
从该方程中我们可以看到随着速度的增加误差变化更快。 2. Geometric Steering Control - Pure Pursuit
2.1 Geometric path tracking
几何路径跟踪控制器使用车辆运动学的几何形状和参考路径来跟踪参考路径。
几何路径跟踪控制器依赖于沿所需路径的参考点该参考点可以是用于计算航向和轨迹偏移误差的参考点也可以是沿路径在车辆前方一定距离处的超前点一此处以红色显示。 2.2 Pure pursuit
下面我们介绍Pure pursuit。其核心思想是可以将参考点放置在路径上距车辆前方固定距离的位置并且可以使用恒定的转向角计算与该点相交所需的转向命令。
在这种方法中后轴的中心为车辆参考点我们将后轴中心与目标参考点之间距离定义为ldl_dld。 车辆的车身方向与连接线之间的角度为α\alphaα。 为了构造Pure pursuit控制器我们再次引入瞬时旋转中心的概念。 轨迹上的目标点后轴中心和瞬时旋转中心形成一个三角形其半径长度为RRR目标点和后轴中心长度为ldl_dld。 从几何关系以及正弦定律我们可以得出路径曲率 κ1R2sinαld\\\kappa\frac{1}{R}\frac{2 \sin \alpha}{l_{d}}κR1ld2sinα
现在让我们看一下自行车模型以计算跟踪此弧线所需的转向角 δtan−1(2Lsinαld)\delta\tan ^{-1}\left(\frac{2 L \sin \alpha}{l_{d}}\right)δtan−1(ld2Lsinα)
轨迹偏移误差eee在这里定义为航向矢量和目标点之间的横向距离因此我们可以得出曲率与误差之间关系 κ2ld2e\kappa\frac{2}{l_{d}^{2}} eκld22e
随着误差的增加曲率也随之增加使车辆更快速地返回至参考路径。
该式也表明Pure pursuit控制器以类似于比例控制的方式工作以使用路径曲率作为控制器的输出来校正轨迹偏移误差。比例增益为2ld2\frac{2}{l_{d}^{2}}ld22。因此随着ldl_dld的增加比例增益减小。
我们也可以对Pure pursuit制器进行修改。 我们可以根据车辆的速度改变距离ldl_dld。 使其与车辆前进速度成正比 ldKddvfl_{d}K_{d d} v_{f}ldKddvf
综合以上式子我们得到转向角和车速之间的关系 δtan−1(2LsinαKddvf)\delta\tan ^{-1}\left(\frac{2 L \sin \alpha}{K_{d d} v_{f}}\right)δtan−1(Kddvf2Lsinα) 3. Geometric Steering Control - Stanley Controller
3.1 Stanley Controller Approach
Stanley Controller 是一种几何路径跟踪控制器。最初是由斯坦福大学的Gabe Hoffman开发的并以斯坦利车名字命名。其主要特点有
将车辆参考点切换到前轴中心。同时考虑航向对齐和轨迹偏移误差。Stanley控制器为其输出设置了上限使输出落在最大转向角范围内。 3.2 Heading control Combined steering law
我们先介绍下如何进行航向控制的。
在左图中可以看到轨迹偏移误差是相对于前轴测量的。 Stanley控制器通常会有以下三个需求
为了消除相对于路径的航向误差转向角直接等于航向角δ(t)ψ(t)\delta(t) \psi(t)δ(t)ψ(t)为了消除轨迹偏移误差使用了比例控制其增益kkk满足以下公式δ(t)tan−1(ke(t)vf(t))\delta(t) tan^{-1}(\frac{ke(t)}{v_f(t)})δ(t)tan−1(vf(t)ke(t))。与误差基本成正比与速度成反比控制函数为反正切函数。最后转向角会限制在一个取值范围内δ(t)∈[δmin,δmax]\delta(t)\in[\delta_{min},\delta_{max}]δ(t)∈[δmin,δmax]。
为了消除航向误差和轨迹偏移误差最终得到的汽车转向角为 δ(t)ψ(t)tan−1(ke(t)vf(t)),δ(t)∈[δmin,δmax]\delta(t) \psi(t) tan^{-1}(\frac{ke(t)}{v_f(t)}), \delta(t)\in[\delta_{min},\delta_{max}]δ(t)ψ(t)tan−1(vf(t)ke(t)),δ(t)∈[δmin,δmax]
现在让我们看一下对于不同的误差系统要求的汽车转向角是多少。
对于航向误差转向命令指向与航向误差相反的方向从而使车辆转弯以纠正与路径未对准的情况。 例如对于较大的航向误差如果航向误差超过最大转向角则控制器将会发出最大转向指令。
对于较大的轨迹偏移误差此时反正切角将会接近π2\frac{\pi}22π。因此我们可以将Stanley控制器输出为航向误差加上π2\frac{\pi}22π。 tan−1(ke(t)vf(t))≈π2→δ(t)≈ψ(t)π2\tan ^{-1}\left(\frac{k e(t)}{v_{f}(t)}\right) \approx \frac{\pi}{2} \rightarrow \delta(t) \approx \psi(t)\frac{\pi}{2}tan−1(vf(t)ke(t))≈2π→δ(t)≈ψ(t)2π
我们讨论以下误差的变化情况。我们代入上式可以得到误差变化率为 e˙(t)−vf(t)sin(ψ(t)−δ(t))−vf(t)sin(tan−1(ke(t)vf(t)))−ke(t)1(ke(t)vf)2\begin{aligned} \dot{e}(t)-v_{f}(t) \sin (\psi(t)-\delta(t)) -v_{f}(t) \sin \left(\tan ^{-1}\left(\frac{k e(t)}{v_{f}(t)}\right)\right) \\ \frac{-k e(t)}{\sqrt{1\left(\frac{k e(t)}{v_{f}}\right)^{2}}} \end{aligned}e˙(t)−vf(t)sin(ψ(t)−δ(t))−vf(t)sin(tan−1(vf(t)ke(t)))1(vfke(t))2−ke(t)
当跨轨偏移误差较小时上式可以简化为
e˙(t)≈−ke(t)\dot{e}(t)\approx-ke(t)e˙(t)≈−ke(t)
上式最有趣的是误差变化率与速度完全无关。 3.3 Case Study
下面我们来看两个仿真例子。
第一个例子对于较大的初始轨迹偏移误差让我们假设初始误差为5米最大允许转向角为25度前进速度为5m/s。 仿真结果显示了Stanley控制器如何纠正较大的轨迹偏移误差并收敛到所需路径。较大的初始误差会导致较大的转向指令从而使车辆迅速转向行驶路径。然后航向误差和跨轨误差项达到平衡车辆沿直线继续朝向路径行驶。
我们还可以在不同的速度下运行相同的模拟。因此让我们尝试2m/s5m/s和10m/s的速度。结果显示了Stanley控制器的主要特征。在所有情况下都可以看到朝向路径的转弯直线前进以及路径的指数衰减。速度越高汽车在到达路径之前行驶的距离就越远。
现在让我们看看第二个仿真例子初始时车辆的航向误差很大。 仿真结果表明航向误差已由Stanley控制器校正。首先在纠正航向误差时转向指令已超过其极限。然后随着轨迹偏移误差开始增加转向命令将继续校正汽车的行驶方向使其与路径对齐。最后汽车像进入指数收敛段。实际上它具有全局稳定性功能这意味着无论初始条件如何控制器都将引导汽车返回原路。
然而在实际情况中需要对Stanley控制器进行一些调整。
由于速度项位于反正切内的分数的分母中因此低速往往会导致转向命令放大。这导致方向盘剧烈晃动这对于驾驶员的舒适性而言是不希望的。因此为了消除此问题并提高解决方案在低速下的稳定性可以添加了一个常数该常数可以在现场进行调整。在较高的速度下我们存在一个问题即转向命令需要缓慢变化以确保横向力不会过大。即使在速度上如此缩放斯坦利在高速下的反应仍然过于激进因此还增加了航向阻尼项。这实质上将航向误差控制部分转换为PD控制器。最后对于具有高曲率的弯曲路径控制器无法很好地跟踪它们因为在几何控制器的推导中未考虑动力学。正如我们在纵向控制中看到的通过向控制器添加前馈项可以提高系统性能并将误差更快收敛为0。 4. Advanced Vehicle Control Methods 难点
4.1 Overview
MPC指的是一种控制设计方法可以在每个时间步长上以数值方式优化和解决问题。
用于自动驾驶汽车的纵向和横向控制是MPC的一个常见应用。模型预测控制通常也可以称为Receding horizon approach因为控制器会在每个时间步长基于固定的有限长度生成一个执行器信号。MPC的主要优势有 MPC控制器的公式简单只需定义目标函数和相关约束然后使用已知的求解器对其进行优化。 由于许多汽车子系统具有严格的执行器约束和多样化的性能目标因此MPC已成为车辆控制的主要工具。 **控制器可以明确地应用于车辆及其子系统的线性或非线性模型这意味着即使模型随时间变化或改进也可以使用相同的方法。
当然MPC与其它控制方法相比其缺点则是需要更多的计算资源。 4.2 Receding Horizon Control
在开始设计MPC控制器之前让我们仔细研究一下Receding Horizon的概念。Receding Horizon Control解决了每个时间步长上的固定大小优化问题该问题根据目标约束和车辆的当前状态确定从当前时间到horizon结束要应用的最佳控制输入。然而在实现中出现的一个问题是由于优化可能需要花费一段时间因此开始优化时的车辆状态将不同于优化完成时的车辆状态。
因此我们必须在优化中使用预测状态来做为实际控制输入。现在我们定义一些术语符号
首先我们定义receding horizon长度TTT。然后我们将优化初始状态设置为在ttt时刻的xxx优化结束时的预测状态。接下来我们使用上次优化中确定的控制输入ut−1u_{t-1}ut−1在车辆从时间ttt的当前状态xt−1x_{t-1}xt−1移动到ttt时刻。最后我们应用receding horizon优化的第一时间步骤中的控制信号并为下一个时间步骤重复该过程。 4.3 MPC Structure
我们可以使用下方左图来可视化MPC结构。
我们有两个主要模块一个是优化器模块它是核心MPC组件另一个是动态模型。 该模型从时间ttt减去一开始获取过去的输入和状态并获取优化器的输出该输出是地平线上每个时间步长的输入UUU的当前序列。 然后该模型在下一个时间步输出预测状态将其与参考轨迹进行比较并作为预测误差传递到优化器中。
优化器还接收更新的约束和要使用的成本函数这些约束可以预先确定也可以根据运行模式的变化而变化。 优化器解决其优化问题然后重复该过程。
现在让我们看一下线性MPC。
我们使用状态空间来表示离散形式的运动模型。 未来状态xt1x_{t1}xt1与当前状态xtx_txt和执行器信号utu_tut线性相关。 注意AAA和BBB是系数矩阵并且被假定为是不变的。 xt1AxtButx_{t1} Ax_t Bu_txt1AxtButMPC试图找到输入的控制策略UUU。 U{ut∣t,ut1∣t,ut2∣t,⋅⋅⋅}U \{u_{t|t},u_{t1|t},u_{t2|t},···\}U{ut∣t,ut1∣t,ut2∣t,⋅⋅⋅}目标函数可以定义为 J(x(t),U)∑jttT−1xj∣tTQxj∣tuj∣tTRuj∣tJ(x(t),U) \sum_{jt}^{tT-1}x^{T}_{j|t}Qx_{j|t}u^T_{j|t}Ru_{j|t}J(x(t),U)jt∑tT−1xj∣tTQxj∣tuj∣tTRuj∣t
在从零到非零的控制输入的状态偏差上具有二次误差类似于最优控制理论的优化问题并在控制性能和输入积极性之间进行权衡。 注意矩阵Q和R称为权重矩阵可以进行适当选择以实现特定类型的响应。
相反如果我们需要跟踪参考信号例如所需轨迹则可以修改公式以包括相对于所需状态的误差增量xxx。 δxj∣txj∣t,des−xj∣tJ(x(t),U)∑jttT−1xj∣tTQxj∣tuj∣tTRuj∣t\delta x_{j|t}x_{j|t,des}-x_{j|t} \\ J(x(t),U) \sum_{jt}^{tT-1}x^{T}_{j|t}Qx_{j|t}u^T_{j|t}Ru_{j|t}δxj∣txj∣t,des−xj∣tJ(x(t),U)jt∑tT−1xj∣tTQxj∣tuj∣tTRuj∣t
下面我们给出其对应的优化公式是一个具有封闭形式的解决方案即线性二次调节器或LQR。 封闭式解决方案使用完整状态反馈这意味着在控制响应中使用了所有状态。 LQR解决方案定义了一个控制增益矩阵KKK该矩阵可以根据状态空间模型的AAA和BBB矩阵以及成本函数的QQQ和RRR矩阵进行计算。 minU≜{ut∣tut1∣t,…}J(x(t),U)xtT∣tTQfxtT∣t∑jttT−1xj∣tTQxj∣tuj∣tTRuj∣ts.t.xj1∣tAxt∣tBut∣t,t≤j≤tT−1\min _{U \triangleq\left\{u_{t \mid t} u_{t1 \mid t, \ldots\}}\right.} J(x(t), U)x_{tT \mid t}^{\mathrm{T}} Q_{f} x_{tT \mid t}\sum_{jt}^{tT-1} x_{j \mid t}^{\mathrm{T}} Q x_{j \mid t}u_{j \mid t}^{\mathrm{T}} R u_{j \mid t}\\\quad s.t. \quad x_{j1 \mid t}A x_{t \mid t}B u_{t \mid t}, \quad t \leq j \leq tT-1U≜{ut∣tut1∣t,…}minJ(x(t),U)xtT∣tTQfxtT∣tjt∑tT−1xj∣tTQxj∣tuj∣tTRuj∣ts.t.xj1∣tAxt∣tBut∣t,t≤j≤tT−1 现在让我们看一下非线性MPC
在更一般的情况下其目标函数是状态和receding horizon输入的任何可微的非线性函数。 对于这样的一般优化问题不存在封闭形式的解决方案。 因此我们必须依靠数值优化来找到解决方案。 4.4 Vehicle Lateral Control
现在让我们看一下MPC控制器在自动驾驶汽车上进行轨迹跟踪的效果。
MPC与之前介绍的反馈结构相同但我们将油门制动和转向命令作为回路中的低级控制器输出。 MPC控制器的输入包括参考路径和速度以及每个时间步的车辆状态。 MPC控制器的输出是遵循所需轨迹所需的横向和纵向力。 之后低级控制器转换为相应的油门断开和转向命令最后在每个时间步长上将操纵信号施加到车辆上并通过反馈回路获得新的车辆状态。
在这里我们定义了跟踪所需轨迹的成本函数包括最小化与所需轨迹的偏差和控制命令幅度。接下来我们根据先前视频中开发的横向和纵向模型定义车辆的运动约束。我们还对轮胎力施加最大限制以将其限制在线性轮胎区域内。
现在让我们看一下控制效果。
对双车道变更场景进行仿真右图显示了带有MPC控制的模拟操纵结果参考轨迹为蓝色实际车辆轨迹为红色。我们可以看到MPC控制器的跟踪性能非常好略有滞后但没有超调或振荡。可以看到模型预测控制器的输出横向和纵向力平滑变化。
同样绘制了在双车道变更操纵期间的车辆偏航率。 总结在本讲我们学习了
如何定义横向路径跟踪问题如何将pure pursuit跟踪和Stanley控制方法MPC控制方法。
