飛控的高度信息主要由氣壓計(jì)來(lái)提供，也可包含GPS、超聲波等。

跟姿態(tài)解算類似，單一的傳感器所提供的信息并不能滿足實(shí)際應(yīng)用要求，所以同樣需要多傳感器數(shù)據(jù)融合。

氣壓計(jì)高度問(wèn)題主要有以下幾點(diǎn)，如圖所示：

1、噪聲相對(duì)較大，數(shù)據(jù)不夠平滑，不過(guò)這個(gè)問(wèn)題不大，可以使用一階低通濾波來(lái)處理；

2、數(shù)據(jù)從上電開(kāi)始會(huì)有漂移，尤其受溫度影響，這里圖中反應(yīng)不明顯；

3、最大的問(wèn)題在于，受外界氣流影響太大，圖中50-90s左右，設(shè)定的高度并未改變，但是由于飛行器劇烈運(yùn)動(dòng)，導(dǎo)致高度變化較大，波動(dòng)范圍達(dá)到5m左右。

所以顯然，單用氣壓計(jì)是無(wú)法獲得精準(zhǔn)的高度的。

GPS的高度信息：

主要與GPS性能密切相關(guān)了，如果不計(jì)成本的，使用價(jià)格高昂的GPS設(shè)備，自然高度信息是比較理想的。但是在消費(fèi)級(jí)飛行器領(lǐng)域，這個(gè)顯然不適合，普通GPS（M8N等）的高度同樣誤差較大，與搜星質(zhì)量也有關(guān)系。

加速度積分：

前面講過(guò)，MEMS的加速度計(jì)數(shù)據(jù)質(zhì)量更差了，單純靠加速度積分得到的高度，會(huì)在很短的時(shí)間內(nèi)漂移很大。

注：這里的數(shù)據(jù)是用pixhawk飛控，去掉GPS的情況下進(jìn)行的飛行測(cè)試。主要是測(cè)試在無(wú)GPS情況下，大機(jī)動(dòng)定高飛行的精度。這里的pixhawk高度估計(jì)不作為精確的參考。

下面，主要描述三種高度估計(jì)（不含GPS數(shù)據(jù)）的算法，并用matlab計(jì)算結(jié)果，觀察。

一、互補(bǔ)濾波

很簡(jiǎn)單的思路，就是將兩個(gè)高度信息，按照一個(gè)權(quán)重進(jìn)行融合。

算法步驟：

1.相關(guān)變量賦值，記錄當(dāng)前高度（氣壓計(jì)、GPS所得的是絕對(duì)高度，而控制器需要的是相對(duì)高度）；

2.將機(jī)體測(cè)得的加速度轉(zhuǎn)換到NED坐標(biāo)系下；

3.加速度數(shù)據(jù)進(jìn)行一階低通濾波，并計(jì)算加速度偏移；

4.將去除偏移的加速度進(jìn)行雙重積分，分別得到速度、位置；

5.與氣壓計(jì)的數(shù)據(jù)進(jìn)行融合；

最終，較為理想的參數(shù)是barokp為0.2，即還是基本相信加速度計(jì)的數(shù)據(jù)。

二、pixhawk的高度估計(jì)算法

具體思路參考:

做定高控制時(shí)，不可避免的就要涉及到如何解算出高度信息，那高度信息又是如何獲取的？參考pixhawk源碼。這里只介紹只有氣壓計(jì)和加速度計(jì)的情況。

概述：

首先要明白，所需的高度信息是地理坐標(biāo)系下的相對(duì)高度，整個(gè)算法的核心思想是由地理坐標(biāo)系下的加速度通過(guò)積分，來(lái)獲得速度、位置信息，而這個(gè)數(shù)據(jù)的精確程度是由機(jī)體測(cè)量的加速度通過(guò)減去偏差，再轉(zhuǎn)換到地理坐標(biāo)系求得的。這里氣壓計(jì)的作用就是計(jì)算一個(gè)校正系數(shù)來(lái)對(duì)加速度偏移量進(jìn)行校正。

算法代碼詳解：

1. 變量初始化。

float z_est[2] = { 0.0f, 0.0f }; // z軸的高度、速度

float acc[] = { 0.0f, 0.0f, 0.0f }; //地理坐標(biāo)系（NED）的加速度數(shù)據(jù)

float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // 機(jī)體坐標(biāo)系下的加速度偏移量

float corr_baro = 0.0f; // 氣壓計(jì)校正系數(shù)

2. 計(jì)算氣壓計(jì)高度的零點(diǎn)偏移，主要是取200個(gè)數(shù)據(jù)求平均。

