Target tracking method and system based on AOA

Abstract

The invention relates to an AOA-based target tracking method and system. The method comprises the following steps: entering a target new stage after target equipment receives positioning information sent by an AOA base station for the first time; a target new stage: receiving the data packets sent by each AOA base station through the target equipment, and selecting the AOA base station with the maximum RSSI signal value to determine a target new position; in the target tracking stage, the target state and the observed value of the state covariance at each moment are obtained through the target equipment according to the determined AOA base station, so that the real-time position of the target equipment is determined; at each moment, determining a recursion value through recursion based on extended Kalman filtering according to the target state at the previous moment and the observed value of the state covariance; and a target extinction stage: carrying out traversal comparison on the observation value at each moment and the recursion value, and judging whether the positioning observation result is invalid or not so as to re-execute the target regeneration stage. Compared with the prior art, the method has the advantages of improving positioning accuracy, reliability, real-time performance, efficiency and the like.

Description

Technical Field

[0001] The present invention relates to the field of wireless positioning technology, and in particular to a target tracking method and system based on AOA. Background Art

[0002] In the field of wireless communication technology, positioning technology is an important application, which can realize the precise positioning of target devices, thereby providing more services and applications. Among them, Bluetooth technology is a technology widely used in the field of wireless communication, which can realize communication between devices by transmitting and receiving Bluetooth signals.

[0003] In the field of positioning technology, there is a positioning method based on the target angle of arrival (AOA). This method locates the target by calculating the direction angle of the target relative to the base station antenna. Specifically, the target sends a Bluetooth excitation signal to the AOA base station, and the base station calculates the direction angle of the target relative to the base station antenna based on the signal and transmits the direction angle back to the target. In three-dimensional positioning, the target can calculate its own position through three direction angles.

[0004] The complexity of this algorithm is very low. The target position can only be calculated when the angle information provided by the base station is absolutely accurate. However, in most cases, due to the multipath effect of the Bluetooth signal, or the measurement error of the base station itself, or the Bluetooth signal packet loss, it will have a great impact on the positioning effect, causing the final positioning result to deviate from the true value. Summary of the invention

[0005] The purpose of the present invention is to provide a target tracking method and system based on AOA in order to overcome the defects of the above-mentioned prior art that the positioning results deviate from the true value due to the multipath effect of Bluetooth signals, measurement errors in the base station itself or Bluetooth signal packet loss.

[0006] The purpose of the present invention can be achieved by the following technical solutions:

[0007] A target tracking method based on AOA includes the following steps:

[0008] When the target device receives the positioning information sent by the AOA base station for the first time, it enters the target rebirth stage;

[0009] Target newborn stage: The target device receives the data packets sent by each AOA base station, selects the AOA base station with the largest RSSI signal value to determine the target newborn position, and then enters the target tracking stage;

[0010] Target tracking stage: The target device obtains the observed values ​​of the target state and state covariance at each moment according to the determined AOA base station, and the target state is the azimuth value sent by the AOA base station, and the real-time position of the target device is determined according to the observed values; at each moment, the target state and state covariance observed values ​​at the previous moment are recursively determined based on the extended Kalman filter, and the recursive values ​​of the target state and state covariance at the current moment are recursively determined;

[0011] Target extinction phase: the observed value at each moment is compared with the recursive value. If the difference between the observed value and the recursive value is greater than the preset abnormal threshold, the observed value is judged to be abnormal; if all the observation values ​​used for target positioning are abnormal, the positioning observation result at the current moment is invalid; if invalid positioning observation results that reach the invalid threshold number appear continuously, the target rebirth phase is re-executed.

[0012] Furthermore, in the target tracking stage, the process of determining the target state at the current moment and the recursive value of the state covariance is specifically as follows:

[0013] Calculate the target predicted state X1 at the current moment through the target state X0 at the previous moment according to the state transfer matrix A;

[0014] The state prediction covariance matrix P1 of the current moment is calculated based on the state covariance P0 of the previous moment according to the state transfer matrix A and the prediction noise Q;

[0015] Calculate the Jacobian matrix H of the recursive vector h, and calculate the Kalman gain coefficient K based on the Jacobian matrix H, the current state prediction covariance matrix P1, and the observation noise R;

