小马哥robofly四轴气压计定高的不完全解读

我看过正点原子的定高算法，好像是直接用气压计测量得到的高度作为反馈进行，而小马哥的robofly是结合气压计获得的高度和运动情况下获得Z轴的位移2者结合来获取高度，但是2者结合的过程看得一知半解，下面仅提供思考的思路，仅做参考，互相学习。（其实robofly并没有定高功能，或者说定高仅是通过单级PID进行控制）

气压计定高基本的思路是：多传感器融合定高

通过传感器检测飞行器或无人机当前的气压和加速度数据
利用当前的气压数据计算高度变化，利用当前加速度数据计算高度变化
根据所述气压数据计算的高度变化对加速度数据计算得出的位移大小进行修正，修正加速度计算的高度中时间累积的影响，根据加速度数据计算的高度修正气流波动对气压计算高度变化的影响，将利用气压和加速度数据计算的高度变化进行相互校正获得飞行器高度数据进行融合
根据融合的数据确定飞行器的当前高度

当气压大幅度波动但加速度变化不明显时，此时对位移的计算以加速度为主要参考量，气压只是将新测量到的气压值作为目标值，逐步逼近，如果后面测量到的气压又恢复到原来值附近，目标值会变回原来值，让气压波动的影响变得非常小，如果新测量到的气压值一直维持大的变化，没有回复到原来值附近，气压结果就趋向新测量到的值，而且趋近速度会越来越快，保证良好的响应速度

如果气压值恒定平稳，加速度值平均变化明显，对速度和位移的计算以气压值为主；如果加速度平均值相对恒定，气压值变化明显，对速度和位移的计算以加速度值为主；如果气压值恒定平稳，加速度平均值相对恒定，对速度和位移的计算长期以气压结果为准、瞬时以加速度结果为准，两者相互融合；如果气压值和加速度值平均值变化均明显，当两者变化一致时认为飞机在运动，当两者不一致时认为传感器异常，进入保护状态，引入经验值辅助修正计算结果

根据加速度计算的速度和位移随着温升的在一个方向上快速变化，对加速度计的温漂进行修正，加速度计修正后的结果和气压结果进行融合，减小气压计温漂的影响

nav_t nav;         //NED frame in earth
extern uint8_t AccbUpdate;
uint8_t Altitude_mode = 0;
float Altitude = 2;   //进行定高控制的高度，2米
float z_est[3];   // estimate z Vz  Az    通过加速度和速度预估无人机在Z轴上的位移
static float w_z_baro=10.0f;   //这3个不知道是啥，可能是常量
static float w_z_acc=30.0f;
static float w_acc_bias=0.05f;/* acceleration in NED frame */
float accel_NED[3] = { 0.0f, 0.0f, -G };   //地球坐标性下的加速度，Z轴选垂直向上为正方向
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
float corr_acc[] = { 0.0f, 0.0f, 0.0f };   // N E D ,  m/s2
float acc_bias[] = { 0.0f, 0.0f, 0.0f };   // body frame , 这是机体坐标的加速度，需要乘以转化矩阵=地球坐标系
float corr_baro = 0.0f;                        //m（气压计矫正系数）

static void inertial_filter_predict(float dt, float x[2],float acc);  //得到机体的位移和速度
static void inertial_filter_correct(float e, float dt, float x[3], int i, float w);  ///* 惯性滤波器的位移与速度预测 */
static void inertial_filter_predict(float dt, float x[2],float acc)
{x[0] += x[1] * dt + acc * dt * dt / 2.0f; // 位移 S = Vt + At^2/2 x[1] += acc * dt;                         // 速度 V = At
}static void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
{float ewdt = e * w * dt;x[i] += ewdt;if (i == 0) {x[1] += w * ewdt;x[2] += w * w * ewdt / 3.0f;}else if (i == 1) {x[2] += w * ewdt;}
}

主要看下面进行融合高度的函数：

