前言
一個人可以走的更快，一群人才能走的更遠(yuǎn)，交流學(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ù)中獲取當(dāng)前偏航角
const float yaw = Eulerf(q).psi();
如果需要重置的話將期望偏航角重置為當(dāng)前偏航角
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，則始終以期望的偏航角方向疊加俯仰和橫滾，而不是以當(dāng)前的偏航
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)的向量中，相當(dāng)于將以期望偏航角為基準(zhǔn)的俯仰和橫滾旋轉(zhuǎn)變換到以當(dāng)前偏航角為基準(zhǔ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;
計算當(dāng)前姿態(tài)四元數(shù)對應(yīng)的機體坐標(biāo)系Z軸單位向量，在機載NED系下表示
const Vector3f e_z = q.dcm_z();
計算期望姿態(tài)四元數(shù)對應(yīng)的機體坐標(biāo)系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é)合當(dāng)前姿態(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);
}
防止積分負(fù)向飽和
if (_mixer_saturation_negative[i]) {
rate_error(i) = math::max(rate_error(i), 0.f);
}
積分項因子i_factor，用于抵消積分過程中的非線性效應(yīng)，否則在誤差較大時積分會快速累積．當(dāng)誤差較小時，該項接近１，對積分項基本無影響，當(dā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));
}
}
|
