Vehicle Lateral Optimal Control
代码实现参考: link
Linear Quadratic Regulator (LQR)
A system is described by the standard linear state space model:
x ˙ = A x + B u y = C x \dot{x} = Ax + Bu \\ y = Cx x˙=Ax+Buy=Cx
The objective is to bring the non-zero initial state to zero in the infinite time horizon.
The cost function takes the quadratic form:
J = 1 2 ∫ 0 ∞ ( x T Q x + u T R u ) d t J = \frac{1}{2} \int_{0}^{\infty} (x^T Qx + u^T Ru)dt J=21∫0∞(xTQx+uTRu)dt
(The advantage of using the quadratic form: avoid negative numbers in the area when the state x is negative; a minimum value can always be found)
Q = [ q 1 0 … 0 0 q 2 … 0 ⋮ ⋮ ⋱ ⋮ 0 0 … q n ] Q = \begin{bmatrix} q_1 & 0 & \ldots & 0 \\ 0 & q_2 & \ldots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \ldots & q_n \end{bmatrix} Q= q10⋮00q2⋮0……⋱…00⋮qn
And ( R ) is:
R = [ r 1 0 … 0 0 r 2 … 0 ⋮ ⋮ ⋱ ⋮ 0 0 … r m ] R = \begin{bmatrix} r_1 & 0 & \ldots & 0 \\ 0 & r_2 & \ldots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \ldots & r_m \end{bmatrix} R= r10⋮00r2⋮0……⋱…00⋮rm
Note from x T Q x = q 1 x 1 2 + q 2 x 2 2 + … + q n x n 2 x^TQx = q_{1}x_{1}^2 + q_{2}x_{2}^2 + \ldots + q_{n}x_{n}^2 xTQx=q1x12+q2x22+…+qnxn2, where ( Q ) is defined as a matrix with diagonal elements ( q_i ). Where q i ≥ 0 q_i \geq 0 qi≥0, for ( i = 1, 2, … , n ) and r i > 0 r_i > 0 ri>0, for ( i = 1, 2, … , m ).
- ( q_i ) are relative weightings among ( x_i ).
- If ( q_1 ) is bigger than ( q_2 ), there is a higher penalty/price on error in ( x_1 ) than in ( x_2 ), and control will try to make ( x_1 ) smaller than ( x_2 ), vice versa. (Pay more attention to ( q_1 ))
The same principle applies to u T R u = r 1 u 1 2 + r 2 u 2 2 + … + r m u m 2 u^TRu = r_1u_1^2 + r_2u_2^2 + \ldots + r_mu_m^2 uTRu=r1u12+r2u22+…+rmum2.
LQR General Solution (Raccati Equation)
For a LQR problem defined as
System:
x
˙
=
A
x
+
B
u
y
=
C
x
\dot{x} = Ax + Bu \\ y = Cx
x˙=Ax+Buy=Cx
State feedback:
u
=
−
K
x
u = -Kx
u=−Kx
The closed loop system is:
x
˙
=
(
A
−
B
K
)
x
=
A
c
l
x
.
\dot{x} = (A - BK)x = A_{cl}x .
x˙=(A−BK)x=Aclx.
Cost function:
J
=
1
2
∫
0
∞
(
x
T
Q
x
+
u
T
R
u
)
d
t
J = \frac{1}{2} \int_{0}^{\infty} (x^T Qx + u^T Ru) dt
J=21∫0∞(xTQx+uTRu)dt
Control law:
u
=
−
K
x
=
−
R
−
1
B
T
P
x
,
u = -Kx = -R^{-1}B^TPx,
u=−Kx=−R−1BTPx,
will bring ( x(\infty) ) to 0 and ( u(\infty) ) to 0
where
A
T
P
+
P
A
+
Q
=
P
B
R
−
1
B
T
P
A^TP + PA + Q = PBR^{-1}B^TP
ATP+PA+Q=PBR−1BTP
that is, the optimal control law is a linear feedback of the state vector ( x ), as assumed.
Dynamic Model of Lateral Vehicle Motion
“Bicycle” dynamic model:
a y = ( d 2 y d t 2 ) inertial = v ˙ y + v x ψ ˙ a_y = \left(\frac{d^2y}{dt^2}\right)_{\text{inertial}} = \dot{v}_y + v_x\dot{\psi} ay=(dt2d2y)inertial=v˙y+vxψ˙
F y f ′ + F y r ′ = m a y = m ( v ˙ y + v x ψ ¨ ) F'_{yf} + F'_{yr} = m a_y = m(\dot{v}_y + v_x \ddot{\psi}) Fyf′+Fyr′=may=m(v˙y+vxψ¨)
l f F y f ′ − l r F y r ′ = I z ψ ˙ l_f F'_{yf} - l_r F'_{yr} = I_z \dot{\psi} lfFyf′−lrFyr′=Izψ˙
after considering the steering angle δ .
( F y f cos ( δ ) − F x f sin ( δ ) ) + F y r = m ( v ˙ y + v x r ) (F_{yf} \cos(\delta) - F_{xf} \sin(\delta)) + F_{yr} = m(\dot{v}_y + v_x r) (Fyfcos(δ)−Fxfsin(δ))+Fyr=m(v˙y+vxr)
l f ( F y f cos ( δ ) − F x f sin ( δ ) ) − l r F y r = I z ψ ˙ = I z r l_f (F_{yf} \cos(\delta) - F_{xf} \sin(\delta)) - l_r F_{yr} = I_z \dot{\psi} = I_z r lf(Fyfcos(δ)−Fxfsin(δ))−lrFyr=Izψ˙=Izr
Front and Rear Tire Forces:
F y f = c f α f = c f ( δ − θ v f ) F_{yf} = c_f\alpha_f = c_f(\delta - \theta_{vf}) Fyf=cfαf=cf(δ−θvf)
F y r = c r α r = c r ( − θ v r ) F_{yr} = c_r\alpha_r = c_r(-\theta_{vr}) Fyr=crαr=cr(−θvr)
θ v f = tan − 1 ( v y f v x f ) = tan − 1 ( v y + l f r v x ) \theta_{vf} = \tan^{-1}\left(\frac{v_{yf}}{v_{xf}}\right) = \tan^{-1}\left(\frac{v_y + l_f r}{v_x}\right) θvf=tan−1(vxfvyf)=tan−1(vxvy+lfr)
θ v r = tan − 1 ( v y r v x r ) = tan − 1 ( v y − l r r v x ) \theta_{vr} = \tan^{-1}\left(\frac{v_{yr}}{v_{xr}}\right) = \tan^{-1}\left(\frac{v_y - l_r r}{v_x}\right) θvr=tan−1(vxrvyr)=tan−1(vxvy−lrr)
Lateral and Yaw Dynamics:
v ˙ y = c f [ δ − tan − 1 ( v y + r l f v x ) ] cos ( δ ) − c r tan − 1 ( v y − r l r v x ) − F x f sin ( δ ) m − v x r \dot{v}_y = \frac{c_f \left[ \delta - \tan^{-1}\left(\frac{v_y + {r}l_f}{v_x}\right) \right] \cos(\delta) - c_r\tan^{-1}\left(\frac{v_y - {r}l_r}{v_x}\right) - F_{xf}\sin(\delta)}{m} - v_xr v˙y=mcf[δ−tan−1(vxvy+rlf)]cos(δ)−crtan−1(vxvy−rlr)−Fxfsin(δ)−vxr
r ˙ = c f l f [ δ − tan − 1 ( v y + r l f v x ) ] cos ( δ ) + c r l r tan − 1 ( v y − r l r v x ) − l f F x f sin ( δ ) I z \dot{r} = \frac{c_f l_f \left[ \delta - \tan^{-1}\left(\frac{v_y + {r}l_f}{v_x}\right) \right] \cos(\delta) + c_r l_r \tan^{-1}\left(\frac{v_y - {r}l_r}{v_x}\right) - l_f F_{xf}\sin(\delta)}{I_z} r˙=Izcflf[δ−tan−1(vxvy+rlf)]cos(δ)+crlrtan−1(vxvy−rlr)−lfFxfsin(δ)
after small angle assumptions and re-group by variables:
cos
(
δ
)
≈
1
\cos(\delta) \approx 1
cos(δ)≈1
sin ( δ ) ≈ 0 \sin(\delta) \approx 0 sin(δ)≈0
tan − 1 ( δ ) ≈ δ \tan^{-1}(\delta) \approx \delta tan−1(δ)≈δ
v ˙ y = − ( c f + c r ) m v x v y + [ ( l r c r − l f c f ) m v x ] v x r + c f m δ \dot{v}_y = -\frac{(c_f + c_r)}{mv_x}v_y + \left[\frac{(l_rc_r - l_fc_f)}{mv_x}\right]v_x r + \frac{c_f}{m}\delta v˙y=−mvx(cf+cr)vy+[mvx(lrcr−lfcf)]vxr+mcfδ
r
˙
=
l
f
c
f
−
l
r
c
r
I
z
v
x
v
y
+
[
−
(
c
f
l
f
2
+
c
r
l
r
2
)
I
z
v
x
]
r
+
l
f
c
f
I
z
δ
\dot{r} = \frac{l_f c_f - l_r c_r}{I_z v_x}v_y + \left[-\frac{(c_f l_f^2 + c_r l_r^2)}{I_z v_x}\right]r + \frac{l_f c_f}{I_z}\delta
r˙=Izvxlfcf−lrcrvy+[−Izvx(cflf2+crlr2)]r+Izlfcfδ
Dynamic Bicycle Model
#### Linearized Dynamic Model of Lateral Vehicle MotionIf we use state
X
=
[
y
v
y
ψ
ψ
˙
]
a
n
d
i
n
p
u
t
δ
,
\mathbf{X} = \begin{bmatrix} y\\ v_y \\ \psi \\ \dot{\psi} \end{bmatrix} \ and \ input \ \delta \ ,
X=
yvyψψ˙
and input δ ,
rewrite in state space model:
X
˙
=
A
X
+
B
δ
,
\mathbf{\dot{X}} = \mathbf{A}\mathbf{X} + \mathbf{B}\delta \ ,
X˙=AX+Bδ ,
it is
d d t [ y v y ψ ψ ˙ ] = [ 0 1 0 0 0 − ( c f + c r ) m v x 0 ( l r c r − l f c f ) m v x − v x 0 0 0 1 0 l f c f − l r c r I z v x 0 − ( c f l r 2 + c r l f 2 ) I z v x ] [ y v y ψ ψ ˙ ] + [ 0 c f m 0 l f c f I z ] δ \frac{d}{dt}\begin{bmatrix} y\\ \ v_y \\ \psi \\ \dot{\psi} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{(c_f + c_r)}{m v_x} & 0 & \frac{(l_r c_r - l_f c_f)}{m v_x} - v_x \\ 0 & 0 & 0 & 1 \\ 0 & \frac{l_f c_f - l_r c_r}{I_z v_x} & 0 & -\frac{(c_f l_r^2 + c_r l_f^2)}{I_z v_x} \end{bmatrix} \begin{bmatrix} y \\ v_y \\ \psi \\ \dot{\psi} \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{c_f}{m} \\ 0 \\ \frac{l_f c_f}{I_z} \end{bmatrix} \delta dtd y vyψψ˙ = 00001−mvx(cf+cr)0Izvxlfcf−lrcr00000mvx(lrcr−lfcf)−vx1−Izvx(cflr2+crlf2) yvyψψ˙ + 0mcf0Izlfcf δ
Trajectory tracking with LQR
Path Coordinates Model (Error dynamics)
For path tracking, it is useful to express the bicycle model with respect to the path function of its length 𝑠 and with the constant longitudinal velocity assumption.
We can choose
x
=
[
e
c
g
e
c
g
˙
e
θ
e
θ
˙
]
T
x = \begin{bmatrix} e_{cg} & \dot{e_{cg}} & e_{\theta} & \dot{e_{\theta}} \end{bmatrix}^T
x=[ecgecg˙eθeθ˙]T
as our system state and u = δ u = \delta u=δ.
- e c g e_{cg} ecg: Orthogonal distance of the C.G. to the nearest path waypoint;
- e c g ˙ \dot{e_{cg}} ecg˙: Relative speed between vehicle C.G and path;
- e θ e_{\theta} eθ: Heading/Yaw difference between vehicle and path, e θ = θ − θ p ( s ) e_{\theta} = \theta - \theta_p(s) eθ=θ−θp(s)
- e θ ˙ \dot{e_{\theta}} eθ˙: Relative yaw rate between vehicle C.G and path, e θ = r − r ( s ) e_{\theta} = r - r(s) eθ=r−r(s) where r ( s ) = θ ( s ) ˙ r(s) = \dot{\theta(s)} r(s)=θ(s)˙ is the yaw rate derived from the path.
Dynamic Bicycle Model in path coordinates
With the constant longitudinal velocity assumption,e ˙ c g = v y + v x tan ( θ − β p ( s ) ) = v y + v x tan ( e θ ) \dot{e}_{cg} = v_y + v_x \tan(\theta - \beta_p(s)) \\= v_y + v_x \tan(e_{\theta}) e˙cg=vy+vxtan(θ−βp(s))=vy+vxtan(eθ)
Thus, the acceleration of C.G. is:
e
˙
c
g
=
(
v
˙
y
+
v
x
r
′
)
−
v
˙
y
(
s
)
=
v
˙
y
+
v
x
(
r
−
r
(
s
)
)
=
v
˙
y
+
v
x
e
˙
θ
.
\dot{e}_{cg} = (\dot{v}_y + v_xr') - \dot{v}_y(s) \\ = \dot{v}_y + v_x(r - r(s)) \\= \dot{v}_y + v_x\dot{e}_\theta.
e˙cg=(v˙y+vxr′)−v˙y(s)=v˙y+vx(r−r(s))=v˙y+vxe˙θ.
Convert lateral dynamic to error dynamics:
v
y
=
e
˙
c
g
−
v
x
sin
(
e
θ
)
v
˙
y
=
e
˙
c
g
−
v
x
e
˙
θ
θ
=
e
θ
+
θ
p
(
s
)
r
=
e
˙
θ
+
r
(
s
)
r
˙
=
e
¨
θ
+
r
˙
(
s
)
v_y = \dot{e}_{cg} - v_x\sin(e_\theta) \\ \dot{v}_y = \dot{e}_{cg} - v_x\dot{e}_\theta \\ \theta = e_\theta + \theta_p (s) \\ r = \dot{e}_\theta + r(s) \\ \dot{r} = \ddot{e}_\theta + \dot{r}(s)
vy=e˙cg−vxsin(eθ)v˙y=e˙cg−vxe˙θθ=eθ+θp(s)r=e˙θ+r(s)r˙=e¨θ+r˙(s)
Now, we have the linear lateral dynamic model. Rewrite it as:
x
˙
=
A
x
+
B
1
δ
+
B
2
r
d
e
s
,
where
x
=
(
e
c
g
e
˙
c
g
e
θ
e
˙
θ
)
T
.
\dot{x} = Ax + B_1\delta + B_2 \ r_{des}, \text{ where } x = (e_{cg} \, \dot{e}_{cg} \, e_\theta \,\dot{e}_\theta)^T.
x˙=Ax+B1δ+B2 rdes, where x=(ecge˙cgeθe˙θ)T.
A = [ 0 1 0 0 0 − ( c f + c r ) m v x c f + c r m ( l r c r − l f c f ) m v x 0 0 1 0 0 l r c r − l f c f I z v x l r c r − l f c f I z − ( c f l f 2 + c r l r 2 ) I z v x 0 ] , B 1 = [ 0 c f m 0 l f c f I z ] , B 2 = [ 0 − l f c r − l r c f m v − v 0 − l f 2 c f + l r 2 c r I z v ] A = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{(c_f + c_r)}{mv_x} & \frac{c_f + c_r}{m} & \frac{(l_r c_r - l_f c_f)}{mv_x} \\ 0 & 0 & 1 & 0 \\ 0& \frac{l_r c_r - l_f c_f}{I_z v_x} & \frac{l_r c_r - l_f c_f}{I_z} & -\frac{(c_f l_f^2 + c_r l_r^2)}{I_z v_x} & 0 \\ \end{bmatrix} , B_1 = \begin{bmatrix} 0 \\ \frac{c_f}{m} \\ 0 \\ \frac{l_f c_f}{I_z} \\ \end{bmatrix} , B_2 = \begin{bmatrix} 0 \\ -\frac{l_f c_r - l_r c_f}{m v} -v\\ 0 \\ -\frac{l_f^2 c_f + l_r^2 c_r}{I_z v} \\ \end{bmatrix} A= 00001−mvx(cf+cr)0Izvxlrcr−lfcf0mcf+cr1Izlrcr−lfcf0mvx(lrcr−lfcf)0−Izvx(cflf2+crlr2)0 ,B1= 0mcf0Izlfcf ,B2= 0−mvlfcr−lrcf−v0−Izvlf2cf+lr2cr
Basic work flow:
- Check the controllability matrix has full rank: [ B 1 , A B 1 , A 2 B 1 , A 3 B 1 ] [B_1, A B_1, A^2 B_1, A^3 B_1] [B1,AB1,A2B1,A3B1].
- Convert the continuous time system to discrete time.
x ( k + 1 ) = A d x ( k ) + B 1 d δ ( k ) + B 2 d r d e s ( k ) x(k+1) = A_dx(k) + B_{1d}\delta(k) + B_{2d}\ r_{des}(k) x(k+1)=Adx(k)+B1dδ(k)+B2d rdes(k) - Use the full state feedback law: δ = − K x = − k 1 e c g − k 2 e ˙ c g − k 3 e θ − k 4 e ˙ θ . \delta = -Kx = -k_1 e_{cg} - k_2 \dot{e}_{cg} - k_3 e_\theta - k_4 \dot{e}_\theta. δ=−Kx=−k1ecg−k2e˙cg−k3eθ−k4e˙θ.
- Add feedforward control to calculate the steering angle theoretically required to drive smoothly along the trajectory without any deviation: δ s = δ f − K x = a t a n ( ρ ∗ ( l r + l f ) ) − k 1 e c g − k 2 e ˙ c g − k 3 e θ − k 4 e ˙ θ . \delta_s = {\delta_f} -Kx = atan(\rho*(l_r + l_f)) -k_1 e_{cg} - k_2 \dot{e}_{cg} - k_3 e_\theta - k_4 \dot{e}_\theta. δs=δf−Kx=atan(ρ∗(lr+lf))−k1ecg−k2e˙cg−k3eθ−k4e˙θ.
Apply LQR in this situation, we have
δ
∗
(
k
)
=
−
K
x
(
k
)
+
δ
f
∗
(
k
)
\delta^*(k) = -Kx(k) + {\delta_f}^*(k)
δ∗(k)=−Kx(k)+δf∗(k)
Where
K
=
(
R
+
B
d
T
P
B
d
)
−
1
B
d
T
P
A
d
.
K = (R + B_d^T P B_d)^{-1} B_d^T P A_d.
K=(R+BdTPBd)−1BdTPAd.
Objective cost function to be minimized by the control is
J
=
∑
k
=
0
∞
x
(
k
)
T
Q
x
(
k
)
+
δ
(
k
)
T
R
δ
(
k
)
J = \sum_{k=0}^{\infty} x(k)^T Q x(k) + \delta(k)^T R \delta(k)
J=k=0∑∞x(k)TQx(k)+δ(k)TRδ(k)
where P satisfies the matrix difference Riccati equation
P
=
A
d
T
P
A
d
−
A
d
T
P
B
d
(
R
+
B
d
T
P
B
d
)
−
1
B
d
T
P
A
d
+
Q
P = A_d^T P A_d - A_d^T P B_d(R + B_d^T P B_d)^{-1} B_d^T P A_d + Q
P=AdTPAd−AdTPBd(R+BdTPBd)−1BdTPAd+Q
(The P matrix provides an estimate of the cost required to reach the target state for a given state. It reflects the performance index in the process of transferring the system state to the target state.)
LQR tuning
Let Q = [ 1 0 0 1 ] Q = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} Q=[1001] , we choose different ( R ):
- When ( R = 10 ):
- Penalty on control effort is large → Less control effort is used → slower response.
- When ( R = 1 ):
- Penalty on control effort is small → More control effort is used → faster response.
- Penalty on control effort is small → More control effort is used → faster response.
Position error response to initial condition
Control effort
Let ( R = 1 ), we choose different ( Q ):
- When
Q
=
[
10
0
0
1
]
Q = \begin{bmatrix} 10 & 0 \\ 0 & 1 \end{bmatrix}
Q=[10001]:
- Penalty on position error is greater than penalty on speed error → prioritizes minimizing position error → may result in a faster response in position tracking.
- When
Q
=
[
1
0
0
1
]
Q = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix}
Q=[1001]:
- Penalty on position error is equal to penalty on speed error → balanced approach → may result in a response that does not prioritize one over the other.
- Penalty on position error is equal to penalty on speed error → balanced approach → may result in a response that does not prioritize one over the other.
Position error response to initial condition
Speed error response to initial condition
代码框架
LqrController
- Properties
- Time step (
ts_
) - Front and rear wheel cornering stiffness (
cf_
,cr_
) - Wheelbase and steer ratio (
wheelbase_
,steer_ratio_
) - Maximum steering angle (
steer_single_direction_max_degree_
) - Vehicle mass and inertia (
mass_
,iz_
) - Front and rear distances from the center of mass (
lf_
,lr_
) - LQR algorithm parameters (
lqr_eps_
,lqr_max_iteration_
)
- Time step (
- Initialization
- Load vehicle and LQR configuration (
LoadControlConf
) - Initialize matrices and control parameters (
Init
)
- Load vehicle and LQR configuration (
- Control Logic
- Update vehicle state and compute control command (
ComputeControlCommand
) - State Update
- Compute lateral errors (
ComputeLateralErrors
) - Update state matrix (
UpdateState
)
- Compute lateral errors (
- Matrix Updates
- Update the state matrix A and discretize the state matrix A (
UpdateMatrix
)
- Update the state matrix A and discretize the state matrix A (
- Control Computation
- Solve LQR problem to obtain control gains (
SolveLQRProblem
) - Compute feedback and feedforward components (
ComputeFeedForward
) - Calculate the final steering angle
- Solve LQR problem to obtain control gains (
- Update vehicle state and compute control command (
- Utilities
- Normalize angles and calculate distances (
NormalizeAngle
,PointDistanceSquare
) - Query the nearest trajectory point (
QueryNearestPointByPosition
)
- Normalize angles and calculate distances (
LoadControlConf()
加载控制器配置,设置车辆动力学参数。如:侧偏刚度,时间步,前后悬质量,前后轴长度,惯性矩,LQR迭代准确性和迭代次数。
Init()
初始化控制器,构建状态空间模型(A, B矩阵)和成本函数(Q, R矩阵)。
ComputeControlCommand(…)
计算并输出控制命令,是控制器的核心逻辑部分。
- 配置A, B矩阵
- 通过UpdateState()更新状态和计算横向误差
UpdateState(const VehicleState &vehicle_state)
根据车辆当前状态更新状态向量,并通过ComputeLateralErrors(…)计算横向误差。
UpdateMatrix(const VehicleState &vehicle_state)
更新状态矩阵A并将状态矩阵A离散化。
- ComputeLateralErrors(…): 计算并更新四个状态量
SolveLQRProblem(matrix_ad_, matrix_bd_, matrix_q_, matrix_r_, lqr_eps_, lqr_max_iteration_, &matrix_k_)
实现LQR算法,计算最优反馈增益矩阵K。
输入参数
- Matrix &A, &B:系统的动态矩阵 ,用于描述车辆的动态行为。
- Matrix &Q, &R:权重矩阵,分别用于量化状态偏差和控制输入的成本。
- double tolerance:算法收敛的容忍度。
- max_num_iteration:算法的最大迭代次数。
- Matrix *ptr_K:指向计算出的反馈增益矩阵K 的指针。
steer_angle_feedback
calculate feedback, steer = -K * state.
steer_angle_feedforward
计算理论上需要的转向角度,以便在没有任何偏差时沿轨迹平稳行驶。
- ComputeFeedForward (const VehicleState &localization, double ref_curvature): 根据给定的 ref_curvature和车辆的轴距来计算前馈转向角,转向角度等于车辆轴距与曲率的乘积的反正切。