zhaohaojie1998 / Control-Algorithm

反馈跟踪控制器，控制算法工具包，ADRC自抗扰控制，PID控制，抗积分饱和PID控制，增量式PID控制，模糊FuzzyPID控制，LQR线性二次型调节器控制，启发算法控制，强化学习控制，无人机轨迹跟踪控制

Control Algorithm

一、控制算法:

v表示参考轨迹，y表示实际轨迹（被控对象输出），x表示状态（当无法得到时需设计观测器）。

1.无模型

算法名	类名	输入	输出	备注
先进PID控制 Proportion Integral Differential	PID	v、y	u	自带先进PID功能
增量式PID控制 Increment PID Control	IncrementPID	v、y	u	自带先进PID功能
自抗扰控制 Active Disturbance Rejection Control	ADRC	v、y	u	缺点：参数巨多。。。

2.基于模型

2.1基于优化（未实现）

基于模型进行优化控制

算法名	类名	输入	输出	备注
模型预测控制 Model Predictive Control	MPC	v_seq、x	u	支持非线性系统 缺点：计算慢，且需要知道未来n步的v
线性二次型调节器 Linear Quadratic Regulator	LQR	v_all、y	u	支持线性时变系统 缺点：线性，要求能控能观，必须已知v的全部轨迹信息
迭代线性二次型调节器 Iterative Linear Quadratic Regulator	ILQR

2.2基于学习（未实现）

模型用来产生训练数据

算法名	类名	输入	输出	备注
SAC-Auto算法控制 Soft Actor Critic Control	SAC	v、y	u	连续控制
DQN算法控制 Deep Q Network Control	DQN	v、y	u	离散控制，需将控制量进行编码
QMIX算法控制 QMIX Control	QMIX	v、y	u	多维离散控制，将每个维度的控制量看成一个强化学习智能体

2.3基于搜索（未实现）

模型用来评估搜索解的好坏

算法名	类名	输入	输出	备注
智能搜索算法控制 AI Search Control	PSO等小动物算法	v_seq、x	u	原理类似MPC，直接搜索u_seq，u_seq带入模型评估搜索结果，利用启发算法优化，执行u_seq[0]，下一时刻重新搜索 缺点：速度慢

3.模糊控制

算法名	类名	输入	输出	备注
模糊PID控制 Fuzzy PID Control	FuzzyPID	v、y	u	模糊规则给PID调参

二、控制器接口:

用于跟踪控制或反馈控制，即y信号（真实状态/观测）跟踪v信号（理想状态/观测），控制器输入v（或v_seq）和y（或x），输出控制量u

1.控制器输入：

输入	定义	向量情况(ndarray)	标量情况(ndarray/float)
v	参考轨迹点	shape = (dim, )	shape = (1, ) / float
v_seq	参考轨迹片段	shape = (n, dim, )	shape = (n, )
v_all	参考轨迹	shape = (t, dim, )	shape = (t, )
y	反馈信号 系统输出	shape = (dim, )	shape = (1, ) / float
x	系统状态	shape = (dim_x, )	shape = (1, ) / float

2.控制器输出：

u为形状为(dim_u, )的向量（一维ndarray），无论v、y是否为标量，输出u都是向量，即使dim_u=1时也不输出float

对于无模型的PID/ADRC控制器：dim==dim_u，对于基于模型的MPC/LQR/AI控制器：dim不一定等于dim_u

3.控制器参数：

超参为(dim, )或(dim_u, )的向量（设置成一维list或ndarray），array长度取决于公式是与v、y相乘的array还是与u相乘的array

超参设置成float时，将自动广播成(dim, )或(dim_u, )的向量

对于MPC/LQR控制器，超参为Qf、Q、R矩阵（设置成二维list或ndarray)

三、用法示例:

import numpy as np
from ctrl import PID
# 设置控制器
dim = 2 # 信号维度
cfg = PID.getConfig()(dt=0.1, dim=dim, Kp=[5,6], Ki=0.1, Kd=1) # 调参
pid = PID(cfg) # 实例化控制器
# 生成输入信号
t_list = np.arange(0.0, 10.0, dt=cfg.dt)
v_list = np.ones((len(t_list), dim)) # 需要跟踪的信号 v: (dim, )
# 被控对象
def PlantModel(y, u, dt=cfg.dt):
    ...
    return y # y: (dim, ), u: (dim, )
# 仿真
y = np.zeros(2) # 被控信号初值 (dim, )
for v in v_list:
    u = pid(v, y) # 调用控制器
    y = PlantModel(y, u) # 更新被控信号
pid.show() # 绘图输出

四、一维信号跟踪效果图:

1.PID控制算法：

参数忘了。。。

2.ADRC控制算法：

3.模糊PID控制算法：

两组对比图参数分别为Kp=5,Ki=0,Kd=0.2和Kp=5,Ki=0,Kd=0.1

五、三维信号跟踪控制：

六、Requirement:

python >= 3.9

numpy >= 1.22.3

matplotlib >= 3.5.1

scipy >= 1.7.3

scikit-fuzzy >= 0.4.2

可选：

pytorch >= 1.10.2