1.建议保留原程序
复制ardupilot文件夹到当前目录,命名为ardupilot_ARDC
cp -r ardupilot ardupilot_copy
2.切换Sub-4.5版本
Sub-4.1版本libraries里没有AC_CustomControl文件夹,我们需要用到这个文件夹所以需要进行切换分支,在当前ardupilot_ARDC终端目录下输入命令切换分支。
git fetch --tags
git checkout -b new-branch origin/Sub-4.5
git submodule update --init --recursive
切换分支后,可能会遗留一些与旧分支不兼容的构建文件。可以尝试清理构建文件:
./waf clean
我这里遇到了一个错误提示我需要安装 empy,安装命令
python -m pip install empy==3.3.4
更新 pip
: 首先,尝试更新 pip
版本。运行以下命令
python3 -m pip install --upgrade pip
更新后可以试试是否可以编译成功
./waf sub
成功切换分支后libraries目录下会出现AC_CustomControl文件夹
添加自定义控制器
1.复制
复制Library/AC_CustomControl文件夹下面的AC_CustomControl_Empty.cpp 和 AC_CustomControl_Empty.h 并重命名,如AC_CustomControl_ADRC.cpp、AC_CustomControl_ADRC.h;
2.更改函数名
将 AC_CustomControl_ADRC.cpp、AC_CustomControl_ADRC.h 中所有的
AC_CustomControl_Empty 替换为 AC_CustomControl_ADRC。
将.cpp .h中所有的宏定义AP_CUSTOMCONTROL_EMPTY_ENABLED替换为 AP_CUSTOMCONTROL_ADRC_ENABLED
3.包含头文件
将实现自定义算法ADRC的头文件包含进AC_CustomControl_ADRC.h中:
4.自定义控制器个数增加一个
将自定义控件变量的最大数量增加 1 ,在文件AC_CustomControl.h中,找不到就全局搜索一下;
当前自定义控制器总共有:NONE,EMPTY,PID 共3中,但是从0开始,因此CUSTOMCONTROL_MAX_TYPES默认值为2,现在多增加了一种ADRC,所以要把CUSTOMCONTROL_MAX_TYPES定义为3,如:
#define CUSTOMCONTROL_MAX_TYPES 3
5.增加一个枚举值
在文件AC_CustomControl.h中找到一下部分,增加 CONT_ADRC = 3,
enum class CustomControlType : uint8_t {CONT_NONE = 0,CONT_EMPTY = 1,CONT_PID = 2,CONT_ADRC = 3,}; // controller that should be used
6. 添加一个新的后端头文件
在 AC_CustomControl.cpp 中增加刚创建的头文件,将其放在其他后端包含下。
#include "AC_CustomControl_ADRC.h"
7.添加新的后端参数
还是AC_CustomControl.cpp 中在参数列表var_info中增加自定义控制器的相关参数。
数组索引、后端参数前缀和参数表索引加 1。将其放在另一个后端的参数下。
// parameters for ADRC controllerAP_SUBGROUPVARPTR(_backend, "3_", 8, AC_CustomControl, _backend_var_info[2]),
8. init 函数中增加相关代码
在 AC_CustomControl.cpp 中的 init 函数中增加允许生成新自定义控制器后台的相关代码,如:
case CustomControlType::CONT_ADRC:_backend = new AC_CustomControl_ADRC(*this, _ahrs, _att_control, _motors, _dt);_backend_var_info[get_type()] = AC_CustomControl_ADRC::var_info;break;
9.添加自定义控制算法文件
在AC_CustomControl文件夹下添加,我添加文件夹名为AC_ADRC,AC_ADRC文件下为AC_ADRC.h,AC_ADRC.cpp
AC_ADRC.h如下
#pragma once#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>class AC_ADRC {
public:AC_ADRC(float B00, float dt);CLASS_NO_COPY(AC_ADRC);float update_all(float target, float measurement, bool limit);void reset_eso(float measurement);float fal(float e, float alpha, float delta);float sign(float x);// Reset filtervoid reset_filter() {_flags.reset_filter = true;}// flagsstruct ap_adrc_flags {bool reset_filter :1; // true when input filter should be reset during next call to set_input} _flags;static const struct AP_Param::GroupInfo var_info[];
protected:// parametersAP_Float _wc; // Response bandwidth in rad/sAP_Float _wo; // State estimation bandwidth in rad/sAP_Float _b0; // Control gainAP_Float _limit;AP_Float _delta;AP_Int8 _order;// ESO interal variablesfloat _z1;float _z2;float _z3;float _dt;};
AC_ADRC.cpp如下
#include <AP_Math/AP_Math.h>
#include "AC_ADRC.h"
#define __AP_LINE__ __LINE__
// table of user settable parameters
const AP_Param::GroupInfo AC_ADRC::var_info[] = {// @Param: WC// @DisplayName: ADRC control bandwidth(rad/s)AP_GROUPINFO("WC",1,AC_ADRC,_wc,10),// @Param: WO// @DisplayName: ADRC ESO bandwidth(rad/s)AP_GROUPINFO("WO",2,AC_ADRC,_wo,15),// @Param: B0// @DisplayName: ADRC control input gainAP_GROUPINFO("B0",3,AC_ADRC,_b0,10),// @Param: DELT// @DisplayName: ADRC control linear zone lengthAP_GROUPINFO("DELT",4,AC_ADRC,_delta,1.0),// @Param: ORDR// @DisplayName: ADRC control model orderAP_GROUPINFO("ORDR",5,AC_ADRC,_order,1),// @Param: LM// @DisplayName: ADRC control output limitAP_GROUPINFO("LM",6,AC_ADRC,_limit,1.0f),AP_GROUPEND
};AC_ADRC::AC_ADRC(float B00, float dt)
{AP_Param::setup_object_defaults(this, var_info);_dt = dt;_b0.set(B00);// reset input filter to first value received_flags.reset_filter = true;
}float AC_ADRC::update_all(float target, float measurement, bool limit)
{// don't process inf or NaNif (!isfinite(target) || !isfinite(measurement)) {return 0.0;}if (_flags.reset_filter) {_flags.reset_filter = false;}// Get controller errorfloat e1 = target - _z1;// control derivation errorfloat e2 = -_z2;// state estimation errorfloat e = _z1 - measurement;float output = 0.0;float output_limited = 0;float dmod = 1.0f;float sigma = 1.0f/(sq(e) + 1.0f);switch (_order){case 1:{// Nonlinear control lawoutput = (_wc * fal(e1,0.5f,_delta) - sigma * _z2)/_b0;// Limit outputif (is_zero(_limit.get())) {output_limited = output;} else {output_limited = constrain_float(output * dmod,-_limit,_limit);}// State estimationfloat fe = fal(e,0.5,_delta);float beta1 = 2 * _wo;float beta2 = _wo * _wo;_z1 = _z1 + _dt * (_z2 - beta1*e + _b0 * output_limited);_z2 = _z2 + _dt * (-beta2 * fe);}break;case 2:{float kp = sq(_wc);float kd = 2*_wc;// Nonlinear control lawoutput = (kp * fal(e1,0.5f,_delta) + kd * fal(e2,0.25,_delta) - sigma * _z3)/_b0;// Limit outputif (is_zero(_limit.get())) {output_limited = output * dmod;} else {output_limited = constrain_float(output * dmod,-_limit,_limit);}// State estimationfloat beta1 = 3 * _wo;float beta2 = 3 * _wo * _wo;float beta3 = _wo * _wo * _wo;float fe = fal(e,0.5,_delta);float fe1 = fal(e,0.25,_delta);_z1 = _z1 + _dt * (_z2 - beta1 * e);_z2 = _z2 + _dt * (_z3 - beta2 * fe + _b0 * output_limited);_z3 = _z3 + _dt * (- beta3 * fe1);;}break;default:output_limited = 0.0;break;}return output_limited;
}void AC_ADRC::reset_eso(float measurement)
{_z1 = measurement;_z2 = 0.0;_z3 = 0.0;
}float AC_ADRC::fal(float e, float alpha, float delta)
{if (is_zero(delta)) {return e;}if (fabsf(e) < delta) {return e / (powf(delta, 1.0f-alpha));} else {return powf(fabsf(e), alpha) * sign(e);}
}float AC_ADRC::sign(float x)
{if (x > 0) {return 1;} else if (x < 0) {return -1;} else {return 0;}
}
10.添加新参数。
可以按照此文章将参数添加到库
在类 AC_CustomControl_ADRC 中添加所需的参数(对象),如:
AC_ADRC _rate_roll_cont;
AC_ADRC _rate_pitch_cont;
AC_ADRC _rate_yaw_cont;
将这些参数登记到 AC_CustomControl_ADRC.cpp 的 var_info 中,包括 @Param ~ @Increment 注释,以便可以在地面站读取和修改:
const AP_Param::GroupInfo AC_CustomControl_ADRC::var_info[] = {// @Param: RAT_RLL_LM// @DisplayName: ADRC roll axis control output limit// @User: AdvancedAP_SUBGROUPINFO(_rate_roll_cont, "RAT_RLL_", 1, AC_CustomControl_ADRC, AC_ADRC),// @Param: RAT_PIT_LM// @DisplayName: ADRC pitch axis control output limit// @User: AdvancedAP_SUBGROUPINFO(_rate_pitch_cont, "RAT_PIT_", 2, AC_CustomControl_ADRC, AC_ADRC),// @Param: RAT_YAW_LM// @DisplayName: ADRC yaw axis control output limit// @User: AdvancedAP_SUBGROUPINFO(_rate_yaw_cont, "RAT_YAW_", 3, AC_CustomControl_ADRC, AC_ADRC),AP_GROUPEND
};
11.在后端的构造函数中初始化类对象
在AC_CustomControl_ADRC.cpp中初始化参数对象
_rate_roll_cont(100.0, dt),
_rate_pitch_cont(100.0, dt),
_rate_yaw_cont(10.0, dt)
别忘记逗号啊!!!
12.将自定义控制算法的代码整合进 AC_CustomControl_ADRC .cpp中的 update 函数和 reset 函数中
其中,update 是控制器单步函数;reset 是重置函数;
在 file 的 update 函数中添加您的控制器。此函数返回一个 3 维向量,分别由 roll、pitch、yaw 和 mixer 输入组成
// Active disturbance rejection controller// only rate control is implementedVector3f rate_target = _att_control->rate_bf_targets();Vector3f gyro_latest = _ahrs->get_gyro_latest();Vector3f motor_out;motor_out.x = _rate_roll_cont.update_all(rate_target.x, gyro_latest.x, _motors->limit.roll);motor_out.y = _rate_pitch_cont.update_all(rate_target.y, gyro_latest.y, _motors->limit.pitch);motor_out.z = _rate_yaw_cont.update_all(rate_target.z, gyro_latest.z, _motors->limit.yaw);return motor_out;
在文件的 reset 函数中添加了 reset 功能。用户有责任添加适当的控制器重置功能。这在很大程度上取决于控制器,如果不在 SITL 中测试,就不应该从另一个后端复制粘贴它。
Vector3f gyro_latest = _ahrs->get_gyro_latest();_rate_roll_cont.reset_eso(gyro_latest.x);_rate_pitch_cont.reset_eso(gyro_latest.y);_rate_yaw_cont.reset_eso(gyro_latest.z);
获取目标姿态:atti_control->get_attitude_target_quat()
获取目标角速度:atti_control->rate_bf_targets()
获取陀螺仪的最新测量值:_ahrs->get_gyro_latest()
13.添加编译路径
在本例子中,因为文件AC_ADRC.h和AC_ADRC.cpp被存放在目录AC_ADRC中,要想编译它们,应该把路径告诉 waf :
在 ardupilot/Tools/ardupilotwaf/boards.py 中找到'AC_CustomControl'添加:
注意添加逗号!!!
'AC_CustomControl/AC_ADRC'
14.编译生成固件
./waf configure --board fmuv3 --enable-custom-controller --target sub
./waf sub