【LADRC】对自抗扰算法的理解及公式推导（附代码）

前言

        在学习和研究自抗扰控制（LADRC）算法的过程中，我积累了一些自己的理解和心得。为了更好地整理这些知识，同时也希望与有共同兴趣的朋友们进行交流和探讨，故撰写此文章。文章中的内容基于我的个人学习经验，难免会存在理解上的偏差或错误。第一次撰写博客，非常欢迎大家批评指正，共同进步！

1、从PID控制器开始

        对于串联积分型，我们可以很容易的使用P和PD控制器完成无稳态误差、无超调量到控制。

一阶系统：

二阶系统：

        当系统不再是积分串联型的时候，如对于一个系统为：

式中d为未知的外部扰动。

        对于传统的PID控制，则引入一个积分器“I”

        传递函数。PID的参数可由极点配置法进行参数整定，当，，发生改变后，系统的特性也会发生改变，导致在含扰动的非积分串联型系统中PID控制器的鲁棒性下降。

2、 线性自抗扰控制（LADRC）

        LADRC优势在于其不依赖于系统的精确数学模型，其思想为通过线性扩张状态观测器（LESO）来实时估计和补偿系统中的扰动和不确定性，将系统转变为一个串联积分型后进行控制，而LADRC相比于ADRC，引入了带宽的概念，减少了要调整的参数数量和调整难度。

同样针对上述系统：

其状态空间为：

控制器设计为：

Fr为前馈项。当x1，x2不可测的时候，我们使用状态观测器来估计x1，x2的状态，则控制器变为：

引入的闭环观测器--Luenberger观测器公式如下：

2.1无对象模型的线性扩张状态观测器（LESO）的设计

        有了以上准备，我们把LADRC的扩张状态观测器(LESO)写作：

LESO中的x3即为“扩张”出来的状态。

        式子中的f为总扰动，这个总扰动不仅包括了系统受到的外部扰动，还有系统数学模型不准确所带来的内部扰动。所以在上述式子中的，我们可以将系统中除去输出的部分全部视作总扰动。

则包含了扩张状态的状态空间方程为：

根据Luenberger观测器的形式:

式中，​​​​​​，​，推导得出：

       

               

选择状态矩阵进行特征根的计算，特征多项式：

      

        选择观测器极点在左半平面内，为了方便调节，将极点选在上。称作观测器的带宽（特征根）。

                                                         

        这样就可以做到只需要调整就能够对观测器进行调节。

2.2状态误差反馈控制律的设计

        当设计的状态观测器能够较准确的估计出总扰动，即。我们就令，则，我们发现当把与输出u无关的参数全部包含到总扰动f中后，经过LESO的估计就能将其全部抵消。这样系统将转变为一个串联积分型，就可以采用最开始的PD控制器进行准确地控制。

        式中r为目标值，为前馈项，旨在提高系统对参考输入的跟踪性能。若不是阶跃输入信号（导数不为0），则PD控制器会产生稳态误差，加入前馈项就能减小稳态误差。绝大部分情况下可忽略此项。

        接下来对进行拉普拉斯变换得到:

        

        开环传递函数，则闭环传递函数为：

        对控制器进行极点配置，选取作为控制器带宽（极点）:

                                     

       这样就可以做到仅仅调整，就能够对整个控制器进行调节。

         对于一个标准的二阶闭环传递函数，上述的控制器闭环传函相当于的情况（临界阻尼比），根据阻尼比与超调量之间的关系，此时控制器的超调量为0。这也就是LADRC算法没有超调量的原因。

2.3关于bo

        bo是输出补偿b的猜测值，我们可以根据所建立的关于系统的数学模型中输出u前的项来进行计算猜测，使其尽可能的解近实际的b。而猜测的bo与实际的b之间的偏差将会被加入到总扰动f中进行估计，所以更加准确的bo能够减小观测器的估计压力。

        式中，。

