阅读说明：本技术 一种空间实时定位方法、计算机装置及计算机可读存储介质 (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.)

1. A method for spatial real-time positioning, comprising:

initializing a positioning mobile tag to obtain 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.

2. The method according to claim 1, wherein:

the number of the mobile tags is more than one.

3. The method according to claim 1, wherein:

and the step of initializing the positioning mobile tag comprises the steps of calculating a plurality of coordinate values by adopting a trilateral positioning method or a least square method, calculating the mean value and the variance of the coordinate values, judging whether the variance is smaller than a preset expected value or not, and if so, applying the mean value as an initial coordinate value.

4. The method according to claim 1, wherein:

initializing the Kalman filter comprises initializing the Kalman filter by the initial coordinate values, the initial velocity, and the noise variance.

5. The method according to claim 1, wherein:

and the moving coordinate value is calculated and obtained by a trilateral positioning method or a least square method.

6. The method according to claim 1, wherein:

the predicted coordinate value is obtained by solving a prediction equation of a Kalman filter, and X is set^T _k＝[x,y,v_x,v_y]As input variables of Kalman filtersWhere (x, y) is the coordinates of the mobile tag, (v)_x,v_y) Are the components of the label moving speed v in the x, y directions.

7. The spatial real-time localization method according to any one of claims 1 to 6, characterized by:

the calculating the speed value of the moving target comprises setting the coordinate values of the moving target obtained for the last 4 times as C respectively_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) If the refresh frequency of the spatial location data is f, the speed values in the x-direction and the y-direction are respectively

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)。

8. The method according to claim 1, wherein: the optimization process comprises the steps of performing track smoothing on the filtered coordinate values and speed values, comparing the predicted value at the current moment with the predicted value at the previous moment, and performing weighted average on the predicted values at adjacent moments through weights at different moments so as to realize a self-adaptive process.

9. A computer device, characterized by: comprising a processor and a memory, said memory storing a computer program which, when executed by said processor, carries out the steps of the method of spatial real-time localization as claimed in any one of claims 1 to 8.

10. A computer-readable storage medium having stored thereon a computer program, characterized in that: the computer program, when being executed by a processor, carries out the steps of the method for spatial real-time localization as claimed in any one of claims 1 to 8.

Technical Field

The invention relates to the field of target positioning, in particular to a space real-time positioning method, a computer device for realizing the method and a computer readable storage medium.

Background

Kalman filtering and integrated navigation are widely applied to indoor and outdoor space positioning and navigation. In an outdoor scene, a GPS and an inertial navigation system are widely utilized to position an object (belonging to integrated navigation positioning), and in an indoor scene, the object is positioned by technologies such as ultra-wideband, WIFI, Bluetooth, inertial navigation and the like. Currently, an integrated navigation technology based on an inertial navigation system and an ultra-wideband is a hot spot of indoor positioning research. The basic working principle of inertial navigation is based on Newton's law of mechanics, and the information of speed, yaw angle, position and the like of the moving object is further calculated by measuring the acceleration of a carrier in an inertial reference system. The ultra-wideband technology has the characteristics of good signal, high measurement precision and the like. The two can be combined to obtain better space positioning effect.

Meanwhile, the existing combined positioning technology has the following defects:

1. two or more hardware devices, namely an inertia device and an ultra-wideband measuring device, are needed, and the cost is high.

2. The data of different hardware devices need to be combined and operated, and the algorithm is complex.

Therefore, aiming at the defects, the invention adopts the idea of simulating the inertial navigation to simulate the movement speed of the object without hardware equipment of the inertial navigation, thereby reducing the hardware cost, the development amount of a navigation system and simplifying an algorithm.

Disclosure of Invention

The first purpose of the present invention is to provide a method for real-time spatial positioning, which can realize real-time spatial positioning without inertial navigation hardware equipment.

The second objective of the present invention is to provide a computer device for implementing the above-mentioned space real-time positioning method.

A third object of the present invention is to provide a computer readable storage medium for implementing the above-mentioned space real-time positioning method.

In order to achieve the first object, the spatial real-time positioning method provided by the invention 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.

According to the scheme, the mobile tag is used for tracking the mobile target, the position coordinate value of the tag is firstly calculated by utilizing the distance between the mobile tag and the base station, the speed value is calculated based on the position, and finally the coordinate value and the speed value are optimized by utilizing a Kalman filter and the real-time coordinate value and the real-time speed value are acquired.

The further scheme is that the number of the mobile tags is more than one, and at least 3 positioning base stations are provided.

