CN104391445A - 基于观测器的车队协同自主控制方法 - Google Patents

Abstract

本发明公开了一种基于观测器的车队协同自主控制方法，包括以下步骤：步骤1：初始化滤波器参数；步骤2：读取传感器的测量信息；步骤3：运用渐消记忆的卡尔曼滤波算法进行状态估计；步骤4：邻居车辆之间进行信息交换；步骤5：输出控制信号，发出控制指令。本发明通过渐消记忆的卡尔曼滤波算法，利用可获得的测量信号，得到间隔误差及其导数的估计值，解决了间隔误差不可测的问题和滤波发散的问题，并利用无线网络进行通信，邻居车辆之间实现信息交换和共享，根据当前车辆及邻居车辆的状态的估计值，得到控制信号，发出控制指令，实现了协同控制。

Description

基于观测器的车队协同自主控制方法

技术领域

本发明涉及一种智能车的协同控制算法技术，特别涉及一种基于观测器的车队协同自主控制方法，该自主控制方法是一种基于车载雷达、无线通信技术和卡尔曼次优滤波的状态反馈控制方法。 

背景技术

车队协同控制是智能交通系统的重要组成部分，协同控制的实现，可以有效的缓解交通拥堵，提高道路通行效率，减少交通事故，具有重要的理论和实用价值。传感检测技术、无线通信技术、计算机技术和车辆控制技术的发展为车队协同控制的实现提供了坚实的基础。本发明中，每辆车上都装有车载雷达、速度传感器、加速度传感器、无线收发模块和计算机。车载雷达能够测量与前方车辆的距离和相对速度；速度、加速度传感器用来测量当前汽车的速度和加速度；无线收发模块用于信息交换，通过计算机，运用卡尔曼滤波算法进行状态估计并输出控制量，发出控制指令，实现车队协同自主控制。 

发明内容

本发明的目的在于克服现有技术的缺点与不足，提供一种基于观测器的车队协同自主控制方法，该协同自主控制方法是分布式的，车队中每辆车的地位是对等的，独立进行滤波运算，不需要信息融合中心，从而降低了对硬件的要求。同时，该算法是可扩展的，适合其他自主车辆加入或者退出车队。无线通信过程中，只有状态估计值在信道中传播，不需要复杂的通信协议来传递信息，便于物理实现。 

本发明的目的通过下述技术方案实现：一种基于观测器的车队协同自主控制方法，包括以下步骤： 

步骤1：初始化滤波器参数； 

步骤2：读取传感器测量信息； 

步骤3：运用渐消记忆的卡尔曼滤波算法进行状态估计； 

步骤4：邻居车辆之间进行信息交换； 

步骤5：输出控制信号，发出控制指令。 

所述步骤1中，所述的滤波器参数包括当前车辆状态估计值、误差协方差矩阵、邻居车辆的状态估计值，所述的状态估计值服从高斯分布。状态变量为间隔误差、速度误差和加速度误差。 

所述步骤2中，所述传感器包括车载雷达、速度传感器和加速度传感器，车载计算机车载雷达获得的与前方车辆的距离和相对速度信号，速度传感器、加速度传感器获得的当前车辆的速度信号和加速度信号。车队中的第一辆车前方没有其他车辆，车载雷达没有测量信号。 

所述步骤3包括以下步骤： 

S31、第一辆车没有雷达测量信号，不进行滤波运算，接受邻居车辆的状态估计值。 

S32、车队中的其他车辆运用渐消记忆的卡尔曼滤波算法进行状态估计，计算间隔误差及速度误差的估计值。 

所述步骤S32中，由于原系统不完全能观，使用基本的卡尔曼滤波算法时存在状态发散的问题，使用渐消记忆的滤波算法，能够有效减小历史测量数据的影响，增大当前测量值的校正作用，使得状态估计值收敛到其真实值。 

所述步骤4包括以下步骤： 

S41、邻居车辆为当前车辆前方的第一辆车和后方的第一辆车。 

S42、在通信过程中，邻居车辆只交换状态估计信息，减小通信量，降低对通信网络的要求。其中状态估计值包括间隔误差、速度误差和加速度误差。 