2.4部分模型已知的线性扩张状态观测器（LESO）的设计

        对于一些系统来说，我们可以使用一些方法得到系统的部分模型，当我们把这些模型加入进LESO中后，能够有效的降低LESO的观测压力，在相同（甚至更低）的观测器带宽条件下获得更好的估计性能。若观测器的带宽过大，可能会导致观测器对实际的估计过于敏感，从而更易受到噪声影响。

        带入已知的模型，将上述式子写作

其中，a0,a1已知，bo部分已知。为实际未知的扰动，仍然将总扰动记为。

将含有扩张状态的状态空间方程写作：

此时，式子中各矩阵变为

，​​​​​​，​​，。

观测器为：

使用与上面相同的方法进行特征根的计算，

                                         

将观测器极点选择在左半平面，为了方便调节，选择在。

可以得到：

因为式子中除为未知量，其余都为模型的已知参数，所以同样只需要调整就能够对观测器进行调节。

3、LADRC参考代码

3.1无对象模型LADRC算法的Python实现

class LADRC:
    
    def __init__(self, ProcessOrder, Compensate, BandwidControlador, BandwidLESO, condicionInit,imprimirMat=1):

        self.u = 0
        self.h = 0.00001 
        self.nx = int(ProcessOrder)
        self.wc = BandwidControlador
        self.wo = BandwidLESO
        self.bo = Compensate
        self.zo = condicionInit
        self.Cg, self.Ac, self.Bc, self.Cc, self.Ee, self.zo, self.L,self.K,self.z = self.Parameter_Init(imprimirMat)


    def Parameter_Init(self, imprimirMat=1):
        
        n = self.nx + 1

        K = np.zeros([1, self.nx])
        for i in range(self.nx):
            K[0, i] = math.pow(self.wc, n - (i + 1)) * (
                    (math.factorial(self.nx)) / (math.factorial((i + 1) - 1) * math.factorial(n - (i + 1)))) #控制器带宽

        L = np.zeros([n, 1])
        for i in range(n):
            L[i] = math.pow(self.wo, i + 1) * (
                        (math.factorial(n)) / (math.factorial(i + 1) * math.factorial(n - (i + 1)))) #观测器带宽

        Cg = 1 / self.bo

        Ac = np.vstack((np.hstack((np.zeros([n - 1, 1]), np.identity(n - 1))), np.zeros([1, n])))
        Bc = np.vstack((np.zeros([self.nx - 1, 1]), self.bo, 0))
        Cc = np.hstack(([[1]], np.zeros([1, n - 1])))
        Ee = np.vstack((np.zeros((2, 1)), 1))
        zo = np.vstack(([[self.zo]], np.zeros([n - 1, 1])))
        z = np.zeros([n, 1])

        if imprimirMat == 1:
            print(f"K =\n {K}\n")
            print(f"L =\n {L}\n")
            print(f"Ac =\n {Ac}\n")
            print(f"Bc =\n {Bc}\n")
            print(f"Cc =\n {Cc}\n")
            print(f"Ee =\n {Ee}\n")
            print(f"z =\n {z}\n")
            print(f"zo =\n {zo}\n")

        return Cg, Ac, Bc, Cc, Ee, zo, L, K, z

    def LESO(self, u, y, z):
        
        return np.matmul(self.Ac, z) + self.Bc * u + self.L * (y - np.matmul(self.Cc, z))

    def Runkut4(self, F, z, h):
        
        k0 = h * F(z)
        k1 = h * F(z + 0.5 * k0)
        k2 = h * F(z + 0.5 * k1)
        k3 = h * F(z + k2)

        return z + (1 / 6) * (k0 + 2 * k1 + 2 * k2 + k3)

    def LADRC_Control(self, r, y):

        leso = partial(self.LESO, self.u, y) 
        self.z = self.Runkut4(leso, self.z, self.h) 

        u0 = self.K[0, 0] * (r - self.z[0, 0])  

        for i in range(self.nx - 1):
            u0 -= self.K[0, i + 1] * self.z[i + 1, 0] 

        self.u = (u0 - self.z[self.nx, 0]) * self.Cg

        return {'output':u0,'Z0':self.z[0,0],'Z1':self.z[1,0],'Z2':self.z[2,0]}

