0 引言

面对电子商务的应用需求,物流仓储行业得以快速壮大,各大物流公司在智能化、自动化等方面进行了激烈的角逐,而这一切都是建立在精准的位置定位及导引基础之上的。在物流仓储的特殊环境下,传统定位系统及定位方法的应用受限,如GPS在仓库环境中定位不准,WiFi受到密集分布的货架、物料的阻断等因素的影响。同时为满足不同的应用场景,人们建设了种类多样的现代物流仓库,不同仓库采用不同构造,配备了大量器具,这就增大了仓储自动导引车的定位难度,虽然单目视觉的定位方法能够解决这一问题,可是却很难实现规模化;传统的基于陀螺仪、里程计的惯性导航方法,始终未能解决累计误差的问题,随着时间的延长,这一方法的导航效果变差;RFID、WiFi无线传感器可以根据信号强度进行测距,可是检测精度不高,无法满足车辆导引的要求。而UWB技术是基于纳秒至微纳秒的非正弦波窄脉冲进行数据传输,由多个接收器接收到参考标签所发射的超宽带脉冲信号进行定位,具有穿透力强,结构简单、适应性强、带宽极宽、造价低廉等优点,从而被广泛应用在无线定位中。但利用UWB技术进行定位受非视距传播(NLOS)、多径干扰等的影响,导致定位精度差。因此本研究以UWB为背景,在结合UWB常用的TDOA测距方法中,构建联合定位算法,并通过仿真验证其正确性和可行性[1,2,3,4,5]。

1 基本方法

1.1 TDOA测距原理

在基于TOA的UWB测距基础上,人们创建了双曲线定位法(TDOA法)。该方法的基本原理是:首先监测各个基站接收到由标签发射的脉冲信号的时间差,然后根据时间差计算出距离差,从而达到定位目的。TDOA方法要求各基站的时钟必须同步,以保证时间差的准确性。

假设3个基站BS1、BS2、BS3,它们的坐标表示为[xi,yi](i=1,2,3),标签的坐标{x,y},那么,TDOA定位原理如图1所示。

图1 TDOA定位方法原理图 

在图1中,3个基站与标签的间距依次是r1、r2、r3,将基站BS1设定为基准基站,那么,基站BS1、BS2与标签的距离差r21算式如下:

r21=r2−r1=(x2−x)2+(y2−y)2−−−−−−−−−−−−−−−−√−(x1−x)2+(y1−y)2−−−−−−−−−−−−−−−−√         (1)$r{}_{21}=r{}_2-r{}_1=\sqrt{(x{}_2-x){}^2+(y{}_2-y){}^2}-\sqrt{(x{}_1-x){}^2+(y{}_1-y){}^2}(1)$

标签一定位于以BS1、BS2为焦点,以与2个焦点之间的距离差为r21的双曲线对上。相应地,基站BS1、BS3与标签的距离差r31,算式如下:

r31=r3−r1=(x3−x)2+(y3−y)2−−−−−−−−−−−−−−−−√−(x1−x)2+(y1−y)2−−−−−−−−−−−−−−−−√         (2)$r{}_{31}=r{}_3-r{}_1=\sqrt{(x{}_3-x){}^2+(y{}_3-y){}^2}-\sqrt{(x{}_1-x){}^2+(y{}_1-y){}^2}(2)$

此时,标签一定位于以BS1、BS3为焦点,以与2个焦点之间的距离差为r31的双曲线对上。