[0016] According to the observation vector Z, Jacobian matrix H, the predicted state X1 at the current moment, and the Kalman gain coefficient K, the actual state X at the current moment can be obtained;

[0017] The actual covariance P2 at the current moment is calculated based on the Kalman gain coefficient K, the current state prediction covariance matrix P1 and the Jacobian matrix H.

[0018] Furthermore, the actual state X at the current moment and the actual covariance P2 at the current moment obtained by solving are used as the recursive value.

[0019] Furthermore, in the target new generation stage, data packets sent by each AOA base station are received within a preset base station selection queue window.

[0020] Furthermore, the base station selects the length of the queue window according to actual conditions.

[0021] Furthermore, each AOA base station is connected to the target device via wireless communication.

[0022] Further, the wireless communication is Bluetooth communication or ultra-wideband communication.

[0023] The present invention also provides a target tracking system based on AOA, including:

[0024] An initialization module, configured to enter the target newborn module after the target device first receives the positioning information sent by the AOA base station;

[0025] A target newborn module, configured to receive data packets sent by each AOA base station through the target device, select the AOA base station with the largest RSSI signal value to determine the target newborn position, and then enter the target tracking module;

[0026] A target tracking module, configured to obtain the observation values of the target state and state covariance at each moment through the target device according to the determined AOA base station, where the target state is the direction angle value sent by the AOA base station, and determine the real-time position of the target device according to the observation values; at each moment, based on the extended Kalman filter, according to the observation values of the target state and state covariance at the previous moment, recursively determine the recursive values of the target state and state covariance at the current moment;

[0027] A target extinction module, configured to traverse and compare the observation value and the recursive value at each moment. If the difference between the observation value and the recursive value is greater than a preset abnormal threshold, it is determined that the observation value is abnormal; if all the observation values for target positioning are abnormal, the positioning observation result at the current moment is invalid; if there are continuously invalid positioning observation results reaching the invalid threshold quantity, the target newborn module is re-executed.

[0028] Further, in the target tracking module, the process of determining the recursive values of the target state and state covariance at the current moment is specifically as follows:

[0029] Calculate the target predicted state X1 at the current moment through the target state X0 at the previous moment according to the state transition matrix A;

[0030] Calculate the state prediction covariance matrix P1 at the current moment through the state covariance P0 at the previous moment according to the state transition matrix A and the prediction noise Q;

[0031] Obtain the Jacobian matrix H of the recursive vector h, and calculate the Kalman gain coefficient K according to the Jacobian matrix H, the state prediction covariance matrix P1 at the current moment, and the observation noise R;

[0032] The actual state X at the current moment can be obtained according to the observation vector Z, the Jacobian matrix H, the predicted state X1 at the current moment, and the Kalman gain coefficient K;

[0033] According to the Kalman gain coefficient K, the current state prediction covariance matrix P1 and the Jacobian matrix H, the actual covariance P2 at the current moment is calculated;

[0034] The actual state X at the current moment and the actual covariance P2 at the current moment obtained by solving are used as the recursive value.

[0035] Further, the target new generation module receives data packets sent by each AOA base station in a preset base station selection queue window;

[0036] The length of the base station selection queue window is selected according to actual conditions;

[0037] Each AOA base station connects to the target device via wireless communication;

[0038] The wireless communication is Bluetooth communication or ultra-wideband communication.

[0039] Compared with the prior art, the present invention has the following advantages:

[0040] (1) Based on the AOA positioning algorithm, the present invention adopts an extended Kalman filter to recursively determine the target state and state covariance at the current moment according to the target state and state covariance at the previous moment through the state transfer matrix and the Kalman gain coefficient, so as to compare them with the observed values ​​at the current moment to determine whether the current positioning observation result is valid. It can effectively solve the problems of Bluetooth signal multipath effect, base station measurement error and signal packet loss, thereby improving the accuracy and reliability of positioning.

[0041] (2) The present invention divides the positioning process into three stages: target creation, target tracking, and target extinction, which can better adapt to the motion state of the target and improve the real-time performance and efficiency of positioning.

[0042] (3) The present invention has functions such as base station selection, target tracking and positioning area restriction. If it is determined that the positioning observation results have failed too many times, the optimal base station can be selected for positioning according to the actual situation, thereby improving the accuracy and efficiency of positioning.