S43、当前车辆监听信道，如果信道空闲，广播当前车辆的状态估计值，持续一段时间后，停止广播，接收邻居车辆的广播信息并进行存储；如果信道繁忙，接收其他车辆的广播信息并进行存储。 

所述步骤5中，当前车辆根据自身的估计值和邻居车辆的估计值，按照给定的控制算法，输出控制信号，发出控制指令。 

本发明的工作原理：本发明基于负反馈控制原理，具体控制规律是基于间隔误差的比例微分控制。在控制规律的实现过程中，由于间隔误差、速度误差等不能直接测量得到，因此使用估计值来代替真实值计算控制量。然而实际的传感器不可避免地会引入观测噪声，本发明中采用卡尔曼滤波算法进行滤波，减小观测噪声的影响，提高估计精度。然而，由于实际的车队系统的状态变量 不完全能观测，基本的卡尔曼滤波算法存在滤波发散的问题。渐消记忆的卡尔曼滤波算法通过引入加权因子，减小历史观测数据对当前估计值的影响，增大当前观测信息的校正作用，能够有效的解决滤波发散问题。因此，本发明中采用渐消记忆的卡尔曼滤波算法估计间隔误差及其导数，利用估计值得到控制信号，发出控制指令。本发明主要介绍了一种基于变间隔的协同控制方法，即想要的车辆间隔随着车速加快而线性增加。车辆间固定间距情形属于本发明的一种特殊情况，本发明中，在设计每辆车的控制规律中，同时考虑了前方车辆和后面车辆对当前车辆的影响。只考虑前方车辆或后面车辆对当前车辆有影响的属于本发明的特例，也在本发明的保护范围之内。通过渐消记忆的卡尔曼滤波算法，利用可获得的测量信号，得到间隔误差及其导数的估计值，解决了间隔误差不可测的问题和滤波发散的问题。利用无线网络进行通信，邻居车辆之间实现信息交换和共享，根据当前车辆及邻居车辆的状态的估计值，得到控制信号，发出控制指令，实现协同控制。 

本发明相对于现有技术具有如下的优点及效果： 

1、本发明中，采用了协同控制技术，邻居车辆之间通过信息交换，能够增强车队的安全性和道路通行效率。 

2、本发明中，提出的控制算法为基于间隔误差的比例微分控制，同时考虑了前方车辆和后面车辆的影响，控制效果更好。 

3、本发明中，采用了渐消记忆的卡尔曼滤波算法，估计间隔误差、速度误差和加速度误差，有效解决了间隔误差不可测量和滤波发散的问题。 

4、本发明中，采用的是分布式的滤波算法，便于扩展，容错能力强，计算量小，对硬件设施要求低。 

附图说明

图1是本发明中间隔误差的仿真曲线，图中的实线、虚线、点划线分别表示第二个、第三个和第四个间隔误差。 

图2是本发明中汽车速度的仿真曲线。 

图3是本发明中汽车加速度的仿真曲线。 

图4是本发明中整个车队系统的示意图。 

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。 

实施例 

对于车队系统，设第i辆车的位置为q_i，速度为p_i，加速度为a_i，i为汽车在车队中编号，i∈{1,…,n}，n为车队中汽车的数量，q_i,p_i,a_i∈R¹。第i辆车和第i-1辆车之间的安全距离定义为： 

δ_i＝d₀+λp_i,i＝2,…,n，    (1) 

其中，λ为时间常数，d₀为最小安全距离，定义间隔误差为： 

e_i＝q_i-1-q_i-δ_i,i＝2,…,n，    (2) 

汽车的动态方程如下： 

