PX4从放弃到精通（六）：PX4姿态控制代码解析

文章目录

• 前言
• 一、期望姿态生成
• 二、姿态角控制
• 三、姿态角速率控制

前言

一个人可以走的更快，一群人才能走的更远，交流学习加qq:2096723956
更多保姆级PX4 ROS学习视频：https:///ZeUDKqy
PX4固件版本1.11.0
参考　https://zhuanlan.zhihu.com/p/81847591

一、期望姿态生成

代码位置

代码在mc_att_control_main.cpp中，具体如下．

void
MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, bool reset_yaw_sp)
{
	vehicle_attitude_setpoint_s attitude_setpoint{};

从四元数中获取当前偏航角

const float yaw = Eulerf(q).psi();

如果需要重置的话将期望偏航角重置为当前偏航角

if (reset_yaw_sp) {
	_man_yaw_sp = yaw;
}

如果遥控器油门大于0.05或者airmode为roll_pitch_yaw

 else if (_manual_control_setpoint.z > 0.05f || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw)

获取最大偏航角速率

  {
		const float yaw_rate = math::radians(_param_mpc_man_y_max.get());

根据最大偏航角速率和遥控器偏航输入(归一化后)计算期望偏航角速率

attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate;

根据期望偏航角速率计算期望偏航角(求积分)

	_man_yaw_sp = wrap_pi(_man_yaw_sp   attitude_setpoint.yaw_sp_move_rate * dt);
}

遥控器横滚俯仰通道低通滤波，如下图

_man_x_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
_man_y_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
_man_x_input_filter.update(_manual_control_setpoint.x * _man_tilt_max);
_man_y_input_filter.update(_manual_control_setpoint.y * _man_tilt_max);
const float x = _man_x_input_filter.getState();
const float y = _man_y_input_filter.getState();

用旋转向量表示期望的倾斜量（即横滚和俯仰），旋转轴即下面的(v(0), v(1), 0.f)，旋转角度即该向量的摸．这里先进行角度限幅，对向量v取模，表示倾斜角度．

Vector2f v = Vector2f(y, -x);
float v_norm = v.norm(); 

倾斜角度限幅

if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle
	v *= _man_tilt_max / v_norm;
}

轴角转四元数

Quatf q_sp_rpy = AxisAnglef(v(0), v(1), 0.f);

转换代码如下图

四元数转欧拉角，分两步进行

Eulerf euler_sp = q_sp_rpy;

第一步四元数转DCM矩阵

第二步DCM矩阵转欧拉角

对期望的横滚和俯仰进行赋值．

attitude_setpoint.roll_body = euler_sp(0);
attitude_setpoint.pitch_body = euler_sp(1);

补偿偏航角
前面的轴角只是取了遥控器的横滚和俯仰的旋转量，因此旋转轴是水平方向的，在转化为欧拉角后，会产生一个额外的偏航角euler_sp(2)，如下，其中q2=0．

attitude_setpoint.yaw_body = _man_yaw_sp   euler_sp(2);

如果是vtol，则始终以期望的偏航角方向叠加俯仰和横滚，而不是以当前的偏航

	if (_vtol) {

计算航向的误差

float yaw_error = wrap_pi(attitude_setpoint.yaw_body - yaw);

定义Z轴向量

Vector3f zB = {0.0f, 0.0f, 1.0f};

定义仅包含俯仰和横滚旋转的旋转矩阵

Dcmf R_sp_roll_pitch = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, 0.0f);

计算旋转z轴后的向量

Vector3f z_roll_pitch_sp = R_sp_roll_pitch * zB;

定义表示偏航角误差的旋转矩阵

Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error);

将偏航角误差补偿到代表横滚和俯仰旋转的向量中，相当于将以期望偏航角为基准的俯仰和横滚旋转变换到以当前偏航角为基准的俯仰和横滚，这在偏航角存在较大误差时，避免了偏航偏航通道与俯仰横滚通道的耦合．

