前言

一個人可以走的更快，一群人才能走的更遠，交流學(xué)習(xí)加qq:2096723956
更多保姆級PX4 ROS學(xué)習(xí)視頻：https:///ZeUDKqy
PX4固件版本1.11.0
參考　https://zhuanlan.zhihu.com/p/81847591

一、期望姿態(tài)生成

代碼位置

代碼在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{};

從四元數(shù)中獲取當前偏航角

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());

根據(jù)最大偏航角速率和遙控器偏航輸入(歸一化后)計算期望偏航角速率

attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate;

根據(jù)期望偏航角速率計算期望偏航角(求積分)

	_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();

用旋轉(zhuǎn)向量表示期望的傾斜量（即橫滾和俯仰），旋轉(zhuǎn)軸即下面的(v(0), v(1), 0.f)，旋轉(zhuǎn)角度即該向量的摸．這里先進行角度限幅，對向量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;
}

軸角轉(zhuǎn)四元數(shù)

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

轉(zhuǎn)換代碼如下圖

四元數(shù)轉(zhuǎn)歐拉角，分兩步進行

Eulerf euler_sp = q_sp_rpy;

第一步四元數(shù)轉(zhuǎn)DCM矩陣

第二步DCM矩陣轉(zhuǎn)歐拉角

對期望的橫滾和俯仰進行賦值．

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

補償偏航角
前面的軸角只是取了遙控器的橫滾和俯仰的旋轉(zhuǎn)量，因此旋轉(zhuǎn)軸是水平方向的，在轉(zhuǎn)化為歐拉角后，會產(chǎn)生一個額外的偏航角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};

定義僅包含俯仰和橫滾旋轉(zhuǎn)的旋轉(zhuǎn)矩陣

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

計算旋轉(zhuǎn)z軸后的向量

Vector3f z_roll_pitch_sp = R_sp_roll_pitch * zB;

定義表示偏航角誤差的旋轉(zhuǎn)矩陣

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

將偏航角誤差補償?shù)酱頇M滾和俯仰旋轉(zhuǎn)的向量中，相當于將以期望偏航角為基準的俯仰和橫滾旋轉(zhuǎn)變換到以當前偏航角為基準的俯仰和橫滾，這在偏航角存在較大誤差時，避免了偏航偏航通道與俯仰橫滾通道的耦合．

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));
}

歐拉角轉(zhuǎn)四元數(shù)，油門取遙控器油門值，賦值并發(fā)布

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);
}

二、姿態(tài)角控制

代碼位置

定義期望姿態(tài)四元數(shù)，由上一節(jié)期望姿態(tài)生成求得．

Quatf qd = _attitude_setpoint_q;

計算當前姿態(tài)四元數(shù)對應(yīng)的機體坐標系Z軸單位向量，在機載NED系下表示

const Vector3f e_z = q.dcm_z();

計算期望姿態(tài)四元數(shù)對應(yīng)的機體坐標系Z軸單位向量，在機載NED系下表示

const Vector3f e_z_d = qd.dcm_z();

計算去除旋轉(zhuǎn)誤差后僅代表傾斜誤差的四元數(shù)，計算公式如下：

Quatf qd_red(e_z, e_z_d);

無旋轉(zhuǎn)運動時，直接賦值為期望四元數(shù)

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

否則將旋轉(zhuǎn)軸從N系轉(zhuǎn)換到B系：

結(jié)合當前姿態(tài)，我們可以得到僅代表傾斜運動的期望四元數(shù)：

qd_red *= q;是將上面兩步合成了一步．

else {
// transform rotation from current to desired thrust vector into a world frame reduced desired attitude
qd_red *= q;
}
根據(jù)期望四元數(shù)和傾斜期望四元數(shù)可以得到代表旋轉(zhuǎn)運動的期望四元數(shù)

Quatf q_mix = qd_red.inversed() * qd;

根據(jù)旋轉(zhuǎn)運動的性質(zhì)，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);

限制旋轉(zhuǎn)誤差，最后結(jié)合傾斜運動和受限制旋轉(zhuǎn)運動組成新的期望四元數(shù)

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

根據(jù)新四元數(shù)姿態(tài)期望計算四元數(shù)誤差

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

根據(jù)實際意義選取姿態(tài)誤差為：eq=2*sign(qe(0))*qe(1:3)

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

根據(jù)姿態(tài)誤差計算期望角速度

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;

三、姿態(tài)角速率控制

代碼位置

角速率控制
獲取角速率誤差，期望姿態(tài)由姿態(tài)角控制生成

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，用于抵消積分過程中的非線性效應(yīng)，否則在誤差較大時積分會快速累積．當誤差較小時，該項接近１，對積分項基本無影響，當誤差較大時，該項接近０，對積分項有較大的削弱．

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));
	}
}