写在前面
源码版本:1.6.0rc1
源码位置:Firmware-1.6.0rc1\src\modules\mc_att_control\mc_att_control_main.cpp
PX4姿态控制源码分析
从功能函数执行开始task_main()
整体框架
- 订阅数据(期望姿态、当前姿态、默认参数拷贝)
- 外环处理
- 内环处理
- 把控制量进行发布
void MulticopterAttitudeControl::task_main()
{
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
_gyro_count = math::min(orb_group_count(ORB_ID(sensor_gyro)), MAX_GYRO_COUNT);
for (unsigned s = 0; s < _gyro_count; s++) {
_sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
}
_sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
/* initialize parameters cache */
parameters_update();
任务开始执行会订阅(subscribe)一些可能用到的数据,以备后面姿态控制使用。最后一行将同路径下参数文件(mc_att_control_params.c)中参数拷贝出来。
/* wakeup source: gyro data from sensor selected by the sensor app */
px4_pollfd_struct_t poll_fds = {};
poll_fds.fd = _sensor_gyro_sub[_selected_gyro];
poll_fds.events = POLLIN;
定义对_sensor_gyro_sub事件进行阻塞等待。
while (!_task_should_exit) {
/* wait for up to 100ms for data */
int pret = px4_poll(&poll_fds, 1, 100);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("mc att ctrl: poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
perf_begin(_loop_perf);//空语句为了兼容ROS
/* run controller on gyro changes */
if (poll_fds.revents & POLLIN) {
static uint64_t last_run = 0;
float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too small (< 2ms) and too large (> 20ms) dt's */
if (dt < 0.002f) {
dt = 0.002f;
} else if (dt > 0.02f) {
dt = 0.02f;
}
/* copy gyro data */
orb_copy(ORB_ID(sensor_gyro), _sensor_gyro_sub[_selected_gyro], &_sensor_gyro);
/* check for updates in other topics */
parameter_update_poll();
vehicle_control_mode_poll();
arming_status_poll();
vehicle_manual_poll();
vehicle_status_poll();
vehicle_motor_limits_poll();
battery_status_poll();
control_state_poll();
sensor_correction_poll();
对需要做阻塞等待的数据进行阻塞等待,通过高精度定时器获取积分时间dt,对其他数据进行检查更新。
以上就是在获取数据。
/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
* or roll (yaw can rotate 360 in normal att control). If both are true don't
* even bother running the attitude controllers */
//判断是否在半自稳模式下 小舵量自稳 大舵量特技
if (_v_control_mode.flag_control_rattitude_enabled) {
//舵量判断(只有pitch roll)大舵量只控制角速度
if (fabsf(_manual_control_sp.y) > _params.rattitude_thres ||
fabsf(_manual_control_sp.x) > _params.rattitude_thres) {
//如果是半自稳模式 将不再进行外环处理 直接进入内环角速度控制
_v_control_mode.flag_control_attitude_enabled = false;
}
}
if (_v_control_mode.flag_control_attitude_enabled) {
if (_ts_opt_recovery == nullptr) {
// the tailsitter recovery instance has not been created, thus, the vehicle
// is not a tailsitter, do normal attitude control
control_attitude(dt);//姿态结算的第二步,外环处理
} else {
vehicle_attitude_setpoint_poll();
_thrust_sp = _v_att_sp.thrust;
math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Quaternion q_sp(&_v_att_sp.q_d[0]);
_ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff);
_ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp);
/* limit rates */
for (int i = 0; i < 3; i++) {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i),_params.mc_rate_max(i));
}
}
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
手动模式(manual)可以化为三种:
1. stabilize(遥感舵量转换出来的是角度控制指令,遥感转换出来的是角度数据)
2. acro(遥感舵量转换出来的是角速度指令,只控制角速度,特技飞行)
3. rattitude(半自稳模式,是上面两种模式的综合,舵量小为1. 舵量大为2. 小舵量自稳 大舵量特技)
如果使能了姿态控制,并且不需要垂直起降则进入最重要的外环控制(control_attitude(dt)),先不跟进他先把整体框架捋顺清晰。我们不要垂直起降,先不看else里的东西。再往后看是对期望角速度(_rates_sp)的限幅,对期望角速度、期望油门填充并发布,以便后面内环控制使用。
} else {
/* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode.flag_control_manual_enabled) {//如果是ACRO模式(特技飞行不控制角度)
/* manual rates control - ACRO mode */
_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
_manual_control_sp.r).emult(_params.acro_rate_max);
_thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);
/* publish attitude rates setpoint */
//角速度期望值来源于遥感舵量
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
} else {
/* attitude controller disabled, poll rates setpoint topic */
vehicle_rates_setpoint_poll();
_rates_sp(0) = _v_rates_sp.roll;
_rates_sp(1) = _v_rates_sp.pitch;
_rates_sp(2) = _v_rates_sp.yaw;
_thrust_sp = _v_rates_sp.thrust;
}
}
这一块是非正常的姿态控制外环,在使用acro模式下,因为不做角度控制,所以期望角速度来源于遥感舵量。
if (_v_control_mode.flag_control_rates_enabled) {
control_attitude_rates(dt);
内环的计算(control_attitude_rates(dt))得到控制量。后面就是对获取的控制量进行发布,这里不再粘贴了,下面进入最重要的环节,姿态结算的外环,内环函数。
/*******************************************************************************************************************************************/
先看外环 control_attitude(dt),内环主要用到的就是解耦合控制和罗德里的旋转。
void MulticopterAttitudeControl::control_attitude(float dt)
{
vehicle_attitude_setpoint_poll();//载具的姿态设置(获取期望值)
_thrust_sp = _v_att_sp.thrust;//油门值储存(姿态结算中用不到)
/* construct attitude setpoint rotation matrix */
//获取期望姿态
math::Quaternion q_sp(_v_att_sp.q_d[0], _v_att_sp.q_d[1], _v_att_sp.q_d[2], _v_att_sp.q_d[3]);
math::Matrix<3, 3> R_sp = q_sp.to_dcm();//R_sp:期望旋转矩阵
/* get current rotation matrix from control state quaternions */
//获取当前姿态
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);//当前四元数
math::Matrix<3, 3> R = q_att.to_dcm();//R:当前旋转矩阵
以上就找到了期望姿态与当前姿态的矩阵表示形式。现在要做的就是如何由当前的姿态旋转到期望姿态。
R_sp = Ryaw Rrollpitch R :先旋转roll pitch(对齐z轴) 再旋转yaw(对齐xy轴), 因为roll pitch 要比yaw响应快。
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));//R_z:当前姿态z轴
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));//R_sp_z:期望姿态z轴
/* axis and sin(angle) of desired rotation*/
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);//e_R:机体坐标系下旋转轴向量
用叉乘找到两个z轴之间的误差,因为两个z轴是基于导航坐标系的,但是飞机的旋转是基于机体系的,所以要把他们的叉乘旋转到机体坐标系上。叉乘的结果是一个向量,这个向量是垂直于两个z轴所在的平面的。所以叉乘结果的向量是可以作为旋转轴的。(我们要先对齐z轴)
/* calculate angle error */
//两z轴误差角度正弦e_R = |a||b|sinθ
float e_R_z_sin = e_R.length();
//两z轴误差角度余弦cos = |a||b|cosθ
float e_R_z_cos = R_z * R_sp_z;
向量e_R是叉乘得到的所以其大小就是 a b sinθ ,点乘得到 a b cosθ。
/* calculate weight for yaw control
*偏航控制权重*/
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
/* calculate rotation matrix after roll/pitch only rotation
* 计算旋转矩阵后,只有滚/俯仰旋转*/
math::Matrix<3, 3> R_rp;//定义roll/pitch旋转矩阵
if (e_R_z_sin > 0.0f) {
/* get axis-angle representation */
float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);//得到对齐z轴需要的旋转角度
//因为e_R_z_sin = e_R.length(),所以,e_R/e_R_z_sin是对e_R进行归一化作为旋转轴
math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
e_R = e_R_z_axis * e_R_z_angle;//航偏误差(轴角法)
yaw_w,R_sp(2,2)表示两个 z 轴 之间夹角的余弦,当两个 z 轴的夹角越大时对偏航角度的影响也越大。
e_R用旋转轴乘以旋转角,就表示两个旋转轴的误差。
/* cross product matrix for e_R_axis */
math::Matrix<3, 3> e_R_cp;//旋转轴叉积矩阵
e_R_cp.zero();
e_R_cp(0, 1) = -e_R_z_axis(2);
e_R_cp(0, 2) = e_R_z_axis(1);
e_R_cp(1, 0) = e_R_z_axis(2);
e_R_cp(1, 2) = -e_R_z_axis(0);
e_R_cp(2, 0) = -e_R_z_axis(1);
e_R_cp(2, 1) = e_R_z_axis(0);
用旋转轴构造旋转轴叉积矩阵
/* rotation matrix for roll/pitch only rotation
* 由当前旋转矩阵R、(rx ryrz)、θ经过罗德里格变换得
* 到只重合z轴所需的旋转矩阵R_rp */
R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
} else {//对应上面if (e_R_z_sin > 0.0f)
/* zero roll/pitch rotation */
R_rp = R;
}
利用罗德里的旋转公式求出R_rp:对齐z轴的中间过渡姿态。
/* R_rp and R_sp has the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
这时候,对齐z轴之后R_sp_x与R_rp_x只差一个偏航角了。
(R_rp_x % R_sp_x) * R_sp_z 这是求 sin。
R_rp_x % R_sp_x求出来是个向量,大小是sin但是还有方向呢,方向就是R_sp_z。 * R_sp_z 是把前面的向量转换为数值。可以算算这个点乘,单位向量,同方向 cos=1,最后剩下的就是仅仅是我们想要的 sin 数值了。
R_rp_x * R_sp_x 求 cos。 atan2f (sin,cos)求偏航角。
最后乘以yaw_w,yaw_w 在先旋转 roll pitch 对齐 z 轴的过程中,对偏航的影响。float yaw_w = R_sp(2, 2) * R_sp(2, 2)。yaw 权重随着 z 轴夹角增大而二次方减小,所以如 果 z 轴夹角越大,就会更加偏向于先通过转动将 z 轴夹角减小。在 z 轴夹角减小后,倾向于偏航转动使 x 轴重合。
如两个 z 轴重合 yaw_w=1,e_R(2) 就是算出来的,相当于转动过程对偏航没 有影响,这种情况确实没有影响。如果如两个 z 轴 90 度 yaw_w=0 其实 e_R(2) 偏航=0, 就是这种情况下根本不用偏航了。 yaw_w 就是对偏航转动的权重分量。如果 z 轴重合,yaw_w 就是 1,权重最大,也就是以 只有偏航转动,或者以其为主。随着 z 轴夹角增大,yaw 会两次方减小,降低偏航权重,使得转动以俯仰滚转为主。
这是小角度的姿态控制过程,小角度指在 90 度以内。如果大角度 90 度以外呢?
if (e_R_z_cos < 0.0f) {//大于 90 度,飞机立起来了,大角度姿态控制
/* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */
math::Quaternion q_error;
q_error.from_dcm(R.transposed() * R_sp);
math::Vector<3> e_R_d = q_error(0) >= 0.0f ? q_error.imag() * 2.0f : -q_error.imag() * 2.0f;
/* use fusion of Z axis based rotation and direct rotation
*使用基于Z轴旋转和直接旋转的融合*/
//R_sp(2,2)其实就是就是 XY 两个轴需要旋转的角度的 cos,
//XY 需要转的越多,那么这个权重就越小,意思就是当 XY 转角较大的时候,yaw 的控制就适当减弱一下。
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
}
e_R_z_cos < 0.0f 大于 90 度,飞机立起来了,大角度姿态控制。现在大角度下直接求现在姿态和期望姿态之间的差值,并用四元数进行表示。
q_error(0)就是 cosθ/2,q_error(0)姿态偏差在 90-180 度
q_error.imag() * 2.0f 求姿态角度差
e_R_d 最终算出来就是现在姿态和期望姿态间的直接差值,不加修饰。
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; //yaw_w = R_sp(2,2)*R_sp(2,2)。
R_sp(2,2)其实就是就是 XY 两个轴需要旋转的角度的 cos,XY 需要转的越多,那么这个权重就越小,意思就是当 XY 转角较大的时候,yaw 的控制就适当减弱一下。
这种平方的权重,当误差很大的时候,如果>1 平方就会更大,补偿更快。比如 飞机遇到风的时候要求快速的补偿误差。当误差很小的手,如<1,平方会衰减的 更快,就是当误差越小的时候我们需要弥补的越少,甚至可以不用弥补了免得浪 费资源。就如同偏航一样,我们不希望飞机机头转的太快。
0>e_R_z_cos>-1,代表两个 z 轴之间的偏差,在 90-180 度之间。随着 z 轴之间的角度变大,e_R_z_cos * e_R_z_cos 为正也变大, e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;最终计算的姿态误差中 e_R_d 大角度计算的这个偏差所占比重 direct_w = e_R_z_cos * e_R_z_cos * yaw_w 也变大。
就是两个 z 轴之间角度越大,大角度姿态控制下,最终姿态偏差越相信 e_R_d。
大角度的情况下,直接就算角度,直接控制迅速点,不讲究稳定先修正把角度拉 过去,不需要体轴那么算了,所以求的直接是角度差,先求 q_error,这个就是两个旋转矩阵之间差的四元数,这个四元数的意义本身就是旋转,然后四元数的 虚部,imag 部分,代表的 0.5*旋转角度,这里可以看看四元数定义,然后就不管什么体轴地轴,先往 R_sp 的方向转那么多角度再说。最后算出大角度下的 姿态误差,产生期望角速度。
_rates_sp = _params.att_p.emult(e_R);
最终作用外环计算产生期望的角速度,即角度有速度肯定要有角速度去弥补。而 且误差越大期望弥补的角速度越大。
/* limit rates */
for (int i = 0; i < 3; i++) {
if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
!_v_control_mode.flag_control_manual_enabled) {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i));
} else {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
}
}
/* feed forward yaw setpoint rate */
//前馈的作用会加速偏航的响应
//因为飞机能提供的偏航的力矩比较小
_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
/* weather-vane mode, dampen yaw rate */
if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
_v_att_sp.disable_mc_yaw_control == true && !_v_control_mode.flag_control_manual_enabled) {
float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
// prevent integrator winding up in weathervane mode
_rates_int(2) = 0.0f;
}
}
对期望角速度做一个限幅,再加入前馈控制,前馈的作用有两个,
- 让控制更加跟手,在你打航向的时候,输入误差会增大很多,这样 输出到下一环的控制量是变大了,所以控制会更快
- 增加抗风性
风吹飞机一般都是小角度,飞机自稳的时候一般杆量也是 0,这些情况下,都能增加下一环的控制输入,让控制更快。四旋翼航向控制优先级是最弱的,所以增加一些前馈,对航向控制有很大帮助,而 roll/pitch 不加是因为基本上串级 PID 都够了。
/*******************************************************************************************************************************************/
再看姿态内环的处理control_attitude_rates(dt)
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
_rates_int.zero();
}
/* get transformation matrix from sensor/board to body frame */
//得到传感器/板到机体系的变换矩阵
get_rot_matrix((enum Rotation)_params.board_rotation, &_board_rotation);
/* fine tune the rotation */
//微调旋转
math::Matrix<3, 3> board_rotation_offset;
board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _params.board_offset[0],
M_DEG_TO_RAD_F * _params.board_offset[1],
M_DEG_TO_RAD_F * _params.board_offset[2]);
_board_rotation = board_rotation_offset * _board_rotation;
// get the raw gyro data and correct for thermal errors
//获取原始陀螺数据并校正热误差
math::Vector<3> rates(_sensor_gyro.x * _sensor_correction.gyro_scale[0] + _sensor_correction.gyro_offset[0],
_sensor_gyro.y * _sensor_correction.gyro_scale[1] + _sensor_correction.gyro_offset[1],
_sensor_gyro.z * _sensor_correction.gyro_scale[2] + _sensor_correction.gyro_offset[2]);
// rotate corrected measurements from sensor to body frame
//旋转修正测量从传感器到机体系
rates = _board_rotation * rates;
// correct for in-run bias errors
rates(0) -= _ctrl_state.roll_rate_bias;
rates(1) -= _ctrl_state.pitch_rate_bias;
rates(2) -= _ctrl_state.yaw_rate_bias;
math::Vector<3> rates_p_scaled = _params.rate_p.emult(pid_attenuations(_params.tpa_breakpoint_p, _params.tpa_rate_p));
math::Vector<3> rates_i_scaled = _params.rate_i.emult(pid_attenuations(_params.tpa_breakpoint_i, _params.tpa_rate_i));
math::Vector<3> rates_d_scaled = _params.rate_d.emult(pid_attenuations(_params.tpa_breakpoint_d, _params.tpa_rate_d));
上面这些都是在计算当前角速度,tpa 的功能类似于一个简单的经验型的非线性 PID,就是根据油门大小调节 P 项的输出,让 控制更加符合心理预期。
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
_att_control = rates_p_scaled.emult(rates_err) +
_rates_int +
rates_d_scaled.emult(_rates_prev - rates) / dt +
_params.rate_ff.emult(_rates_sp);
_rates_sp_prev = _rates_sp;
_rates_prev = rates;
内环的PID计算,并加上了前馈控制使控制更平滑一些。
/* update integral only if motors are providing enough thrust to be effective */
if (_thrust_sp > MIN_TAKEOFF_THRUST) {
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
// Check for positive control saturation
//检查正控制饱和度
bool positive_saturation =
((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_pos) ||
((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_pos) ||
((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_pos);
// Check for negative control saturation
//检查负控制饱和度
bool negative_saturation =
((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_neg) ||
((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_neg) ||
((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_neg);
// prevent further positive control saturation
//防止进一步的正控制饱和
if (positive_saturation) {
rates_err(i) = math::min(rates_err(i), 0.0f);
}
// prevent further negative control saturation
//防止进一步的负控制饱和
if (negative_saturation) {
rates_err(i) = math::max(rates_err(i), 0.0f);
}
// Perform the integration using a first order method and do not propaate the result if out of range or invalid 使用一阶方法执行积分,如果超出范围或无效,则不要传播结果
//积分项
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (PX4_ISFINITE(rate_i) && rate_i > -_params.rate_int_lim(i) && rate_i < _params.rate_int_lim(i)) {
_rates_int(i) = rate_i;
}
}
}
/* explicitly limit the integrator state */
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
_rates_int(i) = math::constrain(_rates_int(i), -_params.rate_int_lim(i), _params.rate_int_lim(i));
}
}//end control_attitude_rates(dt)
PID 计算过程中最麻烦的是积分项的处理,防止进入饱和区,进去饱和区越深反向退出的时 候越慢,会导致飞机外在反映的迟钝。抗积分饱和,当前输出没有达到饱和时,才把本次的误差积分项累加到积分环节中。
最终内环的输出量就是_att_control这个变量,到这里内环就结束了,跳出内环对就是数据进行判断、填充、发布,姿态控制也就结束了。
总结:整体框架
- 订阅数据(期望姿态、当前姿态、默认参数拷贝)
- 外环处理( control_attitude(dt))
- 内环处理(control_attitude_rates(dt))
- 把控制量(_att_control)进行发布
以上是个人对PX4姿态控制的源码理解,如有不正确的地方感谢提出批评指正,欢迎一起讨论学习QQ:1103706199。