z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;

从向量中获取俯仰和横滚的期望角度值

attitude_setpoint.roll_body = -asinf(z_roll_pitch_sp(1));
attitude_setpoint.pitch_body = atan2f(z_roll_pitch_sp(0), z_roll_pitch_sp(2));
}

欧拉角转四元数，油门取遥控器油门值，赋值并发布

Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body);
q_sp.copyTo(attitude_setpoint.q_d);
attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_setpoint.z);
attitude_setpoint.timestamp = hrt_absolute_time();
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
}

二、姿态角控制

代码位置

定义期望姿态四元数，由上一节期望姿态生成求得．

Quatf qd = _attitude_setpoint_q;

计算当前姿态四元数对应的机体坐标系Z轴单位向量，在机载NED系下表示

const Vector3f e_z = q.dcm_z();

计算期望姿态四元数对应的机体坐标系Z轴单位向量，在机载NED系下表示

const Vector3f e_z_d = qd.dcm_z();

计算去除旋转误差后仅代表倾斜误差的四元数，计算公式如下：

Quatf qd_red(e_z, e_z_d);

无旋转运动时，直接赋值为期望四元数

if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
	qd_red = qd;
}

否则将旋转轴从N系转换到B系：

结合当前姿态，我们可以得到仅代表倾斜运动的期望四元数：

qd_red *= q;是将上面两步合成了一步．

else {
// transform rotation from current to desired thrust vector into a world frame reduced desired attitude
qd_red *= q;
}
根据期望四元数和倾斜期望四元数可以得到代表旋转运动的期望四元数

Quatf q_mix = qd_red.inversed() * qd;

根据旋转运动的性质，q_mix一定可以表示为（cos(α_mix/2),0,0,sin(α_mix/2))：

q_mix.canonicalize();
q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);

限制旋转误差，最后结合倾斜运动和受限制旋转运动组成新的期望四元数

qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));

根据新四元数姿态期望计算四元数误差

const Quatf qe = q.inversed() * qd;

根据实际意义选取姿态误差为：eq=2*sign(qe(0))*qe(1:3)

const Vector3f eq = 2.f * qe.canonical().imag();

根据姿态误差计算期望角速度

matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain);

增加偏航角速度前馈

rate_setpoint  = q.inversed().dcm_z() * _yawspeed_setpoint;

角速度期望输出限幅

for (int i = 0; i < 3; i  ) {
	rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
}
return rate_setpoint;

三、姿态角速率控制

代码位置

角速率控制
获取角速率误差，期望姿态由姿态角控制生成

Vector3f rate_error = rate_sp - rate;

PID控制

const Vector3f torque = _gain_p.emult(rate_error)   _rate_int - _gain_d.emult(angular_accel)   _gain_ff.emult(rate_sp);

未落地时才更新积分

if (!landed) {
	updateIntegral(rate_error, dt);
}

return torque;

更新积分
抗积分饱和

for (int i = 0; i < 3; i  ) {

防止积分正向饱和

if (_mixer_saturation_positive[i]) {
	rate_error(i) = math::min(rate_error(i), 0.f);
}

防止积分负向饱和

if (_mixer_saturation_negative[i]) {
	rate_error(i) = math::max(rate_error(i), 0.f);
}

积分项因子i_factor，用于抵消积分过程中的非线性效应，否则在误差较大时积分会快速累积．当误差较小时，该项接近１，对积分项基本无影响，当误差较大时，该项接近０，对积分项有较大的削弱．

float i_factor = rate_error(i) / math::radians(400.f);
i_factor = math::max(0.0f, 1.f - i_factor * i_factor);

求积分

float rate_i = _rate_int(i)   i_factor * _gain_i(i) * rate_error(i) * dt;

限幅

	if (PX4_ISFINITE(rate_i)) {
		_rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i));
	}
}