[转1]
整个系统分为三大部分。
1.角度采集
2.电机数据运算
3.电机驱动

先说第一部分：角度采集.
这里我采用两种传感器：一种是陀螺仪（ENC03），另一种是加速度计（MMA7260）.其实任何其中一
种都可以得出现在的车体倾斜角度值。为什么我们要选择两种呢？原因是这里有一种思想在里面：互补。

先说下陀螺仪：
陀螺仪属于测量旋转运动的传感器，其输出的是角度变化速度。即旋转的越快，输出量越大（不能
无限大，传感器上有极限指标）；旋转的越慢，输出量越小（最小为0，即静止时）。
我们可以通过积分运算（累加），得出相对角度数据，这里假设为A。但是我们知道，积分是有严重
的累积误差的。我们可以一句话概括：陀螺仪短时间准确，长时间不准确。
再说下加速度计：
加速度计属于测量线性运动的传感器，其输出的是传感器沿某个轴向直线运行时的速度变化量。即
速度变化的越快，输出量越大（不能无限大，传感器上有极限指标）；速度变化的越慢，输出量越小（最小
为0，即静止时）。这种特性用来做惯导研究很好。但是如何和我们的角度倾斜量相关呢？
我们知道加速度计测量的是加速度量，而地球上的物体都是受万有引力制约的，即我们时刻都受到
一个大小为g的竖直向下的加速度的作用。可能很多人都快看不下去了，我就直接贴图说明吧，如下：

我们通过反正弦运算，可以得到加速度计当前与重力方向的夹角数据，这里假设为B。
但是又有新的问题了。加速度计是测量线性运动的，即其对震动非常敏感。电机转动，所产生的震
动是不可小视的。所以其会有很大的噪声。而且当加速度计运动时，其输出量是运动加速度与重力加速度的
混合数据，这会严重影响角度计算的准确性。其特点用一句话概括：加速度计长时间较准确，短时间误差大。

现在我们重新审视一下，这两个传感器恰恰成互补关系。要得出比较准确的、长时间稳定的角度数
据，这两种传感器是缺一不可的。这也就是贯穿我整个数据采集过程的基本思想–互补。
我的具体做法是偏向于使用陀螺仪数据。原因是陀螺仪数据的噪声较小，短时间内误差小。而其长
时间的积分误差怎么解决呢？我的方法是使用加速度计角度数据B的平均值来校准陀螺仪的角度数据A.
直接伪代码如下：
A += （B-A）/k;
这里的K是衰减倍数。
其实就这么简单，这就实现了比较准确的角度数据的采集工作。

下面说下滤波问题。我们知道加速度计的噪声很大，我们这里采用卡尔曼的思想来滤波，另一种说法是数据融合。

其实这里就是两种传感器的典型互补了，用其算出来的角度数据相对来说很准了。
设加速度计换算出的角度数据为M，陀螺仪只采数据为s,陀螺仪相对加速度计的因子为k,上次加速度计经过滤波后的角度数据为M_2,卡尔曼
增益选择固定值为g.滤波后的角度数据为real_M;
那么滤波伪代码：
Temp=M_2+s*k;
real_M = Temp*g+(m-Temp)*(1-g);
M_2 = real_M ;

其实整个小车中，最关键、也最难的地方就是角度采集问题。准确的角度，只要一个P作用量就能轻松让小车站起来。

一部分：电机数据运算.
其实网上关于这部分的资料是最多的。我采用的是经典的PID算法。
理论我不讲了。我只讲讲实现过程。
这里如果先试试车体的稳定性，我们先只使用P作用。
现在以能立住为目标。

P,就是比例。假设当前角度偏差为error,目标角度为aim,比例系数为p,那么
PID计算伪代码为PID_result = (error-aim)*p;//(这里aim=0,即竖直状态)。
可以看出，如果车体倾斜度越大，那么计算出的PID的P值比例性的增大。

举实例说明（具体参数与车体和电机等有关）：
我的电机为[email=200RPM@12V]200RPM@12V[/email]，PWM的调制精度为-255~+255，那么当我的P参数p=30时，小车基本能够立住了（不足点就是总喜欢前后跑动）。

