CN105824003B - 一种基于轨迹平滑的室内移动目标定位方法 - Google Patents

一种基于轨迹平滑的室内移动目标定位方法 Download PDF

Info

Publication number
CN105824003B
CN105824003B CN201410779717.0A CN201410779717A CN105824003B CN 105824003 B CN105824003 B CN 105824003B CN 201410779717 A CN201410779717 A CN 201410779717A CN 105824003 B CN105824003 B CN 105824003B
Authority
CN
China
Prior art keywords
moment
positioning result
distance
mobile target
positioning
Prior art date
Application number
CN201410779717.0A
Other languages
English (en)
Other versions
CN105824003A (zh
Inventor
夏玮玮
章跃跃
贾子彦
张瑞
朱亚萍
沈连丰
梁新建
刘世栋
缪巍巍
李炳林
Original Assignee
国家电网公司
中国电力科学研究院
全球能源互联网研究院
东南大学
江苏省电力公司信息通信分公司
国网湖北省电力公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 国家电网公司, 中国电力科学研究院, 全球能源互联网研究院, 东南大学, 江苏省电力公司信息通信分公司, 国网湖北省电力公司 filed Critical 国家电网公司
Priority to CN201410779717.0A priority Critical patent/CN105824003B/zh
Publication of CN105824003A publication Critical patent/CN105824003A/zh
Application granted granted Critical
Publication of CN105824003B publication Critical patent/CN105824003B/zh

Links

Abstract

本发明提出一种基于轨迹平滑的室内移动目标定位方法,所述方法包括(1)移动UWB节点通过轮询的方式获得与各个锚节点之间的距离;(2)将某一时刻锚节点测得距离通过无迹卡尔曼滤波器进行修正;(3)将时刻滤波后的UWB测量值采用三边测量定位法获得移动目标的位置坐标;(4)通过无迹卡尔曼滤波对定位结果进行修正;(5)采用自适应平滑算法对滤波后的定位结果进行平滑,从而到达移动目标当前时刻的位置估计值。本发明充分利用当前时刻与先前时刻的测量值和估计值,采用无迹卡尔曼滤波方式,减小了由于环境、器件等因素对测量结果的影响,提高了三边测量法的定位精度,并对定位结果进行二次滤波和自适应平滑,进一步提高定位精度。

Description

一种基于轨迹平滑的室内移动目标定位方法

技术领域

本发明涉及一种目标定位方法,具体讲涉及一种基于轨迹平滑的室内移动目标定位方法。

背景技术

如何实现对室内移动目标的高精度定位是室内定位系统研究的重点之一。现有的定位技术中,UWB技术可以实现厘米级的测距精度。UWB模块通过测量信号在空中的飞行时间(TOF,Time of Flight)来计算模块之间的距离。但是在实际应用中发现,复杂的室内环境使得电磁波在视距条件和非视距条件下传输存在较大差异,此外,当受到外界因素的影响,UWB器件会使得测距结果产生较大的偏差,直接影响到三边测量的结果。现有的方法是假设噪声为零均值的高斯噪声,对测量的结果进行卡尔曼滤波,但实际的测量噪声并不服从标准正太分布,需要使用无迹卡尔曼滤波。此外,三边测量法获得的是满足条件的最小二乘解,当某些锚节点的出现微小的偏差,其定位结果即会产生较大的偏差。因此需要提出一种滤波平滑算法,对出现较大偏差的定位结果进行修正。

发明内容

针对现有技术的不足,本发明提出一种基于轨迹平滑的室内移动目标定位方法,在超宽带(Ultra Wideband,UWB)测距中将滤波算法与自适应平滑算法相结合,通过在多个阶段分别采用无迹卡尔曼滤波,将相邻时刻定位结果之间的距离建模为与权重系数相关的函数,使得权重系数可以自适应调整,再对定位结果进行加权平均,从而实现对移动目标轨迹的平滑。对定位过程中不同阶段的测量值和估计值分别进行滤波。一方面,通过对UWB测量结果进行无迹卡尔曼滤波,减小由于环境和器件等因素引起的测量误差,提高三边测量法的精度;另一方面,通过对定位结果进行二次滤波和自适应平滑,进一步减少定位误差。

如图2所示,为本发明适用的定位场景,在定位的区域内,至少需要三个UWB锚节点和一个移动UWB节点。

通过在定位过程中不同的阶段,对UWB测量值和三边测量结果分别采用无迹卡尔曼滤波进行误差修正,其算法适用于不服从高斯分布的噪声的影响。最终结合自适应平滑算法,对移动目标的轨迹进行平滑,从而减小环境和UWB器件对测量值的影响,提高了对移动目标的定位精度。

本发明的目的是采用下述技术方案实现的:

一种基于轨迹平滑的室内移动目标定位方法,其改进之处在于,所述方法包括

(1)移动UWB节点通过轮询的方式获得与各个锚节点之间的距离;

(2)将某一时刻锚节点测得距离通过无迹卡尔曼滤波器进行修正;

(3)将时刻滤波后的UWB测量值采用三边测量定位法获得移动目标的位置坐标;

(4)通过无迹卡尔曼滤波对定位结果进行修正;

(5)采用自适应平滑算法对滤波后的定位结果进行平滑,从而到达移动目标当前时刻的位置估计值。

优选的,所述步骤(1)包括轮询的间隔可自适应调整,当移动目标快速运动时,采用较小的轮询间隔;而当移动目标慢速移动或静止时,设定较大的轮询间隔,以降低功耗。

优选的,所述步骤(1)包括通过设定合适的轮询间隔保证在同一位置上,移动目标至少获得与三个锚节点之间的距离。

优选的,所述步骤(2)包括

设n时刻,通过UWB测得的移动目标与其中一个锚节点之间距离的真实值为r(n),速度的真实值为v(n);将r(n)和v(n)作为系统的状态矢量,表示为

将系统的状态方程建模为

x(n)=F1x(n-1)+Gα (2)

其中:F1为状态转移矩阵,表示为α为加速度项,将其建模为系统噪 声,其系数矩阵为T为状态更新的时间间隔;

观测方程建模为

y(n)=H1(n)x(n)+v(n) (3)

其中:rm(n)为距离的测量值,vm(n)为速度的测量值,表示为观测矩阵v(n)为零均值单位方差的观测噪声。

优选的,所述步骤(4)包括

n时刻系统状态矢量为u(n)=[x(n),y(n),vx(n),vy(n)]T,其中:x(n),y(n))为n时刻的坐标,vx(n)和vy(n)为x轴和y轴方向上的速度;

