【电机控制基础】PID控制器

发表于 更新于 分类于 本文字数： 12k 阅读时长 ≈ 11 分钟
本文介绍了一种普遍应用于自动控制系统的反馈控制器——PID控制，包含其基本定义、工作原理、调参流程，以及电机角度为控制对象的例码。

基本定义

PID控制器是一种广泛应用于自动控制系统的反馈控制器。它根据偏差，即设定值与实际值之间的差异来计算控制量，并通过比例、积分和微分三种操作调整控制系统，以使偏差最小化。

工作原理

PID控制器的核心思想是利用比例、积分、微分三种方式对误差进行调整，其中：

  • 比例（Proportional）：根据当前的误差来决定控制量。当前误差越大，比例控制产生的调整信号也越大。
  • 积分（Integral）：根据误差的累积量进行调整，消除长期累积的偏差，从而避免稳态误差。
  • 微分（Derivative）：根据误差的变化率来预测未来的误差变化，减少系统的过冲和振荡，使系统输出更平缓。

PID基于使用用途可分为连续PID和离散PID两类。

  • 连续PID：适用于理论计算和连续控制系统中。它基于时间连续的误差值进行反馈控制。

    \[u(t)=K_p \cdot e(t) + K_i \int_0^t e(\tau)d\tau + K_d\frac{de(t)}{dt} = K_p[e(t)+\frac{1}{T_i}\int_0^t e(\tau)d\tau + T_d\frac{de(t)}{dt}]\]

    其中：\(e(t)\)是当前时刻的误差，\(K_p\)是比例增益，\(K_i\)是积分增益，\(K_d\)是微分增益

  • 离散PID：由于计算机和嵌入式系统通常是离散的，对连续PID公式进行修改，通过离散时间步长对误差进行处理。假设采样时间间隔为\(T\)，则在\(k\)时刻偏差为\(e(k)\)，积分为\(T\cdot(e(k)+e(k-1)+...+e(0))\)，微分为\((e(k)-e(k-1))/T\). 得到离散化的PID公式

    \[u(k)=K_p[e(k)+\frac{T}{T_i}\sum_{n=0}^k e(n) + \frac{T_d}{T}(e(k)-e(k-1))]=K_p \cdot e(k) + \frac{K_pT}{T_i}\sum_{n=0}^k e(n) + \frac{K_pT_d}{T}(e(k)-e(k-1))\]

    由以上公式可知，比例系数为\(K_p\)，积分系数为\(K_p\cdot T/T_i\)，微分系数为\(K_p \cdot T_d/T\)。

  • 离散增量式PID：上面给出的离散PID公式也称位置式PID公式，它要求依次累加历史误差。由于执行器输入是绝对量，相邻时刻的输出跳变可能较大，且输出饱和时积分继续累积会造成严重超调。从位置式PID公式出发，可推导出增量式PID公式，每周期计算输出的增量。我们仅需将\(k\)时刻和\(k-1\)时刻的位置式PID公式相减即可

    \[u(k-1)=K_p \cdot e(k-1) + \frac{K_pT}{T_i}\sum_{n=0}^{k-1} e(n) + \frac{K_pT_d}{T}(e(k-1)-e(k-2))\]

    \[\Delta u(k) = u(k)-u(k-1) = K_p[e(k)-e(k-1)] + \frac{K_pT}{T_i}e(k) + \frac{K_pT_d}{T}[e(k)-2e(k-1)+e(k-2)]\]

    因此\(k\)时刻的PID输出为\(u(k-1) + \Delta u(k)\).

电机角度控制

硬件配置包括：一个N20有刷直流电机(12V)、STM32C011F6U6单片机、MA782绝对式角度传感器
为了实现电机的角度控制，该控制系统包含位置环和速度环。位置环的反馈信号是电机输出轴同步电位器信号的模数转换，速度环的反馈信号是位置值进行微分的结果。

如何理解PID系统的输出？它是否具有确切的物理含义？

