（最终版）(1) - 图文(2)

第3章 双轮平衡小车系统的原理

3.1 小车直立平衡物理分析与数学建模

车模平衡控制是通过负反馈来实现的，与保持木棒直立比较则相对简单。因为车模有两个轮子着地，车体只会在轮子滚动的方向上发生倾斜。控制轮子转动，抵消在一个维度上倾斜的趋势便可以保持车体平衡了。

图3-1 小车平衡模型

3.1.1 小车直立平衡物理分析

如何让小车平衡在直立位置呢？我们来分析一下他的受力情况。

图3-2 小车受力分析

如图3-2，为了要达到平衡直立的目的，只有两个办法：一个是改变重力的方向；另一个是增加额外的受力，使得恢复力与位移方向相反才行。显然能够做到的只有第二种方法。

控制倒立摆底部车轮，使得它作加速运动[2]。此时倒立摆所受到的回复力为：

F=mgsin?-macos?≈mg?-mk1?1 (3-1)

公式（3-1）中，由于?很小，所以进行了线性化。为了使得倒立摆能够尽快地在垂直位置稳定下来，还需要增加阻尼力。因此式（3-1）可变为公式（3-2）。

F=mg?-mk1 ?1-mk2 ?? (3-2)

因此，可得控制车轮加速度的控制算法：

6

a=k1?1+k2?? (3-3)

公式(3-3)中，?1为车模倾角；??为角速度；k1、k2均为比例系数；两项相加后作为车轮加速度的控制量。只要保证在k1>g, k2>0条件下，k1可以使得车模像单摆一样维持在直立状态。这两个系数的作用如图3-3所示。

图3-3 比例系数图

3.1.2 小车直立平衡数学建模

假设倒立车模简化成高度为L，质量为m的简单倒立摆，它放置在可以左右移动的车轮上。假设外力干扰引起车模产生角加速度x (t)。沿着垂直于车模地盘方向进行受力分析，可以得到车模倾角与车轮运动加速度以及外力干扰加速度a (t) 与x (t)之间的运动方程[3]。如图3-4所示：

图3-4 车模简化图

对应车模静止时，系统输入输出的传递函数为：

H(s)=?(s)=X(s) (3-4) g(s2-)L7

1此时系统具有两个极点s=±g。一个极点位于s平面的右半平面，因此车模不L稳定。对函数引入比例和微分。此时，系统传递函数为：

H(s)=?(s) =X(s) (3-5)

k(k-g)s2+2 s+1LL1此时两个系统极点位于：

-k2±k22-4L(k1-g)s= (3-6) 2L系统稳定需要两个极点都位于s平面的左半平面。要满足这一点，需要k1>g, k2>0，直立车模可以稳定。这与前面物理分析所得出的结论是一致的。

在角度反馈控制中，与角度成比例的控制量称为比例控制；与角速度成比例的控制量称为微分控制（角速度是角度的微分）。因此上面系数分别称为比例和微分控制参数。其中微分参数相当于阻尼力，可以有效抑制车模震荡。通过微分抑制控制震荡的思想在后面的速度和方向控制中也同样适用[4]。

总结控制车模直立稳定的条件如下：

（1）能够精确测量车模倾角?1的大小和角速度??的大小； （2）可以控制车轮的加速度。 3.2 角度、角速度及车速的数据采集与处理

两轮车系统用到的惯性传感器有加速度计和陀螺仪，另外还要对电机进行测速，他们分别用于检测车身倾斜角、倾斜角速度和车体实时速度。这三种信号主要反应了两轮车的位姿和运动状态。本节主要讨论如何对来自惯性传感器的数据进行融合处理，以得到最优的车体姿态信息。

3.2.1 角度数据处理

加速度计可以测量动态和静态的线性加速度，静态加速度的一个典型例子就是重力加速度，用加速度计来直接测量物体静态重力加速度可以确定倾斜角度。当加速度传感器静止时，加速度传感器仅仅输出作用在灵敏轴上的重力加速度值，即重力加速度的分量值。

（1）通过两个轴来计算偏移角

图3-5 双轴加速度计夹角

8

通过图3-5可得出，加速度计输出与重力的关系可表示为：

AX=g?sinθ (3-7)

Ay=g?cosθ=g?sin（90°-θ） (3-8) θ=tan-1（AX/Ay） (3-9)

通过公式（3-9）即可得到倾斜角度值。在自平衡小车系统中，由于车体倾斜角度

小于30°，所以可以采用正切三角函数近似得到下式：

θ≈Ax/Ay (3-10)

（2）通过一个轴来计算偏移角

测量其中一个方向上的加速度值，就可以计算出车模倾角，比如使用X轴方向上的加速度信号。其变化的规律为：

AX=g?sinθ≈g?θ (3-11)

公式3-11中，g为重力加速度；θ为车模倾角。当倾角θ比较小的时候，输出电

压的变化可以近似与倾角成正比。

由于车模本身的摆动所产生的加速度会产生很大的干扰信号，它叠加在上述测量信号上使得输出信号无法准确反映车模的倾角，如图3-6所示。

图3-6 车模运动引起加速度信号波动

加速度计测量角度时，车模倾角在两个角度位置过渡中，除了角度变化信号之外，还存在由于运动引起的电压波动，这个电压波动随着车模运动速度增加会变得很大。

3.2.2 角速度数据处理

陀螺仪的直接输出值是相对灵敏轴的角速率，角速率对时间积分即可得到围绕灵敏轴旋转过的角度值。由于系统采用微控制器循环采样程序获取陀螺仪角速率信息，即每

9

隔一段很短的时间采样一次，所以采用累加的方法实现积分的功能来计算角度值：

angle=anglen-1+gyron?dt (3-12)

公式（3-12）中angle为陀螺仪采样到第n次的角度值；anglen-1为陀螺仪第n-1次采样时的角度值；gyron为陀螺仪的第n次采样得到的瞬时角速率值；dt为主循环程序运行一遍所用时间。图3-7可见，用陀螺仪输出值积分计算角度，要求处理器运算速度足够快，采样程序应尽量简练，程序循环一遍所用时间dt越小，采样频率越高，最后积分得到的角度值才能越精确。

图3-7 角速度积分得到角度

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

图3-8 角速度积分漂移现象

如何消除这个累积误差呢？方法就是通过上面的加速度传感器获得的角度信息对此进行校正。通过对比积分所得到的角度与重力加速度所得到的角度，使用它们之间的偏差改变陀螺仪的输出，从而积分的角度逐步跟踪到加速度传感器所得到的角度。如图3-9所示。

10