CN-120428238-B - Weighted filtering state estimation method based on geometric relationship

CN120428238BCN 120428238 BCN120428238 BCN 120428238BCN-120428238-B

Abstract

The invention relates to a weighted filtering state estimation method based on a geometric relation, which comprises the steps of obtaining state estimation information of each unmanned aerial vehicle on a ground target, carrying out Kalman filtering on the state estimation information to obtain posterior estimation of each unmanned aerial vehicle, carrying out asynchronous timestamp approximate alignment on the posterior estimation of each unmanned aerial vehicle, carrying out geometric weighted consistency gain calculation based on the aligned posterior estimation to obtain a weighted state estimation value and consistency gain, and carrying out fusion calculation on the weighted state estimation value and consistency gain to obtain a final target state estimation value. According to the invention, each unmanned aerial vehicle forms consistency according to the estimation of the target by itself and the neighbor node. And (3) calculating observation noise according to the distance from the laser sensor to the target, and when a certain node observes the target and is blocked, adjusting the weight of the node to ensure that the sensor network keeps higher precision on target estimation.

Inventors

Niu Diefeng
ZHANG HONG
Miao Kunzhong
MA ZHAOWEI
WANG CHANG
WU LIZHEN

Assignees

中国人民解放军国防科技大学

Dates

Publication Date: 20260508
Application Date: 20250523

Claims (7)

1.A weighted filter state estimation method based on geometric relationships, comprising: acquiring state estimation information of each unmanned aerial vehicle on a ground target; Carrying out Kalman filtering on the state estimation information to obtain posterior estimation of each unmanned aerial vehicle; Performing asynchronous timestamp approximate alignment on posterior estimation of each unmanned aerial vehicle; Based on the aligned posterior estimation, geometric weighted consistency gain calculation is carried out, and a weighted state estimation value and a consistency gain are obtained; the consistency weights are constructed by applying the following nonlinear feature functions, which, if the object is occluded, Otherwise, 1, operate on these two distances: ; and carrying out fusion calculation on the weighted state estimation value and the consistency gain to obtain a final target state estimation value.
2. The geometric relationship-based weighted filter state estimation method according to claim 1, wherein obtaining state estimation information of each unmanned aerial vehicle to a ground target comprises: Each unmanned aerial vehicle is regarded as an observation node; Collecting sensor data of a target based on an unmanned aerial vehicle, wherein the sensor data comprise a target distance measured by a laser radar, a yaw pitch angle when an optical axis of a photoelectric pod is aligned to the target and an unmanned aerial vehicle posture; Acquiring the position of a target under a camera body coordinate system based on the sensor data; Calculating a difference based on states of adjacent sampling moments, and acquiring the speed of a target; And acquiring state estimation information of the unmanned aerial vehicle on the ground target based on the speed of the target and the position of the target under a camera body coordinate system.
3. The geometric relationship-based weighted filter state estimation method of claim 2, wherein acquiring a position of a target in a camera body coordinate system based on the sensor data comprises: Acquiring coordinates of the target under a camera coordinate system based on the target distance; and based on the coordinates of the target in the camera coordinate system, combining the yaw pitch angle and the unmanned aerial vehicle posture when the optical axis of the photoelectric pod is aligned with the target, and acquiring the position of the target in the camera body coordinate system.
4. The geometric relationship-based weighted filter state estimation method of claim 1, wherein performing a kalman filter on the state estimation information to obtain a posterior estimate for each unmanned aerial vehicle comprises: constructing a target motion state equation based on the state estimation information, and obtaining prior estimation of a target; constructing an observation equation of the unmanned aerial vehicle; Based on the observation equation of the unmanned aerial vehicle and the prior estimation of the target, acquiring the unmanned aerial vehicle in the unmanned aerial vehicle by using a Kalman filtering algorithm for each unmanned aerial vehicle Posterior estimation of time of day.
5. The geometric relationship-based weighted filter state estimation method of claim 1, wherein performing asynchronous timestamp approximate alignment on each of the unmanned aerial vehicle's posterior estimates comprises: And performing approximate synchronization on the posterior estimation of each unmanned aerial vehicle on a system standard time stamp to obtain the posterior estimation of each unmanned aerial vehicle after alignment.
6. The geometrical-relationship-based weighted filter state estimation method of claim 1, wherein performing geometrical weighted consistency gain calculation based on the aligned posterior estimation, obtaining a weighted state estimation value and a consistency gain comprises: The unmanned aerial vehicle carries out consistency Kalman estimation according to the aligned posterior estimation broadcasted by the neighbor nodes, and a consistency factor and a consistency weight are obtained; Calculating the consistency gain based on the consistency factor and consistency weight; And based on the consistency gain, weighting the aligned posterior estimation to obtain a weighted state estimation value.
7. The geometrical-relationship-based weighted filter state estimation method of claim 1, wherein performing a fusion calculation of the weighted state estimation value and the consistency gain comprises: multiplying the weighted state estimation value of each unmanned aerial vehicle node by a corresponding consistency gain; And adding the weighted state estimation values of all the nodes multiplied by the consistency gain to obtain a final target state estimation value.

