Lqr算法

1.横向控制动态模型

a.自行车动态模型

在车辆高速行驶的时候、每个车轮上的方向和车辆的方向一致的假设就不成立，即non-zero slip angle
汽车动力学模型：伸出右手-中指向上☞、食指向前，拇指向右；其中食指->Roll、拇指->pitch、中指->Yao.

b.线性自行车动态模型

自行车运动学模型：
自行车模型做出如下假设：

• 纵向速度恒定、左右车轮集中成一个车轮
• 忽略悬架运动、道路的倾斜、空气阻力影响，以便对汽车的纵向和横向的运动解耦分析；
  $$\dot{\psi }=\frac{V}{R};F(向心)=\frac{V_{{}_x}^2}{R}$$
  代入上式
  $$a_{{}_y}=(\frac{d^{2}y}{d{t}^2})=m(\dot{v_{{}_v}+v_{{}_x}\dot{\psi }})$$
  $$l_{{}_f}\cdot F_{{}_{yf}}-l_{{}_r}\cdot F_{{}_{yr}}=I_{{}_z}\ddot{\psi }$$
  我们可以使用$$r=\dot{\psi }$$代表偏航角yaw的角速度
  在经过了大量的轮胎打滑实验得出结论：
  对于slip-angles较小时，实现得到轮胎滑移角的线性函数
  轮胎变量
• 前轮滑移角${\alpha }_{{}_f}$
• 后轮滑移角${\alpha }_{{}_r}$

