如何利用互相关算法求时延差的C语言算法呢

广义互相关算法用FFT加速的C语言实现

在声音测距的原理中，我们可以利用麦克风对采集的声音信号利用互相关算法测出音源与麦克风距离的差值，而大致求出音源的方位。这也是第十五届智能车竞赛声音信标组定位的基础，因为我参加的是这个组别，所以当时在这方面找了很多相关的资料互相关的原理基本上懂，但是用C语言实现起来感觉和书本上的知识结合不了，但是真的运气好，试了很多很多次，终于写出来了，可能是乐于分享的人运气都不会太差吧，O(∩_∩)O哈哈~

小说明：我的这个代码不是完整的工程，只是把核心的互相关算法及一些相关的代码分享出来了，是基于单片机的，具体的实践还要靠大家自己来结合，这个对于大家应该就是小意思了吧，哈哈。在其中我是用了FFT加速，因为采集的数据比较多，不用FFT加速的话会花费大量的CPU的处理时间，至于FFT算法，网上有很多，我这里就不写了。

//采样点数和FFT运算的点数最好都为2的N次方，且FFT_Pointer =2*Sampling_Num
#define Sampling_Num 2048
#define FFT_Pointer 4096

int Mic12_Delay;
int Mic34_Delay;
uint Max_Array_Num=0;            //互相关运算得到的最大值对应的数组下标
//以下这些参与FFT运算的数组占用的内存相当大(6*4*2*4096=192K),为了减小内存的话可以相互覆盖掉，我这里是为了让大家明白各个数组的作用
cfloat32 Mic1_fftIn[FFT_Pointer];//麦克风Mic1参与FFT的时域输入数组
cfloat32 Mic2_fftIn[FFT_Pointer];//麦克风Mic2参与FFT的时域输入数组
cfloat32 Mic1_fftOut[FFT_Pointer];//Mic1经过FFT之后得到的频域输出数组
cfloat32 Mic2_fftOut[FFT_Pointer];//Mic2经过FFT之后得到的频域输出数组
cfloat32 Mic1_ifftMul[FFT_Pointer];//FFT运算之后两个信号频域的乘积
cfloat32 Mic_ifftOut[FFT_Pointer];//最终经IFFT运算(FFT的逆变换)之后得到的时域信号
//定义浮点型复数的实部和虚部部分，因为FFT过程中既有实数也有复数
typedef struct
{
    float real;               /**< brief Real part */
    float imag;               /**< brief Imaginary part */
} cfloat32;
//两个复数相乘
cfloat32 mul(cfloat32 a, cfloat32 b)
{
        cfloat32 c;
    c.real = a.real*b.real - a.imag*b.imag;
    c.imag = a.real*b.imag + a.imag*b.real;
    return c;
}