我们先来看PID输出在控制领域的标准定义——PID的输出代表对执行器施加的控制信号，它代表了系统需要多大的力、速度、电流或阀门开度，来纠正当前测量值与目标值之间的误差，使系统达到并保持设定状态。
因此PID的输出当然可以有物理含义，只是这取决于我们把它定义成什么量。对于一个直流有刷电机，我们通过PWM调制占空比来改变电枢平均电压，进而改变电流与电磁转矩，使转速跟随。

下面附上有刷电机的电气与机械模型：
电枢电压方程（其中\(K_e\omega\)项为反电动势，\(\omega\)为电机转速） \[V = Ri + L\frac{di}{dt} + K_e\omega\] 机械转矩方程（其中\(J\)为转动惯量，\(K_ti\)项为电磁转矩，\(\tau_{load}\)为负载转矩） \[J\frac{d\omega}{dt} + B\omega = K_ti - \tau_{load}\]

从电枢电压公式可以看出，电压实质上由电流\(i\)和转速\(\omega\)两个变量决定。而在机械方程中，电流\(i\)直接决定了系统提供的总转矩。因此在更规范的伺服系统中，常使用电流环作为内环，速度环作为外环的阻抗控制系统。（这里先挖一个坑，后续单独开一帖详细介绍）

PID控制的一大优点是无需精确的数学物理模型也能够修复误差，因此即便不显式补偿电气环节，PID也能通过闭环误差自己应对\(R\)、\(L\)和各种负载扰动。针对我们最开始提到的速度内环+位置外环系统，不妨直接将速度环的输出设定为归一化的PWM命令 \(u\in[-1,1]\)，直接驱动电机转速的变化。

总结：

  • 位置外环
    输入：\(\theta_d-\theta\) 输出：\(\omega_{ref}\)（速度指令） 限幅：\(|\omega_{ref}| \leq \omega_{max}\)
  • 速度内环
    输入：\(\omega_{ref}-\omega\) 输出：\(u\in[-1,1]\)（占空比） 限幅：\(|u| \leq 1\)

浮点位置式PID实现例程

以下是位置式PID的c语言实现：

  • 头文件pid_controller.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
#ifndef __PID_CONTROLLER_H
#define __PID_CONTROLLER_H

#ifdef __cplusplus
extern "C" {
#endif

// Includes
#include "stm32c0xx_hal.h"
#include "stdlib.h"

typedef struct PID_Controller_ PID_Controller;
typedef struct PID_CtrlData_ PID_CtrlData;

struct PID_Controller_
{
    PID_CtrlData* dat;
    float32_t error; // ek
    float32_t e[3];  // ep, ei, ed
    float32_t* setValue;  // Desired value
    float32_t output_;    // Output before limiting
    float32_t* output;    // PID output
};
    
struct PID_CtrlData_
{
    float32_t k[3];  // PID param: Kp, Ki, Kd
    float32_t l[3];  // Limits: integral_limit, min_output, max_output
};

// Constructor and Destructor
PID_Controller* new_PID_Controller(void);
void delete_PID_Controller(PID_Controller* ctrl);

// Initialization and Configuration
HAL_StatusTypeDef PID_Controller_Init(PID_Controller* ctrl, PID_CtrlData* dat, 
                                      float32_t* setValue, float32_t* output);
// Core Calculation
HAL_StatusTypeDef PID_Controller_Calc(PID_Controller* ctrl, float32_t actValue);

// Parameter Settings
HAL_StatusTypeDef PID_Controller_SetD(PID_Controller* ctrl, float32_t setValue);
HAL_StatusTypeDef PID_Controller_SetK(PID_Controller* ctrl, 
                                      float32_t kp, float32_t ki, float32_t kd);
HAL_StatusTypeDef PID_Controller_SetL(PID_Controller* ctrl, 
                                      float32_t l_int, float32_t l_min, float32_t l_max);
HAL_StatusTypeDef PID_Controller_Reset(PID_Controller* ctrl);

#ifdef __cplusplus
}
#endif