$\left\{\begin{array}{c}{\stackrel{\·}{q}}_i=p_i\\ {\stackrel{\·}{p}}_i=a_i\\ {\stackrel{\·}{a}}_i=-\mathrm{\α}{a}_i+u_i\end{array}\right.,---\left(3\right)$

其中，α为常数，控制规律的设计如下： 

$u_i=\left\{\begin{array}{cc}-k_p{e}_{i+1}-k_v{\stackrel{\·}{e}}_{i+1},& i=1\\ k_p{e}_i+k_v{\stackrel{\·}{e}}_i-k_p{e}_{i+1}-k_v{\stackrel{\·}{e}}_{i+1},& i=2,...,n-1,\\ k_p{e}_i+k_v{\stackrel{\·}{e}}_i,& i=n\end{array}\right.---\left(4\right)$

其中，为e_i对时间的一阶导数，选取状态变量 $x_i={\left[\begin{array}{ccc}{e}_i& {\stackrel{\·}{e}}_i& {\stackrel{\·\·}{e}}_i\end{array}\right]}^T,$ 系统的状态方程可以写成： 

$\stackrel{\·}{x}=\mathrm{Ax}+\mathrm{BFx},---\left(5\right)$

其中： 

$A_i=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& -\mathrm{\α}\end{array}\right],B_i=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right],$ F_ii＝[-2k_p -2k_v-λk_p λk_v]，F_i(i+1)＝[k_p k_v+λk_p λk_v]，F_i(i-1)＝[k_p k_v 0]， $x={\left[\begin{array}{cccc}{x}_2^T& x_3^T& ...& x_n^T\end{array}\right]}^T.$

状态变量x不能直接获得，采用卡尔曼滤波算法进行状态估计，通过估计值获得控制量，即 为x的估计值。测量信号由车上的传感器测量得到，测量信号包括当前车与前面车的距离、相对速度、当前车的速度、加速度，观测向量定义为 $z_i\stackrel{\mathrm{\Δ}}{=}{\left[\begin{array}{cccc}{q}_{i-1}-q_i& p_{i-1}-p_i& p_i& a_i\end{array}\right]}^T,$ i＝2,…,n，由此可以得到系统的观测方程： 

z＝Hx+Y，    (6) 

其中： 

$Y=\left[\begin{array}{c}{Y}_2\\ .\\ .\\ .\\ Y_n\end{array}\right],H_i=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right],Y_i=\left[\begin{array}{c}{\mathrm{\λp}}_i+d_0\\ {\mathrm{\λa}}_i\\ p_i\\ a_i\end{array}\right],$

考虑测量噪声时，观测方程为： 

z＝Hx+Y+V，    (7) 

其中，V为均值为零，方差为R的白噪声向量H∈R^{4(n-1)×3(n-1)}，对系统的能控性和能观性进行分析，由方程(5)和(6)描述的系统是一个完全能控的系统，但该系统不完全能观，不满足卡尔曼滤波的稳定条件，本发明中采用渐消记忆的方法克服滤波发散的问题，其主旨思想是增大滤波增益，减小历史数据的影响，增大当前观测值的对估计值的校正作用。 

下面对本发明的内容作进一步的阐述： 

一种基于车载雷达和无线通信技术的车队协同控制方法包括以下步骤： 

第一步：初始化每辆车的运动状态，其中包括位置、速度、加速度。由此 得到状态变量的初始估计值和协方差矩阵的初始值P_0|0。 

第二步：读取传感器的测量信息，对于车队中第2到第n辆车，在k时刻的测量信息： 

z_i(k)＝H_ix_i(k)+Y_i+v_i(k)，    (8) 

第三步：运用卡尔曼滤波算法进行状态进行估计，得到k时刻的状态估计值 和误差协方差矩阵P(k|k)。 

1)对于第i辆车，离散化后状态方程为： 

x_i(k+1)＝G_ix_i(k)+D_ir_i(k),i＝2,…,n，    (9) 

其中， $G_i=e^{(A_i+B_i{F}_{\mathrm{ii}})T},D_i={\∫}_0^T{e}^{(A_i+B_i{F}_{\mathrm{ii}})t}\mathrm{dt}\×B,$ T为采样周期，本发明中，G_i＝G_j，D_i＝D_j，i≠j且i,j∈{2,…,n}，分别用G和D表示： 

$r_i\left(k\right)=\left\{\begin{array}{cc}{F}_{i(i+1)}{x}_{i+1}\left(k\right),& i=2\\ F_{i(i-1)}{x}_{i-1}\left(k\right)+F_{i(i+1)}{x}_{i+1}\left(k\right),& i=3,...,n-1,\\ F_{i(i-1)}{x}_{i-1}\left(k\right),& i=n\end{array}\right.---\left(10\right)$

2)由预测方程可以得到： 

