CN116299183A - 一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统 - Google Patents

一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统 Download PDF

Info

Publication number
CN116299183A
CN116299183A CN202310589574.6A CN202310589574A CN116299183A CN 116299183 A CN116299183 A CN 116299183A CN 202310589574 A CN202310589574 A CN 202310589574A CN 116299183 A CN116299183 A CN 116299183A
Authority
CN
China
Prior art keywords
terminal
matrix
positioning
base station
estimated distance
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.)
Granted
Application number
CN202310589574.6A
Other languages
English (en)
Other versions
CN116299183B (zh
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.)
Zhikong Huzhou Intelligent System Co ltd
Zhikong Technology Huzhou Innovation Research Institute Co ltd
Original Assignee
Zhikong Huzhou Intelligent System Co ltd
Zhikong Technology Huzhou Innovation Research Institute 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 Zhikong Huzhou Intelligent System Co ltd, Zhikong Technology Huzhou Innovation Research Institute Co ltd filed Critical Zhikong Huzhou Intelligent System Co ltd
Priority to CN202310589574.6A priority Critical patent/CN116299183B/zh
Publication of CN116299183A publication Critical patent/CN116299183A/zh
Application granted granted Critical
Publication of CN116299183B publication Critical patent/CN116299183B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO 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/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
    • G01S5/22—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Abstract

本发明涉及超声定位技术领域，尤其涉及一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统，包括定位服务器、多个基站、至少一终端，定位服务器接收到各个基站发送的终端位置信息进行定位时，结合当前时刻终端的空间位置坐标计算与各个基站的当前时刻的第二估计距离，更新当前时刻的四维的雅可比矩阵，计算卡尔曼增益，结合前一时刻的变化计算观测噪声协方差矩阵和当前时刻的终端精准定位。本发明将声速校正完合整入扩展卡尔曼滤波估计超声定位的计算过程中，可以对实际环境中的声速进行在线估计，从而得到更准确的定位结果。与此同时本发明能够应用于各种场景，包括医疗，物流，工业等领域，具有广泛的应用前景。

Description