#endif

  • 源文件pid_controller.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
#include "pid_controller.h"

PID_Controller* new_PID_Controller(void)
{
    PID_Controller* pObj = (PID_Controller*)calloc(1, sizeof(PID_Controller));
    return pObj;
}

void delete_PID_Controller(PID_Controller* ctrl)
{
    free(ctrl->dat);
    free(ctrl->setValue);
    free(ctrl->output);
    free(ctrl);
}

HAL_StatusTypeDef PID_Controller_Init(PID_Controller* ctrl, PID_CtrlData* dat, 
                                      float32_t* setValue, float32_t* output)
{
    if(ctrl == NULL)  return HAL_ERROR;
    if(ctrl->dat != NULL)
        free(ctrl->dat);
    if(dat == NULL)
        ctrl->dat = (PID_CtrlData*)calloc(1, sizeof(PID_CtrlData));
    else
    	ctrl->dat = dat;
    
    if(ctrl->setValue != NULL)
        free(ctrl->setValue);
    if(setValue == NULL)
        ctrl->setValue = (float32_t*)calloc(1, sizeof(float32_t));
    else
    	ctrl->setValue = setValue;
    
    if(ctrl->output != NULL)
        free(ctrl->output);
    if(output == NULL)
        ctrl->output = (float32_t*)calloc(1, sizeof(float32_t));
    else
        ctrl->output = output;
    
    return HAL_OK;
}

HAL_StatusTypeDef PID_Controller_Calc(PID_Controller* ctrl, float32_t actValue)
{
    if(ctrl == NULL || ctrl->setValue == NULL || 
       ctrl->dat == NULL || ctrl->output == NULL)
        return HAL_ERROR;
    
    ctrl->error = *ctrl->setValue - actValue;
    
    ctrl->e[2] = ctrl->error - ctrl->e[0];  // ed
    ctrl->e[1] += ctrl->error;              // ei
    ctrl->e[0] = ctrl->error;               // ep
    
    // anti-windup
    if(ctrl->e[1] > ctrl->dat->l[0])
        ctrl->e[1] = ctrl->dat->l[0];
    if(ctrl->e[1] < -ctrl->dat->l[0])
        ctrl->e[1] = -ctrl->dat->l[0];
    
    ctrl->output_ = ctrl->dat->k[0] * ctrl->e[0] + 
        			ctrl->dat->k[1] * ctrl->e[1] + 
        			ctrl->dat->k[2] * ctrl->e[2];
    
    // output limit
    if(ctrl->output_ < ctrl->dat->l[1])
        *ctrl->output = ctrl->dat->l[1];
    else if(ctrl->output_ > ctrl->dat->l[2])
        *ctrl->output = ctrl->dat->l[2];
    else
        *ctrl->output = ctrl->output_;
    
    return HAL_OK;
}

HAL_StatusTypeDef PID_Controller_SetD(PID_Controller* ctrl, float32_t setValue)
{
    if(ctrl == NULL || ctrl->setValue == NULL)
        return HAL_ERROR;
    *ctrl->setValue = setValue;
    return HAL_OK;
}

HAL_StatusTypeDef PID_Controller_SetK(PID_Controller* ctrl, 
                                      float32_t kp, float32_t ki, float32_t kd)
{
    if(ctrl == NULL || ctrl->dat == NULL)
        return HAL_ERROR;
    ctrl->dat->k[0] = kp;
    ctrl->dat->k[1] = ki;
    ctrl->dat->k[2] = kd;
    return HAL_OK;
}

HAL_StatusTypeDef PID_Controller_SetL(PID_Controller* ctrl, 
                                      float32_t l_int, float32_t l_min, float32_t l_max)
{
    if(ctrl == NULL || ctrl->dat == NULL)
        return HAL_ERROR;
    ctrl->dat->l[0] = l_int;
    ctrl->dat->l[1] = l_min;
    ctrl->dat->l[2] = l_max;
    return HAL_OK;
}

