參考文獻：Minimum Snap Trajectory Generation and Control for Quadrotors

PX4中多旋翼無人機的控制分為姿態控制和位置控制兩個部分。

大致解讀如下

姿態控制 mc_att_control

角度控制（外環 P）

/*** Attitude controller.* 姿態角控制 外環 P* Input: 'vehicle_attitude_setpoint' topics (depending on mode)* 輸入: 姿態設定值'vehicle_attitude_setpoint'* Output: '_rates_sp' vector, '_thrust_sp'* 輸出: 角速度設定值 '_rates_sp' 油門設定值 '_thrust_sp'*/

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8

由于偏航軸的響應比橫滾俯仰慢，這里采用的是傾轉分離的解耦合進行姿態角控制：

先進行一個傾斜（對齊Z軸），然后旋轉（對齊X軸），這樣以保證能夠將當前機體坐標系迅速地旋轉到目標坐標系，通過這種方式求解旋轉向量（詳見《視覺SLAM十四講》）僅適用于小角度情形；

對于旋轉角度大于90度的情況(cos(z,Z)<0

)時，直接將目標姿態確定的四元數虛部（旋轉瞬軸）作為現在旋轉向量。

進而計算角速度設定值

_rates_sp = _params.att_p.emult(e_R); // P控制

• 1

由于偏航響應較慢，加入前饋控制

_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff; // 偏航前饋

• 1

其中主要是旋轉向量e_R的求取

/* 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)); math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));/* axis and sin(angle) of desired rotation */ // 旋轉向量(軸 角) math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); // 向量叉乘即誤差/* calculate angle error */ float e_R_z_sin = e_R.length(); // R_z 和 R_sp_z皆為單位向量， |x|=1 float e_R_z_cos = R_z * R_sp_z;/* 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;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);math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;e_R = e_R_z_axis * e_R_z_angle; // 旋轉向量: 長度為 theta角大小/* 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 = cos(theta) * I + (1 - cos(theta)) * n * n^T + sin(theta) * skewmatrix(n)R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));} else {/* zero roll/pitch rotation */R_rp = R; }////////////// 對齊X軸 /* 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; //tan = sin / cos//////////////////////// 大角度旋轉 /////////////////////// if (e_R_z_cos < 0.0f) {/* for large thrust vector rotations use another rotation method:* calculate angle and axis for R -> R_sp rotation directly */// 大的推力向量:直接計算 R->R_sp 的旋轉向量(軸、角)math::Quaternion q_error;// 四元數組成為旋轉軸 轉過的角度 q_error.from_dcm(R.transposed() * R_sp);// 四元數實部為 cos(theta) 虛部為 u_x*sin(theta) u_y*sin u_z*sinmath::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軸的旋轉和直接旋轉的融合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; }

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54
• 55
• 56
• 57
• 58
• 59
• 60
• 61
• 62
• 63
• 64
• 65
• 66
• 67
• 68
• 69

角速度控制（內環PID）

/** Attitude rates controller.* 角速度控制環 內環 PID* Input: '_rates_sp' vector, '_thrust_sp'* 輸入: 角速度設定值'_rates_sp' 推力設定值 '_thrust_sp'* Output: '_att_control' vector* 輸出: 姿態控制量 '_att_control'*/

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8

角速度控制過程比較直觀，需要注意的是一個系數tpa(Throttle PID Attenuation，油門PID衰減)。

/* throttle pid attenuation factor */ float tpa = fmaxf(0.0f, fminf(1.0f, 1.0f - _params.tpa_slope * (fabsf(_v_rates_sp.thrust) - _params.tpa_breakpoint))); // 衰減因子范圍在 0 ~ 1 之間

• 1
• 2
• 3

在得到角速度誤差后

/* angular rates error */ // 角速度誤差 math::Vector<3> rates_err = _rates_sp - rates;

• 1
• 2
• 3

進行角速度PID控制(加前饋)

_att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +_params.rate_ff.emult(_rates_sp);

• 1
• 2

這里有幾點需要注意的：

微分控制環節的誤差量選取

標準的PID微分項D=Kd×(e(k)?e(k?1))

，但是這里因為角度的微分就是角速度，而陀螺儀可以直接測出角速度。此處沒有將微分項作為偏差的差可能是有此考慮，直接使用 D=kd×ΔGyro/t

可以增強穩定性。

問：串級PID外環可以只用PI嗎？

答：可以，因為內環相當于外環的D。

TPA的作用

TPA在此處使用是考慮到了抗積分飽和。

當系統存在一個方向的誤差時，由于積分的累加作用會使控制量一直增大，可能會使控制量達到執行器的執行閾值，如果此時誤差的方向發生改變，控制量會逐漸減小，控制量也會退出飽和區，執行器也會在閾值內執行；如果此時誤差方向還是沒有改變，控制量會繼續增大但是執行器會一直保持在閾值，此時控制量就進入了飽和區。進入飽和區越深，退出飽和區時間就會越長，在飽和區時執行器會一直在閾值位置，如果誤差發生反向，執行器不會立刻有反應，控制量會慢慢減小，等執行器推出飽和區才會有反應。這樣就會使控制的動態響應變差，控制性能變差。
抗積分飽和算法
若 u(k-1)>Umax，只累加負的誤差；若u(k-1)

位置控制mc_pos_control

位置控制同樣采用的是串級PID控制。外環為位置P控制，內環為速度PID控制。

整體控制流程入下。

保方向飽和

總結

以上是生活随笔為你收集整理的Pixhawk原生固件PX4之位姿控制算法解读的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。