综上,标签的位置一定是两组双曲线的交点。综合上式(1)、(2)以及速度时间公式,建立以下方程组:

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪(x2−x)2+(y2−y)2−−−−−−−−−−−−−−−−√−(x1−x)2+(y1−y)2−−−−−−−−−−−−−−−−√=r21(x3−x)2+(y3−y)2−−−−−−−−−−−−−−−−√−(x1−x)2+(y1−y)2−−−−−−−−−−−−−−−−√=r31r21=t21×Cr31=t31×C         (3)$\{\begin{array}{l}\sqrt{(x{}_2-x){}^2+(y{}_2-y){}^2}-\sqrt{(x{}_1-x){}^2+(y{}_1-y){}^2}=r{}_{21}\\ \sqrt{(x{}_3-x){}^2+(y{}_3-y){}^2}-\sqrt{(x{}_1-x){}^2+(y{}_1-y){}^2}=r{}_{31}\\ r{}_{21}=t{}_{21}\times C\\ r{}_{31}=t{}_{31}\times C\end{array}(3)$

根据上式(3)看出,只要确定了基站BS1、BS2接收脉冲信号的时间差t21和基站BS1、BS3接收脉冲信号的时间差t31,即可确定标签位置坐标。结合上图1,两组双曲线有2个交点,每一个交点坐标都是方程组的一组解,但其中一个是无效解,需要根据先验信息做出判断,剩余的交点坐标就是最终的标签位置[6,7,8]。

1.2 Taylor算法

UWB无线脉冲信号的传播存在两种情形:其一,视距传播(LOS),即标签与各基站之间并无任何阻碍,信号可以沿着“直线”进行传播,在此情形中的定位精度较高;其二,非视距传播(NLOS),即标签与基站之间存在障碍,信号通过衍射或反射进行传播,传播路径存在“绕路”的现象,因此,增大了信号传播的距离和时间,并最终损害了定位精度,这部分定位误差被称为“NLOS误差”。而在非视距传播定位算法中,常用的算法为Taylor定位算法。其具体的实现过程为:

首先对标签的初始位置进行估计,并以估计值作为初始值,然后根据最小平方误差准则对初始值进行修正,如此循环多次,直到修正后的结果处于预设的门限值以内,随机结束迭代过程。可见,Taylor算法是典型的迭代型算法。

Taylor算法的计算过程,展示如下:

在平面坐标系中,标签坐标是[x,y],建立迭代函数fi(x,y),算式如下:

fi(x,y)=(xi+1−x)2+(yi+1−x)2−−−−−−−−−−−−−−−−−−−√−(x1−x)2+(y1−x)2−−−−−−−−−−−−−−−−−√         (4) fi(x,y)=R⌢i+1,1+εi+1,1         (5)$\begin{array}{l}f{}_i(x,y)=\sqrt{(x{}_{i+1}-x){}^2+(y{}_{i+1}-x){}^2}-\sqrt{(x{}_1-x){}^2+(y{}_1-x){}^2}(4)\\ f{}_i(x,y)=\stackrel{⌢}{R}{}_{i+1,1}+\epsilon {}_{i+1,1}(5)\end{array}$

式中,i=1,2,⋯,N−1,R⌢i,1$i=1,2,\cdots ,{\rm N}-1,\stackrel{⌢}{R}{}_{i,1}$表示标签到基站距离差的估计值,εi,1表示对应的距离差的误差的协方差矩阵。

估计的标签的初始坐标是[xg,yg],满足:

{x=xg+δxy=yg+δy         (6)$\{\begin{array}{l}x=x{}_{\text{g}}+\delta {}_x\\ y=y{}_{\text{g}}+\delta {}_y\end{array}(6)$

式中,δx、δy表示标签坐标的误差。

对迭代函数fi(x,y)进行Taylor级数展开,前两项约等于:

fi,g+αi,1δx+αi,2δy≈R⌢i+1,1+εi+1,1,i=1,2,⋯,N−1         (7)$f{}_{i,\text{g}}+\alpha {}_i{}_{,1}\delta {}_x+\alpha {}_i{}_{,2}\delta {}_y\approx \stackrel{⌢}{R}{}_{i+1,1}+\epsilon {}_{i+1,1},i=1,2,\cdots ,{\rm N}-1(7)$

将上式改写成矩阵形式:

e=D-Aδ

其中,

e=⎡⎣⎢⎢⎢⎢ε2,1ε3,1⋯εN,1⎤⎦⎥⎥⎥⎥,δ=[δxδy],D=⎡⎣⎢⎢⎢⎢⎢⎢R⌢2,1−f1,gR⌢3,1−f2,g⋯R⌢N,1−fN−1,g⎤⎦⎥⎥⎥⎥⎥⎥,A=⎡⎣⎢⎢⎢⎢a1,1a2,1⋯aN−1,1a1,2a2,2⋯aN−1,2⎤⎦⎥⎥⎥⎥$e=\left[\begin{array}{c}\epsilon {}_{2,1}\\ \epsilon {}_{3,1}\\ \cdots \\ \epsilon {}_{{\rm N},1}\end{array}\right],\delta =\left[\begin{array}{c}\delta {}_x\\ \delta {}_y\end{array}\right],D=\left[\begin{array}{c}\stackrel{⌢}{R}{}_{2,1}-f{}_{1,g}\\ \stackrel{⌢}{R}{}_{3,1}-f{}_{2,g}\\ \cdots \\ \stackrel{⌢}{R}{}_{{\rm N},1}-f{}_{{\rm N}-1,g}\end{array}\right],A=\left[\begin{array}{cc}a{}_{1,1}& a{}_{1,2}\\ a{}_{2,1}& a{}_{2,2}\\ \cdots & \cdots \\ a{}_{{\rm N}-1,1}& a{}_{{\rm N}-1,2}\end{array}\right]$

对上式执行加权最小二乘估计,得出:

δ=[ATR−1A]−1ATR−1D         (8)$\delta =\left[A{}^{\text{Τ}}R{}^{-1}A\right]{}^{-1}A{}^{\text{Τ}}R{}^{-1}D(8)$

式中,R表示TDOA测量值的协方差矩阵。

迭代按照如下方式进行:

{x(k+1)=x(k)+δxy(k+1)=y(k)+δy         (9)$\{\begin{array}{l}x{}^{(k+1)}=x{}^{(k)}+\delta {}_x\\ y{}^{(k+1)}=y{}^{(k)}+\delta {}_y\end{array}(9)$

式中,[x(k),y(k)]表示进行k次迭代后标签的位置坐标,如果|εx|+|εy|$\left|\epsilon {}_x\right|+\left|\epsilon {}_y\right|$小于门限值,随机终止迭代过程,由此得出标签估计坐标[x,y]。可见,在已知估计初值以后,适当调低门限值能够通过牺牲计算时间或者增加迭代次数等方式来提高定位精度。简言之,在实操中应当根据现实需要来合理设定门限值。在应用方面,Taylor算法适用于不同的信道环境,并且发挥理想的定位功能。可Taylor算法不能提供明确的表达式解,并且估计初始值不能与标签实际位置的偏差过大,否则会影响算法的收敛过程,最终损害定位精准度。

1.3 Fang算法

Fang算法为TDOA测距的一种,主要通过双曲线方程的方式来获取最优解。该方法的特点在于计算量小,且在视距条件下,得到的定位精度较高。具体计算过程为:

根据图1的TDOA原理,同样标签用[x,y]表示,三个定位基站的坐标可分别表示为[xi,yi],i=1,2,3$\left[x{}_i,y{}_i\right],i=1,2,3$。设三个基站到标签的距离分别为r1,r2,r3,以图1中的BS1作为准基站,那么标签到BS2和BS3的距离查分别为r21、r31,由此有:

ri=(x−xi)2+(y−yi)2−−−−−−−−−−−−−−−−√(i=1,2,3)         (10)$r{}_i=\sqrt{(x-x{}_i){}^2+(y-y{}_i){}^2}(i=1,2,3)(10)$

对上式平方:

ri2=(x-xi)2+(y-yi)2 (11)

令Mi=x2+y2

同时由:

ri2=(r1+ri1)2 (12)

由此得到:

r2i${}_i^2$=(x-xi)2+(y+yi)=Mi-(2xxi+2yyi)+x2+y2 (13)

取i=1,则有:

r21${}_1^2$=Mi-(2xx1+2yy1)+x2+y2 (14)

由式(13)减去式(14),则可以得到:

r21${}_1^2$+2r1ri1=Mi-M1-2x(x1-x1)-2y(y1-y1) (15)

由于基站已知,那么只有x,y,r1未知,那么可以将(15)简化为:

{−2r1r21=r212−x22+2xx2−2r1r31=r312−(x23+y23)+2(xx3+yy3)         (16)$\{\begin{array}{l}-2r{}_{1}r{}_{21}=r{}_{21}{}^2-x{}_2{}^2+2xx{}_2\\ -2r{}_{1}r{}_{31}=r{}_{31}{}^2-(x{}_3^2+y{}_3^2)+2(xx{}_3+yy{}_3)\end{array}(16)$

消去r1,得到:

y=ux+v (17)

其中,

⎧⎩⎨⎪⎪U=r31x2−r21x3r21y3V=x23+y23−r231+r21r31−r31r21x222y3$\{\begin{array}{l}U=\frac{r{}_{31}x{}_2-r{}_{21}x{}_3}{r{}_{21}y{}_3}\\ V=\frac{x{}_3^2+y{}_3^2-r{}_{31}^2+r{}_{21}r{}_{31}-\frac{r{}_{31}}{r{}_{21}}x{}_2^2}{2y{}_3}\end{array}$

将式(17)、式(14)带入式(16),从而得到:

Ax2+Bx+c=0 (18)

其中,

⎧⎩⎨⎪⎪⎪⎪A=4r221(1+U2)−4x22B=8UVr221−4r221x2+4x22C=4V2r221−(x22−r221)2$\{\begin{array}{l}A=4r{}_{21}^2(1+U{}^2)-4x{}_2^2\\ B=8UVr{}_{21}^2-4r{}_{21}^{2}x2+4x{}_2^2\\ C=4V{}^{2}r{}_{21}^2-(x{}_2^2-r{}_{21}^2){}^2\end{array}$

通过式(18)的求解,可得到两个解,采用先验信息消去一个x,并带入式(17),由此可以得到y,即为标签的坐标值[9,10,11,12]。

2 Taylor算法改进

在实际的物流中心运作中,不间断地进行装箱、补货、发货等操作,以及场内叉车、人员、箱体等物体的运动,使得基于TDOA的定位存在NLOS误差。为消除NLOS误差及其影响,需对定位算法进行改进。传统的方法是首先通过Fang算法进行初步定位,然后再通过泰勒级数进行精确定位。但由于UWB通信对信道要求不高,所以通常设定一个接近标签真实位置的估计值,然后再通过泰勒级数进行精确定位。这种设定接近标签真实位置的方法在实际中往往存在很大的主观性。由此,建立Fang-Taylor联合算法,利用Fang算法计算出TDOA测量值,从而估计标签的大致位置,然后以估计坐标充当Taylor算法的初始估计值进行定位迭代运算,最终定位出标签的准确坐标。这种联合算法不仅在LOS环境下的定位精度较高,且适用于NLOS情形中的定位,能够有效控制NLOS误差。具体实现过程如图2所示。

图2 改进Taylor算法流程   

3 Matlab仿真试验

3.1 仿真环境部署

为验证上述改进算法的好坏,选用Matlab R2014a软件进行仿真。设定仿真空间是30 m×30 m的室内环境,将三个基站随机布设在室内空间的边缘,坐标依次是(0,0)、(0,30)、(30,30)。标签位置在空间中随机生成。

3.2 LOS环境下的仿真

在LOS环境中,测量数据都采用误差服从u=0,σ2=0.252的正态分布。设定Fang-Taylor联合算法的门限值是0.01,最大迭代次数是50次,如果经过50次迭代后,Fang-Taylor联合算法仍未收敛,就把第50次的迭代运算结果进行反馈。两算法在各种情况下都执行500次,并对每次的标签定位误差结果进行对比分析。评价对象选择RMSE和平均时延。RMSE计算公式为:

RMSE=1m∑i=1m(h(xi)−yi)2−−−−−−−−−−−−−−−√         (19)$R{\rm M}SE=\sqrt{\frac{1}{m}{\displaystyle \sum _{i=1}^m(h(x{}_i)-y{}_i)}{}^2}(19)$

仿真结果展示如图3和图4。

图3 Fang算法仿真结果   

图4 联合算法在LOS环境中的仿真结果   

对比结果如图5所示。

图5 两种算法在LOS环境中仿真结果的对比图   

综合上图3、图4进行分析,在视距条件下,摆脱了NLOS误差的影响,相较于Fang算法,Fang-Taylor联合算法的平均RMSE与之相近,说明改进算法在定位精度方面并无显著改善,当然,改进算法的定位精度是优于Fang算法的,但这是以花费更多时间为代价的。究其成因,LOS条件下的Fang算法能够实现较高的定位精度,Fang-Taylor联合算法无法继续改善定位精度,并且增大耗时的风险[13,14,15]。

3.3 NLOS环境下的定位仿真

同样以上述的实验环境进行仿真,在NLOS环境下,把测量数据的误差设为服从u=0,σ2=0.252的正态分布的情况,并且对部分测量数据增加服从u=0.2,σ2=0.252的正态分布的非视距误差。Fang-Taylor算法的门限值设定成0.01,最大迭代次数设定为50次。两算法在各种情况下都执行500次,并对每次的标签定位误差结果进行对比分析。通过实验得到图6和图7的结果。

图6 Fang算法在NLOS环境中的仿真结果   

图7 联合算法在NLOS环境下的仿真结果   

上述不同算法的对比结果如图8所示。

图8 两种算法仿真结果对比   

综合上图6、图7的分析看出,在非视距条件下,受到NLOS误差的影响,Fang算法的定位精度并不理想,而改进后的Fang-Taylor联合算法的定位精度通过图8的结果对比,具有明显优势,说明改进算法能够有效抑制NLOS误差,从而改善对标签的定位精度[16]。

3.4 现场试验结果

3.4.1 试验环境部署

模拟150 m(L)×50 m(W)×10 m(H)的物流仓库,并在其中随机布设多个可以移动的0.5 m(L)×0.01 m(W)×1.9(H)m的挡板,用以塑造NLOS环境。为保障AGV实际坐标的精确度,采用人工操作的方式控制AGV的运行。仿真实验中各物体的布设点如表1所示。

表1 物体位置坐标 

3.4.2 测距误差拟合修正

测距数据是定位算法的基础,通过修正3个基站与标签的测量距离,就能够实现测距误差的拟合修正。在实验厂内,固定标签的位置,随机布设3个基站的位置。在完成试验以后,各基站与标签的实际值和测距值,汇总如表2所示。

表2 测量值与实际值对比 

根据上述结果看出,测量值相对于实际值是线性分布的,可以利用线性方程的方法进行拟合,由此绘制出各基站测距值的拟合曲线,具体如图9所示。

图9 各基站测距值拟合曲线   

由此列出各基站的拟合公式,如下:

⎧⎩⎨⎪⎪L1=0.9442l1−0.1232L2=0.8946l2−0.1077L3=0.9237l2−0.2019$\{\begin{array}{l}L{}_1=0.9442l{}_1-0.1232\\ L{}_2=0.8946l{}_2-0.1077\\ L{}_3=0.9237l{}_2-0.2019\end{array}$

其中,l1、l2、l3与L1、L2、L3分别表示基站1、2、3的测距初始值与测距修正值。而在对数据修正以后,UWB模块的测距精度达到较高水准。

3.4.3 静态定位结果与分析

在试验场内,随机选定10个位置,其中,前5个位置符合LOS条件,后5个位置符合NLOS条件,这些位置用作AGV定位点,测试基站-标签距离,并代入Fang算法、Fang-Taylor联合算法进行定位计算。各组定位数据的获取方式是,选择7组基站-标签距离数据,删除7组数据中的最大值、最小值,以剩余5组数据作为样本进行定位计算,求算出多次定位计算的平均值,并确定为最终的定位数据。通过定位计算,得到联合算法对于各个位置的定位结果及误差见表3所示。

表3 定位算法误差 

根据表3分析,联合算法在NLOS条件下的定位精度存在明显差距,其中,Fang算法的定位精度大幅降低,Fang-Taylor联合算法的定位精度依旧较高。据此推断,基于UWB的定位系统能够在AGV静态定位情形中发挥理想效果,而改进的Fang-Taylor联合算法能够满足静态NLOS环境中的定位需求。

4 结语

通过上述的研究,改进后的定位算法在UWB通信定位中具有较高的精度,该结果无论是通过仿真验证,还是通过实际的试验验证,定位精度都要明显高于传统的定位算法。而通过研究也可以看出,Taylor级数展开的本质就是减少误差,进而提高计算的精度,可应用于其他的精度计算中。