CN104297724A - 定位方法及系统 - Google Patents

定位方法及系统 Download PDF

Info

Publication number
CN104297724A
CN104297724A CN201410544898.9A CN201410544898A CN104297724A CN 104297724 A CN104297724 A CN 104297724A CN 201410544898 A CN201410544898 A CN 201410544898A CN 104297724 A CN104297724 A CN 104297724A
Authority
CN
China
Prior art keywords
lambda
positioning
destination node
signal
denoising
Prior art date
Legal status (The legal status 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 status listed.)
Pending
Application number
CN201410544898.9A
Other languages
English (en)
Inventor
熊丹
丁宁
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
SHENZHEN KESONG ELECTRONIC CO Ltd
Original Assignee
SHENZHEN KESONG ELECTRONIC CO Ltd
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 SHENZHEN KESONG ELECTRONIC CO Ltd filed Critical SHENZHEN KESONG ELECTRONIC CO Ltd
Priority to CN201410544898.9A priority Critical patent/CN104297724A/zh
Publication of CN104297724A publication Critical patent/CN104297724A/zh
Pending legal-status Critical Current

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/06Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0294Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开一种定位方法,用于将一个可移动本体定位到一目标节点。所述方法包括:接收该目标节点的多个定位信号;将接收到的定位信号进行小波去噪处理;根据去噪处理后的定位信号的频率与相位差,测量N组距离差;根据所测量的N组距离差进行至少两次加权最小二乘法,计算出预估的坐标,并将所述预估的坐标作为初始迭代位置进行多次迭代收敛,计算出目标节点位置;以及控制可移动本体定位到所述目标节点位置。本发明还提供一种定位系统。

Description

