1 程序架构
(1)主要原理
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——
评论(0)
您还未登录,请登录后发表或查看评论