之前的項目中,使用了超聲波測距傳感器,大多數人都知道超聲波測距在程序中主要是通過測量出超聲波往返時間,而已知超聲波的速度大約340m/s。就可通過公式s =( v*t)/2。計算出離物體之間的距離。
開始時,對距離處理算法只是進行簡單的均值濾波,即將採集5次左右的值取平均值作爲最終值。但在後來的實際應用中發現的超聲波傳感器並不是想象中的那麼穩定,也會出現個別錯誤檢測的情況。爲了弄清超聲波測距是如何產生錯誤的,或者產生的錯誤是何種類型,只有弄清產生的錯誤類型,才能設計一個合適於超聲波測距的處理算法。
通過連續不不斷的對超時波進行距離檢測,並通過串口傳輸給上位機觀察。半個小時內可以發現差不多每5分鐘就會出現一兩次的錯誤數據,每次的錯誤數據一般會連續產生1到3個。爲排除可能單個傳感器不穩定造成的t特殊現現象,因此測試多個超聲波傳感器,均發現這樣的問題,從而可知超聲波測距會有不定期的產生錯誤數據的共性。
現總結測試過程中產生錯誤的幾種類型(實際超聲波和物體距離爲2.3m左右)。
第一種:連續出現過小的數據(一般不會連續超過3個)
2.31 2.32 2.26 1.25 1.22 1.31 2.31 2.28
其中1.20 1.21 1.21三組爲出錯數據
第二種:連續出現過大的數據(一般不會連續超過3個)
2.32 3.40 3.45 3.48 2.31 2.24 2.33 2.32
其中3.40 3.45 3.48三組爲出錯數據
觀察發現,出現錯誤數據有如下幾個特點:
1. 出現錯誤的情況一般不會連續出現,錯誤數據之間一般有一定的間隔時間。
2. 一般出錯都是連續性出錯,並且出錯個數不會超過3個。
3. 產生出錯的數據離實際值都偏離較大
4. 連續出錯的幾個數據都是比較相近的數據。
因此 根據以上的錯誤類型以及特點,需要一種可靠的處理算法,將兩種錯誤的幾個噪點除去並取出正確的數據取均值。
算法處理流程圖如下:
/*********************************************************************
* 函數名 : void UltrasonicWave_StartMeasure(void)
* 描述 : 超聲波距離處理算法函數
* 輸入 : 無
* 返回 : 無
* 說明 :
void UltrasonicWave_StartMeasure(void)
{
u16 Distance[10] = {0};
u8 IS_success =1;
u16 Distance1 = 0;
u16 MAX_error1 = 0;
u8 MAX_error_targe = 0;
u8 count = 0;
u8 i =0;
u8 j =0;
u8 num =0; //實際只測到的次數
u8 measure_num = 8; //需要測量的次數
//===注意,浮點計算稍微費時間,因此處理都已化爲整型數處理(單位MM)===//
//採集8個點
Start_ECHO = 1;
UltrasonicWave_Distance1 = 0;
for(i = 0 ; i < measure_num ; i++)
{
count =0;
Start_ECHO =1;
IS_success=1;//成功測到
GPIO_SetBits(TRIG_PORT,TRIG_PIN1); //送>10US的高電平給超聲波TRIG腳,觸發測距
delay_ms(1); //延時20US
GPIO_ResetBits(TRIG_PORT,TRIG_PIN1);
delay_ms(5); //每個樣點間隔時間5Ms
//注:距離在中斷裏測出,中斷裏測出後 Start_ECHO=0
while(Start_ECHO==1)
{
count++;
delay_ms(3);//30MS內沒測到自動跳出Start_ECHO
if(count>10)
{
count=0;
IS_success =0; //超時沒測到爲0
Start_ECHO =0;
break;
}
IS_success=1;//成功測到爲1
}
if(IS_success)
{
num ++;//記錄下成功採集點的個數
Distance[i] = UltrasonicWave_Distance1;//記錄下採集點
}
}
UltrasonicWave_Distance1 = 0;//清
if(num>2) //要大於兩個值才能用算法
{
//排序
for(i = 0 ; i < num-1 ; i++)
{
for(j = 0 ; j < num-1-i; j++)
{
if(Distance[j] > Distance[j+1] )
{
Distance1 = Distance[j];
Distance[j] = Distance[j+1];
Distance[j+1] = Distance1;
}
}
}
//找出最大差距
MAX_error1 = Distance[1] - Distance[0];
for(i = 1 ; i < num-1 ; i++)
{
if(MAX_error1 < Distance[i+1] - Distance[i] )//如:1 2 3 4 5 8 9 10 MAX_error_targe=4;
{
MAX_error1 = Distance[i+1] - Distance[i];//最大差距
MAX_error_targe = i; //記下最大差距值的位置(這組數中的位置)
}
}
//取出最終值
if(MAX_error_targe+1 > (num+1)/2) //前部分有效 1 2 3 4 5 8 9 10 (如果位於中間,後半部優先)
{
for(i = 0 ; i <= MAX_error_targe ; i++)
{
UltrasonicWave_Distance1 += Distance[i];
}
UltrasonicWave_Distance1 /= (MAX_error_targe+1);//取平均
}
else //後部分有效 1 2 3 7 8 9 10
{
for(i = MAX_error_targe + 1 ; i < num ; i++)
{
UltrasonicWave_Distance1 += Distance[i];
}
UltrasonicWave_Distance1 /= (num - MAX_error_targe -1);//取平均
}
}
else //採集到的個數小於2個
{
UltrasonicWave_Distance1 = 3300; //取無窮遠
}
f_UltrasonicWave_Distance1 = UltrasonicWave_Distance1/100.0; //轉化爲米爲單位的浮點數
MinDistance = f_UltrasonicWave_Distance1;//最終值
}
/*********************************************************************
* 函數名 : EXTI1_IRQHandler
* 描述 : 中斷線1中斷服務程序
* 輸入 : 無
* 返回 : 無
* 說明 : 超聲波的Encho中斷 中斷測距
*********************************************************************/
void EXTI1_IRQHandler(void)
{
delay_us(10); //延時10us
if(EXTI_GetITStatus(EXTI_Line1) != RESET) //進入中斷
{
if(GPIO_ReadInputDataBit(ECHO_PORT,ECHO_PIN1) == SET && Start_ECHO==1)//上升沿且開始測量
{
Timeout_Flag = 0;
TIM_SetCounter(TIM4,0);
TIM_Cmd(TIM4, ENABLE); //開啓時鐘此時開始計時
//等待ECHO端口低電平,且沒有超時(TIM4定時時間自己定,時間到則Timeout_Flag=1)
//加入定時TIM4目的是:節約測距時間。
while(GPIO_ReadInputDataBit(ECHO_PORT,ECHO_PIN1)&&!Timeout_Flag);
TIM_Cmd(TIM4, DISABLE);//定時器2失能 //定時器2失能
if(!Timeout_Flag)//沒有超時情況
{
UltrasonicWave_Distance1=(u16)(TIM_GetCounter(TIM4)*0.01*340/1.93 ); //單位 Cm(整型方便後面的處理速度)
}
else//超出12ms 即超出2.11m
{
UltrasonicWave_Distance1 = 211;//211CM
}
Timeout_Flag = 0;
Start_ECHO =0; //轉化完成標誌
delay_ms(5);
}
}
EXTI_ClearITPendingBit(EXTI_Line1); //清除EXTI7線路掛起位 出中斷
}
程序算法主要步驟是:
1. 採集8個點(經過測試連續錯誤不會超過3個,故採集8個點)
2. 對8個點小到大排序。
3. 相鄰值兩兩做差,並比較。
4. 記錄下最差後最大值的那對數所在的位置
5. 如果該位置位於所有數的前半部分,則取該位置後半部分數,反之,取前半部分數。並作爲有效值。
6. 取有效值的平均值爲最後值
7. 如果採集8個點個數不多餘2個,那無法使用該算法,認爲距離爲無窮遠(取3300)。
總結:
算法思想是,利用少數遵循多數原則,主要爲了將採集的多個數據分成兩波,將相鄰兩個數之差的最大那個作爲兩撥數的分界點。然後取兩波數中個數最多的那波數爲正確的有效值,另一波少數的爲無效值。這樣如果出現的幾個錯誤值,而根據以上分析的錯誤數據特點,連續出現的幾個錯誤數據大小都是比較靠近,而且不會超過3個點,因此在多個點(8個)中能很容易的將這些少數點分離出來。
但有個缺點是,如果沒有出現錯誤的點,那程序也會把數據分兩撥除去另一數,使得本來一些測的有效值被犧牲。解決方法:可以通過人爲加入一閾值,如果相鄰兩個數之差,最大那個不會超過閾值,就不把數據分成兩撥,超出閾值就分成兩撥。