//复数求出幅值
float Absolute_Value(cfloat32 a)
{
           float i,j;
           float value;
           i=a.real*a.real;
           j=a.imag*a.imag;
           value=i+j;
           return value;
}
/*********************************************************
********************* 核心函数 **************************
* 函数功能：1.Mic1和Mic2采集麦克风数据
*          2.利用FFT通过互相关算法求出时延差
********************************************************/
void Mic12_ADC_Data_Handle(void)
{
        uint i;
        uint array[3];
        float MIC_ifftOut[FFT_Pointer];   //互相关运算的结果的模，因为结果可能为复数，参与运算要化为实数
        float Mic_max=0.0;
        for(i=0;i
        {
                Mic1_fftIn.real               =ADC_Read(ADC1);  //读取麦克风1的AD值存到数组中,数组顺序由小（0）到大（Sampling_Num-1）
                Mic2_fftIn[Sampling_Num-i-1].real=ADC_Read(ADC2);  //读取麦克风2的AD值存到数组中,数组顺序由大（Sampling_Num-1）到小（0）
                Mic1_fftIn[i+Sampling_Num].real  =0;    //补零，参与FFT的数组长度为2倍采样AD值的数组长度,即采样2048个点,但是参与FFT的有4096个点
                Mic1_fftIn.imag               =0;    //虚部全部为0
                Mic1_fftIn[i+Sampling_Num].imag  =0;    //虚部全部为0
                Mic2_fftIn[i+Sampling_Num].real  =0;    //补零，参与FFT的数组长度为2倍采样AD值的数组长度
                Mic2_fftIn[Sampling_Num-i-1].imag=0;    //虚部全部为0
                Mic2_fftIn[i+Sampling_Num].imag  =0;    //虚部全部为0
               
        //将采集到的声音信号通过波形图实时显示
/*                array[0]=(uint)Mic1_fftIn.real;
                array[1]=(uint)Mic2_fftIn[Sampling_Num-i-1].real+4096;
                array[2]=4096;
                virtual_Osc_send_array(array,3);*/
        }
        FFT(Mic1_fftOut, Mic1_fftIn, FFT_Pointer);     //Mic1的FFT正变换
        FFT(Mic2_fftOut, Mic2_fftIn, FFT_Pointer);     //Mic2的FFT正变换
        for(i=0;i         {
                Mic1_ifftMul=mul(Mic1_fftOut,Mic2_fftOut);      //两个信号频域的乘积
        }
        IFFT(Mic_ifftOut, Mic1_ifftMul, FFT_Pointer);  //FFT的逆变换
        for(i=0;i         {
                MIC_ifftOut=Absolute_Value(Mic_ifftOut);           //取模值,因为经过IFFT之后得到的可能为复数，不好比较大小，所以通过取模值化为实数
        }
        for(i=0;i         {
                if(MIC_ifftOut>Mic_max)
                {
                        Mic_max=MIC_ifftOut;            //找出幅值最大的点,此即为相关性最好的点
                        Max_Array_Num=i;                   //相关性最好的点对应的数组下标
                }
        }
        //Max_Array_Num为基准线,通过比较Sampling_Num-1在Max_Array_Num的上下方得出音源距离Mic1与Mic2距离的差值
        Mic12_Delay=Max_Array_Num-Sampling_Num+1;  //得到时延的下标差

        //山外虚拟示波器画图,发送函数在我的另一篇博客串口发送中有介绍
/*        array[0]=Max_Array_Num;
        array[1]=Sampling_Num-1;
        virtual_Osc_send_array(array,2);*/
}

好了，以上就是利用互相关算法求时延差的C语言算法，代码的备注我已经尽可能的加在相应代码之后了，如果大家还有不懂的欢迎下方留言或者私信给我，尽量我懂的地方让大家也能懂！或者有什么不对的地方欢迎大家指出，我们共同进步！

广义互相关算法用FFT加速的C语言实现

在声音测距的原理中，我们可以利用麦克风对采集的声音信号利用互相关算法测出音源与麦克风距离的差值，而大致求出音源的方位。这也是第十五届智能车竞赛声音信标组定位的基础，因为我参加的是这个组别，所以当时在这方面找了很多相关的资料互相关的原理基本上懂，但是用C语言实现起来感觉和书本上的知识结合不了，但是真的运气好，试了很多很多次，终于写出来了，可能是乐于分享的人运气都不会太差吧，O(∩_∩)O哈哈~

小说明：我的这个代码不是完整的工程，只是把核心的互相关算法及一些相关的代码分享出来了，是基于单片机的，具体的实践还要靠大家自己来结合，这个对于大家应该就是小意思了吧，哈哈。在其中我是用了FFT加速，因为采集的数据比较多，不用FFT加速的话会花费大量的CPU的处理时间，至于FFT算法，网上有很多，我这里就不写了。

//采样点数和FFT运算的点数最好都为2的N次方，且FFT_Pointer =2*Sampling_Num
#define Sampling_Num 2048
#define FFT_Pointer 4096

int Mic12_Delay;
int Mic34_Delay;
uint Max_Array_Num=0;            //互相关运算得到的最大值对应的数组下标
//以下这些参与FFT运算的数组占用的内存相当大(6*4*2*4096=192K),为了减小内存的话可以相互覆盖掉，我这里是为了让大家明白各个数组的作用
cfloat32 Mic1_fftIn[FFT_Pointer];//麦克风Mic1参与FFT的时域输入数组
cfloat32 Mic2_fftIn[FFT_Pointer];//麦克风Mic2参与FFT的时域输入数组
cfloat32 Mic1_fftOut[FFT_Pointer];//Mic1经过FFT之后得到的频域输出数组
cfloat32 Mic2_fftOut[FFT_Pointer];//Mic2经过FFT之后得到的频域输出数组
cfloat32 Mic1_ifftMul[FFT_Pointer];//FFT运算之后两个信号频域的乘积
cfloat32 Mic_ifftOut[FFT_Pointer];//最终经IFFT运算(FFT的逆变换)之后得到的时域信号
//定义浮点型复数的实部和虚部部分，因为FFT过程中既有实数也有复数
typedef struct
{
    float real;               /**< brief Real part */
    float imag;               /**< brief Imaginary part */
} cfloat32;
//两个复数相乘
cfloat32 mul(cfloat32 a, cfloat32 b)
{
        cfloat32 c;
    c.real = a.real*b.real - a.imag*b.imag;
    c.imag = a.real*b.imag + a.imag*b.real;
    return c;
}

//复数求出幅值
float Absolute_Value(cfloat32 a)
{
           float i,j;
           float value;
           i=a.real*a.real;
           j=a.imag*a.imag;
           value=i+j;
           return value;
}
/*********************************************************
********************* 核心函数 **************************
* 函数功能：1.Mic1和Mic2采集麦克风数据
*          2.利用FFT通过互相关算法求出时延差
********************************************************/
void Mic12_ADC_Data_Handle(void)
{
        uint i;
        uint array[3];
        float MIC_ifftOut[FFT_Pointer];   //互相关运算的结果的模，因为结果可能为复数，参与运算要化为实数
        float Mic_max=0.0;
        for(i=0;i
        {
                Mic1_fftIn.real               =ADC_Read(ADC1);  //读取麦克风1的AD值存到数组中,数组顺序由小（0）到大（Sampling_Num-1）
                Mic2_fftIn[Sampling_Num-i-1].real=ADC_Read(ADC2);  //读取麦克风2的AD值存到数组中,数组顺序由大（Sampling_Num-1）到小（0）
                Mic1_fftIn[i+Sampling_Num].real  =0;    //补零，参与FFT的数组长度为2倍采样AD值的数组长度,即采样2048个点,但是参与FFT的有4096个点
                Mic1_fftIn.imag               =0;    //虚部全部为0
                Mic1_fftIn[i+Sampling_Num].imag  =0;    //虚部全部为0
                Mic2_fftIn[i+Sampling_Num].real  =0;    //补零，参与FFT的数组长度为2倍采样AD值的数组长度
                Mic2_fftIn[Sampling_Num-i-1].imag=0;    //虚部全部为0
                Mic2_fftIn[i+Sampling_Num].imag  =0;    //虚部全部为0
               
        //将采集到的声音信号通过波形图实时显示
/*                array[0]=(uint)Mic1_fftIn.real;
                array[1]=(uint)Mic2_fftIn[Sampling_Num-i-1].real+4096;
                array[2]=4096;
                virtual_Osc_send_array(array,3);*/
        }
        FFT(Mic1_fftOut, Mic1_fftIn, FFT_Pointer);     //Mic1的FFT正变换
        FFT(Mic2_fftOut, Mic2_fftIn, FFT_Pointer);     //Mic2的FFT正变换
        for(i=0;i         {
                Mic1_ifftMul=mul(Mic1_fftOut,Mic2_fftOut);      //两个信号频域的乘积
        }
        IFFT(Mic_ifftOut, Mic1_ifftMul, FFT_Pointer);  //FFT的逆变换
        for(i=0;i         {
                MIC_ifftOut=Absolute_Value(Mic_ifftOut);           //取模值,因为经过IFFT之后得到的可能为复数，不好比较大小，所以通过取模值化为实数
        }
        for(i=0;i         {
                if(MIC_ifftOut>Mic_max)
                {
                        Mic_max=MIC_ifftOut;            //找出幅值最大的点,此即为相关性最好的点
                        Max_Array_Num=i;                   //相关性最好的点对应的数组下标
                }
        }
        //Max_Array_Num为基准线,通过比较Sampling_Num-1在Max_Array_Num的上下方得出音源距离Mic1与Mic2距离的差值
        Mic12_Delay=Max_Array_Num-Sampling_Num+1;  //得到时延的下标差

        //山外虚拟示波器画图,发送函数在我的另一篇博客串口发送中有介绍
/*        array[0]=Max_Array_Num;
        array[1]=Sampling_Num-1;
        virtual_Osc_send_array(array,2);*/
}

好了，以上就是利用互相关算法求时延差的C语言算法，代码的备注我已经尽可能的加在相应代码之后了，如果大家还有不懂的欢迎下方留言或者私信给我，尽量我懂的地方让大家也能懂！或者有什么不对的地方欢迎大家指出，我们共同进步！

举报