ardupilot 上实现ADRC内环角速度控制

目录

摘要

本节主要记录自己在ardupilot上移植adrc的过程，欢迎批评指正，飞行测试不是很好，欢迎交流学习18129927205！

0.写之前

在网上找了很多资料，发现大部分是仿真和理论知识，有参考价值的东西很少，有的可以实现飞行，但代码写的一塌糊涂，不知道为什么就写出来ADRC公式（无任何理论或者工程依据），决定把自己遇到的过程全部写出来，一起学习成长！关于adrc的资料建议看韩老先生的资料，大致知道什么意思就可以！后面使用中在深入了解。废话不多说！

1.关键参考公式

参考公式全部来自韩老先生书中，按照书中的内容板书的公式。
重点把握ADRC: 跟踪微分器（TD）、非线性误差反馈（NSLF）、扩展状态观测器（ESO）

1.先上传一个关键的模型图**

2.用到的关键公式：

1.TD:

2.ESO:

3.NSLF:

---

2.上传代码

对于代码主要包含两种：一种是非线性ADRC和线性ADRC仅供参考！

1.非线性ADRC

.cpp文件

/********************************************************************************************************************
*********************************************************************************************************************
*文件名称：AC_ADRC.cpp
*功能描述：实现adrc控制库函数
*修改作者：
*修改时间：2022-7-13
*修改内容：
*备注信息:自抗扰控制器要涉及三个部分的设计问题：
* 1、安排过渡过程的部分
* 2、扩展状态观测器涉及
* 3、误差反馈涉及
* 这里主要研究的是二阶自抗扰算法
*       PID 控制主要以误差修正误差
*       adrc在经典反馈控制结构的基础上,主要由跟踪微分器（Tracking differentiator），
*        误差反馈控制率（Feedback control law）以及扩张观测器（Extended state observer）组成。
*        TD:跟踪微分器-Tracking differentiator,
*            分为线性和非线性两种，本质上是一种低通滤波器,作用是平滑指令以及减少闭环传递函数的超调（将闭环传递函数的峰值压低到0dB以下）。
*        NLSEF:误差反馈控制率-Feedback control law
*            和经典反馈控制结构中的反馈控制器一样，目的是让误差等于0，有线性组合与各种非线性组合，最常见的就是LADRC中的PD控制器，
*        偶尔也有例如fal函数里类似dynamic gain的处理（误差一定范围内增益不变，随后增益减小或增大，即小误差大增益或者小误差小增益）。
*        ESO:扩张观测器-Extended state observer
*        在系统模型为纯积分串联型的假设基础上，将所有其他项（忽略的模型与外界的扰动（噪声））统一为总扰动，并设计龙贝格观测器对其进行观测。
*        其中对于TD和ESO这两个部分其实是比较好调节的，很容易就能够获得很好的效果。比较难调节的参数主要是在非线性组合部分。
*        比较常见的有如下四种误差反馈率，我尝试过前三种，效果都还不错。
*        TD: r h h0
*        ESO: B01、B02、B03和观测器带宽w0.
*        非线性反馈：（beta1、beta2）用kp和kd代替，b。
*        对于TD，一般的仿真模型r可以尽量大一些，在100~500范围内基本相同，即使再大效果也基本不会有大的提升。h和h0理论上可以相同，即仿真模型中的仿真步长。
*       h0 和h1   这里一般预报时间h1比滤波因子h0稍大1-1.5倍数
*       _b01得到扰动估值值z3的补偿，这里参数b0是决定补偿强弱的“补偿因子”作为可调参数
*       ESO的三个参数和观测器带宽有关，依次设置为3w0、3w0^2、 w0^3就可以满足要求。
*      所以最终需要调节的参数只有四个：kp kd w0 b。这时候就可以控制变量了。
*       基本规律是：
*       b越小调节时间越短，但是过小会导致震荡。
*       w0越小调节时间越长，震荡幅度越小。
*       Kp越大调节时间越短，震荡越大。
*       kd效果不太明显，可在稳定后微调。
*ADRC参数整定经验

		TD:有两个参数r、h
		r越大，快速性越好，但是容易超调和引发振荡
		h越大，静态误差越小，刚开始带来的“超调”越小，初始的误差越小；但会导致上升过慢，快速性不好

		ESO:有6个参数bata01;beta02;beta03;b;T=0.0015;alpha1;alpha2;delta_Eso;
		一般alpha1=0.5;alpha2=0.25;delta_Eso=0.01;是固定参数，只需要调节其他三个参数：
		bata01和1/h是同一个数量级，过大会带来振荡甚至发散;
		beta02过小会带来发散，过大会产生高频噪声;
		beta03过大会产生振荡；过小会降低跟踪速度;

		Nolinear_PD:三个参数：Kp、Kd、delt
		Kp越大，会减少误差，但是会降低快速性
		Kd越大，增加快速性，但是过大会产生振荡
		delt的值基本不影响输出，但是一般在0.01~0.1之间选取，过大会产生振荡

最终参数：
        r = 30;h =0.0025;Kp = 500;Kd = 25;delt = 0.01;
        bata01=100;beta02=300;beta03=1500;alpha1=0.5;alpha2=0.25;delta_Eso=0.01;b=5;T=0.0015;
**********************************************************************************************************************************
**********************************************************************************************************************************/


#include 
#include "AC_ADRC.h"