一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统
技术领域
本发明涉及超声定位技术领域，尤其涉及一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统。
背景技术
基本的定位系统由一个定位服务器、多个接收器、标签组成。标签固定在定位对象上，接收器分散安装在场所各处，并与定位服务器有通信链路，在定位系统中，标签常称为终端，接收器常称为基站。
利用超声等测距技术进行测距时，空气温度、密度等对声速有明显影响。如果基站的部署平面不与终端运动平面在同一个平面上，特别是基站部署平面都在终端运动平面的同一侧的话，那么这种影响造成的定位误差无法抵消。比如，当测距值大于实际值时，相当于定位推算得到的终端位置都会远离基站。在沙盘定位中，仿真对象（即终端）如果在平面上进行运动且略有高低起伏，而超声接收器（即基站）按现有的超声定位系统进行定位，存在定位误差大，精准度不够的技术问题。
发明内容
发明的目的是针对上述技术问题，提出一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统，本发明的目的可通过下列技术方案来实现：
本发明提供一种使用扩展卡尔曼滤波估计声速的超声定位方法，用于定位服务器通过多个基站定位终端，包括以下步骤：
在预设阶段，根据终端的空间位置坐标和预设声速校准因子
Figure SMS_1
得到表示终端初步定位的四维向量；
根据终端的空间位置坐标确认终端与基站的第一估计距离；
根据声速校准因子
Figure SMS_2
和第一估计距离确定第二估计距离并更新终端的四维雅可比矩阵；
定位服务器接收到各个基站发送的终端位置信息进行定位时，结合当前时刻终端的空间位置坐标计算与各个基站的当前时刻的第一估计距离，后计算当前时刻的第二估计距离，再更新当前时刻的四维雅可比矩阵，利用更新的四维所述雅可比矩阵计算四维矩阵的卡尔曼增益，结合前一时刻的变化计算当前时刻的卡尔曼增益和观测噪声协方差矩阵，预测出当前时刻的所述终端精准定位。
较佳地“更新四维的所述雅可比矩阵”中更新雅可比矩阵具体为：
确认终端X的估计位置与基站i的第一估计距离
Figure SMS_3
；
Figure SMS_4
，
其中，k为终端X的编号，i为基站的编号，
Figure SMS_5
是所述终端X在空间中的位置坐标，/>
Figure SMS_6
是基站i在空间中的位置坐标；
确定按照以下公式更新所述终端的雅可比矩阵
Figure SMS_7
，所述雅可比矩阵/>
Figure SMS_8
为M*4维的雅可比矩阵；
Figure SMS_9
；其中，k为终端X的编号，i为基站的编号，/>
Figure SMS_10
是终端X在空间中的位置坐标；/>
Figure SMS_11
是基站i在空间中的位置坐标。
确定第二估计距离进一步包括：
确定终端X的估计位置与基站i的第二估计距离
Figure SMS_12
，/>
Figure SMS_13
，
其中，
Figure SMS_14
为终端X的估计位置与基站i的第一估计距离/>
Figure SMS_15
为预设声速校准因子。
较佳地，对协方差矩阵在内的信息进行状态初始化：当前时刻k=0时，初始化观测噪声协方差矩阵、确定过程噪声矩阵和测量噪声矩阵在内的常数矩阵，其具体为：
初始化观测噪声协方差矩阵，设置为4x4维的单位对角阵：
Figure SMS_16
，
其中
Figure SMS_17
；
确认过程噪声矩阵，设置为4x4维的常数矩阵，单位对角阵：
Figure SMS_18
；
初始化四维向量
Figure SMS_19
，设置为：/>
Figure SMS_20
；
确认测量噪声矩阵，设置为单位对角阵：
Figure SMS_21
，所述R为M*M维的常数矩阵。
较佳的，结合当前时刻终端的空间位置坐标计算与各个基站的当前时刻的第一估计距离，后计算当前时刻的第二估计距离，更新当前时刻的四维的雅可比矩阵，进一步包括：
定位服务器接收到各个基站传送的当前时刻k中所述终端的空间坐标信息，并获得各个基站对应的每一基站空间坐标信息；计算
Figure SMS_22
、第二估计距离/>
Figure SMS_23
和雅可比矩阵/>
Figure SMS_24
。
结合前一时刻的变化更新当前时刻的卡尔曼增益和观测噪声协方差矩阵，预测出当前时刻的所述终端精准定位进一步包括：
计算当前时刻k的卡尔曼增益，
Figure SMS_25
，所述/>
Figure SMS_26
为上一时刻K-1时的观测噪声协方差矩阵，R为测量噪声矩阵，/>
Figure SMS_27
为上一时刻k-1时的雅可比矩阵；
计算时刻k的观测噪声协方差矩阵：
Figure SMS_28
，/>
Figure SMS_29
为观测噪声协方差矩阵的单位矩阵，/>
Figure SMS_30
为时刻k时的卡尔曼增益，/>
Figure SMS_31
为当前时刻k的雅可比矩阵，Q为所述过程噪声矩阵；
计算当前时刻k终端精准估计位置，
Figure SMS_32
，/>
Figure SMS_33
为上一时刻k-1时终端X的精准估计位置，/>
Figure SMS_34
为时刻k的卡尔曼增益，/>
Figure SMS_35
为时刻k时的测距结果；/>
Figure SMS_36
为上一时刻k-1时的第二估计距离。
较佳的，还包括：K=K+1,重新计算下一个时刻的终端X的精准定位：进行结合前一时刻(k-1)的变化预测当前时刻k的终端X的位置信息和协方差矩阵，更新当前时刻k的卡尔曼增益, 得到当前时刻k的终端X的精准定位。
较佳的，定位服务器接收到各个所述基站传送当前时刻k中所述终端的空间坐标信息，并获得各个所述基站对应的每一所述基站空间坐标信息之后，还包括：定位服务器对基站共线的判断，如果基站共线，k=k+1，计算下一个时刻的终端X的精准定位。
本发明还提供了一种使用扩展卡尔曼滤波估计声速的超声定位系统，包括定位服务器、多个基站、至少一终端，进一步包括：
初定位预定单元：用于根据终端的空间位置坐标和预设声速校准因子
Figure SMS_37
得到表示终端初步定位的四维向量；
估计距离确定单元：根据终端的空间位置坐标确认终端与基站的第一估计距离；根据声速校准因子
Figure SMS_38
和第一估计距离确定第二估计距离；
观测信息确定单元，根据第二估计距离，更新观测信息，观测信息包括四维的雅可比矩阵；
定位计算单元：用于结合当前时刻终端的空间位置坐标计算与各个基站的当前时刻的第一估计距离，计算当前时刻的第二估计距离，更新当前时刻的雅可比矩阵，利用更新四维的雅可比矩阵计算四维矩阵的卡尔曼增益，结合前一时刻的变化计算观测噪声协方差矩阵和当前时刻的终端精准定位。
较佳的，定位服务器还包括：接收单元，用于接收基站传送当前时刻终端的空间坐标信息和各个基站对应的每一基站空间坐标信息。
与现有技术相比，本发明存在以下至少一种技术效果：
与传统的定位系统相比，使用扩展卡尔曼滤波算法能够更准确的处理好非线性问题，通过引入声速校正因子作为状态变量，更新观测信息，包括更新M*4维的雅可比矩阵，再利用更新M*4维的雅可比矩阵计算四维矩阵的卡尔曼增益，根据第二估计距离、雅可比矩阵和卡尔曼增益，结合前一时刻的变化计算当前时刻观测噪声协方差矩阵，后结合第二估计距离计算当前时刻的所述终端精准定位信息，整个过程把声速校正完合整入扩展卡尔曼滤波估计超声定位的计算过程中，特别是终端在略有高低起伏的平面上运行，可以对实际环境中的声速进行在线估计，从而得到更准确的定位结果。与此同时本发明能够应用于各种场景，包括医疗，物流，工业等领域，具有广泛的应用前景。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单的介绍：
图1为沙盘中仿真货车进行定位的系统架构图；
图2为沙盘中仿真货车进行定位的系统工作示意图；
图3为本发明一种使用扩展卡尔曼滤波估计声速的超声定位方法的流程图；
图4为本发明一种使用扩展卡尔曼滤波估计声速的超声定位方法的一种定位估算部分的示例图；
图5为一种使用扩展卡尔曼滤波估计声速的超声定位系统的工作原理图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附图及具体实施方式,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施方式仅用以解释本发明,并不用于限定本发明。
对于基本的定位系统，一般是由一个定位服务器、多个接收器以及标签组成的。标签固定在需要定位的对象上，接收器分散安装在场所各处，并在接收器和定位服务器直接建立有通信链路。
常见的高精度定位系统包括GPS系统、UWB定位系统等，它们的特点是接收器部署密度比较低、一维测距精度不容易受环境因素影响。还有一种定位系统，它有比较高的接收器部署密度，但一维测距精度受环境影响比较大，典型的例子是沙盘上的仿真货车定位系统。
对于沙盘上的仿真货车定位系统，沙盘安装在教室固定位置上，沙盘的长宽一般不超过5米，离天花板高度不超过3米。沙盘上有一些道路、停车场、装卸货点等仿真场所，有几十辆仿真货车在上面行驶。在每个仿真货车上都安装了一个标签，在沙盘上方的教室天花板上安装了几十个接收器。这些接收器都通过通信链路，比如2.4G无线铜线、485或以太网等，连接到定位服务器上。
这种沙盘可以对所有仿真货车在沙盘上的二维平面或三维空间位置进行低成本的厘米级高精度定位，最成熟的技术是超声测距。具体来说，仿真货车上的标签采用电池供电，标签带有无线接收器和超声发射器。天花板上的接收器带有超声接收器。定位服务器带有一个无线发射器。定位服务器通过无线发射器，按照预定顺序、周期性的给每个标签发送定位命令；当一个标签通过无线接收器接收到定位命令后，就通过标签的超声发射器发射超声定位信号；这个超声定位信号将被多个安装在天花板上的接收器的超声接收器接收到，最后标签将超声定位信号处理后转发给定位服务器进行定位。以一个对沙盘中一个仿真货车进行定位为例，图1为对沙盘中仿真货车进行定位的系统架构图。如图2所示，为对沙盘中仿真货车进行定位的系统工作示意图。
对于超声定位信号从仿真货车上的标签中发射出来到被一个接收器接收到的时间差，与该标签与接收器的距离成正比。空气中声波的传输速度为366米/秒。如果是3米距离，那么有8.196毫秒（即3/366）左右的时间差。超声定位信号一般都采用40KHz，所以测距分辨率可以达到0.915厘米（即366/40K）。
在沙盘定位中，仿真货车（即终端）基本在平面上运行，略有高低起伏，而超声接收器（即基站）都部署在货车运动平面的同一侧（即天花板）。如果直接使用扩展卡尔曼一般方法进行定位，忽视声速不一定是340米/秒等问题的话，定位误差就会增加。使用超声定位系统，业内一般只能做到10厘米左右的定位精度。
我司发明人考虑到如何提升定位精度，就想到把把声速估算进去，提升定位精度，但是如何将使用声速估算和扩展卡尔曼滤波计算完合整合在一起，是本发明的难点，为此，本申请人经过多次思考后整理出对应的方案。
请参阅图3，其为一种使用扩展卡尔曼滤波估计声速的超声定位方法的流程图，它用于定位服务器通过多个基站定位终端，包括以下步骤：
（一）预设阶段部分
S110:根据终端的空间位置坐标和预设声速校准因子
Figure SMS_39
得到表示终端初步定位的四维向量；
S120:根据终端的空间位置坐标确认终端与基站的第一估计距离；
S130:根据声速校准因子
Figure SMS_40
和第一估计距离确定第二估计距离，更新终端的四维雅可比矩阵；
S140:根据M*4维的雅可比矩阵更新四维矩阵的卡尔曼增益，根据卡尔曼增益和第二估计距离更新所述终端精准定位估计公式；
（二）定位估算部分
S150:定位服务器接收到各个基站发送的终端位置信息进行定位时，结合当前时刻终端的空间位置坐标计算与各个基站的当前时刻的第一估计距离，计算当前时刻的第二估计距离和更新当前时刻的雅可比矩阵，利用更新四维的雅可比矩阵计算四维矩阵的卡尔曼增益，结合前一时刻的变化计算观测噪声协方差矩阵和当前时刻的所述终端精准定位。
具体介绍每个部分。
1、确定第二估计距离可以为：
确定所述终端X的估计位置与所述基站i的第二估计距离
Figure SMS_41
，/>
Figure SMS_42
，
其中，
Figure SMS_43
为终端X的估计位置与基站i的第一估计距离，/>
Figure SMS_44
为预设声速校准因子。
2、更新四维的雅可比矩阵可以具体为：
第一步：确认终端X的估计位置与基站i的第一估计距离
Figure SMS_45
；
Figure SMS_46
，
其中，k为终端X的编号，i为基站的编号，
Figure SMS_47
是所述终端X在空间中的位置坐标，/>
Figure SMS_48
是基站i在空间中的位置坐标；
第二步：确定按照以下公式更新所述终端的雅可比矩阵
Figure SMS_49
，所述雅可比矩阵/>
Figure SMS_50
为M*4维的雅可比矩阵；
Figure SMS_51
；其中，k为终端X的编号，i为基站的编号，所述/>
Figure SMS_52
是所述终端X在空间中的位置坐标；所述/>
Figure SMS_53
是基站i在空间中的位置坐标。
3、更新四维矩阵卡尔曼增益，
Figure SMS_54
，所述/>
Figure SMS_55
为上一时刻K-1时的所述观测噪声协方差矩阵，所述R为所述测量噪声矩阵，所述/>
Figure SMS_56
为上一时刻k-1时的雅可比矩阵；
4、更新观测噪声协方差矩阵：
Figure SMS_57
，所述/>
Figure SMS_58
为所述观测噪声协方差矩阵的单位矩阵，所述/>
Figure SMS_59
为所述时刻k时的所述卡尔曼增益，所述/>
Figure SMS_60
为当前时刻k的雅可比矩阵，所述Q为所述过程噪声矩阵；
5、更新终端精准估计位置，
Figure SMS_61
，所述/>
Figure SMS_62
为上一时刻k-1时所述终端X的精准估计位置，所述/>
Figure SMS_63
为所述时刻k的所述卡尔曼增益，/>
Figure SMS_64
为所述时刻k时的测距结果；所述/>
Figure SMS_65
为上一时刻k-1时的第二估计距离。
上述更新计算过程是本申请人经过多次思考把声波因素考虑后进行的调整，这个调整并非是唯一的。终端精准估计位置的参考因素，不仅考虑到根据终端的三维坐标信息计算第二估计距离，而且也考虑到放入声速校准因子
Figure SMS_66
影响后计算的四维的雅可比矩阵，从后续的仿真效果来看，效果非常好。
具体介绍定位估算部分。
先对包括协方差矩阵在内的信息进行状态初始化（在状态初始化过程中，没有先后顺序）：当前时刻k=0时，初始化观测噪声协方差矩阵，确定过程噪声矩阵和测量噪声矩阵在内的常数矩阵，其具体为：
初始化所述观测噪声协方差矩阵，设置为4x4维的单位对角阵：
Figure SMS_67
，
其中
Figure SMS_68
；
确认所述过程噪声矩阵，设置为4x4维的常数矩阵，单位对角阵：
Figure SMS_69
；初始化所述四维向量/>
Figure SMS_70
，设置为：/>
Figure SMS_71
；
确认所述测量噪声矩阵，设置为单位对角阵：
Figure SMS_72
，所述R为M*M维的常数矩阵。
后结合当前时刻终端的空间位置坐标计算与各个基站的当前时刻的第一估计距离，后计算当前时刻的第二估计距离，再计算当前时刻的雅可比矩阵，其进一步包括：
定位服务器接收到该些基站传送当前时刻k中终端的空间坐标信息，并获得该些基站对应的每一基站空间坐标信息；
计算
Figure SMS_73
、第二估计距离/>
Figure SMS_74
和雅可比矩阵/>
Figure SMS_75
。
利用更新四维的雅可比矩阵计算四维矩阵的卡尔曼增益，结合前一时刻的变化计算观测噪声协方差矩阵和当前时刻的所述终端精准定位进一步包括：
计算当前时刻k的所述卡尔曼增益，
Figure SMS_76
，所述/>
Figure SMS_77
为上一时刻K-1时的所述观测噪声协方差矩阵，所述R为所述测量噪声矩阵，所述/>
Figure SMS_78
为上一时刻k-1时的所述雅可比矩阵；
计算所述时刻k的所述观测噪声协方差矩阵：
Figure SMS_79
，所述
Figure SMS_80
为所述观测噪声协方差矩阵的单位矩阵，所述/>
Figure SMS_81
为所述时刻k时的所述卡尔曼增益，所述/>
Figure SMS_82
为当前时刻k的雅可比矩阵，所述Q为所述过程噪声矩阵；
计算当前时刻k终端精准估计位置，
Figure SMS_83
，所述/>
Figure SMS_84
为上一时刻k-1时所述终端X的精准估计位置，所述/>
Figure SMS_85
为所述时刻k的所述卡尔曼增益，/>
Figure SMS_86
为所述时刻k时的测距结果；所述/>
Figure SMS_87
为上一时刻k-1时的第二估计距离。
简单介绍
Figure SMS_88
将每一条测距结果的记录按照{接收器1：测距值1；接收器2：测距值2；接收器3：测距值3......}的格式转换成一条JSON字符串的形式存储与一行的测距结果队列。这样设计的好处在于，对于接收器主要是指基站，其的数量可扩展，只需要修改JSON字符串中包含的项就可以了。无需在接收器的数量以及型号发生变化时，重新定义测距结果队列。
具体实例一
请参阅图4其为一种定位估算部分的示例图。
图4为状态初始化和递归计算时刻K的终端X的位置状态和协方差矩阵，更新时刻k时的卡尔曼增益步骤流程图。
在使用卡尔曼滤波算法进行超声定位时，对状态信息、协方差矩阵，进行状态初始化（在状态初始化过程中，没有先后顺序）；即当时刻k=0时，状态初始化，协方差矩阵包括观测噪声协方差矩阵、过程噪声矩阵、测量噪声矩阵，具体操作如下：
步骤S11：初始化观测噪声协方差矩阵，设置为4x4维的单位对角阵：
Figure SMS_89
，其中/>
Figure SMS_90
；
步骤S12：初始化过程噪声矩阵，设置为4x4维的常数矩阵，单位对角阵：
Figure SMS_91
；
步骤S13：初始化终端X的估计位置与声速校准因子
Figure SMS_92
构成的四维向量/>
Figure SMS_93
，设置为：
Figure SMS_94
；
步骤S14：初始化测量噪声矩阵，设置为单位对角阵：
Figure SMS_95
，R为M*M维的常数矩阵。
结合前一时刻的变化更新当前时刻的卡尔曼增益和观测噪声协方差矩阵，预测出当前时刻的所述终端精准定位，具体操作如下；
步骤S21：设置初始时刻k=1；
步骤S22：获取时刻k的测距结果
Figure SMS_96
；
步骤S23：判断测距结果
Figure SMS_97
所有非0值对应的基站，是否在X-Y平面共线；
步骤S24：当所有基站共线，时刻k=k+1，跳转到步骤S29；当所有基站不共线时，继续操作下一步；
步骤S25：计算时刻k的卡尔曼增益，卡尔曼增益为一个4*M维矩阵：
Figure SMS_98
，/>
Figure SMS_99
为时刻K-1时的观测噪声协方差矩阵，R为测量噪声矩阵，/>
Figure SMS_100
为时刻k-1时的雅可比矩阵；
步骤S26：计算时刻k的观测噪声协方差矩阵：
Figure SMS_101
，/>
Figure SMS_102
为观测噪声协方差矩阵的单位矩阵，/>
Figure SMS_103
为时刻k时的卡尔曼增益，/>
Figure SMS_104
为时刻k的雅可比矩阵，Q为过程噪声矩阵；
步骤S27：计算时刻k的终端X精准估计位置，
Figure SMS_106
，当出现实际测量距离/>
Figure SMS_108
，那么/>
Figure SMS_112
，/>
Figure SMS_107
为时刻k-1时终端X的估计位置，/>
Figure SMS_109
为时刻k时的卡尔曼增益，/>
Figure SMS_111
为时刻k时，测距结果/>
Figure SMS_113
；/>
Figure SMS_105
为k-1时的估计测量距离/>
Figure SMS_110
；
步骤S28，判断当前时刻k是否是最后的值，如果是则进行步骤S29，如果否则返回步骤S52，继续递归计算。
步骤S29：k=k+1。
步骤S24中，若当前所述测距结果中所有非0值对应的基站，在X-Y平面不共线，是否在X-Y平面共线。比如，有3个非零测距结果，对应的3个基站在X-Y平面上的投影形成一条线，那么就无法算得唯一的位置。另外，这里的“共线”不是说必须严格成直线，只要大致形成直线，就可能导致无法得到定位结果。只有在X-Y平面上不共线的若干个测距结果才能够计算出终端的坐标和声速校正因子。
实施例2
本发明还提供了一种使用扩展卡尔曼滤波估计声速的超声定位系统，包括定位对象，定位服务器和若干个基站。图5为一种使用扩展卡尔曼滤波估计声速的超声定位系统的工作原理图。
定位服务器通过通信总线分别与无线发射器，若干个基站连接。
定位服务器，包括一个无线发射器，用于向定位对象上的终端发送定位命令，并且定位服务器进一步包括：
初定位预定单元：用于根据终端的空间位置坐标和预设声速校准因子
Figure SMS_114
得到表示终端初步定位的四维向量；
估计距离确定单元：根据所述终端的空间位置坐标确认所述终端与所述基站的第一估计距离；根据所述声速校准因子所述第一估计距离确定第二估计距离；
观测信息确定单元，根据所述第二估计距离，更新观测信息，所述观测信息包括四维的雅可比矩阵；
定位计算单元：用于结合当前时刻终端的空间位置坐标计算与各个基站的当前时刻的第一估计距离，计算当前时刻的第二估计距离和更新当前时刻的雅可比矩阵，利用更新四维的雅可比矩阵计算四维矩阵的卡尔曼增益，结合前一时刻的变化计算观测噪声协方差矩阵和当前时刻的所述终端精准定位。
初定位预定单元、估计距离确定单元和观测信息确定单元可以设置在存储器或处理器的缓冲中，而定位计算单元可以用控制器来实现。
定位服务器还包括：
接收单元：用于接收基站传送当前时刻k中终端的空间坐标信息和该些基站对应的每一基站空间坐标信息
定位服务器按照预定顺序，周期性的给每个标签发送定位命令，可以通过方法进行：
（1）确定标签列表：首先，确定所有需要发送定位命令的标签。创建一个包含所有标签的列表或数据库。
（2）制定发送顺序：根据需求，制定标签发送命令的顺序。可以按照标签的编号、优先级或其他属性来确定发送顺序。
（3）设置发送周期：确定发送定位命令的周期。例如，每隔一定时间间隔发送一次命令。
（4）编写定位命令：根据定位系统协议或通信协议，编写相应的定位命令。命令内容可能包括标签的标识信息、定位指令和其他必要的参数。
（5）循环发送命令：在定位服务器中设置一个循环，按照预定的顺序周期性地给每个标签发送定位命令。可以使用编程语言或脚本来实现循环发送命令的逻辑。
（6）监控响应和超时：在发送命令后，定位服务器需要等待标签的响应。设置适当的超时机制来处理标签未响应的情况，并进行相应的处理，例如重试或标记为无响应。
（7）更新命令顺序：如果需要在未来更改标签的发送顺序，可以根据需要进行更新。
对于接收器对定位信号进行处理后生成标签到所述接收器的测距值，可以通过以下步骤进行（仅举例，实际运行时任意一种生成测距值的方法都行）：
（1）接收定位信号：接收器接收来自标签的定位信号。这些信号可以是无线信号（如无线电频率、超高频（UHF）信号、蓝牙等）或其他类型的信号（如红外线、声波等），具体取决于定位系统的技术。
（2）信号处理：接收器对接收到的定位信号进行处理。这包括信号解调、滤波、放大或其他信号处理步骤，以提取有用的定位信息。
（3）时差测量：通过测量接收到的定位信号与参考信号之间的时差，接收器可以计算出标签到接收器的测距值。时差测量可以通过各种方式实现，例如使用时间差测量（Timeof Flight，TOF）或接收信号强度指示（Received Signal Strength Indicator，RSSI）等。
（4）测距计算：根据时差测量结果和定位系统的特定算法，接收器可以计算出标签到接收器的测距值。这可能涉及距离与时差之间的数学关系或其他定位算法，如多普勒效应或三角定位等。
（5）输出测距值：接收器将计算得到的标签到接收器的测距值输出给定位系统或其他应用程序。这些测距值可以用于后续的定位计算、位置估计或其他定位相关的任务。
本发明定位计算单元一般集成在测距计算中完成定位。
本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