定位方法及系统
技术领域
本发明涉及一种定位技术,具体涉及一种基于小波去噪的定位方法及相关系统。
背景技术
随着社会经济的发展,超级商场、机场、车站、会展中心及物流仓库等大型场所的规模和数量不断增加,大中城市的高层、高档商用楼日益增多,其保安自动化需求日趋迫切。巡逻机器人是一个集环境感知、路线规划、动态决策、行为控制以及报警装置于一体的多功能综合系统,采用巡逻机器人实行定时、定点监控巡逻或不间断流动巡逻是目前一种可行的解决方案。将机器人用于保安工作,具有广阔的应用前景,今年来已受到国内外的重视,成为服务机器人的一个新研究方向。
巡逻机器人中使用的定位技术是一种应用广泛的高科技技术,它在移动互联网、物联网、导航技术应用非常广泛,是现今研究的热点技术之一。目前用得最多的是GPS、蜂窝基站、Zigbee、Wifi、UWB等。巡逻机器人定位采用最多的是GPS算法,但由于GPS对时钟要求极高且信号经大气层传播,容易受天气影响,而在室内会受电磁屏蔽效应,往往难以收到GPS信号,因此GPS定位误差常为10m左右。而蜂窝基站定位的优势是定位速度快,但其定位精度不高。多基站定位常用的为TOA及TDOA(到达时间及到达时间差),基于CC2431/Zigbee的定位技术在理想环境下最高精度能达到0.5m,但是在实际应用中定位精度不高。根据巡逻机器人的实际需要,定位精度通常需达到0.1m,而现有的技术中,仅UWB的精度较高。然而,UWB成本太高,无法满足企业要求。
发明内容
为了解决上述的技术问题,本发明提供一种定位方法和相关系统。
在本发明的一个实施例中,提供一种定位方法,用于将一个可移动本体定位到一目标节点。所述方法包括:接收该目标节点的多个定位信号;将接收到的定位信号进行小波去噪处理;根据去噪处理后的定位信号的频率与相位差,测量N组距离差;根据所测量的N组距离差进行至少两次加权最小二乘法,计算出预估的坐标,并将所述预估的坐标作为初始迭代位置进行多次迭代收敛,计算出目标节点位置;以及控制可移动本体定位到所述目标节点位置。
优选的,所述将接收到的定位信号进行小波去噪处理的步骤包括:对所接收的多个定位信号进行预处理;对含噪声的所述定位信号进行正交小波变换,并分解到多个不同频带上,获得一组小波系数;对所述小波系数进行自适应阈值处理,获得估计系数,并使得所述小波系数与所述估计系数的差值最小;以及根据所述估计系数进行小波重构,从而获得去噪处理后的定位信号。
优选的,所述测量N组距离差的步骤包括:对所述去噪处理后的定位信号进行内插还原,并进行解调,得到基带信号频率;计算所述去噪处理后的定位信号的传送/到达时间差;根据所述时间差测量出N组距离差。
优选的,所述根据所述时间差测量N组距离差的步骤包括:根据公式:
测量所述距离差,其中,λ为信号波长,为发射信号与反射信号的相位差,c为光速,其中Δd∈[0,λ]。
优选的,所述计算出预估的坐标的步骤包括:根据目标节点的初始位置,利用坐标点与原点距离计算公式以及时间差,计算第一次获得的目标节点的估计位置;以及根据第一次获得的估计位置及一个已知的附加变量,构造一组新的误差方程组,进行第二次加权最小二乘法估计;根据所述第二次加权最小二乘法的误差估计结果,以及可移动本体的实际位置坐标获得所述预估的坐标。
优选的,所述计算目标节点位置的步骤包括:
根据公式:η=ht-gtδ进行计算,其中 δ = Δx Δy , 表示位置估计误差, h t = r 2,1 - ( r Λ 2 - r Λ 1 ) r 3,1 - ( r Λ 3 - r Λ 1 ) . . . r m - 1 - ( r Λ m - r Λ 1 ) , 表示距离差真实值与测量值之间的差值,
g t = ∂ Δr 1 ∂ x | ( x Λ , y Λ ) ∂ Δ r 1 ∂ y | ( x Λ , y Λ ) ∂ Δ r 2 ∂ x | ( x Λ , y Λ ) ∂ Δ r 2 ∂ y | ( x Λ , y Λ ) . . . . . . ∂ Δ r m ∂ x | ( x Λ , y Λ ) ∂ Δ r m ∂ y | ( x Λ , y Λ ) = x 1 - x Λ r Λ 1 - x 2 - x Λ r Λ 2 y 1 - y Λ r Λ 1 - y 2 - y Λ r Λ 2 x 1 - x Λ r Λ 1 - x 3 - x Λ r Λ 3 y 1 - y Λ r Λ 1 - y 3 - y Λ r Λ 3 . . . . . . x 1 - x Λ r Λ 1 - x m - x Λ r Λ m y 1 - x Λ r Λ 1 - y m - y Λ r Λ m , r Λ i = ( x i - x Λ ) 2 + ( y i - y Λ ) 2 , 其中为估计的目标初始迭代位置与预定的多个参考节点之间的距离。
优选的,所述接收该目标节点的多个定位信号的步骤之前还包括:根据一测距请求发送定位测距信号请求;以及根据所述定位测距信号请求,通过无线的方法通知所述目标节点发送所述定位信号。
在本发明的另一个实施例中,提供一种定位系统,包括至少一个后台服务器、至少一个可移动本体与至少三个基站,所述可移动本体根据所述后台服务器的控制命令运动至一目标节点。所述基站用于接收所述目标节点的多个定位信号,并将所述目标节点的多个定位信号传输至所述后台服务器;所述后台服务器包括:接收模块,用于接收所述基站传输的所述目标节点的多个定位信号;去噪模块,用于将接收到的定位信号进行小波去噪处理;测量模块,用于根据去噪处理后的定位信号的频率与相位差,测量N组距离差;计算模块,用于根据所测量的N组距离差进行至少两次加权最小二乘法,计算出预估的坐标,并将所述预估的坐标作为初始迭代位置进行多次迭代收敛,计算出目标节点位置;以及控制模块,用于将所述目标节点位置传送给所述可移动本体,以控制可移动本体定位到所述目标节点位置。
优选的,所述去噪模块包括:预处理单元,用于对所接收的多个定位信号进行预处理;分解单元,用于对含噪声的所述定位信号进行正交小波变换,并分解到多个不同频带上,获得一组小波系数;系数去噪单元,用于对所述小波系数进行自适应阈值处理,获得估计系数,并使得所述小波系数与所述估计系数的差值最小;以及信号重构单元,用于根据所述估计系数进行小波重构,从而获得去噪处理后的定位信号。
优选的,所述接收模块、所述去噪模块与所述测量模块整合在一FPGA中,所述可移动本体包括一定位标签,所述定位标签用于接收和发射定位信号。
本发明的巡逻机器人定位系统和方法中,先对收集到的定位信号进行小波去噪处理,再计算得出N组距离差,并通过两次加权最小二乘法及多次迭代收敛得出最终目标节点位置坐标。该方法有效解决了TDOA(到达时间差)的同步问题,可提高定位精度,在预埋的轨道进行巡逻能达到0.1m定位需求。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1是采用本发明的定位系统的节点布置图。
图2是采用本发明的定位系统的示意图。
图3是本发明提供的定位方法的流程图。
图4是本发明提供的定位系统的功能模块图。
具体实施方式
下面结合附图和具体实施方式对本发明的技术方案作进一步更详细的描述。显然,所描述的实施例仅仅是本发明的一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例,都应属于本发明保护的范围。
请参阅图1,其为采用本发明的定位系统的节点布置图。其中,T为目标节点,M为中心锚节点(参考节点)。所述目标节点的定位信号通过中心锚节点获取,中心锚节点通过N根基站的天线接收目标节点的定位信号。更具体的是,其中一根天线O位于中心锚节点处,其他天线则通过一定长度的同轴电缆对称引出,使得目标节点被所述N根天线包围。由于该N根天线的位置为已知,因此可以据此求出T节点位置。请结合图1与图2,本发明提供一种基于小波去噪相位差的定位系统,该定位系统执行一定位方法。本实施例中,以巡逻机器人的定位系统定位方法对本发明进行详细描述。从总体上来说,所述巡逻机器人定位方法包括步骤:首先,在巡逻机器人循迹范围内布置移动节点,巡逻机器人后台服务器(后续将以模块进一步说明)中的FPGA(Field-Programmable Gate Array,即现场可编程门阵列,是一种通信模块,通常由触发器和查找表组成)获取移动节点的定位信号。FPGA收到定位信号后,开始对定位信号进行小波去噪处理。然后,巡逻机器人的后台服务器计算去噪重构后的信号基带频率,以及计算发射信号与反射信号的相位差,再通过多线路到达信号相位差,测得N组距离差给后台服务器中的运算服务器,并通知基站使目标节点T(其具有定位标签)停止发送定位信号。再者,巡逻机器人的后台服务器进行至少两次加权最小二乘,多次迭代收敛处理后计算得出目标节点T的位置,最后将最终坐标传递给巡逻机器人本体。经过模拟仿真,可知通过该方法可在预埋的轨迹进行巡逻能达到0.1m的精度,相比于现有技术,大大提高了定位精度。
更具体的,请参阅图3,本发明提供的巡逻机器人定位方法包括步骤:
步骤S301,接收该目标节点T的多个定位信号。具体的,机器人后台服务器收到定位指令时,将测距请求发送给基站,基站通过无线的方式通知目标节点T发送定位信号,目标节点T发送定位信号并返回确认。本实施例中,通过所述目标节点T上的定位标签发送定位信号,并通过上述N根天线同步接收所述定位信号。
步骤S303,将接收到的定位信号进行小波去噪处理。由于定位容易受周边环境的影响,真实环境下收集的信号都含有噪声,特别在室外,会严重影响定位精度,因此对原始信号进行去噪处理是非常有必要的。具体实现方法包括步骤:
S303a,对所接收的多个定位信号进行预处理。具体的,一个经预处理后的含噪声的一维信号的模型可表示为:
S(k)=f(k)+δe(k)    (1)
其中:f(k)为真实信号,e(k)为噪声信号,δ为噪声强度,S(k)为含噪声的信号。
S303b,对含噪声的所述定位信号进行正交小波变换,并分解到多个不同频带上,获得一组小波系数。本实施例中,对含噪声的信号S(k)进行正交小波变换,分解到若干不同频带上,获得一组小波系数Wj,k
S303c,对所述小波系数进行自适应阈值处理,获得估计系数,并使得所述小波系数与所述估计系数的差值最小。本实施例中,通过对Wj,k进行自适应阈值处理,获得估计系数使得与Wj,k两者的差值尽可能小。
S303d,根据所述估计系数进行小波重构,从而获得去噪信号。本实施例中,利用进行小波重构,获得估计信号S(k)即为去噪后的信号。
由于低频子带有较高的幅度值,较低频率回波脉冲保留了较详细的轮廓;而高频子带有相对较低的幅度值,只保留回波脉冲的高频细节部分,根据以上特征,上述小波阈值去噪方法,可滤除幅度较低的高频杂波和噪声,使得小波重构后的信号更接近真实信号。
步骤S305,根据去噪处理后的定位信号的频率与相位差,测量N组距离差。
具体的,FPGA负责信号处理及测距计算。本实施例中,在FPGA中,对多路射频前端传送过来的去噪重构后的数字信号进行内插还原,再进行解调,得出基带信号频率,并计算该些信号的传送/到达时间差。根据所述时间差,测量出N组距离差,并将该N组距离差的值传送给后台服务器,以进行后续的定位算法计算。
更具体的,设多路去噪重构信号的相位差为,fc为待定位节点(如目标节点T)的发射基带频率,则目标节点T到具有多线路的所述参考节点M的到达时间差为从而目标节点T到多线路参考节点M的距离差为:
其中,式中λ为信号波长,为发射信号与反射信号的相位差,c为光速,其中Δd∈[0,λ]。
步骤S307,根据所测量的N组距离差进行至少两次加权最小二乘法,计算出预估的坐标,并将所述预估的坐标作为初始迭代位置进行多次迭代收敛,计算出目标节点位置。本实施例中,进行两次加权最小二乘法。具体如下:将目标节点T的初始位置坐标设为(x0,y0),其中由经典距离公式:
r i 2 = ( x i - x 0 ) 2 + ( y i - y 0 ) 2 = x i 2 + y i 2 - 2 x i x 0 - 2 x i x 0 - 2 y i y 0 + x 0 2 + y 0 2 = k i - 2 x i x 0 - 2 y i y 0 + x 0 2 + y 0 2 - - - ( 3 ) ,
以及根据TDOA(步骤S305中的到达时间差)测量值ti,1可得:
ri,1=c×Δt=cti,1=ri-r1    (4),其中,c为光速,ri,1表示目标节点和传感器i与传感器1的距离差。
式(4)进行平方后,将(3)式代入,得到:
r i , 1 2 + 2 r i , 1 r 1 = r i 2 - r 1 2 = k i - 2 x i , 1 - 2 y i , 1 y 0 - k 1 - - - ( 5 ) ,
其中:xi,1=xi-x1,yi,1=yi-y1,令za=(x0,y0,r1)T为未知矢量,则(5)式可建立以za为变量的方程组:
h=gaza    (6),
则巡逻机器人本体的估计位置所对应的误差矢量为:
ψ = h - g a z a 0 - - - ( 7 ) ,
其中, z a 0 = x y r 1 0 为巡逻机器人本体的实际位置坐标, h = 1 2 r 2,1 2 - k 2 + k 1 . . . r m 2 - k m + k 1 , g a = - x 2,1 y 2,1 r 2,1 . . . . . . . . . x m , 1 y m , 1 r m , 1 .
根据(7)式,(6)式可转换为如下:
1 2 r 2,1 2 - k 2 + k 1 . . . r m 2 - k m + k 1 = - x 2,1 y 2,1 r 2,1 . . . . . . . . . x m , 1 y m , 1 r m , 1 x 0 y 0 r 1 - - - ( 8 ) ,
根据加权最小二乘法,用TDOA测量值的协方差矩阵Q近似代替误差ψ的协方差矩阵:
Q = E ( nn T ) = 1 0 . . . 0 0 1 . . . 0 . . . . . . . . . . . . 0 0 0 1 , 其中, n = n 2,1 n 3,1 . . . n m , 1 , 其是以距离为单位的TDOA测量误差,近似服从正态分布,由(8)式可获得:
za=(x0,y0,r1)T=arg min{(h-gaza)TQ-1(h-gaza)}=(ga TQ-1ga)-1(ga TQ-1h)    (9)。
在误差方程(7)中,由于za中的r1实际上是与(x0,y0)有关的量,用Q矩阵近似代替误差矢量ψ的协方差矩阵也会带来一定的误差。为了更精确估计位置,可根据第一次获得的估计位置及附加变量r1等已知的约束条件,构造一组新的误差方程组,进行第二次加权最小二乘估计,便获得改进的巡逻机器人本体的估计位置。
具体步骤如下:
式(7)中由TDOA测量值造成的误差ψ的协方差矩阵:
ψ=E[ψψT]=BQB    (10),
其中B≈ri 0I,ο代表Schor乘积,ri 0是巡逻机器人本体和第i个参考节点的实际距离。
可将测量获得的ri代替ri 0,则za的第二次加权最小二乘法估计值为:
za=(ga Tψ-1ga)-1(ga Tψ-1h)    (11),
利用za值可获得新的矩阵:
z a ′ ≈ ( g a ′ T ψ - 1 g a ′ ) - 1 ( g a ′ T ψ - 1 ) h ′ = ( g a ′ T B ′ - 1 g a T Q - 1 g a B ′ - 1 g a ′ ) - 1 ( g a ′ T B ′ - 1 g a T Q - 1 g a B ′ - 1 ) h ′ - - - ( 12 ) ,
其中, h ′ = ( z a , 1 - x 1 ) 2 ( z a , 2 - y 1 ) 2 z a , 3 2 , g a ′ = 1 0 0 1 1 1 , z a ′ = ( x - x 1 ) 2 ( y - y 1 ) 2 , B ′ = z a ( 1 ) - x 1 0 0 0 z a ( 2 ) - y 1 0 0 0 z a ( 3 )
则巡逻机器人本体的定位计算结果为:
z p ′ = ± z a ′ + x 1 y 1 - - - ( 13 ) ,
为了更进一步提高定位精度,将(13)式获得的定位结果估计位置作为初始迭代,每一步迭代方向都是沿着当前点函数值下降的方向。巡逻机器人本体的定位模型转化为:
η=ht-gtδ    (14),其中 δ = Δx Δy 表示位置估计误差, h t = r 2,1 - ( r Λ 2 - r Λ 1 ) r 3,1 - ( r Λ 3 - r Λ 1 ) . . . r m - 1 - ( r Λ m - r Λ 1 ) , 表示距离差真实值与测量值之间的差值, g t = ∂ Δr 1 ∂ x | ( x Λ , y Λ ) ∂ Δ r 1 ∂ y | ( x Λ , y Λ ) ∂ Δ r 2 ∂ x | ( x Λ , y Λ ) ∂ Δ r 2 ∂ y | ( x Λ , y Λ ) . . . . . . ∂ Δ r m ∂ x | ( x Λ , y Λ ) ∂ Δ r m ∂ y | ( x Λ , y Λ ) = x 1 - x Λ r Λ 1 - x 2 - x Λ r Λ 2 y 1 - y Λ r Λ 1 - y 2 - y Λ r Λ 2 x 1 - x Λ r Λ 1 - x 3 - x Λ r Λ 3 y 1 - y Λ r Λ 1 - y 3 - y Λ r Λ 3 . . . . . . x 1 - x Λ r Λ 1 - x m - x Λ r Λ m y 1 - x Λ r Λ 1 - y m - y Λ r Λ m ,
为估计的目标初始迭代位置与各参考节点之间的距离。
式(14)利用加权最小二乘法可求解出巡逻机器人本体的坐标偏差:
δ = Δx Δy = ( g t T Q - 1 g t ) - 1 ( g t T Q - 1 h t ) - - - ( 15 ) ,
式中Q为参考节点M之间的时延估计值的协方差矩阵。
作为下一次迭代的初始值,重复迭代过程,直至Δx,Δy足够小,能满足所设定的阈值μ,其中|Δx|+|Δy|≤μ之后输出的估计值(x′,y′)就能获得巡逻机器人本体的位置坐标。
步骤S309,控制巡逻机器人本体20定位到所述目标节点位置,也就是步骤S307中的位置坐标。
本发明的定位方法首先在FPGA中对收集到的定位信号进行小波去噪处理,再通过多线路到达信号相位差得出N组距离差,通过两次加权最小二乘法及多次迭代收敛得出最终目标节点位置坐标。该方法有效解决了TDOA(到达时间差)的同步问题,可提高定位精度,在预埋的轨道进行巡逻能达到0.1m定位需求。
请参阅图4,本发明还提供一种巡逻机器人定位系统100,其包括至少一个后台服务器10、至少一个所述巡逻机器人本体20以及至少3个基站。其中,所述巡逻机器人本体20包含定位标签。所述后台服务器10通过所述至少3个基站接收所述巡逻机器人本体20的定位标签发送的信号,以及接收一目标节点T的多个定位信号,并将该目标节点T的多个定位信号传输至后台服务器10。本实施例中,所述后台服务器10通过所述多根同步天线接收所述多个定位信号。所述巡逻机器人本体20通过所述至少3个基站接收所述后台服务器10返回的定位信息,并根据所述后台服务器10的定位控制命令,在预定的多个移动节点运动。
更具体的,所述后台服务器10包括接收模块11、去噪模块13、测量模块15、计算模块17以及控制模块19。
所述接收模块11用于接收一目标节点T的多个定位信号。具体的,当后台服务器10收到定位指令时,将测距请求发送给FPGA,FPGA发送定位测距信号请求给基站(请参阅图4),基站通过无线的方式通知目标节点T发送定位信号,目标节点T发送定位信号并返回确认。所述目标节点T发送的定位信号通过所述N根天线接收,并到达所述后台服务器10。同时,基站给FPGA返回信号发送确认。本实施例中,所述FPGA通过USB接口与后台服务器10连接。
由于定位容易受周边环境的影响,真实环境下收集的信号都含有噪声,特别在室外,会严重影响定位精度,因此对原始信号进行去噪处理是非常有必要的。所述去噪模块13用于将接收到的定位信号进行小波去噪处理。具体的,所述去噪模块13包括预处理单元、分解单元、系数去噪单元以及信号重构单元。所述去噪模块13的具体实现方法包括步骤:
S303a,预处理单元对所接收的多个定位信号进行预处理。具体的,一个经预处理后的含噪声的一维信号的模型可表示为:
S(k)=f(k)+δe(k)    (1)
其中:f(k)为真实信号,e(k)为噪声信号,δ为噪声强度,S(k)为含噪声的信号。
S303b,分解单元对含噪声的所述定位信号进行正交小波变换,并分解到多个不同频带上,获得一组小波系数。本实施例中,对含噪声的信号S(k)进行正交小波变换,分解到若干不同频带上,获得一组小波系数Wj,k
S303c,系数去噪单元对所述小波系数进行自适应阈值处理,获得估计系数,并使得所述小波系数与所述估计系数的差值最小。本实施例中,通过对Wj,k进行自适应阈值处理,获得估计系数使得与Wj,k两者的差值尽可能小。
S303d,信号重构单元根据所述估计系数进行小波重构,从而获得去噪信号。本实施例中,利用进行小波重构,获得估计信号S(k)即为去噪后的信号。
由于低频子带有较高的幅度值,较低频率回波脉冲保留了较详细的轮廓;而高频子带有相对较低的幅度值,只保留回波脉冲的高频细节部分,根据以上特征,上述小波阈值去噪方法,可滤除幅度较低的高频杂波和噪声,使得小波重构后的信号更接近真实信号。
所述测量模块15用于根据去噪处理后的定位信号的频率与相位差,测量N组距离差。具体的,由所述FPGA负责信号处理及测距计算。本实施例中,在FPGA中,对多路射频前端传送过来的去噪重构后的数字信号进行内插还原,再进行解调,得出基带信号频率,并计算该些信号的传送/到达时间差。根据所述时间差,测量出N组距离差,并将该N组距离差的值传送给后台服务器,以进行后续的定位算法计算。
更具体的,设多路去噪重构信号的相位差为,fc为待定位节点(如目标节点T)的发射基带频率,则目标节点T到具有多线路的所述参考节点M的到达时间差为从而目标节点T到多线路参考节点M的距离差为:
其中,式中λ为信号波长,为发射信号与反射信号的相位差,c为光速,其中Δd∈[0,λ]。
所述计算模块17用于根据所测量的N组距离差进行至少两次加权最小二乘法,计算出预估的坐标,并将所述预估的坐标作为初始迭代位置进行多次迭代收敛,计算出目标节点位置。本实施例中,进行两次加权最小二乘法。
具体如下:将目标节点T的初始位置坐标设为(x0,y0),其中由经典距离公式:
r i 2 = ( x i - x 0 ) 2 + ( y i - y 0 ) 2 = x i 2 + y i 2 - 2 x i x 0 - 2 x i x 0 - 2 y i y 0 + x 0 2 + y 0 2 = k i - 2 x i x 0 - 2 y i y 0 + x 0 2 + y 0 2 - - - ( 3 ) ,
以及根据TDOA(步骤S305中的到达时间差)测量值ti,1可得:
ri,1=c×Δt=cti,1=ri-r1    (4),其中,c为光速,ri,1表示目标节点和传感器i与传感器1的距离差。
式(4)进行平方后,将(3)式代入,得到:
r i , 1 2 + 2 r i , 1 r 1 = r i 2 - r 1 2 = k i - 2 x i , 1 - 2 y i , 1 y 0 - k 1 - - - ( 5 ) ,
其中:xi,1=xi-x1,yi,1=yi-y1,令za=(x0,y0,r1)T为未知矢量,则(5)式可建立以za为变量的方程组:
h=gaza    (6),
则巡逻机器人本体的估计位置所对应的误差矢量为:
ψ = h - g a z a 0 - - - ( 7 ) ,
其中, z a 0 = x y r 1 0 为巡逻机器人本体的实际位置坐标, h = 1 2 r 2,1 2 - k 2 + k 1 . . . r m 2 - k m + k 1 , g a = - x 2,1 y 2,1 r 2,1 . . . . . . . . . x m , 1 y m , 1 r m , 1 .
根据(7)式,(6)式可转换为如下:
1 2 r 2,1 2 - k 2 + k 1 . . . r m 2 - k m + k 1 = - x 2,1 y 2,1 r 2,1 . . . . . . . . . x m , 1 y m , 1 r m , 1 x 0 y 0 r 1 - - - ( 8 ) ,
根据加权最小二乘法,用TDOA测量值的协方差矩阵Q近似代替误差ψ的协方差矩阵:
Q = E ( nn T ) = 1 0 . . . 0 0 1 . . . 0 . . . . . . . . . . . . 0 0 0 1 , 其中, n = n 2,1 n 3,1 . . . n m , 1 , 其是以距离为单位的TDOA测量误差,近似服从正态分布,由(8)式可获得:
za=(x0,y0,r1)T=arg min{(h-gaza)TQ-1(h-gaza)}=(ga TQ-1ga)-1(ga TQ-1h)    (9)。
在误差方程(7)中,由于za中的r1实际上是与(x0,y0)有关的量,用Q矩阵近似代替误差矢量ψ的协方差矩阵也会带来一定的误差。为了更精确估计位置,可根据第一次获得的估计位置及附加变量r1等已知的约束条件,构造一组新的误差方程组,进行第二次加权最小二乘估计,便获得改进的巡逻机器人本体的估计位置。
具体步骤如下:
式(7)中由TDOA测量值造成的误差ψ的协方差矩阵:
ψ=E[ψψT]=BQB    (10),
其中B≈ri 0I,ο代表Schor乘积,ri 0是巡逻机器人本体和第i个参考节点的实际距离。
可将测量获得的ri代替ri 0,则za的第二次加权最小二乘法估计值为:
za=(ga Tψ-1ga)-1(ga Tψ-1h)    (11),
利用za值可获得新的矩阵:
z a ′ ≈ ( g a ′ T ψ - 1 g a ′ ) - 1 ( g a ′ T ψ - 1 ) h ′ = ( g a ′ T B ′ - 1 g a T Q - 1 g a B ′ - 1 g a ′ ) - 1 ( g a ′ T B ′ - 1 g a T Q - 1 g a B ′ - 1 ) h ′ - - - ( 12 ) ,
其中 h ′ = ( z a , 1 - x 1 ) 2 ( z a , 2 - y 1 ) 2 z a , 3 2 , g a ′ = 1 0 0 1 1 1 , z a ′ = ( x - x 1 ) 2 ( y - y 1 ) 2 , B ′ = z a ( 1 ) - x 1 0 0 0 z a ( 2 ) - y 1 0 0 0 z a ( 3 )
则巡逻机器人本体的定位计算结果为:
z p ′ = ± z a ′ + x 1 y 1 - - - ( 13 ) ,
为了更进一步提高定位精度,将(13)式获得的定位结果估计位置作为初始迭代,每一步迭代方向都是沿着当前点函数值下降的方向。巡逻机器人本体的定位模型转化为:
η=ht-gtδ    (14),
其中 δ = Δx Δy 表示位置估计误差, h t = r 2,1 - ( r Λ 2 - r Λ 1 ) r 3,1 - ( r Λ 3 - r Λ 1 ) . . . r m - 1 - ( r Λ m - r Λ 1 ) 表示距离差真实值与测量值之间的差值, g t = ∂ Δr 1 ∂ x | ( x Λ , y Λ ) ∂ Δ r 1 ∂ y | ( x Λ , y Λ ) ∂ Δ r 2 ∂ x | ( x Λ , y Λ ) ∂ Δ r 2 ∂ y | ( x Λ , y Λ ) . . . . . . ∂ Δ r m ∂ x | ( x Λ , y Λ ) ∂ Δ r m ∂ y | ( x Λ , y Λ ) = x 1 - x Λ r Λ 1 - x 2 - x Λ r Λ 2 y 1 - y Λ r Λ 1 - y 2 - y Λ r Λ 2 x 1 - x Λ r Λ 1 - x 3 - x Λ r Λ 3 y 1 - y Λ r Λ 1 - y 3 - y Λ r Λ 3 . . . . . . x 1 - x Λ r Λ 1 - x m - x Λ r Λ m y 1 - x Λ r Λ 1 - y m - y Λ r Λ m ,
为估计的目标初始迭代位置与各参考节点之间的距离。
式(14)利用加权最小二乘法可求解出巡逻机器人本体的坐标偏差:
δ = Δx Δy = ( g t T Q - 1 g t ) - 1 ( g t T Q - 1 h t ) - - - ( 15 ) ,
式中Q为参考节点M之间的时延估计值的协方差矩阵。
作为下一次迭代的初始值,重复迭代过程,直至Δx,Δy足够小,能满足所设定的阈值μ,其中|Δx|+|Δy|≤μ之后输出的估计值(x',y')就能获得巡逻机器人本体的位置坐标。
所述控制模块19用于将所述目标节点T的位置坐标传送给所述巡逻机器人本体20,以控制巡逻机器人本体20定位到所述目标节点位置。
优选地,所述接收模块、所述去噪模块与所述测量模块整合在一FPGA中,所述可移动本体包括一定位标签,所述定位标签用于接收和发射定位信号。
本发明的巡逻机器人定位系统100和方法中,在去噪模块13中对收集到的定位信号进行小波去噪处理,再通过测量模块15得出N组距离差,并通过两次加权最小二乘法及多次迭代收敛得出最终目标节点位置坐标。该方法有效解决了TDOA(到达时间差)的同步问题,可提高定位精度,在预埋的轨道进行巡逻能达到0.1m定位需求。
以上所揭露的仅为本发明实施例中的较佳实施例而已,当然不能以此来限定本发明之权利范围,因此依本发明权利要求所作的等同变化,仍属本发明所涵盖的范围。

