超聲波測到的數據會有一點毛刺,先用中值濾波,可以很好去掉突變太大的數據.為什么不用均值濾波呢,均值濾波對數據的實時性影響比較大.

對中值濾波后的數據做一定的統計,計算出數據的變化量和離散程度.我能想到的就是前后數據的差值,若干個數據的方差. 差值能直接反應數據的突變,方差能夠強烈的反應數據的波動.

對數據進行判斷,差值太大的不能要,方差太大的也不能要.這都說明超聲波受到了明顯的干擾:前方突然出現遮擋.干擾過后再等一下下,防止有連續的干擾出現.

開始重新記錄數據,最后高度的變化就是前后數據的差值的積分.

優點：能實現厘米級定高

缺點:在剛好出現越障礙物時，如果出現在高度變化就測量不到了。這時就要再結合，加速度計和氣壓計做擬合了

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;//新數據插入到最后一個} }

?

總結

以上是生活随笔為你收集整理的超声波定高--过滤突然出现的障碍物的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。