玩转树莓派

        对于四轴的平衡控制算法，我们进行这样的分析：由于各式各样的原因（安装时重力不稳、气流影响、各个电机转数略有偏差等）导致四轴并不能保持平衡，它在飞行过程中必将出现倾斜，我们希望四轴在飞行过程中能够保持平衡，当出现倾斜时它可以根据当前状态自动恢复到平衡状态。例如：

        上图中我们的四轴向右侧倾斜了20°我们自然而然的希望让其恢复平衡，于是我们可以通过调整电机0和电机2的转数来使其恢复平衡，我们知道向右倾斜时，电机2处于电机0的下方（以水平方向为基准）那么我们就可以通过加大电机2的转数同时减少电机0的转数，使电机2的拉力加大，从而让电机2的位置逐渐升高。同理，我们需要减小电机0的转数，使电机0的拉力减小，从而让电机0的位置逐渐降低。这样我们的四轴就会恢复到水平的状态下。对于X轴的电机0和电机2的增减幅度是一样的，由于四轴的对称性X轴出现倾斜则对电机0和电机2同时做转数调整，而对于Y轴出现倾斜则需要对电机1和电机3同时做转数调整。而对于Z轴的自旋，我们需要同时对0、1、2、3这四个电机都做出调整，需要根据反扭力的作用同时对0、2增加（减少）对1、3减少（增加），它们的增量幅度都是一样的。

        那么问题就来了，我们虽然知道了对电机的转数调整，但是调整的数值究竟是多少呢？如果增减的数值太小，就不足以使四轴恢复平衡，如果增减的数值太大，则会使四轴在水平位置不断的摆动甚至发生侧翻。一个理性的认知是：根据倾斜角度的大小来对电机转数的数值做调整。倾斜角度越大，调整电机的数值也就越大；倾斜角度越小，调整电机的数值也就越小。这也是我们PID反馈控制的其中一个部分，它叫做比例（Proportion），另外两个控制分别是积分（Integral）和微分（Differential）

        我们首先来看一下PID反馈控制，它的完整名称为：“比例、积分、微分反馈控制方法”。PID控制是一种非常成熟、有效的控制方法，它的数学公式为：      

        这是数学上的公式，积分和微分都需要在连续的函数上做计算，但在实际控制过程中我们是无法做到数学上的“连续”的，因为反馈控制的参数是通过传感器读入数据，而传感器通常都是通过采样来获取数据的，比如我们有一个陀螺仪用于读取当前角度，我们可以每一秒种读取10次，也可以每秒读取100次，或200次，假设陀螺仪支持的话，我们可以设定采样频率为1000次/S、10000次/S……等等，但无论将采样频率设置成多么的高，采样过程本身都是离散的，而不是数学上的“连续”过程。即使我们可以把采样频率提高到极限，让传感器的采样间隔无限接近于0（这是不可能的），但是我们的处理程序也不可能将这些连续的数据完全处理好，因为即使计算速度最快的计算机它的处理器每处理一条指令也是需要消耗时间的，所以我们的PID控制还是采用离散式控制方式，它的公式如下：

        我们通过陀螺仪可能检测出当前X、Y、Z三轴的倾斜角和旋转角速度然后根据这6个读数对4个电机做反馈控制。我们采用的是“双环串级PID反馈控制”：

• 外环输入：欧拉角（陀螺仪测量的倾斜角）
• 外环输出：期望角速度
• 内环输入：期望角速度 - 当前角速度（陀螺仪测量的角速度值）
• 内环输出：电机转数（实际上输出的是PWM信号的平衡补偿值）

        为了便于理解，我们来看一下串级PID控制的图形化表示：

        我们了解了串级PID控制的原理，接下来我们来看看代码的实现：

//外环PID对根据欧拉角控制期望角速度
//这里应该是期望角度-当前实际角度，所以这里为 0 - x_et
xv_et = engine_outside_pid(-e->tx, -x_last, &x_sum);

//内环角速度PID
//这里应该期望角速度-当前实际角速度
xv_et = xv_et - e->tgx;
e->xv_devi = engine_inside_pid(xv_et, xv_last, &xv_sum);

        这里面使用了两个函数，一个是engine_outside_pid()用于计算外环PID的期望角速度，另一个是engine_inside_pid()用于计算内环PID电机补偿值，这两个函数实现如下：

