ADRC控制器 Ardupilot & SIMULINK 飞控联合开发

simulink 同时被 3 个专栏收录
2 篇文章 0 订阅
13 篇文章 1 订阅
12 篇文章 2 订阅

使用ADRC替换掉ardupilot中的PID控制器,首先我们需要借助simulink搭建ADRC控制器,进行初步调参,仿真通过后利用simulink to C 生成我们想要的ADRC代码,插入到ardupilot中,最终调参得到ADRC飞控,开发思路借鉴基于模型的开发思路,真实飞机控制效果还待验证,已经通过Ardupilot中SITL仿真。

Adrc控制器

ADRC的控制框图如下:主要包括跟踪微分器(TD)、非线性状态误差反馈(NLSEF)、扩张状态观测器(ESO)。引用
在这里插入图片描述
L-ADRC中最重要的是ESO,ESO的核心思想是建立新的状态方程,输入被观测模型的输出与被观测模型的输入,将状态方程对被观测模型进行辨识,最终得到被观测模型的响应特性以及外部扰动,而ADRC的主要思想是对ESO观测出的外部扰动量进行补偿(博主认为就是将扰动作为前馈)。ESO的公式:
在这里插入图片描述
在simulink中搭建被控对象的模型,搭建ADRC控制器。
控制系统&模型仿真
面对ADRC的众多参数,可以参考韩京清教授的书对参数进行简化,甚至可以用粒子群算法进行寻优,此处simulink中仿真步长建议和Ardupilot中控制器更新频率保持一致400HZ。
在这里插入图片描述

博主大概调到一个不错的效果就进行下一步了。

Simulink to C++

这里使用Simulink自带模块Embedded coder,将系统中的control system单拿出来进行自动代码生成。Embedded coder功能见下图,博主使用的是MATLAB 2020A,功能模块是下图这样的。使用图中红框的Quick start向导进行设置,会检测你的模型是否有错误,编译通过会生成代码。
在这里插入图片描述

Ardupilot PID库替换

自动代码生成源文件

Ardupilot中PID库主要用在角速率环使用,而我的ADRC正是要换掉这部分的控制器,那让我们开始。
首先simulink生成的代码更新函数是rt_OneStep()。这个函数位于simulink生成代码XXX(你的.slx名).cpp中。
在这里插入图片描述

我们需要的ADRC库文件就是生成的文件中所有文件。

PID库

此处博主直接对APM的PID库进行修改。首先将ADRC库进行实例化,添加进AC_PID类(libraries/AC_PID/AC_PID.h)的末尾。

 sModelClass ADRC_model_Obj;

将ADRC参数的初始化加入到AC_PID的构造函数中(libraries/AC_PID/AC_PID.cpp)。

ADRC_model_Obj.initialize();

Ardupilot中对pid的更新是调用了

float update_all(float target, float measurement, bool limit = false);	

我们需要将ADRC的更新函数添加到update_all中

float AC_PID::update_all(float target, float measurement, bool limit)
{
    // don't process inf or NaN
    if (!isfinite(target) || !isfinite(measurement)) {
        return 0.0f;
    }

    // reset input filter to value received
    if (_flags._reset_filter) {
        _flags._reset_filter = false;
        _target = target;
        _error = _target - measurement;
        _derivative = 0.0f;
    } else {
        float error_last = _error;
        _target += get_filt_T_alpha() * (target - _target);
        _error += get_filt_E_alpha() * ((_target - measurement) - _error);

        // calculate and filter derivative
        if (_dt > 0.0f) {
            float derivative = (_error - error_last) / _dt;
            _derivative += get_filt_D_alpha() * (derivative - _derivative);
        }
    }

    // update I term
    update_i(limit);

    float P_out = (_error * _kp);
    float D_out = (_derivative * _kd);

    _pid_info.target = _target;
    _pid_info.actual = measurement;
    _pid_info.error = _error;
    _pid_info.P = P_out;
    _pid_info.D = D_out;
    
    ADRC_model_Obj.b=_b;								//ADRC参数b的更新
    ADRC_model_Obj.rtU.target_angle=_target;			//ADRC目标量
    ADRC_model_Obj.rtU.zhuangtai=measurement;		//ADRC ESO需要的反馈回来的信息
    ADRC_model_Obj._kp=_kp;ADRC_model_Obj._ki=_ki;ADRC_model_Obj._kd=_kd;		//ADRC线性组合两个参数的更新
    ADRC_model_Obj._kimax=_kimax;ADRC_model_Obj._kimin=-_kimax;
    ADRC_model_Obj._dt=_dt;


    rt_OneStep();						//ADRC的更新函数
    return ADRC_model_Obj.rtY.caozong;	//控制器返回ADRC的输出
}