const AP_Param::GroupInfo AC_ADRC::var_info[] = {
		 AP_GROUPINFO_FLAGS("ENB", 0, AC_ADRC, _enable, 0, AP_PARAM_FLAG_ENABLE),
         //TD参数
		 AP_GROUPINFO("H0",    1, AC_ADRC, _h0, 0.025),       //h0=10h
		 AP_GROUPINFO("H1",    2, AC_ADRC, _h, 0.0025),       //滤波因子,系统调用步长
		 AP_GROUPINFO("R0",   3, AC_ADRC, _r0, 5.0),          //快速跟踪因子
         //ESO参数
		 AP_GROUPINFO("B",    4, AC_ADRC, _b, 0.2),           //系统系数b
		 AP_GROUPINFO("BT0",    5, AC_ADRC, _beta0, 10),      //扩张状态观测器反馈增益1
		 AP_GROUPINFO("BT1",    6, AC_ADRC, _beta1, 300),     //扩张状态观测器反馈增益2
		 AP_GROUPINFO("BT2",    7, AC_ADRC, _beta2, 1000),    //扩张状态观测器反馈增益3
		 AP_GROUPINFO("DETA",    8, AC_ADRC, _delta, 0.0125), //delta为fal（e，alpha，delta）函数的线性区间宽度 delta取值范围在5h<=delta<=10h,h为ADRC控制周期
	     AP_GROUPINFO("P",    9, AC_ADRC, _kp, 0.12),        //跟踪输入信号增益
	     AP_GROUPINFO("D",    10, AC_ADRC, _kd, 0.001),       //跟踪微分信号增益
	     AP_GROUPINFO("A1",   11, AC_ADRC, _alfa1, 0.75),
	     AP_GROUPINFO("A2",   12, AC_ADRC, _alfa2, 1.5),
    AP_GROUPEND
};

/*********************************************************************************************************************
*主要的参数记录
* TD：
*     h0:根据仿真可以看出,参数h0的扩大起着很好的滤波效果，因此我们将把参数h0称作跟踪微分器的滤波因子：表示滤波因子
*        h0选择适当大于步长h的参数
*     h:步长一般选择周期值
*     _r0:表示速度因子，若速度因子r小，则TD的输出比较光滑地跟踪参考输入,若比较大，更接近原始输入，有棱有角,具体选择多大的速度因子
*     取决与受控对象的承受能力和可提供的控制能力没对象的承受能力和可提供的控制能力大,速度因子可以取大200，适当大的参数r跟踪微分效果是很好
* ESO：状态观测器参数beta01=1/h  beta02=1/(3*h^2)  beta03=2/(8^2*h^3)
*     _beta0 ：3w     40       0.1
*     _beta1 ：3w*w   533      0.3
*     _beta2 ：w*w*w  2000     1
*     _b： 0.1 参数b即便是状态的函数或时变参数，但其变化范围不很大，大于0.5就会很差
*     _delta:
*
* NSL:
*     _kp
*     _kd
*     _alfa1： 0<_alfa1<1<_alfa2
*     _alfa2：
*
********************************************************************************************************************/

/********************************************************************************************************************
*函数原型：AC_ADRC()
*函数功能：构造函数
*修改日期：2022-7-13
*修改作者：
*备注信息：
**********************************************************************************************************/
AC_ADRC::AC_ADRC(float h0,float h, float r0 ,                              //TD
		float b, float beta0, float beta1, float beta2,float delta ,      //ESO
		float kp, float kd, float a1, float a2) //:

{
    //加载默认参数 load parameter values from eeprom
    AP_Param::setup_object_defaults(this, var_info);
    //需要设定的TD参数
	_h0 = h0;
	_h = h;
	_r0 = r0;
	 //需要设定的ESO参数
	_b = b;
	_beta0 = beta0;
	_beta1 = beta1;
	_beta2 = beta2;
	_delta =  delta;
    if(_b<=0)
    {
    	_b=1;
    }
    //NSL
	_kp = kp ;
	_kd = kd;
	_alfa1 = a1;
	_alfa2 = a2;

     //初始值
	 _z[0]=0;
	 _z[1]=0;
	 _z[2]=0;
	_v1 = _v2 = 0.0f;
	_e1 = _e2 = 0.0f;
	_eso_error = 0.0f;
	_disturb_u = _disturb = 0.0f;
	_u0 = 0;
	_u = 0;

}
/********************************************************************************************************
*函数原型：float AC_ADRC::adrc_constrain(float val, float min, float max)
*函数功能：限制函数
*修改日期：2022-7-13
*修改作者：
*备注信息：
**********************************************************************************************************/
float AC_ADRC::adrc_constrain(float val, float min, float max)
{
	return (val < min) ? min : ((val > max) ? max : val);
}
/********************************************************************************************************
*函数原型：void AC_ADRC::adrc_td(float in)
*函数功能：构造函数
*修改日期：2022-7-13
*修改作者：
*备注信息：TD:跟踪微分器-Tracking differentiator,
*           分为线性和非线性两种，本质上是一种低通滤波器,作用是平滑指令以及减少闭环传递函数的超调（将闭环传递函数的峰值压低到0dB以下）。
*            fh=fhan(x1(k)-v(k),x2(k),r,h0)
*            主要参考韩京清的教材；fhan为最优控制函数，主要参考：2.6.25 和2.6.26
*            这里一般预报时间h1比滤波因子h0稍大1-1.5倍数
**********************************************************************************************************/
void AC_ADRC::adrc_td(float in)
{
	_v1 += _h * _v2;                                //td_x1=v1; v1(t+1) = v1(t) + T * v2(t)
	_v2 += _h * fhan(_v1 - in, _v2, _r0, _h0);      //td_x2=v2 v2(t+1) = t2(t) + T fst2(v1(t) - in, v2(t), r, h)
}
/********************************************************************************************************
*函数原型：float AC_ADRC::fsg(float x,float d)
*函数功能：构造函数
*修改日期：2022-7-13
*修改作者：
*备注信息：符号函数的另外转换形式
**********************************************************************************************************/
float AC_ADRC::fsg(float x,float d)
{
    float value;
    value=(sign_adrc(x+d)-sign_adrc(x-d))*0.5f;
	return(value);
}
/********************************************************************************************************
*函数原型：float AC_ADRC::fhan(float x1, float x2, float r, float h)
*函数功能：最优控制函数,这个函数的实现参考2.7.24公式，教材107页
*修改日期：2022-7-13
*修改作者：
*备注信息：h：表示步长
**********************************************************************************************************/
float AC_ADRC::fhan(float x1, float x2, float r, float h)
{
	float y=0;
	float a0=0,a1,a2;
	float a=0;
	float fh=0;
	float d=0;
	float sa;

	d = r * h * h;                                              //d=r*h d0=h*d
	//中间变量a0
	a0 = h * x2;                                                //a0=h*x2
	//计算出y
	y = x1 + a0;                                                //y=x1+a0
	//计算出教材中的a1
	a1 = sqrtf(d*(d + 8.0 * fabsf(y)));                         //a1=sqrtf(d*(d+8*(|y|)))
	//计算出a2
	a2 = a0 + sign_adrc(y) * (a1-d) * 0.5f;                     //a2=a0+sign(y)*(a1-d)/2
    //最终计算得到a0
	a = (a0 + y - a2) * fsg(y,d) + a2;                          //a=(a0+y-a2)*fsg(y,d)+a2
	sa = fsg(a,d);                                              //fsg(a,d)=(sign(a+d)-sign(a-d))/2
	fh = -r * (a / d - sign_adrc(a)) * sa - r * sign_adrc(a);  //fhan=-r*fsg(a,d)-r*sign(a)(1-fsg(a,d))
	return(fh);
}