Claims (10)

1.一种使用扩展卡尔曼滤波估计声速的超声定位方法，用于定位服务器通过多个基站定位终端，其特征在于，包括以下步骤：
在预设阶段，根据所述终端的空间位置坐标和预设声速校准因子
Figure QLYQS_1
得到表示所述终端初步定位的四维向量；
根据所述终端的空间位置坐标确认所述终端与所述基站的第一估计距离；
根据所述声速校准因子
Figure QLYQS_2
和所述第一估计距离确定第二估计距离，更新所述终端的四维雅可比矩阵；
所述定位服务器接收到各个所述基站发送的终端位置信息进行定位时，结合当前时刻所述终端的空间位置坐标计算与各个所述基站的当前时刻的所述第一估计距离，后计算当前时刻的所述第二估计距离，更新当前时刻的四维的所述雅可比矩阵，利用更新的四维所述雅可比矩阵计算四维矩阵的卡尔曼增益，结合前一时刻的变化计算观测噪声协方差矩阵和当前时刻的所述终端精准定位。
2.由权利要求1所述的使用扩展卡尔曼滤波估计声速的超声定位方法，其特征在于，更新四维的所述雅可比矩阵具体为：
确认终端X的估计位置与所述基站i的第一估计距离
Figure QLYQS_3
；
Figure QLYQS_4
，
其中，所述k为所述终端X的编号，所述i为所述基站的编号，所述
Figure QLYQS_5
是所述终端X在空间中的位置坐标，所述/>
Figure QLYQS_6
是所述基站i在空间中的位置坐标；
确定按照以下公式更新所述终端X的雅可比矩阵
Figure QLYQS_7
，所述雅可比矩阵/>
Figure QLYQS_8
为M*4维的雅可比矩阵；
Figure QLYQS_9
；其中，所述k为所述终端X的编号，所述i为所述基站的编号，所述/>
Figure QLYQS_10
是所述终端X在空间中的位置坐标；所述
Figure QLYQS_11
是基站i在空间中的位置坐标。
3.由权利要求2所述的使用扩展卡尔曼滤波估计声速的超声定位方法，其特征在于：确定所述第二估计距离进一步包括：
确定所述终端X的估计位置与所述基站i的所述第二估计距离
Figure QLYQS_12
，/>
Figure QLYQS_13
；其中，所述/>
Figure QLYQS_14
为所述终端X的估计位置与所述基站i的所述第一估计距离，所述/>
Figure QLYQS_15
为预设所述声速校准因子。
4.由权利要求3所述的使用扩展卡尔曼滤波估计声速的超声定位方法，其特征在于，还包括对协方差矩阵在内的信息进行状态初始化：当前时刻k=0时，初始化所述观测噪声协方差矩阵、确定过程噪声矩阵和测量噪声矩阵在内的常数矩阵，其具体为：
初始化所述观测噪声协方差矩阵，设置为4x4维的单位对角阵：
Figure QLYQS_16
，
其中
Figure QLYQS_17
；
确认所述过程噪声矩阵，设置为4x4维的常数矩阵，单位对角阵：
Figure QLYQS_18
；
初始化所述四维向量
Figure QLYQS_19
，设置为：/>
Figure QLYQS_20
；
确认所述测量噪声矩阵，设置为单位对角阵：
Figure QLYQS_21
，所述R为M*M维的常数矩阵。
5.由权利要求4所述的使用扩展卡尔曼滤波估计声速的超声定位方法，其特征在于，结合当前时刻所述终端的空间位置坐标计算与各个所述基站的当前时刻的所述第一估计距离，后计算当前时刻的所述第二估计距离，更新当前时刻的四维的所述雅可比矩阵，进一步包括：
所述定位服务器接收到各个所述基站传送的当前时刻k中所述终端的空间坐标信息，并获得各个所述基站对应的每一所述基站空间坐标信息；计算所述所述第一估计距离
Figure QLYQS_22
、所述第二估计距离/>
Figure QLYQS_23
和所述雅可比矩阵/>
Figure QLYQS_24
。
6.由权利要求1或5所述的使用扩展卡尔曼滤波估计声速的超声定位方法，其特征在于，利用更新四维的所述雅可比矩阵计算四维矩阵的所述卡尔曼增益，结合前一时刻的变化计算所述观测噪声协方差矩阵和当前时刻的所述终端精准定位进一步包括：
计算当前时刻k的所述卡尔曼增益，
Figure QLYQS_25
，所述/>
Figure QLYQS_26
为上一时刻K-1时的所述观测噪声协方差矩阵，所述R为所述测量噪声矩阵，所述/>
Figure QLYQS_27
为上一时刻k-1时的所述雅可比矩阵；
计算所述时刻k的所述观测噪声协方差矩阵：
Figure QLYQS_28
，所述/>
Figure QLYQS_29
为所述观测噪声协方差矩阵的单位矩阵，所述/>
Figure QLYQS_30
为所述时刻k时的所述卡尔曼增益，所述/>
Figure QLYQS_31
为当前时刻k的所述雅可比矩阵，所述Q为所述过程噪声矩阵；
计算当前时刻k所述终端精准估计位置，
Figure QLYQS_32
，所述/>
Figure QLYQS_33
为上一时刻k-1时所述终端X的精准估计位置，所述/>
Figure QLYQS_34
为所述时刻k的所述卡尔曼增益，所述/>
Figure QLYQS_35
为所述时刻k时的测距结果；所述/>
Figure QLYQS_36
为上一时刻k-1时的所述第二估计距离。
7.由权利要求6所述的使用扩展卡尔曼滤波估计声速的超声定位方法，其特征在于，还包括：K=K+1,重新计算下一个时刻的所述终端X的精准定位：进行结合前一时刻(k-1)的变化预测当前时刻k的所述终端X的位置信息和所述协方差矩阵，更新当前时刻k的卡尔曼增益, 得到当前时刻k的所述终端X的精准定位。
8.由权利要求6所述的使用扩展卡尔曼滤波估计声速的超声定位方法，所述定位服务器接收到各个所述基站传送当前时刻k中所述终端的空间坐标信息，并获得各个所述基站对应的每一所述基站空间坐标信息之后，还包括：
所述定位服务器对所述基站共线的判断，如果所述基站共线，k=k+1，计算下一个时刻的所述终端X的精准定位。
9.一种使用扩展卡尔曼滤波估计声速的超声定位系统，包括定位服务器、多个基站、至少一终端，其特征在于，所述定位服务器进一步包括：
初定位预定单元：用于根据所述终端的空间位置坐标和预设声速校准因子
Figure QLYQS_37
得到表示所述终端初步定位的四维向量；
估计距离确定单元：根据所述终端的空间位置坐标确认所述终端与所述基站的第一估计距离；根据所述声速校准因子
Figure QLYQS_38
和所述第一估计距离确定第二估计距离；
观测信息确定单元，根据所述第二估计距离，更新观测信息，所述观测信息包括四维的雅可比矩阵；
定位计算单元：用于结合当前时刻所述终端的空间位置坐标计算与各个所述基站的当前时刻的所述第一估计距离，计算当前时刻的所述第二估计距离，更新当前时刻的所述雅可比矩阵，利用更新四维的所述雅可比矩阵计算四维矩阵的卡尔曼增益，结合前一时刻的变化计算观测噪声协方差矩阵和当前时刻的所述终端精准定位。
10.由权利要求9所述的一种使用扩展卡尔曼滤波估计声速的超声定位系统，其特征在于，定位服务器还包括：
接收单元：用于接收所述基站传送当前时刻所述终端的空间坐标信息和各个所述基站对应的每一所述基站空间坐标信息。
CN202310589574.6A 2023-05-24 2023-05-24 一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统 Active CN116299183B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310589574.6A CN116299183B (zh) 2023-05-24 2023-05-24 一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310589574.6A CN116299183B (zh) 2023-05-24 2023-05-24 一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统