The method further includes the steps of initializing the positioning mobile tag, calculating a plurality of coordinate values by adopting a trilateral positioning method or a least square method, calculating a mean value and a variance of the coordinate values, and judging whether the variance is smaller than a preset expected value, wherein if the variance is smaller than the preset expected value, the mean value is used as an initial coordinate value.

Further, initializing the kalman filter includes initializing the kalman filter by the initial coordinate value, the initial velocity, and the noise variance.

Further, the moving coordinate value is calculated and obtained by a trilateral localization method or a least square method.

Therefore, the initialization of the position coordinate and the speed and the acquisition of the moving coordinate value can be realized by adopting the mobile tag and the positioning base station to carry out distance measurement and calculation.

Further, the predicted coordinate value is obtained by solving the prediction equation of the Kalman filter, and X is set^T _k＝[x,y,v_x,v_y]Is an input variable of the Kalman filter, where (x, y) is the coordinate of the moving tag, (v)_x,v_y) Are the components of the label moving speed v in the x, y directions.

Further, the calculating the moving speed of the moving object includes setting the coordinate values of the moving object obtained in the last 4 times as 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) The refresh frequency of the spatial location data is f, the speed in the x-direction and the speed in the y-direction are respectively

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)。

The further scheme is that the optimization process comprises the steps of performing track smoothing on the filtered coordinate values and the filtered speed values, comparing the predicted value at the current moment with the predicted value at the previous moment, and performing weighted average on the predicted values at adjacent moments through weights at different moments so as to realize a self-adaptive process.

From the above, the prediction equation requires input v_xAnd v_y，v_xAnd v_yAnd calculating and obtaining the coordinate values of the moving target obtained in the last 4 times. Compared with the prior art, the method can realize derivation of parameters such as speed and the like without depending on inertial navigation hardware, and hardware cost anddifficulty in research and development. Meanwhile, through an optimized algorithm, the problem of 'jitter' of the positioning coordinates is better eliminated, a smoother motion track can be obtained, and the real-time positioning precision can be effectively improved.

In order to achieve the second object, the present invention provides a computer device having a processor and a memory, wherein the memory stores a computer program, and the computer program is executed by the processor to implement the steps of the spatial real-time positioning method or to execute the steps of the spatial real-time positioning method.

To achieve the third objective, the present invention provides a computer-readable storage medium, which stores a computer program, and the computer program is executed by a processor to implement the steps of the above-mentioned space real-time positioning method, or to execute the steps of the above-mentioned space real-time positioning method.

Drawings

Fig. 1 is a flowchart of an embodiment of a spatial real-time positioning method according to the present invention.

FIG. 2 is a schematic of trilateration.

The invention is further explained with reference to the drawings and the embodiments.

Detailed Description

The embodiment of the space real-time positioning method comprises the following steps:

referring to fig. 1, fig. 1 is a flowchart of an embodiment of a spatial real-time positioning method according to the present invention. First, step S1 is executed to initialize and position the mobile tag and obtain an initial coordinate value, specifically, the present embodiment uses four Ultra Wideband (UWB) positioning chips to perform positioning operation, which includes one mobile tag and 3 positioning base stations. Referring to fig. 2, fig. 2 is a schematic diagram of trilateration. In this embodiment, a trilateration method is used to calculate a plurality of coordinate values, where A, B, C is the position of three base stations, which is a known quantity, D is the position of the mobile tag, which is an unknown quantity, the coordinate of D is (x, y, z), and the coordinate of a is (x, y, z)₁,y₁,z₁) The coordinates of B are (x)₂,y₂,z₂) And the coordinate of C is (x)₃,y₃,z₃)，d₁、d₂、d₃Are respectively provided withThe radius of the sphere with A, B, C as the center of the sphere, and D is the intersection point of A, B, C three spheres

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

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

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

From the above formula, D (x, y, z) can be calculated. Meanwhile, D cannot be precisely the intersection point of A, B, C trigrams due to measurement errors, and therefore, the mean and variance of the coordinate values need to be calculated to determine whether the variance is smaller than a predetermined expected value, and if so, the mean is applied as the initial coordinate value.

Next, step S2 is executed to initialize the kalman filter, specifically, the kalman filter is initialized by the initial coordinate values, the initial velocity, and the noise variance. Then, step S3 is executed to calculate the moving coordinate value of the moved moving object, specifically, the moving coordinate value in this embodiment is calculated and obtained by the above-mentioned trilateration method. Next, step S4 is executed to obtain and optimize the predicted coordinate value of the moving target after the next movement predicted by the kalman filter, specifically, the predicted coordinate value is obtained by solving the prediction equation of the kalman filter, and X is set^T _k＝[x,y,v_x,v_y]Is an input variable of the Kalman filter, where (x, y) is the coordinate of the moving tag, (v)_x,v_y) Are the components of the label moving speed v in the x, y directions. Then, step S5 is executed to calculate the velocity value of the moving object, specifically, the coordinate values of the moving object obtained in the last 4 times are respectively set as 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) The refresh frequency of the spatial location data is f, the velocity values in the x-direction and the y-directionIs otherwise provided with

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)。