/********************************************************************************************************
*函数原型：float AC_ADRC::fal(float e,float alfa,float delta)
*函数功能：构造函数
*修改日期：2022-7-13
*修改作者：
*备注信息：原点附近具有线性段的连续的幂次函数
**********************************************************************************************************/
float AC_ADRC::fal(float e,float alfa,float delta)
{
	float f = 0.0f;
	float s=0;
	s=fsg(e,delta);
	//fal(e,a,d)
	f=e*s/(powf(delta,1-alfa))+powf(fabsf(e),alfa)*sign_adrc(e)*(1-s);
	return(f);
}


/********************************************************************************************************
*函数原型：float AC_ADRC::adrc_eso(float v,float y,float u,float T,float MAX)
*函数功能：非线性观测器
*修改日期：2022-7-13
*修改作者：
*备注信息：公式4.7.4 4.3.19
*备注信息：ESO输入量 Y和U
*备注信息：其中_beta0=3w _beta1=3w*w _beta2=w*w*w
**********************************************************************************************************/
void AC_ADRC::adrc_eso(float y,float u,float h,float MAX)
{
	float fe,fe1;
	//得到误差值
	_eso_error = _z[0] - y; //其中y是输入
	//调用原点附近具有线性段的连续的幂次函数
	fe = fal(_eso_error, 0.5f, _delta);
	fe1=fal(_eso_error, 0.25f, _delta);
	_z[0] += h * (_z[1] - _beta0*_eso_error);
	_z[1] += h * (_z[2] - _beta1 * fe + _b *  u); //u也是输入，x1对应的是x0的导数
	_z[2] += -h * _beta2 * fe1; //对应总扰动
}





/********************************************************************************************************
*函数原型：float AC_ADRC::adrc_nsl(float v,float y,float u,float T,float MAX)
*函数功能：非线性状态反馈
*修改日期：2022-7-13
*修改作者：
*备注信息：参考公式：5.2.1
*备注信息：kd=2wc
*备注信息：kp=wc*wc wc=w0/(3-10)
**********************************************************************************************************/
float AC_ADRC::adrc_nsl(float MAX)
{

	//得到误差e1
	_e1 = _v1 - _z[0];
	//得到误差e2
	_e2 = _v2 - _z[1];
	//公式5.2.1第二个公式,要求0<_alfa1<1<_alfa2
	_u0 = _kp * fal(_e1, _alfa1, _delta) + _kd * fal(_e2,_alfa2,_delta); //0<_alfa1<1<_alfa2

	if(_b<=0.0f)
	{
		_b=1.0f;
	}
	//得到扰动估值值z3的补偿，这里参数b0是决定补偿强弱的“补偿因子”作为可调参数
	_disturb_u = _z[2] / _b;
	//最终得到控制量信息
	_u=_u0-_disturb_u;
	//对控制量信息进行限制
	_u=adrc_constrain(_u, -MAX, MAX);
	return (_u);

}



