一种基于WSN的机器人三维精确定位算法

郭 龙，熊 伟，李牧东

(空军工程大学电讯工程学院，陕西西安710077)

0 引言

WSN［1］主要是用来监测网络部署区域内各种环境特性，如温度、湿度、光照、声强、磁场强度、压力(强)、运动物体加速度(速度)和化学物质浓度(不同的特性需要不同的传感器)，但对这些传感数据在不知道相应位置信息的情况下，往往是没有意义的。因此，如何确定无线传感器网络中节点的位置信息(节点定位)成为必须解决的关键问题之一。

1 节点定位

在传感器网络节点定位技术中，根据是否已知自身的位置，把传感器节点分锚节点(Beacon Node)和盲节点(Unknown Node)。锚节点在网络节点中所站的比例很小，可以通过携带GPS定位设备等手段获得自身的精确位置。锚节点是盲节点定位的参考点。除了锚节点外，其他传感器节点就是盲节点，它们通过锚节点的位置信息来确定自身位置。通信半径内的所有节点都是该节点的邻居节点。在传感器节点定位［2，3］过程中，盲节点在获得对于邻近锚节点的距离后，或者获得邻近的锚节点与盲节点之间的相对角度后，一般使用三边测量法、三角测量法和极大似然估计法来计算自己的位置此算法采用的是极大似然估计法。

已知锚节点坐标分别为 (x1，y1，z1) ，(x2，y2，z2) ，(x3，y3，z3)，…，(xn，yn，zn)，通过 TDOA［4-7］算法测得的锚节点与盲节点之间的距离分别为d1，d2，d3，…，dn，设盲节点 P 坐标为 (x，y，z)，则

变形为:

上式的线性方程表示方式为:AX=b，

最后，通过最小均方差算法求出P坐标为X=(ATA)-1ATb。

2 机器人定位算法

2.1 TDOA 定位

基于距离的定位机制(Range-based)是通过测量相邻节点间的实际距离或方位进行定位。具体过程分为3个阶段:①测距阶段，盲节点首先测量到邻居节点的距离或角度，然后进一步计算到邻近锚节点的距离或方位，在计算到邻近锚节点的距离时，可以计算盲节点到锚节点的直线距离，也可以用二者之间的跳段距离作为直线距离的近似;②定位阶段，盲节点在计算出到达3个或3个以上锚节点的距离或角度后，利用三边测量法、三角测量法或极大似然估计法计算盲节点的坐标;③ 修正阶段，对求得的节点坐标进行求精，提高定位精度，减少误差。

在基于距离的定位中，常用的测量节点间距离或角度的技术有 RSSI、TDOA、AOA和 TOA四种。RSSI和TDOA两种方法最为常用。此算法采用精度高的TDOA技术。

TDOA测距技术被广泛应用在无线传感器网络定位方案中。一般在节点上安装超声波收发器和RF收发器。测距时，在发射端2种收发器同时发射信号，利用声波与电磁波在空气中传播速度的巨大差异，在接收端通过记录2种不同信号到达时间的差异。基于已知信号传播速度，可以直接把时间差异转化为距离。该技术的测距精度较RSSI高，可达到cm级，但受限于超声波传播距离有限以及NLOS问题对超声波信号的传播影响。

锚节点周期性地发射无线射频信号与超声波信号。假设RF的传播速度为vr，超声波信号的传播速度为vu，ΔT为2种信号到达盲节点的时间差。则通过式(3)可以求得锚节点到盲节点的距离d:

此处，锚节点有2个作用:提供自身的位置信息以及ID:而为了减少功率消耗，超声波信号只是一个窄脉冲，并不含任何数据的脉冲信号。在得到超声波信号的传播时间以及其传播速度已知的情况下，就能计算出2个节点之间的距离。

2.2 机器人定位模型

为了对机器人进行精确定位，需要在机器人内部安装3个固定的盲节点，这3个盲节点安装要求是必须安装在机器人某一平面上，此平面与机器人完全保持一致，这3个盲节点跟随机器人一起运动。首先，3个盲节点的坐标利用无线传感器网络定位技术进行确定;然后，根据几何关系就可以求出机器人的具体位置以及移动方位，如图1所示。

