在前面一篇文章《》中我们已经介绍了两轮平衡车如何保持不倒的控制原理，这篇我们来整体介绍一下两轮平衡车如何既能保持不倒还可以以一定速度移动的？深入探讨串级PID控制算法同时讲解其C代码实现，让你一文搞懂串级PID。
两轮自平衡车如何保持不倒？原理关于这部分其实前面文章已讲，但是这里我还是做个简单回顾。
倾角状态控制
我们先来感性大白话理解一下。如上图，左一这种状态就是属于一种直立状态，稳定状态，此时车身的倾斜角度β为0,当然这也是我们希望小车保持的理想目标状态。但是如上图左二情况，如果说车身先前有一定倾斜，此时倾斜角度β>0,这时候我们就需要让电机带动车辆顺时针旋转，这时候车轮受到地面对车子的反作用力，该作用力向右或者说向前，产生向右运动加速度，平衡车下半部往前运动，这样倾斜角度β就会减小。而且倾斜角度β越大，产生的加速度a越大。反方向倾斜情况是一样的道理，只是车子向后运动而已。
所以我们可以看到，如果我们可以通过车载传感器能够测量到车身倾斜角度β，而且我们知道车子需要的加速度和倾斜角β正相关的，我们只要将传感器测量的车身实际倾斜角和我们需要车身保持的目标倾斜角β_targe作比较，角度误差作为控制状态。
那么我们用一个PID的比例P调节不就完了吗？公式如下：
a=k_p*(β-β_target)
其中k_p是比例调节系数。a为输出加速度，给电机。
也就是说如果当前车子倾斜状态和我们目标状态有误差，我们就让电机按误差比例响应就完事了。比如让车子保持直立状态，那就是β_target为0。
我们知道，PID控制，比例调节可以让系统快速响应，这正是此案例需要的，因为我们就是要车子回到目标角度状态动作响应快。但是还不够，比例控制很容易让车子控制超调，比如回到目标角度的时候很容易反向越过。为了解决这个问题还要引入一个微分D控制，整体PD控制离散公式如下：
β_err = β-β_target
a=k_p*β_err k_d*(β_err-β_err_pre)/dt
其中β_err是当前倾斜角度误差，β_err_pre为上一次倾斜角度误差，k_p为比例控制系数，k_d为微分控制比例系数，dt为离散时间间隔，控制周期。(β_err-β_err_pre)/dt即为误差微分项。
直立PD控制原理图如下：
PD直立控制原理
直立PD控制C代码// PD controller implementation(Proportional, derivative). DT in seconds
float stabilityPDControl(float DT, float input, float setPoint,
float Kp, float Kd)
{
float error;
float output;
error = setPoint - input;
output = Kp * error Kd * (error - errorOld)) / DT;
errorOld = error;
return (output);
}
代码比较简单，完全按上面公式来，其中input参数为传感器测量新的角度值，setPoint为目标角度，errold变量即为上一次角度误差。
平衡车如何有速度？原理上面部分我们已经弄清楚如何让两轮平衡车保持直立不倒了。当然这是最核心的控制环了，因为两轮车我们首先要确保车子不倒下，其次才能谈其它的。既然我们已经能让车子不倒下了，下面我们继续来看看如何让两轮车能以一定速度前后移动。
我们一步一步拆解。先来看看如何让车向前运动？
其实这个问题我们前面已经提到了。通过上面分析，如果我们车身有一个前倾角，那么在直立PD控制器作用下，小车就会产生向前的加速度，向前运动来逐步减小倾斜角度偏差，而且是倾斜角度偏差越大，那么直立PD控制系统让电机产生的加速度越大，对应速度V也就越大。一句话总结就是：小车的速度其实是和倾斜角度相关的。
那么我们控制车子的速度控制子系统的控制状态量是车速V,输出控制量就是车身倾斜角度。那么我们可以在直立PD控制前面加一个速度控制环，速度环控制输入是我们的目标速度和传感器测得的实际车速，为了保证速度准确性，我们采用PI控制，然后速度环输出是需要调节的控制量角度值，我们将这个角度值给到直立PD控制的目标值，也就是直立控制环的目标就是速度环算出的控制量角度值，然后直立控制环再根据速度环给的目标值来调整车加速度。形成一个串级PID控制，如下图：
串级PID
整体流程如下，将我们希望的车速和车子实际测得的额速度输入给PD速度控制器，它会输出一个期望角度值，该角度值和传感器IMU侧得到实际倾斜角度值输入到PD直立控制器，它会输出一个最终让电机执行的加速度值。
公式整理如下 ：
整体公式
(1)公式是直立PD控制，(2)公式是速度PI控制，a1是速度环输出作为目标值给到(1)式中。
速度PI控制C代码// PI controller implementation (Proportional, integral). DT in seconds
float speedPIControl(float DT, int16_t input, int16_t setPoint, float Kp,
float Ki)
{
int16_t error;
float output;
error = setPoint - input;
PID_errorSum = constrain(error, -ITERM_MAX_ERROR, ITERM_MAX_ERROR);
PID_errorSum = constrain(PID_errorSum, -ITERM_MAX, ITERM_MAX);
//Serial.println(PID_errorSum);
output = Kp * error Ki * PID_errorSum * DT; // DT is in miliseconds...
return (output);
}
代码就是个PI控制非常简单，相信大家都能读懂。
总的控制流程代码这个总的控制流程代码就是体现串级完整流程了。如下：
// 主控制函数
void control_loop() {
float target_velocity = 1.0; // 设置目标速度
float current_angle = read_angle_sensor(); // 读取当前倾斜角度传感器值
float current_velocity = read_velocity_sensor(); // 读取当前速度传感器值
float velocity_output = speedPIControl(target_velocity, current_velocity); // 速度PI控制输出
float balance_output = stabilityPDControl(velocity_output, current_angle); // 直立PD控制输出
set_motor_speed(balance_output); // 设置电机速度
}
后续持续更新系列高质量文章，码字不易，欢迎关注、点赞、收藏以及提问。
