APM的姿態控制部分有大量的用於變換的函數，最近分析了兩個常用的函數，以前看代碼只看框架，覺得不就是姿態嗎，不就是歐拉角，四元數，旋轉矩陣嗎？現在看過演算法細節才知道每個演算法的實現背後都是嚴謹的物理假設和數學推導，基礎知識太重要，還有太多需要學習。

函數名：AC_PosControl::accel_to_lean_angles

位置：librariesAC_AttitudeControlAC_PosControl.cpp

用途：將NED坐標系下的期望加速度轉換成機體坐標系下的傾斜角度(pitch,roll)源程序：

void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const
{
float accel_right, accel_forward;

// rotate accelerations into body forward-right frame
accel_forward = accel_x_cmss*_ahrs.cos_yaw() + accel_y_cmss*_ahrs.sin_yaw();
accel_right = -accel_x_cmss*_ahrs.sin_yaw() + accel_y_cmss*_ahrs.cos_yaw();

// update angle targets that will be passed to stabilize controller
pitch_target = atanf(-accel_forward/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI);
float cos_pitch_target = cosf(pitch_target*M_PI/18000.0f);
roll_target = atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI);
}

數學推導：

假設飛行器z軸受力平衡，即在z軸上保持靜止。

已知NED坐標系下期望加速度accel_x_cmss，accel_y_cmss(融合之後的加速度數據為NED坐標系下)和遙控輸入的yaw，求機體坐標系下pitch角和roll角。1.需要先將yaw角進行補償，即只剩下傾斜角。將NED坐標系下的水平加速度做繞z旋轉，得到沒有旋轉只有傾斜的中間狀態,該狀態下的x軸方向加速度為accel_forward，y軸加速度為accel_righ。NED坐標系轉機體坐標系繞z軸旋轉矩陣為：

2.通過accel_forward，accel_righ，計算pitch，roll。應為飛機在z軸上受力平衡，所以機體坐標系下飛機受到的合外力將在z軸方向上產生一個-G的力才能使飛機在z軸受力平衡。如圖

即支持飛機在NED坐標系z軸保持靜止需要-G，而且需要x軸上產生accel_forward，y軸上產生accel_righ，這些都是由機體坐標系下的合外力-F提供的。

所以有：

其中

是機體坐標系轉到NED坐標系的旋轉矩陣(之前已經進行過yaw方向的旋轉了所以這個矩陣裏的yaw為0)。

可得：

所以有：

所以：

函數名：void Quaternion::to_axis_angle

位置：librariesAP_Mathquaternion.cpp

功能：將四元數轉換成軸角形式原函數：

void Quaternion::to_axis_angle(Vector3f &v)
{
float l = sqrtf(sq(q2)+sq(q3)+sq(q4));
v = Vector3f(q2,q3,q4);
if (!is_zero(l)) {
v /= l;
v *= wrap_PI(2.0f * atan2f(l,q1));
}
}

單位四元素可以表示為：

單位四元數有：

又因為：

可以得到：

所以：

但是函數裏的四元素不是單位四元素所以要進行單位化

令:

所以：

以上兩個函數反覆運用在姿態控制部分，深入理解能夠幫助你瞭解姿態的本質，和姿態控制的思想。

附上我畫的姿態控制流程圖可以幫助理解：

注微信公眾號，回復【姿態】即可獲取結構圖Visio文件，同時歡迎加我的個人微信進行交流哦。

weixin.qq.com/r/BiiosH- (二維碼自動識別)

推薦閱讀：

相關文章