平衡小车PID控制

最新推荐文章于 2024-09-07 08:35:30 发布

Corazones

最新推荐文章于 2024-09-07 08:35:30 发布

阅读量1.9k

点赞数 10

文章标签：单片机

本文链接：https://blog.csdn.net/m0_57333603/article/details/138119048

版权

在不加外力的情况下，由于重力的作用，双轮小车不会平衡地立在地上，肯定会左右倾倒。

为了使小车平衡，需要加入直立环，速度环和转向换三种PID控制。

小车物理模型

小车平衡原理类似于手托木棍的原理，要使一根木棍竖直立在手上，需要木棍倒向哪边就用手向哪边接住，小车也是如此，小车向哪边倾倒，就让车轮向哪边前进，以实现小车平衡。

直立环控制

直立环主要为了控制小车平衡在机械中值，而不会左右倾倒，所谓机械中值就是小车平衡时的倾角，正常情况下倾角应该为0度，由于小车装配误差以及陀螺仪安装误差导致实际机械中值不是0度，需要自行测量机械中值。

PD控制回路

由于受到重力的作用，小车只要不平衡就会倾倒，不会产生稳态误差，而且小车的直立环要求响应速度快，如果增加积分环节可能导致积分饱和，降低响应速度，所以采用PD控制回路，去掉积分环节，比例环节用于快速响应，微分环节为了消除震荡。

公式推导

标准的位置式PID公式为：

只取比例和微分项，公式简化为：

第k次偏差减去第k-1次偏差其实是两次返回的实际角度差，为什么可以用角速度表示呢？

因为PID算法是离散的，每隔一个周期会计算一次，角速度就相当于1s时间内的角度差，而PID计算的偏差是一个周期内角度的偏差，如果周期为1s就正好等于角速度，一般周期要比1s快很多，所以偏差和角速度是成比例的，之所以直接用角速度代替是便于计算，差出来的比例可以在微分系数上中和掉。

整合后的PD公式为：

$PWM = K_{p}*(Zero-\theta )+K_{d}*\omega$

直立环参数调节

直立环使用PD控制器，需要调节Kp和Kd这两个参数。我们的 PWM 设定 7200代表占空比为 100%，平衡小车自平衡时前后摆动角度大概在+10°，所以我们可以知道Kp在 0-720 之间，Kd 的值大概在 0-2 之间。先确定Kp的极性。尝试调节Kp大小为 200，屏蔽转向环和速度环，并且设置Kd值为 0，若小车倾斜时车轮也向倾斜的方向加速，那么极性就是对的，反之，即为错的，如果极性是反的需要给Kp和Kd加上负号。

接下来可以开始调节Kp和Kd的大小了，先调节Kp，从0开始步进100地增加，直到达到合适的状态，如果小车向一个方向倾倒，说明Kp过小，如果小车左右震荡，说明Kp过大，将Kp设置在小车低频振荡的状态就可以了，然后增加Kd，慢慢消除小车的震荡，根据经验通常把直立环的参数乘0.6作为最后的结果，便于引入速度环。

速度环控制

只有直立环的话只能尽量保持小车不倒，如果需要使小车原地不动，需要加入速度环设置速度为0，如果要小车平衡移动，也需要速度环来控制小车移动速度，速度环采用PI控制，因为速度控制要求缓慢平稳，以减少对直立环的影响，采用串级PID控制，速度环为外环，直立环为内环，速度环的输出作为直立环的输入，速度采集可以直接用编码器的读数，不用换算成速度，因为速度和编码器计数也是成比例关系，可以在PI参数中中和掉。。

速度环加上直立环的公式推导这里就省略了，原理是将标准PID的PI部分取出来作为速度环，将速度环的输出作为直立环的输入带进去，经过计算推倒可以得出最后的结果就是速度环的PI和直立环的PD直接相加，所以可以将速度环PI和直立环PD写成两个函数，最后输出取两个函数返回值的和，下面的公式绿色部分就是速度环，红色部分是直立环。

小车平衡的关键就是使轮子不断摆动，最初的直立环效果就是在速度零的位置摆动，相当于一个来回震荡的曲线，加入速度环后就是在特定速度处震荡，曲线的形状没有改变，只是纵坐标发生了改变，所以直接相加即可。

速度环参数调节

速度环使用 PI 控制，需要调节Kp和Ki这两个参数。一般把Ki 设置为Kp/200先估算Kp的大小。我们每5ms读取一次编码器，左右编码器相加最大值大概在84左右。假设速度偏差达到最大速度的 50%的时候，系统输出电机最快速度即 PWM 输出 7200。我们可以大概估算:最大值=7200/(84*50%)=170。另外，速度环是正反馈过程，当倾斜加速时，想要速度慢下来，速度环 PWM 就必须输出更大,让小车快速摆正。如果使用负反馈，当小车以一定的速度运行时,我们通过减速让小车慢下来，小车会因为惯性向前倒下。先把直立环屏蔽，单独调试速度环。当转动轮子时，轮子速度越来越快,那么速度环极性就是对的。

速度环直接调节Kp即可，Ki将Kp除以200得到，以一定的步长增加Kp，平衡车会左右大幅度摆动说明Kp过大，此时响应速度应该是较快的；kp过小时，平衡车可能会来回徘徊，直立状态较好，但是响应速度慢；最完美的情况应该是小车在原地偶尔抖动一下，且响应速度较快。

转向环控制

直立环加速度环只能实现小车直线行驶，要实现小车转向还需要加入转向环。

转向环的两种用法如下：

①抑制转向：由于小车两个电机和车轮的误差，即使两个轮子给相同的PWM也可能输出不同的速度导致小车原地小幅转动，这时需要转向环来抑制小车转动，使其直线运动。可以将参数选用负极性来抑制转动，采集z轴角速度，如果是左转就将转向环参数取正加到左轮上，然后右轮减去转向环，增大左轮速度，减小右轮速度就会向右转，采用的是微分环节。

②期望转向，如果用蓝牙遥控小车转向时就需要小车具备转向功能，就应该取消抑制作用，将期望转向的角度作为输入，以比例环节来进行调节。