/********************************************************************************************************
*函数原型：float AC_ADRC::control_adrc(float v,float y,float u,float MAX)
*函数功能：adrc控制函数
*修改日期：2022-7-13
*修改作者：
*备注信息：v输入值 y反馈值
**********************************************************************************************************/
float AC_ADRC::control_adrc(float v,float y,float u,float MAX)
{
	//定义输出数据
	float output_u=0;

	if(_enable == 0)
	{
		return 0;
	}
	else
	{
		//安排过渡过程,根据设定值V安排过渡过程V1并提取器微分信号V2
		//根据对象的输出和输入信号y，u估计出对象的状态x1 x2和作用于对象的综合扰动x3
		//状态误差的非线性反馈律。系统的状态误差是指e1=v1-z1 e2=v2-z2，误差反馈律是根据误差e1,e2来决定的控制纯积分器串联型对象的控制规律u0
		//对误差反馈控制量u0用扰动估计值Z3的补偿来决定最终控制量
		//其中参数b0是决定补偿强弱的“补偿因子”
		adrc_td(v);                    //微分跟踪器TD
		//记录控制器的数据值
	    target=v; //输入值
	    actual=y; //实际
	    disu=u;   //返回的值

		adrc_eso(y, u, _h, MAX);       //状态观测器ESO
		//数据输出出去
		output_u=adrc_nsl(MAX);        //非线性误差反馈
		return output_u;
	}

}

/*************************************************************************************************************************
*                              file_end
*************************************************************************************************************************/

.h文件

#ifndef AC_ADRC_H_
#define AC_ADRC_H_

#include 
#include 


class AC_ADRC {
public:

	AC_ADRC(float h0=0.025,float h=0.0025,float r0 = 400.0f,
			float b=1000,float beta0 = 10, float beta1 = 300,float beta2 = 1000,float delta = 0.02,
			float kp=0.12, float kd=0.001, float a1=0.75, float a2=1.5);

	void init()
	{

		if(_enable == 0)
			return;
	}

	void reset(){
		 actual=0; //实际
	     target=0; //输入值
	     disu=0;   //返回的值
		_v1 = _v2 = 0.0f;
		_e1 = _e2 = 0.0f;
		_eso_error = 0.0f;
		 _z[0]=0;
		 _z[1]=0;
		 _z[2]=0;
		_disturb_u = _disturb = 0.0f;
		_u0 = 0;
		_u = 0;
	}

	int8_t get_enable(void) {
		return _enable.get();
	}

	float adrc_constrain(float val, float min, float max);
	float control_adrc(float v,float y,float u,float MAX);

	float get_e1() const
	{
		return _e1;
	}

	float get_e2() const
	{
		return _e2;
	}

	float get_z3() const
	{
		return _z[2];
	}

	float get_z2() const
	{
		return _z[1];
	}

	float get_z1() const
	{
		return _z[0];
	}

	float get_x1() const
	{
		return _v1;
	}

	float get_x2() const
	{
		return _v2;
	}

	float get_eso_error() const
	{
		return _eso_error;
	}

	float get_u() const
	{
		return _u;
	}

	float get_target() const
	{
		return target;
	}

	float get_actual() const
	{
		return actual;
	}
	float get_disu() const
	{
		return disu;
	}


    static const struct AP_Param::GroupInfo var_info[];
protected:
	float fst(float x1,float x2,float w,float h);
	float fal(float e,float alfa,float delta);
	float fhan(float x1,float x2,float r, float h);
	float fsg(float x,float d);

	void adrc_eso(float y,float u,float h,float MAX);
	float adrc_nsl(float MAX);
	void adrc_td(float in);

	int8_t sign_adrc(float input) //adrc的 sign函数
	{
		//int8_t output
		if(input > 1e-6)
		{
			return 1;
		}
		else if(input < -1e-6)
		{
			return -1;
		}
		else
		{
			return 0;
		}

	}

	AP_Int8 _enable;

	//DT
	AP_Float _h0, _h;
	AP_Float _r0;
	//ESO
	AP_Float _b;
	AP_Float _beta0, _beta1, _beta2;
	AP_Float _delta;
    //NSL
	AP_Float _kp, _kd;
	AP_Float _alfa1, _alfa2;

	float actual; //实际
    float target; //输入值
    float disu;   //返回的值

	float _v1, _v2; //输入信号的一阶导数 二阶导数
	float _z[3];    // 锟脚讹拷锟斤拷息

	float _e1, _e2;
	float _eso_error;
	float _disturb, _disturb_u;
	float _u0;
	float _u;

};





#endif /* AUTOPILOT_LIBRARIES_AC_ADRC_AC_ADRC_H_ */

2.线性ADRC

/********************************************************************************************************************
*********************************************************************************************************************
*文件名称：AC_LADRC.cpp
*功能描述：实现adrc控制库函数
*修改作者：
*修改时间：2022-7-13
*修改内容：
*备注信息:自抗扰控制器要涉及三个部分的设计问题：
* 1、安排过渡过程的部分
* 2、扩展状态观测器涉及
* 3、误差反馈涉及
* 这里主要研究的是二阶自抗扰算法
*
**********************************************************************************************************************************
**********************************************************************************************************************************/


#include 
#include "AC_LADRC.h"




const AP_Param::GroupInfo AC_LADRC::var_info[] = {
		 AP_GROUPINFO_FLAGS("ENB", 0, AC_LADRC, _enable, 0, AP_PARAM_FLAG_ENABLE),
         //TD参数
		 AP_GROUPINFO("H",    1, AC_LADRC, _h, 0.0025),        //h0=10h
		 AP_GROUPINFO("R",   2, AC_LADRC, _r, 100),           //快速跟踪因子
         //ESO参数
		 AP_GROUPINFO("B",    3, AC_LADRC, _b, 100),           //系统系数b
         //ESO参数
		 AP_GROUPINFO("W0",    4, AC_LADRC, _w0, 50),          //
		 AP_GROUPINFO("Wc",    5, AC_LADRC, _wc, 10),           观测器带宽

    AP_GROUPEND
};

