Positioning methods, devices, equipment, media and procedures products

Abstract

This invention provides a positioning method, apparatus, device, medium, and program product. The method includes: acquiring a preliminary positioning result of a detected target using at least one of an Angle of Arrival (AOA) base station, a Time Difference of Arrival (TDOA) base station, and a detected target equipped with an ultra-wideband sensor and an inertial measurement unit; acquiring a spatial weighted value and a temporal weighted value of the detected target; adjusting the observation noise covariance based on the spatial weighted value; and adjusting the process noise covariance based on the temporal weighted value; calculating a Kalman gain based on a state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance, wherein the state transition matrix is ​​constructed from the acceleration data of the detected target collected by the ultra-wideband sensor; and obtaining a final positioning result of the detected target based on the Kalman gain and the preliminary positioning result. This method improves the accuracy of the positioning result.

Description

Technical Field

[0001] This invention relates to the field of positioning technology, and in particular to a positioning method, apparatus, device, medium, and program product. Background Technology

[0002] Existing UAV positioning methods include: Time Difference of Arrival (TDOA) positioning: Calculating the position of the transmitter based on the time difference between the signal emitted from the transmitter and its arrival at multiple receiving stations; Angle of Arrival (AOA) positioning: Estimating angle information by analyzing the phase difference of the signal arriving at each antenna based on the signal acquired from the receiver in the hangar, and then calculating the position of the transmitter based on the angle information; Inertial Measurement Unit (IMU) positioning: Calculating the distance traveled by the UAV during this time period by integrating the acceleration and heading angle information collected by the accelerometer and gyroscope, thereby achieving positioning.

[0003] In existing methods, TDOA and AOA are affected by external factors such as distance and signal reflection, resulting in continuous errors in the detection results. IMU positioning, on the other hand, suffers from the problem of increasing errors over time. Summary of the Invention

[0004] The purpose of this invention is to provide a positioning method, device, equipment, medium, and program product to solve the problems of large positioning error and low accuracy in existing positioning methods.

[0005] To achieve the above objectives, in a first aspect, embodiments of the present invention provide a positioning method, comprising: The preliminary positioning result of the detected target is obtained by using at least one of the following: Angle of Arrival (AOA) base station, Time Difference of Arrival (TDOA) base station, and a detected target equipped with an ultra-wideband sensor and an inertial measurement unit. Obtain the spatial weighted value and time weighted value of the detected target; adjust the observation noise covariance based on the spatial weighted value; and adjust the process noise covariance based on the time weighted value; The Kalman gain is calculated based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance. The state transition matrix is ​​constructed from the acceleration data of the detected target collected by the ultra-wideband sensor. Based on the Kalman gain and the preliminary localization result, the final localization result of the detected target is obtained.

[0006] In some embodiments, obtaining preliminary positioning results of the detected target via an angle-of-arrival (AOA) base station includes: Using each of the AOA base stations within the target monitoring area, the phase difference of the received wireless signal transmitted by the detected target is measured, and the azimuth and elevation angles of the detected target relative to the base station are calculated. N AOA base stations are deployed in the target monitoring area, where N is a positive integer greater than or equal to 3, and the N AOA base stations are not collinear. Based on the spatial coordinates of the AOA base station, the azimuth angle, and the elevation angle, a position trajectory ray is constructed; Based on N position trajectory rays, a nonlinear least squares algorithm is used to calculate the preliminary positioning result of the detected target.

[0007] In some embodiments, obtaining preliminary location results of the detected target by using a Time Difference of Arrival (TDOA) base station includes: Using each TDOA base station within the target monitoring area, the time difference between the detected target and the TDOA base station is calculated based on the received wireless signal transmitted by the detected target. M TDOA base stations are deployed in the target monitoring area, where M is a positive integer greater than or equal to 4, and the M TDOA base stations are non-coplanar. Based on the time difference between the detected target and different TDOA base stations and the spatial coordinates of each TDOA base station, M-1 hyperboloids are constructed; Based on the M-1 hyperboloids, a preset analytical positioning algorithm is used to calculate the preliminary positioning result of the detected target.

[0008] In some embodiments, the ultra-wideband sensor and inertial measurement unit are used to output motion state data of the detected target, the motion state data including preliminary positioning results and acceleration data of the detected target.

[0009] In some embodiments, obtaining the spatial weighted value and time weighted value of the detected target includes: Based on the preliminary positioning result of the detected target obtained through the AOA base station and the spatial coordinates of the AOA base station, the spatial weighted value of the detected target is calculated; or, based on the preliminary positioning result of the detected target obtained through the TDOA base station and the spatial coordinates of the TDOA base station, the spatial weighted value of the detected target is calculated. The time-weighted value of the detected target is calculated based on the current time and the initial time of the ultra-wideband sensor, where the initial time of the ultra-wideband sensor is the location signal disconnection time.

[0010] In some embodiments, calculating the Kalman gain based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance includes: The prediction error covariance is obtained based on the state transition matrix and the adjusted process noise covariance. The Kalman gain is calculated based on the adjusted observation noise covariance and the prediction error covariance.

[0011] In some embodiments, obtaining the final location result of the detected target based on the Kalman gain and the preliminary location result includes: If the preliminary positioning result is obtained through the AOA base station, the TDOA base station, or a target equipped with an ultra-wideband sensor and an inertial measurement unit, the final positioning result of the detected target is calculated based on the Kalman gain and the preliminary positioning result; or, If the preliminary positioning result is obtained by at least two of the AOA base station, the TDOA base station, and the detection target equipped with an ultra-wideband sensor and an inertial measurement unit, the preliminary positioning result with the highest weight is selected from them, and the final positioning result of the detection target is calculated based on the Kalman gain and the preliminary positioning result with the highest weight.

[0012] In some embodiments, the method further includes: Based on the Kalman gain, the prediction error covariance is updated to obtain the actual error covariance, which is used for the prediction of the next error covariance.

