参考：
2.WHEELTEC B570 平衡小车开发手册
1. 控制系统
为了使小车闭环稳定，最简单的想法是通过PID控制器控制小车倾角θ\thetaθ，使其为0。但实际应用时，会发现由于测量倾角θ\thetaθ和实际重心倾角有误差，即当测量的θ=0\theta=0θ=0时，小车重心实际存在倾斜，导致小车持续加速。故实际上多采用小车倾角和速度双环PID级联的方式：倾角环多采用PD控制器，因为角度控制需要保证响应速度且允许稳态误差；速度环多采用PI控制器，因为速度控制不能有稳态误差。另外需注意：速度环反馈为正反馈，倾角环反馈为负反馈。
双环并联PID控制器控制框图如下：
其中：
θ∗{\theta}^*θ∗为倾角目标值（恒为0），
θ{\theta}θ为倾角测量值，
dθd_\thetadθ为倾角扰动，
Cθ{\rm C}\thetaCθ为倾角PD控制器（Cθ=k1+kds{\rm C}\theta=k_1+k_dsCθ=k1+kds），
Pθ{\rm P}_\thetaPθ为PWM倾角模型，
v∗{v}^*v∗为速度目标值，
vvv为速度测量值，
dvd_vdv为速度扰动，
Cv{\rm C}_vCv为速度PI控制器（Cv=k2+kis−1{\rm C}_v = k_2+k_is^{-1}Cv=k2+kis−1），
Pv{\rm P}_vPv为PWM速度模型，
Tp{\rm T}_pTp为给电机的PWM信号。
2. 小车软硬件设计
小车实物图如下：
主控ESP32，姿态角测量MPU6050，电机驱动tb6612，电池为塔克R5配套12v锂电池。
电机控制周期10ms，PWM频率5kHz，精度8192分频，姿态角通过6050自带的IMU读取，读取周期5ms。
3. 模式识别
3.1 PWM->倾角模型
上一篇文章得到了小车模型传递函数为：
Pθ(s)=−Qr+Mlr{MJ+(2m+2Ir2)(J+Ml2)}s2−MlgP_\theta(s)=\frac{-\frac{Qr+Ml}{r}}{\{MJ+(2m+\frac{2I}{r^2})(J+Ml^2)\}s^2-Mlg}Pθ(s)={MJ+(2m+r22I)(J+Ml2)}s2−Mlg−rQr+Ml
其中：
Q=M+2m+2Ir2Q=M+2m+\frac{2I}{r^2}Q=M+2m+r22I
MMM为车体质量
lll为车体质心到车轮轴点的长度
mmm为车轮质量
rrr为车轮半径
III为车轮转动惯量
JJJ为车体转动惯量
TTT为电机输出扭矩和
θ\thetaθ为车体倾斜角度
为了简化分析，将小车模型改写为以下形式：
Pθ(s)=−ks2−a2P_\theta(s)=\frac{-k}{s^2-a^2}Pθ(s)=s2−a2−k,
其中k,ak,ak,a为正常数。
实验装置如下所示：
（略）
当给出恒定PWM信号，即Tp=2000{\rm T}_p=2000Tp=2000时，小车倾角如下图所示（采样周期10ms）。
其中，黑线为角度测量值，红虚线为当k=0.4,a=3k=0.4,a=3k=0.4,a=3时PθP_\thetaPθ的模型仿真响应。
所以，小车PWM->倾角模型为：
Pθ=−0.4s2−9P_\theta=\frac{-0.4}{s^2-9}Pθ=s2−9−0.4。
3.2 PWM->速度模型
实验装置如下所示：
（略）
当给出恒定PWM信号，即Tp=2000T_p=2000Tp=2000时，电机平均转速如下图所示（采样周期10ms）。
图中黑线为速度测量值，红虚线为令v=0.02Tpsv=\frac{0.02{\rm T}_p}{s}v=s0.02Tp，即Pv=0.02sP_v=\frac{0.02}{s}Pv=s0.02时的模型仿真输出。
可以看出，在最初一段时间（200ms）内，施加恒定PWM信号下，速度可视为匀加速变化，故可视PvP_vPv为一纯比例积分环节。
虽然车轮实际扭矩无法测量，但车轮扭矩与车轮线速度理论关系为v=rTIsv=\frac{r{\rm T}}{Is}v=IsrT，与识别出的PvP_vPv结构一致。所以，我们可以认为在电机初始阶段，也就是大负载或堵转情况下，PWM信号（TpT_pTp）和车轮扭矩为比例关系。由于自平衡小车电机几乎都工作在大负载工况下，所以在上一篇文章（双轮平衡车建模及控制1）中，我们认为小车模型的输入，即给电机的PWM信号，代表扭矩。
综上所述，小车PWM->速度模型为：
Pv=0.02sP_v=\frac{0.02}{s}Pv=s0.02。
4. 控制器调参
4.1 现有调参步骤
CθCv{\rm C}_\theta {\rm C}_vCθCv调参，首先参考《2.WHEELTEC B570 平衡小车开发手册》P46:
先调角度环：使k2k_2k2，kik_iki，kdk_dkd为0。从0逐渐增大k1k_1k1，直至小车出现低频抖动。保留此时的k1k_1k1，然后从0逐渐增大kdk_dkd，直至出现高频抖动，保留此时的kdk_dkd。将记录的k1k_1k1，kdk_dkd乘以 0.6 作为Cθ{\rm C}_\thetaCθ的最终参数。
再调速度环：保留上一步识别出的Cθ{\rm C}_\thetaCθ参数，从0逐渐增大k2k_2k2， kik_iki值设定为k2k_2k2的 1/200， 观察小车是否能立在原地。 若小车出现抖动， 而且用手推动小车会发生大幅回摆， 说明k2k_2k2 、kik_iki过高， 应适量减少。
4.2 对现有步骤的定性分析
针对角度环调参:
系统角度误差可视为冲击信号。当kd=0k_d=0kd=0，k1k_1k1逐渐增加时，系统灵敏度函数，即：
S=11+Ct∗Pt，dθ→θ.S=\frac{1}{1+C_t*P_t}，d_\theta \rightarrow \theta.S=1+Ct∗Pt1，dθ→θ.
的冲击响应如下所示：
当k1k_1k1从0增加时，首先为了让系统稳定，根据稳定判据，需要闭环系统特征方程的解都在负半轴。针对本系统，当k1k>a2k_1k>a^2k1k>a2，即k1>22.5k_1>22.5k1>22.5时系统稳定。从上图可看出：k1=20k_1=20k1=20时系统冲击响应不稳定；k1>100k_1>100k1>100后，系统冲级响应的幅值和频率也随着k1k_1k1增大越来越大。但是，是不是k1k_1k1越大越好呢？这要看系统输入灵敏度函数，即:
U=Pt1+Ct∗Pt,dθ→Tp.U=\frac{P_t}{1+C_t*P_t}, d_\theta \rightarrow T_p.U=1+Ct∗PtPt,dθ→Tp.
当k1k_1k1逐渐增加时，UUU冲击响应如下所示：
从上图可看出，当k1k_1k1较小时（例如为420）时，UUU冲击响应产生的PWM信号较小。 当使用配减速器的有刷电机时，较小的PWM信号产生的电机扭矩被减速器衰减过大，无法在车轮产生所需扭矩。 同理，当k1k_1k1过大（例如为1220）时，产生的PWM超过了100%输出的对应值（即8192），也不会产生仿真中的扭矩。保证PWM响应处于适宜区间内是控制器设计的重要考虑因素。
对于kdk_dkd，理论上能抑制超调，增加响应速度，但是kdk_dkd也不是越大越好。k1=820k_1=820k1=820时，不同kdk_dkd对应U的波特图如下所示：
可以看到，当kdk_dkd增大时，对系统高频噪音的放大作用会加强。
针对速度环调参:
单独分析速度环没有意义，因为速度环是正反馈。当角度环控制器参数固定时，速度环参数对冲击响应的主要影响为：k2k_2k2增加时，系统振动幅值增大；kik_iki会略微影响稳态误差。这也合理，因为速度环作用主要是抑制角度测量误差中的恒定成分，此误差可视为阶跃信号。系统双环灵敏度函数为：
k2k_2k2对系统阶跃响应的影响如下图：
图中所示，增加k2k_2k2会增加系统震荡幅度，同时加快系统响应。增加kik_iki能减少稳态误差。
5. 控制效果
待后续