/*********************************************************************************************************************
*主要的参数记录
* TD：
*     h0:根据仿真可以看出,参数h0的扩大起着很好的滤波效果，因此我们将把参数h0称作跟踪微分器的滤波因子：表示滤波因子
*        h0选择适当大于步长h的参数
*     h:步长一般选择周期值
*     _r0:表示速度因子，若速度因子r小，则TD的输出比较光滑地跟踪参考输入,若比较大，更接近原始输入，有棱有角,具体选择多大的速度因子
*     取决与受控对象的承受能力和可提供的控制能力没对象的承受能力和可提供的控制能力大,速度因子可以取大200，适当大的参数r跟踪微分效果是很好
* ESO：状态观测器参数beta01=1/h  beta02=1/(3*h^2)  beta03=2/(8^2*h^3)
*     _beta0 ：3w     40       0.1
*     _beta1 ：3w*w   533      0.3
*     _beta2 ：w*w*w  2000     1
*     _b： 0.1 参数b即便是状态的函数或时变参数，但其变化范围不很大，大于0.5就会很差
*     _delta:
*
* NSL:
*     _kp
*     _kd
*     _alfa1： 0<_alfa1<1<_alfa2
*     _alfa2：
*
********************************************************************************************************************/

/********************************************************************************************************************
*函数原型：AC_ADRC()
*函数功能：构造函数
*修改日期：2022-7-13
*修改作者：
*备注信息：
**********************************************************************************************************/
AC_LADRC::AC_LADRC(float h,float r,float w0,float wc,float b )

{
    //加载默认参数 load parameter values from eeprom
    AP_Param::setup_object_defaults(this, var_info);
    //需要设定的TD参数

	_h = h;
	_r = r;
	//w0观测器带宽
	_w0=w0;
	//wc状态误差反馈率带宽
	_wc=wc;
	_b=b;
     //初始值
	 _z[0]=0;
	 _z[1]=0;
	 _z[2]=0;
	_v1 = _v2 = 0.0f;
	_e1 = _e2 = 0.0f;
	_eso_error = 0.0f;
	_disturb_u = _disturb = 0.0f;
	_u0 = 0;
	_u = 0;

}
/********************************************************************************************************
*函数原型：float AC_ADRC::adrc_constrain(float val, float min, float max)
*函数功能：限制函数
*修改日期：2022-7-13
*修改作者：
*备注信息：
**********************************************************************************************************/
float AC_LADRC::ladrc_constrain(float val, float min, float max)
{
	return (val < min) ? min : ((val > max) ? max : val);
}
/********************************************************************************************************
*函数原型：void AC_ADRC::adrc_td(float in)
*函数功能：构造函数
*修改日期：2022-7-13
*修改作者：
*备注信息：TD:跟踪微分器-Tracking differentiator,
*           分为线性和非线性两种，本质上是一种低通滤波器,作用是平滑指令以及减少闭环传递函数的超调（将闭环传递函数的峰值压低到0dB以下）。
*            fh=fhan(x1(k)-v(k),x2(k),r,h0)
*            主要参考韩京清的教材；fhan为最优控制函数，主要参考：2.6.25 和2.6.26
*            这里一般预报时间h1比滤波因子h0稍大1-1.5倍数
**********************************************************************************************************/
void AC_LADRC::ladrc_td(float in)
{
	 float fh=0;
	 fh= -_r*_r*(_v1)-2*_r*_v2;
	_v1 += _h * _v2;
	_v2 += _h * fh;
}
/********************************************************************************************************
*函数原型：float AC_ADRC::fsg(float x,float d)
*函数功能：构造函数
*修改日期：2022-7-13
*修改作者：
*备注信息：符号函数的另外转换形式
**********************************************************************************************************/
float AC_LADRC::fsg(float x,float d)
{
    float value;
    value=(sign_ladrc(x+d)-sign_ladrc(x-d))*0.5f;
	return(value);
}
/********************************************************************************************************
*函数原型：float AC_ADRC::fhan(float x1, float x2, float r, float h)
*函数功能：最优控制函数,这个函数的实现参考2.7.24公式，教材107页
*修改日期：2022-7-13
*修改作者：
*备注信息：h：表示步长
**********************************************************************************************************/
float AC_LADRC::fhan(float x1, float x2, float r, float h)
{
	float y=0;
	float a0=0,a1,a2;
	float a=0;
	float fh=0;
	float d=0;
	float sa;

	d = r * h * h;                                              //d=r*h d0=h*d
	//中间变量a0
	a0 = h * x2;                                                //a0=h*x2
	//计算出y
	y = x1 + a0;                                                //y=x1+a0
	//计算出教材中的a1
	a1 = sqrtf(d*(d + 8.0 * fabsf(y)));                         //a1=sqrtf(d*(d+8*(|y|)))
	//计算出a2
	a2 = a0 + sign_ladrc(y) * (a1-d) * 0.5f;                     //a2=a0+sign(y)*(a1-d)/2
    //最终计算得到a0
	a = (a0 + y - a2) * fsg(y,d) + a2;                          //a=(a0+y-a2)*fsg(y,d)+a2
	sa = fsg(a,d);                                              //fsg(a,d)=(sign(a+d)-sign(a-d))/2
	fh = -r * (a / d - sign_ladrc(a)) * sa - r * sign_ladrc(a);  //fhan=-r*fsg(a,d)-r*sign(a)(1-fsg(a,d))
	return(fh);
}