Publications (2)

Publication Number Publication Date
CN116299183A true CN116299183A (zh) 2023-06-23
CN116299183B CN116299183B (zh) 2023-08-15

Family

ID=86783641

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310589574.6A Active CN116299183B (zh) 2023-05-24 2023-05-24 一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统

Country Status (1)

Country Link
CN (1) CN116299183B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101390776B1 (ko) * 2013-03-14 2014-04-30 인하대학교 산학협력단 퍼지 확장 칼만 필터를 이용한 위치인식 장치, 방법 및 이동로봇
CN108613679A (zh) * 2018-06-14 2018-10-02 河北工业大学 一种移动机器人扩展卡尔曼滤波同步定位与地图构建方法
CN109302690A (zh) * 2018-09-30 2019-02-01 桂林电子科技大学 一种基于优化卡尔曼滤波的非视距室内定位方法
CN114152910A (zh) * 2020-09-07 2022-03-08 罗维智联(北京)科技有限公司 一种基于主被动基站信息融合的定位方法及系统
WO2022087998A1 (zh) * 2020-10-30 2022-05-05 网络通信与安全紫金山实验室 通信终端的定位跟踪方法、系统、设备和可读存储介质

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101390776B1 (ko) * 2013-03-14 2014-04-30 인하대학교 산학협력단 퍼지 확장 칼만 필터를 이용한 위치인식 장치, 방법 및 이동로봇
CN108613679A (zh) * 2018-06-14 2018-10-02 河北工业大学 一种移动机器人扩展卡尔曼滤波同步定位与地图构建方法
CN109302690A (zh) * 2018-09-30 2019-02-01 桂林电子科技大学 一种基于优化卡尔曼滤波的非视距室内定位方法
CN114152910A (zh) * 2020-09-07 2022-03-08 罗维智联(北京)科技有限公司 一种基于主被动基站信息融合的定位方法及系统
WO2022087998A1 (zh) * 2020-10-30 2022-05-05 网络通信与安全紫金山实验室 通信终端的定位跟踪方法、系统、设备和可读存储介质

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
刘明佳;骆曦;刘志新;: "基于ToA技术与扩展卡尔曼滤波的室内无线定位算法设计", 舰船电子工程, no. 11, pages 87 - 91 *

