超声波定高--过滤突然出现的障碍物

超声波测到的数据会有一点毛刺,先用中值滤波,可以很好去掉突变太大的数据.为什么不用均值滤波呢,均值滤波对数据的实时性影响比较大.
对中值滤波后的数据做一定的统计,计算出数据的变化量和离散程度.我能想到的就是前后数据的差值,若干个数据的方差. 差值能直接反应数据的突变,方差能够强烈的反应数据的波动.
对数据进行判断,差值太大的不能要,方差太大的也不能要.这都说明超声波受到了明显的干扰:前方突然出现遮挡.干扰过后再等一下下,防止有连续的干扰出现.
开始重新记录数据,最后高度的变化就是前后数据的差值的积分.
优点：能实现厘米级定高
缺点:在刚好出现越障碍物时，如果出现在高度变化就测量不到了。这时就要再结合，加速度计和气压计做拟合了

sonarAltRaw = UltralSonicRanging_GetDistance();//原始超声波数据sonarAltRaw = applySonarAltMedianFilter(sonarAltRaw);//中值滤波，过滤数据毛刺if (sonar_available){//float cosTiltAngle = getCosTiltAngle();//倾角//sonarAlt = sonarAlt * cosTiltAngle;/*int32_t boarStable = getSonarErrSum();*/if ((ABS(sonarVariance.error) < 10))//正常时前后差不大于10,没遮挡时方差不大于10{sonarAltStableCnt++;//这时候的相对高度是稳定的。if (millis() - sonarAlt_timeRecord > 100)//数据稳定后等一下{if (!sonarAltDt)//没记录过数据{sonarAltDt = 1;sonarAlt_last = sonarAltRaw;//记录第一个    sonarStartAlt = sonarAltRaw;//记录第一个稳定的高度}else{sonarAlt_dt = (sonarAltRaw - sonarAlt_last);//数据变化sonarAlt += sonarAlt_dt;//积分获得位移sonarAlt_last = sonarAltRaw;sonarAltDtStable = 1;/*sonarAlt /= 10;*/}}                   }else//检测到下方有东西突然出现{sonarAltDt = 0;sonarAlt_timeRecord = millis();//开始计时sonarAltDtStable = 0;    sonarAltStableCnt = 0;//有一次不稳定都清零}if (sonarAltStableCnt > 1000)//超声波测距长时间稳定{//高度信息是从开始高度的变化开始积分的，即：高度 = 测量高度 - 起始高度sonarAltStableCnt = 0;sonarAlt = sonarAltRaw - sonarStartAlt;//修正积分误差}}int32_t applySonarAltMedianFilter(int32_t newSonarReading)//中值滤波对脉冲噪声有良好的滤除作用
{static int32_t sonarFilterSamples[10];static int currentFilterSampleIndex = 0;static bool medianFilterReady = false;int nextSampleIndex;if (newSonarReading < SONAR_OUT_OF_RANGE) // only accept samples that are in range{nextSampleIndex = (currentFilterSampleIndex + 1);if (nextSampleIndex == 10) {nextSampleIndex = 0;medianFilterReady = true;}sonarFilterSamples[currentFilterSampleIndex] = newSonarReading;currentFilterSampleIndex = nextSampleIndex;}if (medianFilterReady)return quickMedianFilter5(sonarFilterSamples);elsereturn newSonarReading;
}int32_t quickMedianFilter5(int32_t * v)
{int32_t p[5];QMF_COPY(p, v, 5);QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[4]); QMF_SORT(p[0], p[3]);QMF_SORT(p[1], p[4]); QMF_SORT(p[1], p[2]); QMF_SORT(p[2], p[3]);QMF_SORT(p[1], p[2]);return p[2];
}Variancec_s sonarVariance = {.average = 0,.dataCnt = 0,.sum = 0,.sum2=0,.variance=0
};
void varianceCalculate(int32_t newInput, Variancec_s *_variance)//求数据方差
{if (_variance->dataCnt < VARIANCE_LEN)//数据还不足{_variance->storage[_variance->dataCnt] = newInput;_variance->dataCnt++;}else{_variance->sum = 0;for (uint8_t i=0; i < VARIANCE_LEN; i++)//数据积分{_variance->sum += _variance->storage[i];}_variance->average = _variance->sum / VARIANCE_LEN;//数据平均值_variance->sum2 = 0;for (uint8_t i = 0; i < VARIANCE_LEN; i++)//数据平均差积分{_variance->sum2 += ((_variance->storage[i]- _variance->average)* (_variance->storage[i] - _variance->average));}_variance->variance = _variance->sum2 / VARIANCE_LEN;for (uint8_t i = 0; i < (VARIANCE_LEN-1); i++){_variance->storage[i] = _variance->storage[i + 1];//扔掉第一个数据}_variance->error = newInput - _variance->storage[VARIANCE_LEN - 1];_variance->storage[VARIANCE_LEN-1] = newInput;//新数据插入到最后一个}
}

