基于自适应滤波与卡尔曼滤波的多机器人协同定位方法研究

基于自适应滤波与卡尔曼滤波的多机器人协同定位方法研究

（北京理工大学 北京 100081）

胡波 冯俊 冯岩 靳松

摘要 本文将超声波和无线网络定位的方法应用于多机器人协同定位的问题当中，并提出利用机器人网络与自适应滤波的方法实现超声波信号的处理。仿真实验表明，本文提出的方法可以在有与发射超声波频率相同的复杂噪声的情况下检测出超声波信号，并在机器人发生碰撞的情况下完成机器人定位的任务。

关键词 自适应滤波 超声传感器 多机器人 协同定位 卡尔曼滤波

1．引言

在自主式移动机器人相关技术的研究中，导航技术是其研究核心。而定位问题是移动机器人导航的最基本环节。自主移动机器人只有准确地知道其自身的位置、工作空间中障碍物的位置以及障碍物的运动情况，才能安全有效地进行运动。所以，自主移动机器人的自身定位问题就显得尤为重要，是其最重要的功能之一。

近年来多机器人的合作定位受到了越来越多的重视和研究，逐渐成为一个活跃的研究领域。如果机器人群能够探测到它们的同伴，并且可以与同伴进行信息交换，则可以利用机器人之间的相对观测信息来提高它们的定位精度，尤其是对异类机器人群，合作定位的优势更加明显[1]。

本文将文献[2]中提出的一种利用超声波和无线网络定位的方法应用于多机器人协同定位的问题当中，并提出利用机器人网络与自适应滤波的方法实现超声波信号的处理与用卡尔曼滤波来完成协同定位与自主定位的融合。

2．多机器人协同定位原理

在多机器人的协同定位中，根据机器人体系，将控制机器人当做基站，它发送超声波，假设一组中至少有两台执行机器人，那么，当两台机器人都有超声波接受器与发射器时，他们可以测得基站与各自的距离，以及两台机器人之间的距离，每次至少有一台机器人处于静止，并且它与基站的连线偏离x轴的角度是已知的，则可以计算出另一台机器人的位置。

多机器人协同定位原理如图2-1所示，图中小圆圈代表机器人。假定多机器人的体系由三台机器人组成，其中一台为控制机器人，一台为环境探测机器人，一台为辅助定位机器人，它们各自的任务是： K——二级控制机器人，接收超声波，算出距离。 R1——环境探测机器人，发送超声波，用来进行定位。 R2——辅助定位机器人，接收超声波，算出距离。 假设K与R2一开始静止，他们之间的距离可以通过超声传感器获得，且R2与x轴的夹角θ2已知，则可以知道R1与x轴的夹角θ1为：

?1??2??3(d3) ?(d2)2?(d1)2?3 ?arcos2d2d32

（1）

（2）

图1超声波定位原理图

式中：

d1——R1与R2之间的距离； d2——K与R1之间的距离； d3——K与R2之间的距离。

由于超声传感器的测量范围有限，而多机器人体系必须向超出范围的区域移动。所以当K与R2准备

移动到新的指定位置时，其定位的职责必然要转移给其他机器人，使K或R2与其他机器人的职责互换。只要确保群体内有两台机器人保持不动，通过坐标系的变换，多机器人体系才能不断地完成定位和向前运

动。

3．自适应滤波系数调整LMS算法[3]

在多机器人协同定位中，需要在有与机器人发射超声波频率相同的复杂噪声的情况下检测出机器人的超声波信号，这对多机器人的协同定位是非常重要的。在定位中采用自适应滤波的方法实现对超声波信号的处理。

自适应滤波系数调整LMS算法基本思想：

假定有一个FIR型滤波器，其可调整系数为{h（k），0≤k≤n-1}，设{x(n)}表示滤波器的输入序列，相应的输出序列为{y（n）}，其中：

y(n)?h(k)x(n?k)（3）

k?0

假设也有一个预期输出序列{d（n）}，将FIR滤波器输出{y(n)}与{d (n)}进行比较。于是就可以取d

?n?1（n）与y（n）之差形成误差序列{e（n）}。即e（n）=d(n)-y(n),n=0,…,m，选择FIR滤波器系数使平方误差和最小。

LMS算法步骤：

从任意选择的一组{h(k)}初始值开始，接着在每个新的输入采样值{x(n)}进入自适应滤波器后，计算相应的输出（{y（n）}），再形成误差信号e（n）=d(n)-y(n)，并根据以下方程不断修正滤波器的系数：

hn(k)?hn?1(k)??e(n)x(n?k)式中:

（ 4 ）

?——步长参数；

x(n?k)——n时刻输入信号在滤波器的第k个

抽头处的采样值;

e(n)x(n?k)——滤波器第k个系数的负梯度的

近似值。

图2所示为期望信号为频率为1，幅值为5的正弦波，噪声是高斯白噪声，自适应滤波的效果图。可以看出从复杂噪声中提取出预期信号的效果还是比较理想的。

4 .自适应滤波器在多机器人协同定位中的应用

首先假定多机器人系统中的每一个机器人都具有与其它机器人进行通信的功能。根据多机器人协同定位的原理，在环境探测机器人发出超声波的同时发出通信信号，通信信号中有环境探测机器人激励压电陶瓷的频率以及激励电压的大小，还有其传感器压电参数等的特性。当控制机器人接收到通信信号后，将进行以下的工作： （1）立即启动定时器；

（2）计算出环境探测机器人超声波信号的特征、频率、以及幅值；

（3）根据本身超声传感器接收电路的特性算出采样后的信号的特征、频率、以及幅值，形成预期值。

