1 程序架构

(1)主要原理

Chenglin Li:自抗扰控制理论(一)ADRC的原理

Chenglin Li:自抗扰控制理论(八)扩张状态观测器跟踪误差分析

Chenglin Li:自抗扰控制理论(十六)对N阶非线性系统设计自抗扰控制器

(2)RungeKutta计算微分方程组;

Chenglin Li:数值计算(四十八)捕食者与被捕食者模型

Chenglin Li:数值计算(六十)RungeKutta求解微分方程组的C++程序

(3)LADRC封装类的接口:

  • 参考指令ref,
  • 控制器带宽Wc,
  • 观测器带宽Wo,
  • 控制输出系数b,
  • 步长h,
  • 传感器测量输出y;

2 头文件封装定义LADRC类

#pragma once
#include<iostream>
#include<iomanip>
#include<vector>
#include"Const.h"
 
using namespace std;
/*
程序功能:
1、LADRC类的功能
2、提供的接口包含:
   参考输入信号ref、系统输出返回值y、控制量输出u


By Chenglin Li
_2020.11.30_

*/
class LADRC  
//class LADRC
{
private:
	float _h;// 步长
	float _t1 ; //初始和末尾时刻 
	//LESO和控制器参数
	float _ref;
	float _y;
	float _u;
	float _Wo;//LESO带宽
	float _Wc;//控制带宽
	float _b;//控制输入系数
	float beta_Wo[ODE_NUM + 1];//LESO系数
	float beta_Wc[ODE_NUM ];//控制器系数
	vector<float> _z; //表示LESO返回值
	vector<float> _x;////表示RK计算的当前节点值
	float _t;
	vector<float> _s();
	//vector<float> _dx;
	//vector<float> _y1;
	//vector<float> _y2;
	//vector<float> _y3;
	//vector<float> _y4;

public:
	//构造函数
	//LADRC(float, float , float , float , float,float );
	LADRC(float Wo, float Wc, float b, float y, float h, float ref);
	void Controller();
	float Output();
	vector<float> LESO(float, vector<float>);
	//void LESO();
	//LADRC计算中使用到的相关函数
	float jiecheng(int n);
	float comb(int n, int m);
	float powj(float Wo, int n);
	void calc();
	//vector<float>Lx(float , vector<float>  );
	void Lx();
};

3 LADRC类的实现

#include"LADRC.h"
#include<iostream>
#include<vector>
using namespace std;
/*
程序功能:
1、LADRC控制器的实现
2、RungeKutta计算微分方程组,封装为RungeKutta头文件和类
3、LESO和System微分方程组作为子函数
4、Controller作为子函数

*/

//构造函数初始化

