基于非视距环境下超宽频测距实现室内定位方法与流程

本发明涉及一种室内定位技术，特别涉及一种基于非视距环境下超宽频测距实现室内定位方法。

背景技术：

移动机器人代表了机电一体化较高的水平，该机器人的广泛应用以自主导航作为前提，而较高的导航精度离不开较高的定位精度。在室内环境中布置传感器网络可以较好完成机器人定位与导航的功能。

现有技术中，通常使用多个距离信息组成多个距离方程组，使用最小二乘法求出目标点的坐标，从而使得方程的残差最小，但是由于室内环境的复杂性，传感器网络在测距过程中由于遮挡或者多径效应等原因，所得的距离信息会与实际距离信息产生较大差异，从而会使得定位信息存在较大偏差。

非视距为通信两点之间视线受阻；超宽频技术：通过对具有很陡上升和下降时间的冲激脉冲进行直接调制，使信号具有ghz量级的带宽。

技术实现要素：

本发明是针对室内根据传感器网络的距离信息定位，非视距存在测距偏差的问题，提出了一种基于非视距环境下超宽频测距实现室内定位方法,识别并削弱由于非视距产生的误差，从而提高在非视距环境中的定位精度。

本发明的技术方案为：一种基于非视距环境下超宽频测距实现室内定位方法，具体包括如下步骤：

1)建立室内测试系统：被定位目标上的一个标签和n个基站组成，标签与基站均采用超宽带模块，每个基站与标签之间通信采用超宽频传输技术，所有的基站安装在同一水平面上，被定位目标与基站可在同一水平面，也可不在同一上水平面，但被定位目标运动平面与基站平面要求定高，每个基站与标签之间采用飞行时间tof原理测距；

2)将所有基站和被测目标设在同一水平面，针对二维环境下进行定位计算：

a:对步骤1)建立的室内测试系统进行预测模型估计：

预测阶段如下：

xk|k-1＝axk-1|k-1

pk|k-1＝apk-1|k-1a^t+q

其中，x＝[xyvxvy]^t，xk-1|k-1为k-1时刻状态变量x的最优估计，xk|k-1为k时刻对于状态变量x的预测向量，pk|k-1为k时刻的预测误差协方差矩阵，pk-1|k-1为k-1时刻的校正误差协方差矩阵，q为过程噪声协方差矩阵，设置为对角矩阵；

校正阶段如下：

其中(xi,yi)为第i个基站的位置信息，h(xk|k-1)为k时刻每个基站与被测目标之间的估计距离所组成的列向量；

b：通过建立一个矩阵，让多组测量距离信息与系统预测模型所估计出的距离信息进行比较，判定出非视距测距数据：

根据基站个数n，定义一个矩阵满足：

c＝i-ca·cb

其中，i为n×n维度的单位阵，

对于cai，满足

其中，dvar为距离差阈值，为正数，dk,i为第k时刻第i个基站与被测目标之间的水平距离，hk,i为第k时刻h(xk|k-1)中第i行元素；

对于

其中，为的模，evar为误差阈值，

其中，为根据第i个基站所测量的误差所估计出的目标坐标误差列向量，ri为第i个基站与被测目标之间的距离；

c:定位运算：

定义h为h的雅克比矩阵：

h′k＝chk+(i-c)hk-1

卡尔曼增益如下所示：

kk＝pk|k-1h′k^t(h′kpk|k-1h′k^t+r)

其中r为测量协方差矩阵，设置为对角矩阵，

状态校正式如下：

xk|k＝xk|k-1+kkc(dk-hk)

pk|k＝pk|k-1-kkh′kpk|k-1

其中，dk为第k时刻测量的距离向量，pk|k为校正误差协方差矩阵，xk|k即为在第k时刻的坐标最优估计列向量；

3)若被测目标与基站不在同一水平面上，采用直角三角形的几何关系来求解出被测目标与基站之间的水平距离：

其中，d测为实际测量距离，δh为基站与目标的高度差，d为目标与基站的水平距离。

本发明的有益效果在于：本发明基于非视距环境下超宽频测距实现室内定位方法，根据多组测量距离信息与系统预测模型所估计出的距离信息进行比较，一方面通过测量距离与估计距离差值阈值进行是否为非视距测距数据的认定，另一方面，通过计算测量距离与估计距离差值在估计点对实际坐标偏移大小的阈值进行是否作为非视距测距数据的认定。两种判定方案共同决定距离数据是否为非视距测距数据，然后对非视距测量数据进行排除，再进行相应的定位运算，从而提高定位精度。

附图说明

图1为本发明室内环境下的基站与被测目标排布示意图。

具体实施方式

一种基于无线传感器网络的室内定位系统，对于单目标定位系统来说，由一个标签和多个基站组成，标签与基站均采用超宽带模块，标签和基站采用可充电的锂电池供电。每个基站与标签之间通信采用超宽频传输技术(该技术具有抗干扰性能强、传输速率高、系统容量大、发送功率小、精度高等特点)，所有的基站安装在同一水平面上，被定位目标不要求与基站在同一水平面上，但是要求定高，可将标签安装至移动机器人上。若被测目标与基站不在同一水平面上，此时可采用直角三角形的几何关系来求解出被测目标与基站之间的水平距离：

