三环控制和PID-以大疆M3508、M2006为例
三环控制和PID在电机的应用
前言：
最近用到了大疆的直流无刷（BLDC）减速电机M3508和M2006。做RoboMaster比赛的同学应该对它们很熟悉，这两款电机质量都不错，配套电调C620、C610功能强大，应用场景广泛。当然价格不算低。
我作为第一次接触电机控制的新手，在搜索PID和三环控制资料的时候常常得到的是一些理论论述，而且千篇一律。虽然PID是较为简单的控制算法，新人上手难度还是有些大。那么我就彻彻底底地回顾一下搭建最简单的电机控制算法的流程。提供一个新的理解视角。
本文依托大疆官方M2006电机例程，其与M3508电机配套例程在CAN通信驱动、PID等核心部分基本相似甚至可以直接替换（M2006例程里的一些文件注释写的是3508）。最大的不同就是M3508使用了FreeRTOS实时系统，这一点并不方便我们的学习。而2006就是裸机前后台，非常简单明晰。
例程均是基于Keil 、STM32F429（我使用407）、HAL库
例程下载：M2006例程、M3508例程
2021/7/23：
第一次接触这两款电机或者CAN通信的朋友可以看一下这篇文章：
大疆M3508、M2006必备CAN总线知识与配置方法
0x00 PID到底怎么部署到电机上？
我们都知道PID应用广泛，效果良好，在网上能搜到大量生动形象的文章。这些文章讲述了比例、积分、微分有什么用处和缺陷，什么是死区，并且配有动态曲线图来展示调参效果。但是举的例子往往是这样：一个水缸要固定水位，加水量是输出，反馈是水位之差。我当时看过好多类似文章之后还是不明白怎么把PID应用于更复杂的系统,比如电机控制。
答案是 拆分 复杂系统，运用多次控制算法层层递进
比如在电机上，常用的就是三环控制：电流环、速度环、位置环。
三环层层递进并且具有因果关系。
这个关系非常好理解，通过电磁学知识我们知道电机能转是因为有电流产生磁场。所以，电流是电机转动的根本原因，也就是：
中学物理也讲过，（角）速度是描述运动的物理量，包括方向、大小。如果希望控制速度，比如定速转动，那就也要控制电流。再进一步，运动能改变物体位置，如果想确定达到某个位置那就控制速度。然而控制速度归根结底是控制电流。得出：
得出结论：如果要控制位置，那就要控速度，如果要控速度，就必须控制电流。
位置环的输出是速度值，速度环的输出是电流值。而他们的反馈（输入）都是电机的实时数据：实时速度，实时位置。
就这样，最末端的电机^ 1反馈出三个值，依次给最外面的位置，中间的速度，里面的电流。形成了三个闭合的环。如下图
不难发现，很多例子像水缸加水和直升机定高，他们都是直接的位置环，输出量是加水的速度和向上飞的升力。没有底层的两环。
接下来我们以M2006例程为例子看一看怎么实现上述过程。
0x01 剖析例程--多少行代码能实现电机控速？
先说明，M3508支持PWM和CAN两种控制方式，其中PWM可以直接控速但是没有数据反馈。M2006仅支持CAN总线。两种电机的总线编码都是从0x200开始，驱动文件
bsp_can.c和can.c几乎一样可以替换。本文仅介绍PID的部署实现，不涉及CAN通信内容。
总结下来，PID就是一个结构体三个函数。
一个结构体PID_TypeDef:
typedef struct _PID_TypeDef
{
float target; //目标值
float kp; //比例系数
float ki; //积分系数
float kd; //微分系数
float measure; //测量值
float err; //误差
float last_err; //上次误差
float pout; //比例项
float iout; //积分项
float dout; //微分项
float output; //本次输出
float last_output; //上次输出
float MaxOutput; //输出限幅
float IntegralLimit; //积分限幅
float DeadBand; //死区（绝对值）
float Max_Err; //最大误差
void (*f_param_init) //参数初始化
void (*f_pid_reset) //pid三个参数修改
float (*f_cal_pid) //pid计算
}PID_TypeDef;
注意：为了简单明晰，我删去了原文件里一些用不到的参数，最后三个函数指针的参数列表也被删除。其中不乏很重要的计算周期，但是在简单的控制下，时间间隔是可以忽略的。
这个结构体的核心就是三个系数，调参调的也就是这三个。
target目标值和output输出值还有measure反馈值，再就是err和last_err两个误差。
其他的一些限幅，和死区[^2]无非就是防止问题发生的补丁。
三个函数：f_param_init、f_pid_reset、f_cal_pid
名副其实，分别是结构体的参数初始化，三个系数修改、最重要的输出值计算。
static float pid_calculate(PID_TypeDef* pid, float measure)
{
pid->measure = measure; //目标速度
pid->last_err = pid->err; //更新前一次误差
pid->err = pid->target - pid->measure; //计算当前误差
pid->last_output = pid->output;
if((ABS(pid->err) > pid->DeadBand)) //是否进入死区，如果进入则直接跳过，返回上一次的output结果
{
pid->pout = pid->kp * pid->err;
pid->iout += (pid->ki * pid->err); //注意是加等于
pid->dout = pid->kd * (pid->err - pid->last_err);
//积分是否超出限制
if(pid->iout > pid->IntegralLimit)
pid->iout = pid->IntegralLimit;
if(pid->iout < - pid->IntegralLimit)
pid->iout = - pid->IntegralLimit;
//pid输出和
pid->output = pid->pout + pid->iout + pid->dout;
//限制输出的大小
if(pid->output>pid->MaxOutput)
{
pid->output = pid->MaxOutput;
}
if(pid->output < -(pid->MaxOutput))
{
pid->output = -(pid->MaxOutput);
}
}
return pid->output;
}
这是计算函数，我们要把实时测量值measure传入函数，用来更新误差，产生新的输出。
measure是电机发来的速度或者位置数据，在CAN中断函数里自动更新。
在这里我们用误差之差代替微分，并且对积分项和输出结果进行了限幅。这些幅度都是自定义的。
得出流程如下：
0x02 一个小例子
M2006例程相对M3508虽然简单，但还是包括了一些上位机通信控制的代码。
还是简单明晰的原则。下面是一个最最简单的demo框架。
#include "pid.h"
#define NUM_OF_MOTOR 1
PID_TypeDef moto_pid[NUM_OF_MOTOR];
float set_spd;
int main(){
init_all();
for(int i=0;i<NUM_OF_MOTOR;i++){
pid_init(&moto_pid[i]);//把结构体里的函数指针赋值，三个函数
moto_pid[i].f_param_init(&moto_pid[i],PID_Speed,16384,5000,10,0,8000,0,1.5,0.1,0);
//确定结构体内的参数，幅值，死区大小，PID系数
}
for(;;){
get_set_spd_from_USART();//从串口得到设定值set_spd
for(int i=0; i<NUM_OF_MOTOR; i++)
{
motor_pid[i].target = set_spd; //更新目标值
motor_pid[i].f_cal_pid(&motor_pid[i],measure[i]); //PID计算。measure由CAN中断更新
}
set_moto_current(&hcan1,motor_pid[0].output, //将PID的计算结果通过CAN发送到电机
motor_pid[1].output,
motor_pid[2].output,
motor_pid[3].output);
HAL_Delay(10);//延时10ms控制周期
}
return 0;
}
到此为止，一个简单的PID电机控制就做好了。如果写的紧凑一点，代码可能不超过50行，还是非常简单的。
注意事项：
我选用的是F407，带有FPU浮点运算单元的MCU。尽量选择CM4，这样浮点运算会快很多。
实际使用的时候要把Keil的option里面target一栏里floating point Hardware选成Single Precision
演示视频在微信视频号上：扫码查看微信文章底部的视频
0x03 总结
以上就是最简单的电机控制部署，本着怎么简单怎么来的原则，希望能帮助朋友们节约一些学习时间。
除了官方例程，我还有自己移植的基于F407的版本，去除例程的无用部分，加入了串口的通信解析，可以比较方便的调参，在线修改速度、pid参数等。有需要的可以加微信公众号直接问我要。
关注嵌入式、电机控制的朋友也可以添加公众号，最近会更新有关上位机通信，CAN通信等电机控制相关内容
大一技术新人，如果发现文中错误请各位大佬不吝赐教，一定指出，如果有意见或建议同样欢迎。谢谢。
欢迎转载，请注明原文链接：(https://www.cnblogs.com/huxiaoan/p/14727970.html)