LADRC::LADRC(float Wo, float Wc, float b, float y, float h ,float ref) 
: _b(b), _ref(ref), _z(ODE_NUM + 1, 0), _x(ODE_NUM + 1, 1), _Wc(Wc), _Wo(Wo), _t1(0), _h(h), _u(0), _y(y)
{
	int i;
	//生成LESO系数
	for (i = 0; i < ODE_NUM + 1; i++)
	{
		beta_Wo[i] = powj(_Wo, i + 1)*comb(ODE_NUM + 1, i + 1);

	}
	//生成LESO系数
	for (i = 0; i < ODE_NUM + 1; i++)
	{
		beta_Wo[i] = powj(_Wc, i + 1)*comb(ODE_NUM + 1, i + 1);
	}
	//控制律系数
	for (i = 0; i < ODE_NUM; i++)
	{
		beta_Wc[i] = powj(_Wc, ODE_NUM - i)*comb(ODE_NUM, ODE_NUM - i);
	}
	for (i = 0; i < ODE_NUM; i++)
		cout << "beta_Wc[" << i << "]=" << beta_Wc[i] << endl;
	for (i = 0; i < ODE_NUM+1; i++)
		cout << "beta_Wo[" << i << "]=" << beta_Wo[i] << endl;
	Controller();// 构造函数调用控制器计算

}
//控制量输出
float LADRC::Output()
{         
	return _u;
}
//控制器计算函数
//vector<float> fx(float t, float x[]);
void LADRC::Controller()
{
	int i;
	calc();
	_u = _z[ODE_NUM]+beta_Wc[0]*(_z[0]-_ref);;//取z3
	for (i = 1; i < ODE_NUM; i++)
	{
		_u = _u + beta_Wc[i] * _z[i];
	}
	_u = -_u / _b;

	
	 
}
float LADRC::powj(float Wo, int n)
{
	int i;
	float z=1;
	if (n == 0)
		return 1;
	else
	{
		for (i = n; i >= 1; i--)
		{
			z = z*Wo;
		}
	}
	return z;

}
//组合数计算公式
float LADRC::comb(int n, int m)
{
	if (n < 0 || n < 0)
		return 0;
	else
		return jiecheng(n) / jiecheng(m) / jiecheng(n - m);

}
//计算阶乘子函数
float LADRC::jiecheng(int n)
{
	if (n < 0)
		return 0;
	else if (n == 0 || n == 1)
		return 1.0;
	else
		return n*jiecheng(n - 1);
}
//LESO微分方程组定义
vector<float> LADRC::LESO(float t, vector<float> x )
//void LADRC::LESO()
{
	int i;
	float e;
	vector<float> dx(ODE_NUM+1,0);//
	e = _y - _x[0];
	for (i = 0; i < ODE_NUM -1; i++)
	{
		dx[i] = beta_Wo[i]*e+x[i + 1];
	}
	dx[ODE_NUM - 1] = beta_Wo[ODE_NUM - 1] * e + _b*_u;
	dx[ODE_NUM] = beta_Wo[ODE_NUM] * e;
	return dx;

}
//迭代计算函数 
void LADRC::calc()
{
	int j;
	//vector<float> s(ODE_NUM+1,0);
	Lx();
	vector<float> _s(ODE_NUM + 1, 0);
	for (j = 0; j<ODE_NUM+1; j++)
		_x[j] =_x[j] + _s[j];//更新当前ODE计算的值
	_t = _t + _h;// 更新时间节点
	_z = _x;//返回当前节点解算的LESO

}
//RungeKutta计算所需项 
//vector<float>  LADRC::Lx(float _t, vector<float> _x)
void  LADRC::Lx()
{
	int j;
	vector<float> x1(ODE_NUM + 1, 0),x2(ODE_NUM + 1, 0), x3(ODE_NUM + 1, 0);
	vector<float> y1(ODE_NUM + 1, 0),y2(ODE_NUM + 1, 0),y3(ODE_NUM + 1, 0),y4(ODE_NUM + 1, 0);// 使用vector数据类型,子函数可返回数组
	//N = ODE_NUM + 1;
	vector<float> _s(ODE_NUM+1, 0);//
	y1 = LESO(_t, _x);
	

	for (j = 0; j<ODE_NUM+1; j++)
		x1[j] = _x[j] + _h / 2 *y1[j];
	y2 = LESO(_t + _h / 2, x1);
	for (j = 0; j<ODE_NUM+1; j++)
		x2[j] = _x[j] + _h / 2 * y2[j];
	y3 = LESO(_t + _h / 2, x2);
	for (j = 0; j<ODE_NUM+1; j++)
		x3[j] = _x[j] + _h*y3[j];
	y4 = LESO(_t + _h, x3);
	for (j = 0; j<ODE_NUM+1; j++)
		_s[j] = _s[j] + y1[j];
	for (j = 0; j<ODE_NUM+1; j++)
		_s[j] = _s[j] + 2 * y2[j];
	for (j = 0; j<ODE_NUM+1; j++)
		_s[j] = _s[j] + 2 * y3[j];
	for (j = 0; j<ODE_NUM+1; j++)
		_s[j] = _s[j] + y4[j];
	for (j = 0; j < ODE_NUM+1; j++)
		_s[j] = _s[j] * _h / 6;

}
 

4 Const.h头文件

#pragma once
#define ODE_NUM 2

5 测试主函数

#include<iostream>
#include"LADRC.h"
using namespace std;

int main()
{
	float u = 0;
	//LADRC(float Wo, float Wc, float b, float y, float h, float ref);
	LADRC ladrc(10, 10, 1, 0.8, 0.02, 1);
	u=ladrc.Output();
	cout << "u=" << u<<endl;

	return 0;


}  

——2020.12.01——