CN116299184A - 一种基于非线性优化的定位方法及系统 - Google Patents

一种基于非线性优化的定位方法及系统 Download PDF

Info

Publication number
CN116299184A
CN116299184A CN202310590034.XA CN202310590034A CN116299184A CN 116299184 A CN116299184 A CN 116299184A CN 202310590034 A CN202310590034 A CN 202310590034A CN 116299184 A CN116299184 A CN 116299184A
Authority
CN
China
Prior art keywords
ranging
ranging result
positioning
receivers
tag
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
CN202310590034.XA
Other languages
English (en)
Other versions
CN116299184B (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 Technology Huzhou Innovation Research Institute Co ltd
Zhikong Huzhou Intelligent System Co ltd
Original Assignee
Zhikong Technology Huzhou Innovation Research Institute Co ltd
Zhikong Huzhou Intelligent System 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 Technology Huzhou Innovation Research Institute Co ltd, Zhikong Huzhou Intelligent System Co ltd filed Critical Zhikong Technology Huzhou Innovation Research Institute Co ltd
Priority to CN202310590034.XA priority Critical patent/CN116299184B/zh
Publication of CN116299184A publication Critical patent/CN116299184A/zh
Application granted granted Critical
Publication of CN116299184B publication Critical patent/CN116299184B/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/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/12—Simultaneous equations, e.g. systems of linear equations
    • 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

本发明涉及系统定位技术领域，提供了一种基于非线性优化的定位方法，包括：S1：建立测距结果队列，并初始化为空；S2：尝试获取一个测距结果，测距结果为任意一个标签向N个接收器发射定位后计算得到N个测距值的矢量集合；S3：若当前测距结果中所有非0值对应的接收器，在X‑Y平面不共线，将测距结果添加到测距结果队列尾部，根据预先建立的目标函数，采用非线性最优化函数根据目标函数计算测距结果队列中每个测距结果对应的标签的最优的坐标和声速校正因子。使用非线性最优化方法，通过动态估计环境中空气的实际声速，以及增加每次定位的测距数量，提高了整体系统定位精度。

Description

一种基于非线性优化的定位方法及系统
技术领域
本发明涉及系统定位的技术领域，尤其涉及一种基于非线性优化的定位方法及系统。
背景技术
对于基本的定位系统，一般是由一个定位服务器、多个接收器以及标签组成的。标签固定在需要定位的对象上，接收器分散安装在场所各处，并在接收器和定位服务器直接建立有通信链路。
常见的高精度定位系统包括GPS系统、UWB定位系统等，它们的特点是接收器部署密度比较低、一维测距精度不容易受环境因素影响。还有一种定位系统，它有比较高的接收器部署密度，但一维测距精度受环境影响比较大，典型的例子是沙盘上的仿真货车定位系统。
对于沙盘上的仿真货车定位系统，沙盘安装在教室固定位置上，沙盘的长宽一般不超过5米，离天花板高度不超过3米。沙盘上有一些道路、停车场、装卸货点等仿真场所，有几十辆仿真货车在上面行驶。在每个仿真货车上都安装了一个标签，在沙盘上方的教室天花板上安装了几十个接收器。这些接收器都通过通信链路，比如2.4G无线铜线、485或以太网等，连接到定位服务器上。
这种沙盘可以对所有仿真货车在沙盘上的二维平面或三维空间位置进行低成本的厘米级高精度定位，最成熟的技术是超声测距。具体来说，仿真货车上的标签采用电池供电，标签带有无线接收器和超声发射器。天花板上的接收器带有超声接收器。定位服务器带有一个无线发射器。定位服务器通过无线发射器，按照预定顺序、周期性的给每个标签发送定位命令；当一个标签通过无线接收器接收到定位命令后，就通过标签的超声发射器发射超声定位信号；这个超声定位信号将被多个安装在天花板上的接收器的超声接收器接收到，最后标签将超声定位信号处理后转发给定位服务器进行定位。如图1所示，为对沙盘中一个仿真货车进行定位的示意图，下面的球体代表仿真货车，上面的球体阵列代表接收器。如图2所示，为对沙盘中仿真货车进行定位的系统架构图。如图3所示，为对沙盘中仿真货车进行定位的系统工作示意图。
对于超声定位信号从仿真货车上的标签中发射出来到被一个接收器接收到的时间差，与该标签与接收器的距离成正比。空气中声波的传输速度为366米/秒。如果是3米距离，那么有8.196毫秒（即3/366）左右的时间差。超声定位信号一般都采用40KHz，所以测距分辨率可以达到0.915厘米（即366/40K）。
现有技术中，在通过接收器获取到标签的超声定位信号之后，一般通过三角定位法对标签所在仿真货车的实际位置进行求解。三角定位法的原理为：
如图4所示，平面上有三个不共线的接收器A、B、C，和一个未知的对设置于仿真货车上的标签M，并已测出三个接收器到标签M的距离分别为d1，d2,d3，则以三个接收器的坐标为圆心，三个接收器到未知的标签距离为半径可以画出三个相交的圆，如图4所示，未知的标签的坐标即为三个圆的相交点。
在实际测量中，往往由于测量的误差，使三个圆并不交于一点，而相交于一块区域，如图5所示，在此种情况下需要采用最小二乘法进行求解。
对于有n个接收器的定位系统，建立方程，得到如下方程组：
Figure SMS_1
，
其中，（x1,y1）...（xn,yn）分别为n个接收器的坐标值，d1...dn为n个接收器到某一个标签的距离。用上述的前n-1个方程一次减去第n个方程，可以得到如下的矩阵表示形式：AX=b，
其中：
Figure SMS_2
，
对其采用最小二乘法求解，则是求AX=b二范数的最小值，即：
Figure SMS_3
，
得：
Figure SMS_4
求得的X便是未知的标签的坐标计算值。
三角定位法的优点是很自然就可以推广到三维定位（在距离公式中增加z轴坐标即可），而且可以不局限于只使用3个接收器或3个定位信号，接收器的个数n大于3的情况下，三角定位计算公式依然成立。但是三角定位法也存在几个问题：
（1）隐含假设所有测距值的误差分布都是正态分布，而且方差相同，但许多测距技术，比如超声、RSSI等，测距误差的分布不一定是高斯，而且方差与距离成正相关。距离越远，方差越大。
（2）超声等测距技术，空气密度等对声速有明显影响。如果接收器的部署平面不与标签运动平面在同一个平面上，特别是接收器部署平面都在标签运动平面同一侧的话，那么这种影响造成的定位误差无法抵消，就特别明显。比如，当测距值大于实际值时，相当于标签位置都会远离接收器。另外，风速等对超声测距精度也有影响。
在沙盘定位中，设置有标签的仿真货车基本在平面上运行，略有高低起伏，而超声接收器都部署在仿真货车运动平面的同一侧（即天花板）。如果直接使用三角定位法，忽视测距误差分布与距离的相关性、声速不一定是366/秒等问题的话，定位误差就会增加。使用超声定位系统，业内一般只能做到10厘米左右的定位精度。
发明内容
针对上述问题，本发明的目的在于提供一种基于非线性优化的定位方法及系统,使用非线性最优化方法，通过动态估计环境中空气的实际声速，以及增加每次定位的测距数量，提高了整体系统定位精度。
本发明的上述发明目的是通过以下技术方案得以实现的：
一种基于非线性优化的定位方法，包括以下步骤：
S1：建立测距结果队列，并对所述测距结果队列进行初始化，将所述测距结果队列初始化为空；
S2：定位服务器尝试获取一个新的测距结果，其中，所述测距结果为任意一个标签向N个接收器发射定位后计算得到N个测距值的矢量集合；
S3：若当前所述测距结果中所有非0值对应的所述接收器，在X-Y平面不共线，将所述测距结果添加到所述测距结果队列的尾部，根据预先建立的包含声速校正因子和所述测距值的目标函数，采用非线性最优化函数根据所述目标函数计算所述测距结果队列中每个所述测距结果对应的所述标签的最优的所述标签的坐标和所述声速校正因子。
进一步地，在步骤S1中，建立所述测距结果队列，具体为：
所述测距结果队列为小于等于K行，N列的矩阵，当所述测距结果队列的行数超过K行时，删除最早的所述测距结果；
其中，所述测距结果队列的行数K的取值根据定位的实际应用场景以及所述定位服务器的计算速度进行确定，所述测距结果队列的列数N为所述接收器的数量。
进一步地，在步骤S2中，所述定位服务器尝试获取一个新的所述测距结果，具体为：
所述定位服务器按照预定顺序，周期性的给每个所述标签发送定位命令，当前所述标签接收到所述定位命令之后，向N个所述接收器发射定位信号，N个所述接收器对所述定位信号进行处理后生成所述标签到所述接收器的测距值，转发给所述定位服务器，生成一条包含N个所述测距值的测距结果。
进一步地，在步骤S3中，预先建立包含所述声速校正因子和所述测距值的所述目标函数，具体为：
假设N个所述接收器的坐标为Yn=(xn,yn,zn)，M个所述标签的坐标为Ym=(xm,ym,zm)，其中所述接收器的坐标已知，所述标签的坐标未知，需要求解；
第k次的所述测距结果为dkn,=1..N，是一个所述标签发射定位信号后，被每个所述接收器接收到的所述测距值，所述声速校正因子为
Figure SMS_5
，/>
Figure SMS_6
为真实的所述测量值；
当有K个所述测距结果时，设定非线性最优化的目标函数为：
Figure SMS_7
，
其中，xk可以存在相同编号的所述标签。
进一步地，在步骤S3中，采用所述非线性最优化函数根据所述目标函数计算所述测距结果队列中每个所述测距结果对应的所述标签的最优的所述标签的坐标和所述声速校正因子，具体为：
采用MATLAB中的fminsearch函数作为所述非线性最优化函数对所述目标函数求最优解；
在所述目标函数中，所述接收器的坐标Yn和所述测距结果dkn为常量，所述声速校正因子
Figure SMS_8
和所述标签的坐标xk为需要被估计的变量；
采用所述fminsearch函数对所述目标函数寻找所述目标函数的最小值作为局部最优解，并获取最小值时对应的所述声速校正因子
Figure SMS_9
和所述标签的坐标xk。
进一步地，基于非线性优化的定位方法，还包括：为所述目标函数添加约束条件，假设所有的xk都相等以加快所述fminsearch函数的计算速度，以加快获取所述目标函数的所述局部最优解。
进一步地，基于非线性优化的定位方法，还包括：为所述目标函数添加包括预设时间内同一个所述多次的所述测距结果的最大速度和最大加速度约束、声速约束、所述测距结果的范围约束在内的约束条件；
在所述目标函数中，对超过所述约束条件的定位值增加惩罚值，以使所述声速校正因子
Figure SMS_10
和所述标签的坐标xk的估算值更合理。
一种用于执行如上述的基于非线性优化的定位方法的基于非线性优化的定位系统，包括：
结果队列建立模块，用于建立测距结果队列，并对所述测距结果队列进行初始化，将所述测距结果队列初始化为空；
测距结果获取模块，用于提供给定位服务器等待获取一个新的测距结果，其中，所述测距结果为任意一个标签向N个接收器发射定位后计算得到N个测距值的矢量集合；
最优解计算模块，用于若当前所述测距结果中所有非0值对应的所述接收器，在X-Y平面不共线，将所述测距结果添加到所述测距结果队列的尾部，根据预先建立的包含声速校正因子和所述测距值的目标函数，采用非线性最优化函数根据所述目标函数计算所述测距结果队列中每个所述测距结果对应的所述标签的最优的所述标签的坐标和所述声速校正因子。
一种计算机设备，包括存储器和一个或多个处理器，所述存储器中存储有计算机代码，所述计算机代码被所述一个或多个处理器执行时，使得所述一个或多个处理器执行如上述的方法。
一种计算机可读存储介质，所述计算机可读存储介质存储有计算机代码，当所述计算机代码被执行时，如上述的方法被执行。
与现有技术相比，本发明包括以下至少一种有益效果是：
（1）通过提供一种基于非线性优化的定位方法，包括：S1：建立测距结果队列，并对所述测距结果队列进行初始化，将所述测距结果队列初始化为空；S2：定位服务器尝试获取一个新的测距结果，其中，所述测距结果为任意一个标签向N个接收器发射定位后计算得到N个测距值的矢量集合；S3：若当前所述测距结果中所有非0值对应的所述接收器，在X-Y平面不共线，将所述测距结果添加到所述测距结果队列的尾部，根据预先建立的包含声速校正因子和所述测距值的目标函数，采用非线性最优化函数根据所述目标函数计算所述测距结果队列中每个所述测距结果对应的所述标签的最优的所述标签的坐标和所述声速校正因子。上述技术方案，采用非线性最优化方法，通过动态估计环境中空气的实际声速，以及增加每次定位的测距数量，提高了整体定位系统的定位精度。
（2）采用非线性最优化方法，可以对超声误差分布进行更加准确的建模，可以对实际声速进行建模，可以对各种约束灵活建模，从而得到更准确的定位结果。
附图说明
图1为现有技术中定位系统定位示意图；
图2为现有技术中定位系统架构图；
图3为现有技术中定位系统工作示意图；
图4为现有技术中三角定位示意图；
图5为现有技术中三角定位中三个圆不相交于一点的示意图；
图6为本发明一种基于非线性优化的定位方法的整体流程图；
图7为本发明一种基于非线性优化的定位系统的整体结构图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本发明的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。
第一实施例
如图6所示，本实施例提供了一种基于非线性优化的定位方法，包括以下步骤：
S1：建立测距结果队列，并对所述测距结果队列进行初始化，将所述测距结果队列初始化为空。
具体的，为了对测距结果进行很好的保存，并通过测距结果计算局部最优解。本发明采用测距结果队列的形式对测距结果进行保存。所述测距结果队列为小于等于K行，N列的矩阵，当所述测距结果队列的行数超过K行时，删除最早的所述测距结果；其中，所述测距结果队列的行数K的取值根据定位的实际应用场景以及所述定位服务器的计算速度进行确定，所述测距结果队列的列数N为所述接收器的数量。
举个例子来说，若定位服务器采用普通的X86服务器，对于几十个接收器构成的定位系统，K取值在几千以内，每隔1秒计算一次，计算时间上完全来得及。
进一步地，对于当测距结果队列的行数超过K行时，删除最早的测距结果采用“先进先出”的策略进行，具体的原理如下：
（1）定义队列容量：首先确定队列的容量，即可以存储的最大数据的行数，在本实施例中为K行；
（2）初始化队列：创建一个具有指定容量的队列，并进行初始化。对应到本实施例中为创建一个K行，N列的矩阵形式的测距结果队列，并将测距结果队列初始化为空；
（3）入队操作：当有新的测距结果进入测距结果队列时，首先检测测距结果队列是否已满，如果测距结果队列未满，则将新的测距结果放入测距结果队列的末尾，并更新测距结果队列的状态；
（4）出队操作：当测距结果队列已满，有新的测距结果需要进入测距结果队列时，从测距结果队列的头部删除数据，同时将测距结果队列中的其他测距结果向前移动一个位置，以填补删除数据的空位，并将新的测距结果放到测距结果队列的末尾；
（5）队列状态更新：跟踪测距结果队列的头部和尾部指针，以便入队和出队操作的管理。
通过上述方式，当有新的数据需要进入测距结果队列并且队列已满时，最早进入队列的测距结果将被删除，从而保持测距结果队列的固定长度。
同时，对于测距结果队列的存储方式，在本实施例中采用小于等于K行，N列的矩阵的形式进行保存，但是在实际使用中也不局限于此。举个例子来说，也可以采用小于等于K行，但是仅1列的矩阵形式的测距结果队列。但是将每一条测距结果的记录按照{接收器1：测距值1；接收器2：测距值2；接收器3：测距值3......}的格式转换成一条JSON字符串的形式存储与一行的测距结果队列。这样设计的好处在于，对于接收器的数量可扩展，只需要修改JSON字符串中包含的项就可以了。无需在接收器的数量以及型号发生变化时，重新定义测距结果队列。
此外，对于测距结果除了使用队列的形式进行保存之外，还可以使用以下形式进行保存：
（1）采用包括MySQL、PostgreSQL、MongoDB在内的数据库来保存测距结果，可以在数据库中创建一个表或者集合来存储测距结果，每个测距结果可以使用一行记录或者一个文档来表示。使用数据库可以提供结构化数据存储、查询和索引的能力，以方便后续的数据管理和分析。
（2）将测距结果保存文件，可以将每个测距结果保存为一个文本文件或二进制文件，每行或每个文件对应一个测距结果。
（3）采用包括Hadoop HDFS、Apache Cassandra、Apache HBase在内的分布式存储系统存储测距结果。
（4）对于实时性要求较高且访问频繁的测距结果，可以将定位结果保存在缓存中，如Redis、Memcached等。
以上是可以适用于本发明测距结果存储形式的一些举例，但是并不用于对本发明的测距结果的存储形式进行限制。
S2：定位服务器尝试获取一个新的测距结果，其中，所述测距结果为任意一个标签向N个接收器发射定位后计算得到N个测距值的矢量集合。
具体的，所述定位服务器按照预定顺序，周期性的给每个所述标签发送定位命令，当前所述标签接收到所述定位命令之后，向N个所述接收器发射定位信号，N个所述接收器对所述定位信号进行处理后生成所述标签到所述接收器的测距值，转发给所述定位服务器，生成一条包含N个所述测距值的测距结果。
其中，定位服务器按照预定顺序，周期性的给每个标签发送定位命令，可以通过方法进行：
（1）确定标签列表：首先，确定所有需要发送定位命令的标签。创建一个包含所有标签的列表或数据库。
（2）制定发送顺序：根据需求，制定标签发送命令的顺序。可以按照标签的编号、优先级或其他属性来确定发送顺序。
（3）设置发送周期：确定发送定位命令的周期。例如，每隔一定时间间隔发送一次命令。
（4）编写定位命令：根据定位系统协议或通信协议，编写相应的定位命令。命令内容可能包括标签的标识信息、定位指令和其他必要的参数。
（5）循环发送命令：在定位服务器中设置一个循环，按照预定的顺序周期性地给每个标签发送定位命令。可以使用编程语言或脚本来实现循环发送命令的逻辑。
（6）监控响应和超时：在发送命令后，定位服务器需要等待标签的响应。设置适当的超时机制来处理标签未响应的情况，并进行相应的处理，例如重试或标记为无响应。
（7）更新命令顺序：如果需要在未来更改标签的发送顺序，可以根据需要进行更新。
对于接收器对定位信号进行处理后生成标签到所述接收器的测距值，可以通过以下步骤进行（仅举例，实际运行时任意一种生成测距值的方法都行）：
（1）接收定位信号：接收器接收来自标签的定位信号。这些信号可以是无线信号（如无线电频率、超高频（UHF）信号、蓝牙等）或其他类型的信号（如红外线、声波等），具体取决于定位系统的技术。
（2）信号处理：接收器对接收到的定位信号进行处理。这包括信号解调、滤波、放大或其他信号处理步骤，以提取有用的定位信息。
（3）时差测量：通过测量接收到的定位信号与参考信号之间的时差，接收器可以计算出标签到接收器的测距值。时差测量可以通过各种方式实现，例如使用时间差测量（Timeof Flight，TOF）或接收信号强度指示（Received Signal Strength Indicator，RSSI）等。
（4）测距计算：根据时差测量结果和定位系统的特定算法，接收器可以计算出标签到接收器的测距值。这可能涉及距离与时差之间的数学关系或其他定位算法，如多普勒效应或三角定位等。
（5）输出测距值：接收器将计算得到的标签到接收器的测距值输出给定位系统或其他应用程序。这些测距值可以用于后续的定位计算、位置估计或其他定位相关的任务。
S3：若当前所述测距结果中所有非0值对应的所述接收器，在X-Y平面不共线，将所述测距结果添加到所述测距结果队列的尾部，根据预先建立的包含声速校正因子和所述测距值的目标函数，采用非线性最优化函数根据所述目标函数计算所述测距结果队列中每个所述测距结果对应的所述标签的最优的所述标签的坐标和所述声速校正因子。
具体的，需要判断测距结果中所有的非0值对应的接收器，是否在X-Y平面上共线。比如，测距结果中有3个非零测距结果，对应的3个接收器在X-Y平面上的投影形成一条线，那么就无法算得唯一的位置。另外，这里的“共线”不是说必须严格成直线，只要大致形成直线，就可能导致无法得到定位结果。只有在X-Y平面上不共线的若干个测距结果才能够计算出标签的坐标和声速校正因子。
在本步骤中，需要预先建立包含所述声速校正因子和所述测距值的所述目标函数，具体为：
假设N个所述接收器的坐标为Yn=(xn,yn,zn)，M个所述标签的坐标为Ym=(xm,ym,zm)，其中所述接收器的坐标已知，所述标签的坐标未知，需要求解；
第k次的所述测距结果为dkn,=1..N，是一个所述标签发射定位信号后，被每个所述接收器接收到的所述测距值，所述声速校正因子为
Figure SMS_11
，/>
Figure SMS_12
为真实的所述测量值；
当有K个所述测距结果时，设定非线性最优化的目标函数为：
Figure SMS_13
，
其中，xk可以存在相同编号的所述标签。注意，我们使用
Figure SMS_14
，而不使用/>
Figure SMS_15
，是因为这样更接近超声测距误差分布于距离大体成正比的情况。
在目标函数建立完成后，采用所述非线性最优化函数根据所述目标函数计算所述测距结果队列中每个所述测距结果对应的所述标签的最优的所述标签的坐标和所述声速校正因子，具体为：
采用MATLAB中的fminsearch函数作为所述非线性最优化函数对所述目标函数求最优解。在所述目标函数中，所述接收器的坐标Yn和所述测距结果dkn为常量，所述声速校正因子
Figure SMS_16
和所述标签的坐标xk为需要被估计的变量。采用所述fminsearch函数对所述目标函数寻找所述目标函数的最小值作为局部最优解，并获取最小值时对应的所述声速校正因子
Figure SMS_17
和所述标签的坐标xk。
其中，在MATLAB中，fminsearch函数是一个用于寻找函数的最小值的优化函数。它采用了一种称为“单纯形搜索”（Nelder-Mead）的无导数优化算法。
fminsearch函数的语法如下：
Figure SMS_18
，
参数说明：
fun：要最小化的目标函数（单变量或多变量函数），可以是MATLAB函数句柄、匿名函数或函数名称。
x0：包含初始点的向量，作为搜索的起点。
options（可选）：用于指定优化选项的结构体。可以通过optimset函数创建。
返回值说明：
x：找到的使目标函数最小化的解（向量）。
fval：目标函数在找到的最小值处的函数值。
fminsearch函数通过对目标函数进行迭代搜索来寻找最小值。它会根据提供的初始点和搜索算法，在函数空间中移动，逐步接近局部最小值。搜索过程会根据函数值的变化来确定下一步的移动方向和步长，直到达到停止条件。
进一步地，在最优解的计算过程中，为所述目标函数添加约束条件，假设所有的xk都相等以加快所述fminsearch函数的计算速度，以加快获取所述目标函数的所述局部最优解。或者假设X1，X2...Xk是线性的，或二次线性的，以加快目标函数的求解。
进一步地，在实际计算过程中，还可以增加其他约束条件，如：为所述目标函数添加包括预设时间内同一个所述多次的所述测距结果的最大速度和最大加速度约束、在Z轴（沙盘垂直方式）上的移动范围非常有限，建立声速必须在合理范围内的约束、每个测距结果必须落在合理范围内的范围约束在内的约束条件；在所述目标函数中，对超过所述约束条件的定位值增加惩罚值，以使所述声速校正因子
Figure SMS_19
和所述标签的坐标xk的估算值更合理。
第二实施例
如图7所示，本实施例提供了一种用于执行如第一实施例中的基于非线性优化的定位方法的基于非线性优化的定位系统，包括：
结果队列建立模块1，用于建立测距结果队列，并对所述测距结果队列进行初始化，将所述测距结果队列初始化为空；
测距结果获取模块2，用于提供给定位服务器等待获取一个新的测距结果，其中，所述测距结果为任意一个标签向N个接收器发射定位后计算得到N个测距值的矢量集合；
最优解计算模块3，用于若当前所述测距结果中所有非0值对应的所述接收器，在X-Y平面不共线，将所述测距结果添加到所述测距结果队列的尾部，根据预先建立的包含声速校正因子和所述测距值的目标函数，采用非线性最优化函数根据所述目标函数计算所述测距结果队列中每个所述测距结果对应的所述标签的最优的所述标签的坐标和所述声速校正因子。
一种计算机可读存储介质，计算机可读存储介质存储有计算机代码，当计算机代码被执行时，如上述方法被执行。本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器 (ROM ，Read Only Memory) 、随机存取存储器 (RAM ，Random Access Memory)、磁盘或光盘等。
以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
应当说明的是，上述实施例均可根据需要自由组合。以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种基于非线性优化的定位方法，其特征在于，包括以下步骤：
S1：建立测距结果队列，并对所述测距结果队列进行初始化，将所述测距结果队列初始化为空；
S2：定位服务器尝试获取一个新的测距结果，其中，所述测距结果为任意一个标签向N个接收器发射定位后计算得到N个测距值的矢量集合；
S3：若当前所述测距结果中所有非0值对应的所述接收器，在X-Y平面不共线，将所述测距结果添加到所述测距结果队列的尾部，根据预先建立的包含声速校正因子和所述测距值的目标函数，采用非线性最优化函数根据所述目标函数计算所述测距结果队列中每个所述测距结果对应的所述标签的最优的所述标签的坐标和所述声速校正因子。
2.根据权利要求1所述的基于非线性优化的定位方法，其特征在于，在步骤S1中，建立所述测距结果队列，具体为：
所述测距结果队列为小于等于K行，N列的矩阵，当所述测距结果队列的行数超过K行时，删除最早的所述测距结果；
其中，所述测距结果队列的行数K的取值根据定位的实际应用场景以及所述定位服务器的计算速度进行确定，所述测距结果队列的列数N为所述接收器的数量。
3.根据权利要求1所述的基于非线性优化的定位方法，其特征在于，在步骤S2中，所述定位服务器尝试获取一个新的所述测距结果，具体为：
所述定位服务器按照预定顺序，周期性的给每个所述标签发送定位命令，当前所述标签接收到所述定位命令之后，向N个所述接收器发射定位信号，N个所述接收器对所述定位信号进行处理后生成所述标签到所述接收器的测距值，转发给所述定位服务器，生成一条包含N个所述测距值的测距结果。
4.根据权利要求2所述的基于非线性优化的定位方法，其特征在于，在步骤S3中，预先建立包含所述声速校正因子和所述测距值的所述目标函数，具体为：
假设N个所述接收器的坐标为Yn=(xn,yn,zn)，M个所述标签的坐标为Ym=(xm,ym,zm)，其中所述接收器的坐标已知，所述标签的坐标未知，需要求解；
第k次的所述测距结果为dkn,=1..N，是一个所述标签发射定位信号后，被每个所述接收器接收到的所述测距值，所述声速校正因子为
Figure QLYQS_1
，/>
Figure QLYQS_2
为真实的所述测量值；
当有K个所述测距结果时，设定非线性最优化的目标函数为：
Figure QLYQS_3
其中，Xk可以存在相同编号的所述标签。
5.根据权利要求4所述的基于非线性优化的定位方法，其特征在于，在步骤S3中，采用所述非线性最优化函数根据所述目标函数计算所述测距结果队列中每个所述测距结果对应的所述标签的最优的所述标签的坐标和所述声速校正因子，具体为：
采用MATLAB中的fminsearch函数作为所述非线性最优化函数对所述目标函数求最优解；
在所述目标函数中，所述接收器的坐标Yn和所述测距结果dkn为常量，所述声速校正因子
Figure QLYQS_4
和所述标签的坐标Xk为需要被估计的变量；
采用所述fminsearch函数对所述目标函数寻找所述目标函数的最小值作为局部最优解，并获取最小值时对应的所述声速校正因子
Figure QLYQS_5
和所述标签的坐标Xk。
6.根据权利要求5所述的基于非线性优化的定位方法，其特征在于，还包括：
为所述目标函数添加约束条件，假设所有的Xk都相等以加快所述fminsearch函数的计算速度，以加快获取所述目标函数的所述局部最优解。
7.根据权利要求5所述的基于非线性优化的定位方法，其特征在于，还包括：
为所述目标函数添加包括预设时间内同一个所述多次的所述测距结果的最大速度和最大加速度约束、声速约束、所述测距结果的范围约束在内的约束条件；
在所述目标函数中，对超过所述约束条件的定位值增加惩罚值，以使所述声速校正因子
Figure QLYQS_6
和所述标签的坐标Xk的估算值更合理。
8.一种用于执行如权利要求1-7任意一项所述的基于非线性优化的定位方法的基于非线性优化的定位系统，其特征在于，包括：
结果队列建立模块，用于建立测距结果队列，并对所述测距结果队列进行初始化，将所述测距结果队列初始化为空；
测距结果获取模块，用于提供给定位服务器尝试获取一个新的测距结果，其中，所述测距结果为任意一个标签向N个接收器发射定位后计算得到N个测距值的矢量集合；
最优解计算模块，用于若当前所述测距结果中所有非0值对应的所述接收器，在X-Y平面不共线，将所述测距结果添加到所述测距结果队列的尾部，根据预先建立的包含声速校正因子和所述测距值的目标函数，采用非线性最优化函数根据所述目标函数计算所述测距结果队列中每个所述测距结果对应的所述标签的最优的所述标签的坐标和所述声速校正因子。
9.一种计算机设备，包括存储器和一个或多个处理器，所述存储器中存储有计算机代码，所述计算机代码被所述一个或多个处理器执行时，使得所述一个或多个处理器执行如权利要求1至7中任一项所述的方法。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机代码，当所述计算机代码被执行时，如权利要求1至7中任一项所述的方法被执行。
CN202310590034.XA 2023-05-24 2023-05-24 一种基于非线性优化的定位方法及系统 Active CN116299184B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310590034.XA CN116299184B (zh) 2023-05-24 2023-05-24 一种基于非线性优化的定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310590034.XA CN116299184B (zh) 2023-05-24 2023-05-24 一种基于非线性优化的定位方法及系统

Publications (2)

Publication Number Publication Date
CN116299184A true CN116299184A (zh) 2023-06-23
CN116299184B CN116299184B (zh) 2023-09-01

Family

ID=86783657

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310590034.XA Active CN116299184B (zh) 2023-05-24 2023-05-24 一种基于非线性优化的定位方法及系统

Country Status (1)

Country Link
CN (1) CN116299184B (zh)

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011075448A (ja) * 2009-09-30 2011-04-14 Tokai Rika Co Ltd 受信波の波源位置推定装置及び波源方向推定装置、並びに受信波の波源位置推定方法及び波源方向推定方法
CN105425212A (zh) * 2015-11-18 2016-03-23 西北工业大学 一种声源定位方法
CN106054134A (zh) * 2016-05-20 2016-10-26 东南大学 一种基于tdoa的快速定位方法
CN108489495A (zh) * 2018-03-29 2018-09-04 东南大学 一种基于svr和pso的rfid标签室内定位方法及设备
CN108603405A (zh) * 2015-09-28 2018-09-28 赫尔实验室有限公司 利用多站分析进行的实时轨迹估计
CN110389597A (zh) * 2018-04-17 2019-10-29 北京京东尚科信息技术有限公司 基于声源定位的摄像头调整方法、装置和系统
CN110794409A (zh) * 2019-10-21 2020-02-14 哈尔滨工程大学 一种可估计未知有效声速的水下单信标定位方法
US20210141051A1 (en) * 2017-03-07 2021-05-13 Sonitor Technologies As Ultrasound position-determination system
CN114353795A (zh) * 2021-12-06 2022-04-15 中南设计集团（武汉）工程技术研究院有限公司 一种基于uwb设备的室内三维定位系统与方法
CN115774258A (zh) * 2022-11-22 2023-03-10 浙江大学 一种超声波主动式目标定位方法与装置

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011075448A (ja) * 2009-09-30 2011-04-14 Tokai Rika Co Ltd 受信波の波源位置推定装置及び波源方向推定装置、並びに受信波の波源位置推定方法及び波源方向推定方法
CN108603405A (zh) * 2015-09-28 2018-09-28 赫尔实验室有限公司 利用多站分析进行的实时轨迹估计
CN105425212A (zh) * 2015-11-18 2016-03-23 西北工业大学 一种声源定位方法
CN106054134A (zh) * 2016-05-20 2016-10-26 东南大学 一种基于tdoa的快速定位方法
US20210141051A1 (en) * 2017-03-07 2021-05-13 Sonitor Technologies As Ultrasound position-determination system
CN108489495A (zh) * 2018-03-29 2018-09-04 东南大学 一种基于svr和pso的rfid标签室内定位方法及设备
CN110389597A (zh) * 2018-04-17 2019-10-29 北京京东尚科信息技术有限公司 基于声源定位的摄像头调整方法、装置和系统
CN110794409A (zh) * 2019-10-21 2020-02-14 哈尔滨工程大学 一种可估计未知有效声速的水下单信标定位方法
CN114353795A (zh) * 2021-12-06 2022-04-15 中南设计集团（武汉）工程技术研究院有限公司 一种基于uwb设备的室内三维定位系统与方法
CN115774258A (zh) * 2022-11-22 2023-03-10 浙江大学 一种超声波主动式目标定位方法与装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
闫青丽;陈建峰;: "风场环境中声速修正的分布式声源定位算法", 声学学报, no. 04 *
陈冠旭: "GNSS-声学位置服务中声速误差修正方法", 测绘学报, vol. 52, no. 4 *

Also Published As

Publication number Publication date
CN116299184B (zh) 2023-09-01

Similar Documents

Publication Publication Date Title
CN112035591B (zh) 一种路网匹配方法、装置、设备和存储介质
CN108362294A (zh) 一种应用于自动驾驶的多车协同建图方法
CN111708048B (zh) 点云的运动补偿方法、装置和系统
CN112034431B (zh) 一种雷达和rtk的外参标定方法及装置
JP2015534640A (ja) 無線周波数信号を用いた測位システム
CN110779496B (zh) 三维地图构建系统、方法、设备和存储介质
CN112505671B (zh) Gnss信号缺失环境下毫米波雷达目标定位方法及装置
CN112154356A (zh) 点云数据处理方法及其装置、激光雷达、可移动平台
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN113131985A (zh) 一种基于信息年龄最优路径规划的多无人机数据收集方法
CN104507097A (zh) 一种基于WiFi位置指纹的半监督训练方法
CN110095108B (zh) 一种基于bim无人机测绘装置及测绘方法
JP2017072423A (ja) 推定装置、制御方法、プログラム及び記憶媒体
CN116299184B (zh) 一种基于非线性优化的定位方法及系统
CN114623823A (zh) 一种融合里程计的uwb多模态定位系统、方法及装置
CN109903367B (zh) 构建地图的方法、装置和计算机可读存储介质
CN113822944A (zh) 一种外参标定方法、装置、电子设备及存储介质
CN111696159B (zh) 激光里程计的特征存储方法、电子设备和存储介质
KR101106575B1 (ko) 물품의 측위 장치 및 측위 방법
CN114763998B (zh) 基于微型雷达阵列的未知环境并行导航方法和系统
CN115856911A (zh) 一种定位方法及装置
CN113382354B (zh) 一种基于因子图的无线定位非视距信号判别方法
CN116299183B (zh) 一种使用扩展卡尔曼滤波估计声速的超声定位方法及系统
CN112987010A (zh) 一种用于机器人的多雷达建图的系统及方法
TWI720923B (zh) 定位系統以及定位方法

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