超声波定高--过滤突然出现的障碍物相关推荐

零基础DIY四轴飞行器超级详细保姆级教程（STM32F407ZGT6主控、WIFI图传、陀螺仪平衡、气压计/超声波定高、手机蓝牙控制等功能）
前言大四毕业后暑假没事做就花了一点DIY了一个四轴飞行器,是比较大的那种F450机架,不是那种PCB板做机架的小四轴,因为我也是从零基础开始做的四轴,现在就想把自己的过程写成博客分享在网上.下面我会 ...
四旋翼定高篇之惯导加速度+速度+位置三阶互补融合方案
笔者最近正在做四旋翼惯性导航的部分,利用加速度计进行速度估计.位置估计,从而实现四旋翼的垂直方向上的定高.水平方向上的定点控制. 首先在这里引用学长之前参考APM飞控里面互补滤波惯导融合方案:原文见四 ...
小马哥robofly四轴气压计定高的不完全解读
我看过正点原子的定高算法,好像是直接用气压计测量得到的高度作为反馈进行,而小马哥的robofly是结合气压计获得的高度和运动情况下获得Z轴的位移2者结合来获取高度,但是2者结合的过程看得一知半解,下面 ...
一直没看的无人机定高
除了定高的部分,其他没看的还有一些控制函数. 定高的原理的话,也是利用了两级pid, 按照原本的理解,从外环开始看的话,反馈高度比较容易获得,就是激光测距得到的高度.期望高度好像无法得到.内环的话,期 ...
如何搞定高并发系统设计？
作者 | 菜菜责编 | 郭芮 YY妹:菜菜哥,上次你给我讲的分库分表策略对我帮助很大. 菜菜:有帮助就好,上次请我的咖啡也很好喝. YY妹:不过随着访问量的不断加大,网站我又加了Nginx做负载均衡 ...
PPT做的又慢又难看，收下这4个模板资源站，10分钟搞定高逼格PPT
办公急需一些材料的时候,行业规则指引着我们去问百度呀,班之后工作上遇到一些问题,换着一颗请教的心去问别的老员工,换来的只有一句:你不会上百度查吗? 工作中我们常常需要一些实用资源,工作汇报还有各种报告 ...
linux 网络编程 51cto,Linux服务器开发，2小时搞定高并发网络编程
一.使用多线程处理高并发的弊端多线程处理高并发是常用同时处理多个并发用户请求的方式,但线程数过多会增加系统的资源消耗(线程本身占用的资源+线程切换带来的系统开销),同时因硬件和软件的限制,操作系统支 ...
关键词词云怎么做_自媒体人必备！一键搞定高逼格词云图片
相信做过新媒体的小伙伴们对词云都不陌生,它的优点除了可以展示大量文本数据,从而让读者快速抓住重点外,更重要的是它的颜值够高,放在文章中,就是绝对的焦点. 虽然制作词云的工具有很多,但真正体验好的却很少 ...
3分钟搞定高逼格PPT封底——简约型
封底想要高逼格又简约? 发现了这五类,看完不会制作算我输. 一.纯文字白色背景下,一段结束语,或提问或感谢. 重叠文字,看上去非常有创意. 没有操作难度,END放大字号,颜色设置为浅灰置底. 再输入 ...

超声波定高--过滤突然出现的障碍物

超声波定高--过滤突然出现的障碍物相关推荐

最新文章

热门文章