前后轮侧向力
轮胎滑移角（split_angles）针对与轮胎和车辆的纵轴而言；
对于小split-angles，实验证明，轮胎侧向力与“split-angles”呈现正比关系
$$F_{{}_{yf}}=2C_{{}_f}\alpha {}_{{}_f}=2C_{{}_f}(\delta -{\theta }_{{}_{vf}})$$
$$F_{{}_{yr}}=2C_{{}_r}\alpha {}_{{}_r}=2C_{{}_r}(\delta -{\theta }_{{}_{vr}})$$
对于自行车模型的前轮，前轮速度方向和在车身x轴和y轴的分量构成直角三角形
$${\theta }_{{}_{vf}}=ta{n}^{-1}\frac{v_{{}_{fy}}}{v_{{}_{fx}}}=ta{n}^{-1}(\frac{v_{{}_y}+l_{{}_r}r}{V_{{}_x}})$$
$${\theta }_{{}_{vr}}=ta{n}^{-1}\frac{v_{{}_{ry}}}{v_{{}_{rx}}}=ta{n}^{-1}(\frac{v_{{}_y}-l_{{}_r}r}{V_{{}_x}})$$
当$r=\dot{\psi }$偏航速度等于偏航轴角速度
由于牛顿第一定律
$${\dot{F}}_{{}_{yf}}+{\dot{F}}_{{}_{yr}}=m{a}_{{}_y}=m(\dot{v_{{}_y}}+v_{{}_x}\dot{\psi })$$
$$l_{{}_f}\dot{F}-l_{{}_r}{\dot{F}}_{{}_{yr}}=I_{{}_z}\ddot{\psi }$$
$$(F_{{}_{yf}}cos(\delta )-F_{{}_{xf}}sin(\delta ))+F_{{}_{yr}}=m(\dot{v_{{}_y}}+v_{{}_x}r)$$
$$l_{{}_f}(F_{{}_{yf}}cos(\delta )-F_{{}_{xf}}sin(\delta ))-l_{{}_r}{F}_{{}_{yr}}=I_{{}_z}\ddot{\psi }=I_{{}_z}\dot{r}$$
总结所示的横向力（假设纵向速度是可控的）
其中r是关于偏航角的角速度
摒弃上节所讲滑移约束，假设是匀速运动
可以通过：
$${\alpha }_{{}_f}=\delta -ta{n}^{-1}(\frac{v_{{}_y}+l_{{}_f}r}{v_{{}_x}})$$
$${\alpha }_{{}_r}=-ta{n}^{-1}(\frac{v_{{}_y}-l_{{}_r}r}{v_{{}_x}})$$
$$F_{{}_{yf}}=c_{{}_f}{\alpha }_{{}_f}=c_{{}_f}[\theta -ta{n}^{-1}(\frac{v_{{}_f}+l_{{}_r}r}{v_{{}_x}})]$$
$$F_{{}_{yr}}=c_{{}_r}{\alpha }_{{}_r}=-c_{{}_r}ta{n}^{-1}(\frac{v_{{}_y}-l_{{}_r}r}{v_{{}_x}})$$
得到：
$${\dot{v}}_{{}_y}=\frac{c_{{}_f}[\theta -ta{n}^{-1}(\frac{v_{{}_y}+l_{{}_f}r}{v_{{}_x}})]cos(\delta )-c_{{}_r}ta{n}^{-1}(\frac{v_{{}_y}-l_{{}_r}r}{v_{{}_x}})-F_{{}_{xy}}sin(\delta )}{m}-v_{{}_x}r$$
$$\dot{r}=\frac{l_{{}_f}{c}_{{}_f}[\delta -ta{n}^{-1}(\frac{v_{{}_y}+l_{{}_f}r}{v_{{}_x}})]+l_{{}_r}{c}_{{}_r}ta{n}^{-1}(\frac{v_{{}_y}-l_{{}_r}r}{v_{{}_x}})-l_{{}_f}{F}_{{}_{xf}}sin(\delta )}{I_{{}_z}}$$
1.应用小角度假设简化模型
*假设$\delta$=0 所以$sin(\delta )\approx 0,cos(\delta )\approx 1,ta{n}^{-1}(\delta )\approx \theta$集合上面的两个式子得到
$${\dot{v}}_{{}_y}=\frac{-c_{{}_f}{v}_{{}_y}-c_{{}_f}{l}_{{}_f}r}{m{v}_{{}_x}}+\frac{c_{{}_f}\delta }{m}+\frac{-c_{{}_r}{v}_{{}_y}+c_{{}_r}{l}_{{}_r}r}{m{v}_{{}_x}}-v_{{}_x}r$$
$$\dot{r}=\frac{-l_{{}_f}{c}_{{}_f}{v}_{{}_y}-l_{{}_f}^2{c}_{{}_f}r}{I_{{}_z}{v}_{{}_x}}+\frac{l_{{}_f}{c}_{{}_f}\delta }{I_{{}_z}}+\frac{l_{{}_z}{c}_{{}_r}{v}_{{}_y}-l^2{c}_{{}_r}r}{I_{{}_z}{v}_{{}_x}}$$
2.根据变量重新分类

c.模型参数辨识

与之前的模型不同，动态自行车模型的参数没有那么方便直接测量，然而一个可行的办法就是给车的四个轮子下放上称；用一根无质量的杆将两个点质量结合在一起；

车辆的转动惯量，近似的看作

2.LQR-线性二次调节器

a.什么是Lqr

b.一维标量示例

c.通解和Raccati方程

d.LQR使用案例

3.车辆横向最优控制

a.路径坐标模型

b.使用Lqr进行轨迹跟踪

c.带有Preview的轨迹跟踪

lqr的好处：若我们将系统设置为以误差为参考的系统，就可以直接套用lqr的结论得到一个相对优化的控制器。并且在实验的过程中如果，横向误差收敛的很慢，可以考虑提高；若前轮转角出现震荡，可以考虑降低;​​​​​​​

lqr的缺点：

1、由于lqr是一个线性控制器，默认小 ，面对曲率过大的情况表现不佳；2、lqr只关注于当前点的情况，并没有考虑到未来路径的情况，即较短视；
lqr性能的提高
思路：用已知的路径做出预判，提前打方向盘，就可以提高性能