[0013] Secondly, embodiments of the present invention also provide a positioning device, comprising: The first acquisition module is used to acquire the preliminary positioning result of the detected target by at least one of the angle of arrival (AOA) base station, time difference of arrival (TDOA) base station, and a detected target equipped with an ultra-wideband sensor and an inertial measurement unit; The second acquisition module is used to acquire the spatial weighted value and the temporal weighted value of the detected target; adjust the observation noise covariance based on the spatial weighted value; and adjust the process noise covariance based on the temporal weighted value. The calculation module is used to calculate the Kalman gain based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance. The state transition matrix is ​​constructed from the acceleration data of the detected target collected by the ultra-wideband sensor. The positioning module is used to obtain the final positioning result of the detected target based on the Kalman gain and the preliminary positioning result.

[0014] Thirdly, embodiments of the present invention also provide a positioning device, including a processor and a transceiver, wherein the transceiver receives and transmits data under the control of the processor, and the processor is configured to perform the following operations: The preliminary positioning result of the detected target is obtained by using at least one of the following: Angle of Arrival (AOA) base station, Time Difference of Arrival (TDOA) base station, and a detected target equipped with an ultra-wideband sensor and an inertial measurement unit. Obtain the spatial weighted value and time weighted value of the detected target; adjust the observation noise covariance based on the spatial weighted value; and adjust the process noise covariance based on the time weighted value; The Kalman gain is calculated based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance. The state transition matrix is ​​constructed from the acceleration data of the detected target collected by the ultra-wideband sensor. Based on the Kalman gain and the preliminary localization result, the final localization result of the detected target is obtained.

[0015] Fourthly, embodiments of the present invention also provide a positioning device, including a memory, a processor, and a program stored in the memory and executable on the processor; when the processor executes the program, it implements the positioning method as described in the first aspect.

[0016] Fifthly, embodiments of the present invention also provide a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the positioning method as described in the first aspect.

[0017] In a sixth aspect, embodiments of the present invention also provide a computer program product, including computer instructions, which, when executed by a processor, implement the steps in the positioning method as described in the first aspect.

[0018] The above-described technical solution of the present invention has at least the following beneficial effects: In this embodiment of the invention, a preliminary positioning result of the detected target is obtained by using at least one of an Angle of Arrival (AOA) base station, a Time Difference of Arrival (TDOA) base station, and a detection target equipped with an ultra-wideband sensor and an inertial measurement unit; the spatial weighting value and the temporal weighting value of the detected target are obtained; the observation noise covariance is adjusted based on the spatial weighting value; and the process noise covariance is adjusted based on the temporal weighting value; the Kalman gain is calculated based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance, wherein the state transition matrix is ​​constructed from the acceleration data of the detected target collected by the ultra-wideband sensor; and the detected target is obtained based on the Kalman gain and the preliminary positioning result. The final positioning result of the target is obtained by dynamically fusing the preliminary positioning results obtained through the AOA base station, the preliminary positioning results obtained through the TDOA base station, and / or the preliminary positioning results obtained through the detection target equipped with an ultra-wideband sensor and an inertial measurement unit. This allows for the extraction of the advantages of AOA, TDOA, and IMU, while mitigating the measurement problems of AOA, TDOA, and IMU. Furthermore, a dual weighting mechanism of spatial and temporal weighting is introduced to drive the real-time adaptive adjustment of the process noise covariance and observation noise covariance of the Kalman filter. This achieves the technical effect of maintaining robust positioning at the centimeter to decimeter level even under conditions of multipath interference, sparse base stations, or no external correction for a long time, greatly improving the accuracy of the positioning results. Attached Figure Description

[0019] Figure 1 A flowchart illustrating the positioning method according to an embodiment of the present invention; Figure 2 This is a schematic diagram illustrating the overall design process of the present invention; Figure 3 A schematic diagram of the positioning device according to an embodiment of the present invention; Figure 4 This is a schematic diagram illustrating the hardware structure of a positioning device according to an embodiment of the present invention. Detailed Implementation

[0020] To make the technical problems, technical solutions and advantages of the present invention clearer, a detailed description will be given below in conjunction with the accompanying drawings and specific embodiments.

[0021] To address the aforementioned technical problems, this invention provides a positioning method, apparatus, device, medium, and program product. The method and apparatus are based on the same concept as described in the application. Since the principles by which the method and apparatus solve the problem are similar, their implementations can be mutually referenced, and repeated details will not be elaborated further.

[0022] like Figure 1 As shown, an embodiment of the present invention provides a positioning method, which may include: Step 101: Obtain the preliminary positioning result of the detected target by at least one of the angle of arrival (AOA) base station, time difference of arrival (TDOA) base station, and a detected target equipped with an ultra-wideband sensor and an inertial measurement unit; As an optional implementation, the preliminary positioning result of the detected target is obtained through an angle-of-arrival (AOA) base station, including: Using each of the AOA base stations within the target monitoring area, the phase difference of the received wireless signal transmitted by the detected target is measured, and the azimuth and elevation angles of the detected target relative to the base station are calculated. N AOA base stations are deployed in the target monitoring area, where N is a positive integer greater than or equal to 3, and the N AOA base stations are not collinear. Here, no fewer than three AOA base stations are deployed within the target monitoring area. Each base station must have a high-precision antenna array (such as a uniform linear array or a uniform circular array) to support phase difference measurement of incident wireless signals (such as UWB, Wi-Fi, or 5G NR), thereby calculating the azimuth and elevation angles of the signal source (i.e., the detected target) relative to the base station. The AOA base stations are spatially geometrically non-collinear to ensure the observability of three-dimensional positioning.

[0023] Based on the spatial coordinates of the AOA base station, the azimuth angle, and the elevation angle, a position trajectory ray is constructed; Here, the spatial coordinates of the AOA base station need to be calibrated through high-precision mapping or real-time dynamic differential positioning (RTK) - Global Navigation Satellite System (GNSS), and the timestamps need to be synchronized to the nanosecond level.

