Vehicle Lateral Optimal Control【Autonomous Vehicle Planning and Control】

代码实现参考： link

Linear Quadratic Regulator (LQR)

A system is described by the standard linear state space model:

$$\begin{gathered} \dot{x}=Ax+Bu \\ y=Cx \end{gathered}$$

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=\frac{1}{2}{\int }_0^{\infty }(x^{T}Qx+u^{T}Ru)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=\left[\begin{array}{cccc}{q}_1& 0& \dots & 0\\ 0& q_2& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \dots & q_n\end{array}\right]$$

And ( R ) is:

$$R=\left[\begin{array}{cccc}{r}_1& 0& \dots & 0\\ 0& r_2& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \dots & r_m\end{array}\right]$$

Note from $$x^{T}Qx=q_1{x}_1^2+q_2{x}_2^2+\dots +q_n{x}_n^2$$, where ( Q ) is defined as a matrix with diagonal elements ( q_i ). Where $$q_i\ge 0$$, for ( i = 1, 2, … , n ) and $$r_i>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}Ru=r_1{u}_1^2+r_2{u}_2^2+\dots +r_m{u}_m^2$$.

LQR General Solution (Raccati Equation)

For a LQR problem defined as

System:
$$\begin{gathered} \dot{x}=Ax+Bu \\ y=Cx \end{gathered}$$

State feedback:
$$u=-Kx$$

The closed loop system is:
$$\dot{x}=(A-BK)x=A_{cl}x.$$

Cost function:
$$J=\frac{1}{2}{\int }_0^{\infty }(x^{T}Qx+u^{T}Ru)dt$$

Control law:
$$u=-Kx=-R^{-1}{B}^{T}Px,$$
will bring ( x(\infty) ) to 0 and ( u(\infty) ) to 0

where
$$A^{T}P+PA+Q=PB{R}^{-1}{B}^{T}P$$

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={\left(\frac{d^{2}y}{d{t}^2}\right)}_{\text{inertial}}={\dot{v}}_y+v_x\dot{\psi }$$

$$F_{yf}^{\prime }+F_{yr}^{\prime }=m{a}_y=m({\dot{v}}_y+v_x\ddot{\psi })$$

$$l_f{F}_{yf}^{\prime }-l_r{F}_{yr}^{\prime }=I_z\dot{\psi }$$

after considering the steering angle δ .

$$(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\dot{\psi }=I_{z}r$$

Front and Rear Tire Forces:

$$F_{yf}=c_f{\alpha }_f=c_f(\delta -{\theta }_{vf})$$

$$F_{yr}=c_r{\alpha }_r=c_r(-{\theta }_{vr})$$

$${\theta }_{vf}={\tan }^{-1}\left(\frac{v_{yf}}{v_{xf}}\right)={\tan }^{-1}\left(\frac{v_y+l_{f}r}{v_x}\right)$$

$${\theta }_{vr}={\tan }^{-1}\left(\frac{v_{yr}}{v_{xr}}\right)={\tan }^{-1}\left(\frac{v_y-l_{r}r}{v_x}\right)$$

Lateral and Yaw Dynamics:

$${\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_{x}r$$

$$\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}$$

after small angle assumptions and re-group by variables:
$$\cos (\delta )\approx 1$$

$$\sin (\delta )\approx 0$$

$${\tan }^{-1}(\delta )\approx \delta$$

$${\dot{v}}_y=-\frac{(c_f+c_r)}{m{v}_x}{v}_y+\left[\frac{(l_r{c}_r-l_f{c}_f)}{m{v}_x}\right]v_{x}r+\frac{c_f}{m}\delta$$

$$\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$$

Dynamic Bicycle Model

#### Linearized Dynamic Model of Lateral Vehicle Motion

If we use state
$$\mathbf{X}=\left[\begin{array}{c}y\\ v_y\\ \psi \\ \dot{\psi }\end{array}\right]andinput\delta ,$$
rewrite in state space model:
$$\dot{\mathbf{X}}=\mathbf{AX}+\mathbf{B}\delta ,$$
it is

$$\frac{d}{dt}\left[\begin{array}{c}y\\ v_y\\ \psi \\ \dot{\psi }\end{array}\right]=\left[\begin{array}{cccc}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{array}\right]\left[\begin{array}{c}y\\ v_y\\ \psi \\ \dot{\psi }\end{array}\right]+\left[\begin{array}{c}0\\ \frac{c_f}{m}\\ 0\\ \frac{l_f{c}_f}{I_z}\end{array}\right]\delta$$

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={\left[\begin{array}{cccc}{e}_{cg}& \dot{e_{cg}}& e_{\theta }& \dot{e_{\theta }}\end{array}\right]}^T$$

as our system state and $u=\delta$.

• $e_{cg}$: Orthogonal distance of the C.G. to the nearest path waypoint;
• $\dot{e_{cg}}$: Relative speed between vehicle C.G and path;
• $e_{\theta }$: Heading/Yaw difference between vehicle and path, $e_{\theta }=\theta -{\theta }_p(s)$
• $\dot{e_{\theta }}$: Relative yaw rate between vehicle C.G and path, $e_{\theta }=r-r(s)$ where $r(s)=\dot{\theta (s)}$ is the yaw rate derived from the path.

Dynamic Bicycle Model in path coordinates

With the constant longitudinal velocity assumption,

$$\begin{gathered} {\dot{e}}_{cg}=v_y+v_x\tan (\theta -{\beta }_p(s)) \\ =v_y+v_x\tan (e_{\theta }) \end{gathered}$$

Thus, the acceleration of C.G. is:
$$\begin{gathered} {\dot{e}}_{cg}=({\dot{v}}_y+v_x{r}^{\prime })-{\dot{v}}_y(s) \\ ={\dot{v}}_y+v_x(r-r(s)) \\ ={\dot{v}}_y+v_x{\dot{e}}_{\theta }. \end{gathered}$$

Convert lateral dynamic to error dynamics:
$$\begin{gathered} 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) \end{gathered}$$