将系统的状态方程建模为

u(n)=F2u(n-1)+w(n) (4)

其中:F2为状态转移矩阵,表示为w(n)为零均值单位方差的 系统噪声;T为状态更新的时间间隔;

观测方程建模为

z(n)=H2(n)u(n)+σ(n) (5)

其中:z(n)=[xt(n),yt(n),vxt(n),vyt(n)]T,xt(n)和yt(n)为n时刻通过三边测量 法得到的定位结果坐标,vxt(n)和vyt(n)分别为x轴和y轴方向上的速度的测量值;观测矩阵σ(n)为零均值单位方差的观测噪声。

优选的,所述步骤(5)包括

设n时刻对三边测量的定位结果进行滤波后,得到移动目标的位置为sr(n),其坐标为(xr(n),yr(n)),则n-1时刻滤波后移动目标的定位结果为sr(n-1);n时刻平滑后的移动目标的定位结果为坐标表示为(xest(n),yest(n)),则n-1时刻平滑后的移动目标的定位结果为

定义相邻两个时刻三边测量结果的距离Dr(n)为

设n时刻平滑后的定位结果为n-1时刻平滑后的定位结果和n时刻滤波后的定位结果的加权平均,定义为

其中ρ应满足

进一步地,通过自适应平滑算法对滤波后的定位结果进行轨迹平滑,将当前时刻的定位估计值与前一时刻的定位估计值进行比较,当两者的距离越大,则当前时刻的定位估计值的权重越小;而当两者的距离较小,则当前时刻定位估计值的权重越大,通过不同时刻的权重对相邻时刻的定位估计值进行加权平均,从而实现自适应的过程。

与现有技术比,本发明的有益效果为:

本发明充分利用当前时刻与先前时刻的测量值和估计值,采用无迹卡尔曼滤波方式,减小了由于环境、器件等因素对测量结果的影响,提高了三边测量法的定位精度,并对定位结果进行二次滤波和自适应平滑,进一步提高定位精度。

附图说明