baro_offset += sensor.baro_alt_meter;

baro_offset /= (float) baro_init_cnt;

3. 將傳感器獲取的機(jī)體加速度數(shù)據(jù)轉(zhuǎn)換到地理坐標(biāo)系下。

加速度數(shù)據(jù)要先去除偏移量；

sensor.accelerometer_m_s2[0] -= acc_bias[0];

sensor.accelerometer_m_s2[1] -= acc_bias[1];

sensor.accelerometer_m_s2[2] -= acc_bias[2];

然后轉(zhuǎn)換坐標(biāo)系；

acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];

地理坐標(biāo)系下的z軸加速度是有重力加速度的，因此補(bǔ)償上去。

acc[2] += CONSTANTS_ONE_G;

4. 計(jì)算氣壓計(jì)的校正系數(shù)

corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];

5. 加速度偏移向量校正

accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;

6. 將偏移向量轉(zhuǎn)換到機(jī)體坐標(biāo)系

c += PX4_R(att.R, j, i) * accel_bias_corr[j];

acc_bias[i] += c * params.w_acc_bias * dt;

7. 加速度推算高度

inertial_filter_predict(dt, z_est, acc[2]);

8. 氣壓計(jì)校正系數(shù)進(jìn)行校正

inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);

預(yù)測(cè)-校正函數(shù)：

仿真測(cè)試：

通過(guò)實(shí)際飛行，取出一份飛行數(shù)據(jù)，在matlab下編程進(jìn)行數(shù)據(jù)分析。

仿真結(jié)果并未完全與飛控自身計(jì)算的高度吻合，猜測(cè)由于平臺(tái)編輯器的問(wèn)題，還有數(shù)據(jù)本身受到控制環(huán)的影響。至少表明該算法是有效。

三、卡爾曼濾波

建立以下模型，進(jìn)行卡爾曼估計(jì)。

狀態(tài)量設(shè)為：Z軸高度、Z軸速度、Z軸加速度、Z軸加速度偏移，觀測(cè)量為氣壓計(jì)的高度和NED系Z軸加速度。理論推導(dǎo)相對(duì)簡(jiǎn)單，得到：

其中：

對(duì)比三種算法的結(jié)果如下：

互補(bǔ)濾波，由于對(duì)氣壓計(jì)的權(quán)重設(shè)的較大0.9，而數(shù)據(jù)噪聲大，所以對(duì)應(yīng)的速度估計(jì)噪聲很大，不能使用。如果對(duì)氣壓計(jì)數(shù)據(jù)進(jìn)行低通濾波，同時(shí)，氣壓計(jì)的權(quán)重為0.05，也就是主要相信加速度數(shù)據(jù)。

可以看到噪聲問(wèn)題有所好轉(zhuǎn)，但是速度估計(jì)靜態(tài)效果不好，因?yàn)橹挥袡z測(cè)到加速度運(yùn)動(dòng)，相應(yīng)的速度估計(jì)才會(huì)更新值，由于氣壓計(jì)的權(quán)重小了，如果緩慢的高度變化并不能準(zhǔn)確測(cè)量。所以互補(bǔ)濾波的參數(shù)，需要根據(jù)實(shí)際的飛行情況不斷調(diào)整。而另外的卡爾曼濾波與pixhawk的算法效果相對(duì)較好。

小結(jié)：

1、 氣壓計(jì)、GPS的高度是絕對(duì)高度，因此為了計(jì)算，需要初始化記錄當(dāng)前高度，從而求得相對(duì)高度；

2、 本文只是提出幾種算法，并不絕對(duì)說(shuō)明哪種算法一定好，與前面的文章一樣，沒(méi)有參考數(shù)據(jù)的比較沒(méi)有意義；

3、 算法不是萬(wàn)能的，如果傳感器的數(shù)據(jù)質(zhì)量差，再牛逼的估計(jì)算法也很難得到精準(zhǔn)的信息，所以最好對(duì)傳感器做好結(jié)構(gòu)、硬件上的處理，比如加速度減震、氣壓計(jì)的密封、遮光等等；

4、 調(diào)參怎么調(diào)？可以先取數(shù)據(jù)，分別畫(huà)出各個(gè)中間數(shù)據(jù)的波形，調(diào)整得到個(gè)大概可用的值。然后根據(jù)實(shí)際的飛行情況進(jìn)行調(diào)整，主要就是決定更相信哪個(gè)數(shù)據(jù)。

部分代碼（kalman M代碼）：

總結(jié)

以上是生活随笔為你收集整理的大神对飞控精准高度估计算法解读的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

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