3.2部分模型已知LADRC算法的Python实现

class LADRC:

    def __init__(self, ProcessOrder, Compensate, BandwidControlador, BandwidLESO, condicionInit, ModelDisturbance, Disturbance0, Disturbance1,
                 imprimirMat=1):

        self.u = 0
        self.h = 0.00001 
        self.nx = int(ProcessOrder)
        self.wc = BandwidControlador
        self.wo = BandwidLESO
        self.bo = Compensate
        self.fh = ModelDisturbance
        self.zo = condicionInit
        self.a0 = Disturbance0
        self.a1 = Disturbance1
        self.Cg, self.Ac, self.Bc, self.Cc, self.Ee, self.zo, self.L, self.K, self.z = self.Parameter_Init(imprimirMat)

    def Parameter_Init(self, imprimirMat=1):
        
        n = self.nx + 1
        a0 = self.a0
        a1 = self.a1

        K = np.zeros([1, self.nx])
        for i in range(self.nx):
            K[0, i] = math.pow(self.wc, n - (i + 1)) * (
                    (math.factorial(self.nx)) / (math.factorial((i + 1) - 1) * math.factorial(n - (i + 1)))) #控制器带宽

        L = np.zeros([n, 1])
        for i in range(n):
            L[i] = math.pow(self.wo, i + 1) * (
                        (math.factorial(n)) / (math.factorial(i + 1) * math.factorial(n - (i + 1)))) #观测器带宽

        L[0] = L[0] - a1
        L[1] = L[1] - a1 * self.wo - a0 + a1 * a1
        L[2] = L[2] - 3 * a1 * self.wo * self.wo + 3  * self.wo * (a1 * a1 - a0) + 2 * a0 * a1 - math.pow(a1 , 3)  

        Cg = 1 / self.bo
        a0array = np.array([[a0]]) 
        a1array = np.array([[a1]]) 
        Ac = np.vstack((np.hstack((np.zeros([n - 1, 1]), np.identity(n - 1))), np.hstack((np.zeros((1, 1)), -a0array , -a1array))))
        Bc = np.vstack((np.zeros([self.nx - 1, 1]), self.bo, -a1array*self.bo))
        Cc = np.hstack(([[1]], np.zeros([1, n - 1])))
        Ee = np.vstack((np.zeros((2, 1)), 1 ))
        zo = np.vstack(([[self.zo]], np.zeros([n - 1, 1])))
        z = np.zeros([n, 1])

        if imprimirMat == 1:
            print(f"K =\n {K}\n")
            print(f"L =\n {L}\n")
            print(f"Ac =\n {Ac}\n")
            print(f"Bc =\n {Bc}\n")
            print(f"Cc =\n {Cc}\n")
            print(f"Ee =\n {Ee}\n")
            print(f"z =\n {z}\n")
            print(f"zo =\n {zo}\n")

        return Cg, Ac, Bc, Cc, Ee, zo, L, K, z

    def LESO(self, u, y, z):
        
        return np.matmul(self.Ac, z) + self.Bc * u + self.L * (y - np.matmul(self.Cc, z)) + self.Ee * self.fh

    def Runkut4(self, F, z, h):
        
        k0 = h * F(z)
        k1 = h * F(z + 0.5 * k0)
        k2 = h * F(z + 0.5 * k1)
        k3 = h * F(z + k2)

        return z + (1 / 6) * (k0 + 2 * k1 + 2 * k2 + k3)

    def LADRC_Control(self, r, y):

        leso = partial(self.LESO, self.u, y)  
        self.z = self.Runkut4(leso, self.z, self.h) 

        u0 = self.K[0, 0] * (r - self.z[0, 0])  

        for i in range(self.nx - 1):
            u0 -= self.K[0, i + 1] * self.z[i + 1, 0] 

        self.u = (u0 - self.z[self.nx, 0]) * self.Cg

        return {'output':u0,'Z0':self.z[0,0],'Z1':self.z[1,0],'Z2':self.z[2,0]}