Claims (10)

1.一种定位方法,用于将一个可移动本体定位到一目标节点,所述方法包括:
接收该目标节点的多个定位信号;
将接收到的定位信号进行小波去噪处理;
根据去噪处理后的定位信号的频率与相位差,测量N组距离差;
根据所测量的N组距离差进行至少两次加权最小二乘法,计算出预估的坐标,并将所述预估的坐标作为初始迭代位置进行多次迭代收敛,计算出目标节点位置;以及
控制可移动本体定位到所述目标节点位置。
2.如权利要求1所述的定位方法,其特征在于,所述将接收到的定位信号进行小波去噪处理的步骤包括:
对所接收的多个定位信号进行预处理;
对含噪声的所述定位信号进行正交小波变换,并分解到多个不同频带上,获得一组小波系数;
对所述小波系数进行自适应阈值处理,获得估计系数,并使得所述小波系数与所述估计系数的差值最小;以及
根据所述估计系数进行小波重构,从而获得去噪处理后的定位信号。
3.如权利要求1所述的定位方法,其特征在于,所述测量N组距离差的步骤包括:
对所述去噪处理后的定位信号进行内插还原,并进行解调,得到基带信号频率;
计算所述去噪处理后的定位信号的传送/到达时间差;
根据所述时间差测量出N组距离差。
4.如权利要求3所述的定位方法,其特征在于,所述根据所述时间差测量N组距离差的步骤包括:
根据公式:
测量所述距离差,其中,λ为信号波长,为发射信号与反射信号的相位差,c为光速,其中Δd∈[0,λ]。
5.如权利要求3所述的定位方法,其特征在于,所述计算出预估的坐标的步骤包括:
根据目标节点的初始位置,利用坐标点与原点距离计算公式以及时间差,计算第一次获得的目标节点的估计位置;以及
根据第一次获得的估计位置及一个已知的附加变量,构造一组新的误差方程组,进行第二次加权最小二乘法估计;
根据所述第二次加权最小二乘法的误差估计结果,以及可移动本体的实际位置坐标获得所述预估的坐标。
6.如权利要求1所述的定位方法,其特征在于,所述计算目标节点位置的
根据公式:η=ht-gtδ进行计算,其中 δ = Δx Δy , 表示位置估计误差, h t = r 2,1 - ( r Λ 2 - r Λ 1 ) r 3,1 - ( r Λ 3 - r Λ 1 ) . . . r m , 1 - ( r Λ m - r Λ 1 ) , 表示距离差真实值与测量值之间的差值,
g t = ∂ Δ r 1 ∂ x | ( x Λ , y Λ ) ∂ Δ r 1 ∂ y | ( x Λ , y Λ ) ∂ Δ r 2 ∂ x | ( x Λ , y Λ ) ∂ Δ r 2 ∂ y | ( x Λ , y Λ ) . . . . . . ∂ Δ r m ∂ x | ( x Λ , y Λ ) ∂ Δ r m ∂ y | ( x Λ , y Λ ) = x 1 - x Λ r Λ 1 - x 2 - x Λ r Λ 2 y 1 - y Λ r Λ 1 - y 2 - y Λ r Λ 2 x 1 - x Λ r Λ 1 - x 3 - x Λ r Λ 3 y 1 - y Λ r Λ 1 - y 3 - y Λ r Λ 3 . . . . . . x 1 - x Λ r Λ 1 - x m - x Λ r Λ m y 1 - y Λ r Λ 1 - y m - y Λ r Λ m , r Λ i = ( x i - x Λ ) 2 + ( y i - y Λ ) 2 ,
其中为估计的目标初始迭代位置与预定的多个参考节点之间的距离。
7.如权利要求1所述的定位方法,其特征在于,所述接收该目标节点的多个定位信号的步骤之前还包括:
根据一测距请求发送定位测距信号请求;以及
根据所述定位测距信号请求,通过无线的方法通知所述目标节点发送所述定位信号。
8.一种定位系统,包括至少一个后台服务器、至少一个可移动本体与至少三个基站,所述可移动本体根据所述后台服务器的控制命令运动至一目标节点,其特征在于,
所述基站用于接收所述目标节点的多个定位信号,并将所述目标节点的多个定位信号传输至所述后台服务器;
所述后台服务器包括:
接收模块,用于接收所述基站传输的所述目标节点的多个定位信号;
去噪模块,用于将接收到的定位信号进行小波去噪处理;
测量模块,用于根据去噪处理后的定位信号的频率与相位差,测量N组距离差;
计算模块,用于根据所测量的N组距离差进行至少两次加权最小二乘法,计算出预估的坐标,并将所述预估的坐标作为初始迭代位置进行多次迭代收敛,计算出目标节点位置;以及
控制模块,用于将所述目标节点位置传送给所述可移动本体,以控制可移动本体定位到所述目标节点位置。
9.如权利要求8所述的定位系统,其特征在于,所述去噪模块包括:
预处理单元,用于对所接收的多个定位信号进行预处理;
分解单元,用于对含噪声的所述定位信号进行正交小波变换,并分解到多个不同频带上,获得一组小波系数;
系数去噪单元,用于对所述小波系数进行自适应阈值处理,获得估计系数,并使得所述小波系数与所述估计系数的差值最小;以及
信号重构单元,用于根据所述估计系数进行小波重构,从而获得去噪处理后的定位信号。
10.如权利要求8所述的定位系统,其特征在于,所述接收模块、所述去噪模块与所述测量模块整合在一FPGA中,所述可移动本体包括一定位标签,所述定位标签用于接收和发射定位信号。
CN201410544898.9A 2014-10-15 2014-10-15 定位方法及系统 Pending CN104297724A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201410544898.9A CN104297724A (zh) 2014-10-15 2014-10-15 定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201410544898.9A CN104297724A (zh) 2014-10-15 2014-10-15 定位方法及系统

