WENTAO的日记
自动驾驶——控制部分
汽车横向动力学模型
d d t [ y y ˙ ϕ ϕ ˙ ] = [ 0 1 0 0 0 − 2 C α f + 2 C α r m V x 0 − V x − 2 C α f l f − 2 C α r l r m V x 0 0 0 1 0 − 2 C α f l f − 2 C α r l r I z V x 0 − 2 C α f l f 2 + 2 C α r l r 2 I z V x ] [ y y ˙ ϕ ϕ ˙ ] + [ 0 2 C α f m 0 2 C α f l f I z ] δ \frac{d}{dt}\begin{bmatrix} y \\ \dot y \\ \phi \\ \dot \phi \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{2C_{\alpha f}+2C_{\alpha r}}{m V_x} & 0 & -V_x-\frac{2C_{\alpha f}l_f-2C_{\alpha r}l_r}{m V_x} \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{2C_{\alpha f}l_f-2C_{\alpha r}l_r}{I_z V_x} & 0 & -\frac{2C_{\alpha f}l_f^2+2C_{\alpha r}l_r^2}{I_z V_x} \end{bmatrix} \begin{bmatrix} y \\ \dot y \\ \phi \\ \dot \phi \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{2C_{\alpha f}}{m} \\ 0 \\ \frac{2C_{\alpha f}l_f}{I_z}\end{bmatrix} \delta dtd⎣⎢⎢⎡yy˙ϕϕ˙⎦⎥⎥⎤=⎣⎢⎢⎢⎡00001−mVx2Cαf+2Cαr0−IzVx2Cαflf−2Cαrlr00000−Vx−mVx2Cαflf−2Cαrlr1−IzVx2Cαflf2+2Cαrlr2⎦⎥⎥⎥⎤⎣⎢⎢⎡yy˙ϕϕ˙⎦⎥⎥⎤+⎣⎢⎢⎡0m2Cαf0Iz2Cαflf⎦⎥⎥⎤δ
状态空间定义
s
t
a
t
e
=
[
l
a
t
e
r
a
l
e
r
r
o
r
l
a
t
e
r
a
l
e
r
r
o
r
r
a
t
e
h
e
a
d
i
n
g
e
r
r
o
r
h
e
a
d
i
n
g
e
r
r
o
r
r
a
t
e
s
t
a
t
i
o
n
e
r
r
o
r
s
p
e
e
d
e
r
r
o
r
]
state = \begin{bmatrix} lateral\ error \\ lateral\ error\ rate \\ heading\ error \\ heading\ error\ rate \\ station\ error \\ speed\ error\end{bmatrix}
state=⎣⎢⎢⎢⎢⎢⎢⎡lateral errorlateral error rateheading errorheading error ratestation errorspeed error⎦⎥⎥⎥⎥⎥⎥⎤
D
e
s
i
r
e
P
o
i
n
t
纵
向
方
向
：
(
c
o
s
θ
d
e
s
,
s
i
n
θ
d
e
s
)
D
e
s
i
r
e
P
o
i
n
t
横
向
方
向
：
(
−
s
i
n
θ
d
e
s
,
c
o
s
θ
d
e
s
)
Desire\ Point纵向方向：(cos\theta_{des}, sin\theta_{des}) \quad \quad Desire\ Point横向方向：(-sin\theta_{des}, cos\theta_{des})
Desire Point纵向方向：(cosθdes,sinθdes)Desire Point横向方向：(−sinθdes,cosθdes)
R
e
a
l
P
o
i
n
t
坐
标
：
(
x
,
y
)
D
e
s
i
r
e
P
o
i
n
t
坐
标
：
(
x
d
e
s
,
y
d
e
s
)
Real\ Point坐标：(x,y) \qquad \qquad \qquad \qquad \quad \ \ \ Desire\ Point坐标：(x_{des},y_{des})
Real Point坐标：(x,y) Desire Point坐标：(xdes,ydes)
D
e
s
i
r
e
P
o
i
n
t
指
向
R
e
a
l
P
o
i
n
t
的
向
量
：
(
d
x
,
d
y
)
=
(
x
,
y
)
−
(
x
d
e
s
,
y
d
e
s
)
Desire\ Point指向Real\ Point的向量：(dx, dy) = (x,y) - (x_{des},y_{des})
Desire Point指向Real Point的向量：(dx,dy)=(x,y)−(xdes,ydes)
l
a
t
e
r
a
l
e
r
r
o
r
=
(
d
x
,
d
y
)
∗
(
−
s
i
n
θ
d
e
s
,
c
o
s
θ
d
e
s
)
=
d
y
c
o
s
θ
d
e
s
−
d
x
s
i
n
θ
d
e
s
lateral\ error=(dx, dy)*(-sin\theta_{des}, cos\theta_{des})=dy\ cos\theta_{des}-dx\ sin\theta_{des}
lateral error=(dx,dy)∗(−sinθdes,cosθdes)=dy cosθdes−dx sinθdes
l
a
t
e
r
a
l
e
r
r
o
r
r
a
t
e
=
V
∗
s
i
n
(
ϕ
−
ϕ
d
e
s
)
lateral\ error\ rate=V*sin\ (\phi -\phi_{des})
lateral error rate=V∗sin (ϕ−ϕdes)
h
e
a
d
i
n
g
e
r
r
o
r
=
ϕ
−
ϕ
d
e
s
heading\ error=\phi -\phi_{des}
heading error=ϕ−ϕdes
h
e
a
d
i
n
g
e
r
r
o
r
r
a
t
e
=
ϕ
˙
−
ϕ
˙
d
e
s
heading\ error\ rate=\dot \phi -\dot \phi_{des}
heading error rate=ϕ˙−ϕ˙des
s
t
a
t
i
o
n
e
r
r
o
r
=
−
(
d
x
,
d
y
)
∗
(
c
o
s
θ
d
e
s
,
s
i
n
θ
d
e
s
)
=
−
(
d
x
c
o
s
θ
d
e
s
+
d
y
s
i
n
θ
d
e
s
)
station\ error=-(dx, dy)*(cos\theta_{des}, sin\theta_{des})=-(dx\ cos\theta_{des}+dy\ sin\theta_{des})
station error=−(dx,dy)∗(cosθdes,sinθdes)=−(dx cosθdes+dy sinθdes)
s
p
e
e
d
e
r
r
o
r
=
V
d
e
s
−
V
∗
c
o
s
(
△
ϕ
)
/
k
speed\ error=V_{des}-V*cos( \triangle \phi)/k
speed error=Vdes−V∗cos(△ϕ)/k
方向盘模型
d d t [ e 1 e ˙ 1 e 2 e ˙ 2 ] = [ 0 1 0 0 0 − 2 C α f + 2 C α r m V x 2 C α f + 2 C α r m − 2 C α f l f + 2 C α r l r m V x 0 0 0 1 0 − 2 C α f l f − 2 C α r l r I z V x 2 C α f l f − 2 C α r l r I z − 2 C α f l f 2 + 2 C α r l r 2 I z V x ] [ e 1 e ˙ 1 e 2 e ˙ 2 ] + [ 0 2 C α f m 0 2 C α f l f I z ] δ + [ 0 − 2 C α f l f − 2 C α r l r m V x − V x 0 − 2 C α f l f 2 + 2 C α r l r 2 I z V x ] ϕ ˙ d e s + [ 0 0 0 1 ] ϕ ¨ d e s \frac{d}{dt}\begin{bmatrix} e_1 \\ \dot e_1 \\ e_2 \\ \dot e_2 \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{2C_{\alpha f}+2C_{\alpha r}}{m V_x} & \frac{2C_{\alpha f}+2C_{\alpha r}}{m} & -\frac{2C_{\alpha f}l_f+2C_{\alpha r}l_r}{m V_x} \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{2C_{\alpha f}l_f-2C_{\alpha r}l_r}{I_z V_x} & \frac{2C_{\alpha f}l_f-2C_{\alpha r}l_r}{I_z} & -\frac{2C_{\alpha f}l_f^2+2C_{\alpha r}l_r^2}{I_z V_x} \end{bmatrix} \begin{bmatrix} e_1 \\ \dot e_1 \\ e_2 \\ \dot e_2 \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{2C_{\alpha f}}{m} \\ 0 \\ \frac{2C_{\alpha f}l_f}{I_z}\end{bmatrix}\delta + \begin{bmatrix} 0 \\ -\frac{2C_{\alpha f}l_f-2C_{\alpha r}l_r}{m V_x}-V_x \\ 0 \\ -\frac{2C_{\alpha f}l_f^2+2C_{\alpha r}l_r^2}{I_z V_x}\end{bmatrix}\dot \phi_{des} + \begin{bmatrix} 0 \\ 0 \\ 0 \\ 1 \end{bmatrix} \ddot \phi_{des} dtd⎣⎢⎢⎡e1e˙1e2e˙2⎦⎥⎥⎤=⎣⎢⎢⎢⎡00001−mVx2Cαf+2Cαr0−IzVx2Cαflf−2Cαrlr0m2Cαf+2Cαr0Iz2Cαflf−2Cαrlr0−mVx2Cαflf+2Cαrlr1−IzVx2Cαflf2+2Cαrlr2⎦⎥⎥⎥⎤⎣⎢⎢⎡e1e˙1e2e˙2⎦⎥⎥⎤+⎣⎢⎢⎡0m2Cαf0Iz2Cαflf⎦⎥⎥⎤δ+⎣⎢⎢⎢⎡0−mVx2Cαflf−2Cαrlr−Vx0−IzVx2Cαflf2+2Cαrlr2⎦⎥⎥⎥⎤ϕ˙des+⎣⎢⎢⎡0001⎦⎥⎥⎤ϕ¨des
Apollo LQR控制器原理
- 微分方程： x ˙ = A 0 x + B 0 u \dot x=A_0x+B_0u x˙=A0x+B0u
- 离散处理： x k + 1 = A x k + B u k x_{k+1}=Ax_k+Bu_k xk+1=Axk+Buk
- 构建优化目标： J = ∑ k = 1 N ( X T Q X + u T R u ) J=\sum_{k=1}^N(X^TQX+u^TRu) J=k=1∑N(XTQX+uTRu)
- 最优控制：
u
=
−
[
(
R
+
B
T
P
B
)
−
1
B
T
P
A
]
X
=
−
K
X
u=-[(R+B^TPB)^{-1}B^TPA]X=-KX
u=−[(R+BTPB)−1BTPA]X=−KX
其中： P = A T P A − A T P B ( R + B T P B ) − 1 B T P A + Q P=A^TPA-A^TPB(R+B^TPB)^{-1}B^TPA+Q P=ATPA−ATPB(R+BTPB)−1BTPA+Q
注：LQR只能给出横向的控制
附录
1、latex中的希腊字母：
https://blog.csdn.net/xxzhangx/article/details/52778539
2、latex编写矩阵：
0
1
1
0
(
0
−
i
i
0
)
[
0
−
1
1
0
]
{
1
0
0
−
1
}
∣
a
b
c
d
∣
∥
i
0
0
−
i
∥
\begin{gathered} \begin{matrix} 0 & 1 \\ 1 & 0 \end{matrix} \begin{pmatrix} 0 & -i \\ i & 0 \end{pmatrix} \begin{bmatrix} 0 & -1 \\ 1 & 0 \end{bmatrix} \begin{Bmatrix} 1 & 0 \\ 0 & -1 \end{Bmatrix} \begin{vmatrix} a & b \\ c & d \end{vmatrix} \begin{Vmatrix} i & 0 \\ 0 & -i \end{Vmatrix} \end{gathered}
0110(0i−i0)[01−10]{100−1}∣∣∣∣acbd∣∣∣∣∥∥∥∥i00−i∥∥∥∥
3、github下载repo中特定文件夹: http://kinolien.github.io/gitzip/#
4、typedef Matrix<double ,Dynamic,Dynamic > MatrixXd;
5、LQR求解原理: https://blog.csdn.net/datase/article/details/78487126
6、Frenet坐标变换：
- https://blog.csdn.net/l898985121/article/details/109048855
- https://blog.csdn.net/weixin_37395438/article/details/112973098?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-1.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-1.control
7、LQR求解原理：https://blog.csdn.net/datase/article/details/78487126
C++ debug
1. override关键字：
Override是伪代码，所以是可写可不写的．它表示方法重写，写上会给我们带来好处．
- 可以当注释用,方便阅读．
- 告诉阅读你代码的人，这是方法的复写．
- 编译器可以给你验证@Override下面的方法名是否是你父类中所有的，如果没有则报错
2. static 函数：
声明该函数的文件能用，其他文件不能用。
3. C++的三角函数：
C++中cos,sin,asin,acos这些三角函数操作的是弧度,而非角度,
1178
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