${\hat{x}}_i\left(k\right|k-1)=G{\hat{x}}_i(k-1|k-1)+D{\hat{r}}_i(k-1|k-1),---\left(11\right)$

预测误差方差矩阵为： 

P(k|k-1)＝GP(k-1|k-1)G^T，    (12) 

增益矩阵： 

$K\left(k\right)=P\left(k\right|k-1)H_i^T{[H_{i}P\left(k\right|k-1)H_i^T+R]}^{-1},---\left(13\right)$

滤波方程： 

${\hat{x}}_i\left(k\right|k)={\hat{x}}_i\left(k\right|k-1)+K\left(k\right)[z\left(k\right)-Y_i\left(k\right)-H{\hat{x}}_i\left(k\right|k-1)],---\left(14\right)$

本发明中采用渐消记忆卡尔曼滤波，权重为w，滤波误差方差矩阵： 

P(k|k)＝[I-K(k)H]P(k|k-1)[I-K(k)H]^Tw+K(k)RK(k)，    (15) 

第四步：通过无线进行信息交换，当前车辆将状态估计值通过无线网络发送给邻居车辆，接受邻居车辆的发送的状态估计值并进行存储。 

第五步：对当前车辆施加控制量，依据公式(4)，可以计算得到当前车的控制量的大小，即： 

$u_i\left(k\right)=\left\{\begin{array}{cc}-\left[\begin{array}{ccc}{k}_p& k_v& 0\end{array}\right]{\hat{x}}_{i+1}^T\left(k\right|k),& i=1,\\ \left[\begin{array}{ccc}{k}_p& k_v& 0\end{array}\right]{\hat{x}}_i^T\left(k\right|k)-\left[\begin{array}{ccc}{k}_p& k_v& 0\end{array}\right]{\hat{x}}_{i+1}^T\left(k\right|k),& i=2,...,n-1,\\ \left[\begin{array}{ccc}{k}_p& k_v& 0\end{array}\right]{\hat{x}}_i^T\left(k\right|k),& i=n,\end{array}\right.---\left(16\right)$

为了说明上述方法的有效性，采用以下方法进行验证： 

在直线道路上依次随机分布4辆车，位置是均值分别为100,75,50,25，方差为1的随机数，速度是均值为10，方差为1的随机数，加速度为0，控制器参数k_p＝1，k_v＝4，α＝4，λ＝2，采样周期T＝0.02s，加权因子w＝1.75， 

$G=\left[\begin{array}{ccc}1& 0.05& 0\\ 0& 1& 0.05\\ -0.1& -0.5& 0.4\end{array}\right],D=\left[\begin{array}{c}0\\ 0\\ 0.05\end{array}\right],P\left(0\right|0)=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],R=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right].$

间隔误差仿真结果如图1所示，随着时间的变化，间隔误差趋向于0，说明了被提出的控制算法的有效性。如图2和图3所示，分别展示了速度和加速度随时间的变化曲线，从图2和图3中可以看出，车队中所有车的速度最终趋向一致，加速趋向于零，由于存在观测噪声，加速度在0附近波动。 

整个车队系统如图4所示，整个车队系统中的所有汽车3排成一列纵队，以车队的形式整体向前行驶，车载雷达2测量汽车3与前方车辆的距离和相对速度，然后将车载雷达2测得的距离和相对速度信息通过无线网络1发送给邻居汽车。汽车使用卡尔曼滤波算法进行状态估计，利用自身传感器获得的信息和通过无线网络1接收到的信息对估计值进行校正，得到状态变量的最优估计值，根据给出的控制算法输出控制量。 

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。 

Claims (10)

1.基于观测器的车队协同自主控制方法，其特征在于，包括以下步骤：

步骤1：初始化滤波器参数；

步骤2：读取传感器的测量信息；

步骤3：运用渐消记忆的卡尔曼滤波算法进行状态估计；

步骤4：邻居车辆之间进行信息交换；

步骤5：输出控制信号，发出控制指令。

2.根据权利要求1所述的基于观测器的车队协同自主控制方法，其特征在于，所述步骤1中，所述的滤波器参数包括当前车辆状态变量、误差协方差矩阵和邻居车辆的状态变量。

3.根据权利要求2所述的基于观测器的车队协同自主控制方法，其特征在于，所述状态变量包括间隔误差、速度误差和加速度误差，所述的状态变量的初始值服从高斯分布。