Publications (1)

Publication Number Publication Date
CN104297724A true CN104297724A (zh) 2015-01-21

Family

ID=52317526

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201410544898.9A Pending CN104297724A (zh) 2014-10-15 2014-10-15 定位方法及系统

Country Status (1)

Country Link
CN (1) CN104297724A (zh)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105353351A (zh) * 2015-10-27 2016-02-24 杭州电子科技大学 一种基于多信标到达时间差改进型定位方法
CN105951618A (zh) * 2016-05-09 2016-09-21 徐洪军 一种减速带型发射车辆定位机器人
CN106856594A (zh) * 2016-12-13 2017-06-16 中国南方电网有限责任公司调峰调频发电公司 基于rssi的室内定位方法与系统
CN107133548A (zh) * 2016-02-27 2017-09-05 林项武 部分或整体人体轮廓数据的获取装置和方法及其应用方法
CN108376273A (zh) * 2018-01-23 2018-08-07 四川精工伟达智能技术股份有限公司 多目标定位跟踪方法、装置、计算机可读介质及电子设备
CN109084779A (zh) * 2018-09-29 2018-12-25 上海思依暄机器人科技股份有限公司 一种机器人导航方法及系统
CN109565781A (zh) * 2016-08-08 2019-04-02 索尼公司 定位装置、方法、移动节点和无线通信设备
CN109758274A (zh) * 2019-02-28 2019-05-17 清华大学 单髁膝关节置换术中股骨远端相对于假体垫片力线轨迹的测量方法与系统
WO2020056586A1 (zh) * 2018-09-18 2020-03-26 深圳市大疆创新科技有限公司 高度确定方法、装置、电子设备和计算机可读存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102026370A (zh) * 2010-12-21 2011-04-20 西安电子科技大学 基于监测节点呈圆周分布的tdoa定位方法
WO2012016355A1 (en) * 2010-08-05 2012-02-09 Telefonakietolaget Lm Ericsson (Publ) Method of and system for locating the position of user equipment
CN103925925A (zh) * 2014-03-14 2014-07-16 四川九洲空管科技有限责任公司 一种用于多点定位系统的实时高精度位置解算方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2012016355A1 (en) * 2010-08-05 2012-02-09 Telefonakietolaget Lm Ericsson (Publ) Method of and system for locating the position of user equipment
CN102026370A (zh) * 2010-12-21 2011-04-20 西安电子科技大学 基于监测节点呈圆周分布的tdoa定位方法
CN103925925A (zh) * 2014-03-14 2014-07-16 四川九洲空管科技有限责任公司 一种用于多点定位系统的实时高精度位置解算方法