其中，d测为实际测量距离，δh为基站与目标的高度差，d为目标与基站的水平距离。基站摆放可以设置为如图1的形式，其中图1中五角星为基站，四角星为被测目标，黑线即为遮挡物。

每个基站与标签之间测距采用tof(飞行时间timeofflight)原理，具体实现如下：

将所有的基站与标签设置同步时钟。其次基站向四周发送无线信号，其中该信号中包含发送时刻的时间戳。标签接收到该信号时，根据标签的时间戳与接收到信息中的时间戳进行比对，通过时间差来计算出标签与基站之间的距离，如下式所示：

d测＝c·(treceive-tsend)(2)

其中，c为光在真空中的传播速度，treceive为接收到数据的时间，tsend为发送数据的时间。

通过tof测距原理，得到标签与每个基站之间的距离信息，所获得的距离信息存在噪声，其描述如下：

d(t)＝dreal(t)+n(t)+nlos(t)(3)

其中，d(t)为在t时刻所测量的距离值，dreal(t)为t时刻的真实距离，n(t)为满足均值为0、方差为σ²的高斯随机变量，nlos(t)为t时刻的非视距误差，该值满足nlos(t)≥0。

当标签与基站不在同一水平面时，需要使用公式(1)得到标签与基站之间的水平距离d，然后用二维环境的解法来进行求解；反之，若标签与基站在同一水平面时，则可直接使用获得的距离信息来进行求解，即d＝d测，公式(3)是所测量的距离的误差模型，对所有定位测量均实用。

将所有基站和被测目标设在同一水平面，针对二维环境下的定位计算，目标运动模型和观测模型可以描述如下：

其中，x、y为被测目标的位置坐标，θ为被测目标的偏航角，v为被测目标的速度大小，ω为被测目标的偏航角速度，分别为x、x、y、θ的一阶导数。

其中，(xi,yi)为第i个基站的坐标位置，共n个基站，(x,y)为被测目标的位置，zi为第i个基站与被测目标之间的观测距离。z为观测距离所构成的向量。

为方便运算，将运动模型简化：

其中，x(k)和vx(k)为k时刻目标在世界坐标系下x方向的位置与速度，y(k)和vy(k)为k时刻目标在世界坐标系下y方向的位置与速度。

因此，预测阶段如下：

xk|k-1＝axk-1|k-1(7)

pk|k-1＝apk-1|k-1a^t+q(8)

其中，x＝[xyvxvy]^t，xk-1|k-1为k-1时刻状态变量x的最优估计，xk|k-1为k时刻对于状态变量x的预测向量，pk|k-1为k时刻的预测误差协方差矩阵，pk-1|k-1为k-1时刻的校正误差协方差矩阵，q为过程噪声协方差矩阵，设置为对角矩阵。

校正阶段如下：

其中(xi,yi)为第i个基站的位置信息，h(xk|k-1)为k时刻每个基站与被测目标之间的估计距离所组成的列向量。

根据基站个数n，定义一个矩阵满足：

c＝i-ca·cb(11)

其中，i为n×n维度的单位阵。

对于cai，满足

其中，dvar为距离差阈值，为正数，dk,i为第k时刻第i个基站与被测目标之间的水平距离，hk,i为第k时刻h(xk|k-1)中第i行元素。

对于cbi，求取方式如下：

首先得出各个基站与目标之间的坐标方程：

其中，ri为第i个基站与被测目标之间的距离。

将(13)式中前n-1个方程分别与第n个方程相减，并整理得到(14)式。

并整理成(15)式形式。

其中

为被测目标的坐标列向量。

由于非视距误差的存在，会使得实际测量值大于真实值，因此有：

其中：

δri为测量距离与真实距离的差，i＝1,2,...,n,为使用最小二乘法所得的目标坐标列向量。

(18)式减(15)式，根据最小二乘法，得到

其中，δb＝b′-b＝f(r1,...,rn,δr1,...δrn)，

因此，有

其中，为根据第i个基站所测量的误差所估计出的目标坐标误差列向量。

因此，

其中，为的模，evar为误差阈值。

定义h为h的雅克比矩阵：

h′k＝chk+(i-c)hk-1(24)

卡尔曼增益如下所示：

kk＝pk|k-1h′k^t(h′kpk|k-1h′k^t+r)(25)

其中r为测量协方差矩阵，设置为对角矩阵。

状态校正式如下：

xk|k＝xk|k-1+kkc(dk-hk)(26)

pk|k＝pk|k-1-kkh′kpk|k-1(27)

其中，dk为第k时刻测量的距离向量，pk|k为校正误差协方差矩阵，xk|k即为在第k时刻的坐标最优估计列向量。

根据(26)式即可以得到在k时刻目标的坐标点，然后重复上述方法进行多次迭代即可。

上述方法可从二维环境扩展至三维环境，在三维环境下。基站可不安装在同一水平面上，且预测模型可扩展至三维环境之中，求解方法相似。