HAL_StatusTypeDef PID_Controller_Reset(PID_Controller* ctrl)
{
    if(ctrl == NULL)
        return HAL_ERROR;
    ctrl->error = 0;
    ctrl->output_ = 0;
    for(int i=0; i<3; i++)
        ctrl->e[i] = 0;
    return HAL_OK;
}

定点位置式PID实现例程

为了进一步提高运算效率，这里重写了一个全定点数（Q15）的位置式PID模板。

  • pid_controller_q15.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
#ifndef __PID_CONTROLLER_Q15_H
#define __PID_CONTROLLER_Q15_H

#ifdef __cplusplus
extern "C" {
#endif

#include "stm32c0xx_hal.h"
#include "arm_math.h"      // q15_t, q31_t
#include <stdlib.h>
#include <string.h>
#include <stdint.h>

typedef struct PID_CtrlData_Q15_ PID_CtrlData_Q15;
typedef struct PID_Controller_Q15_ PID_Controller_Q15;

struct PID_CtrlData_Q15_
{
    q31_t k[3];     // Kp, Ki, Kd (Q15)
    q15_t l[3];     // Limits in Q15 scaling but stored in q31_t: {I_limit, out_min, out_max}
};

struct PID_Controller_Q15_
{
    PID_CtrlData_Q15* dat;

    q31_t error;
    q31_t e[3];    // ep = e(k), ei = integral e, ed = e(k)-e(k-1)
    q31_t output_;  // Output before output limiting, Q15 stored in q31_t

    q15_t* setValue; // Desired value pointer (Q15)
    q15_t* output;   // Output pointer (Q15)
};

// Constructor and Destructor
PID_Controller_Q15* new_PID_Controller_Q15(void);
void delete_PID_Controller_Q15(PID_Controller_Q15* ctrl);

// Init and Calc
HAL_StatusTypeDef PID_Controller_Q15_Init(PID_Controller_Q15* ctrl, PID_CtrlData_Q15* dat,
                                          q15_t* setValue, q15_t* output);

HAL_StatusTypeDef PID_Controller_Q15_Calc(PID_Controller_Q15* ctrl, q15_t actValue);

// Parameter Settings
HAL_StatusTypeDef PID_Controller_Q15_SetD(PID_Controller_Q15* ctrl, q15_t setValue);
HAL_StatusTypeDef PID_Controller_Q15_SetK(PID_Controller_Q15* ctrl, 
                                          float32_t kp, float32_t ki, float32_t kd);
HAL_StatusTypeDef PID_Controller_Q15_SetL(PID_Controller_Q15* ctrl, 
                                          q15_t l_int, q15_t l_min, q15_t l_max);

#ifdef __cplusplus
}
#endif

#endif

  • pid_controller_q15.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
#include "pid_controller_q15.h"

PID_Controller_Q15* new_PID_Controller_Q15(void)
{
    PID_Controller_Q15* pObj = (PID_Controller_Q15*)calloc(1, sizeof(PID_Controller_Q15));
    return pObj;
}

void delete_PID_Controller_Q15(PID_Controller_Q15* ctrl)
{
    free(ctrl->dat);
    free(ctrl->setValue);
    free(ctrl->output);
    free(ctrl);
}

HAL_StatusTypeDef PID_Controller_Q15_Init(PID_Controller_Q15* ctrl, PID_CtrlData_Q15* dat,
                                          q15_t* setValue, q15_t* output)
{
    if(ctrl == NULL)  
        return HAL_ERROR;
    if(ctrl->dat != NULL)
        free(ctrl->dat);
    if(dat == NULL)
        ctrl->dat = (PID_CtrlData_Q15*)calloc(1, sizeof(PID_CtrlData_Q15));
    else
    	ctrl->dat = dat;
    
    if(ctrl->setValue != NULL)
        free(ctrl->setValue);
    if(setValue == NULL)
        ctrl->setValue = (q15_t*)calloc(1, sizeof(q15_t));
    else
    	ctrl->setValue = setValue;
    
    if(ctrl->output != NULL)
        free(ctrl->output);
    if(output == NULL)
        ctrl->output = (q15_t*)calloc(1, sizeof(q15_t));
    else
        ctrl->output = output;
    
    return HAL_OK;
}

