0、关于本文
回到家已经一个多月了,疫情闹得沸沸扬扬,家里管制做的还行没有确诊病例,倒是舒舒服服过了个年,不能总是混吃等死啥也不干了,起床起床= =
最近老王又有了新想法,这几天在家没事就大致给自己定了一个新的研究方向,不过在此之前得先把传统四旋翼的控制方式搞清楚，匿名开源的代码看起来结构还是很清晰的，这里先解读一下控制部分的源代码。
1、控制部分代码
1
2
3
#include "ANO_FlyControl.h"
ANO_FlyControl fc;
先整定内环，后整定外环
参数整定找最佳，从小到大顺序查
先是比例后积分，最后再把微分加
曲线振荡很频繁，比例度盘要放大
曲线漂浮绕大湾，比例度盘往小扳
曲线偏离回复慢，积分时间往下降
曲线波动周期长，积分时间再加长
曲线振荡频率快，先把微分降下来
动差大来波动慢。微分时间应加长
理想曲线两个波，前高后低4比1
1
2
3
4
5
6
ANO_FlyControl::ANO_FlyControl()
{
yawRate = 120;
//重置PID参数
PID_Reset();
}
ROLL和PITCH轴向按照以上公式(ANO_PID.ccp:void get_pid())计算PID输出，但YAW轴比较特殊，因为偏航角法线方向刚好和地球重力平行，这个方向的角度无法由加速度计直接测得，需要增加一个电子罗盘来替代加速度计。如果不使用罗盘的话，我们可以单纯的通过角速度积分来测得偏航角， [–pid[PIDYAW].set_pid (80, 35, 0, 2000000)].缺点是由于积分环节中存在积分漂移，偏航角随着时间的推移会偏差越来越大。我们不使用罗盘就没有比例项，只仅使用微分环节来控制。
1
2
3
4
5
6
7
8
9
void ANO_FlyControl::PID_Reset(void)
{
pid[PIDROLL].set_pid (50, 10, 80, 2000000);//ROLL角度的内环控制系数；2000000为积分上限
pid[PIDPITCH].set_pid(50, 10, 80, 2000000);//PITCH角度的内环控制系数
pid[PIDYAW].set_pid (80, 35, 0, 2000000);//YAW角度的内环控制系数
pid[PIDLEVEL].set_pid(300, 0, 0, 0);//外环控制系数
pid[PIDMAG].set_pid (15, 0, 0, 0); //电子罗盘控制系数
}
串级PID
采用外环角度P 内环角速度PID
–>角度的误差【errorAngle】被作为期望输入到内环角速度控制器中 （角度的微分就是角速度）
串级 PID 算法中，角速度内环占着极为重要的地位。在对四旋翼飞行的物理模型进行分析后，可以知道造成系统不稳定的物理表现之一就是不稳定的角速度。因此，若能够直接对系统的角速度进行较好的闭环控制，必然会改善系统的动态特性及其稳定性，通常也把角速度内环称为增稳环节。而角度外环的作用则体现在对四旋翼飞行器的姿态角的精确控制。
外环：输入为角度,输出为角速度 位置式
内环：输入为角速度，输出为PWM增量 增量式
使用串级pid，分为：角度环控制环+角速度增稳环。
主调角度环（外环），副调角速度环（内环）。
参数整定原则为先内后外，故在整定内环时将外环的PID均设为0
所谓外环就是只是一个P在起作用，也就是比例在起作用；P也就是修正力度，越大越容易使飞机震荡。
震荡的特点是：频率小、幅度大
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
//飞行器姿态外环控制
void ANO_FlyControl::Attitude_Outter_Loop(void)
{
int32_t errorAngle[2];//【errorAngle*P=内环设定值】
Vector3f Gyro_ADC;
/*
期望值与当前自身状态的反馈求角度偏差
期望值：遥控器输入rc.Command
反馈：para：imu.angle.x
*/
errorAngle[ROLL] = constrain_int32((rc.Command[ROLL] * 2) , -((int)FLYANGLE_MAX), +FLYANGLE_MAX) - imu.angle.x * 10;
errorAngle[PITCH] = constrain_int32((rc.Command[PITCH] * 2) , -((int)FLYANGLE_MAX), +FLYANGLE_MAX) - imu.angle.y * 10;
//获取角速度【内环反馈】
Gyro_ADC = mpu6050.Get_Gyro() / 4;
//得到【内环Rateerror】
RateError[ROLL] = pid[PIDLEVEL].get_p(errorAngle[ROLL]) - Gyro_ADC.x;
RateError[PITCH] = pid[PIDLEVEL].get_p(errorAngle[PITCH]) - Gyro_ADC.y;
RateError[YAW] = ((int32_t)(yawRate) * rc.Command[YAW]) / 32 - Gyro_ADC.z;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
//飞行器姿态内环控制
void ANO_FlyControl::Attitude_Inner_Loop(void)
{
int32_t PIDTerm[3];
for(u8 i=0; i<3;i++)
{
//当油门低于检查值时积分清零
//若飞机没飞起来时就开始有积分，会导致起飞时不稳定
if ((rc.rawData[THROTTLE]) < RC_MINCHECK)
pid[i].reset_I();
//计算内环3 paras(ROLL+PITCH+YAW) PID，直接输出转为电机控制量
PIDTerm[i] = pid[i].get_pid(RateError[i], PID_INNER_LOOP_TIME);
}
//对YAW角继续处理，加入遥控控制
//在I值小于限幅值（这个值大概在5%油门）或者rate_error与i值异号时将rate_error累加到I中。
PIDTerm[YAW] = -constrain_int32(PIDTerm[YAW], -300 - abs(rc.Command[YAW]), +300 + abs(rc.Command[YAW]));
//PID输出转为【电机控制量】
motor.writeMotor(rc.Command[THROTTLE], PIDTerm[ROLL], PIDTerm[PITCH], PIDTerm[YAW]);
}
附录1
说实话我真的太喜欢visio画图了,手画的太寒酸就不往上面贴了,也就丛老师能看得下去:P
- 代码原理如下
