电磁组 湖北汽车工业学院-电磁二队-技术报告(3)

映车模倾角变化，从而减缓对于车模车轮控制。另一方面也会将车模角速度变化信息滤掉。上述两方面的滤波效果使得车模无法保持平衡。因此对于车模直立控制所需要的倾角信息需要通过另外一种器件获得，那就是角速度传感器-陀螺仪。

（2）角速度传感器-陀螺仪

陀螺仪可以用来测量物体的旋转角速度。竞赛允许选用村田公司出品的ENC-03系列的加速度传感器。它利用了旋转坐标系中的物体会受到科里奥利力的原理，在器件中利用压电陶瓷做成振动单元。当旋转器件时会改变振动频率从而反映出物体旋转的角速度。

由于陀螺仪输出的是车模的角速度，不会受到车体运动的影响，因此该信号中噪声很小。车模的角度又是通过对角速度积分而得，这可进一步平滑信号，从而使得角度信号更加稳定。因此车模控制所需要的角度

和角速度可以使用陀螺仪所得到的信号。

由于从陀螺仪角速度获得角度信息，需要经过积分运算。如果角速度信号存在微小的偏差和漂移，经过积分运算之后，变化形成积累误差。这个误差会随着时间延长逐步增加，最终导致电路饱和，无法形成正确的角度信号。 如何消除这个累积误差呢？

通过上面的加速度传感器获得的角度信息对此进行校正，通过对比积分所得到的角度与重力加速度所得到的角度，使用它们之间的偏差改变陀螺仪的输出，从而积分的角度逐步跟踪到加速度传感器所得到的角度。

2.4 传感器融合与卡尔曼滤波

在此我们引入一个数学工具作为获得修正角度的方法-----卡尔曼滤波器。 卡尔曼滤波器是一个高效的递归滤波器，它可以实现从一系列的噪声测量中，估 计动态系统的状态。广泛应用于包含Radar、计算机视觉在内的等工程应用领域，在控制理论和控制系统工程中也是一个非常重要的课题。连同线性均方规划，卡尔曼滤波器可以用于解决LQG(Linear-quadratic-Gaussian control)问题。卡尔曼滤波器，线性均方归化及线性均方高斯控制器，是大部分控制领域基础难题的主要解决途径。

11

第七届飞思卡尔智能汽车邀请赛技术报告

下面的描述，会涉及一些基本的概念知识，包括概率（Probability） ，随即变量（Random Variable） ，高斯或正态分配（Gaussian Distribution）还有State-space Model等等。但对于卡尔曼滤波器的详细证明，这里不能一一描述。

首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方（Linear Stochastic Difference equation）来描述：

X(k)=A X(k-1)+B U(k)+W(k)

再加上系统的测量值：

Z(k)=H X(k)+V(k)

上两式子中，X(k)是 k 时刻的系统状态，U(k)是 k 时刻对系统的控制量。A和 B是系统参数，对于多模型系统，他们为矩阵。Z(k)是 k 时刻的测量值，H 是测量系统的参数，对于多测量系统，H 为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的 covariance 分别是 Q，R（这里我们假设他们不随系统状态变化而变化）。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的 covariances 来估算系统的最优化输出。

首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是 k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：

X(k|k-1)=A X(k-1|k-1)+B U(k) ???(1)

式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为 0。

到现在为止，我们的系统结果已经更新了，可是，对应于 X(k|k-1)的 covariance 还没更新。我们用 P表示 covariance：

P(k|k-1)=A P(k-1|k-1) A’+Q ???(2) 式(2)中，P(k|k-1)是 X(k|k-1)对应的 covariance，P(k-1|k-1)是 X(k-1|k-1)对应的covariance，A’表示 A的转置矩阵，Q是系统过程的 covariance。式子 1，2 就是卡尔曼滤波器 5 个公式当中的前两个，也就是对系统的预测。

12

现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值 X(k|k)：

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ???(3) 其中 Kg 为卡尔曼增益(Kalman Gain)：

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ??? (4) 到现在为止，我们已经得到了 k 状态下最优的估算值 X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新 k 状态下 X(k|k)的 covariance：

P(k|k)=（I-Kg(k) H）P(k|k-1) ???(5)

其中 I 为 1 的矩阵，对于单模型单测量，I=1。当系统进入 k+1 状态时，P(k|k)就是式子(2)的 P(k-1|k-1)。

这样，算法就可以自回归的运算下去。

卡尔曼滤波器的原理基本描述了，式子 1，2，3，4 和 5 就是他的 5 个基本公式。根据这 5 个公式，可以很容易的实现计算机的程序。

下面，将具体进行传感器融合和算法分析：

以陀螺仪测量的角速度作为预测值的控制量，加速度传感器测量的角度作为观测值。下面程序中angle_m为测量角度，gyro_m为测量角速度，gyro_m*dt为控制量。