/********************************************************************************************************
*函数原型：float AC_ADRC::fal(float e,float alfa,float delta)
*函数功能：构造函数
*修改日期：2022-7-13
*修改作者：
*备注信息：原点附近具有线性段的连续的幂次函数
**********************************************************************************************************/
float AC_LADRC::fal(float e,float alfa,float delta)
{
	float f = 0.0f;
	float s=0;
	s=fsg(e,delta);
	//fal(e,a,d)
	f=e*s/(powf(delta,1-alfa))+powf(fabsf(e),alfa)*sign_ladrc(e)*(1-s);
	return(f);
}


/********************************************************************************************************
*函数原型：float AC_ADRC::adrc_eso(float v,float y,float u,float T,float MAX)
*函数功能：非线性观测器
*修改日期：2022-7-13
*修改作者：
*备注信息：公式4.7.4 4.3.19
*备注信息：ESO输入量 Y和U
*备注信息：其中_beta0=3w _beta1=3w*w _beta2=w*w*w  w0观测器带宽，其值会影响线性扩张状态观测器的观测精度，当选择较高值时
*备注信息：观测的精度也会提高，但是噪声信号也同样会被放大
**********************************************************************************************************/
void AC_LADRC::ladrc_eso(float y,float u,float h,float MAX)
{
	float beita_01=3*_w0;
	float beita_02=3*_w0*_w0;
	float beita_03=_w0*_w0*_w0;

	//得到误差值
	_eso_error = _z[0] - y; //其中y是输入
	//调用原点附近具有线性段的连续的幂次函数

	_z[0] += h * (_z[1] - beita_01*_eso_error);
	_z[1] += h * (_z[2] - beita_02 * _eso_error + _b *  u); //u也是输入，x1对应的是x0的导数
	_z[2] += -h * beita_03 * _eso_error; //对应总扰动
}





/********************************************************************************************************
*函数原型：float AC_ADRC::adrc_nsl(float v,float y,float u,float T,float MAX)
*函数功能：非线性状态反馈
*修改日期：2022-7-13
*修改作者：
*备注信息：参考公式：5.2.1
*备注信息：kd=2wc
*备注信息：kp=wc*wc wc=w0/(3-10)控制带宽
**********************************************************************************************************/
float AC_LADRC::ladrc_nsl(float MAX)
{
    float kp=_wc*_wc;
    float kd=3*_wc;
	//得到误差e1
	_e1 = _v1 - _z[0];
	//得到误差e2
	_e2 = _v2 - _z[1];
	//公式5.2.1第二个公式,要求0<_alfa1<1<_alfa2
	_u0 = kp * _e1 + kd * _e2; //0<_alfa1<1<_alfa2
	//得到扰动估值值z3的补偿，这里参数b0是决定补偿强弱的“补偿因子”作为可调参数
	if(_b<=0.0f)
	{
		_b=1.0f;
	}
	_disturb_u = _z[2] / _b;
	//最终得到控制量信息
	_u=_u0-_disturb_u;
	//对控制量信息进行限制
	_u=ladrc_constrain(_u, -MAX, MAX);
	return (_u);

}



/********************************************************************************************************
*函数原型：float AC_ADRC::control_adrc(float v,float y,float u,float MAX)
*函数功能：adrc控制函数
*修改日期：2022-7-13
*修改作者：
*备注信息：v输入值 y反馈值
**********************************************************************************************************/
float AC_LADRC::control_ladrc(float v,float y,float u,float MAX)
{
	//定义输出数据
	float output_u=0;

	if(_enable == 0)
	{
		return 0;
	}
	else
	{
		//安排过渡过程,根据设定值V安排过渡过程V1并提取器微分信号V2
		//根据对象的输出和输入信号y，u估计出对象的状态x1 x2和作用于对象的综合扰动x3
		//状态误差的非线性反馈律。系统的状态误差是指e1=v1-z1 e2=v2-z2，误差反馈律是根据误差e1,e2来决定的控制纯积分器串联型对象的控制规律u0
		//对误差反馈控制量u0用扰动估计值Z3的补偿来决定最终控制量
		//其中参数b0是决定补偿强弱的“补偿因子”
		ladrc_td(v);                    //微分跟踪器TD
		//记录控制器的数据值
	    target=v; //输入值
	    actual=y; //实际
	    disu=u;   //返回的值

		ladrc_eso(y, u, _h, MAX);       //状态观测器ESO
		//数据输出出去
		output_u=ladrc_nsl(MAX);        //非线性误差反馈
		return output_u;
	}

}

/*************************************************************************************************************************
*                              file_end
*************************************************************************************************************************/

#ifndef FIRMWARE_LIBRARIES_AC_ADRC_AC_LADRC_H_
#define FIRMWARE_LIBRARIES_AC_ADRC_AC_LADRC_H_



#include 
#include 


class AC_LADRC {
public:
	//w0观测器带宽//wc状态误差反馈率带宽
	AC_LADRC(float h=0.0025,float r = 100.0f,
			float w0=50,float wc = 10,float b=200);

	void init()
	{

		if(_enable == 0)
			return;
	}

	void reset(){
		 actual=0; //实际
	     target=0; //输入值
	     disu=0;   //返回的值
		_v1 = _v2 = 0.0f;
		_e1 = _e2 = 0.0f;
		_eso_error = 0.0f;
		 _z[0]=0;
		 _z[1]=0;
		 _z[2]=0;
		_disturb_u = _disturb = 0.0f;
		_u0 = 0;
		_u = 0;
	}

	int8_t get_enable(void) {
		return _enable.get();
	}