超声换能器发送的是频率为4x104 Hz，幅值成方波变化的超声波信号，信号经放大器放大后，分两路传给了处理器，一路是经过肖特基二极管进行检波后传给比较器比较，设定大于0.5，则比较器输出高电平，小于0.5伏则为低电平，最后信号接处理器管脚，触发中断，定时器停止，则可得到一个时间，乘以声速可以得到R1与K的距离，同理可以得到R1与R2的距离。另一路是经过AD采样芯片后，把数字信号

图2 自适应滤波效果

传给处理器。处理器根据通信的得到得预期的信号值，然后进行自适应滤波，其步骤如下：

（1）将滤波后的信号与预期值进行模式匹配，由于已经经过了滤波，所以，只需要提取幅值为特征。取采样点绝对值的平均值与期望的信号进行比较。

（2）给定一定的阀值，采样点绝对值的平均值满足阀值要求时，认为有超声波信号，转向下一步。否则，认为没有超声波信号，转向（4）。

（3）以K为原点，指向R2方向为x轴，R1所在象限为第一象限（假定R1只在一个象限内运动）建立坐标系，然后根据公式（1）计算出R1的位姿。 （4）定时器溢出。

5.机器人系统协同定位与机器人自主定位的融合

假设单个机器人安装有码盘与电子罗盘，可以自主定位。利用卡尔曼滤波算法预测位置，可以很好的解决实际过程中由于码信息进行推测，此时所带来的误差是非常大的。

图3假设机器人在坐标（15，15）发生一次碰撞，导致机器人偏移到（10，15）处，实线是机器人的轨迹，虚线是利用协同定位校正后的轨迹，由于机器人带有电子罗盘，还可以找准航向，但是位置已经偏离预测值很远。

根据卡尔曼滤波的公式

图3 机器人发生碰撞卡尔曼滤波效果图

盘误差所带来的影响。但是当单个机器人受到外来力的作用时，位姿被迫改变，但是系统还是按照以前的

?k?Ax?k?1?Buk (5) x?k?1?k(zk?Hx?k?1) (6) ?k?xxpk?Apk?1AT?Q (7) pk?(I?KkH)pk (8)

Kk?pkHT(HpkHT?R)?1 (9)

式中：

?k——下一步的预测值； xuk——控制值；

pk——观测前的协方差； pk——观测后的协方差；

?k——利用观测数据的更新值。 x?k?1，利利用机器人的协同定位方法得到的值x来代机器人协同定位得到的定位值替换式（5）中的x?k，则可以推测出机器人的真实值。 用新观测的数据来更新x

6.仿真实验

6.1利用自适应滤波方法的仿真实验

模式匹配算法采用提取幅值为特征，采样周期为2.5x10s，采样频率为4x10.提取平均的幅值，即把 采样点去除符号相加除以采样点的个数。如果大于4.5小于5.5，则认为匹配成功。否则，匹配失败。 图6-1为接收到的信号中有超声换能器发出的信号，为频率为4x10，幅值为5v的方波，噪声频率

4

-5

8

为4x10，幅值为5v的正弦波和高斯白噪声，经过自适应滤波器滤波。经过计算，幅值均值为4.956，模式匹配成功。

图6-2为接受到得信号中没有超声波换能器发出的信号，噪声频率为4x10，幅值为5v的正弦波和高斯白噪声，经过自适应滤波器滤波. 经过计算，幅值均值为4.126，模式匹配失败。

带噪声信号100-1000.511.5期望信号100-1000.511.5滤波结果100-1000.511.522.5x 103-54

4

带噪声信号10022.5x 103-5-1000.511.5期望信号22.5x 103-5100-1022.5x 103-500.511.5滤波结果22.5x 103-5100-1000.511.522.5x 103-5

图4 信号中有超声换能器发出信号滤波效果图

图5信号中没有超声换能器发出信号滤波效果图

估计滤波码盘读数真是轨迹6.2机器人系统协同定位与机器人自主定位的融合

图6是利用协同定位的值更新的轨迹。假设机器人在坐标（15，15）发生一次碰撞。实线是机器人的轨迹，虚线是利用协同定位校正后的轨迹，从图中看，机器人可以在发生碰撞的情况下完成定位。 7.结论

通过仿真实验表明，本文提出的方法可以在有复杂干扰的情况下检测出超声波信号，并在机器人发生碰撞的情况下完成机器人定位的任务。

由于超声波的特性，所以当超声发射器与超声接

2520151050 0510152025图6 利用协同定位的值更新的轨迹

收器之间有障碍物时，接收器很有可能不能接收到超声信号。误差量是变大的。

并且，本定位方法是一种基于测量的方法，尽管超声波测距的误差很小，但是，随着机器人体系的移动，

所以，本文作为一种辅助定位的方法是可行的，也就是说，当多机器人体系内的自主机器人可以通过惯性法进行定位时，利用本文的方法可以进行修正自主定位所带来的误差。

参考文献：

[1] 王玲.位置环境中基于相对观测量的多机器人合作定位研究.国防科技大学 学位论文 2006 [2] 庄毅等.无线传感器网络超声波定位算法的研究.航空计算技术 Vol. 38 No. 4July. 2008 [3] Vinay K.Ingle等著，陈怀琛等译.数字信号处理及其matlab实现.电子工业出版社 1998 [4] 何琳 朱海潮等. 声学理论与工程应用.科学出版社 2006

作者简介：胡波，男，硕士研究生，北京理工大学机械与车辆学院， 联系方式：Tel:010-68914017（O）,传真：010-68914017 E-mail:fengjun@bit.edu.cn,

通讯地址：北京市中关村南大街5号，北京理工大学三院 （100081）

本文来源：https://www.bwwdw.com/article/ofow.html