阅读说明：本技术 一种空间实时定位方法、计算机装置及计算机可读存储介质 (Space real-time positioning method, computer device and computer readable storage medium ) 是由 张强 李伟 兰海鹏 李萌 董建博 许未来 于 2019-09-26 设计创作，主要内容包括：本发明提供一种空间实时定位方法,包括初始化定位移动标签,获得初始坐标值；初始化卡尔曼滤波器；计算移动后的移动目标的移动坐标值；获取经卡尔曼滤波器预测的下一次移动后的移动目标的预测坐标值并优化；计算移动目标的速度值；重新计算移动后的移动目标的更新移动坐标值,并获取经卡尔曼滤波器预测的移动目标的更新预测坐标值,基于更新预测坐标值对更新移动坐标值及移动目标的速度值进行优化并获取移动目标的实时坐标值和实时速度值。本发明还提供实现上述方法的计算机装置及计算机可读存储介质。本发明实现不再依赖惯性导航硬件来推导速度参数,并利用优化的卡尔曼滤波器算法,更好地消除定位坐标的“抖动”问题,能够有效提高实时定位精度。(The invention provides a space real-time positioning method, which comprises the steps of initializing a positioning mobile tag and obtaining an initial coordinate value; initializing a Kalman filter; calculating the moving coordinate value of the moved moving target; obtaining a predicted coordinate value of the moving target after the next movement predicted by the Kalman filter and optimizing the predicted coordinate value; calculating the speed value of the moving target; and recalculating the updated moving coordinate value of the moved moving target, acquiring the updated predicted coordinate value of the moving target predicted by the Kalman filter, optimizing the updated moving coordinate value and the speed value of the moving target based on the updated predicted coordinate value, and acquiring the real-time coordinate value and the real-time speed value of the moving target. The invention also provides a computer device and a computer readable storage medium for realizing the method. The invention can realize that the velocity parameter is deduced without depending on inertial navigation hardware, and the optimized Kalman filter algorithm is utilized to better eliminate the problem of 'jitter' of the positioning coordinate, thereby effectively improving the real-time positioning precision.)

一种空间实时定位方法、计算机装置及计算机可读存储介质

技术领域

本发明涉及目标定位领域，具体是涉及一种空间实时定位方法，还设置实现这种方法的计算机装置以及计算机可读存储介质。

背景技术

卡尔曼滤波与组合导航被广泛应用于室内外空间定位与导航。在室外场景下，广泛利用GPS和惯性导航系统对物体进行定位(属于组合导航定位)，在室内场景下，常用超宽带、WIFI、蓝牙、惯性导航等技术进行定位。目前，基于惯导系统和超宽带的组合导航技术是室内定位研究的一个热点。惯性导航的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，进而推算出该运动物体的速度、偏航角和位置等信息。超宽带技术具有信号好、测量精度高等特点。二者结合可以获得更好的空间定位效果。

同时，现有组合定位技术存在以下不足：

1、需要惯性装置和超宽带测量装置两种甚至更多的硬件设备，成本较高。

2、需要把不同硬件设备的数据进行组合运算，算法比较复杂。

因此，针对上述不足，本发明采用模拟惯性导航的思想模拟出物体运动的速度，而不需要惯性导航的硬件设备，从而减少硬件成本、导航系统的开发量及简化算法。

发明内容

本发明的第一目的是提供一种空间实时定位方法，可在无惯性导航硬件设备的情况下实现空间实时定位。

本发明的第二目的是提供一种实现上述空间实时定位方法的计算机装置。

本发明的第三目的是提供一种实现上述空间实时定位方法的计算机可读存储介质。

为了实现上述的第一目的，本发明提供的空间实时定位方法包括初始化定位移动标签，获得初始坐标值；初始化卡尔曼滤波器；计算移动后的移动目标的移动坐标值；获取经卡尔曼滤波器预测的下一次移动后的移动目标的预测坐标值并优化；计算移动目标的速度值；重新计算移动后的移动目标的更新移动坐标值，并获取经卡尔曼滤波器预测的移动目标的更新预测坐标值，基于更新预测坐标值对更新移动坐标值及移动目标的速度值进行优化并获取移动目标的实时坐标值和实时速度值。