	float ladrc_constrain(float val, float min, float max);
	float control_ladrc(float v,float y,float u,float MAX);

	float get_e1() const
	{
		return _e1;
	}

	float get_e2() const
	{
		return _e2;
	}

	float get_z3() const
	{
		return _z[2];
	}

	float get_z2() const
	{
		return _z[1];
	}

	float get_z1() const
	{
		return _z[0];
	}

	float get_x1() const
	{
		return _v1;
	}

	float get_x2() const
	{
		return _v2;
	}

	float get_eso_error() const
	{
		return _eso_error;
	}

	float get_u() const
	{
		return _u;
	}

	float get_target() const
	{
		return target;
	}

	float get_actual() const
	{
		return actual;
	}
	float get_disu() const
	{
		return disu;
	}


    static const struct AP_Param::GroupInfo var_info[];
protected:
	float fst(float x1,float x2,float w,float h);
	float fal(float e,float alfa,float delta);
	float fhan(float x1,float x2,float r, float h);
	float fsg(float x,float d);

	void ladrc_eso(float y,float u,float h,float MAX);
	float ladrc_nsl(float MAX);
	void ladrc_td(float in);

	int8_t sign_ladrc(float input) //adrc的 sign函数
	{
		//int8_t output
		if(input > 1e-6)
		{
			return 1;
		}
		else if(input < -1e-6)
		{
			return -1;
		}
		else
		{
			return 0;
		}

	}

	AP_Int8 _enable;

	//DT
	AP_Float  _h;
	AP_Float _r;
	//ESO
	AP_Float _b;
	AP_Float _w0;
	AP_Float _wc;


	float actual; //实际
    float target; //输入值
    float disu;   //返回的值

	float _v1, _v2; //输入信号的一阶导数 二阶导数
	float _z[3];    // 锟脚讹拷锟斤拷息

	float _e1, _e2;
	float _eso_error;
	float _disturb, _disturb_u;
	float _u0;
	float _u;

};





#endif

---

3.移植细节

按照ardupilot 角速度环的控制模仿写adrc的控制输入函数即可，把角速度环用到的全局查找，直接拷贝替换成adrc的就可以，另外建议增加日志，记录各个信号的变化过程，方便调试排查问题！