Now, we have the linear lateral dynamic model. Rewrite it as:
$$\dot{x}=Ax+B_1\delta +B_2{r}_{des},\text{ where }x=(e_{cg}{\dot{e}}_{cg}{e}_{\theta }{\dot{e}}_{\theta }{)}^T.$$

$$A=\left[\begin{array}{ccccc}0& 1& 0& 0& \\ 0& -\frac{(c_f+c_r)}{m{v}_x}& \frac{c_f+c_r}{m}& \frac{(l_r{c}_r-l_f{c}_f)}{m{v}_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{array}\right],B_1=\left[\begin{array}{c}0\\ \frac{c_f}{m}\\ 0\\ \frac{l_f{c}_f}{I_z}\end{array}\right],B_2=\left[\begin{array}{c}0\\ -\frac{l_f{c}_r-l_r{c}_f}{mv}-v\\ 0\\ -\frac{l_f^2{c}_f+l_r^2{c}_r}{I_{z}v}\end{array}\right]$$

Basic work flow:

• Check the controllability matrix has full rank: $[B_1,A{B}_1,A^2{B}_1,A^3{B}_1]$.
• Convert the continuous time system to discrete time.
  $$x(k+1)=A_{d}x(k)+B_{1d}\delta (k)+B_{2d}{r}_{des}(k)$$
• Use the full state feedback law: $$\delta =-Kx=-k_1{e}_{cg}-k_2{\dot{e}}_{cg}-k_3{e}_{\theta }-k_4{\dot{e}}_{\theta }.$$
• Add feedforward control to calculate the steering angle theoretically required to drive smoothly along the trajectory without any deviation: $${\delta }_s={\delta }_f-Kx=atan(\rho \ast (l_r+l_f))-k_1{e}_{cg}-k_2{\dot{e}}_{cg}-k_3{e}_{\theta }-k_4{\dot{e}}_{\theta }.$$

Apply LQR in this situation, we have
$${\delta }^{\ast }(k)=-Kx(k)+{{\delta }_f}^{\ast }(k)$$
Where $$K=(R+B_d^{T}P{B}_d{)}^{-1}{B}_d^{T}P{A}_d.$$

Objective cost function to be minimized by the control is
$$J=\sum _{k=0}^{\infty }x(k{)}^{T}Qx(k)+\delta (k{)}^{T}R\delta (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$$

(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=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$$ , 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.
    

Position error response to initial condition

Control effort

Let ( R = 1 ), we choose different ( Q ):

• When $$Q=\left[\begin{array}{cc}10& 0\\ 0& 1\end{array}\right]$$:
  • 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=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$$:
  • 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_)
• Initialization
  • Load vehicle and LQR configuration (LoadControlConf)
  • Initialize matrices and control parameters (Init)
• Control Logic
  • Update vehicle state and compute control command (ComputeControlCommand)
  • State Update
    • Compute lateral errors (ComputeLateralErrors)
    • Update state matrix (UpdateState)
  • Matrix Updates
    • Update the state matrix A and discretize the state matrix A (UpdateMatrix)
  • Control Computation
    • Solve LQR problem to obtain control gains (SolveLQRProblem)
    • Compute feedback and feedforward components (ComputeFeedForward)
    • Calculate the final steering angle
• Utilities
  • Normalize angles and calculate distances (NormalizeAngle, PointDistanceSquare)
  • Query the nearest trajectory point (QueryNearestPointByPosition)

LoadControlConf()

加载控制器配置，设置车辆动力学参数。如：侧偏刚度，时间步，前后悬质量，前后轴长度，惯性矩，LQR迭代准确性和迭代次数。

Init()

初始化控制器，构建状态空间模型（A, B矩阵）和成本函数（Q, R矩阵）。

ComputeControlCommand(…)

计算并输出控制命令，是控制器的核心逻辑部分。

1. 配置A, B矩阵
2. 通过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和车辆的轴距来计算前馈转向角，转向角度等于车辆轴距与曲率的乘积的反正切。