以上完成了ADRC控制器的添加。

参数添加

ADRC中存在大量可调参数,根据系统带宽已经可以减少掉许多参数,但是这里我还需在地面站中添加几个参数,方便我们调试。
在AC_PID类中添加一个变量,作为接受地面站设置的参数。

AP_Float _b;

每个库的参数表会放在.cpp文件的开始,AC_PID库也不例外,在AC_PID.cpp的开头的const AP_Param::GroupInfo AC_PID::var_info[] 中添加

AP_GROUPINFO("b", 12, AC_PID, _b, 200),

其中b是地面站中显示的参数,注意还会再添加前缀显示再地面站,这里应该会最终显示为ATC_RAT_PIT_b,会随着调用pid库的类改变而增加前缀,具体参照我那篇添加参数的博客。_b是库中接受地面站参数的变量。

仿真&实验

仿真和试飞两项工作博主只完成了第一项,效果不错,利用Ardupilot提供的软件在环仿真工具包进行ADRC控制器的调参仿真。直接使用下列指令

./Tools/autotest/sim_vehicle.py -v ArduCopter --console --output 192.168.145.1 --map -D 

直接和windows下的地面站联合仿真,ps(本人不太会Xplane与ardupilot联合仿真,听说Xplane可以自建新机型,求教)。

  • 0
    点赞
  • 1
    评论
  • 2
    收藏
  • 一键三连
    一键三连
  • 扫一扫,分享海报

matlabsimulink中代数环问题的讲解及解决方法1-解决代数环方法.doc 本帖最后由 小小2008鸟 于 2012-11-30 11:26 编辑 什么是代数环?发生在两个或多个模块在输入端口具有信号直接传递而形成反馈的情况时,直接传递的模块在不知输入端口的值的情况下无法计算出输出端的值,也就是现在时刻的输出是依赖现在时刻的输入值来计算的。当这种情况出现时simulink会在每一次迭代言算完成时,去决定它是否会有解。代数回路会减缓方真执行的速度并可能会没有解。     当方程两边都出现同一个变量时,即产生了代数环。如:3y 4=y 当Simulink中存在非模型延迟时,通常会产生代数环。比如,传感器将输出信号反馈到输入端。解决代数环      连续模型,在构成代数环的反馈环节加入memory模块;      离散模型就加入单位延迟1/Z,不过加入memory后仿真会慢些,但用了加速后,这个牺牲还是值得的。另还有拆解法等,具体见下面第9行论文内容。Simulink提供了一些专门手段来拆解代数环,例如代数约束模块,积分模块的状态输出端等。这些手段可以解决一些特定的代数环问题。下面举一个利用积分模块的状态输出端的例子。    从积分模块的输人端口到输出端口是非直通的,但从积分模块的初始值输人端口到输出端口,以及从复位输人端口到输出端口却都是直通的。因此,如果从积分模块的输出端口引出的信号再经过一些直通模块后又反馈到积分模块的初始值输人端口或者复位输人端口,则构成一个代数环。    为了解决这个问题,Simulink专门为积分模块设计了一个状态端口,其输出与输出端口完全相同,仅在内部计算的时序上有细微区别,而无论是从积分模块的初始值输人端口还是从复位输人端口到状态端口都是非直通的。因此,当出现上述的代数环问题时,可 以从积分模块的状态端口引出信号。这样,代数环就被拆解了
©️2021 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值