void AC_AttitudeControl_Multi::rate_controller_run()
{
    // move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
    update_throttle_rpy_mix();

    _rate_target_ang_vel += _rate_sysid_ang_vel;

    Vector3f gyro_latest = _ahrs.get_gyro_latest();

    //是否使用adrc控制
    if((_ladrc_rate_roll.get_enable() == 0 && _ladrc_rate_pitch.get_enable() == 0 && _ladrc_rate_yaw.get_enable() ==0)
    		&&((_adrc_rate_roll.get_enable() >0 && _adrc_rate_pitch.get_enable()  >0 && _adrc_rate_yaw.get_enable()  >0)))
    {
    	//ADRC横滚控制,其他的是俯仰和偏航PID控制
    	if(_adrc_control_rpy==1)
    	{
    		 //adrc+roll
    		_motors.set_roll(_adrc_rate_roll.control_adrc(_rate_target_ang_vel.x, gyro_latest.x, _motors.get_roll(), 0.5));
    		 //pid+pitch
    		_motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch) + _actuator_sysid.y);
            _motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
            //pid+yaw
            _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z);
            _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
    	}
    	else if(_adrc_control_rpy==2)//ADRC俯仰控制,其他的是横滚和偏航PID控制
    	{
    		 //pid+roll
    	    _motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll) + _actuator_sysid.x);
    	    _motors.set_roll_ff(get_rate_roll_pid().get_ff());
    	    //adrc+pitch
    	    _motors.set_pitch(_adrc_rate_pitch.control_adrc(_rate_target_ang_vel.y, gyro_latest.y, _motors.get_pitch(), 0.5));
    	    //pid+yaw
    	    _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z);
    	    _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
    	}
    	else if(_adrc_control_rpy==3)//ADRC横滚+俯仰控制,其他偏航是PID控制
    	{
    		//adrc+roll
       		_motors.set_roll(_adrc_rate_roll.control_adrc(_rate_target_ang_vel.x, gyro_latest.x, _motors.get_roll(), 0.5));
       	    //adrc+pitch
        	_motors.set_pitch(_adrc_rate_pitch.control_adrc(_rate_target_ang_vel.y, gyro_latest.y, _motors.get_pitch(), 0.5));
    	    //pid+yaw
    	    _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z);
    	    _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
    	}
    	else if(_adrc_control_rpy==4)//ADR偏航控制,其他横滚,俯仰控制PID
    	{
    		//pid+roll
            _motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll) + _actuator_sysid.x);
            _motors.set_roll_ff(get_rate_roll_pid().get_ff());
            //pid+pitch
            _motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch) + _actuator_sysid.y);
            _motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
    	    //adrc+yaw
        	_motors.set_yaw(_adrc_rate_yaw.control_adrc(_rate_target_ang_vel.z, gyro_latest.z, _motors.get_yaw(), 0.5));
    	}
    	else if(_adrc_control_rpy==7)//ADRC横滚+俯仰+偏航控制
    	{
    		//adrc+roll
       		_motors.set_roll(_adrc_rate_roll.control_adrc(_rate_target_ang_vel.x, gyro_latest.x, _motors.get_roll(), 0.5));
       	    //adrc+pitch
        	_motors.set_pitch(_adrc_rate_pitch.control_adrc(_rate_target_ang_vel.y, gyro_latest.y, _motors.get_pitch(), 0.5));
    	    //adrc+yaw
        	_motors.set_yaw(_adrc_rate_yaw.control_adrc(_rate_target_ang_vel.z, gyro_latest.z, _motors.get_yaw(), 0.5));
    	}
    	else
    	{
    		//pid+roll
            _motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll) + _actuator_sysid.x);
            _motors.set_roll_ff(get_rate_roll_pid().get_ff());
            //pid+pitch
            _motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch) + _actuator_sysid.y);
            _motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
            //pid+yaw
            _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z);
            _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
    	}
    }
    //是否使用adrc控制
    else if((_ladrc_rate_roll.get_enable() > 0 && _ladrc_rate_pitch.get_enable() > 0 && _ladrc_rate_yaw.get_enable() > 0)
    		&&((_adrc_rate_roll.get_enable() == 0 && _adrc_rate_pitch.get_enable() == 0 && _adrc_rate_yaw.get_enable() ==0)))
    {
    	//ADRC横滚控制,其他的是俯仰和偏航PID控制
    	if(_ladrc_control_rpy==1)
    	{
    		 //adrc+roll
    		_motors.set_roll(_ladrc_rate_roll.control_ladrc(_rate_target_ang_vel.x, gyro_latest.x, _motors.get_roll(), 0.5));
    		 //pid+pitch
    		_motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch) + _actuator_sysid.y);
            _motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
            //pid+yaw
            _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z);
            _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
    	}
    	else if(_ladrc_control_rpy==2)//ADRC俯仰控制,其他的是横滚和偏航PID控制
    	{
    		 //pid+roll
    	    _motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll) + _actuator_sysid.x);
    	    _motors.set_roll_ff(get_rate_roll_pid().get_ff());
    	    //adrc+pitch
    	    _motors.set_pitch(_ladrc_rate_pitch.control_ladrc(_rate_target_ang_vel.y, gyro_latest.y, _motors.get_pitch(), 0.5));
    	    //pid+yaw
    	    _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z);
    	    _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
    	}
    	else if(_ladrc_control_rpy==3)//ADRC横滚+俯仰控制,其他偏航是PID控制
    	{
    		//adrc+roll
       		_motors.set_roll(_ladrc_rate_roll.control_ladrc(_rate_target_ang_vel.x, gyro_latest.x, _motors.get_roll(), 0.5));
       	    //adrc+pitch
        	_motors.set_pitch(_ladrc_rate_pitch.control_ladrc(_rate_target_ang_vel.y, gyro_latest.y, _motors.get_pitch(), 0.5));
    	    //pid+yaw
    	    _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z);
    	    _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
    	}
    	else if(_ladrc_control_rpy==4)//ADR偏航控制,其他横滚,俯仰控制PID
    	{
    		//pid+roll
            _motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll) + _actuator_sysid.x);
            _motors.set_roll_ff(get_rate_roll_pid().get_ff());
            //pid+pitch
            _motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch) + _actuator_sysid.y);
            _motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
    	    //adrc+yaw
        	_motors.set_yaw(_ladrc_rate_yaw.control_ladrc(_rate_target_ang_vel.z, gyro_latest.z, _motors.get_yaw(), 0.5));
    	}
    	else if(_ladrc_control_rpy==7)//ADRC横滚+俯仰+偏航控制
    	{
    		//adrc+roll
       		_motors.set_roll(_ladrc_rate_roll.control_ladrc(_rate_target_ang_vel.x, gyro_latest.x, _motors.get_roll(), 0.5));
       	    //adrc+pitch
        	_motors.set_pitch(_ladrc_rate_pitch.control_ladrc(_rate_target_ang_vel.y, gyro_latest.y, _motors.get_pitch(), 0.5));
    	    //adrc+yaw
        	_motors.set_yaw(_ladrc_rate_yaw.control_ladrc(_rate_target_ang_vel.z, gyro_latest.z, _motors.get_yaw(), 0.5));
    	}
    	else
    	{
    		//pid+roll
            _motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll) + _actuator_sysid.x);
            _motors.set_roll_ff(get_rate_roll_pid().get_ff());
            //pid+pitch
            _motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch) + _actuator_sysid.y);
            _motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
            //pid+yaw
            _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z);
            _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
    	}
    }
    else
    {
			//pid+roll
			_motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll) + _actuator_sysid.x);
			_motors.set_roll_ff(get_rate_roll_pid().get_ff());
			//pid+pitch
			_motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch) + _actuator_sysid.y);
			_motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
			//pid+yaw
			_motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z);
			_motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
    }

    //设置为0
    _rate_sysid_ang_vel.zero();
    _actuator_sysid.zero();

    control_monitor_update();
}

以上主要是核心实现控制代码，把这里替换即可实现adrc代码的使用，注意一定要按照pid角速度的写法去写，移植起来非常快，比如找到get_rate_pitch_pid这个变量全局查找，同样位置增加adrc即可

其他内容就不上传了，遇到问题可以联系我！

4.飞行视频

视频

5.难点

adrc难点在于参数的理解与调试，很多变量彼此之间还是有联系，要想搞明白需要把理论补充上！

非线性ADRC总共有12个参数：H H0 R B DETA BT0 BT1 BT2 A1 A2 KP KD，可以通过书中的理论减少调试的参数数量

线性ADRC总共有5个参数，重点是WC W0 B H R

6.遗留问题

参数的理解 与具体如何实现调试！