其实就这么简单。

剩下的I和D参数的调制方法，大家可以查阅网上的丰富的资料。I是减小误差的，D是阻碍小车姿态变化的。

3.电机驱动
这部分是最基础的。我才用的是L298驱动芯片。加了光耦隔离（原因是减速电机正反转切换时，产生的反电动势会造成单片机死机）。电机控制器为ATmega8，脉宽调制是使用的其内部自带的硬件PWM。为了简单起见，直接给两个轮子临时加上了码盘，而ATmega8也承担了由码盘数据得出电机转速和行程的数据任务。其与控制器ATmega168之间使用TWI(其实就是I2C)协议通讯。

 

 

[转2]

主控制流程如下：

下面对“过程一”和“过程二”作具体介绍：
? 过程一：加速度计算：
如图：

(原文件名:新建图像.JPG)  引用图片
点O为轮轴，点P为机体（系统）质心；
假设已知：机体质量m , 重力加速度g ,
机轴倾角θ（具体测法参见“实验报告”）。
设机体以加速度a加速，
则受到假想的惯性力F*= ma;
要使机体回复平衡状态，就要使得：
ΣM = 0 （M为力矩）；
如图以点O（即轮轴）为转轴，支持力N，过转轴，力矩为零；摩擦力为被动力，且轮胎不与机身固连，不需要考虑。
即：mglsinθ+ F* = 0;
∴  a ≥ g?tanθ;
从而可以令：a = k?tanθ?g （k≥1）;
进一步修正：
又知a应与P点相对于点O的角（线）速度（ω=d θ/d  t）成正比以获得更好的回复效果;
∴可以令  a = k1 ?ω?tanθ?g; （★）
其中：k1为常量，其取值可通过实验获得；ω为优化修正量，没有量纲。

?过程二：加速度实现：
假设轮子与地面不发生相对滑动，
∴  a = at    （at为车轮的线加速度）；
∴  a = R?d ω/d t    （R为车轮半径）；
而  J = ΣJi = ω?I    （J为角动量；I为车轮转动惯量。由于质点组（车轮）角动量方向相同，可以叠加。）；
∴  at = R?d ω/d t = R/I?(d J/d t) = (R/I)?M ；
其中：  M = M1 – M0    （其中M0为阻力力矩，设其为定值；M1为马达产生的力矩）；
∴  M1 = at?I/R + M0;
已知：M1M1与马达扭力F成正比,
而：P = F?V    （其中，P为功率，V为车轮转速）；
∴  可以令：M1 = k?P/V    (*)；
∴  P = (1/k)?V?(atI/R + M0)    (其中V可以通过测定轮轴的转速来获得)；
进一步修正：
车轮可以近似看作均匀圆盘，∴I ≈ 2mR2    (其中m为单个车轮的质量)
a = at;       令：1/k = k2
∴  P = k2?V?(2mR?a + M0) ；
∴  单个车轮的输出功率为：P = k2?ma?V?R + 0.5 k2?V?M0 ；（★）
其中：k2 、M0均为常量，可从实验中归纳。
注明：由（*）可知，k的量纲为“m”,所以k2的量纲为“1/m”。
? 过程一与二的统一：
将两（★）式联立，可以得到：
P = k1 k2?tanθ?ω?mg?V?R + 0.5 k2?V?M0
其中，ω为优化修正量，没有量纲，k2有量纲（1/m）。

[转3]

Torque = AngleError * Kangle + AngularRate * Krate + VelocityError * Kvel + IntegratedVelocityError * Kivel

• Angle error is the displacement from vertical (balance)
• Angular rate is the rate of rotation (deg/sec)
• Velocity Error is the difference between the commanded velocity and the actual velocity
  Integral Velocity Error is an accumulation of velocity errors used to maintain the forward velocity over uneven terrain.
• The various Kxxx are the gains for each term.  The value range from 1-30 or so and are dependent upon lots of other factors like the mass of the robot, the resolution of the encoders, the loop rate for controlling the motors, etc.  I determined all of my values by trial and error.

http://e-book2003.blog.163.com/blog/static/17552073201082995130537/