Description

Weighted filtering state estimation method based on geometric relationship Technical Field The invention relates to the technical field of multi-sensor target state estimation, in particular to a weighted filtering state estimation method based on a geometric relationship. Background In the field of distributed state estimation, a Kalman consistency filter based on a multi-sensor network has remarkably progressed, and an incomplete graph communication architecture is constructed by fusing a local observation and a neighborhood consensus mechanism, so that each node only exchanges information with an adjacent sensor, and the fault tolerance of the system under the condition of abnormal links is enhanced. Even if local communication is interrupted, the network can maintain the basic state estimation function through a redundant path, so that global paralysis caused by single-point faults in a centralized architecture is avoided, and the method has important application value in dynamic scenes such as multi-agent target state estimation and the like. However, most existing distributed estimation methods assume constant measurement noise covariance. For a time-of-flight sensor, its measurement accuracy will decrease as the distance from the target increases, and the noise covariance is positively correlated with the target-to-sensor distance. When a target is occluded, the effective observation distance of the occluded node is greatly increased (or signal quality is deteriorated), and if fixed weights are still adopted, the abnormal observations can significantly affect the global estimation accuracy. In addition, the link abnormality causes communication topology dimension reduction, weakens consistency accuracy among nodes, and finally reduces consistency convergence of overall estimation. This indicates that the current consistency kalman filtering algorithm has inherent contradiction between fault tolerance mechanism and estimation precision under complex network environment, which limits the wide application. Disclosure of Invention The invention aims to provide a weighted filtering state estimation method based on geometric relation, so as to solve the problems existing in the prior art, such as the decline of measurement accuracy of a flight time sensor along with the increase of the distance between the flight time sensor and a target, and the method introduces a geometric weighted observation noise method, so that the whole system still has high estimation accuracy and robustness when the observation effect of a single observation node is poor or the target is blocked; the optimal Kalman consistency filtering leads to increased calculation complexity due to the introduction of a consistency gain, and the invention provides a suboptimal distance weighting method which reduces the calculated amount and ensures the estimation precision. In order to achieve the above object, the present invention provides the following solutions: A weighted filtering state estimation method based on geometric relation comprises the following steps: acquiring state estimation information of each unmanned aerial vehicle on a ground target; Carrying out Kalman filtering on the state estimation information to obtain posterior estimation of each unmanned aerial vehicle; Performing asynchronous timestamp approximate alignment on posterior estimation of each unmanned aerial vehicle; Based on the aligned posterior estimation, geometric weighted consistency gain calculation is carried out, and a weighted state estimation value and a consistency gain are obtained; and carrying out fusion calculation on the weighted state estimation value and the consistency gain to obtain a final target state estimation value. Optionally, obtaining the state estimation information of each unmanned aerial vehicle to the ground target includes: Each unmanned aerial vehicle is regarded as an observation node; Collecting sensor data of a target based on an unmanned aerial vehicle, wherein the sensor data comprise a target distance measured by a laser radar, a yaw pitch angle when an optical axis of a photoelectric pod is aligned to the target and an unmanned aerial vehicle posture; Acquiring the position of a target under a camera body coordinate system based on the sensor data; Calculating a difference based on states of adjacent sampling moments, and acquiring the speed of a target; And acquiring state estimation information of the unmanned aerial vehicle on the ground target based on the speed of the target and the position of the target under a camera body coordinate system. Optionally, based on the sensor data, acquiring the position of the target in the camera body coordinate system includes: Acquiring coordinates of the target under a camera coordinate system based on the target distance; and based on the coordinates of the target in the camera coordinate system, combining the yaw pitch angle and the unmanned aerial vehicle posture when the optic