平衡机器人学习笔记2

控制流程

基于模型设计（MBD）的设计思路。为减少控制的复杂程度和提高代码的效率，采用较为简单的双闭环PID控制思想来具体实现平衡模型能够成功运行的嵌入式代码的实验。

首先分析系统的平衡控制，整个系统通过陀螺仪加速度计传感器的数据计算到角度，根据编码器信号计算出速度信息，角度和速度作为系统输入量给到控制芯片计算出电机的控制量，最后以PWM信号的方式输出给驱动芯片，驱动执行部件一有刷电机进行运动。

基础平衡计算由角度偏差值计算得到，但平衡量不足以维持平衡，加入速度控制量与转向控制量，三者线性叠加得到最终的电机PWM控制量进行输出。

平衡控制系统

假设整体质量为M，中心距离车轴高度约为L，相对垂直面倾斜角度为0，运动加速度为 $a(t)$ ，同时车身有个角加速度
$F(t)$ ，可以分析其受力方程如式下所示。

$$ L \frac {d^ {2}\theta (t)}{dt^ {2}} =g \sin \theta (t)-a(t) \cos \theta(t)+L*F(t) $$

在接近平衡的时候，角度$\theta$可以认为很小，因此可以将 $\sin\theta(t)$ 和$\cos\theta(t)$简化为下式

$$ L \frac {d^ {2}\theta (t)}{dt^ {2}} =g* \theta (t)-a(t)+L*F(t) $$

(4.2)

当平台趋于静止平衡的时候，加速度应该为0. 此时：

$$ L \frac {d^ {2}\theta (x)}{dt^ {2}} =g* \sin \theta (t)+L*F(t) $$

对应可以求出系统传递函数：

$$ H(s)= \frac {\theta (s)}{F(s)} = \frac {1}{s^ {2}-\frac {g}{L}} $$

由上面的公式可以知道该系统一共有两个极点，分别为土√g/L，但是根据奈奎斯特判据，其中一个极点位于s平面的右半面会引起系统的不稳定性，为了消除这种情况，可以采用加入反馈环节的方法，能够有效避免闭环过程中的参数波动所产生的扰乱系统控制效果的影响，同时能够减小系统时间常数和非线性的影响。反馈控制器可以是PI，可以是PD，也可以是PID，这里考虑到角度控制环节中，由于角度信息由陀螺仪加速度计采集，这一类惯性测量仪器容易有信号噪声和零点漂移，为了防止这些误差被积分环节进一步扩大，因而决定引入比化例和微分反馈。1此时系统如左下图所示。

系统传递函数变为式（4.5）的形式

$$ H\left( s\right) = \frac{\theta \left( s\right) }{F\left( s\right) } = \frac{1}{{s}^{2} + \frac{{k}_{2}}{L}S + \frac{{k1} - \mathrm{g}}{L}} $$

(4. 5)

此时系统的两个极点可表示为式（4.6）形式

$${S}_{P} = \frac{-{k}_{2} \pm \sqrt{{k}_{2}^{2} - {4L}\left( {{k}_{1} - \mathrm{g}}\right) }}{2L}$$

(4. 6)

从式（4.6）可知，当k1>9,k2>0时，满足两个极点位于s平面左半平面的稳定条件，在整个角度环路中，k1与k2的阶分别代表了角度与角速度，因此可以得出结论，控制平衡的控制量由角度与角速度计算而来。由于控制器的离散特性，需要将以上结论进行另一种形式的转化。我们通过直接分析其平衡规律也可以得到同样的结论，首先因为产生倾角进而产生控制量，前倾的时候车轮也要向前运动，后倾则车轮向后，所以认为电机控制量与倾角之间存在比例关系如下式（4.7）。

$$ x={k}_{1}{\theta} $$

(4.7)

但是当0为0时按照上式中控制量×也为0，此时由于绕轴的转动惯量的存在而产生的惯性使得车体并不能维持这种平衡状态，需要引入一种阻尼力来保持平衡，这时候便可以引入一个与角速度相关的阻尼器，控制量的计算变为如下形式，如式（4.8）。

$$x={k}_{1}{\theta}+{k}_{2}{\theta}^{`}$$

(4.8)

同样可以得到这样一个PD控制器，其控制原理如下图所示。

速度控制器

在保持平衡的前提下做速度控制是一件较为复杂的事，因为改变速度的同时不能影响基本的平衡控制，故不能将速度的控制效果直接加到电机速度的改变中去；因此，为了简化控制量与控制结果之间的关系，我们将速度控制看作是与角度相关的平衡控制的外环控制来看，即将直立控制的目标值看作是速度控制的结果分析可以得到如图所示的控制原理图。

由图可以看出原先直立平衡中的输入量变成了速度控制的输出量，由此形成一个串级控制系统，由于前面平衡控制器采用的是PD控制，这里为了消除系统的静态误差，提高响应速度以及抗扰能力，选定速度控制器为PI控制器。

$$x = \mathrm{{kp}} * \left( {\theta - {x}_{1}}\right) + \mathrm{{kd}} * {\theta }^{\prime }$$

(4.9)

$${x}_{1} = {\mathrm{{kp}}}_{1} * e\left( k\right) + {\mathrm{{ki}}}_{1} * \sum e\left( k\right)$$

(4. 10)

其中式（4.9）为直立平衡控制，式（4.10）为速度控制， $\theta$ 代表角度， ${\theta}^{`}$ 代表角速度， $e(k)$ 是实际速度与设定速度的偏差，偏差求和代表积分，可见两式可以进行结合，从而达到简化系统的目的，将式（4.10）代入式（4.9），可得出式(4.11)。

$$x = \mathrm{{kp}}\theta + \mathrm{{kd}}{\theta }^{\prime } - \mathrm{{kp}}\left\lbrack {{\mathrm{{kp}}}_{1}e\left( k\right) + {\mathrm{{ki}}}_{1}\sum e\left( k\right) }\right\rbrack$$

(4. 11)

再由式（4.11），可以看出两者控制可以进一步拆分为一个负反馈的角度控制与正反馈的叠加结果如图4.6所示。