Non-Patent Citations (8)

* Cited by examiner, † Cited by third party
Title
付明: "小波变换在信号降噪中的应用研究", 《微计算机信息》 *
刘静 等: "自适应小波阈值去噪方法", 《现代电子技术》 *
周帅 等: "基于改进阈值函数和自适应阈值的小波去噪方法", 《协议 算法及仿真》 *
徐彤阳: "NLOS环境下无线传感器网络TOA定位算法", 《计算机工程》 *
林茂六 等: "高速采样信号数字内插理论与正弦内插算法研究", 《电子学报》 *
潘继飞 等: "基于内插采样技术的高精度时间间隔测量方法", 《系统工程与电子技术》 *
钱苏敏 等: "基于小波阈值改进的去噪算法研究", 《研究与开发》 *
陈宝全 等: "小波分析法解决无线定位跟踪问题", 《电信科学》 *

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105353351A (zh) * 2015-10-27 2016-02-24 杭州电子科技大学 一种基于多信标到达时间差改进型定位方法
CN107133548A (zh) * 2016-02-27 2017-09-05 林项武 部分或整体人体轮廓数据的获取装置和方法及其应用方法
CN105951618A (zh) * 2016-05-09 2016-09-21 徐洪军 一种减速带型发射车辆定位机器人
CN109565781A (zh) * 2016-08-08 2019-04-02 索尼公司 定位装置、方法、移动节点和无线通信设备
CN106856594A (zh) * 2016-12-13 2017-06-16 中国南方电网有限责任公司调峰调频发电公司 基于rssi的室内定位方法与系统
CN108376273A (zh) * 2018-01-23 2018-08-07 四川精工伟达智能技术股份有限公司 多目标定位跟踪方法、装置、计算机可读介质及电子设备
WO2020056586A1 (zh) * 2018-09-18 2020-03-26 深圳市大疆创新科技有限公司 高度确定方法、装置、电子设备和计算机可读存储介质
CN109084779A (zh) * 2018-09-29 2018-12-25 上海思依暄机器人科技股份有限公司 一种机器人导航方法及系统
CN109758274A (zh) * 2019-02-28 2019-05-17 清华大学 单髁膝关节置换术中股骨远端相对于假体垫片力线轨迹的测量方法与系统
CN109758274B (zh) * 2019-02-28 2020-12-29 清华大学 单髁膝关节置换术中股骨远端相对于假体垫片力线轨迹的测量方法与系统

