机器人避障之滑动窗口处理数据

目的

构建一个滑动窗，窗口大小固定，进出窗口顺序按照FIFO模型，即先入先出，让然后判断该窗口中数值趋近律，是否满足递减要求。

场景

机器人在避障时，有时仅仅依靠传感器数据（激光雷达，线激光，超声波，深度摄像头）反馈的距离是不可靠的，这时就需要引入距离和距离的变化率两种数据。

好处

可以有效避免传感器异常点导致的误检现象，窗口可以结合多种数据源，来辅助判断。比如超声波窗口满足递减，但激光雷达窗口不满足递减，这时就可以做取舍。一般是通过模糊决策方式，融合多个数据源。

if( 距离满足避障要求 && 距离的变化率满足递减要求)
{
	// 需要避障
}

源码展示

#include 

typedef unsigned char uint8_t;
typedef char16_t int16_t;

#define MAX_SIZE 3

/* Insert num with slide window,the window size is max_size*/
void ValueSlideInsert(uint8_t addr[],uint8_t max_size,int16_t num)
{
    for(int i = 1;i < max_size;i++)
    {
        addr[i - 1] = addr[i];
    }

    addr[max_size - 1] = num;
}

bool JudgeTheApproachTrend(uint8_t addr[],uint8_t max_size)
{
    bool return_tmp = true;
    for(int i = 1;i < max_size;i++)
    {
        if(addr[i - 1] >= addr[i])
        {
            return_tmp = false;
            break;
        }
    }
    return return_tmp;
}

int main(void) { 
    
    uint8_t front_dis[MAX_SIZE];
    
    ValueSlideInsert(front_dis,MAX_SIZE,40);
    ValueSlideInsert(front_dis,MAX_SIZE,30);
    ValueSlideInsert(front_dis,MAX_SIZE,20);
    
    bool tmp = JudgeTheApproachTrend(front_dis,MAX_SIZE);
    
    printf("the tmp : {%d}",tmp);
	
	return 0;
}