大神对飞控精准高度估计算法解读

飞控的高度信息主要由气压计来提供，也可包含GPS、超声波等。

跟姿态解算类似，单一的传感器所提供的信息并不能满足实际应用要求，所以同样需要多传感器数据融合。

气压计高度问题主要有以下几点，如图所示：

1、噪声相对较大，数据不够平滑，不过这个问题不大，可以使用一阶低通滤波来处理；

2、数据从上电开始会有漂移，尤其受温度影响，这里图中反应不明显；

3、最大的问题在于，受外界气流影响太大，图中50-90s左右，设定的高度并未改变，但是由于飞行器剧烈运动，导致高度变化较大，波动范围达到5m左右。

所以显然，单用气压计是无法获得精准的高度的。

GPS的高度信息：

主要与GPS性能密切相关了，如果不计成本的，使用价格高昂的GPS设备，自然高度信息是比较理想的。但是在消费级飞行器领域，这个显然不适合，普通GPS（M8N等）的高度同样误差较大，与搜星质量也有关系。

加速度积分：

前面讲过，MEMS的加速度计数据质量更差了，单纯靠加速度积分得到的高度，会在很短的时间内漂移很大。

注：这里的数据是用pixhawk飞控，去掉GPS的情况下进行的飞行测试。主要是测试在无GPS情况下，大机动定高飞行的精度。这里的pixhawk高度估计不作为精确的参考。

下面，主要描述三种高度估计（不含GPS数据）的算法，并用matlab计算结果，观察。

一、互补滤波

很简单的思路，就是将两个高度信息，按照一个权重进行融合。

算法步骤：

1.相关变量赋值，记录当前高度（气压计、GPS所得的是绝对高度，而控制器需要的是相对高度）；

2.将机体测得的加速度转换到NED坐标系下；

3.加速度数据进行一阶低通滤波，并计算加速度偏移；

4.将去除偏移的加速度进行双重积分，分别得到速度、位置；

5.与气压计的数据进行融合；

最终，较为理想的参数是barokp为0.2，即还是基本相信加速度计的数据。

二、pixhawk的高度估计算法

具体思路参考:

做定高控制时，不可避免的就要涉及到如何解算出高度信息，那高度信息又是如何获取的？参考pixhawk源码。这里只介绍只有气压计和加速度计的情况。

概述：

首先要明白，所需的高度信息是地理坐标系下的相对高度，整个算法的核心思想是由地理坐标系下的加速度通过积分，来获得速度、位置信息，而这个数据的精确程度是由机体测量的加速度通过减去偏差，再转换到地理坐标系求得的。这里气压计的作用就是计算一个校正系数来对加速度偏移量进行校正。

算法代码详解：

1. 变量初始化。

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

float acc[] = { 0.0f, 0.0f, 0.0f }; //地理坐标系（NED）的加速度数据

float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // 机体坐标系下的加速度偏移量

float corr_baro = 0.0f; // 气压计校正系数

2. 计算气压计高度的零点偏移，主要是取200个数据求平均。

baro_offset += sensor.baro_alt_meter;

baro_offset /= (float) baro_init_cnt;

3. 将传感器获取的机体加速度数据转换到地理坐标系下。

加速度数据要先去除偏移量；

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

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

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

然后转换坐标系；

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

地理坐标系下的z轴加速度是有重力加速度的，因此补偿上去。

acc[2] += CONSTANTS_ONE_G;

4. 计算气压计的校正系数

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. 将偏移向量转换到机体坐标系

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. 气压计校正系数进行校正

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

预测-校正函数：

仿真测试：

通过实际飞行，取出一份飞行数据，在matlab下编程进行数据分析。

仿真结果并未完全与飞控自身计算的高度吻合，猜测由于平台编辑器的问题，还有数据本身受到控制环的影响。至少表明该算法是有效。

三、卡尔曼滤波

建立以下模型，进行卡尔曼估计。

状态量设为：Z轴高度、Z轴速度、Z轴加速度、Z轴加速度偏移，观测量为气压计的高度和NED系Z轴加速度。理论推导相对简单，得到：

其中：

对比三种算法的结果如下：

互补滤波，由于对气压计的权重设的较大0.9，而数据噪声大，所以对应的速度估计噪声很大，不能使用。如果对气压计数据进行低通滤波，同时，气压计的权重为0.05，也就是主要相信加速度数据。

可以看到噪声问题有所好转，但是速度估计静态效果不好，因为只有检测到加速度运动，相应的速度估计才会更新值，由于气压计的权重小了，如果缓慢的高度变化并不能准确测量。所以互补滤波的参数，需要根据实际的飞行情况不断调整。而另外的卡尔曼滤波与pixhawk的算法效果相对较好。

小结：

1、气压计、GPS的高度是绝对高度，因此为了计算，需要初始化记录当前高度，从而求得相对高度；

2、本文只是提出几种算法，并不绝对说明哪种算法一定好，与前面的文章一样，没有参考数据的比较没有意义；

3、算法不是万能的，如果传感器的数据质量差，再牛逼的估计算法也很难得到精准的信息，所以最好对传感器做好结构、硬件上的处理，比如加速度减震、气压计的密封、遮光等等；

4、调参怎么调？可以先取数据，分别画出各个中间数据的波形，调整得到个大概可用的值。然后根据实际的飞行情况进行调整，主要就是决定更相信哪个数据。

部分代码（kalman M代码）：

大神对飞控精准高度估计算法解读相关推荐

大神干货：腾讯广告算法大赛亚军女极客生存图鉴
为了助力你更好地了解这次比赛,这期我们特地邀请到了去年腾讯广告大赛的亚军--来自北京邮电大学的"烧卖"同学,让她帮你为今年的广告大赛划重点,分享去年迎战各路技术大神的经验,晒出自己 ...
大神干货：腾讯广告算法大赛亚军宝藏指南带你顺利出圈
这期我们请到了去年腾讯广告算法大赛的亚军--来自电子科技大学的徐安同学.千字比赛心得,让你轻松把握比赛,避免踩坑. 大家好,我是电子科技大学的徐安,很荣幸能有机会和大家分享我在腾讯广告算法大赛中的一些 ...
大神齐聚，算法大赛复赛晋级名单揭晓！
6月23日12:00:00,由腾讯广告携手腾讯云.腾讯大数据.腾讯招聘及腾讯高校合作等合作伙伴联袂举办的2020腾讯广告算法大赛正式进入复赛阶段. 本届赛事共有来自1,000+所国内外院校和3,000 ...
[转]知乎大神YaqiLYU关于tracking方向的2013-2016趋势总览！！！强烈推荐！！！
真真实实的学习tracking也有半年了,tracking的主流趋势年年改变,每年都有新的突破.从CT.IVT到struck,到TLD.到CF,再到火热的CNN也应用到了tracking领域.作为初入 ...
大神TP_舍弃闪现转战上路韩服大神卡萨丁全新玩法
在近期版本中我们发现韩服有一名卡萨丁使用了卡萨丁一种全新的玩法--上单卡萨丁.并且上单卡萨丁在召唤师技能的选择上也非常的不同,一起来看看这种全新的打法吧: 韩服大神卡萨丁全新思路解析套路解读从韩服 ...
大盘点|6D姿态估计算法汇总（下）
作者:Tom Hardy Date: 2019-12-28 来源:大盘点|6D姿态估计算法汇总(下)
大盘点|6D姿态估计算法汇总(上)
作者:Tom Hardy Date:2019-12-27 来源:大盘点|6D姿态估计算法汇总(上)
这款NLP神器火了！关键词一键提取、结果高度可视化，堪称「小白进阶大神」的实用工具包 | 开源...
点击上方,选择星标或置顶,不定期资源大放送! 阅读大概需要15分钟 Follow小博主,每天更新前沿干货转载自:量子位如何快速优雅地处理你的NLP数据集? 试试这款号称「从小白到大神」的Texth ...
数学大神攻克猜字游戏Wordle，求解算法成绩逼近理论极限，连信息论都用上了...
梦晨发自凹非寺量子位 | 公众号 QbitAI 免费猜字小游戏Wordle正在席卷全球,火到以数百万美元的价格被收购,全球玩家数量也突破了200万. 如果你在微博.微信等地方看到这些神神秘秘的方 ...

大神对飞控精准高度估计算法解读

大神对飞控精准高度估计算法解读相关推荐

最新文章

热门文章