图1 机器人定位模型

图1中，n为锚节点的个数;P、Q、G为安装在机器人内部的盲节点。3个盲节点的坐标P，Q，G可以通过无线传感器网络极大似然的方法计算得到。假设盲节点坐标 P，Q，G 的坐标分别为:(xp，yp，zp)，(xq，yq，zq)，(xg，yg，zg)，机器人的坐标可以用P和G的中点坐标(xm，ym，zm)精确表示为:

以世界坐标系为参考系，θ1和θ2分别为机器人偏离正北或正南方向和向上或向下方向的角度。根据几何关系可以得到:

当xq-xg＜0，yq-yg＞0时，θ1为北偏西的角度;

当xq-xg＞0，yq-yg＞0时，θ1为北偏东的角度;

当xq-xg＜0，yq-yg＜0时，θ1为南偏西的角度;

当xq-xg＞0，yq-yg＜0时，θ1为南偏东的角度;

当zq-zg＜0，时，θ2为向下的角度;

当zq-zg＜0，时，θ2为向上的角度。

3 实验结果和分析

试验中，有6个锚节点，它们的坐标信息是预先知道的，它们各自的坐标如表1所示。

表1 锚节点坐标信息

在测量锚节点到盲节点距离过程中需要知道超声波的传播速度，超声波的传播速度由式(7)计算得到。通过传感器节点的测距设备测量得到传播时间差，再根据式(4)就可以计算得到各锚节点到盲节点的直线距离。

式中，v0=331.45;T0=273.15;T为温度传感器测量的环境温度。每个锚节点到盲节点的距离通过试验得到，为了减少测量误差，每组做10次求平均值，测量结果如表2所示

表2 测量各个锚节点到盲节点的距离数据

根据公式X=(ATA)-1ATb，可以计算得到P的坐标是(41.47，50.58，12.60)，Q 的坐标是(45.23，46.34，12.60)，G 的坐标是(50.61，43.41，15.10)。锚节点和盲节点的分布情况如图2所示。

根据式(5)可以计算得到 xm=46.04，ym=47.99，zm=13.85，可以用坐标 (46.04，47.99，13.85)精确表示机器人的位置。根据式(6)可以计算得到 θ1=51.110o，θ2=0.389o，由于 xq-xg＜0，yq-yg＞0且zq-zg＜0，所以机器人现在的移动方向为北偏西 51.110o，下坡角度为 0.389o。达到了对机器人坐标和运动姿态进行判断的效果。

图2 锚节点与盲节点分布图

4 结束语

通过TDOA技术测量位置节点到锚节点的距离得到距离信息后，采用最大似然估计算法求出盲节点的坐标，进而求出机器人的坐标及运动方位。该方法在小区域内能实现较高精度的定位，能够满足机器人的定位需求。运用实验的数据对定位方法进行了验证，达到了预期的效果。

［1］孙利民，李建中，陈 渝，等.无线传感器网络［M］.北京:清华大学出版社，2005.

［2］石琴琴.无线传感器网络节点自定位系统及其算法研究［M］.上海:上海交通大学，2009.

［3］王小平，罗 军，沈昌祥.无线传感器网络定位理论和算法［J］.计算机研究与发展，2011，48(3):353-363.

［4］SAVVIDES A，HAN C C，SRIVASTAVA M B.Dynamic Finge-grained Localization in Ad-hoc Networks of Sensor［C］.Italy:In:Proc 7th Annual Int′1 Conf on Mobile Computing and Networking(MobiCom)，2001:166-179.

［5］BULUSU N，HEIDEMANN J，ESTRIN D，et al.Selfconfiguringlocalization Systems:Design and Experimental Evaluation ［J］.ACM Transactions on Embedded Computing Systems，2004，3(1):24-60.

［6］CRAMER O.The Variation of the Specific Heat Ratio and the Speed of Sound in Air with Temperature，Pressure，Humidity，and CO2 Concentration ［J］.Acoustical Society of AmericaJournal，1993，93(5):2 510-2 516.