图1为本发明提供的一种基于轨迹平滑的室内移动目标定位方法流程图。

图2为本发明提供的定位场景示意图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步的详细说明。

如图1所示,为本发明提出的一种基于轨迹平滑的室内移动目标定位方法,该方法使用多阶段无迹卡尔曼滤波和自适应平滑算法,包括如下步骤:

(1)移动UWB节点通过轮询的方式获得与各个锚节点之间的距离;

轮询的间隔可以自适应调整,当移动目标快速运动时,采用较小的轮询间隔;而当移动目标慢速移动或静止时,设定较大的轮询间隔,以降低功耗;

(2)将某一时刻所有锚节点测得距离通过无迹卡尔曼滤波器进行修正;

其中,对UWB测量值进行无迹卡尔曼滤波具体为:

假设n时刻,通过UWB测得的移动目标与其中一个锚节点之间距离的真实值为r(n),速度的真实值为v(n)。将r(n)和v(n)作为系统的状态矢量,表示为

将系统的状态方程建模为

x(n)=F1x(n-1)+Gα (2)

其中:F1为状态转移矩阵,表示为α为加速度项,将其建模为系统噪 声,其系数矩阵为T为状态更新的时间间隔。

观测方程建模为

y(n)=H1(n)x(n)+v(n) (3)

其中:rm(n)为距离的测量值,vm(n)为速度的测量值,表示为观测矩阵v(n)为零均值单位方差的观测噪声。

在实际的测量过程中,对测得的与各个锚节点之间的距离分别进行无迹卡尔曼滤波的修正,从而提高各个锚节点测量的精度。

(3)将(2)中该时刻滤波后的UWB测量值采用三边测量定位法,获得移动目标的位置坐标;

(4)通过无迹卡尔曼滤波对(3)中的定位结果进行修正;

其中,对三边测量法的定位结果进行滤波具体为:

n时刻系统状态矢量为u(n)=[x(n),y(n),vx(n),vy(n)]T,其中:(x(n),y(n)为n时刻的坐标,vx(n)和vy(n)为x轴和y轴方向上的速度。

将系统的状态方程建模为

u(n)=F2u(n-1)+w(n) (4)

其中:F2为状态转移矩阵,表示为w(n)为零均值单位方差的 系统噪声;T为状态更新的时间间隔。

观测方程建模为

z(n)=H2(n)u(n)+σ(n) (5)

其中:z(n)=[xt(n),yt(n),vxt(n),vyt(n)]T,xt(n)和yt(n)为n时刻通过三边测量 法得到的定位结果坐标,vxt(n)和vyt(n)分别为x轴和y轴方向上的速度的测量值;观测矩阵σ(n)为零均值单位方差的观测噪声。

(5)采用自适应平滑算法对(4)中滤波后的定位结果进行平滑,从而到达移动目标当前时刻的位置估计值。

其中,自适应平滑过程具体为:

假设n时刻对三边测量的定位结果进行滤波后,得到移动目标的位置为sr(n),其坐标为(xr(n),yr(n)),则n-1时刻滤波后移动目标的定位结果为sr(n-1)。n时刻平滑后的移动目标的定位结果为坐标表示为(xest(n),yest(n)),则n-1时刻平滑后的移动目标的定位结果为

定义相邻两个时刻三边测量结果的距离Dr(n)为

设n时刻平滑后的定位结果为n-1时刻平滑后的定位结果和n时刻滤波后的定位结果的加权平均,定义为

其中ρ应满足

由此可见,将当前时刻的定位估计值与前一时刻的定位估计值进行比较,当两者的距离越大,则当前时刻的定位估计值的权重越小;而当两者的距离较小,则当前时刻定位估计值的权重越大,通过不同时刻的权重对相邻时刻的定位估计值进行加权平均,从而实现自适应的过程。

本发明通过设定合适的轮询间隔保证在同一位置上,移动目标至少获得与三个锚节点之间的距离。

本发明对移动目标接收的与各个锚节点之间距离进行无迹卡尔曼滤波,再将滤波后的距离测量值采用三边测量法进行定位。对三边测量法的定位结果再次利用无迹卡尔曼滤波,从而进一步提高定位的精度。

最后应当说明的是:以上实施例仅用以说明本发明的技术方案而非对其限制,所属领域的普通技术人员参照上述实施例依然可以对本发明的具体实施方式进行修改或者等同替换,这些未脱离本发明精神和范围的任何修改或者等同替换,均在申请待批的本发明的权利要求保护范围之内。

Claims (4)

1.一种基于轨迹平滑的室内移动目标定位方法,其特征在于,所述方法包括
(1)移动UWB节点通过轮询的方式获得与各个锚节点之间的距离;
(2)将某一时刻锚节点测得距离通过无迹卡尔曼滤波器进行修正;
(3)将所述时刻滤波后的UWB测量值采用三边测量定位法获得移动目标的位置坐标;
(4)通过无迹卡尔曼滤波对定位结果进行修正;
(5)采用自适应平滑算法对滤波后的定位结果进行平滑,从而得到 移动目标当前时刻的位置的估计值;
所述步骤(1)包括轮询的间隔可自适应调整,当移动目标快速运动时,采用较小的轮询间隔;而当移动目标慢速移动或静止时,设定较大的轮询间隔,以降低功耗。
所述步骤(1)包括通过设定合适的轮询间隔保证在同一位置上,移动目标至少获得与三个锚节点之间的距离;
所述步骤(5)包括
设n时刻对三边测量的定位结果进行滤波后,得到移动目标的位置为sr(n),其坐标为(xr(n),yr(n)),则n-1时刻滤波后移动目标的定位结果为sr(n-1);n时刻平滑后的移动目标的定位结果为坐标表示为(xest(n),yest(n)),则n-1时刻平滑后的移动目标的定位结果为
定义相邻两个时刻三边测量结果的距离Dr(n)为
设n时刻平滑后的定位结果为n-1时刻平滑后的定位结果和n时刻滤波后的定位结果的加权平均,定义为
其中ρ应满足
2.如权利要求1所述的一种基于轨迹平滑的室内移动目标定位方法,其特征在于,所述步骤(2)包括
设n时刻,通过UWB测得的移动目标与其中一个锚节点之间距离的真实值为r(n),速度的真实值为v(n);将r(n)和v(n)作为系统的状态矢量,表示为
将系统的状态方程建模为
x(n)=F1x(n-1)+Gα (2)
其中:F1为状态转移矩阵,表示为α为加速度项,将其建模为系统噪声,其系数矩阵为T为状态更新的时间间隔;
观测方程建模为
y(n)=H1(n)x(n)+v(n) (3)
其中:rm(n)为距离的测量值,vm(n)为速度的测量值,表示为观测矩阵v(n)为零均值单位方差的观测噪声。
3.如权利要求1所述的一种基于轨迹平滑的室内移动目标定位方法,其特征在于,所述步骤(4)包括
n时刻系统状态矢量为u(n)=[x(n),y(n),vx(n),vy(n)]T,其中:(x(n),y(n))为n时刻的坐标,vx(n)和vy(n)为x轴和y轴方向上的速度;
将系统的状态方程建模为
u(n)=F2u(n-1)+w(n) (4)
其中:F2为状态转移矩阵,表示为w(n)为零均值单位方差的系统噪声;T为状态更新的时间间隔;
观测方程建模为
z(n)=H2(n)u(n)+σ(n) (5)
其中:z(n)=[xt(n),yt(n),vxt(n),vyt(n)]T,xt(n)和yt(n)为n时刻通过三边测量法得到的定位结果坐标,vxt(n)和vyt(n)分别为x轴和y轴方向上的速度的测量值;观测矩阵σ(n)为零均值单位方差的观测噪声。
4.如权利要求1所述的一种基于轨迹平滑的室内移动目标定位方法,其特征在于,通过自适应平滑算法对滤波后的定位结果进行轨迹平滑,将当前时刻的定位估计值与前一时刻的定位估计值进行比较,当两者的距离越大,则当前时刻的定位估计值的权重越小;而当两者的距离较小,则当前时刻定位估计值的权重越大,通过不同时刻的权重对相邻时刻的定位估计值进行加权平均,从而实现自适应的过程。
CN201410779717.0A 2014-12-16 2014-12-16 一种基于轨迹平滑的室内移动目标定位方法 CN105824003B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201410779717.0A CN105824003B (zh) 2014-12-16 2014-12-16 一种基于轨迹平滑的室内移动目标定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201410779717.0A CN105824003B (zh) 2014-12-16 2014-12-16 一种基于轨迹平滑的室内移动目标定位方法

Publications (2)

Publication Number Publication Date
CN105824003A CN105824003A (zh) 2016-08-03
CN105824003B true CN105824003B (zh) 2019-02-22

Family

ID=56986762

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201410779717.0A CN105824003B (zh) 2014-12-16 2014-12-16 一种基于轨迹平滑的室内移动目标定位方法

Country Status (1)

Country Link
CN (1) CN105824003B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106710293A (zh) * 2016-10-31 2017-05-24 中原智慧城市设计研究院有限公司 地下停车场车辆动态智能引导方法
CN106793060A (zh) * 2017-03-08 2017-05-31 哈尔滨工程大学 一种超宽带室内定位方法
CN106908054A (zh) * 2017-03-14 2017-06-30 深圳蓝因机器人科技有限公司 一种基于超宽频信号的定位寻径方法和装置

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103152695A (zh) * 2013-02-04 2013-06-12 太原理工大学 基于td-scdma系统的井下人员精确定位方法
CN103491627A (zh) * 2013-08-03 2014-01-01 东北大学 一种集成多种算法的近距离实时精确定位方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8175613B2 (en) * 2006-08-04 2012-05-08 Misonimo Chi Acquisitions L.L.C. Systems and methods for determining location of devices within a wireless network
US7239277B2 (en) * 2004-04-12 2007-07-03 Time Domain Corporation Method and system for extensible position location
CN101483805A (zh) * 2009-02-11 2009-07-15 江苏大学 一种视距和非视距混合环境下的无线定位方法
CN103152826A (zh) * 2013-03-08 2013-06-12 天津大学 一种基于nlos状态检测补偿的移动目标追踪方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103152695A (zh) * 2013-02-04 2013-06-12 太原理工大学 基于td-scdma系统的井下人员精确定位方法
CN103491627A (zh) * 2013-08-03 2014-01-01 东北大学 一种集成多种算法的近距离实时精确定位方法

Also Published As

Publication number Publication date
CN105824003A (zh) 2016-08-03

Similar Documents

Publication Publication Date Title
Wang et al. Position accuracy of time-of-arrival based ranging using visible light with application in indoor localization systems
Zhan et al. Iterated unscented Kalman filter for passive target tracking
Chang et al. Spinning beacons for precise indoor localization
CN102158956B (zh) 一种在无线传感器网络中基于rssi的改进加权三边定位方法
CN103148848A (zh) 用于基于磁场地图的定位系统的移动终端及其方法
TWI447420B (zh) 室內定位方法與系統,及電腦程式產品
Wann et al. Hybrid TDOA/AOA indoor positioning and tracking using extended Kalman filters
De Angelis et al. Indoor positioning by ultrawide band radio aided inertial navigation
WO2010106530A3 (en) A location and tracking system
Gharghan et al. Accurate wireless sensor localization technique based on hybrid PSO-ANN algorithm for indoor and outdoor track cycling
Li et al. Adapting sample size in particle filters through KLD-resampling
CN104849740B (zh) 集成卫星导航与蓝牙技术的室内外无缝定位系统及其方法
Shchekotov Indoor localization method based on Wi-Fi trilateration technique
Ganti et al. VLC-based indoor positioning system with tracking capability using Kalman and particle filters
CN103197279A (zh) 一种移动目标协同定位系统及定位方法
Wang et al. The research of indoor positioning based on visible light communication
WO2016070796A1 (zh) 一种获取目标位置信息的方法及装置
Wang et al. High-precision RSSI-based indoor localization using a transmission power adjustment strategy for wireless sensor networks
CN103124396A (zh) 基于交叉粒子群的无线传感器网络移动节点定位方法
Lim Ubiquitous 3D positioning systems by led-based visible light communications
EP2870490B1 (en) Improved trilateration processing
US20140249771A1 (en) Location estimation using a mobile device
WO2014164106A1 (en) Differential ultra-wideband indoor positioning method
TW201300813A (zh) 電子裝置、定位方法與系統、電腦程式產品與記錄媒體
CN110276786A (zh) 确定跟踪目标的位置信息的方法及装置、跟踪装置及系统

Legal Events

Date Code Title Description
C06 Publication
C10 Entry into substantive examination
GR01