一种针对超声波测距的滤波处理算法

之前的项目中，使用了超声波测距传感器，大多数人都知道超声波测距在程序中主要是通过测量出超声波往返时间，而已知超声波的速度大约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个）中能很容易的将这些少数点分离出来。
但有个缺点是，如果没有出现错误的点，那程序也会把数据分两拨除去另一数，使得本来一些测的有效值被牺牲。解决方法：可以通过人为加入一阈值，如果相邻两个数之差，最大那个不会超过阈值，就不把数据分成两拨，超出阈值就分成两拨。