[0024] After each AOA base station receives the wireless signal emitted by the target (such as a drone), it uses the known spatial coordinates P of the AOA base station to... i (x i ,y i ,z i and the calculated direction vector ,in The pitch angle, Let L be the azimuth angle. A ray (i.e., the position trajectory ray) can be constructed starting from the AOA base station and extending along the direction vector: L i =P i +d i The ray represents the spatial trajectory where the signal source may be located.

[0025] Based on N position trajectory rays, a nonlinear least squares algorithm is used to calculate the preliminary positioning result of the detected target.

[0026] It should be noted that three or more location trajectory rays can be obtained through three or more AOA base stations. Normally, due to noise, they will not intersect at a single point. Ideally, multiple AOA rays should converge at the location of the actual signal source (i.e., the detected target). However, due to factors such as multipath effects, antenna calibration errors, and environmental interference, the rays are usually not concurring at a single point, forming an "approximate intersection" problem. To solve this problem, this invention employs a nonlinear least squares algorithm to calculate the preliminary location result of the detected target. Specifically, the nonlinear least squares algorithm is used to obtain the position corresponding to the minimum distance from N location trajectory rays, which is the preliminary location result of the detected target. Here, the objective function is defined as the distance from the estimated position (i.e., the position of the detected target) x to all AOA rays L. i The sum of weighted squared distances:

[0027] in, Let x represent the orthogonal projection of x onto the i-th ray. This invention employs the LM algorithm of nonlinear least squares to iteratively solve this nonconvex optimization problem. Because it combines the stability of gradient descent with the fast convergence of the Gauss-Newton method, it is suitable for medium-scale nonlinear systems. The final output position x is used as the AOA fusion positioning result (i.e., the preliminary positioning result obtained through AOA base stations).

[0028] As an optional implementation, the preliminary location result of the detected target is obtained by using a Time Difference of Arrival (TDOA) base station, including: Using each TDOA base station within the target monitoring area, the time difference between the detected target and the TDOA base station is calculated based on the received wireless signal transmitted by the detected target. M TDOA base stations are deployed in the target monitoring area, where M is a positive integer greater than or equal to 4, and the M TDOA base stations are non-coplanar. Here, four or more TDOA base stations are deployed (or constructed) within the target monitoring area, and their spatial layout must meet the non-coplanar condition (i.e., no four base stations are coplanar) to ensure the unique interpretability of the hyperboloid intersection points in three-dimensional space. High-precision clock synchronization must be achieved between base stations (e.g., through Precision Time Protocol (PTP) or fiber optic synchronization), with time synchronization errors controlled at the sub-nanosecond level to support centimeter-level positioning accuracy.

[0029] Based on the time difference between the detected target and different TDOA base stations and the spatial coordinates of each TDOA base station, M-1 hyperboloids are constructed; Here, when a TDOA base station receives a wireless signal transmitted by a detected target, it can calculate the time difference between the target and different TDOA base stations. Then, by combining this with the spatial coordinates of the TDOA base stations, it can be determined that the detected target is located on a fixed hyperboloid. Using a reference TDOA base station (e.g., base station 0) as a baseline, the remaining TDOA base stations i form a TDOA pair with it, and the time difference is measured. Corresponding distance difference (c speed of light). The detected target's position x satisfies:

[0030] The equation describes a hyperboloid of revolution with the two base stations as foci, on which the signal source (i.e. the detection target) must lie.

[0031] Three or more fixed hyperboloids can be obtained using four or more TDOA base stations. Three independent TDOA observations (e.g., 1-0, 2-0, 3-0) can be constructed using four TDOA base stations, corresponding to three hyperboloids.

[0032] Based on the M-1 hyperboloids, a preset analytical positioning algorithm is used to calculate the preliminary positioning result of the detected target.

[0033] It should be noted that, theoretically, M-1 (e.g., 3) non-degenerate hyperboloids intersect at two points in three-dimensional space (mirror image solution). Based on the obtained M-1 hyperboloids, a pre-defined analytical positioning algorithm (such as the Chan-Ho algorithm) is used to transform the nonlinear hyperbolic equations into a pseudo-linear system, which is then solved using weighted least squares (WLS) to obtain the preliminary positioning result of the detected target. This algorithm has low computational complexity and is not sensitive to initial values. The intersection points of the hyperboloids are calculated, and the final output position x is used as the TDOA fusion positioning result (i.e., the preliminary positioning result obtained through TDOA base stations).

[0034] Optionally, the ultra-wideband (UWB) sensor and inertial measurement unit (IMU) are used to output the motion state data of the detected target, which includes the preliminary positioning result and acceleration data of the detected target. Specifically, the preliminary positioning result (i.e., the position of the detected target) and acceleration data of the detected target can be obtained through the UWB sensor. The detected target (such as a UAV) of the present invention is equipped with UWB and IMU (i.e., IMU+UWB transceiver module) and can output the motion state data of the detected target in real time. Among them, the motion state data includes the position of the detected target itself (i.e., the preliminary positioning result of the detected target), three-axis acceleration, angular velocity and other state information. The UWB sensor can provide relative position data with a high update rate (≥100Hz) and can provide short-time high-precision dynamic response. The fusion of the two improves the occlusion capability.

[0035] It should be noted that the preliminary positioning results of the detected target include the preliminary positioning results obtained through the AOA base station, the preliminary positioning results obtained through the TDOA base station, and / or the preliminary positioning results obtained through the detected target equipped with an ultra-wideband sensor and an inertial measurement unit.

[0036] Step 102: Obtain the spatial weighted value and time weighted value of the detected target; adjust the observation noise covariance based on the spatial weighted value; and adjust the process noise covariance based on the time weighted value. Here, the spatial weighting of the detected target reflects the reliability of AOA or TDOA positioning under the current geometric configuration. The farther the base station is from the detected target, the lower the weight is considered to be. Through spatial weighting, the weighting factor of the detected target based on AOA or TDOA base stations is different in different spaces.

[0037] The time-weighted value of the detected target is dynamically reflected by an exponential decay function, starting from the moment the positioning signal (GPS signal) is interrupted. Through time weighting, the weighting factor changes synchronously with the increase of time in the IMU-based measurement value.

[0038] This invention innovatively introduces a dual weighting mechanism of "spatial weighting" and "temporal weighting (i.e., time decay weighting)" to drive the real-time adaptive adjustment of the process noise covariance (Q) and observation noise covariance (R) of the Kalman filter. This technique solves the problem of positioning divergence or sharp drop in accuracy caused by fixed sensor weights and rigid noise models in existing fusion positioning systems under complex electromagnetic environments or GNSS denial scenarios. It achieves robust positioning at the centimeter to decimeter level even under conditions of multipath interference, sparse base stations, or no external correction for extended periods.

[0039] Step 103: Calculate the Kalman gain based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance. The state transition matrix is ​​constructed from the acceleration data of the detected target collected by the ultra-wideband sensor. Step 104: Based on the Kalman gain and the preliminary localization result, the final localization result of the detected target is obtained.

[0040] It should be noted that by dynamically fusing the preliminary positioning results obtained through the AOA base station, the preliminary positioning results obtained through the TDOA base station, and / or the preliminary positioning results obtained through the detection target equipped with an ultra-wideband sensor and an inertial measurement unit, the advantages of AOA, TDOA, and IMU can be extracted, the measurement problems of AOA, TDOA, and IMU can be weakened, and higher accuracy positioning capability can be achieved.

[0041] The positioning method of this invention obtains a preliminary positioning result of the detected target by at least one of the following: Angle of Arrival (AOA) base station, Time Difference of Arrival (TDOA) base station, and a detection target equipped with an ultra-wideband sensor and an inertial measurement unit (IMU); obtains the spatial weighting value and time weighting value of the detected target; adjusts the observation noise covariance based on the spatial weighting value; and adjusts the process noise covariance based on the time weighting value; calculates the Kalman gain based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance, wherein the state transition matrix is ​​constructed from the acceleration data of the detected target collected by the ultra-wideband sensor; and obtains the target positioning result based on the Kalman gain and the preliminary positioning result. The final positioning result of the target is obtained by dynamically fusing the preliminary positioning results obtained through the AOA base station, the preliminary positioning results obtained through the TDOA base station, and / or the preliminary positioning results obtained through the target detection equipped with an ultra-wideband sensor and an inertial measurement unit. This allows for the extraction of the advantages of AOA, TDOA, and IMU, while mitigating the measurement problems of AOA, TDOA, and IMU. Furthermore, a dual weighting mechanism of spatial and temporal weighting is introduced to drive the real-time adaptive adjustment of the process noise covariance and observation noise covariance of the Kalman filter. This achieves the technical effect of maintaining robust positioning at the centimeter to decimeter level even under conditions of multipath interference, sparse base stations, or no external correction for a long time, greatly improving the accuracy of the positioning results.

[0042] In some embodiments, step 102 above, obtaining the spatial weighted value and time weighted value of the detected target, includes: Based on the preliminary positioning result of the detected target obtained through the AOA base station and the spatial coordinates of the AOA base station, the spatial weighted value of the detected target is calculated; or, based on the preliminary positioning result of the detected target obtained through the TDOA base station and the spatial coordinates of the TDOA base station, the spatial weighted value of the detected target is calculated. Here, based on the preliminary positioning results of the detected target obtained through the AOA base station and the spatial coordinates of the AOA base station, the spatial weighted value of the detected target (here referred to as the AOA spatial weighted value) can be calculated using the following formula:

[0043] Among them, A k denoted by , where represents the spatial weighted value of the currently detected target; p represents the current location information of the detected target (i.e., the preliminary location result of the detected target); p i This represents the location information of the i-th base station. At this point, the i-th base station is the i-th AOA base station.

[0044] It should be noted that, based on the preliminary positioning results of the detected target obtained through the TDOA base station and the spatial coordinates of the TDOA base station, the spatial weighted value of the detected target (here referred to as the TDOA spatial weighted value) can also be calculated using the above formula. In this case, the i-th base station is the i-th TDOA base station. The weights are dynamically adjusted considering factors such as the sparsity of the base station's geometric distribution and the signal-to-noise ratio.

[0045] The time-weighted value of the detected target is calculated based on the current time and the initial time of the ultra-wideband sensor, where the initial time of the ultra-wideband sensor is the location signal disconnection time.

[0046] Here, the GPS signal loss time is set as the initial time of the UWB sensor. When the detected target (such as a drone) enters a GPS-denied environment (such as indoors, canyons, or electromagnetic interference areas), the system detects the GPS signal loss time t0 and sets it as the starting reference time of the UWB local time system for subsequent time alignment and status prediction.

[0047] Here, based on the current time and initial time of the UWB sensor, the time-weighted value S of the detected target is calculated using the following formula. k :

[0048] Where t represents the initial time of the UWB sensor; t k This indicates the current time of the UWB sensor.

[0049] It should be noted that while integrating the data measured by the IMU itself over time can yield real-time location information, errors exist between the calculated results and reality. Consequently, the positioning error increases over time, making the weight directly affected by time. Therefore, time and process noise are considered inversely proportional. As time progresses, the pure IMU / UWB integration error accumulates, reducing positioning reliability and consequently lowering the weight.

[0050] In some embodiments, adjusting the observation noise covariance based on the spatial weighting value may include: Based on the spatial weighting of AOA or TDOA, the observation noise covariance is adjusted using the following formula:

[0051] Among them, R k-1 Let A represent the preceding observation noise covariance matrix, initialized as the identity matrix. k This represents the spatial weighting value of the detected target.

[0052] In some embodiments, adjusting the process noise covariance based on the time-weighted value may include: Based on the time-weighted value, the process noise covariance is adjusted using the following formula:

[0053] Among them, Q k-1 S represents the noise covariance matrix of the preceding process, initially set to the identity matrix. k This represents the spatial weighting value of the detected target.

[0054] In some embodiments, step 103 above, which calculates the Kalman gain based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance, includes: The prediction error covariance is obtained based on the state transition matrix and the adjusted process noise covariance. Here, as mentioned above, the state transition matrix is ​​constructed from the acceleration data of the detected target collected by the UWB sensor. Specifically, a discrete-time state-space model is constructed, and the state vector includes position, velocity, acceleration bias, etc. The current acceleration data 'a' of the detected target is used. k Construct the state transition matrix F k :

[0055] Here, a 1 in the state transition matrix indicates that the current state is uniformly accelerated. Indicates speed status. Indicates the position status.

[0056] As an optional implementation, the prediction error covariance is obtained based on the state transition matrix and the adjusted process noise covariance, including: According to the state transition matrix F k and the adjusted process noise covariance Q k The prediction error covariance is calculated using the following formula. :

[0057] Among them, P k-1 This represents the covariance of the preceding error.

[0058] The Kalman gain is calculated based on the adjusted observation noise covariance and the prediction error covariance.

[0059] As an optional implementation, based on the adjusted observation noise covariance R k and the prediction error covariance The Kalman gain K is calculated using the following formula. k :

[0060] in, This is the observation matrix (i.e., the identity matrix used in the calculation).

[0061] In some embodiments, step 104 above, obtaining the final location result of the detected target based on the Kalman gain and the preliminary location result, includes: When the preliminary positioning result is obtained through the AOA base station, the TDOA base station, or a target equipped with an ultra-wideband sensor and an inertial measurement unit, the Kalman gain K is used to determine the location. k And the preliminary positioning result Z k The final location result of the detected target is calculated. ; Alternatively, if the preliminary localization result is obtained through at least two of the AOA base station, the TDOA base station, and the detected target equipped with an ultra-wideband sensor and an inertial measurement unit, the preliminary localization result with the highest weight is selected, and the result is determined according to the Kalman gain K. k And the preliminary localization result Z with the highest weight k The final location result of the detected target is calculated. .

[0062]

[0063] in, Z is the predicted value of the target for this detection. k To achieve adaptive fusion localization of multi-source heterogeneous sensors, the current effective observations (preliminary localization results with the highest weight are selected first) are used.

[0064] In some embodiments, the method of the present invention further includes: According to the state transition matrix F k The predicted value of the detected target is calculated. Here, the predicted value of the detected target is used to characterize the predicted position of the detected target in this operation.

[0065] As an optional implementation, based on the state transition matrix F k The predicted value of the detected target is calculated using the following formula. :

[0066] here, The target location result obtained from the previous cyclic filtering calculation can be compared with the current predicted value. and the final location result of the detected target It can determine the accuracy of the state transition matrix. If the difference after comparison exceeds a preset threshold, the state transition matrix F can be fine-tuned. k .

[0067] In some embodiments, the method of the present invention further includes: Based on the Kalman gain, the prediction error covariance is updated to obtain the actual error covariance, which is used for the prediction of the next error covariance.

[0068] As an optional implementation, based on the Kalman gain K... k The prediction error covariance is updated using the following formula. The actual error covariance P is obtained. k :

[0069] in, This is the observation matrix (i.e., the identity matrix used in the calculation). Here, to complete this filtering loop, the actual error covariance P... k Prepare for predictions of the next moment.

[0070] See Figure 2 The overall design process of this invention will be briefly described below.

[0071] Perception layer: AOA base station cluster (≥3, non-collinear), TDOA base station cluster (≥4, non-coplanar), UAV equipped with UWB+IMU (including position and acceleration).

[0072] Signal processing and preliminary localization: Based on the AOA base station group, the AOA direction angle (such as azimuth and elevation angle) is obtained and calculated to construct the position trajectory ray; then, the AOA intersection point is solved by the LM algorithm, and the preliminary localization result x_AOA is output; Based on the TDOA base station cluster, the TDOA delay difference is extracted, and a hyperboloid equation is constructed. Then, the TDOA intersection point is solved by the Chan-Ho algorithm, and the preliminary positioning result x_TDOA is output.

[0073] Based on a UWB sensor and an inertial measurement unit, it outputs its own position, triaxial acceleration, angular velocity and other status information.

[0074] Weights and adaptive parameter generation: AOA spatial weighting (based on GDOP / covariance); TDOA spatial weighting (based on GDOP / covariance); UWB time-weighted value ( , =Current time - UWB initial time); →Dynamically adjust process noise covariance Q and observation noise covariance R.

[0075] State prediction and fusion filtering: State vector: [position, velocity, acceleration bias]; State transition matrix F ← UWB acceleration; predict: ; ; Observation selection: Select the preliminary localization result Z with the highest weight. k ; Kalman gain: renew: .

[0076] The method of this invention constructs a heterogeneous observation source based on the geometric intersection of AOA rays (solved using the LM nonlinear least squares method) and the intersection of TDOA hyperboloids (solved using the Chan-Ho closed-form algorithm). It innovatively introduces a dual dynamic weighting mechanism of "spatial weighting" and "temporal decay weighting" to drive the real-time adaptive adjustment of the process noise covariance (Q) and observation noise covariance (R) of the Kalman filter. Specifically, the AOA / TDOA spatial weighting quantifies the current observation reliability based on the base station geometry (such as GDOP or positioning residual covariance trace), while the UWB temporal weighting dynamically reflects the degree of IMU / UWB integral drift accumulation starting from the GPS signal interruption time and following an exponential decay function. This technique solves the problem of positioning divergence or sharp drop in accuracy caused by fixed sensor weights and rigid noise models in existing fusion positioning systems under complex electromagnetic environments or GNSS denial scenarios. It achieves robust positioning at the centimeter to decimeter level even under conditions of multipath interference, sparse base stations, or prolonged periods without external correction.

[0077] like Figure 3 As shown, an embodiment of the present invention provides a positioning device, the device comprising: The first acquisition module 301 is used to acquire the preliminary positioning result of the detected target by at least one of the angle of arrival (AOA) base station, time difference of arrival (TDOA) base station, and a detected target equipped with an ultra-wideband sensor and an inertial measurement unit. The second acquisition module 302 is used to acquire the spatial weighted value and the time weighted value of the detected target; adjust the observation noise covariance based on the spatial weighted value; and adjust the process noise covariance based on the time weighted value. The calculation module 303 is used to calculate the Kalman gain based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance. The state transition matrix is ​​constructed from the acceleration data of the detected target collected by the ultra-wideband sensor. The positioning module 304 is used to obtain the final positioning result of the detected target based on the Kalman gain and the preliminary positioning result.

[0078] In some embodiments, the first acquisition module 301 includes: The first calculation unit is used to measure the phase difference of the received wireless signal transmitted by the detected target using each of the AOA base stations in the target monitoring area, and calculate the azimuth and elevation angles of the detected target relative to the base station. N AOA base stations are deployed in the target monitoring area, where N is a positive integer greater than or equal to 3, and the N AOA base stations are not collinear. The first processing unit is used to construct a position trajectory ray based on the spatial coordinates of the AOA base station, the azimuth angle, and the elevation angle; The second calculation unit is used to calculate the preliminary positioning result of the detected target based on N position trajectory rays and using a nonlinear least squares algorithm.

[0079] In some embodiments, the first acquisition module 301 includes: The third calculation unit is used to calculate the time difference between the detected target and the TDOA base station by using each TDOA base station in the target monitoring area and the received wireless signal transmitted by the detected target. M TDOA base stations are deployed in the target monitoring area, where M is a positive integer greater than or equal to 4, and the M TDOA base stations are not coplanar. The second processing unit is used to construct M-1 hyperboloids based on the time difference between the detected target and different TDOA base stations and the spatial coordinates of each TDOA base station; The fourth calculation unit is used to calculate the preliminary positioning result of the detected target based on the M-1 hyperboloids and using a preset analytical positioning algorithm.

[0080] Optionally, the ultra-wideband sensor and inertial measurement unit are used to output motion state data of the detected target, the motion state data including the preliminary positioning result and acceleration data of the detected target.

[0081] In some embodiments, the second acquisition module 302 includes: The fifth calculation unit is used to calculate the spatial weighted value of the detected target based on the preliminary positioning result of the detected target obtained through the AOA base station and the spatial coordinates of the AOA base station; or, to calculate the spatial weighted value of the detected target based on the preliminary positioning result of the detected target obtained through the TDOA base station and the spatial coordinates of the TDOA base station. The sixth calculation unit is used to calculate the time-weighted value of the detected target based on the current time of the ultra-wideband sensor and the initial time of the ultra-wideband sensor, wherein the initial time of the ultra-wideband sensor is the location signal disconnection time.

[0082] In some embodiments, the computing module 303 includes: The seventh calculation unit is used to obtain the prediction error covariance based on the state transition matrix and the adjusted process noise covariance. The eighth calculation unit is used to calculate the Kalman gain based on the adjusted observation noise covariance and the prediction error covariance.

[0083] In some embodiments, the positioning module 304 includes: The third processing unit is configured to, when the preliminary positioning result is obtained through the AOA base station, the TDOA base station, or a target equipped with an ultra-wideband sensor and an inertial measurement unit, calculate the final positioning result of the detected target based on the Kalman gain and the preliminary positioning result; or, The fourth processing unit is configured to select the preliminary positioning result with the highest weight from the preliminary positioning results obtained by at least two of the AOA base station, the TDOA base station, and the detection target equipped with an ultra-wideband sensor and an inertial measurement unit, and calculate the final positioning result of the detection target based on the Kalman gain and the preliminary positioning result with the highest weight.

[0084] In some embodiments, the method of the present invention further includes: The processing module is used to update the prediction error covariance based on the Kalman gain to obtain the actual error covariance, which is used for the prediction of the error covariance in the next iteration.

[0085] The positioning device of this invention obtains a preliminary positioning result of a detected target by at least one of an angle-of-arrival (AOA) base station, a time-difference-of-arrival (TDOA) base station, and a detected target equipped with an ultra-wideband sensor and an inertial measurement unit; obtains the spatial weighted value and time weighted value of the detected target; adjusts the observation noise covariance based on the spatial weighted value; and adjusts the process noise covariance based on the time weighted value; calculates the Kalman gain based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance, wherein the state transition matrix is ​​constructed from the acceleration data of the detected target collected by the ultra-wideband sensor; and obtains the preliminary positioning result based on the Kalman gain and the detected target's position. The final positioning result of the target is obtained by dynamically fusing the preliminary positioning results obtained through the AOA base station, the preliminary positioning results obtained through the TDOA base station, and / or the preliminary positioning results obtained through the target detection equipped with an ultra-wideband sensor and an inertial measurement unit. This allows for the extraction of the advantages of AOA, TDOA, and IMU, while mitigating the measurement problems of AOA, TDOA, and IMU. Furthermore, a dual weighting mechanism of spatial and temporal weighting is introduced to drive the real-time adaptive adjustment of the process noise covariance and observation noise covariance of the Kalman filter. This achieves the technical effect of maintaining robust positioning at the centimeter to decimeter level even under conditions of multipath interference, sparse base stations, or no external correction for a long time, greatly improving the accuracy of the positioning results.

[0086] To better achieve the above objectives, such as Figure 4 As shown, this embodiment of the invention also provides a positioning device, including a processor 400 and a transceiver 410. The transceiver 410 receives and transmits data under the control of the processor 400, and the processor 400 is used to perform the following process: The preliminary positioning result of the detected target is obtained by using at least one of the following: Angle of Arrival (AOA) base station, Time Difference of Arrival (TDOA) base station, and a detected target equipped with an ultra-wideband sensor and an inertial measurement unit. Obtain the spatial weighted value and time weighted value of the detected target; adjust the observation noise covariance based on the spatial weighted value; and adjust the process noise covariance based on the time weighted value; The Kalman gain is calculated based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance. The state transition matrix is ​​constructed from the acceleration data of the detected target collected by the ultra-wideband sensor. Based on the Kalman gain and the preliminary localization result, the final localization result of the detected target is obtained.

[0087] In some embodiments, the processor 400 is further configured to: Using each of the AOA base stations within the target monitoring area, the phase difference of the received wireless signal transmitted by the detected target is measured, and the azimuth and elevation angles of the detected target relative to the base station are calculated. N AOA base stations are deployed in the target monitoring area, where N is a positive integer greater than or equal to 3, and the N AOA base stations are not collinear. Based on the spatial coordinates of the AOA base station, the azimuth angle, and the elevation angle, a position trajectory ray is constructed; Based on N position trajectory rays, a nonlinear least squares algorithm is used to calculate the preliminary positioning result of the detected target.

[0088] In some embodiments, the processor 400 is further configured to: Using each TDOA base station within the target monitoring area, the time difference between the detected target and the TDOA base station is calculated based on the received wireless signal transmitted by the detected target. M TDOA base stations are deployed in the target monitoring area, where M is a positive integer greater than or equal to 4, and the M TDOA base stations are non-coplanar. Based on the time difference between the detected target and different TDOA base stations and the spatial coordinates of each TDOA base station, M-1 hyperboloids are constructed; Based on the M-1 hyperboloids, a preset analytical positioning algorithm is used to calculate the preliminary positioning result of the detected target.

[0089] In some embodiments, the ultra-wideband sensor and inertial measurement unit are used to output motion state data of the detected target, the motion state data including preliminary positioning results and acceleration data of the detected target.

[0090] In some embodiments, the processor 400 is further configured to: Based on the preliminary positioning result of the detected target obtained through the AOA base station and the spatial coordinates of the AOA base station, the spatial weighted value of the detected target is calculated; or, based on the preliminary positioning result of the detected target obtained through the TDOA base station and the spatial coordinates of the TDOA base station, the spatial weighted value of the detected target is calculated. The time-weighted value of the detected target is calculated based on the current time and the initial time of the ultra-wideband sensor, where the initial time of the ultra-wideband sensor is the location signal disconnection time.

[0091] In some embodiments, the processor 400 is further configured to: The prediction error covariance is obtained based on the state transition matrix and the adjusted process noise covariance. The Kalman gain is calculated based on the adjusted observation noise covariance and the prediction error covariance.

[0092] In some embodiments, the processor 400 is further configured to: If the preliminary positioning result is obtained through the AOA base station, the TDOA base station, or a target equipped with an ultra-wideband sensor and an inertial measurement unit, the final positioning result of the detected target is calculated based on the Kalman gain and the preliminary positioning result; or, If the preliminary positioning result is obtained by at least two of the AOA base station, the TDOA base station, and the detection target equipped with an ultra-wideband sensor and an inertial measurement unit, the preliminary positioning result with the highest weight is selected from them, and the final positioning result of the detection target is calculated based on the Kalman gain and the preliminary positioning result with the highest weight.

[0093] In some embodiments, the processor 400 is further configured to: Based on the Kalman gain, the prediction error covariance is updated to obtain the actual error covariance, which is used for the prediction of the next error covariance.

[0094] The positioning device of this invention obtains a preliminary positioning result of a detected target by at least one of an angle-of-arrival (AOA) base station, a time-difference-of-arrival (TDOA) base station, and a detected target equipped with an ultra-wideband sensor and an inertial measurement unit; obtains the spatial weighted value and time weighted value of the detected target; adjusts the observation noise covariance based on the spatial weighted value; and adjusts the process noise covariance based on the time weighted value; calculates the Kalman gain based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance, wherein the state transition matrix is ​​constructed from the acceleration data of the detected target collected by the ultra-wideband sensor; and obtains the preliminary positioning result based on the Kalman gain and the detected target's acceleration data. The final positioning result of the target is obtained by dynamically fusing the preliminary positioning results obtained through the AOA base station, the preliminary positioning results obtained through the TDOA base station, and / or the preliminary positioning results obtained through the target detection equipped with an ultra-wideband sensor and an inertial measurement unit. This allows for the extraction of the advantages of AOA, TDOA, and IMU, while mitigating the measurement problems of AOA, TDOA, and IMU. Furthermore, a dual weighting mechanism of spatial and temporal weighting is introduced to drive the real-time adaptive adjustment of the process noise covariance and observation noise covariance of the Kalman filter. This achieves the technical effect of maintaining robust positioning at the centimeter to decimeter level even under conditions of multipath interference, sparse base stations, or no external correction for a long time, greatly improving the accuracy of the positioning results.

[0095] This invention also provides a positioning device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the various processes described in the positioning method embodiments above and achieves the same technical effect. To avoid repetition, these will not be repeated here.

[0096] This invention also provides a computer-readable storage medium storing a computer program. When executed by a processor, this program implements the various processes described in the positioning method embodiments above, achieving the same technical effects. To avoid repetition, further details are omitted here. The computer-readable storage medium may be a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0097] This invention also provides a computer program product, including computer instructions, which, when executed by a processor, implement the above-described functionality. Figure 1 The steps in the positioning method shown.

