[常用控制算法]
传统控制算法：PID，模糊，神经网络控制算法。
现代控制算法有比例，LQR算法(用于线性系统)，自适应控制算法，滑模控制算法。
参考：CSDN
参考：B站
非常亲民的参考：CSDN
A、拿控制小车直线行驶到一个目的地来说
我们控制的是小车的速度，（位置式）
1.如果只根据误差量 乘以 kp 作为速度输出,那么到达终点时速度可能不为零，那么就会超调和震荡；
2.此时加入微分量Kd,根据误差的变化率（一般为负值）来削弱快到终点的速度，就可以减弱超调和震荡；
3.由于快到终点（在终点前或者超过终点）时控制量（不管是比例项还是微分项）都会变得很小，会导致小车永远到达不了终点，因此加入积分项，把当前所有误差累加起来作为正反馈，在此时变大控制量激励小车继续向前。
B、拿控制一个电机的PWM让其达到一个速度（增量式）
我们并不知道到达该速度需要多大的PWM占空比，或者电机老化了摩擦增大
- 根据误差 乘以 Kp 获得第一次的占空比值，此时与目标速度仍有差距
- 因此再多次次由误差算得一个占空比，若最后一次占空比提供的力矩刚好和摩擦力平衡，
并且由此时的误差算到的占空比数值与此时占空比相等，那么控制系统就平衡（蚌埠）住了，
就无法到达目标点了。- 因此需要加上KI，KD控制。
参考：模拟PID到离散型PID的转换
这是模拟PID公式，对于这个公式，你要知道：
Kp ：放大系数
e(t)：误差（t代表第t时刻采样得到的误差）
Ti ：积分时间常数
Td：微分时间常数
我们使用单片机进行PID运算，是不能采用模拟型PID的，因为单片机本身就是一个数字系统，里面只有0和1，因此不能处理模拟信号，（单片机中DAC处理模拟信号是将连续的模拟量转化为离散的数字量）只能将模拟信号转换成离散的数字信号，才能被单片机所用。
单片机实际上也不能连续积分，只能离散化的拟合积分。
Sk：是积分输出项
Ek：表示误差（第k次采样的误差）
T：采样时间
Ti：积分常数
以上说的PID公式属于位置式PID
Ek是当前误差
Ek-1是上次的误差
Ek-2是上上次的误差
首先定义一个当前误差iError和一个计算出来的增量值iIncpid。当前误差iError=目标值SetPoint-传感器采集的当前值PresentPoint，之后进行增量式PID运算，得出增量值iIncpid。
/********************增量式PID控制设计************************************/
int IncPIDCalc(int PresentPoint)
{
int iError , iIncpid; //iIncpid增量值 iError当前误差
iError = SetPoint-PresentPoint; //增量计算
iIncpid =(Proportion * iError) //E[k]项
-(Integral * LastError) //E[k-1]项
+(Derivative * PrevError); //E[k-2]项
PrevError = LastError;
LastError=iError;
return (iIncpid);
}
代码解答：增量式的简化
/********************位置式 PID 控制设计************************************/
unsigned int LocPIDCalc(int NextPoint)
{
int iError,dError;
iError = SetPoint - NextPoint; //求出当前偏差
SumError += iError; //对偏差进行积分
dError = iError - LastError; //对偏差求微分
LastError = iError; //转换赋值
//返回位置式PID计算结果
return(Proportion * iError //比例项
+ Integral * SumError //积分项
+ Derivative * dError); //微分项
}
DJI GM6020 控制 DEMO运用的也是位置模式
float pid_calc(pid_struct_t *pid, float ref, float fdb)
{
pid->ref = ref;
pid->fdb = fdb;
pid->err[1] = pid->err[0]; //上次误差
pid->err[0] = pid->ref - pid->fdb; //本次误差
pid->p_out = pid->kp * pid->err[0];
pid->i_out += pid->ki * pid->err[0];
pid->d_out = pid->kd * (pid->err[0] - pid->err[1]); //个人推断积分的累加频率就是while( )循环的频率
LIMIT_MIN_MAX(pid->i_out, -pid->i_max, pid->i_max);
pid->output = pid->p_out + pid->i_out + pid->d_out;
LIMIT_MIN_MAX(pid->output, -pid->out_max, pid->out_max);
return pid->output; //输出的是一个电压给定值，在main( )中被限定在3000
}
通过外部中断检测mpu6050INT引脚，检测到信号之后就进行一次pid运算。以此来严格控制PID计算周期。可能有些人会有疑问，为什么不放到while中循环检测？因为放在while循环的话，这个循环是可以被中断打断的。小车平衡对实时性要求高，如果在while循环里，姿态矫正时，程序被其他模块中断，小车就立不起来了。因此外部中断的优先级要配置成最高。
参考：CSDN
关键词：
由于速度环和直立环都是控制小车电机的转速，因此两者需串在一起。又因为平衡小车最主要的是直立，因为速度环的输出作为直立环的输入，直立环的输出最终作用到小车电机上。最终形成下图框图的形式：
1.给定目标速度，电机转动，编码器就会输出脉冲，单片机通过编码器模式检测脉冲值获得轮速，并把该值代入速度环计算出一个角度输出；
2.上述速度环计算得到的角度作为直立环的输入，直立环努力保持这个角度，输出PWM给电机，以此循环。
速度环和直立环分离框图：
表达式：
a1是速度环算出的角度，并把这个角度作为直立环的输入
a是直立环的输出
这样把速度控制系统输出看做是一个角度，原本要保持角度为零就变成保持一个小的角度a1，所以我们就有了一个这样的算法，两个式子合并。
/**************************************************************************
函数功能：直立PD控制
入口参数：角度、角速度
返回 值：直立控制PWM
作 者：平衡小车之家
**************************************************************************/
int balance(float Angle,float Gyro)
{
float Bias;
int balance;
Bias=Angle-ZHONGZHI; //===求出平衡的角度中值 和机械相关
balance=Balance_Kp*Bias+Gyro*Balance_Kd; //===计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数
return balance;
}
/**************************************************************************
函数功能：速度PI控制 修改前进后退速度，请修Target_Velocity，比如，改成60就比较慢了
入口参数：左轮编码器、右轮编码器
返回 值：速度控制PWM
作 者：平衡小车之家
**************************************************************************/
int velocity(int encoder_left,int encoder_right)
{
static float Velocity,Encoder_Least,Encoder,Movement;
static float Encoder_Integral,Target_Velocity;
//=============速度PI控制器=======================//
Encoder_Least =(encoder_left+encoder_right)-0; //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
Encoder *= 0.7; //===一阶低通滤波器
Encoder += Encoder_Least*0.3; //===一阶低通滤波器
Encoder_Integral +=Encoder; //===积分出位移 积分时间：10ms
//Encoder_Integral=Encoder_Integral-Movement; //===接收遥控器数据，控制前进后退
if(Encoder_Integral>10000) Encoder_Integral=10000; //===积分限幅
if(Encoder_Integral<-10000) Encoder_Integral=-10000; //===积分限幅
Velocity=Encoder*Velocity_Kp+Encoder_Integral*Velocity_Ki; //===速度控制
return Velocity;
}
/**************************************************************************
函数功能：转向控制 修改转向速度，请修改Turn_Amplitude即可
入口参数：左轮编码器、右轮编码器、Z轴陀螺仪
返回 值：转向控制PWM
作 者：平衡小车之家
**************************************************************************/
int turn(int encoder_left,int encoder_right,float gyro)//转向控制
{
static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count;
float Kd=1.3;
//=============转向PD控制器=======================//
Turn=-gyro*Kd; //===结合Z轴陀螺仪进行PD控制
return Turn;
}
int EXTI9_5_IRQHandler(void)
{
int PWM_out;
if(EXTI_GetITStatus(EXTI_Line5)!=0)//一级判定
{
if(PBin(5)==0)//二级判定
{
EXTI_ClearITPendingBit(EXTI_Line5);//清除中断标志位
//进入中断首先读取数据----编码器数据和角度数据
Encoder_Left=-Read_Encoder(2); //===读取编码器的值
Encoder_Right=Read_Encoder(4); //===读取编码器的值
mpu_dmp_get_data(&Pitch,&Roll,&Yaw); //角度
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //陀螺仪
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //加速度
Balance_Pwm =balance(Pitch,gyroy); //===角度环
Velocity_Pwm=velocity(Encoder_Left,Encoder_Right); //===速度环PID控制 记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点
Turn_Pwm =turn(Encoder_Left,Encoder_Right,gyroz); //===转向环PID控制
PWM_out=Balance_Pwm - Balance_Kp*Velocity_Pwm;//最终输出
Moto1=PWM_out+Turn_Pwm; //===计算左轮电机最终PWM
Moto2=PWM_out-Turn_Pwm; //===计算右轮电机最终PWM
Xianfu_Pwm(); //===PWM限幅
Set_Pwm(Moto1,Moto2); //===赋值给PWM寄存器
}
}
return 0;
}
注意：PWM_out=Balance_Pwm - Balance_Kp*Velocity_Pwm;//最终输出
由于是串行PID，因此速度环的输出就是直立环的输入，如上面的框图和整合表达式一样。
基于串级PID的整合版姿态控制
DJI GM6020电机PID控制程序
float pid_calc(pid_struct_t *pid, float ref, float fdb)
{
pid->ref = ref;
pid->fdb = fdb;
pid->err[1] = pid->err[0];
pid->err[0] = pid->ref - pid->fdb;
pid->p_out = pid->kp * pid->err[0];
pid->i_out += pid->ki * pid->err[0];
pid->d_out = pid->kd * (pid->err[0] - pid->err[1]);
LIMIT_MIN_MAX(pid->i_out, -pid->i_max, pid->i_max);
pid->output = pid->p_out + pid->i_out + pid->d_out;
LIMIT_MIN_MAX(pid->output, -pid->out_max, pid->out_max);
//printf("pid->output = %f", pid->output);
return pid->output;
}
管道机器人姿态控制代码（位置模式）
float pid_calc(pid_struct_t *pid, float Angle, float Gyro_Y)
{
}
target speed1 = rc.ch4/3.5; //CH1和CH4通道混控
target speed2 = rc.ch4/3.5 - 2*pid->output;
FOC平衡小车
参考：稚晖君
FOC（Field-Oriented Control），直译是磁场定向控制，也被称作矢量控制（VC，Vector Control），是目前无刷直流电机（BLDC）和永磁同步电机（PMSM）高效控制的最优方法之一。
FOC分类
有感FOC（依靠传感器获取电机位置）
无感FOC （需要的算法更复杂，但机械安装简单）
（BLDC：Brushless Direct Current Motor）
一般无刷直流电机原理视频上都说需要知道转子位置才能准确的控制定子线圈产生对应的磁场。
参考：浅析无刷电机之FOC第5部分
当然也有电调这种无感无刷驱动器，只是不适合低速带载。
解答1：（这估计是电调的原理）
直流无刷电机为获得转子当前位置，需要采用某种转子位置检测环节。
1.在有位置传感器的系统中，转子位置的检测是通过一系列霍尔效应传感器来实现的。
2.但位置传感器的存在诸多缺点；
3.因此，使用无感直流无刷电机控制，具有其一定的优势。
解决办法：反电动势法
无感直流无刷电机的转子位置检测分析
对于无位置传感器的直流无刷电机，必须通过一定的方法检测转子位置信息才能准确换相。反电动势法是其中最成熟和应用最广泛的位置检测方法。在六步换相控制中，每一个换相周期，将有一相绕组处于不导通状态。因此通过检测第三相反电动势信号，可检测到转子磁极在该绕组经过的时刻。如上图所示，在AB绕组通电时，应检测C相反电动势电压。
解答2：
如果位置不匹配，在不同的启动位置，有可能出现电机实际转向与期待转向完全相反的危险情况。
因此，转子位置的检测十分重要。
检测转子位置通常是有霍尔的做法，还有一种无转子传感器的，通常是通过反电动势过零检测。
解答3：
由于BLDC电机中没有电刷，因此换向是电子控制的。为了使电机旋转，必须顺序地给定子绕组通电，并且必须知道转子的位置（即转子的北极和南极）才能精确地给一组特定的定子绕组通电。
参考：浅析无刷电机之FOC第5部分
FOC当然也是要根据转子位置来“换向”，既然是矢量控制，那么就可以非常方便、随意的来进行控制了。
依靠Park变换获得“转子位置”
Park变换负责把两相静止坐标系α和β，变换成两相旋转坐标系。反过来就是Park逆变换或者叫反Park变换。变换的结果就是我们只要给定旋转坐标系的电流，不用关注转子角度就可以让无刷电机持续的旋转起来了（就和有刷电机一样，只要通电就行了）。 对于用户来说就只需要给定电流大小，因此效果就和直流有刷一样，给定电流就可以转，给的越大转得越快，反着给电流就反转。所以也有不严谨的说法是FOC把无刷变成了有刷。
无感FOC转子位置来源于估算器
灯哥FOC1.0的板子，没有电流采集电路，不用编码器实现开环速度控制的原理吗，因为之前看到的无刷电机驱动原理都提到了要知道转子的位置或者所在扇区才能用六步换向法让电机转起来。
解答：
原来这个驱动芯片就有电流反馈功能
SENSE1、2：连接到该引脚的感测电阻为桥1和2的电机电流控制提供反馈。
但灯哥这里除了三个IN引脚，单片机之和EN引脚连接了？？？？
无刷直机电机的驱动方式按不同类别可分多种驱动方式，它们各有特点。
按驱动波形：
方波驱动，这种驱动方式实现方便，易于实现电机无位置传感器控制;
正弦驱动：
这种驱动方式可以改善电机运行效果，使输出力矩均匀，但实现过程相对复杂。同时，这种方法又有SPWM和SVPWM(空间矢量PWM)两种方式，SVPWM的效果好于SPWM。
采用FOC可以克服六步换向法产生的速度和扭矩波动的问题
（在六步换向法中，定子磁场和转子磁场之间的角度在60度-120度之间波动，因此导致电机速度和扭矩波动）
下图左侧和右侧分别是BLDC六步换向和PMSM FOC控制时，转子磁场（蓝色箭头）和定子磁场的关系。
在六步换向法中，定子磁场和转子磁场之间的角度在60度-120度之间周期性的变化；
在FOC中，定子磁场和转子磁场之间始终保持正交，所能输出的扭矩最大化。
磁场定向控制大大降低了系统响应的波纹，使电机运行更加平稳。
1.FOC需要更复杂的控制算法
FOC使用的是交流电而不是直流电来控制电机
参考：稚晖君
控制框图包含
1.Clarke/Park 正变换和逆变换
2.正交量iq和id的两路PI控制器
在FOC控制中主要用到三个PID环，从内环到外环依次是：电流环、速度环、位置环。
也就是说：我们通过电流反馈来控制电机电流（扭矩） -> 然后通过控制扭矩来控制电机的转速 -> 再通过控制电机的转速控制电机位置。
电流环控制框图
速度环：
位置环：
可以控制电机旋转到某个精确的角度并保持
参考：CSDN
SVPWM出现在SPWM之后，是针对电机负载对SPWM的做出改良后的技术。
1. SVPWM全称Space Vector Pulse Width Modulation，意为空间矢量脉冲宽度调制，简称空间矢量脉宽调制。矢量控制和直接转矩控制变频器均基于SVPWM技术。
2. SVPWM是由三相功率逆变器的六个功率开关元件组成的特定开关模式产生的脉宽调制波，能够使输出电流波形尽可能接近于理想的正弦波形。
3. SVPWM技术与SPWM相比较，绕组电流波形的谐波成分小，使得电机转矩脉动降低，旋转磁场更逼近圆形，而且使直流母线电压的利用率有了很大提高，且更易于实现数字化。
SVPWM算法的目的就是使用三相桥的开关状态把在空间中旋转的U(t)矢量表示出来
想要让BLDC电机转子旋转并获得最大力矩，那就让定子线圈产生的合磁场呈现旋转的姿态！
要得到一个恒定大小的旋转磁场，可以先来得到一个恒定大小的旋转电压矢量。如下图：
我们可以通过互差120度，大小随着时间按正弦规律变化的3个分矢量来合成一个大小不变旋转的总矢量。
UA、UB、UC
如下图，将这三个分量加以一定的相位的正弦变化特性便能产生一个旋转合磁场
下图不太准确，意义不大仅用于引发想象
想控制这三个分量得控制三相功率逆变器的三组MOS管（a、b、c三组）开闭获得
1.六个功率开关元件其实就是6个MOS管
2.用 1 or 0 代表a、b、c三组MOS管的开闭情况；
3.只要某组MOS管开闭时间合适，就可以获得一个SPWM波，使得其对应的线圈获得一个正弦变化的电压输入，从而产生一个正弦变化的磁场
那么a、b、c三组MOS管如何配合呢？
1.我们先把360度分为6个扇区，并把UA、UB、UC（对应U4、U2、Uc）三个分量放进去，
2.a、b、c三组MOS管全部可能组合共有八个
6个非零矢量：
Ul(001)、U2(010)、U3(011)、U4(100)、U5(101)、U6(110)、
和两个零矢量 ：
U0(000)、U7(111)，（不产生转矩）
把8种情况产生的磁场的相位关系放在扇区图中：
上图中，6个非零矢量幅值相同，相邻的矢量间隔60度。两个零矢量幅值为零，位于中心。
先拿扇区I来分析
可以通过公式算出来此时为了生成Uref 对应的U4和U6的占空比（Tn/T）
下面就是产生此时需要的方波，
1.为了减少开关次数
SVPWM 调制方案中，零矢量的选择是最具灵活性的，适当选择零矢量，可最大限度地减少开关次数，尽可能避免在负载电流较大的时刻的开关动作，最大限度地减少开关损耗。
**疑问：**为啥只用两个分量去合成Uref，不考虑第三个分量的贡献了？？？？？？？？？？？？？？
1.上图中的T0/2等指的是两竖直虚线之间的耗时
2.上图A、B、C对应的是三组MOS管的高低电平，不是指三组线圈的PWM波形or占空比
3.A、B、C三组MOS管的高低电平（1 or 0）便可对应U1-U6矢量的发力，如下图所示：
FOC算法，框图如下：
上图中的第7部分就是SVPWM，其作为FOC最终的执行部分，接收FOC传来的Uα 和Uβ，然后通过上面的过程转换成MOS开关的控制信号，控制定子绕组产生旋转磁场。
FOC与SVPWM的对接
1.首先接收FOC传来的Uα 和Uβ，做合成矢量Uref所处扇区N 的判断 （推测是和转子当前位置呼应）
（推测：SVPWM是根据FOC提供的包含编码器角度信息的参数Uα 和Uβ，进行实时计算的）
2.然后基本矢量作用时间计算与三相 PWM 波形的合成
FOC与SVPWM的对接就变成了，先根据N=4C+2B+A判断合成矢量所在扇区，然后查表2-4得出两非零矢量的作用时间，最后得出三相开关管PWM波的占空比，查表让算法更高效。
taon、tbon、tcon的波形呈马鞍形，如下：