Similar Documents

Publication Publication Date Title
CN104297724A (zh) 定位方法及系统
Vossiek et al. Wireless local positioning
KR101049603B1 (ko) Rfid를 이용한 이동체 실시간 위치 결정 시스템 및 그 방법과, 그를 위한 무선중계장치 설치 방법
CN102098782B (zh) 一种基于调频广播的终端定位方法及专用定位装置
CN106793087B (zh) 一种基于aoa和pdoa的阵列天线室内定位方法
CN103068043B (zh) 一种基于wifi和加速度传感器的室内精确定位方法
KR101597437B1 (ko) 무선신호의 상대적 수신세기 비율정보를 이용한 실내측위시스템 및 방법
CN104020444B (zh) 一种双轮差动式机器人室内组合定位方法
CN106154230A (zh) 一种基于声音测距的智能移动终端的室内定位方法
CN104076349A (zh) 一种基于多普勒频移的被动式移动目标定位方法
CN102014489A (zh) 环境自适应的rssi局部定位系统及方法
CN103389487A (zh) 室内定位方法和装置
CN103644905A (zh) 一种情境相关的室内定位方法及系统
CN103874020A (zh) 非直达径环境下单接收器的超宽带定位方法
CN106686722A (zh) 基于css技术的大型室内环境定位的微基站及工作方法
Astafiev et al. Development of indoor positioning algorithm based on Bluetooth Low Energy beacons for building RTLS-systems
CN104569912A (zh) 一种固定监测站单站无源区域定位方法
CN106873015A (zh) Css与gps相结合的室内外无缝定位系统及定位方法
Rose et al. 3D trilateration localization using RSSI in indoor environment
CN106888504B (zh) 基于fm与dtmb信号的室内位置指纹定位方法
CN203416427U (zh) 基于ZigBee技术的自组网定位系统
CN104683949A (zh) 一种应用于无线Mesh网中基于天线阵列的混合自定位方法
CN103152822A (zh) 一种基于apit的煤矿井下人员定位方法
CN105873210A (zh) 一种移动趋向混合定位方法
CN105979581B (zh) 一种基于功率差的室内定位方法

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
CB02 Change of applicant information

Address after: 518000 Guangdong province Shenzhen City Guangming Street million Daiheng high tech Industrial Park No. 5 Building 5 floor

Applicant after: Shenzhen City Zhongzhi Kechuang Robot Co., Ltd.

Address before: 518000 Guangdong province Shenzhen City Guangming Street million Daiheng high tech Industrial Park No. 5 Building 5 floor

Applicant before: Shenzhen Kesong Electronic Co., Ltd.

COR Change of bibliographic data
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20150121