自动驾驶技术基础赛车仿真

本文最后更新于:2022年7月18日 下午

用尽各种手段控制最终依旧屈服于调参

项目简介

​ 拿到GitHub仓库之后我就开始搜索相关资料,代码和苏黎世联邦理工学院自动控制实验室 (IfA) 的MPCC仿真有很大部分重合。但在实际调试之后我发现,老师使用的控制是LQR,车的模型参数是一致的但其他代码基本都是多余的,核心就是slx的仿真文件。

模型建立

关于高速转向车辆建模,老师在课上有详细的推导过程,这里只展示结果: \[ \begin{aligned} {\left[\begin{array}{c} \dot{X} \\ \dot{Y} \\ \dot{\psi} \\ \dot{v}_{x} \\ \dot{v}_{y} \\ \dot{\omega} \end{array}\right]=\left[\begin{array}{c} v_{x} \cos \psi+v_{x} \sin \psi \\ v_{y} \cos \psi-v_{y} \sin \psi \\ \omega\\ \frac{1}{m}\left(F_{x r}+\cos \delta v_{x} \sin \psi\right. \\ \frac{1}{m}\left(F_{y r}+\sin \delta F_{y f}+m v_{y} \omega\right) \\ \frac{1}{I_{z}}\left(l_{f} F_{x f} \sin \delta+\cos \delta F_{y f}-m v_{x} \omega\right) \\ \end{array}\right] } \\ \alpha_{r}=\arctan \left(\frac{v_{y}-l_{r} \omega}{v_{x}}\right) \\ \alpha_{f}=-\delta+\arctan \left(\frac{v_{y}+l_{f} \omega}{v_{x}}\right) \\ F_{y r}=2 D_{r} \sin \left(C_{r} \arctan \left(-\alpha_{r} B_{r}\right)\right) \\ F_{y f}=2 D_{f} \sin \left(C_{f} \arctan \left(-\alpha_{f} B_{f}\right)\right) \\ F_{x r}=F_{x}=\left(C_{m 1}-C_{m 2} v_{x}\right) d-C_{r} N-C_{d} v_{x}^{2} \\ F_{x f}=-C_{r} N-C_{d} v_{x}^{2} \end{aligned} \]

体现在系统的s-function中如下:

控制仿真

​ 简单介绍一下使用到的控制方法:

LQR

​ 最优控制理论主要探讨的是让动力系统以在最小成本来运作,若系统动态可以用一组线性微分方程表示,而其成本为二次泛函,这类的问题称为线性二次(LQ)问题。此类问题的解即为线性二次调节器(英语:linear–quadratic regulator),简称LQR。

有无限时间/有限时间,离散时间/连续时间几种类型。思路如下:

  • 选择参数矩阵Q,R
  • 求解Riccati方程得到矩阵 \(P\)
  • 根据P计算 \(K=R^{-1} B^{T} P\)
  • 计算控制量 \(u=-K x\)

PID

​ PID控制器(比例-积分-微分控制器),由比例单元(Proportional)、积分单元(Integral)和微分单元(Derivative)组成。可以透过调整这三个单元的增益\(K_P\)\(K_I\)\(K_D\)来调定其特性。PID控制器主要适用于基本上线性,且动态特性不随时间变化的系统。 \[ \mathrm{u}(t)=\mathrm{MV}(t)=K_{p} e(t)+K_{i} \int_{0}^{t} e(\tau) d \tau+K_{d} \frac{d}{d t} e(t) \]

其中 \(K_{p}\) : 比例增益,是调适参数 \(K_{i}\) : 积分增益,也是调适参数 \(K_{d}\) : 微分增益,也是调适参数 \(e\) : 误差 \(=\) 设定值 \((\mathrm{SP})-\) 回授值 (PV) \(t\) : 目前时间 \(\tau\) : 积分变数,数值从 0 到目前时间 \(t\)

​ PID的使用非常简单,在simulink中也只是添加一个单输入单输出的单元,本次实验的提升几乎靠的是PID控制。

MPC

\[ \begin{array}{ll} \min & \sum_{k=1}^{N}\left[\begin{array}{c} \hat{e}_{k}^{c} \\ \hat{e}_{k}^{l} \end{array}\right]^{T}\left[\begin{array}{cc} q_{c} & 0 \\ 0 & q_{l} \end{array}\right]\left[\begin{array}{c} \hat{e}_{k}^{c} \\ \hat{e}_{k}^{l} \end{array}\right]-q_{v} v_{\theta, k}+\Delta u_{k}^{T} R_{\Delta} \Delta u_{k} \\ \text { s.t. } & x_{0}=x(0) \\ & x_{k+1}=f\left(x_{k}, u_{k}\right) \\ & \hat{e}^{c}\left(x_{k}\right)=\sin \left(\Phi^{\mathrm{ref}}\left(\theta_{k}\right)\right)\left(X_{k}-X^{\mathrm{ref}}\left(\theta_{k}\right)\right)-\cos \left(\Phi^{\mathrm{ref}}\left(\theta_{k}\right)\right)\left(Y_{k}-Y^{\mathrm{ref}}\left(\theta_{k}\right)\right) \\ & \hat{e}^{l}\left(x_{k}\right)=-\cos \left(\Phi^{\mathrm{ref}}\left(\theta_{k}\right)\right)\left(X_{k}-X^{\mathrm{ref}}\left(\theta_{k}\right)\right)-\sin \left(\Phi^{\mathrm{ref}}\left(\theta_{k}\right)\right)\left(Y_{k}-Y^{\mathrm{ref}}\left(\theta_{k}\right)\right) \\ & \Delta u_{k}=u_{k}-u_{k-1} \\ & x_{k} \in \mathcal{X}_{\text {Track }} \\ & \underline{x} \leq x_{k} \leq \bar{x} \\ & \underline{u} \leq u_{k} \leq \bar{u} \\ & \Delta u \leq \Delta u_{k} \leq \overline{\Delta u} \end{array} \]

\[ \begin{array}{ll} F_{f, y}=D_{f} \sin \left(C_{f} \arctan \left(B_{f} \alpha_{f}\right)\right) \quad \text { where } & \alpha_{f}=-\arctan \left(\frac{\dot{\varphi} l_{f}+v_{y}}{v_{x}}\right)+\delta \\ F_{r, y}=D_{r} \sin \left(C_{r} \arctan \left(B_{r} \alpha_{r}\right)\right) \quad \text { where } \quad \alpha_{r}=\arctan \left(\frac{\dot{\varphi} l_{r}-v_{y}}{v_{x}}\right) \\ F_{r, x}=\left(C_{m 1}-C_{m 2} v_{x}\right) d-C_{r}-C_{d} v_{x}^{2} \end{array} \]

​ 最后,问题的状态和输入结果如下:

\[ \begin{aligned} &x=\left[X, Y, \varphi, v_{x}, v_{y}, \omega, \theta\right] \\ &u=\left[d, \delta, v_{\theta}\right] \end{aligned} \]

​ 因为有MPCC的这个仓库作为参考,故这也是我最开始的改进方向,接下来会详细介绍过程与结果。

实验过程与结果分析

​ 首先我把仓库中的所有代码文件简单浏览了一遍,将vx改到16,时间降低到17.2690s。然后我开始研究如何将MPC控制加入进来。

​ 可以选择的方法有两种,一种是根据原理编写m文件,另外一种是利用simulink中的元件。可惜最后实现效果均不理想,我没能将控制成功使用进来,反而是让结果越来越差。

​ 于是我就考虑既然IFA的MPCC仿真我可以成功运行,没能从中借鉴到控制的使用,能否将我的已知轨迹放到程序中规划,再将得到的最优轨迹导出,作为我的仿真的引导。

​ 程序按照我设想地运行,但是结果很不理想,每次都运行一半车就跑出轨道,几个优化包算出来的结果都是inf,估计与约束有关。

​ 不得已我放弃了这种想法,继续在仿真中的PID调参,结果有所提升。其中的某些结果路径比较符合我的期望,结合MPC的滚动优化的思想,我想是否可以将我每次运行的路径保存下来,作为下一次运行的引导线加入仿真中,再结合已有的控制器轨迹追踪,以此来提升效果。

​ 可惜结果依旧不理想,最终我放弃了这个想法。这期间我还试着查询了MPC的相关论文,使用python,pytorch,ros等平台来仿真,但原本的控制没有让我从轨迹修改获得提升。不得已只能采取vx,PID的三个增益这4类参数的调优,调试思路总体沿着齐格勒-尼科尔斯方法,不断修改,直至获得最终时间为:12.866s。

​ 默认将小车视为质点,已经放大检查过了,上述结果没有与边界发生碰撞。

​ 由于没有使用PID tuner,参数是随机试出来的,且除了\(K_i\)均为整数。我估计如果愿意从小数点后一点点慢慢调试可能达到更优的结果,但试到这里我觉得再改下去没有什么意义了。本次项目是我目前做到的最“自由”的作业,老师从开学第一节课开始铺垫,我也尝试了卡尔曼滤波,Pure Pursuit,MPC等等方法,即使实力不够没能成功将这些控制实现,但在回忆老师指导内容,查阅资料的过程中也收获了很多乐趣,深刻感受到了控制之美。


自动驾驶技术基础赛车仿真
http://enderfga.cn/2022/07/18/auto3/
作者
Enderfga
发布于
2022年7月18日
许可协议