[0043] (4) The algorithm complexity of the present invention is relatively low, and it can quickly obtain positioning results when processing a large amount of base station data, thereby improving the real-time performance of positioning.

[0044] (5) The positioning method of the present invention is not only applicable to Bluetooth signals, but can also be applied to other types of wireless signals, and has strong versatility and practicality. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 A schematic diagram of a flow chart of a target tracking method based on AOA provided in an embodiment of the present invention;

[0046] Figure 2 A schematic diagram of a three-dimensional positioning principle provided in an embodiment of the present invention;

[0047] Figure 3 A schematic diagram of a two-dimensional positioning principle provided in an embodiment of the present invention;

[0048] Figure 4 The figure is a schematic diagram of a judgment process of a target tracking method based on AOA provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0049] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings here can be arranged and designed in various different configurations.

[0050] Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the invention claimed for protection, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0051] It should be noted that similar reference numerals and letters denote similar items in the following drawings, and therefore, once an item is defined in one drawing, further definition and explanation thereof is not required in subsequent drawings.

[0052] Example 1

[0053] like Figure 1 and Figure 4 As shown, this embodiment provides a target tracking method based on AOA, including the following steps:

[0054] S1: When the target device receives the positioning information sent by the AOA base station for the first time, it enters the target rebirth stage;

[0055] Target new phase S2: The target device receives data packets sent by each AOA base station, selects the AOA base station with the largest RSSI signal value to determine the target new position, and then enters the target tracking phase S3;

[0056] When three AOA base stations are used to locate the target, the positioning process and principle are as follows: Figure 2 As shown;

[0057] When two AOA base stations are used to locate the target, the positioning process and principle are as follows: Figure 3 As shown;.

[0058] Target tracking stage S3: The target device obtains the observed values ​​of the target state and state covariance at each moment according to the determined AOA base station. The target state is the azimuth value sent by the AOA base station, and the real-time position of the target device is determined according to the observed values. At each moment, the target state and state covariance observed values ​​at the previous moment are recursively determined based on the extended Kalman filter, and the recursive values ​​of the target state and state covariance at the current moment are recursively determined.

[0059] Target extinction stage S4: The observed value at each moment is traversed and compared with the recursive value. If the difference between the observed value and the recursive value is greater than the preset abnormal threshold, the observed value is judged to be abnormal; if all the observation values ​​used for target positioning are abnormal, the positioning observation result at the current moment is invalid; if invalid positioning observation results that reach the invalid threshold number appear continuously, the target rebirth stage S2 is re-executed.

[0060] The target rebirth stage S2 is specifically as follows: the target enters the target rebirth stage after receiving the positioning information sent by the AOA base station for the first time. The target needs to select the angle value given by the base station with the largest RSSI signal value from all the base station Bluetooth data packets received in the rebirth stage for calculation, and the calculation result is used as the target rebirth position.

[0061] When the target is newly generated, it enters the tracking phase. At this time, the base station needs to select the base station with the largest RSSI in a certain period of time, and then use this base station and the adjacent base stations as the positioning base stations for this time. In the EKF algorithm, because there is no other control signal, the constant speed model is used.

[0062] In the target tracking stage S3, the process of determining the target state and the recursive value of the state covariance at the current moment is specifically as follows:

[0063] Calculate the target predicted state X1 at the current moment through the target state X0 at the previous moment according to the state transfer matrix A;

[0064] The state prediction covariance matrix P1 of the current moment is calculated based on the state covariance P0 of the previous moment according to the state transfer matrix A and the prediction noise Q;

[0065] Calculate the Jacobian matrix H of the recursive vector h, and calculate the Kalman gain coefficient K based on the Jacobian matrix H, the current state prediction covariance matrix P1, and the observation noise R;

[0066] According to the observation vector Z, Jacobian matrix H, the predicted state X1 at the current moment, and the Kalman gain coefficient K, the actual state X at the current moment can be obtained;

[0067] The actual covariance P2 at the current moment is calculated based on the Kalman gain coefficient K, the current state prediction covariance matrix P1 and the Jacobian matrix H.

[0068] The actual state X at the current moment and the actual covariance P2 at the current moment obtained by solving are used as recursive values.

[0069] The specific calculation expression corresponds to:

[0070] predict:

[0071] P′ k =AP k-1 A T +Q

[0072] Calibration: K k =P′ k H T (HP′ k H T +R) -1

[0073] x k = x′ K +K k (Z k -Hx′ k )

[0074] P k =(IK k H)P′ k

[0075] The parameters in the formula are explained as follows:

[0076] x k : The state at time k

[0077] Prediction status at time k (not optimized)

[0078] Optimized prediction status at time k

[0079] State update at time k (state - predicted state)

[0080] Z k : Observation value at time k

[0081] Predicted observation value at time k (not optimized)

[0082] Optimized predicted observations at time k

[0083] Observational innovation at time k (observation - predicted observation)

[0084] A state transition matrix

[0085] B: Input Control Matrix

[0086] H observation matrix

[0087] Q: prediction noise covariance matrix

[0088] R: Observation noise covariance matrix

[0089] P: Error matrix

[0090] K k : Kalman gain (observation weight) at time k

[0091] ω k : Process noise at time k

[0092] v k : Observation noise at time k

[0093] u k : The effect of the outside world on the system at time k

[0094] Finally, the observed value and the recursive value are traversed and compared. If the difference between the two is too large, the observed value is considered to be abnormal. When all the observed values ​​are judged to be abnormal, the frame positioning observation result is considered invalid. When the solver receives multiple invalid positioning messages in succession, the system regenerates the target.

[0095] In the target newborn stage, the data packets sent by each AOA base station are received in the preset base station selection queue window. The length of the base station selection queue window is selected according to the actual situation.

[0096] That is, the length of the base station selection queue window is calculated based on the actual situation. The longer the window, the more stable the positioning result, but the positioning delay will be higher. Conversely, the delay is low, but the fluctuation of the positioning result will be more obvious.

[0097] The AOA base station in this solution connects to the target device via wireless communication; the wireless communication method is not only applicable to Bluetooth signals, but can also be applied to other types of wireless signals, and has strong versatility and practicality.

[0098] Example 2

[0099] This embodiment provides a target tracking system based on AOA, including:

[0100] An initialization module is used to enter a target new module after the target device receives the positioning information sent by the AOA base station for the first time;

[0101] The target regeneration module is used to receive data packets sent by each AOA base station through the target device, select the AOA base station with the largest RSSI signal value to determine the target regeneration position, and then enter the target tracking module;

[0102] The target tracking module is used to obtain the observed values ​​of the target state and state covariance at each moment through the target device according to the determined AOA base station. The target state is the azimuth value sent by the AOA base station, and the real-time position of the target device is determined according to the observed values; at each moment, based on the extended Kalman filter, the recursive values ​​of the target state and state covariance at the previous moment are determined by recursion;

[0103] The target extinction module is used to traverse and compare the observed value at each moment with the recursive value. If the difference between the observed value and the recursive value is greater than the preset abnormal threshold, the observation value is judged to be abnormal; if all the observation values ​​used for target positioning are abnormal, the positioning observation result at the current moment is invalid; if invalid positioning observation results appear continuously and reach the invalid threshold number, the target regeneration module is re-executed.

[0104] It should be noted that the specific content and beneficial effects of the system of the present application can be found in the above-mentioned method embodiment, which will not be repeated here.

[0105] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.

Claims

1. A target tracking method based on AOA, characterized in that: The following steps are involved: When the target device receives the positioning information sent by the AOA base station for the first time, it enters the target rebirth stage; Target newborn stage: The target device receives the data packets sent by each AOA base station, selects the AOA base station with the largest RSSI signal value to determine the target newborn position, and then enters the target tracking stage; Target tracking stage: The target device obtains the observed values ​​of the target state and state covariance at each moment according to the determined AOA base station, wherein the target state is the azimuth value sent by the AOA base station, and the real-time position of the target device is determined according to the observed values; At each moment, based on the extended Kalman filter, according to the observed values ​​of the target state and state covariance at the previous moment, the recursive values ​​of the target state and state covariance at the current moment are determined by recursion; Target extinction stage: the observed value at each moment is compared with the recursive value. If the difference between the observed value and the recursive value is greater than the preset abnormal threshold, the observed value is judged to be abnormal. If all the observation values ​​used for target positioning are abnormal, the positioning observation results at the current moment are invalid; If invalid positioning observation results that reach the invalid threshold number appear continuously, the target regeneration phase is re-executed.