[0098] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-readable storage media (including, but not limited to, disk storage and optical storage) containing computer-usable program code.

[0099] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 A device for one or more processes and / or the functions specified in one or more boxes.

[0100] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce a paper article including an instruction means, the instruction means being implemented in a process Figure 1One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0101] These computer program instructions can also be loaded onto a computer or other programmable data processing equipment, causing the computer or other programmable equipment to perform a series of operational steps to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0102] The above description represents the preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A positioning method, characterized in that, include: The preliminary positioning result of the detected target is obtained by using at least one of the following: Angle of Arrival (AOA) base station, Time Difference of Arrival (TDOA) base station, and a detected target equipped with an ultra-wideband sensor and an inertial measurement unit. Obtain the spatial weighted value and time weighted value of the detected target; adjust the observation noise covariance based on the spatial weighted value; and adjust the process noise covariance based on the time weighted value; The Kalman gain is calculated based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance. The state transition matrix is ​​constructed from the acceleration data of the detected target collected by the ultra-wideband sensor. Based on the Kalman gain and the preliminary localization result, the final localization result of the detected target is obtained.

2. The method according to claim 1, characterized in that, By using the angle-of-arrival (AOA) base station, the preliminary positioning results of the detected target are obtained, including: Using each of the AOA base stations within the target monitoring area, the phase difference of the received wireless signal transmitted by the detected target is measured, and the azimuth and elevation angles of the detected target relative to the base station are calculated. N AOA base stations are deployed in the target monitoring area, where N is a positive integer greater than or equal to 3, and the N AOA base stations are not collinear. Based on the spatial coordinates of the AOA base station, the azimuth angle, and the elevation angle, a position trajectory ray is constructed; Based on N position trajectory rays, a nonlinear least squares algorithm is used to calculate the preliminary positioning result of the detected target.