HAL_StatusTypeDef PID_Controller_Q15_SetD(PID_Controller_Q15* ctrl, q15_t setValue)
{
    if (ctrl == NULL || ctrl->setValue == NULL) 
        return HAL_ERROR;
    *ctrl->setValue = setValue;
    return HAL_OK;
}

HAL_StatusTypeDef PID_Controller_Q15_SetK(PID_Controller_Q15* ctrl, 
                                          float32_t kp, float32_t ki, float32_t kd)
{
    if (ctrl == NULL || ctrl->dat == NULL) 
        return HAL_ERROR;
    
    // 1.0 -> 0x8000
    // Proportional gain
    if(kp > 0xFFFF)
        ctrl->dat->k[0] = 0x7fffffff;
    else if(kp < -0xFFFF)
        ctrl->dat->k[0] = -0x7fffffff;
    else
        ctrl->dat->k[0] = kp * 0x8000;   
    
    // Integral gain
    if(ki > 0xFFFF)
        ctrl->dat->k[1] = 0x7fffffff;
    else if(ki < -0xFFFF)
        ctrl->dat->k[1] = -0x7fffffff;
    else
        ctrl->dat->k[1] = ki * 0x8000;   
    
    // Derivative gain
    if(kd > 0xFFFF)
        ctrl->dat->k[2] = 0x7fffffff;
    else if(kd < -0xFFFF)
        ctrl->dat->k[2] = -0x7fffffff;
    else
        ctrl->dat->k[2] = kd * 0x8000;
    
    return HAL_OK;
}

HAL_StatusTypeDef PID_Controller_Q15_SetL(PID_Controller_Q15* ctrl, 
                                          q15_t l_int, q15_t l_min, q15_t l_max)
{
    if (ctrl == NULL || ctrl->dat == NULL) 
        return HAL_ERROR;

    ctrl->dat->l[0] = l_int;   // integral limit
    ctrl->dat->l[1] = l_min;   // out_min
    ctrl->dat->l[2] = l_max;   // out_max

    return HAL_OK;
}

HAL_StatusTypeDef PID_Controller_Q15_Calc(PID_Controller_Q15* ctrl, q15_t actValue)
{
    if (ctrl == NULL || ctrl->dat == NULL || 
        ctrl->setValue == NULL || ctrl->output == NULL)
        return HAL_ERROR;

    ctrl->error = (q31_t)*ctrl->setValue - actValue;  // [-1,1] restored in q31

    ctrl->e[2] = ctrl->error - ctrl->e[0];  // [-2,2]
    ctrl->e[1] += ctrl->error;              // [-2,2]
    ctrl->e[0] = ctrl->error;               // [-1,1]

    if(ctrl->e[1] > ctrl->dat->l[0])  // [-1,1]
        ctrl->e[1] = ctrl->dat->l[0];
    if(ctrl->e[1] < -ctrl->dat->l[0])
        ctrl->e[1] = -ctrl->dat->l[0];

    ctrl->output_ = (((q63_t)ctrl->dat->k[0] * ctrl->e[0])	        
                     + ((q63_t)ctrl->dat->k[1] * ctrl->e[1])	    
                     + ((q63_t)ctrl->dat->k[2] * ctrl->e[2])) >> 15;
    
    if(ctrl->output_ > (q31_t)ctrl->dat->l[2])
        ctrl->output_ = (q31_t)ctrl->dat->l[2];
    else if(ctrl->output_ < (q31_t)ctrl->dat->l[1])
        ctrl->output_ = (q31_t)ctrl->dat->l[1];
	
    *ctrl->output = (q15_t)__SSAT(ctrl->output_, 16);

    return HAL_OK;
}