以下程序是按卡尔曼滤波的五个公式来编写的。 X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1) P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3) Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4) P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)

对于单输入单输出系统，A、B、H、I不为矩阵且值都为1。

卡尔曼滤波参数的调整：其参数有三个，p0是初始化最优角度估计的协方差（初始化最优角度估计可设为零），它是一个初值。Q是预测值的协方差，R是测量值的协方差。对Q和R的设定只需记住，Q/(Q+R)的值就是卡尔曼增益的收敛值，比如其值为0.2，那么卡尔曼增益会向0.2收敛（对于0.2的含义解释一下，比如预测角度值是5度，角度测量值是10度，那么最优化角度为：5+0.2*（10-5）=6。从这里可以看出，卡尔曼增益越小，说明预

13

第七届飞思卡尔智能汽车邀请赛技术报告

测值越可靠，最优化角度越接近预测值；相反的，卡尔曼增益越大，说明测量值越可靠，最优化角度越接近测量值）。p0/(Q+R)反映收敛的快慢程度，该值设定越小，收敛越快，该值越大，收敛越慢（这里的p0是指初始最优角度值的协方差），因为卡尔曼增益收敛总的来说是很快的，所以该值设定大一点或小一点都没什么关系。

注：以下程序只用于说明算法，存在语法错误，初始的参数也是随意给定的。

x=0; /* 最优角度初值*/

p=1; /* 最优角度对应协方差初值*/ dt=0.02; Q=0.0025； R=0.25；

void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure {

x=x+ gyro_m*dt; //等号右边的x表示上一次最优角度值，等号左边的x表示这一次的角度的预测值。

p=p+Q; //等号右边的p表示上一次最优角度值的协方差，等号左边的p表示这一次的角度 预测值的协方差。

k=p/(p+R); //k值为卡尔曼增益（k值每次计算都不一样，它会越来越趋近于Q/(Q+R)这个收敛值）。

x=x+k*( angle_m-x); //等号左边的x表示根据预测值和测量值计算出来的这一次的最优角度值（从这里可以看出，k越大，等号左边的最优值x与等号右边的测量值angle_m越接近；k越小，等号左边的最优值x与等号右边的预测值x越接近；）

p=(1-k)*p; // 等号左边的p表示这一次最优角度值的协方差 }

从上面的程序可以看出，卡尔曼滤波是一个递推过程，初始的最优角度值可设为x=0，初始最优角度值的协方差p一定不能设为零，dt是采样周期，Q 与R

14

可共同决定卡尔曼增益收敛的大小。

2.5车模速度控制

对于直立车模速度的控制相对于普通车模的速度控制则比较复杂。由于在速度控制过程中需要始终保持车模的平衡，因此车模速度控制不能够直接通过改变电机转速来实现。下面先分析一下引起车模速度变化的原因。

假设车模在上面直立控制调节下已经能够保持平衡了，但是由于安装误差，传感器实际测量的角度与车模角度有偏差，因此车模实际不是保持与地面垂直，而是存在一个倾角。在重力的作用下，车模就会朝倾斜的方向加速前进。图2- 2显示了这个关系。控制速度只要通过控制车模的倾角就可以实现了。具体实现需要解决三个问题：

（1）如何测量车模速度？

（2）如何通过车模直立控制实现车模倾角的改变？ （3）如何根据速度误差控制车模倾角？

第一个问题可以通过安装在电机输出轴上的光码盘来测量得到车模的车轮速度。利用控制单片机的计数器测量在固定时间间隔内速度脉冲信号的个数可以反映电机的转速。

第二个问题可以通过角度控制给定值来解决。给定车模直立控制的设定值，在角度控制调节下，车模将会自动维持在一个角度。通过前面车模直立控制算法可以知道，车模倾角最终是跟踪重力加速度Z轴的角度。因此车模的倾角给定值与重力加速度Z轴角度相减，便可以最终决定车模的倾角。

第三个问题分析起来相对比较困难，远比直观进行速度负反馈分析复杂。首先对一个简单例子进行分析。假设车模开始保持静止，然后增加给定速度，为此需要车模往前倾斜以便获得加速度。在车模直立控制下，为了能够有一个往前的倾斜角度，车轮需要往后运动，这样会引起车轮速度下降（因为车轮往负方向运动了）。由于负反馈，使得车模往前倾角需要更大。如此循环，车模很快就会倾倒。原本利用负反馈进行速度控制反而成了“正”反馈.

为什么负反馈控制在这儿失灵了呢？原来在直立控制下的车模速度与车模倾角之间传递函数具有非最小相位特性（在此省略了分析），在反馈控制下容易

15

百度搜索“77cn”或“免费范文网”即可找到本站免费阅读全部范文。收藏本站方便下次阅读，免费范文网，提供经典小说综合文库电磁组 湖北汽车工业学院-电磁二队-技术报告(3)在线全文阅读。