//内环PID输入角度输出角速度
f32 engine_outside_pid(f32 et, f32 et2, float* sum)
{
	//输出期望角速度
	f32 palstance = 0.0;
	s_engine* e = &engine;
	*sum += params.ki * et;
	//积分限幅
	engine_limit_palstance(sum);
	//XY轴PID反馈控制
	palstance = params.kp * et + (*sum) + params.kd * (et - et2);
	//输出限幅
	engine_limit_palstance(&palstance);
	return palstance;
}

//内环PID输入角速度输出PWM比例（千分比）
f32 engine_inside_pid(f32 et, f32 et2, float* sum)
{
	//输出期望pwm千分比
	f32 pwm = 0.0;
	s_engine* e = &engine;
	//积分限幅
	*sum += params.v_ki * et;
	engine_limit_pwm(sum);
	//XY轴PID反馈控制
	pwm = params.v_kp * et + (*sum) + params.v_kd * (et - et2);
	//输出限幅
	engine_limit_pwm(&pwm);
	return pwm;
}

        可以看到这两个PID函数内部大同小异，无非就是在做PID计算的参数不同而已。

        由四轴的对称性原理X、Y、Z三个轴的电机补偿值就可以根据PID计算得出，我们假设结果分别为：xv_devi、yv_devi、zv_devi。我们根据四轴的飞行模式是I型或X型对四个电机做转数补偿（请按自己安装陀螺仪时是I型还是X在下面两种模式任选其一即可）：

//四电机补偿I型
speed[0] = v + xv_devi - zv_devi;
speed[1] = v + yv_devi + zv_devi;
speed[2] = v - xv_devi - zv_devi;
speed[3] = v - yv_devi + zv_devi;

//四电机补偿X型
speed[0] =  v + (xv_devi / 2) - (yv_devi / 2) - zv_devi;
speed[1] =  v + (xv_devi / 2) + (yv_devi / 2) + zv_devi;
speed[2] =  v - (xv_devi / 2) + (yv_devi / 2) - zv_devi;
speed[3] =  v - (xv_devi / 2) - (yv_devi / 2) + zv_devi;

        值得注意的是，我们通过PID计算出来的电机数值，应该在油门速度v的基础上进行增减，而不是直接把这个结果作用到电机上。

        明白了四轴的平衡原理我们就可以对四轴进行调参，也就是调节这6个参数的值：Kp、Ki、Kd、vKp、vKi、vKd前3个参数为外环PID控制参数，后3个为内环PID控制参数。PID调参不是一件简单的事，尤其是共有6个参数同时能影响到系统的稳定性。下面我们就来谈谈PID控制原理希望会对那些不知道应该如何调参的朋友有所帮助。PID有3个参数，分别为Kp、Ki、Kd：

        Kp：比例参数，它决定了系统误差与输出的幅度大小。离期望值越近，输出的结果就越小；离期望值越远，输出的结果就越大。例如：假设Kp = 100，当前倾斜角为0.087（弧度制）时得到的输出为8.7；当前倾斜角为0.523时得到的输出为52.3。

        Ki：积分参数，它决定了系统消除稳态误差的快慢。实际上积分的计算结果是一个累加和（Sum），当系统通过比例参数输出结果不足以使系统完美平衡时（多数系统的误差-平衡过程不是线性的，也无法直接过通比例参数恢复平衡），积分控制就可以通过多次的累加使系统逐渐将期望值接近，最终达到平衡的目的。

        Di：微分参数，它决定了系统恢复平衡速度的快慢。其实微分参数是对比例和积分的反作用控制，它可以根据当前系统的变化率调节输出的快慢。例如：假设Kd = 30时，当前倾斜角为0.3而上一次的倾斜角为0.2，根据微分参数计算出100 × (0.3 - 0.2) = 10。它的实际作用是四轴原本只有0.2的倾斜角，但现倾斜角变成了0.3，也就是说倾斜角变的更大了。微分项计算出的结果为10,使输出变大，抑制斜角变大的趋势。如果当前倾斜角为0.2而上一次的倾斜角为0.3，根据微分参数计算出100 × (0.2 - 0.3) = -10。它的实际作用是四轴原本有0.3的倾斜角，现倾斜角变成了0.2，也就是说倾斜角变的更小了。微分项计算出的结果为-10，使输出变小，防止四轴在接近平衡点时输出太大从而越过平衡点，导致在平衡点左右一直振荡或侧翻。

        调参的过程并不简单，需要很多的经验来调整它们，不过值得庆幸的是每一个参数都不需要十分精确，我们只要保证四轴在串级PID控制下误差值能够收敛就可以了。建议先调整Kp参数再调整Pi，最后调整Pd。