4.根据权利要求1所述的基于观测器的车队协同自主控制方法，其特征在于，在步骤2中，所述传感器包括车载雷达，速度传感器和加速度传感器；所述车载雷达装在车头，能够获得当前车与前面车辆的距离以及当前车与前面车之间的相对速度；所述速度传感器和加速度传感器安装在汽车内部，用于获得当前汽车的速度值和加速度值，并得到观测方程：

z_i(k)＝H_ix_i(k)+Y_i+v_i(k)，

其中，z_i为当前汽车的观测值，x_i为当前汽车的状态值，Y_i为常数，v_i为观测噪声，H_i为观测矩阵。

5.根据权利要求1所述的基于观测器的车队协同自主控制方法，其特征在于，在步骤3中，车队中除第一辆以外的汽车均运用所述的卡尔曼滤波算法进行状态估计。

6.根据权利要求1所述的基于观测器的车队协同自主控制方法，其特征在于，在步骤3中，所述卡尔曼滤波算法进行状态估计后，得到状态变量的估计值，滤波算法包括预测滤波算法和滤波算法，所述滤波算法的预测方程为：

${\hat{x}}_i\left(k\right|k-1)=G{\hat{x}}_i(k-1|k-1)+D{\hat{r}}_i(k-1|k-1),$

P(k|k-1)＝GP(k-1|k-1)G^T，

其中，为当前汽车的状态变量在k时刻的预测值，为k-1时刻状态变量的估计值，G为汽车的系统矩阵，D为汽车的输入矩阵，P(k|k-1)为k时刻协方差矩阵的预测值，为外部输入；滤波方程：

$K\left(k\right)=P\left(k\right|k-1)H_i^T{[H_{i}P\left(k\right|k-1)H_i^T+R]}^{-1},$

${\hat{x}}_i\left(k\right|k)={\hat{x}}_i\left(k\right|k-1)+K\left(k\right)[z\left(k\right)-Y_i\left(k\right)-H{\hat{x}}_i\left(k\right|k-1)],$

P(k|k)＝[I-K(k)H]P(k|k-1)[I-K(k)H]^Tw+K(k)RK(k)，

其中，K为滤波增益矩阵，P(k|k)为k时刻的协方差矩阵，为k时刻状态变量的估计值，w为加权因子，R为观测噪声协方差矩阵。

7.根据权利要求1所述的基于观测器的车队协同自主控制方法，在步骤4中，所述邻居车辆包括当前车辆前方的第一辆车和后方的第一辆车。

8.根据权利要求1所述的基于观测器的车队协同自主控制方法，其特征在于，在步骤4中，所述邻居车辆只交换估计信息，减小通信数据量，降低对通信网络的要求；估计信息包括间隔误差，速度误差和加速度误差。

9.根据权利要求1所述的基于车载雷达和无线通信技术的车队协同控制方法，其特征在于，在步骤4中，在通信过程中，当前车辆监听信道，如果信道空闲，当前车辆广播自身的状态变量估计值，持续一段时间后，停止广播，接收邻居车辆的广播信息并进行存储；如果信道繁忙，接收邻居车辆的广播信息并进行存储。

10.根据权利要求1所述的基于车载雷达和无线通信技术的车队协同控制方法，其特征在于，所述步骤5中，当前车辆根据自身的估计值和邻居车辆的估计值，按照如下的控制算法：

$u_i\left(k\right)=\left\{\begin{array}{cc}-\left[\begin{array}{ccc}{k}_p& k_v& 0\end{array}\right]{\hat{x}}_{i+1}^T\left(k\right|k),& i=1,\\ \left[\begin{array}{ccc}{k}_p& k_v& 0\end{array}\right]{\hat{x}}_i^T\left(k\right|k)-\left[\begin{array}{ccc}{k}_p& k_v& 0\end{array}\right]{\hat{x}}_{i+1}^T\left(k\right|k),& i=2,...,n-1,\\ \left[\begin{array}{ccc}{k}_p& k_v& 0\end{array}\right]{\hat{x}}_i^T\left(k\right|k),& i=n,\end{array}\right.$

输出控制信号，发出控制指令。