姿態解算篇D

基礎知識

研究多旋翼無人機前期主要需要了解其氣動布局和復雜的動力學模型，然后就是姿態解算和控制器的設計。為了實現精確四旋翼飛行器的姿態估計，首先就是需要了解各傳感器采集的數據和誤差存在的原因，然后就是各種數學運算（都是基于一定的數學平臺），比如DCM隨時間的變化、四元數微分方程、四元數隨時間的變化、重正交化等等。

在理解構建四旋翼動力學模型時，需要了解它是屬于四輸入六輸出的欠驅動系統，欠驅動主要就是輸入只值靠四個電機的拉力實現控制角速度、角度、上升速度、下降速度、上升距離和下降距離。動力學模型還在研究中。

實現四旋翼飛行器的自主飛行主要就是靠姿態解算和姿態控制。姿態控制是四旋翼飛行器實現各項工作的核心，位置控制建立在完善的姿態控制的基礎上。前面已經介紹過，僅僅依靠陀螺儀積分獲取角度值是不可靠的，由于陀螺儀由于其本身的固有的drift，積分帶來的誤差隨時間的積累越來越大；而加速度計又容易受到噪聲和震動的干擾。

因此在此基礎上使用各種算法對兩種數據進行融合，以獲取較為理想的姿態數據。比較常見的融合算法有EKF（Extended Kalman Filter）、UKF（Unscented Kalman Filter）、梯度下降法、CKF（Cubature Kalman Filter）、PF（Particle Filter）和CF（Complementary Filter）。

其中mahony姿態解算算法主要就是基于CF設計的數據融合濾波器，該算法需要重點掌握其原理思想其分為AHRS（陀螺儀、加速度計、磁力計）和IMU（陀螺儀、加速度計）兩種。

目前研究多集中在非線性控制領域，由于非線性控制對模型準確性有較強的依賴，在模型誤差存在的條件下，PID控制更加實用，所以現有的基于mahony互補濾波器設計的算法中大多數都是基于PID反饋控制器的。

接下來就是使用Simulink進行控制器建模仿真，也在研究中。

代碼算法分析

1、傳感器的數值模型（mahony互補濾波器）

首先還是貼圖吧，word編輯的公式還是搞不進來。

2、互補濾波器的設計（基于四元數）

這里講的互補濾波就是在短時間內采用陀螺儀得到的角度做為最優值，定時對加速度采樣來的加速度值進行取平均值來校正陀螺儀的得到的角度。短時間內用陀螺儀比較準確，以它為主；長時間用加速度計比較準確，這時候加大它的比重，這就是互補了，不過加速度計要濾掉高頻信號，陀螺儀要濾掉低頻信號，互補濾波器就是根據傳感器特性不同，通過不同的濾波器（高通或低通，互補的），然后再相加得到整個頻帶的信號。

例如，加速度計測加速度值，其動態響應較慢，在高頻時信號不可用，所以可通過低通濾波器抑制高頻干擾；陀螺儀響應快，積分后可測傾角，不過由于零點漂移等，在低頻段信號不好，通過高通濾波器可抑制低頻干擾。將兩者結合，就將陀螺儀和加速度計的優點融合起來，得到在高頻和低頻都較好的信號，互補濾波需要選擇切換的頻率點，即高通和低通的頻率。

在深入一點

1、對mahony互補濾波算法和PX4Firmware源代碼中的姿態解算算法的一點見解

首先是最原始mahony濾波算法中的加速度計數據修正陀螺儀數據，源碼如下。

1. // measured by Accelerate sensor 2. ax = ax / norm; 3. ay = ay / norm; 4. az = az / norm; 5. 6. // estimated direction of gravity (v) 7. vx = 2*(q1q3 - q0q2); 8. vy = 2*(q0q1 + q2q3); 9. vz = q0q0 - q1q1 - q2q2 + q3q3; 10. 11. // error is sum ofcross product between reference direction of fields and direction measured by Accelerate sensor 12. ex = ay*vz - az*vy ; 13. ey = az*vx - ax*vz ; 14. ez = ax*vy - ay*vx ; 15.

對上述代碼的理解：[ax,ay,az] 表示由機體上的加速度計實際測量到的加速度數據，根據數據融合修正原理，加速度計在低頻的情況下效果比較好，陀螺儀在高頻情況下效果好；在低頻下，忽略機體的運動加速度，即加速度計測量到的數據就是近似的純重力加速度向量。[vx,vy,vz]表示由陀螺儀積分以后的姿態矩陣中獲取的重力加速度向量（如何得到的不再贅述）。并且這兩個向量都是在機體坐標系中，兩個重力加速度向量做外積得到的向量就是所謂的誤差向量（12、13、14行代碼），且叉積的大小和陀螺儀的積分誤差成正比，用該誤差向量修正下一次的陀螺儀數據即可達到用加速度計修正陀螺儀數據誤差的效果。

然后就是PX4Firmware源碼中的加速度計數據修正陀螺儀數據，源碼如下。

1. // Accelerometer correction 2. // Project 'k' unit vector of earth frame to body frame 3. // Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f)); n系到 4. // b系 5. // Optimized version with dropped zeros 6. Vector<3> k( 7. 2.0f * (_q(1) * _q(3) - _q(0) * _q(2)), 8. 2.0f * (_q(2) * _q(3) + _q(0) * _q(1)), 9. (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3)) 10. ); 11. corr += (k % (_accel - _pos_acc).normalized()) * _w_accel; 12.

該算法中，Vector<3> k就是由陀螺儀積分以后的姿態矩陣中獲取的重力加速度向量（等同于mahony算法中的[vx,vy,vz]）；這部分都類似，主要的不同就是下面的加速度計測量數據的處理，首先通過加速度計獲取總的加速度向量，然后同過GPS或者空速管獲取運動加速度向量，然后通過這兩個向量之差求取純重力加速度，即：總的加速度 =運動加速度 + 重力加速度（此處考慮了低頻情況下的運動加速度，修正效果更好）。_w_accel代表權重，即類似于Kp。

上述代碼中的“%”運算符定義為叉積運算：

1. Vector<3> operator %(const Vector<3> &v) const { 2. return Vector<3>( 3. data[1] * v.data[2] - data[2] * v.data[1], 4. data[2] * v.data[0] - data[0] * v.data[2], 5. data[0] * v.data[1] - data[1] * v.data[0] 6. ); 7. } 8. };

總結

以上是生活随笔為你收集整理的Pixhawk代码分析-姿态解算篇D的全部內容，希望文章能夠幫你解決所遇到的問題。

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