由上述方案可见，本发明通过移动标签实现对移动目标的追踪，利用移动标签和基站之间的距离，首先解算出标签的位置坐标值，并基于位置求出速度值，最后利用卡尔曼滤波器将坐标值和速度值进行优化并实现对实时坐标值和实时速度值的获取。

进一步的方案是，所述移动标签的数量为一个以上，至少3个定位基站。

更进一步的方案是，初始化定位移动标签包括采用三边定位法或最小二乘法计算多个坐标值，并计算多个坐标值的均值及方差，判断方差是否小于预设期望值，如是，则应用均值为初始坐标值。

更进一步的方案是，初始化卡尔曼滤波器包括通过所述初始坐标值、初始速度、及噪声方差对所述卡尔曼滤波器进行初始化。

更进一步的方案是，移动坐标值通过三边定位法或最小二乘法进行计算获取。

由上可见，通过采用移动标签及定位基站进行测距及计算，可实现对位置坐标、速度的初始化及移动坐标值的获取。

更进一步的方案是，预测坐标值通过卡尔曼滤波器的预测方程进行求解获取，并设X^T _k＝[x,y,v_x,v_y]为卡尔曼滤波器的输入变量，其中(x,y)是移动标签的坐标，(v_x,v_y)是标签移动速度v在x、y方向上的分量。

更进一步的方案是，计算移动目标运动速度包括设最后4次获得的所述移动目标的坐标值分别为C_k-3(x_k-3,y_k-3)，C_k-2(x_k-2,y_k-2)，C_k-1(x_k-1,y_k-1)，C_k(x_k,y_k)，空间定位数据刷新频率为f，则x方向和y方向的速度分别为

v_x＝f/4·(x_k-x_k-2)+(x_k-1-x_k-3)；

v_y＝f/4·(y_k-y_k-2)+(y_k-1-y_k-3)。

更进一步的方案是，优化过程包括对滤波后的坐标值和速度值进行轨迹平滑，将当前时刻的预测值与前一时刻的预测值进行比较，通过不同时刻的权重对相邻时刻的预测值进行加权平均，从而实现自适应的过程。

由上可见，预测方程需要输入v_x和v_y，v_x和v_y通过最后4次获得的移动目标的坐标值进行计算获取。对比现有技术，该方法可实现不再依赖惯性导航硬件来推导速度等参数，减少了硬件成本和研发难度。同时，通过优化的算法，更好地消除定位坐标的“抖动”问题，可得到比较平滑的运动轨迹，能够有效提高实时定位精度。

为了实现上述的第二目的，本发明提供的计算机装置具有处理器以及存储器，存储器存储有计算机程序，计算机程序被处理器执行时实现上述空间实时定位方法的各个步骤，或者执行上述空间实时定位方法的各个步骤。

为实现上述的第三目的，本发明提供计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现上述空间实时定位方法的各个步骤，或者执行上述空间实时定位方法的各个步骤。

附图说明

图1是本发明一种空间实时定位方法实施例的流程图。

图2是三边定位法示意图。

以下结合附图及实施例对本发明作进一步说明。

具体实施方式

一种空间实时定位方法实施例：

参见图1，图1是本发明一种空间实时定位方法实施例的流程图。首先，执行步骤S1，初始化定位移动标签，获得初始坐标值，具体地，本实施例采用四个超宽带(UWB)定位芯片进行定位操作，其中包括一个移动标签以及3个定位基站。参见图2，图2是三边定位法示意图。本实施例采用三边定位法计算多个坐标值，其中，A、B、C分别是三个基站位置，为已知量，D为移动标签的位置，为未知量，设D的坐标为(x,y,z)，A的坐标为(x₁,y₁,z₁)，B的坐标为(x₂,y₂,z₂)，C的坐标为(x₃,y₃,z₃)，d₁、d₂、d₃分别为以A、B、C为球心作的球形的半径，设D为A、B、C三球形的相交点，则