void Altitude_Combine(void)
{uint8_t i,j;float dt = 0.005; //次程序没次被调用的时间间隔float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; /* 加速度计的偏移矫正 */if(AccbUpdate)  //这个的判断条件在imu.c里面{accb[0] -= acc_bias[0];  //去零偏处理，这里无论是mpu6050还是其他传感器都要进行去零偏处理accb[1] -= acc_bias[1];accb[2] -= acc_bias[2];for(i=0; i<3; i++){accel_NED[i]=0.0f;for(j=0; j<3; j++){accel_NED[i] += DCMgb[j][i]* accb[j];  //二重循环进行矩阵的相乘（1 3 ）*（3 3）=（1 3）}}accel_NED[2]=-accel_NED[2];   //注意方向的正负corr_acc[2] = accel_NED[2] + G - z_est[2]; //corr_acc[] 机身坐标系 转换到 地理坐标系的加速度AccbUpdate = 0;} if(ALT_Updated){corr_baro = 0 - FBM.AltitudeFilter - z_est[0];    //初始化值为零 测量值与估计值求差ALT_Updated = 0;}accel_bias_corr[2] -= corr_baro * w_z_baro * w_z_baro; //这里不知道为啥if(accel_bias_corr[2]){for (i = 0; i < 3; i++)  //矩阵相乘，其实这里的c也可定义为数组c[3]{float c = 0.0f;for (j = 0; j < 3; j++) {c += DCMgb[i][j] * accel_bias_corr[j];}acc_bias[i] += c * w_acc_bias * dt;     //accumulate bias偏差积分（acc_bias[i] 较正后的机体系下的加速度）}acc_bias[2]=-acc_bias[2];/* 惯性滤波器的位移与速度预测 */inertial_filter_predict(dt, z_est,z_est[2]);/* 高度惯性滤波器的较正 */inertial_filter_correct(corr_baro, dt, z_est, 0, w_z_baro);      //0.5f (corr_baro气压计较正系数 ;  z_est预测的Z轴数据 z , vz , az)inertial_filter_correct(corr_acc[2], dt, z_est, 2, w_z_acc);       //20.0f(corr_acc[] 机身坐标系 转换到 地理坐标系的加速度值 ;  z_est预测的Z轴数据 z , vz , az)}nav.z =z_est[0];  //以上都是为了获取nav.z但是后面的程序中却没有用到nav.vz=z_est[1];nav.az=z_est[2];//    printf("nav.z:%0.2f  nav.vz:%0.2f  nav.az:%0.2f  Altiude:%0.2f\r\n",nav.z,nav.vz,nav.az,FBM.Altitude);
}//下面的其实就是简单的PID处理
void Altitude_Control(void)
{PID_Postion_Cal(&PID_ALT,Altitude,FBM.AltitudeFilter);if(Altitude_mode && RC_Control.THROTTLE>200)THROTTLE = 480 + PID_ALT.OutPut;
}

我并没有获取到FBM320的数据，但是它却在I2C的线路上，可能是有虚焊，讲一下焊接FBM320的技巧吧，先在焊盘上上锡，然后涂上助焊剂，用镊子夹住气压计，并固定（手抖症后期患者误解），用热吹风枪吹即可，或者在焊盘上涂上焊锡膏，一定要用镊子固定住，用热吹风枪吹。因为我用的是BMP280，但还没移植成功，移植成功了再放到博客上。

互相学习批评指正！

小马哥robofly四轴气压计定高的不完全解读相关推荐

零基础DIY四轴飞行器超级详细保姆级教程（STM32F407ZGT6主控、WIFI图传、陀螺仪平衡、气压计/超声波定高、手机蓝牙控制等功能）
前言大四毕业后暑假没事做就花了一点DIY了一个四轴飞行器,是比较大的那种F450机架,不是那种PCB板做机架的小四轴,因为我也是从零基础开始做的四轴,现在就想把自己的过程写成博客分享在网上.下面我会 ...
四旋翼定高篇之惯导加速度+速度+位置三阶互补融合方案
笔者最近正在做四旋翼惯性导航的部分,利用加速度计进行速度估计.位置估计,从而实现四旋翼的垂直方向上的定高.水平方向上的定点控制. 首先在这里引用学长之前参考APM飞控里面互补滤波惯导融合方案:原文见四 ...
小马哥RoboFly开源STM32F103四轴简介
RoboFly是小马哥团队在2018年8月推出的一款完全开源的小四轴. 这款四轴面向的人群是电子相关专业(包括自动化.电气自动化.电子信息工程.计算机.测控等专业)的大学生,通过一个完整的四轴项目来学 ...
超声波定高--过滤突然出现的障碍物
超声波测到的数据会有一点毛刺,先用中值滤波,可以很好去掉突变太大的数据.为什么不用均值滤波呢,均值滤波对数据的实时性影响比较大. 对中值滤波后的数据做一定的统计,计算出数据的变化量和离散程度.我能想到 ...
第1讲、Cadence Allergo绘制小马哥DragonFly四轴飞行器PCB四层板教程简介
初学者在学习单片机的时候大多都没有很明确的学习目的,所以往往学了很多课程之后,还是不能独立开发一个比较简单的项目,甚至不能完成一个简单项目的单独软件或者硬件部分的开发.这是因为大多数人是被动的.没有目 ...
一直没看的无人机定高
除了定高的部分,其他没看的还有一些控制函数. 定高的原理的话,也是利用了两级pid, 按照原本的理解,从外环开始看的话,反馈高度比较容易获得,就是激光测距得到的高度.期望高度好像无法得到.内环的话,期 ...
【四轴飞行器】【定高部分】BMP280——硬件IIC
原理实现采用BMP280模块,实时获取飞行器所在位置的气压值,判断和设定的停留高度的差别,实现高度纠正. BMP280代码采用的硬件IIC,所以移植极为简单,直接把文件添加进工程,并且使能IIC即 ...
如何搞定高并发系统设计？
作者 | 菜菜责编 | 郭芮 YY妹:菜菜哥,上次你给我讲的分库分表策略对我帮助很大. 菜菜:有帮助就好,上次请我的咖啡也很好喝. YY妹:不过随着访问量的不断加大,网站我又加了Nginx做负载均衡 ...
PPT做的又慢又难看，收下这4个模板资源站，10分钟搞定高逼格PPT
办公急需一些材料的时候,行业规则指引着我们去问百度呀,班之后工作上遇到一些问题,换着一颗请教的心去问别的老员工,换来的只有一句:你不会上百度查吗? 工作中我们常常需要一些实用资源,工作汇报还有各种报告 ...

小马哥robofly四轴气压计定高的不完全解读

小马哥robofly四轴气压计定高的不完全解读相关推荐

最新文章

热门文章