Also Published As

Publication number Publication date
CN116299183B (zh) 2023-08-15

Similar Documents

Publication Publication Date Title
US11670121B2 (en) Systems and methods for monitoring and reporting road quality
CN109548141B (zh) 基于卡尔曼滤波算法的室内环境基站坐标位置标定方法
KR102593435B1 (ko) 공간 내의 태그들을 로케이팅하기 위한 시스템들 및 방법들
US20160316325A1 (en) Location Based Services for RFID and Sensor Networks
CN103841642A (zh) 一种三维室内定位方法
US20160091341A1 (en) Method and apparatus for object localization
WO2017121168A1 (zh) 簇式磁场定位的方法、装置和系统
KR100977246B1 (ko) 순방향 링크 ａｏａ를 이용한 위치 추정 장치 및 방법
JP7155284B2 (ja) 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体
US11106837B2 (en) Method and apparatus for enhanced position and orientation based information display
JP2015219089A (ja) 無線測位装置、無線測位用地図データベース
CN113759349B (zh) 激光雷达与定位设备的标定方法、设备及自动驾驶车辆
JP2017072423A (ja) 推定装置、制御方法、プログラム及び記憶媒体
CN116299183B (zh) 一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统
KR101106265B1 (ko) Ｒｆｉｄ를 이용한 이동 로봇의 위치측정 장치 및 위치측정 방법
US20160091349A1 (en) Method and apparatus for object localizing
CN115103299B (zh) 一种基于rfid的多传感器融合定位方法
CN111563934A (zh) 单目视觉里程计尺度确定方法和装置
US20220261008A1 (en) Circuitry and method
Parashar et al. Particle filter based localization of access points using direction of arrival on mobile robots
US20210374300A1 (en) Method and apparatus for improved position and orientation based information display
WO2019107237A1 (ja) 位置推定装置、位置推定方法とプログラムを格納する記録媒体、並びに、位置推定システム
CN116299184B (zh) 一种基于非线性优化的定位方法及系统
CN113218392A (zh) 室内定位导航方法及导航装置
JP6603122B2 (ja) 情報処理装置、記録方法、及びプログラム

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant