pixhawk的高度解算算法解读

内容导读

互联网集市收集整理的这篇技术教程文章主要介绍了pixhawk的高度解算算法解读，小编现在分享给大家，供广大互联网技能从业者学习和参考。文章包含2200字，纯文字阅读大概需要4分钟。

内容图文

转载自：http://blog.sina.com.cn/s/blog_8fe4f2f40102wo50.html

pixhawk的高度解算算法解读

(2016-04-11 11:40:39)

转载▼

标签： 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下编程进行数据分析。

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

 

以上所述：如有错误，欢迎指出。

另：1.预测函数很好理解，但是校正函数是根据什么理论推导出来的？目前尚不清楚。
2.该算法用来气压计和加速度数据，因此减震需要做好，气压计还受温飘影响，数据在开始阶段会漂移。

内容总结

以上是互联网集市为您收集整理的pixhawk的高度解算算法解读全部内容，希望文章能够帮你解决pixhawk的高度解算算法解读所遇到的程序开发问题。 如果觉得互联网集市技术教程内容还不错，欢迎将互联网集市网站推荐给程序员好友。

内容备注

版权声明：本文内容由互联网用户自发贡献，该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请发送邮件至 gblab@vip.qq.com 举报，一经查实，本站将立刻删除。

内容手机端

---