姿態(tài)解算篇D

基礎(chǔ)知識(shí)

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

在理解構(gòu)建四旋翼動(dòng)力學(xué)模型時(shí)，需要了解它是屬于四輸入六輸出的欠驅(qū)動(dòng)系統(tǒng)，欠驅(qū)動(dòng)主要就是輸入只值靠四個(gè)電機(jī)的拉力實(shí)現(xiàn)控制角速度、角度、上升速度、下降速度、上升距離和下降距離。動(dòng)力學(xué)模型還在研究中。

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

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

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

目前研究多集中在非線性控制領(lǐng)域，由于非線性控制對(duì)模型準(zhǔn)確性有較強(qiáng)的依賴，在模型誤差存在的條件下，PID控制更加實(shí)用，所以現(xiàn)有的基于mahony互補(bǔ)濾波器設(shè)計(jì)的算法中大多數(shù)都是基于PID反饋控制器的。

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

代碼算法分析

1、傳感器的數(shù)值模型（mahony互補(bǔ)濾波器）

首先還是貼圖吧，word編輯的公式還是搞不進(jìn)來。

2、互補(bǔ)濾波器的設(shè)計(jì)（基于四元數(shù)）

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

例如，加速度計(jì)測(cè)加速度值，其動(dòng)態(tài)響應(yīng)較慢，在高頻時(shí)信號(hào)不可用，所以可通過低通濾波器抑制高頻干擾；陀螺儀響應(yīng)快，積分后可測(cè)傾角，不過由于零點(diǎn)漂移等，在低頻段信號(hào)不好，通過高通濾波器可抑制低頻干擾。將兩者結(jié)合，就將陀螺儀和加速度計(jì)的優(yōu)點(diǎn)融合起來，得到在高頻和低頻都較好的信號(hào)，互補(bǔ)濾波需要選擇切換的頻率點(diǎn)，即高通和低通的頻率。

在深入一點(diǎn)

1、對(duì)mahony互補(bǔ)濾波算法和PX4Firmware源代碼中的姿態(tài)解算算法的一點(diǎn)見解

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

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.

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

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

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就是由陀螺儀積分以后的姿態(tài)矩陣中獲取的重力加速度向量（等同于mahony算法中的[vx,vy,vz]）；這部分都類似，主要的不同就是下面的加速度計(jì)測(cè)量數(shù)據(jù)的處理，首先通過加速度計(jì)獲取總的加速度向量，然后同過GPS或者空速管獲取運(yùn)動(dòng)加速度向量，然后通過這兩個(gè)向量之差求取純重力加速度，即：總的加速度 =運(yùn)動(dòng)加速度 + 重力加速度（此處考慮了低頻情況下的運(yùn)動(dòng)加速度，修正效果更好）。_w_accel代表權(quán)重，即類似于Kp。

上述代碼中的“%”運(yùn)算符定義為叉積運(yùn)算：

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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。