3. The method according to claim 1, characterized in that, By using a Time Difference of Arrival (TDOA) base station, the preliminary location result of the detected target is obtained, including: Using each TDOA base station within the target monitoring area, the time difference between the detected target and the TDOA base station is calculated based on the received wireless signal transmitted by the detected target. M TDOA base stations are deployed in the target monitoring area, where M is a positive integer greater than or equal to 4, and the M TDOA base stations are non-coplanar. Based on the time difference between the detected target and different TDOA base stations and the spatial coordinates of each TDOA base station, M-1 hyperboloids are constructed; Based on the M-1 hyperboloids, a preset analytical positioning algorithm is used to calculate the preliminary positioning result of the detected target.

4. The method according to claim 1, characterized in that, The ultra-wideband sensor and inertial measurement unit are used to output the motion state data of the detected target, which includes the preliminary positioning results and acceleration data of the detected target.

5. The method according to claim 1, characterized in that, The step of obtaining the spatial weighted value and time weighted value of the detected target includes: Based on the preliminary positioning result of the detected target obtained through the AOA base station and the spatial coordinates of the AOA base station, the spatial weighted value of the detected target is calculated; or, based on the preliminary positioning result of the detected target obtained through the TDOA base station and the spatial coordinates of the TDOA base station, the spatial weighted value of the detected target is calculated. The time-weighted value of the detected target is calculated based on the current time and the initial time of the ultra-wideband sensor, where the initial time of the ultra-wideband sensor is the location signal disconnection time.