(x₁-x₀)²+(y₁-y₀)²＝d₁ ²；

(x₂-x₀)²+(y₂-y₀)²＝d₂ ²；

(x₃-x₀)²+(y₃-y₀)²＝d₃ ²。

通过以上公式，可计算求得D(x,y,z)。同时，由于测量误差，D并不能精确地成为A、B、C三球形的相交点，因此，需计算多个坐标值的均值及方差，判断方差是否小于预设期望值，如是，则应用均值为初始坐标值。

接着，执行步骤S2，初始化卡尔曼滤波器，具体地，通过初始坐标值、初始速度、及噪声方差对卡尔曼滤波器进行初始化。然后，执行步骤S3，计算移动后的移动目标的移动坐标值，具体地，本实施例中移动坐标值通过上述三边定位法进行计算获取。接着，执行步骤S4，获取经卡尔曼滤波器预测的下一次移动后的移动目标的预测坐标值并优化，具体地，预测坐标值通过卡尔曼滤波器的预测方程进行求解获取，并设X^T _k＝[x,y,v_x,v_y]为卡尔曼滤波器的输入变量，其中(x,y)是移动标签的坐标，(v_x,v_y)是标签移动速度v在x、y方向上的分量。然后，执行步骤S5，计算移动目标的速度值，具体地，设最后4次获得的移动目标的坐标值分别为C_k-3(x_k-3,y_k-3)，C_k-2(x_k-2,y_k-2)，C_k-1(x _k-1,y_k-1)，C_k(x_k,y_k)，空间定位数据刷新频率为f，则x方向和y方向的速度值分别为

v_x＝f/4·(x_k-x_k-2)+(x_k-1-x_k-3)；

v_y＝f/4·(y_k-y_k-2)+(y_k-1-y_k-3)。

接着，执行步骤S6，重新计算移动后的移动目标的更新移动坐标值，并获取经卡尔曼滤波器预测的移动目标的更新预测坐标值，具体地，更新移动坐标值的计算与前述步骤S3的移动坐标值的计算一致、更新预测坐标值的获取与前述步骤S4的预测坐标值的获取一致，此处不再赘述。

最后，执行步骤S7，对更新移动坐标值及移动目标的速度值进行优化并获取移动目标的实时坐标值和实时速度值，具体地，对滤波后的坐标值和速度值进行轨迹平滑，将当前时刻的预测值与前一时刻的预测值进行比较，通过不同时刻的权重对相邻时刻的预测值进行加权平均，从而实现自适应的过程。

可见，本实施例采用四个超宽带(UWB)定位芯片进行定位操作，采用模拟惯性导航的思想，通过坐标值数据模拟出物体运动的速度，而不需要惯性导航的加速度计、陀螺仪等硬件设备，减少了硬件成本和研发难度。同时，利用了优化的卡尔曼滤波器算法，更好地消除定位坐标的“抖动”问题，得到比较平滑的运动轨迹，能够有效提高实时定位精度，非常适用于匀速直线运动，也适用于加速、减速、转弯等运动过程。

计算机装置实施例：

本实施例的计算机装置包括有处理器、存储器以及存储在存储器中并可在处理器上运行的计算机程序，计算机程序被处理器执行时实现前述空间实时定位方法的各个步骤。

例如，计算机程序可以被分割成一个或多个模块，一个或者多个模块被存储在存储器中，并由处理器执行，以完成本发明的各个模块。一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述计算机程序在终端设备中的执行过程。

本发明所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，处理器是电器的控制中心，利用各种接口和线路连接整个电器的各个部分。

存储器可用于存储计算机程序和/或模块，处理器通过运行或执行存储在存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现电器的各种功能。存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据电器的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart MediaCard,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

计算机可读存储介质实施例：

计算机装置的存储器所存储的计算机程序如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，该计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述空间实时定位方法的各个步骤。

其中，计算机程序包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读介质可以包括：能够携带计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

最后需要强调的是，本发明不限于上述实施方式，如对空间实时定位方法的具体过程、定位芯片类型的改变等，这些改变也应该包括在本发明权利要求的保护范围内。