Next, step S6 is executed to recalculate the updated moving coordinate value of the moved moving object and acquire the updated predicted coordinate value of the moving object predicted by the kalman filter, specifically, the calculation of the updated moving coordinate value is consistent with the calculation of the moving coordinate value in step S3, and the acquisition of the updated predicted coordinate value is consistent with the acquisition of the predicted coordinate value in step S4, which is not repeated here.

And finally, executing step S7, optimizing the updated moving coordinate value and the moving target speed value, and obtaining the real-time coordinate value and the real-time speed value of the moving target, specifically, performing track smoothing on the filtered coordinate value and speed value, comparing the predicted value at the current moment with the predicted value at the previous moment, and performing weighted average on the predicted values at adjacent moments by the weights at different moments, thereby implementing the adaptive process.

Therefore, in the embodiment, four Ultra Wide Band (UWB) positioning chips are adopted for positioning operation, the idea of simulating inertial navigation is adopted, the speed of the object motion is simulated through coordinate value data, hardware devices such as accelerometers, gyroscopes and the like for inertial navigation are not needed, and hardware cost and research and development difficulty are reduced. Meanwhile, the optimized Kalman filter algorithm is utilized, the problem of 'shaking' of the positioning coordinate is better eliminated, a relatively smooth motion track is obtained, the real-time positioning precision can be effectively improved, and the method is very suitable for uniform linear motion and is also suitable for the motion processes of acceleration, deceleration, turning and the like.

The embodiment of the computer device comprises:

the computer device of this embodiment includes a processor, a memory, and a computer program stored in the memory and capable of running on the processor, and when the computer program is executed by the processor, the computer program implements the steps of the aforementioned spatial real-time positioning method.

For example, a computer program may be partitioned into one or more modules that are stored in a memory and executed by a processor to implement the modules of the present invention. One or more of the modules may be a series of computer program instruction segments capable of performing certain functions, which are used to describe the execution of the computer program in the terminal device.

The Processor may be a Central Processing Unit (CPU), or may be other general-purpose Processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), an off-the-shelf Programmable Gate Array (FPGA) or other Programmable logic device, a discrete Gate or transistor logic device, a discrete hardware component, or the like. The general purpose processor may be a microprocessor or the processor may be any conventional processor or the like, the processor being the control center of the appliance and connected to the various parts of the overall appliance by various interfaces and lines.

The memory may be used to store computer programs and/or modules, and the processor may implement various functions of the appliance by operating or executing the computer programs and/or modules stored in the memory and calling data stored in the memory. The memory may mainly include a storage program area and a storage data area, wherein the storage program area may store an operating system, an application program required by at least one function (such as a sound playing function, an image playing function, etc.), and the like; the storage data area may store data (such as audio data, a phonebook, etc.) created according to the use of the appliance, and the like. In addition, the memory may include high speed random access memory, and may also include non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) Card, a Flash memory Card (Flash Card), at least one magnetic disk storage device, a Flash memory device, or other volatile solid state storage device.

Computer-readable storage medium embodiments:

the computer program stored in the memory of the computer device may be stored in a computer-readable storage medium if it is implemented in the form of software functional units and sold or used as a stand-alone product. Based on such understanding, all or part of the processes in the method according to the above embodiments may be implemented by a computer program, which may be stored in a computer readable storage medium and used by a processor to implement the steps of the spatial real-time positioning method.

Wherein the computer program comprises computer program code, which may be in the form of source code, object code, an executable file or some intermediate form, etc. The computer readable medium may include: any entity or device capable of carrying computer program code, recording medium, U.S. disk, removable hard disk, magnetic disk, optical disk, computer Memory, Read-Only Memory (ROM), Random Access Memory (RAM), electrical carrier wave signals, telecommunications signals, software distribution media, and the like. It should be noted that the computer readable medium may contain other components which may be suitably increased or decreased as required by legislation and patent practice in jurisdictions, for example, in some jurisdictions, in accordance with legislation and patent practice, the computer readable medium does not include electrical carrier signals and telecommunications signals.

Finally, it should be emphasized that the present invention is not limited to the above-described embodiments, such as the specific process of the spatial real-time positioning method, the change of the type of positioning chip, etc., and these changes should also be included in the protection scope of the present claims.