6. The method according to claim 1, characterized in that, The calculation of the Kalman gain based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance includes: The prediction error covariance is obtained based on the state transition matrix and the adjusted process noise covariance. The Kalman gain is calculated based on the adjusted observation noise covariance and the prediction error covariance.

7. The method according to claim 1, characterized in that, The step of obtaining the final location result of the detected target based on the Kalman gain and the preliminary location result includes: If the preliminary positioning result is obtained through the AOA base station, the TDOA base station, or a target equipped with an ultra-wideband sensor and an inertial measurement unit, the final positioning result of the detected target is calculated based on the Kalman gain and the preliminary positioning result; or, If the preliminary positioning result is obtained by at least two of the AOA base station, the TDOA base station, and the detection target equipped with an ultra-wideband sensor and an inertial measurement unit, the preliminary positioning result with the highest weight is selected from them, and the final positioning result of the detection target is calculated based on the Kalman gain and the preliminary positioning result with the highest weight.

8. The method according to claim 6, characterized in that, The method further includes: Based on the Kalman gain, the prediction error covariance is updated to obtain the actual error covariance, which is used for the prediction of the next error covariance.

9. A positioning device, characterized in that, include: The first acquisition module is used to acquire the preliminary positioning result of the detected target by at least one of the angle of arrival (AOA) base station, time difference of arrival (TDOA) base station, and a detected target equipped with an ultra-wideband sensor and an inertial measurement unit; The second acquisition module is used to acquire the spatial weighted value and the temporal weighted value of the detected target; adjust the observation noise covariance based on the spatial weighted value; and adjust the process noise covariance based on the temporal weighted value. The calculation module is used to calculate the Kalman gain based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance. The state transition matrix is ​​constructed from the acceleration data of the detected target collected by the ultra-wideband sensor. The positioning module is used to obtain the final positioning result of the detected target based on the Kalman gain and the preliminary positioning result.

10. A positioning device, comprising a processor and a transceiver, wherein the transceiver receives and transmits data under the control of the processor, characterized in that, The processor is used to perform the following operations: The preliminary positioning result of the detected target is obtained by using at least one of the following: Angle of Arrival (AOA) base station, Time Difference of Arrival (TDOA) base station, and a detected target equipped with an ultra-wideband sensor and an inertial measurement unit. Obtain the spatial weighted value and time weighted value of the detected target; adjust the observation noise covariance based on the spatial weighted value; and adjust the process noise covariance based on the time weighted value; The Kalman gain is calculated based on the state transition matrix, the adjusted process noise covariance, and the adjusted observation noise covariance. The state transition matrix is ​​constructed from the acceleration data of the detected target collected by the ultra-wideband sensor. Based on the Kalman gain and the preliminary localization result, the final localization result of the detected target is obtained.

11. A positioning device, comprising a memory, a processor, and a computer program stored in the memory and running thereon, characterized in that, When the processor executes the program, it implements the positioning method as described in any one of claims 1 to 8.

12. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps in the positioning method as described in any one of claims 1 to 8.

13. A computer program product, characterized in that, It includes computer instructions that, when executed by a processor, implement the steps in the positioning method as described in any one of claims 1 to 8.

Images