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/0294Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
    • 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

Abstract

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

Description

定位方法及系统
技术领域
[0001] 本发明涉及一种定位技术,具体涉及一种基于小波去噪的定位方法及相关系统。
背景技术
[0002] 随着社会经济的发展,超级商场、机场、车站、会展中心及物流仓库等大型场所的 规模和数量不断增加,大中城市的高层、高档商用楼日益增多,其保安自动化需求日趋迫 切。巡逻机器人是一个集环境感知、路线规划、动态决策、行为控制以及报警装置于一体的 多功能综合系统,采用巡逻机器人实行定时、定点监控巡逻或不间断流动巡逻是目前一种 可行的解决方案。将机器人用于保安工作,具有广阔的应用前景,今年来已受到国内外的重 视,成为服务机器人的一个新研究方向。
[0003] 巡逻机器人中使用的定位技术是一种应用广泛的高科技技术,它在移动互联网、 物联网、导航技术应用非常广泛,是现今研究的热点技术之一。目前用得最多的是GPS、蜂 窝基站、Zigbee、Wifi、UWB等。巡逻机器人定位采用最多的是GPS算法,但由于GPS对时钟 要求极高且信号经大气层传播,容易受天气影响,而在室内会受电磁屏蔽效应,往往难以收 到GPS信号,因此GPS定位误差常为10m左右。而蜂窝基站定位的优势是定位速度快,但其 定位精度不高。多基站定位常用的为Τ0Α及TD0A (到达时间及到达时间差),基于CC2431/ Zigbee的定位技术在理想环境下最高精度能达到0. 5m,但是在实际应用中定位精度不高。 根据巡逻机器人的实际需要,定位精度通常需达到〇. lm,而现有的技术中,仅UWB的精度较 高。然而,UWB成本太高,无法满足企业要求。
发明内容
[0004] 为了解决上述的技术问题,本发明提供一种定位方法和相关系统。
[0005] 在本发明的一个实施例中,提供一种定位方法,用于将一个可移动本体定位到一 目标节点。所述方法包括:接收该目标节点的多个定位信号;将接收到的定位信号进行小 波去噪处理;根据去噪处理后的定位信号的频率与相位差,测量N组距离差;根据所测量的 N组距离差进行至少两次加权最小二乘法,计算出预估的坐标,并将所述预估的坐标作为初 始迭代位置进行多次迭代收敛,计算出目标节点位置;以及控制可移动本体定位到所述目 标节点位置。
[0006] 优选的,所述将接收到的定位信号进行小波去噪处理的步骤包括:对所接收的多 个定位信号进行预处理;对含噪声的所述定位信号进行正交小波变换,并分解到多个不同 频带上,获得一组小波系数;对所述小波系数进行自适应阈值处理,获得估计系数,并使得 所述小波系数与所述估计系数的差值最小;以及根据所述估计系数进行小波重构,从而获 得去噪处理后的定位信号。
[0007] 优选的,所述测量N组距离差的步骤包括:对所述去噪处理后的定位信号进行内 插还原,并进行解调,得到基带信号频率;计算所述去噪处理后的定位信号的传送/到达时 间差;根据所述时间差测量出N组距离差。
[0008] 优选的,所述根据所述时间差测量N组距离差的步骤包括:根据公式:
[0009]
Figure CN104297724AD00061
测量所述距离差,其中,λ为信号波长,f 为发射信号与反射信号的相位差,C为光速,其中
Figure CN104297724AD00062
[0010] 优选的,所述计算出预估的坐标的步骤包括:根据目标节点的初始位置,利用坐标 点与原点距离计算公式以及时间差,计算第一次获得的目标节点的估计位置;以及根据第 一次获得的估计位置及一个已知的附加变量,构造一组新的误差方程组,进行第二次加权 最小二乘法估计;根据所述第二次加权最小二乘法的误差估计结果,以及可移动本体的实 际位置坐标获得所述预估的坐标。
[0011] 优选的,所述计算目标节点位置的步骤包括:
Figure CN104297724AD00063
[0012] 根据公式:Π = ht_gt δ进行计算,其中 表示位置估计误差,
Figure CN104297724AD00064
f 表示距离差真实值与测量值之间的差值, >
[0013]
Figure CN104297724AD00065
Figure CN104297724AD00066
其中^为估计的目标初始迭代位置与预定的多个参考节点之间 t 1 的距离。
[0014] 优选的,所述接收该目标节点的多个定位信号的步骤之前还包括:根据一测距请 求发送定位测距信号请求;以及根据所述定位测距信号请求,通过无线的方法通知所述目 标节点发送所述定位信号。
[0015] 在本发明的另一个实施例中,提供一种定位系统,包括至少一个后台服务器、至少 一个可移动本体与至少三个基站,所述可移动本体根据所述后台服务器的控制命令运动至 一目标节点。所述基站用于接收所述目标节点的多个定位信号,并将所述目标节点的多个 定位信号传输至所述后台服务器;所述后台服务器包括:接收模块,用于接收所述基站传 输的所述目标节点的多个定位信号;去噪模块,用于将接收到的定位信号进行小波去噪处 理;测量模块,用于根据去噪处理后的定位信号的频率与相位差,测量N组距离差;计算模 块,用于根据所测量的N组距离差进行至少两次加权最小二乘法,计算出预估的坐标,并将 所述预估的坐标作为初始迭代位置进行多次迭代收敛,计算出目标节点位置;以及控制模 块,用于将所述目标节点位置传送给所述可移动本体,以控制可移动本体定位到所述目标 节点位置。
[0016] 优选的,所述去噪模块包括:预处理单元,用于对所接收的多个定位信号进行预处 理;分解单元,用于对含噪声的所述定位信号进行正交小波变换,并分解到多个不同频带 上,获得一组小波系数;系数去噪单元,用于对所述小波系数进行自适应阈值处理,获得估 计系数,并使得所述小波系数与所述估计系数的差值最小;以及信号重构单元,用于根据所 述估计系数进行小波重构,从而获得去噪处理后的定位信号。
[0017] 优选的,所述接收模块、所述去噪模块与所述测量模块整合在一 FPGA中,所述可 移动本体包括一定位标签,所述定位标签用于接收和发射定位信号。
[0018] 本发明的巡逻机器人定位系统和方法中,先对收集到的定位信号进行小波去噪处 理,再计算得出N组距离差,并通过两次加权最小二乘法及多次迭代收敛得出最终目标节 点位置坐标。该方法有效解决了 TD0A(到达时间差)的同步问题,可提高定位精度,在预埋 的轨道进行巡逻能达到0. lm定位需求。
附图说明
[0019] 为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现 有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本 发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可 以根据这些附图获得其他的附图。
[0020] 图1是采用本发明的定位系统的节点布置图。
[0021] 图2是采用本发明的定位系统的示意图。
[0022] 图3是本发明提供的定位方法的流程图。
[0023] 图4是本发明提供的定位系统的功能模块图。
具体实施方式
[0024] 下面结合附图和具体实施方式对本发明的技术方案作进一步更详细的描述。显 然,所描述的实施例仅仅是本发明的一部分实施例,而不是全部的实施例。基于本发明中的 实施例,本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例, 都应属于本发明保护的范围。
[0025] 请参阅图1,其为采用本发明的定位系统的节点布置图。其中,T为目标节点,Μ为 中心锚节点(参考节点)。所述目标节点的定位信号通过中心锚节点获取,中心锚节点通过 Ν根基站的天线接收目标节点的定位信号。更具体的是,其中一根天线0位于中心锚节点 处,其他天线则通过一定长度的同轴电缆对称引出,使得目标节点被所述Ν根天线包围。由 于该Ν根天线的位置为已知,因此可以据此求出Τ节点位置。请结合图1与图2,本发明提 供一种基于小波去噪相位差的定位系统,该定位系统执行一定位方法。本实施例中,以巡逻 机器人的定位系统定位方法对本发明进行详细描述。从总体上来说,所述巡逻机器人定位 方法包括步骤:首先,在巡逻机器人循迹范围内布置移动节点,巡逻机器人后台服务器(后 续将以模块进一步说明)中的FPGA(Field_Programmable Gate Array,即现场可编程门阵 列,是一种通信模块,通常由触发器和查找表组成)获取移动节点的定位信号。FPGA收到 定位信号后,开始对定位信号进行小波去噪处理。然后,巡逻机器人的后台服务器计算去噪 重构后的信号基带频率,以及计算发射信号与反射信号的相位差,再通过多线路到达信号 相位差,测得N组距离差给后台服务器中的运算服务器,并通知基站使目标节点T (其具有 定位标签)停止发送定位信号。再者,巡逻机器人的后台服务器进行至少两次加权最小二 乘,多次迭代收敛处理后计算得出目标节点T的位置,最后将最终坐标传递给巡逻机器人 本体。经过模拟仿真,可知通过该方法可在预埋的轨迹进行巡逻能达到0. lm的精度,相比 于现有技术,大大提高了定位精度。
[0026] 更具体的,请参阅图3,本发明提供的巡逻机器人定位方法包括步骤:
[0027] 步骤S301,接收该目标节点T的多个定位信号。具体的,机器人后台服务器收到定 位指令时,将测距请求发送给基站,基站通过无线的方式通知目标节点T发送定位信号,目 标节点T发送定位信号并返回确认。本实施例中,通过所述目标节点T上的定位标签发送 定位信号,并通过上述N根天线同步接收所述定位信号。
[0028] 步骤S303,将接收到的定位信号进行小波去噪处理。由于定位容易受周边环境的 影响,真实环境下收集的信号都含有噪声,特别在室外,会严重影响定位精度,因此对原始 信号进行去噪处理是非常有必要的。具体实现方法包括步骤:
[0029] S303a,对所接收的多个定位信号进行预处理。具体的,一个经预处理后的含噪声 的一维信号的模型可表示为:
[0030] S (k) = f (k) + δ e (k) (1)
[0031] 其中:f(k)为真实信号,e(k)为噪声信号,δ为噪声强度,s(k)为含噪声的信号。
[0032] S303b,对含噪声的所述定位信号进行正交小波变换,并分解到多个不同频带上, 获得一组小波系数。本实施例中,对含噪声的信号S(k)进行正交小波变换,分解到若干不 同频带上,获得一组小波系数W jik。
[0033] S303c,对所述小波系数进行自适应阈值处理,获得估计系数,并使得所述小波系 数与所述估计系数的差值最小。本实施例中,通过对W" k进行自适应阈值处理,获得估计系 数
Figure CN104297724AD00081
两者的差值尽可能小。
[0034] S303d,根据所述估计系数进行小波重构,从而获得去噪信号。本实施例中,利用
Figure CN104297724AD00082
进行小波重构,获得估计信号S(k)即为去噪后的信号。
[0035] 由于低频子带有较高的幅度值,较低频率回波脉冲保留了较详细的轮廓;而高频 子带有相对较低的幅度值,只保留回波脉冲的高频细节部分,根据以上特征,上述小波阈值 去噪方法,可滤除幅度较低的高频杂波和噪声,使得小波重构后的信号更接近真实信号。
[0036] 步骤S305,根据去噪处理后的定位信号的频率与相位差,测量N组距离差。
[0037] 具体的,FPGA负责信号处理及测距计算。本实施例中,在FPGA中,对多路射频前 端传送过来的去噪重构后的数字信号进行内插还原,再进行解调,得出基带信号频率,并计 算该些信号的传送/到达时间差。根据所述时间差,测量出N组距离差,并将该N组距离差 的值传送给后台服务器,以进行后续的定位算法计算。
[0038] 更具体的,设多路去噪重构信号的相位差为f,f。为待定位节点(如目标节 点T)的发射基带频率,则目标节点T到具有多线路的所述参考节点Μ的到达时间差为
Figure CN104297724AD00091
从而目标节点Τ到多线路参考节点Μ的距离差为:
[0039]
Figure CN104297724AD00092
[0040] 其中,式中λ为信号波长,Ρ为发射信号与反射信号的相位差,c为光速,其中
Figure CN104297724AD00093
[0041] 步骤S307,根据所测量的Ν组距离差进行至少两次加权最小二乘法,计算出预 估的坐标,并将所述预估的坐标作为初始迭代位置进行多次迭代收敛,计算出目标节点位 置。本实施例中,进行两次加权最小二乘法。具体如下:将目标节点Τ的初始位置坐标设为 (X。,yQ),其中< =< + <,由经典距离公式:
[0042]
Figure CN104297724AD00094
(3),
[0043] 以及根据TD0A (步骤S305中的到达时间差)测量值ti;1可得:
[0044]
Figure CN104297724AD00095
⑷,其中,c为光速,表示目标节点和传感 器i与传感器1的距离差。
[0045] 式⑷进行平方后,将⑶式代入,得到:
[0046]
Figure CN104297724AD00096
(5),
[0047] 其中
Figure CN104297724AD00097
为未知矢量,则(5)式可建立 以za为变量的方程组:
[0048] h = gaza (6),
[0049] 则巡逻机器人本体的估计位置所对应的误差矢量为:
[0050]
Figure CN104297724AD00098
(7),
Figure CN104297724AD00099
Figure CN104297724AD000910
[0051] 其中, 为巡逻机器人本体的实际位置坐标, 9
Figure CN104297724AD000911
[0052] 根据(7)式,(6)式可转换为如下:
[0053]
Figure CN104297724AD00101
(8).
[0054] 根据加权最小二乘法,用TD0A测量值的协方差矩阵Q近似代替误差Ψ的协方差 矩阵:
[0055]
Figure CN104297724AD00102
其是以距离为单位的TD0A测量 误差,近似服从正态分布,由(8)式可获得:
[0056]
Figure CN104297724AD00103
(9)。
[0057] 在误差方程(7)中,由于za中的Γι实际上是与( X(l,y(l)有关的量,用Q矩阵近似代 替误差矢量Ψ的协方差矩阵也会带来一定的误差。为了更精确估计位置,可根据第一次获 得的估计位置及附加变量A等已知的约束条件,构造一组新的误差方程组,进行第二次加 权最小二乘估计,便获得改进的巡逻机器人本体的估计位置。
[0058] 具体步骤如下:
[0059] 式(7)中由TD0A测量值造成的误差Ψ的协方差矩阵:
[0060]
Figure CN104297724AD00104
[0061] 其中
Figure CN104297724AD00105
〇代表Schor乘积,ri°是巡逻机器人本体和第 i个参考节点的实际距离。
[0062] 可将测量获得的A代替ri°,则za的第二次加权最小二乘法估计值为:
[0063]
Figure CN104297724AD00106
(11),
[0064] 利用za值可获得新的矩阵:
[0065]
Figure CN104297724AD00107
[0067] 则巡逻机器人本体的定位计算结果为:
[0068]
Figure CN104297724AD00111
(ί3),
[0069] 为了更进一步提高定位精度,将(13)式获得的定位结果估计位置作为初始 迭代,每一步迭代方向都是沿着当前点函数值下降的方向。巡逻机器人本体的定位模型转 化为:
[0070] n = ht-gt δ (14),其中
Figure CN104297724AD00112
表示位置估计误差,表示 距离差真实值与测量值之间的差值,
Figure CN104297724AD00113
Figure CN104297724AD00114
[0071]
Figure CN104297724AD00115
为估计的目标初始迭代位置
Figure CN104297724AD00116
与各参考节点之间 的距离。
[0072] 式(14)利用加权最小二乘法可求解出巡逻机器人本体的坐标偏差:
[0073]
Figure CN104297724AD00117
(15),
[0074] 式中Q为参考节点Μ之间的时延估计值的协方差矩阵。
[0075]
Figure CN104297724AD00118
作为下一次迭代的初始值,重复迭代过程,直至Λ X,Ay 足够小,能满足所设定的阈值μ,其中
Figure CN104297724AD00119
之后输出的估计值(X',y')就 能获得巡逻机器人本体的位置坐标。
[0076] 步骤S309,控制巡逻机器人本体20定位到所述目标节点位置,也就是步骤S307中 的位置坐标。
[0077] 本发明的定位方法首先在FPGA中对收集到的定位信号进行小波去噪处理,再通 过多线路到达信号相位差得出N组距离差,通过两次加权最小二乘法及多次迭代收敛得出 最终目标节点位置坐标。该方法有效解决了 TD0A(到达时间差)的同步问题,可提高定位 精度,在预埋的轨道进行巡逻能达到0. lm定位需求。
[0078] 请参阅图4,本发明还提供一种巡逻机器人定位系统100,其包括至少一个后台服 务器10、至少一个所述巡逻机器人本体20以及至少3个基站。其中,所述巡逻机器人本体 20包含定位标签。所述后台服务器10通过所述至少3个基站接收所述巡逻机器人本体20 的定位标签发送的信号,以及接收一目标节点T的多个定位信号,并将该目标节点T的多个 定位信号传输至后台服务器10。本实施例中,所述后台服务器10通过所述多根同步天线接 收所述多个定位信号。所述巡逻机器人本体20通过所述至少3个基站接收所述后台服务 器10返回的定位信息,并根据所述后台服务器10的定位控制命令,在预定的多个移动节点 运动。
[0079] 更具体的,所述后台服务器10包括接收模块11、去噪模块13、测量模块15、计算模 块17以及控制模块19。
[0080] 所述接收模块11用于接收一目标节点T的多个定位信号。具体的,当后台服务器 10收到定位指令时,将测距请求发送给FPGA,FPGA发送定位测距信号请求给基站(请参阅 图4),基站通过无线的方式通知目标节点T发送定位信号,目标节点T发送定位信号并返 回确认。所述目标节点T发送的定位信号通过所述N根天线接收,并到达所述后台服务器 10。同时,基站给FPGA返回信号发送确认。本实施例中,所述FPGA通过USB接口与后台服 务器10连接。
[0081] 由于定位容易受周边环境的影响,真实环境下收集的信号都含有噪声,特别在室 夕卜,会严重影响定位精度,因此对原始信号进行去噪处理是非常有必要的。所述去噪模块13 用于将接收到的定位信号进行小波去噪处理。具体的,所述去噪模块13包括预处理单元、 分解单元、系数去噪单元以及信号重构单元。所述去噪模块13的具体实现方法包括步骤:
[0082] S303a,预处理单元对所接收的多个定位信号进行预处理。具体的,一个经预处理 后的含噪声的一维信号的模型可表示为:
[0083]
Figure CN104297724AD00121
[0084] 其中:f(k)为真实信号,e(k)为噪声信号,δ为噪声强度,S(k)为含噪声的信号。
[0085] S303b,分解单元对含噪声的所述定位信号进行正交小波变换,并分解到多个不同 频带上,获得一组小波系数。本实施例中,对含噪声的信号S(k)进行正交小波变换,分解到 若干不同频带上,获得一组小波系数W jik。
[0086] S303c,系数去噪单元对所述小波系数进行自适应阈值处理,获得估计系数,并使 得所述小波系数与所述估计系数的差值最小。本实施例中,通过对W" k进行自适应阈值处 理,获得估计系数
Figure CN104297724AD00122
使得
Figure CN104297724AD00123
两者的差值尽可能小。
[0087] S303d,信号重构单元根据所述估计系数进行小波重构,从而获得去噪信号。本实 施例中,利用
Figure CN104297724AD00124
进行小波重构,获得估计信号S(k)即为去噪后的信号。
[0088] 由于低频子带有较高的幅度值,较低频率回波脉冲保留了较详细的轮廓;而高频 子带有相对较低的幅度值,只保留回波脉冲的高频细节部分,根据以上特征,上述小波阈值 去噪方法,可滤除幅度较低的高频杂波和噪声,使得小波重构后的信号更接近真实信号。
[0089] 所述测量模块15用于根据去噪处理后的定位信号的频率与相位差,测量N组距离 差。具体的,由所述FPGA负责信号处理及测距计算。本实施例中,在FPGA中,对多路射频 前端传送过来的去噪重构后的数字信号进行内插还原,再进行解调,得出基带信号频率,并 计算该些信号的传送/到达时间差。根据所述时间差,测量出N组距离差,并将该N组距离 差的值传送给后台服务器,以进行后续的定位算法计算。
[0090] 更具体的,设多路去噪重构信号的相位差为f»,f。为待定位节点(如目标节 点T)的发射基带频率,则目标节点T到具有多线路的所述参考节点Μ的到达时间差为
Figure CN104297724AD00131
从而目标节点Τ到多线路参考节点Μ的距离差为: f
[0091]
Figure CN104297724AD00132
[0092] 其中,式中λ为信号波长,f»为发射信号与反射信号的相位差,c为光速,其中 Ad e [0, λ ]。
[0093] 所述计算模块17用于根据所测量的Ν组距离差进行至少两次加权最小二乘法,计 算出预估的坐标,并将所述预估的坐标作为初始迭代位置进行多次迭代收敛,计算出目标 节点位置。本实施例中,进行两次加权最小二乘法。
[0094] 具体如下:将目标节点Τ的初始位置坐标设为(X(l,y(l),其中
Figure CN104297724AD00133
f由经典距 离公式:
[0095]
Figure CN104297724AD00134
[0096] 以及根据TD0A (步骤S305中的到达时间差)测量值ti,1可得:
[0097]
Figure CN104297724AD00135
⑷,其中,c为光速,ri;1表示目标节点和传感 器i与传感器1的距离差。
[0098] 式⑷进行平方后,将⑶式代入,得到:
[0099]
Figure CN104297724AD00136
⑶,
[0100] 其中
Figure CN104297724AD00137
为未知矢量,贝1J (5)式可建立 以za为变量的方程组:
[0101] h = gaza (6),
[0102] 则巡逻机器人本体的估计位置所对应的误差矢量为:
[0103]
Figure CN104297724AD00138
(7),
[0104] 其中,,
Figure CN104297724AD00139
为巡逻机器人本体的实际位置坐标,
Figure CN104297724AD001310
Figure CN104297724AD00141
[0105] 根据(7)式,(6)式可转换为如下:
[0106]
Figure CN104297724AD00142
(8),
Figure CN104297724AD00143
[0107] 根据加权最小二乘法,用TD0A测量值的协方差矩阵Q近似代替误差Ψ的协方差 矩阵:
Figure CN104297724AD00144
[0108] 其中, 其是以距离为单位的TD0A测量 , t 误差,近似服从正态分布,由(8)式可获得:
[0109]
Figure CN104297724AD00145
(9)。
[0110] 在误差方程⑵中,由于za中的Γι实际上是与(X(l, y(l)有关的量,用Q矩阵近似代 替误差矢量Ψ的协方差矩阵也会带来一定的误差。为了更精确估计位置,可根据第一次获 得的估计位置及附加变量h等已知的约束条件,构造一组新的误差方程组,进行第二次加 权最小二乘估计,便获得改进的巡逻机器人本体的估计位置。
[0111] 具体步骤如下:
[0112] 式(7)中由TD0A测量值造成的误差Ψ的协方差矩阵:
[0113]
Figure CN104297724AD00146
[0114] 其中
Figure CN104297724AD00147
〇代表Schor乘积,ri°是巡逻机器人本体和第 i个参考节点的实际距离。
[0115] 可将测量获得的^代替1^°,则za的第二次加权最小二乘法估计值为:
[0116]
Figure CN104297724AD00148
(11),
[0117] 利用za值可获得新的矩阵:
[0118]
Figure CN104297724AD00149
Figure CN104297724AD00151
[0120] 则巡逻机器人本体的定位计算结果为:
[0121]
Figure CN104297724AD00152
"3),
[0122] 为了更进一步提高定位精度,将(13)式获得的定位结果估计位置
Figure CN104297724AD00153
作为初始 迭代,每一步迭代方向都是沿着当前点函数值下降的方向。巡逻机器人本体的定位模型转 化为:
[0123]
Figure CN104297724AD00154
[0124] 其中
Figure CN104297724AD00155
表示位置估计误差,
Figure CN104297724AD00156
表示距离差真实值与测量值 之间的差值,
Figure CN104297724AD00157
[0125]
Figure CN104297724AD00158
为估计的目标初始迭代位置
Figure CN104297724AD00159
与各参考节点之间 的距离。
[0126] 式(14)利用加权最小二乘法可求解出巡逻机器人本体的坐标偏差:
[0127]
Figure CN104297724AD001510
(15),
[0128] 式中Q为参考节点Μ之间的时延估计值的协方差矩阵。
[0129]
Figure CN104297724AD001511
作为下一次迭代的初始值,重复迭代过程,直至Λ X,Ay 足够小,能满足所设定的阈值μ,其中
Figure CN104297724AD001512
之后输出的估计值(X',y')就能 获得巡逻机器人本体的位置坐标。
[0130] 所述控制模块19用于将所述目标节点T的位置坐标传送给所述巡逻机器人本体 20,以控制巡逻机器人本体20定位到所述目标节点位置。
[0131] 优选地,所述接收模块、所述去噪模块与所述测量模块整合在一 FPGA中,所述可 移动本体包括一定位标签,所述定位标签用于接收和发射定位信号。
[0132] 本发明的巡逻机器人定位系统100和方法中,在去噪模块13中对收集到的定位信 号进行小波去噪处理,再通过测量模块15得出Ν组距离差,并通过两次加权最小二乘法及 多次迭代收敛得出最终目标节点位置坐标。该方法有效解决了 TD0A(到达时间差)的同步 问题,可提高定位精度,在预埋的轨道进行巡逻能达到〇. lm定位需求。
[0133] 以上所揭露的仅为本发明实施例中的较佳实施例而已,当然不能以此来限定本发 明之权利范围,因此依本发明权利要求所作的等同变化,仍属本发明所涵盖的范围。

Claims (10)

1. 一种定位方法,用于将一个可移动本体定位到一目标节点,所述方法包括: 接收该目标节点的多个定位信号; 将接收到的定位信号进行小波去噪处理; 根据去噪处理后的定位信号的频率与相位差,测量N组距离差; 根据所测量的N组距离差进行至少两次加权最小二乘法,计算出预估的坐标,并将所 述预估的坐标作为初始迭代位置进行多次迭代收敛,计算出目标节点位置;以及 控制可移动本体定位到所述目标节点位置。
2. 如权利要求1所述的定位方法,其特征在于,所述将接收到的定位信号进行小波去 噪处理的步骤包括: 对所接收的多个定位信号进行预处理; 对含噪声的所述定位信号进行正交小波变换,并分解到多个不同频带上,获得一组小 波系数; 对所述小波系数进行自适应阈值处理,获得估计系数,并使得所述小波系数与所述估 计系数的差值最小;以及 根据所述估计系数进行小波重构,从而获得去噪处理后的定位信号。
3. 如权利要求1所述的定位方法,其特征在于,所述测量N组距离差的步骤包括: 对所述去噪处理后的定位信号进行内插还原,并进行解调,得到基带信号频率; 计算所述去噪处理后的定位信号的传送/到达时间差; 根据所述时间差测量出N组距离差。
4. 如权利要求3所述的定位方法,其特征在于,所述根据所述时间差测量N组距离差的 步骤包括: 根据公式:
Figure CN104297724AC00021
测量所述距离差,其中,λ为信号波长,f为发射信号与反射信号的相位差,c为光速, 其中 Ad e [〇, λ]。
5. 如权利要求3所述的定位方法,其特征在于,所述计算出预估的坐标的步骤包括: 根据目标节点的初始位置,利用坐标点与原点距离计算公式以及时间差,计算第一次 获得的目标节点的估计位置;以及 根据第一次获得的估计位置及一个已知的附加变量,构造一组新的误差方程组,进行 第二次加权最小二乘法估计; 根据所述第二次加权最小二乘法的误差估计结果,以及可移动本体的实际位置坐标获 得所述预估的坐标。
6. 如权利要求1所述的定位方法,其特征在于,所述计算目标节点位置的
Figure CN104297724AC00022
根据公式:η = ht-gt δ进行计算,其弓 表示位置估计误差, f 表示距离差真实值与测量值之间的差值,
Figure CN104297724AC00031
9
Figure CN104297724AC00032
其中^为估计的目标初始迭代位置与预定的多个参考节点之间的距离。
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 (8)

* 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的室内定位方法与系统
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 (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 徐洪军 一种减速带型发射车辆定位机器人
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) 定位方法及系统
Woo et al. Application of WiFi-based indoor positioning system for labor tracking at construction sites: A case study in Guangzhou MTR
KR101049603B1 (ko) Rfid를 이용한 이동체 실시간 위치 결정 시스템 및 그 방법과, 그를 위한 무선중계장치 설치 방법
CN106793087B (zh) 一种基于aoa和pdoa的阵列天线室内定位方法
CN102098782B (zh) 一种基于调频广播的终端定位方法及专用定位装置
CN103813448A (zh) 一种基于rssi的室内定位方法
CN103068043B (zh) 一种基于wifi和加速度传感器的室内精确定位方法
CN104020444B (zh) 一种双轮差动式机器人室内组合定位方法
KR101247964B1 (ko) 비콘을 이용한 전파식별 리더의 위치 측정 방법 및 그를 위한 전파식별 시스템
CN103644905A (zh) 一种情境相关的室内定位方法及系统
CN103389487A (zh) 室内定位方法和装置
CN102014489A (zh) 环境自适应的rssi局部定位系统及方法
CN106154230A (zh) 一种基于声音测距的智能移动终端的室内定位方法
KR101597437B1 (ko) 무선신호의 상대적 수신세기 비율정보를 이용한 실내측위시스템 및 방법
CN106686722A (zh) 基于css技术的大型室内环境定位的微基站及工作方法
CN107544054A (zh) 一种基于环境反向散射的室内定位方法和装置
CN107356903A (zh) 基于相位差测量的无源rfid定位方法和装置
CN106873015A (zh) Css与gps相结合的室内外无缝定位系统及定位方法
CN104066175A (zh) 基于WiFi的室内定位系统及方法
CN104683949A (zh) 一种应用于无线Mesh网中基于天线阵列的混合自定位方法
CN203416427U (zh) 基于ZigBee技术的自组网定位系统
CN104569912A (zh) 一种固定监测站单站无源区域定位方法
CN105979581A (zh) 一种基于功率差的室内定位方法
Wei et al. Indoor localization method comparison: Fingerprinting and Trilateration algorithm
CN103152822A (zh) 一种基于apit的煤矿井下人员定位方法

Legal Events

Date Code Title Description
PB01 Publication
C06 Publication
SE01 Entry into force of request for substantive examination
C10 Entry into 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

Application publication date: 20150121

RJ01 Rejection of invention patent application after publication