传送门
系列博客
【控制实践------二轮平衡车】【一】运动分析及动力学建模
【控制实践------二轮平衡车】【二】实物设计和开源结构&代码
【控制实践------二轮平衡车】【三】基于PID的直立控制
前言
在前面两篇博客中，分别对二轮平衡车的动力学进行分析 ，并且制作实物 用于验证算法可行性。
后面的博客将采用不同的控制算法 来实现二轮平衡车的控制。算法的实现代码将保存在开源仓库 的不同分支中。
直立运动分析
在第一篇的博客中，对不同坐标系下的运动过程进行受力分析，得到了两个描述二轮平衡车的运动公式。
- 惯性系下（以地面为参考系）
( m p + m w ) x ⋅ ⋅ w = m p l α ⋅ ⋅ c o s α − m p l α ⋅ 2 s i n α (1) \boldsymbol{(m_{p}+m_{w})} \boldsymbol{\overset{\cdot \cdot}x_{w}}=\boldsymbol{m_{p}l}\boldsymbol{\overset{\cdot \cdot}\alpha cos \alpha}-\boldsymbol{m_{p}l}\boldsymbol{\overset{\cdot }\alpha^{2}sin\alpha}\ \tag{1} (mp+mw)x⋅⋅w=mplα⋅⋅cosα−mplα⋅2sinα (1) - 非惯性系下（以车轮中心为参考系）
J α ⋅ ⋅ = m p g l s i n α + m p x ⋅ ⋅ w l c o s α (2) \boldsymbol{J \overset{\cdot\cdot}{\alpha}} = \boldsymbol{m_{p}gl sin\alpha}+\boldsymbol{m_{p} \overset{\cdot \cdot}x_{w}lcos\alpha} \tag{2} Jα⋅⋅=mpglsinα+mpx⋅⋅wlcosα(2) - 不同角度 α ˙ 0 \boldsymbol{\dot{\alpha}{0}} α˙0， α 0 \boldsymbol{\alpha{0}} α0下，二轮平衡车的角度、角速度、角加速度变化公式
( J − m p 2 l 2 c o s 2 α m p + m w ) α ⋅ ⋅ = m p g l s i n α − m p 2 l 2 s i n α c o s α m p + m w α ⋅ 2 (3) \boldsymbol{(J-\frac{m_{p}^{2}l^{2}cos^{2}\alpha}{m_{p}+m_{w}}) \overset{\cdot\cdot}{\alpha}} =\ \boldsymbol{m_{p}gl sin\alpha}-\ \boldsymbol{\frac{m_{p}^{2}l^{2}sin\alpha cos\alpha}{m_{p}+m_{w}} \overset{\cdot}\alpha^{2}} \tag{3} (J−mp+mwmp2l2cos2α)α⋅⋅= mpglsinα− mp+mwmp2l2sinαcosαα⋅2(3)
因为一定知道了初始状态，就可以计算代入等式右边得到角加速度，进而计算出新的角速度和角度，周而复始。
分析三个公式，哪个公式适合用来做控制分析呢。
- 公式（1） 描述的是转动 α \boldsymbol{\alpha} α对轮子运动 x w \boldsymbol{x_{w}} xw的影响，此时轮子是被动的一方。即
α → x w \boldsymbol{\alpha} \rightarrow \boldsymbol{x_{w}} α→xw - 公式（2） 描述的是轮子运动 x ¨ w \boldsymbol{\ddot{x}{w}} x¨w对转动 α ¨ \boldsymbol{\ddot{\alpha}} α¨的影响，进而影响角度 α \boldsymbol{\alpha} α。即
x ¨ w → α ¨ \boldsymbol{\ddot{x}{w}} \rightarrow \boldsymbol{\ddot{\alpha}} x¨w→α¨ - 公式（3） 描述的是初始角度 α 0 \boldsymbol{\alpha_{0}} α0和初始角速度 α ˙ 0 \boldsymbol{\dot{\alpha}_{0}} α˙0对自身转动的影响，即自身角度转动的演化趋势。
显然，在直立控制时，希望控制的是平衡车的转动运动 ，并且需要有一个可控量 来完成，因此只有公式（2）适合作为被控对象模型。即通过控制轮子运动来控制转动运动，正好符合二轮平衡车的工作原理。
基于PID控制器的直立控制
设状态向量为 X = [ α α ˙ ] \bold{X}=\begin{bmatrix} \boldsymbol{\alpha}\\ \boldsymbol{\dot{\alpha}}\end{bmatrix} X=[αα˙]，系统输入为 U = x ¨ w \bold{U}=\boldsymbol{\ddot{x}{w}} U=x¨w，系统状态方程为：
X ˙ = [ 0 1 m p g l s i n 0 ] X + [ 0 m p l c o s α ] U (4) \bold{\dot{X}}=\begin{bmatrix}0 & 1\\\boldsymbol{m{p}gl sin} &0\end{bmatrix}\bold{X}+\begin{bmatrix}0 \\ \boldsymbol{m_{p}lcos\alpha}\end{bmatrix}\bold{U} \tag{4} X˙=[0mpglsin10]X+[0mplcosα]U(4)
列出状态方程的目的，主要是为了能够直观地描述直立运动的状态转移过程，帮助理解控制设计的原理。下面采用PID控制器进行控制设计。
角度环控制
- 从状态方程可以看出，角度的变化只受到角速度的影响，即：
d α d t = α ˙ (5) \boldsymbol{\frac{ d\alpha}{dt}} = \boldsymbol{\dot{\alpha}} \tag{5} dtdα=α˙(5)
设角度期望为 α r e f \alpha_{ref} αref，则角度误差 e r r o r α error_{\alpha} errorα为：
e r r o r α = α r e f − α (6) error_{\alpha}=\boldsymbol{\alpha_{ref}} - \boldsymbol{\alpha} \tag{6} errorα=αref−α(6)
因此在做角度的控制的时候，将 α ˙ \boldsymbol{\dot{\alpha}} α˙作为系统输入 U α \bold{U_{\alpha}} Uα，并且根据PID控制器的控制律可以得到：
U α = K p e r r o r α + K i ∫ e r r o r α d t + K d ∗ d e r r o r α d t (7) \bold{U_{\alpha}}=K_{p}error_{\alpha}+K_{i}\int{error_{\alpha}dt}+K_{d}*\frac{derror_{\alpha}}{dt} \tag{7} Uα=Kperrorα+Ki∫errorαdt+Kd∗dtderrorα(7)
得到的角度控制律 U α \bold{U_{\alpha}} Uα原本是直接作用到被控对象里来影响角度 α \boldsymbol{\alpha} α，但是被控对象无法直接决定角速度的数值 ，因此此处的控制律 U α \bold{U_{\alpha}} Uα即为期望的角速度数值 α ˙ r e f \boldsymbol{\dot{\alpha}{ref}} α˙ref。即：
α ˙ r e f = U α (8) \boldsymbol{\dot{\alpha}{ref}} = \bold{U_{\alpha}} \tag{8} α˙ref=Uα(8)
角速度控制
- 从状态方程可以看出，角速度的变化受到了角加速度（
重力分量m p g l s i n α \boldsymbol{m_{p}gl sin\alpha} mpglsinα和惯性力分量m p l c o s α x ¨ w \boldsymbol{m_{p}lcos\alpha\ddot{x}{w}} mplcosαx¨w）影响。
d α ˙ d t = α ¨ = 1 J ( m p g l s i n α + m p x ⋅ ⋅ w l c o s α ) (9) \boldsymbol{\frac{ d \dot{\alpha}}{dt}} = \boldsymbol{\ddot{\alpha}}=\frac{1}{\boldsymbol{J}}(\boldsymbol{m{p}gl sin\alpha}+\boldsymbol{m_{p} \overset{\cdot \cdot}x_{w}lcos\alpha}) \tag{9} dtdα˙=α¨=J1(mpglsinα+mpx⋅⋅wlcosα)(9)
角速度误差 e r r o r α ˙ error_{\dot{\alpha}} errorα˙为：
e r r o r α ˙ = α ˙ r e f − α ˙ (10) error_{\dot{\alpha}} = \boldsymbol{\dot{\alpha}{ref}} - \boldsymbol{\dot{\alpha}} \tag{10} errorα˙=α˙ref−α˙(10)
同样在做角速度的控制时，将 α ¨ \boldsymbol{\ddot{\alpha}} α¨作为系统输入 U α ˙ \bold{U{\dot{\alpha}}} Uα˙，并且根据PID控制器的控制律可以得到：
U α ˙ = K p e r r o r α ˙ + K i ∫ e r r o r α ˙ d t + K d ∗ d e r r o r α ˙ d t (11) \bold{U_{\dot{\alpha}}}=K_{p}error_{\dot{\alpha}}+K_{i}\int{error_{\dot{\alpha}}dt}+K_{d}*\frac{derror_{\dot{\alpha}}}{dt} \tag{11} Uα˙=Kperrorα˙+Ki∫errorα˙dt+Kd∗dtderrorα˙(11)
同样，可以得到：
α ¨ r e f = U α ˙ (12) \boldsymbol{\ddot{\alpha}{ref}} = \bold{U{\dot{\alpha}}} \tag{12} α¨ref=Uα˙(12)
根据公式（9） 可以得到期望的轮子加速度 x ¨ w , r e f \boldsymbol{\ddot{x}{w,ref}} x¨w,ref
x ¨ w , r e f = ( J α ¨ r e f − m p g l s i n α ) / m p l c o s α (13) \boldsymbol{\ddot{x}{w,ref}} = (\boldsymbol{J \ddot{\alpha}{ref}} - \boldsymbol{m{p}gl sin\alpha})/\boldsymbol{m_{p} lcos\alpha} \tag{13} x¨w,ref=(Jα¨ref−mpglsinα)/mplcosα(13)
总结
前面的角度角速度控制，其实就是很常见的串级PID控制器，故意拆成两部分来分析，是为了更清晰地展示串级PID控制器的原理，以及为后续一些算法的变种做铺垫，即，角度控制用一种控制算法，角速度控制用另外的算法是否也可行？
- 直立控制的框图：
- m a t l a b matlab matlab 仿真
仿真参数如下：
m = 0.857 k g m=0.857kg m=0.857kg
g = 9.8 m / s 2 g=9.8m/s^2 g=9.8m/s2
l = 0.05 m l=0.05m l=0.05m
J = 0.0021 k g ⋅ m 2 J = 0.0021kg \cdot m^2 J=0.0021kg⋅m2
T = 0.002 s T =0.002s T=0.002s
K p o u t = 5 ， K i o u t = 0.1 ， K d o u t = 0 Kp_{out}=5，Ki_{out}=0.1，Kd_{out}=0 Kpout=5，Kiout=0.1，Kdout=0
K p i n = 100 ， K i i n = 1 ， K d i n = 0 Kp_{in}=100，Ki_{in}=1，Kd_{in}=0 Kpin=100，Kiin=1，Kdin=0
这些参数是第二篇博客平衡车的实物参数，以及控制参数也与实际的控制参数一致
matlab
T = 0.002;
%离散时间
n = 10000;
%仿真步数
m = 0.857;
%平衡车重量，单位kg
g = 9.8;
%重力加速度，单位m/s2
l = 0.05;
%轮子中心到质心距离，单位m
J = m*l*l;
%转动惯量
kp_out = 5;
ki_out = 0.1;
kd_out = 0;
%角度pid参数
kp_in = 100;
ki_in = 1;
kd_in = 0;
%角速度pid参数
a_ref = zeros(n,1);
%角度期望，单位°
da_ref = zeros(n,1);
%角速度期望，单位rad/s
dda_ref = zeros(n,1);
%角加速度期望，单位rad/s2
u = zeros(n,1);
%系统输入，即轮子的加速度，单位m/s2
a = zeros(n,1);
%角度状态，单位°
da = zeros(n,1);
%角速度状态，单位rad/s
dda = zeros(n,1);
%角加速度状态，单位rad/s2
time = zeros(n,1);
%时间轴
a_integral = 0;
%角度误差积分
da_integral = 0;
%角速度误差积分
a_last_error = 0;
%角度上次误差
da_last_error = 0;
%角速度上次误差
for i = 1:n
time(i) = (i-1)*T;
a_ref(i) = 10 * sin(time(i));
% --------------------------- 仿真计算 ---------------------------%
a_error = a_ref(i) - a(i);
%角度误差
a_integral = a_integral + a_error;
%积分
a_diff = (a_error - a_last_error)/T;
%微分
a_output = kp_out * a_error + ki_out * a_integral * T + kd_out * a_diff;
%角度PID计算,对应公式7
% --------------------------- 角度环 ---------------------------%
da_ref(i) = a_output;
%角速度期望
da_error = da_ref(i) - da(i);
%角速度误差
da_integral = da_integral + da_error;
%积分
da_diff = (da_error - da_last_error)/T;
%差分
da_output = kp_in * da_error + ki_in * da_integral * T + kd_in * da_diff;
%角速度PID计算,对应公式11
% --------------------------- 角速度环 ---------------------------%
dda_ref(i) = da_output;
%角加速度期望
u(i) = (J*dda_ref(i) - m*g*l*sin(a(i)/180*pi)) / (m*l*cos(a(i)/180*pi));
%计算系统输入,对应公式13
% --------------------------- 控制解算 ---------------------------%
if i == n
break;
end
dda(i) = (m*g*l*sin(a(i)/180*pi) + m*l*cos(a(i)/180*pi)*u(i)) / J;
%状态方程，对应公式2
da(i+1) = da(i) + dda(i) * T;
%角速度计算
a(i+1) = a(i) + da(i) * T / pi * 180;
%角度计算
% --------------------------- 被控对象 ---------------------------%
end
figure(1);
subplot(2,2,1);
plot(time,a,'r',time,a_ref,'g');
title('角度响应曲线');
legend('角度','角度期望');
xlabel('time/s');
ylabel('角度/°');
subplot(2,2,2);
plot(time,da,'r',time,da_ref,'g');
title('角速度响应曲线');
legend('角速度','角速度期望');
xlabel('time/s');
ylabel('rad/s');
subplot(2,2,3);
plot(time,dda,'r',time,dda_ref,'g');
title('角加速度曲线');
legend('角加速度','角加速度期望');
xlabel('time/s');
ylabel('rad/s^2');
subplot(2,2,4);
plot(time,u,'r');
title('系统输入曲线');
legend('U');
xlabel('time/s');
ylabel('m/s^2');仿真结果如下：
这里所用到的控制参数与实际使用的参数一致，有兴趣的同学可以去开源的控制代码里查看，代码在control.c文件中
电机转速的控制
前言
在直立控制中，采用的系统输入 U \bold{U} U是电机轮子的加速度 x ¨ w \boldsymbol{\ddot{x}_{w}} x¨w，因此实现平衡车直立的关键因素就是控制电机实现我们期望的运动，对电机的转速控制具有十分重要的意义。
电机转速控制
由于不同的电机转速的转速控制方式不同，因此这篇博客仅给出转速控制的一些思路，不做仿真和模型讨论。
电机转速控制的关键是获取电机的当前转速信息，常用的传感器有编码器，光电传感等。
而对于不同类型的电机采用的控制算法也不同。
- 对于直流驱动的电机来说，通常采用
增量式PID控制。 - 对于带有FOC的无刷电机来说，通常采用
位置式PID控制。
以上是个人经验所给出的建议，仅供参考。
无论以何种方式，在实现电机转速控制后，需要根据直立控制的解算结果，来得到当前的期望转速 x ˙ w , r e f \boldsymbol{\dot{x}{w,ref}} x˙w,ref，即满足以下关系：
x ˙ w , r e f ( k + 1 ) = x ˙ w ( k ) + x ¨ w , r e f ( k ) ∗ T (14) \boldsymbol{\dot{x}{w,ref}}(k+1) = \boldsymbol{\dot{x}{w}}(k) + \boldsymbol{\ddot{x}{w,ref}}(k) * T \tag{14} x˙w,ref(k+1)=x˙w(k)+x¨w,ref(k)∗T(14)
其中， T T T为离散周期， k k k为时刻序号。
最后，再将公式（14）计算得到的期望电机转速传入电机转速控制器中，即可完成整个平衡车的控制周期。即完整的控制框图为：
结语
实践是检验真理的唯一标准，看懂原理的同时也需要多实践。
最后再贴一下开源的代码 按需自取：https://gitee.com/HaveALitttleSao/balance_car_control_code
放一下我个人调试的效果视频
- 俯视角度
二轮平衡车调试视频------俯视视角
- 侧面角度
二轮平衡车调试视频------侧面视角
以防有人说我把平衡车固定死造假 \doge