平衡小车PID学习

离散式PID：

位置环：

入口参数：位置测量值，编码器的位置测量值

系统的参数调定要求：

目标： 准确性，稳定性，快速性。
指标： 最大超调量，响应时间，静差。
最大超调量->稳定性
响应时间 ->快速性
静差 ->准确性

参数：

P：提高响应速度。

• KP大会减小静差，提高速度。但是过大会振荡（引入微分）
• KP小且KI为0时会出现静差

I：减小静差。

• 响应速度更快。
• 弊端是if有误差，随着时间推移会越来越大。
  D：抑制振荡
• 防止超调，减少振荡次数。
• 弊端：响应速度变慢。因为系统的阻尼增大

位置环调节：先只调P，增大P直至振荡，引入D，增大阻尼以消除振荡。此时根据系统对静差和响应时间要求来调P和I。
位置环我们用的是负反馈。

速度环

在规定时间内测量所产生的脉冲个数来获得被测速度，称为M法测速。
使用增量式PID。

速度环我们用的是正反馈。

直立控制

算法原理

为了保持小车的直立，小车往前倾时需要车轮往前走，后倾时车轮往后走，所以直立控制最本质就是让小车的加速度与倾角成正比。
其实小车的直立可以与单摆类比，都是一维的。单摆收到重力的分力产生的回复力（与角度成正比）使单摆回到垂直平衡位置。而如果理想状态下，没有阻力是一定不会停下的，这就需要空气阻力作为阻尼（与速度成正比）。
所以回到小车，我们最开始是设置，加速度与速度成正比 ，（这就是比例控制）但是由于惯性，小车在达到直立的时候由于转动惯量，会往另一个方向倒去，这样就一直在平衡位置振荡，这时候的这个力相当于回复力。而我们要让车能直立，还需要一个阻尼力。这就需要微分发挥作用。此时
加速度 = b1角度偏差+b2角度偏差的变化率（即角速度）
这就是直立的算法表达式，比例控制——回复力。微分控制——阻尼力，转动惯量（质量与重心位置）决定微分系数。

代码

在这里插入代码片

速度控制

算法原理

速度环使用的是正反馈。如果是负反馈，当小车向前走的时候，车需要往后，速度会下降，如果是负反馈，就会继续让车轮向后加速，倾角继续增大，小车就倒了。
（其实这里有个问题就是车轮需要往后让小车向前倾斜，才能向前走吗，车往后又与速度下降有什么关系呢）

小车前行的原理：想提高小车向前运动的速度，就需要增加小车的前倾角。倾角加大之后，由于直立环的控制，需要车轮往前，这样就实现保持平衡，并加速向前走。
这样其实就把速度环的输出作为直立环的输入，形成了串级PID。直立使用PD，速度使用PI。

获取小车倾角：

加速度计对于短时间内的角度变化测量误差较大，适合长时间的测量，不适合短时间测量。
陀螺仪应用起来有很大的局限性，对长期的角度变化测量误差较大。
陀螺仪不适合长时间测量，加速度计不适合短时间测量。
MPU6050通过配置，可以精确测量角度的变化，通过DMP处理，可以输出16位的原始数据（16位ADC），然后通过算法得到四元数，再转换为欧拉角，我们就可以根据欧拉角的变化来进一步是小车处于平衡状态！

如果做自由落体，加速度计输出为0，也就是加速度计设计是为了测量重力加速度在坐标轴的分量导致的。

• 机体好似一条船，姿态就是航向（船头的方位），重力是灯塔，陀螺（角速度积分）是舵手，加速度计是瞭望手。舵手负责估计和把稳航向，他相信自己，本来船向北开的，就一定会一直往北开，觉得转了90度弯，那就会往东开。当然如果舵手很牛逼，也许能估计很准确，维持很长时间。不过只信任舵手，肯定会迷路，所以一般都有瞭望手来观察误差。

• 瞭望手根据地图灯塔方位和船的当前航向，算出灯塔理论上应该在船的X方位。然而看到实际灯塔在船的Y方位，那肯定船的当前航向有偏差了，偏差就是ERR=X-Y。舵手收到瞭望手给的ERR报告，觉得可靠，那就听个90%ERR，觉得天气不好、地图误差大，那就听个10%ERR，根据这个来纠正估算航向。

DMP硬件解算

在Crazepony上，测试了软件解算四元素，然后通过四元素解算姿态角这种实现方式，其实总的来说，并没感觉36MHz的主控压力有多大，没有出现机身不稳，卡死的情况。

同时，本着务实他的态度，我们也测试了MPU6050的硬解四元素，即从IIC总线上读到的数据不再是MPU60x0的AD值，而是通过初始化对DMP引擎的配置，从IIC总线上读到的直接就是四元素的值，从而跳过了程序通过AD值计算四元素这个看起来繁琐的步骤。测试结果是，机身反应的确要比之前反应灵活，最关键的一点是，这样得出的偏航角（Yaw）很稳很稳，基本不会漂移或者说漂移小到了可以容忍的地步。

最后，MPU60x0的强大之处不仅于此，它支持一个从IIC接口，可以外部接上一个磁力计，如HMC5883，这样一来，DMP引擎可以直接输出一个绝对的方向姿态，即能够输出一个带东西南北的姿态数据包，很厉害的样子。