Control-Algorithm/demo_pid.py at master · zhaohaojie1998/Control-Algorithm · GitHub

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
"""PID 示例"""
import numpy as np
from controller.utils import timer, matplotlib_context
from controller.siso import PID, PIDConfig


# 一维被控模型 (u 为 1 维, 跟踪第 1 个 y)
class PlantModel:
    def __init__(self, dt: float, with_noise=True):
        self.dt = dt
        self.t = 0                             # 初始时刻
        self.x = np.zeros(3, dtype=np.float32) # 初始状态
        self.u = 0                             # 初始控制
        self.with_noise = with_noise           # 是否存在干扰

    def __call__(self, u: np.ndarray):
        """更新状态和观测"""
        x_new = np.zeros_like(self.x)
        # 确保u是标量，即使控制器返回数组
        u = float(np.array(u).flatten()[0])
        if self.with_noise:
            f = -25 * self.x[1] + 33 * np.sin(np.pi*self.t) + 0.01*np.random.randn()
            x_new[0] = self.x[0] + self.x[1] * self.dt + 0.001*np.random.randn()
            x_new[1] = self.x[1] + self.x[2] * self.dt + 0.001*np.random.randn()
            x_new[2] = f + 133 * u
        else:
            f = -25 * self.x[1] + 33 * np.sin(np.pi*self.t)
            x_new[0] = self.x[0] + self.x[1] * self.dt
            x_new[1] = self.x[1] + self.x[2] * self.dt
            x_new[2] = f + 133 * u

        self.x = x_new
        self.u = u
        self.t += self.dt
        return self.y

    @property
    def y(self):
        """观测方程"""
        return self.x[0]


# 一维阶跃信号跟踪Demo
@timer
def step_singnal_demo(cfg: PIDConfig, with_noise=True):
    # 实例化控制算法
    dt = cfg.dt
    ctrl = cfg.build()
    print(ctrl)
    # 生成参考轨迹
    t_list = np.arange(0.0, 10.0, dt)
    v_list = np.sign(np.sin(t_list))
    # 初始化被控对象
    plant = PlantModel(dt, with_noise)
    y = plant.y
    # 仿真
    for i in range(len(t_list)):
        # 获取参考轨迹
        v = v_list[i]
        # 控制信号产生
        u = ctrl(y, v)
        # 更新观测
        y = plant(u)
    #end
    ctrl.show(name="Step")


# 一维余弦信号跟踪Demo
@timer
def cosine_singnal_demo(cfg: PIDConfig, with_noise=True):
    # 实例化控制算法
    dt = cfg.dt
    ctrl = cfg.build("IncrementPID")
    print(ctrl)
    # 生成参考轨迹
    t_list = np.arange(0.0, 10.0, dt)
    v_list = np.cos(t_list)
    # 初始化被控对象
    plant = PlantModel(dt, with_noise)
    y = plant.y
    # 仿真
    for i in range(len(t_list)):
        # 获取参考轨迹
        v = v_list[i]
        # 控制信号产生
        u = ctrl(y, v)
        # 更新观测
        y = plant(u)
    #end
    ctrl.show(name="Cosine")


# 状态调节器Demo
@timer
def state_regulator_demo(cfg: PIDConfig, with_noise=True):
    # 实例化控制算法
    dt = cfg.dt
    ctrl = cfg.build()
    print(ctrl)
    # 初始化被控对象
    plant = PlantModel(dt, with_noise)
    y = plant.y
    # 仿真
    t_list = np.arange(0.0, 10.0, dt)
    for i in range(len(t_list)):
        # 控制信号产生
        u = ctrl(y)
        # 更新观测
        y = plant(u)
    #end
    ctrl.show(name="Regulator")


if __name__ == '__main__':
    cfg = PIDConfig(dt=0.01, dim=1, Kp=5.0, Ki=0.01, Kd=0.1)
    with matplotlib_context():
        step_singnal_demo(cfg, with_noise=True)
    with matplotlib_context():
        cosine_singnal_demo(cfg, with_noise=True)