车辆横向控制-状态反馈

概述

根据车辆模型-跟踪误差模型，在小滑移角假设下，车辆横向动力学模型的状态空间方程可以写为
$$\dot{x}=Ax+B_1\delta +B_2{\dot{\psi }}_{des}\text{\hspace{1em}(1)}$$

其中，$x=[e_y,{\dot{e}}_y,e_{\psi },{\dot{e}}_{\psi }{]}^T$，$e_y$代表车辆重心与目标曲线的距离，$e_{\psi }$代表车辆与目标跟踪曲线之间的偏航角误差，$\delta$代表前轮转向角，${\dot{\psi }}_{des}$代表期望偏航角速度，该参数由道路曲率和车速决定，矩阵$A$，$B_1$，$B_2$在车辆模型-跟踪误差模型中已经提出。

仿真

使用实际车辆的参数带入上述动力学模型中

物理量	数值	单位
质量($M$)	1573	$kg$
$z$轴转动惯量($I_z$)	2873	$ks/m^2$
前轴到重心距离($l_f$)	1.10	$m$
后轴到重心距离($l_r$)	1.58	$m$
前轮总侧偏刚度($C_{\alpha f}$)	80000	$N/rad$
后轮总侧偏刚度($C_{\alpha r}$)	80000	$N/rad$
$x$轴速度($V_x$)	30	$m/s$
转弯半径($R$)	1000	$m$

开环系统

如下图所示，对于开环系统，矩阵$A$有两个特征值位于坐标原点，所以系统是不稳定的。

闭环系统

研究表明，$(A,B_1)$对是可控的，因此使用状态反馈法
$$\delta =-Kx=-k_1{e}_y-k_2{\dot{e}}_y-k_3{e}_{\psi }-k_4{\dot{e}}_{\psi }\text{\hspace{1em}(2)}$$
闭环矩阵$(A-BK)$的特征值可以放置于任何想要的位置，闭环系统的状态反馈控制如下：
$$\dot{x}=(A-B_{1}K)x+B_2{\dot{\psi }}_{des}\text{\hspace{1em}(3)}$$
使用Python的control库中的place函数去设置闭环系统的特征值

K = ct.place(A,B1,P)

上述函数产生一个反馈矩阵$K$，使得矩阵$(A-B_{1}K)$的特征值位于指定向量$P$的位置。
下面将特征值放置在${\left[\begin{array}{cccc}-5-3j& -5+3j& -7& -10\end{array}\right]}^T$位置，零极点分布如下：

阶跃响应如下：

仿真代码

import numpy as np
import control as ct
import matplotlib.pyplot as plt

# 车辆参数
M   = 1573.0 #(kg) 总质量 mass
I_z = 2873.0 # (kg/m^2) 绕Z轴的转动惯量 
l_f = 1.10  # (m)
l_r = 1.58  # (m)
C_alpha_f = 80000.0 #(N/rad) 前轮总侧偏刚度
C_alpha_r = 80000.0 #(N/rad) 后轮总侧偏刚度
V_x = 30.0 # (m/s)

A = np.array([
        [0.,1.,0.,0.],
        [0.,-2.*(C_alpha_f + C_alpha_r)/(M*V_x),2.*(C_alpha_f + C_alpha_r)/M,-2.*(C_alpha_f*l_f - C_alpha_r*l_r)/(M*V_x)],
        [0.,0.,0.,1.],
        [0.,-2.*(C_alpha_f*l_f - C_alpha_r*l_r)/(I_z*V_x),2.*(C_alpha_f*l_f - C_alpha_r*l_r)/I_z,-2.*(C_alpha_f*l_f**2 + C_alpha_r*l_r**2)/(I_z*V_x)]
        ])

B1 = np.array([
        [0.],
        [2.*C_alpha_f/M],
        [0.],
        [2.*l_f*C_alpha_f/I_z]
        ])

B2 = np.array([[0.],[-2.*(C_alpha_f*l_f - C_alpha_r*l_r)/(M*V_x) - V_x],[0.],[-2.*(C_alpha_f*l_f**2 + C_alpha_r*l_r**2)/(I_z*V_x)]])

C = np.array([[1., 0., 0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,0.,0.,1.]])

D = np.array([[0.],[0.],[0.],[0]])

w,v = np.linalg.eig(A) # 求取数组的特征值
print("特征值：",w)
#print(v)
P = np.array([-5.-3.j,-5.+3.j,-7.,-10.])
K = ct.place(A,B1,P)
print("反馈值K:",K)

sys_init = ct.ss(A,B1,C,D)
ct.pzmap(sys_init)
sys_task = ct.ss(A-B1*K,B2,C,D)
ct.pzmap(sys_task)

t = np.linspace(0, 10, 101)
u = np.zeros(len(t))
u[11:101] = 0.03 #rad/s

f_t,f_yout,f_xout = ct.forced_response(sys_task,t,u)

#plt.close()
plt.figure()
plt.clf()

plt.subplot(3,1,1)
plt.grid()
plt.ylabel("(rad/s)")
plt.plot(f_t,u)
plt.subplot(3,1,2)
plt.grid()
plt.ylabel("e1:(m)")
plt.plot(f_t,f_yout[0])
plt.subplot(3,1,3)
plt.grid()
plt.xlabel("time:(s)")
plt.ylabel("e2:(rad)")
plt.plot(f_t,f_yout[2])

结论

由于$B_2{\dot{\psi }}_{des}$项的存在，，即使矩阵$(A-B_{1}K)$稳定的情况下，跟踪误差也无法达到0。稳定状态下的$e_y$和$e_{\psi }$不为0，主要因为道路曲率${\dot{\psi }}_{des}$不为0。