目录
文章目录
摘要
本节主要记录自己学习ardupilot PID控制算法过程，欢迎批评指正。
1.PID简介
在工程实际中，应用最为广泛的调节器控制规律为比例、积分、微分控制，简称PID控制，又称PID调节。PID控制器问世至今已有近70年历史，它 以其结构简单、稳定性好、工作可靠、调整方便而成为工业控制的主要技术之一。当被控对象的结构和参数不能完全掌握，或得不到精确的数学模型时，控制理论的 其它技术难以采用时，系统控制器的结构和参数必须依靠经验和现场调试来确定，这时应用PID控制技术最为方便。即当我们不完全了解一个系统和被控对象，或 不能通过有效的测量手段来获得系统参数时，最适合用PID控制技术。PID控制，实际中也有PI和PD控制。PID控制器就是根据系统的误差，利用比例、 积分、微分计算出控制量进行控制的。最好的PID教程参考：PID教学视频
1.比例（P）控制
比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差（Steady-state error）。比例控制只有误差才起作用，没有误差不进行任何处理，带来的坏处就是当误差等于0时，会导致系统不受控。
2.积分（I）控制
在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的 或简称有差系统（System with Steady-state Error）。为了消除稳态误差，在控制器中必须引入“积分项”。积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积 分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到接近于零。因此，比例+积分（PI）控制器，可以使系统在进入稳态后几乎无稳 态误差。
积分项对过去小的误差可以实现大的累计控制作用，缺陷是根据过去累计经验来评估当前信息，过去是好的，认为现在就是好的，当积分累计误差等于0时，对系统不进行控制。
3.微分（D）控制
在微分控制中，控制器的输出与输入误差信号的微分（即误差的变化率）成正比关系。自动控制系统在克服误差的调节过程中可能会出现振荡甚至失稳。其原因是由于存在有较大惯性组件（环节）或有滞后（delay）组件，具有抑制误差的作用， 其变化总是落后于误差的变化。解决的办法是使抑制误差的作用的变化“超前”，即在误差接近零时，抑制误差的作用就应该是零。这就是说，在控制器中仅引入 “比例”项往往是不够的，比例项的作用仅是放大误差的幅值，而需要增加的是“微分项”，它能预测误差变化的趋势，这样，具有比例+微分的控制器，就能 够提前使抑制误差的控制作用等于零，甚至为负值，从而避免了被控量的严重超调。所以对有较大惯性或滞后的被控对象，比例+微分（PD）控制器能改善系统在 调节过程中的动态特性。
2.PID分类
数字PID控制算法通常分为位置式PID和增量式PID：
1.位置式PID
2.增量式PID
3.ardupilot PID控制算法源码
#include <AP_Math/AP_Math.h>
#include "AC_PID.h"
const AP_Param::GroupInfo AC_PID::var_info[] = {
// @Param: P
// @DisplayName: PID Proportional Gain
// @Description: P Gain which produces an output value that is proportional to the current error value
//P增益，产生与当前误差值成比例的输出值
AP_GROUPINFO("P", 0, AC_PID, _kp, 0),
// @Param: I
// @DisplayName: PID Integral Gain
// @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error
//I增益，它产生的输出与误差的大小和累积量成正比
AP_GROUPINFO("I", 1, AC_PID, _ki, 0),
// @Param: D
// @DisplayName: PID Derivative Gain
// @Description: D Gain which produces an output that is proportional to the rate of change of the error
//D增益，产生与误差变化率成比例的输出
AP_GROUPINFO("D", 2, AC_PID, _kd, 0),
// 3 was for uint16 IMAX
// @Param: FF
// @DisplayName: FF FeedForward Gain
// @Description: FF Gain which produces an output value that is proportional to the demanded input
// FF增益，产生与所需输入成比例的输出值
AP_GROUPINFO("FF", 4, AC_PID, _kff, 0),
// @Param: IMAX
// @DisplayName: PID Integral Maximum
// @Description: The maximum/minimum value that the I term can output
//I项可以输出的最大/最小值
AP_GROUPINFO("IMAX", 5, AC_PID, _kimax, 0),
// 6 was for float FILT
// 7 is for float ILMI and FF
// index 8 was for AFF
// @Param: FLTT
// @DisplayName: PID Target filter frequency in Hz
// @Description: Target filter frequency in Hz
// @Units: Hz
//PID目标滤波器频率（单位：Hz）
AP_GROUPINFO("FLTT", 9, AC_PID, _filt_T_hz, AC_PID_TFILT_HZ_DEFAULT),
// @Param: FLTE
// @DisplayName: PID Error filter frequency in Hz
// @Description: Error filter frequency in Hz
// @Units: Hz
//PID误差滤波器频率（单位：Hz）
AP_GROUPINFO("FLTE", 10, AC_PID, _filt_E_hz, AC_PID_EFILT_HZ_DEFAULT),
// @Param: FLTD
// @DisplayName: PID Derivative term filter frequency in Hz
// @Description: Derivative filter frequency in Hz
// @Units: Hz
//PID微分项滤波器频率（单位：Hz）
AP_GROUPINFO("FLTD", 11, AC_PID, _filt_D_hz, AC_PID_DFILT_HZ_DEFAULT),
AP_GROUPEND
};
//定义构造函数，初始化相关参数－－－－－ Constructor
AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, float dt) :
_dt(dt),
_integrator(0.0f),
_error(0.0f),
_derivative(0.0f)
{
//从ｅｅｐｒｏｍ加载相关参数值－－－－load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
_kp = initial_p; //初始化ｋｐ
_ki = initial_i; //初始化ｋｉ
_kd = initial_d; //初始化ｋｄ
_kff = initial_ff;//初始化ｆｆ
_kimax = fabsf(initial_imax); //初始化积分项最大值
filt_T_hz(initial_filt_T_hz); //PID目标滤波器频率（单位：Hz）
filt_E_hz(initial_filt_E_hz); //PID误差滤波器频率（单位：Hz）
filt_D_hz(initial_filt_D_hz); //PID微分项滤波器频率（单位：Hz）
//将输入过滤器重置为收到的第一个值
_flags._reset_filter = true;
//初始化ｐｉｄ ｌｏｇ信息全部为０
memset(&_pid_info, 0, sizeof(_pid_info));
}
//set_dt-设置步长时间步长（单位是秒）－－－－－－ set_dt - set time step in seconds
void AC_PID::set_dt(float dt)
{
// set dt and calculate the input filter alpha
//设置dt并计算输入滤波器alpha
_dt = dt;
}
// filt_T_hz - set target filter hz
void AC_PID::filt_T_hz(float hz)
{
_filt_T_hz.set(fabsf(hz));
}
// filt_E_hz - 设置误差滤波频率－－－－－－ set error filter hz
void AC_PID::filt_E_hz(float hz)
{
_filt_E_hz.set(fabsf(hz));
}
// filt_D_hz -设置微分滤波频率ｈｚ------- set derivative filter hz
void AC_PID::filt_D_hz(float hz)
{
_filt_D_hz.set(fabsf(hz));
}
// update_all - set target and measured inputs to PID controller and calculate outputs
// target and error are filtered
// the derivative is then calculated and filtered
// the integral is then updated based on the setting of the limit flag
update_all-设置目标和测量输入值到PID控制器，并计算输出目标和误差。然后计算微分项并进行过滤。最后根据设定的积分极限限制标志更新积分项
float AC_PID::update_all(float target, float measurement, bool limit)
{
//不处理无穷大或者无穷小值－－－－don't process inf or NaN
//在一些情况会出现无效的浮点数，例如除0，例如负数求平方根等，像这类情况，获取到的浮点数的值是无效的。
//NaN 即 Not a Number 非数字
//INF 即 Infinite 无穷大
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); //根据limit这个值设定是否执行积分限制
//计算出比例项
float P_out = (_error * _kp);
//计算出微分项
float D_out = (_derivative * _kd);
//传递pid log信息
_pid_info.target = _target;
_pid_info.actual = measurement;
_pid_info.error = _error;
_pid_info.P = P_out;
_pid_info.D = D_out;
//最终计算出PID=Ｐ＋Ｉ＋Ｄ
return P_out + _integrator + D_out;
}
// update_error - set error input to PID controller and calculate outputs
// target is set to zero and error is set and filtered
// the derivative then is calculated and filtered
// the integral is then updated based on the setting of the limit flag
// Target and Measured must be set manually for logging purposes.
// todo: remove function when it is no longer used.
//update_error---设置误差到ＰＩＤ控制器和计算输出
//目标设定为０话误差是设置滤波
//然后进行计算微分和滤波处理
//积分项跟进设定的限制标志位，目标和测量必须通过手动设置才能进行日志记录
float AC_PID::update_error(float error, bool limit)
{
// don't process inf or NaN
//isfinite()函数是cmath标头的库函数，用于检查给定值是否为有限值？
//它接受一个值( float ， double或long double )，如果该值是有限的，则返回1；否则，返回1。 0，否则。
if (!isfinite(error))
{
return 0.0f;
}
//初始化为０
_target = 0.0f;
//复位滤波器和接收值－－－－－－reset input filter to value received
if (_flags._reset_filter)
{
_flags._reset_filter = false;
_error = error;
_derivative = 0.0f;
} else
{
//传递误差给上次
float error_last = _error;
//计算误差累计并进行滤波处理
_error += get_filt_E_alpha() * (error - _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 = 0.0f;
_pid_info.actual = 0.0f;
_pid_info.error = _error;
_pid_info.P = P_out;
_pid_info.D = D_out;
//得到误差值
return P_out + _integrator + D_out;
}
// update_i - update the integral
// If the limit flag is set the integral is only allowed to shrink
//update_i-更新积分项
//如果设置了极限标志，则积分只允许收缩
void AC_PID::update_i(bool limit)
{
//判断积分选项是否设置０同时ｄｔ是否为正直
if (!is_zero(_ki) && is_positive(_dt))
{
// Ensure that integrator can only be reduced if the output is saturated
//确保只有当输出饱和时，积分器才能减小
if (!limit || ((is_positive(_integrator) && is_negative(_error))
|| (is_negative(_integrator) && is_positive(_error)))) //出现饱和
{
//通过limit进行限制，当limit=0,不进行限制,当limit=1,进行限制
_integrator += ((float)_error * _ki) * _dt;
//限制在一定范围内
_integrator = constrain_float(_integrator, -_kimax, _kimax);
}
} else
{
_integrator = 0.0f; //设置积分等于０
}
//传递log信息
_pid_info.I = _integrator;
}
//获取比例项结果
float AC_PID::get_p() const
{
return _error * _kp;
}
//获取积分项结果
float AC_PID::get_i() const
{
return _integrator;
}
//获取微分项结果
float AC_PID::get_d() const
{
return _kd * _derivative;
}
//获取ｆｆ作用后结果
float AC_PID::get_ff()
{
_pid_info.FF = _target * _kff;
return _target * _kff;
}
// todo: remove function when it is no longer used.
//todo:不再使用时删除函数。
float AC_PID::get_ff(float target)
{
float FF_out = (target * _kff);
_pid_info.FF = FF_out;
return FF_out;
}
//复位积分
void AC_PID::reset_I()
{
_integrator = 0;
}
//复位积分平滑
void AC_PID::reset_I_smoothly()
{
float reset_time = AC_PID_RESET_TC * 3.0f;
uint64_t now = AP_HAL::micros64();
if ((now - _reset_last_update) > 5e5 ) {
_reset_counter = 0;
}
if ((float)_reset_counter < (reset_time/_dt))
{
_integrator = _integrator - (_dt / (_dt + AC_PID_RESET_TC)) * _integrator;
_reset_counter++;
} else
{
_integrator = 0;
}
_reset_last_update = now;
}
//加载参数
void AC_PID::load_gains()
{
_kp.load();
_ki.load();
_kd.load();
_kff.load();
_kimax.load();
_kimax = fabsf(_kimax);
_filt_T_hz.load();
_filt_E_hz.load();
_filt_D_hz.load();
}
// save_gains -保存增益到eeprom---- save gains to eeprom
void AC_PID::save_gains()
{
_kp.save();
_ki.save();
_kd.save();
_kff.save();
_kimax.save();
_filt_T_hz.save();
_filt_E_hz.save();
_filt_D_hz.save();
}
/// Overload the function call operator to permit easy initialisation
//重载这个函数允许容易实现初始化
void AC_PID::operator()(float p_val, float i_val,
float d_val, float ff_val, float imax_val, float input_filt_T_hz, float input_filt_E_hz, float input_filt_D_hz, float dt)
{
_kp = p_val;
_ki = i_val;
_kd = d_val;
_kff = ff_val;
_kimax = fabsf(imax_val);
_filt_T_hz = input_filt_T_hz;
_filt_E_hz = input_filt_E_hz;
_filt_D_hz = input_filt_D_hz;
_dt = dt;
}
// get_filt_T_alpha - get the target filter alpha
//获取目标滤波系数
float AC_PID::get_filt_T_alpha() const
{
return get_filt_alpha(_filt_T_hz);
}
// get_filt_E_alpha - get the error filter alpha
//获取误差滤波系数
float AC_PID::get_filt_E_alpha() const
{
return get_filt_alpha(_filt_E_hz);
}
// get_filt_D_alpha - get the derivative filter alpha
//获取微分项滤波系数
float AC_PID::get_filt_D_alpha() const
{
return get_filt_alpha(_filt_D_hz);
}
// get_filt_alpha - calculate a filter alpha
//获取过滤器α-计算过滤器α
float AC_PID::get_filt_alpha(float filt_hz) const
{
//判断参数是否等于０
if (is_zero(filt_hz))
{
return 1.0f;
}
//计算alpha------calculate alpha
float rc = 1 / (M_2PI * filt_hz);
return _dt / (_dt + rc);
}
//设置积分项限幅度
void AC_PID::set_integrator(float target, float measurement, float i)
{
set_integrator(target - measurement, i);
}
//设置积分项限幅度
void AC_PID::set_integrator(float error, float i)
{
_integrator = constrain_float(i - error * _kp, -_kimax, _kimax);
_pid_info.I = _integrator;
}
//设置积分项限幅度
void AC_PID::set_integrator(float i)
{
_integrator = constrain_float(i, -_kimax, _kimax);
_pid_info.I = _integrator;
}
4.ardupilot PID参数
1.姿态角速度控制器
void Copter::fast_loop()
{
....................
//运行只需要IMU数据的低电平速率控制器
attitude_control->rate_controller_run(); //角速度控制器
...............
}
执行函数
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();
//进行横滚角度的PID控制,最终输出数据到电机控制
_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控制,最终输出数据到电机控制
_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控制,最终输出数据到电机控制
_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);
//数据清零
_rate_sysid_ang_vel.zero();
_actuator_sysid.zero();
//控制监视更新
control_monitor_update();
}
从中可以看出主要调用的函数是update_all（）函数
float AC_PID::update_all(float target, float measurement, bool limit)
{
//不处理无穷大或者无穷小值－－－－don't process inf or NaN
//在一些情况会出现无效的浮点数，例如除0，例如负数求平方根等，像这类情况，获取到的浮点数的值是无效的。
//NaN 即 Not a Number 非数字
//INF 即 Infinite 无穷大
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); //根据limit这个值设定是否执行积分限制
//计算出比例项
float P_out = (_error * _kp);
//计算出微分项
float D_out = (_derivative * _kd);
//传递pid log信息
_pid_info.target = _target;
_pid_info.actual = measurement;
_pid_info.error = _error;
_pid_info.P = P_out;
_pid_info.D = D_out;
//最终计算出PID=Ｐ＋Ｉ＋Ｄ
return P_out + _integrator + D_out;
}
其中积分限制幅度的理解如下：
void AC_PID::update_i(bool limit)
{
//判断积分选项是否设置０同时ｄｔ是否为正直
if (!is_zero(_ki) && is_positive(_dt)) //积分时间是0.0025s=400hz
{
// Ensure that integrator can only be reduced if the output is saturated
//确保只有当输出饱和时，积分器才能减小
//出现饱和
//当limit=1时，完全取决于后面两项
//当limit=0时，if函数必然进行
//update_i（）函数你理解有点小问题：
//当limit=1时，if函数不一定进行，仅有两种情况才会进行积分限制幅度:。
//情况1：(is_positive(_integrator) && is_negative(_error)---具体含义是过去一段时间积分累计总和未达标
// （误差一直存在也就是基本没有达到设定值），同时这个时候误差是负值，说明比例项认为当前控制已经超过设定值，
// 比例认为已经超标，此时积分限制幅度就要起作用，
//情况2：(is_negative(_integrator) && is_positive(_error))---具体含义是过去一段时间积分累计总和超标
// （误差一直存在，也就是过去一段时间累计一级超标），同时这个时候误差是正值，说明比例项认为当前控制未超过设定值，
// 此时积分限制幅度也要要起作用，总的来说就是比例作用和积分作用对控制器是反作用
if (!limit || ((is_positive(_integrator) && is_negative(_error)) //未达标+超过了，不一致就进行限制幅度
|| (is_negative(_integrator) && is_positive(_error)))) //超标+未超过，不一致就进行限制幅度
{
//通过limit进行限制，当limit=0,不进行限制,当limit=1,进行限制
_integrator += ((float)_error * _ki) * _dt;
//限制在一定范围内
_integrator = constrain_float(_integrator, -_kimax, _kimax);
}
} else
{
_integrator = 0.0f; //设置积分等于０
}
//传递log信息
_pid_info.I = _integrator;
}
其中MP地面站对应的参数如下：
串口打印出来的数据
//读取到角速度环PID参数
//pid_roll_kp=0.15; pid_roll_ki=0.135 ;pid_roll_kd=0.0036 ;pid_roll__kff=0; T=20 E=0 D=20 I=0.5
//pid_pitch_kp=0.15; pid_pitch_ki=0.135 ;pid_pitch_kd=0.0036 ;pid_pitch__kff=0; T=20 E=0 D=20 I=0.5
//pid_yaw_kp=0.2; pid_yaw_ki=0.018 ;pid_yaw_kd=0.007 ;pid_yaw__kff=0; T=20 E=2.5 D=0 I=0.5
2.姿态控制器
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
//从姿态误差计算目标角速度信息
_rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);
Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(const Vector3f &attitude_error_rot_vec_rad)
{
Vector3f rate_target_ang_vel;
// Compute the roll angular velocity demand from the roll angle error
//根据横滚角误差计算横滚角速度
if (_use_sqrt_controller)
{
rate_target_ang_vel.x = sqrt_controller(attitude_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt);
} else {
rate_target_ang_vel.x = _p_angle_roll.kP() * attitude_error_rot_vec_rad.x;
}
// todo: Add Angular Velocity Limit
// Compute the pitch angular velocity demand from the pitch angle error
//todo:增加角速度限制
//根据俯仰角误差计算俯仰角速度
if (_use_sqrt_controller)
{
rate_target_ang_vel.y = sqrt_controller(attitude_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt);
} else
{
rate_target_ang_vel.y = _p_angle_pitch.kP() * attitude_error_rot_vec_rad.y;
}
// todo: Add Angular Velocity Limit
// Compute the yaw angular velocity demand from the yaw angle error
//todo:增加角速度限制
//根据偏航角误差计算偏航角速度需求
if (_use_sqrt_controller)
{
//采用平方根控制器
rate_target_ang_vel.z = sqrt_controller(attitude_error_rot_vec_rad.z, _p_angle_yaw.kP(),
constrain_float(get_accel_yaw_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS), _dt);
} else
{
//误差
rate_target_ang_vel.z = _p_angle_yaw.kP() * attitude_error_rot_vec_rad.z;
}
//todo: Add Angular Velocity Limit
//todo:增加角速度限制
return rate_target_ang_vel;
}
MP地面站对应的参数如下：
3.位置PID控制器
1.高度控制器PID
pos_control->update_z_controller();
//调用z轴位置控制器
run_z_controller();
1.计算垂直高度误差
//使用开平方控制器从_pos_error.z计算出_vel_target.z
_vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms, _dt);
2.计算垂直速度PID
_accel_target.z = _p_vel_z.get_p(_vel_error.z);
3.计算垂直加速度PID
thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f;
