基于kalman滤波的超声波测距系统设计

基于kalman滤波的超声波测距系统设计

当独立移动机器人执行目标跟踪和局部避障行为时，应了解目标、障碍物和其他状态信息（如障碍物的位置、速度、速度等）。超声位移作为一种非接触检测的类型，不易受光、烟雾弥漫、电磁等因素的影响，因此在移动机器人的障碍物检测中得到了广泛应用。然而，由于超声在空气中传播的衰减很大，很难从远处测量，而且很难保证精度。提高超声发射的功率和效率，提高超声接收的灵敏度和稳定性，可以有效增加检测剂量，但导致盲点扩大。随着剩余振动强度的增加，接收灵敏度的提高，残余波对回波信号的影响，藏怀刚等人提出了一种自动调整发射能量的测量方法。在缓慢移动目标的测量中，通过前一次测量反射波的强度来确定测量参数，但该方法仅根据初始数据中反射波的强度来确定本次测量的参数。对于高速运动目标的测量效果不理想，且超声在空气中的传播速度慢以及测量周期长问题没有得到解决。作者针对超声波测距在移动式机器人障碍物探测时,存在的大量程与小盲区之间的矛盾及测量周期长的问题进行研究,设计了易于控制的超声波测距系统,采用基于Kalman滤波的量程自适应跟踪测量方法.利用Kalman滤波器对目标状态进行估计,根据预测值选择合适的量程,对系统的测量参数进行调整.1超声波接收电路本研究选用收发一体静电式超声波换能器,设计了基于脉冲回波法的超声波测距系统,系统原理如图1所示.工作时,首先由单片机输出一定频率的脉冲序列,经发射电路放大整形后形成较大功率的脉冲信号,通过阻抗匹配网络施加在超声波换能器的两端,激励换能器向外辐射超声波;超声波遇到被测目标后返回,被换能器接收并转化为电信号;接收电路对此微弱的电信号进行放大、滤波、整形等处理,使其转化为单片机可识别的电平信号,触发单片机中断;最后单片机对检测信息进行处理得到测量结果.1.1等效电阻的确定发射模块由升压单元、阻抗匹配网络和余波抑制电路构成,由单片机MSP430进行控制,发射电路如图2所示.脉冲变压器在本系统中有两个用途:一是将5V电压升至400V;二是使其输出阻抗与换能器的阻抗相匹配.本系统采用收发一体模式,ECHO为回波信号的输入端,接收回路也是谐振回路,既增加了接收灵敏度,又排除了其它频率波的干扰.静电式超声波换能器为容性元件,在谐振频率附近其等效阻抗Z0为式中:C0为等效电容;R0为等效电阻;ω为谐振频率,ω=2πf.在本电路中变压器次级线圈的电感值较大,忽略初级线圈的影响,则次级线圈的等效阻抗ZT为式中:LT为等效电感;RT为等效电阻.在电路中串联一个电容Cc,此时电路的阻抗Z为式中:X=;Zc为电容Cc的等效阻抗.选择合适的电容Cc,使得等效电容的容抗与等效电感的感抗相互抵消,此时换能器作为超声波发射器的效率最高.本系统参数:C0=450pF,LT=22mH,最大量程测量模式下的工作频率f=55.6kHz,计算得到Cc=2116pF,在实际电路中选取Cc=2200pF.为减小盲区,模块中设计了余波抑制电路,通过控制谐振回路的损耗加速余波结束.余波抑制电路由发射电路中的耗能电阻R2和开关管Q1构成,由单片机的IO端口SUPC进行控制.在脉冲序列发送完毕后,单片机控制开关管导通,强制短路变压器初级,快速消耗谐振能量,达到消除余波的目的.1.2tl652动态增益控制回路本模块以超声波信号处理专用芯片TL852为核心,设计了具有输入保护、前置放大、多级可变增益、滤波放大、输出滤波、阈值比较等功能的回波接收电路,如图3所示.其中,回波信号ECHO从TL852的XIN端口输入,经过处理后变为稳定的单片机可识别的电平信号,从INT端口输出,触发中断.TL852的增益控制端口GCA,GCB,GCC,GCD分别与单片机的4个IO连接,由单片机进行定时增益控制.根据超声波传播按指数规律衰减的特性,为系统设定12级增益值,以保证回波信号大致在同一位置触发中断,准确确定回波到达时间.为抑制接收信号中的脉冲扰动和杂波,在TL852的输出端REC和地之间增加了输出滤波电容C4.然而,余振信号使输出滤波电容充电,在其放电结束之前,无法启动回波接收.为了能够进行及时的测量,在滤波电容C4的两端跨接一个开关管Q3,在余波信号衰减到设定的阈值前,单片机控制开关管导通,短路滤波电容,此时较强的余振信号不对滤波电容充电,待余振信号衰减到阈值以下,便可断开滤波器放电回路,同时启动回波接收.2测距流程设计基于Kalman滤波的量程自适应超声波测距流程如图4所示.下面依次介绍量程级数设定,量程自适应切换策略,系统测量参数自适应和宽测量范围下系统误差的修正.2.1测量周期设定基于Kalman滤波的量程自适应测量方法的应用,首先应根据实际需要,为系统设定多级量程.本系统针对移动机器人障碍物探测的应用,设定5级量程:0.02~0.20m,0.20~1.00m,1.00~5.00m,5.00~20.00m,20.00~30.00m,相应的测量周期分别为2,10,50,125和200ms.其中量程0.02~0.20m的设定是为了减小盲区;量程0.20~1.00m对应着10ms的短测量周期,有助于机器人的紧急避碰;量程1.00~5.00m适于机器人在障碍物较密集的环境中进行快速、高精度的障碍物探测(本系统在此测量范围内具有很高的精度).将5.00~30.00m划分为远近2个量程,其中量程20.00~30.00m适于机器人在空旷的环境中进行远距离目标探测,量程5.00~20.00m是为了弥补量程20.00~30.00m测量周期过长的不足.多级量程的设定及自动切换不仅解决了大量程与小盲区之间的矛盾,同时还缩短了平均测量周期.2.2基于模型的一般方法量程的自适应切换需要提前对测量结果做出预测,根据预测值调整系统测量参数.在众多的预测滤波方法中,Kalman滤波是一种线性、无偏、最小方差的统计估值方法,在系统特性已知、系统噪声以及观测噪声的统计特性都先验已知的情况下,Kalman滤波算法能够实现最优估计.Kalman滤波是基于模型的估值算法,建立目标的运动模型是进行状态估计的基础.移动机器人在运行时,一般处于匀速或匀加速运动状态,以机器人坐标系作为参考坐标系,前方目标(障碍物)的运动模型可以用状态方程和测量方程描述:式中:Xk=d[kvkak]T;Zk=z[k];采用Kalman滤波器对状态X进行预测,首先应设定状态初始值X0,然后通过时间更新进行状态预测,最后利用测量更新对结果进行修正.2.2.1单次误差滤波算法的初始值X0=[d0v0a0]T,对其收敛速度具有较大影响.若将初始值设置为X0=[000]T,得到的跟踪过程在初始阶段会出现较大的波动.在移动机器人行进过程中,障碍物是随机出现的,将初始值设置为任何固定值都是不合理的,因此本系统中采用实际距离观测值作为初始值d0,取前2次距离测量值的一阶差分作为速度初始值v0,取前3次距离测量值的二阶差分作为加速度初始值a0.将测量值作为初始值,后续的跟踪具有较好的收敛效果和较高的精度.2.2.2时间更新最优预测估计方程为最优预测估计误差方差阵方程为式中Q为过程噪声协方差阵.在时间更新环节中,根据当前目标的状态X^k|k,预估下一时刻目标的状态X^k+1|k,将预测值X^k+1|k与预先设定的阈值进行比较,从而选择对应的量程,调整系统参数进行测量.2.2.3基于控制变量的业分迭代算法最优滤波估计方程为最优滤波增益方程为最优滤波估计误差方差阵方程为依照选定的量程测量完成后,进入测量更新环节.根据实际测量值Zk+1,对上次的一步预估值X^k+1|k进行修正,得到本次最优的估计值X^k+1|k+1.可以看到,虽然在计算最优滤波增益矩阵Kk+1时要进行矩阵求逆的运算,但在本系统中由于为一维矩阵,计算量小,并未降低系统的实时性.量程切换的原则如图5所示.系统设有多级量程,各级量程都存在一定的盲区.当突然出现的障碍物处于当前量程的盲区时,系统并非不能检测到目标,而是余振信号对回波信号产生干扰,得到的测量值精度略低,据此依然可以将量程切换到合适的范围,进行高精度的测距.当预测距离大于最高阈值时,意味着待测目标的距离将超出本测距系统的最大测量范围,系统将切换到最大量程测量模式,进行目标搜索.当预测值处于相邻量程的边界附近时,为避免在量程间进行频繁切换,系统设定了具有滞回特性的阈值.2.3系统的测量参数调整各级量程下都设定了相应的测量参数,在实际测距时进行动态调整.2.3.1发射频率的选择调整发射功率、发射频率和发射时间能够有效调节超声波测距的范围.其中发射功率通过PWM信号的占空比进行控制.当使用谐振频率驱动时,发射电路才有最大的发射效率,于是可以通过调整激励信号的频率来控制发射能量.本系统选用的静电式换能器工作频带很宽,选择发射频率主要考虑超声波的衰减特性,在大量程测量时减小发射频率,以减小衰减率;在小量程测量时提高发射频率,以提高测量精度.换能器有一个起振的过程,因此适当减少激励脉冲的个数,能够有效减小振幅.同时,缩短发射时间也使系统能够尽早地接收回波信号.因此,实时调整发射时间能有效地控制盲区与量程的大小.2.3.2测量周期和所需赔偿本系统的动态测量范围宽,大量程与小量程测距所需的测量时间相差很大.设定与量程相适应的测量周期,在保证该量程探测所需渡越时间的前提下,缩短其测量周期.在移动式机器人避障的应用中,当机器人处于较为空旷的环境时,障碍物距离较远,此时测量周期较长;而当机器人处于障碍物较密集的环境中,近距离的障碍物较多,需要机器人做出快速反应,因此要求短的测量周期.2.3.3拓展波长测量方式不同量程测量模式下对应的发射能量不同,因此余波及回波的强度也不同,需要对余波抑制电路、回波可变增益电路和输出滤波器进行实时控制,以保证系统可靠地接收回波,并保证较高的测量精度.2.4延迟时间测量单片机系统延迟与硬件电路延迟造成了测距系统有一定的时间延迟.部分电路的延迟难以通过理论计算准确获得,因此采用实验测量的方法.用本测距系统对已知的实际标准距离dr进行测量,利用测量值ds,根据下式即可计算得到系统延迟时间td.式中:tr为实际渡越时间;c为声速.在确定了系统时延后,便可进行误差修正,修正后的测量距离dc为式中t为测量时间.本系统中设有多级量程,各级量程下的测量参数不同,对应的时间延迟也略有不同.为进一步提高精度,测定各级量程相应的系统时延,建立误差修正表,在实际测距时采用查表法修正测量结果.此外本系统采用数字温度计DS18B20采集温度信号,对声速进行修正,减小因温度变化引起的误差.3测量实验结果本文中设计的超声波测距系统是自主移动式机器人平台的一部分,测距系统实物如图6所示.对本系统进行测距实验:将测距模块安装在移动机器人上,包括机械固定与电气连接.超声波换能器安装在机器人的正前方,距地面高度为0.6m,换能器平面与地面垂直.被测目标为表面平整、面积为1m×1m的矩形木板,垂直于地面放置,且该木板的中心垂线与超声波换能器的中心垂线相重合.测距实验在空旷的平地上进行,测距系统将测量数据通过串口发送给移动机器人上的主控计算机,并保存实验数据,以便后续处理.进行常规测距和基于Kalman滤波的量程自适应测距的对比实验,包括1.00~30.00m范围内运动目标的跟踪测量与0.02~30.00m范围内静止目标的定点测量两部分.其中跟踪测距实验是通过控制机器人向前做匀速直线运动,对前方静止目标进行测距来实现的.机器人与目标间的初始距离为30m,机器人以0.5m/s的速度向目标靠近.常规测距方法的测量范围为0.15~20.00m,测量周期固定为125ms.为量程自适应系统设定5级量程.两种测距方法进行目标的跟踪测量结果如图7所示.两种测距方法进行静止目标定点测距,3组实验结果如表1和表2所示.表中,dm为测量距离,eamax为最大绝对误差,ermax为最大相对误差,Tm为测量周期.实验结果表明,相比于常规测距方法,基于Kalman滤波的量程自适应测距方法,由于动态调整测量参数,使系统的最大测量距离由20m提高到30m,同时盲区从0.15m降到0.02m.采用Kalman滤波技术对测量结果进行修正,使系统在0.02~5.00m范围内绝对误差不超过0.010m,具有很高的测量精度.随着距离的增大,绝对误差有所增加,这主要是由于远距离测量时,回波信号微弱,干扰信号侵入影响了测量结果.由于其相对误差仍然保持在较低水平(不超过0.8%),因此可以认为在大量程测量时,仍具有较高的测量精度.相比于常规方法125ms的长测量周期,量程自适应方法的测量周期根据测量范围动态调整,在保证该量程探测所需渡越时间的前提下,选择较短的测量周期,增强了系统的实时性.4基于kalman滤波的