Control-Algorithm/demo_adrc.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
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
"""ADRC 示例"""
import numpy as np
from controller.utils import timer, matplotlib_context
from controller.siso import ADRC, ADRCConfig


# 一维被控模型 (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: ADRCConfig, 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: ADRCConfig, with_noise=True):
    # 实例化控制算法
    dt = cfg.dt
    ctrl = cfg.build()
    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: ADRCConfig, 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 = ADRCConfig(
        dt = 0.001,
        dim = 1,
        # 跟踪微分器
        r = 100,                # 快速跟踪因子
        # 扩张状态观测器
        b0 = 133,               # 被控系统系数
        delta = 0.015,          # fal(e, alpha, delta)函数线性区间宽度
        eso_beta01 = 150,       # ESO反馈增益1
        eso_beta02 = 250,       # ESO反馈增益2
        eso_beta03 = 550,       # ESO反馈增益3
        # 非线性状态反馈控制率
        nlsef_beta1 = 10,       # 跟踪输入信号增益1
        nlsef_beta2 = 0.0009,   # 跟踪输入信号增益2
        nlsef_alpha1 = 200/201, # 0 < alpha1 < 1
        nlsef_alpha2 = 201/200, # alpha2 > 1
    )
    with matplotlib_context():
        step_singnal_demo(cfg, with_noise=True)
    with matplotlib_context():
        cosine_singnal_demo(cfg, with_noise=True)