2. The target tracking method based on AOA according to claim 1, characterized in that: In the target tracking phase, the process of determining the target state at the current moment and the recursive value of the state covariance is specifically as follows: Calculate the target predicted state X1 at the current moment through the target state X0 at the previous moment according to the state transfer matrix A; The state prediction covariance matrix P1 of the current moment is calculated based on the state covariance P0 of the previous moment according to the state transfer matrix A and the prediction noise Q; Calculate the Jacobian matrix H of the recursive vector h, and calculate the Kalman gain coefficient K based on the Jacobian matrix H, the current state prediction covariance matrix P1, and the observation noise R; According to the observation vector Z, Jacobian matrix H, the predicted state X1 at the current moment, and the Kalman gain coefficient K, the actual state X at the current moment can be obtained; The actual covariance P2 at the current moment is calculated based on the Kalman gain coefficient K, the current state prediction covariance matrix P1 and the Jacobian matrix H.

3. The target tracking method based on AOA according to claim 2, characterized in that: The actual state X at the current moment and the actual covariance P2 at the current moment obtained by solving are used as the recursive values.

4. The target tracking method based on AOA according to claim 1, characterized in that: The target new generation stage receives data packets sent by each AOA base station in a preset base station selection queue window.

5. The target tracking method based on AOA according to claim 1, characterized in that: The length of the base station selection queue window is selected according to actual conditions.

6. The target tracking method based on AOA according to claim 1, characterized in that: Each AOA base station connects to the target device via wireless communication.

7. The target tracking method based on AOA according to claim 6, characterized in that: The wireless communication is Bluetooth communication or ultra-wideband communication.

8. A target tracking system based on AOA, characterized in that: include: An initialization module is used to enter a target new module after the target device receives the positioning information sent by the AOA base station for the first time; The target regeneration module is used to receive data packets sent by each AOA base station through the target device, select the AOA base station with the largest RSSI signal value to determine the target regeneration position, and then enter the target tracking module; A target tracking module is used to obtain the observed value of the target state and state covariance at each moment through the target device according to the determined AOA base station, wherein the target state is the direction angle value sent by the AOA base station, and the real-time position of the target device is determined according to the observed value; At each moment, based on the extended Kalman filter, according to the observed values ​​of the target state and state covariance at the previous moment, the recursive values ​​of the target state and state covariance at the current moment are determined by recursion; The target extinction module is used to traverse and compare the observed value at each moment with the recursive value. If the difference between the observed value and the recursive value is greater than the preset abnormal threshold, the observed value is judged to be abnormal; If all the observation values ​​used for target positioning are abnormal, the positioning observation results at the current moment are invalid; If invalid positioning observation results that reach the invalid threshold number appear continuously, the target new generation module is re-executed.

9. The AOA-based target tracking system according to claim 8, characterized in that: In the target tracking module, the process of determining the target state at the current moment and the recursive value of the state covariance is specifically as follows: Calculate the target predicted state X1 at the current moment through the target state X0 at the previous moment according to the state transfer matrix A; The state prediction covariance matrix P1 of the current moment is calculated based on the state covariance P0 of the previous moment according to the state transfer matrix A and the prediction noise Q; Calculate the Jacobian matrix H of the recursive vector h, and calculate the Kalman gain coefficient K based on the Jacobian matrix H, the current state prediction covariance matrix P1, and the observation noise R; According to the observation vector Z, Jacobian matrix H, the predicted state X1 at the current moment, and the Kalman gain coefficient K, the actual state X at the current moment can be obtained; According to the Kalman gain coefficient K, the current state prediction covariance matrix P1 and the Jacobian matrix H, the actual covariance P2 at the current moment is calculated; The actual state X at the current moment and the actual covariance P2 at the current moment obtained by solving are used as the recursive value.

10. The AOA-based target tracking system according to claim 8, characterized in that: The target new generation module receives data packets sent by each AOA base station in a preset base station selection queue window; The length of the base station selection queue window is selected according to actual conditions; Each AOA base station connects to the target device via wireless communication; The wireless communication is Bluetooth communication or ultra-wideband communication.

Images