Inertial navigation error compensation method and system for multi-mobile agent in target tracking

Abstract

This invention provides an inertial navigation error compensation method and system for target tracking using multiple mobile agents. The method includes: first, an agent equipped with a camera acquires a target image, and then processes the image data to obtain a target angle measurement; simultaneously, an agent equipped with radar acquires a target point cloud, and then processes the point cloud data to obtain target distance and angle measurements; further, pseudo-distance estimation of the angle measurement is performed using the least squares method, leveraging the cooperation of multiple agents to address the problem of insufficient distance information in camera angle measurements; finally, target tracking is achieved using distributed Kalman filtering, and the target state is consistently fused. This invention can be applied to inertial navigation multi-mobile agent target tracking systems with a variable number of agents and two types of sensors, achieving compensation for multi-agent inertial navigation positioning errors and stable target tracking solely through the tracking of the target by the agent cluster.

Description

Technical Field

[0001] This invention relates to the field of target tracking and inertial navigation, specifically to an inertial navigation error compensation method and system for multiple mobile intelligent agents in target tracking. Background Technology

[0002] In the field of target tracking, multi-agent systems commonly utilize cameras or radar for target tracking. Compared to single-agent target tracking, multi-agent target tracking offers higher tracking accuracy and better stability, overcoming the limitations of single-agent target tracking such as limited observation range and single observation angle. The poor observability of single-agent pure angle measurement sensors like cameras can also be addressed through multi-agent collaboration.

[0003] In real-world scenarios, intelligent agents are often in motion themselves. If the agent's own positioning is inaccurate, it will negatively impact target tracking accuracy. When it's difficult to use external navigational landmarks for positioning, the agent must rely on its own inertial navigation system (INS). However, the inherent cumulative error of INS will cause the positioning error to increase over time. Since sensors measure the target on the agent platform while the agent is moving, target tracking and agent positioning are coupled in the measurement model. Therefore, the positioning error accumulated by INS will lead to a decrease in target tracking accuracy. Without adding additional sensors, relying on radar or cameras equipped with multiple agents to track the target and communication between agent clusters, fusing target tracking information, and estimating and compensating for INS errors is of significant practical importance for improving the positioning and target tracking accuracy of multi-agent systems. Summary of the Invention

[0004] To address the problem of accumulated errors in the self-localization of inertial navigation multi-agent systems, this invention proposes an inertial navigation error compensation method for target tracking scenarios, comprising:

[0005] The measurement data for unified multi-agent systems is in the form of distance and angle measurements.

[0006] Each agent performs Kalman filtering locally;

[0007] By fusing the measurement information and the prediction information from the Kalman filter, target tracking is performed to obtain target state estimation information, while simultaneously estimating the inertial navigation error compensation value;

[0008] Each agent communicates the target state estimation information with other agents within its communication range, and obtains target tracking information through consensus fusion.

[0009] Preferably, the measurement data of the unified multi-agent system is measurement information in the form of distance and angle, including:

[0010] The system acquires raw image data collected by an intelligent agent equipped with a camera, and obtains the angle information of the target relative to the camera by detecting the target in the image.

[0011] The system acquires point clouds collected by an intelligent agent equipped with radar, and obtains the distance and angle information of the target relative to the radar by performing clustering processing on the raw point cloud data.

[0012] Using the target information measured by the camera and the target information measured by the radar at the same time as input, a pseudo-range estimate of the angle measurement is obtained. The measurement information of the intelligent agent equipped with both camera and radar is in the form of range and angle.

[0013] Preferably, the step of acquiring raw image data collected by an intelligent agent equipped with a camera, and obtaining the angle information of the target relative to the camera by detecting the target in the image, includes:

[0014] Detecting the location of targets in an image using image target detection methods;

[0015] Based on the relative positional relationship between the stated position and the image center point;

[0016] Obtain the angle of the target relative to the camera.

[0017] Preferably, the clustering method includes DBSCAN and K-means clustering algorithms to obtain the distance and angle information of the measured target relative to the radar.

[0018] Preferably, the pseudo-range estimate of the angle measurement is obtained by taking the target information measured by the camera and the target information measured by the radar at the same time as input. The measurement information of the intelligent agents equipped with cameras and radar are all in the form of distance and angle. The target measurement information that needs to be communicated with other intelligent agents includes at least one of the two types of measurement, camera and radar. The position estimate of the target is calculated using the least squares method.

[0019] Preferably, the pseudo-distance estimation includes:

[0020] For an agent i equipped with a camera, based on its target position and the camera's angle measurement, a system of linear equations can be obtained after transformation:

[0021]

[0022] in These represent the azimuth and elevation angles of the target relative to itself, measured by the camera relative to agent i, respectively. i ,y i ,z i (x, y, z) represents the position of agent i, and (x, y, z) represents the target position.

[0023] The system of linear equations formed by all camera measurements is denoted as:

[0024] A C X = b C

[0025] For an agent j equipped with radar, a system of linear equations is obtained based on its location and radar measurements.

[0026]

[0027] in These represent the distance, azimuth, and elevation angles of the target relative to itself as measured by the radar, respectively, for agent j. j ,y j ,z j (x, y, z) represents the position of agent j, and (x, y, z) represents the target position.

[0028] The system of equations for all radar measurements is denoted as:

[0029] A L x = b L

[0030] Combining the equations from the camera and radar measurements, we get:

[0031]

[0032] The target position estimate is obtained using the least squares method:

[0033]

[0034] Calculate the pseudo-range measurement estimate of the target from the camera-equipped agent i.

[0035]

[0036] Where ||·|| represents the Euclidean norm, X i The position of agent i.

[0037] Preferably, the Kalman filtering includes linear and nonlinear Kalman filtering methods.

[0038] Each agent communicates the target state estimation information with agents within its communication range, fusing measurement information and Kalman filter prediction information; target tracking is performed to obtain the target state estimation information, while simultaneously estimating the inertial navigation error compensation value. Using the target state and the inertial navigation error compensation value as state variables, the steps include:

[0039] a. Sampling process: For the estimated state variables at time k-1 Covariance P k-1|k-1The Sigma point set is obtained through UT transformation. Where L is the number of sampling points. The weights of the corresponding sampling points;

[0040] b. Prediction process: Given the state equation f(·), the measurement equation h(·), and the process noise variance matrix Q t ;

[0041]

[0042]

[0043]

[0044]

[0045]

[0046] The predicted values ​​of the state variables at time k were obtained through the prediction process. State variable covariance P k|k-1 and prediction of measured values

[0047] c. Measurement Update: Given the measurement noise variance matrix R t :

[0048]

[0049]

[0050]

[0051]

[0052]

[0053] The estimated value of the state variable at time k was obtained by measurement update. and state variable covariance P k|k .

[0054] Preferably, each agent communicates the target state estimation information with agents within the communication range, and obtains target tracking information through consensus fusion, including:

[0055] Each agent's state variable x includes the target state x. A and inertial navigation error compensation value x B Two parts; this step only applies to the target state x. A A consensus fusion is performed. Each agent estimates the posterior state of the target at time k. Covariance Convert to information vector and information matrix form:

[0056]

[0057]

[0058] Agent i sends to agents in the communication neighborhood And perform consistent weighted fusion with the information vectors and information matrices transmitted from the communication neighborhood agents:

[0059]

[0060]

[0061] τ←τ+1

[0062] Where w is the consistency weight, N i Let τ be the set of indices of the neighboring agents of agent i, and let τ be the number of consensus iterations.

[0063] We obtain the local state estimate (target tracking information) and covariance:

[0064]

[0065]

[0066] According to a second aspect of the present invention, an inertial navigation error compensation system is provided for an inertial navigation multi-mobile intelligent agent system in target tracking applications, comprising:

[0067] The image data processing module acquires raw image data collected by the intelligent agent equipped with a camera, and obtains the angle information of the target relative to the camera by detecting the target in the image.

[0068] The point cloud data processing module acquires point clouds collected by an intelligent agent equipped with radar, and obtains the distance and angle information of the target relative to the radar by performing clustering processing on the raw point cloud data.

[0069] The pseudo-range estimation module for angle measurement takes the target information measured by the camera and the target information measured by the radar at the same time as input to obtain the pseudo-range estimate of the angle measurement. The measurement information of the intelligent agent equipped with both camera and radar is in the form of distance and angle.

[0070] The target tracking module based on distributed Kalman filtering takes the target's distance and angle as input information. Each agent performs Kalman filtering locally. The prediction information of Kalman filtering and the input information are fused to perform target tracking to obtain target state estimation information, and at the same time estimate the inertial navigation error compensation value.

[0071] The target state consistency fusion module allows each agent to communicate the target state estimation information only with agents within its communication range, and obtains target tracking information through consistency fusion.

[0072] The advantages of this invention compared to the prior art are as follows:

[0073] This invention relates to an inertial navigation error compensation method and system applicable to target tracking of multiple mobile intelligent agents with varying numbers and two types of sensors. It can fuse target tracking information from radar and cameras, suppressing the accumulation of inertial navigation errors to a certain extent, and realizing inertial navigation error compensation in target tracking applications of multi-agent systems, thereby improving target tracking accuracy. It achieves multi-agent inertial navigation positioning error compensation and stable target tracking solely by relying on the target tracking of the intelligent agent cluster. Attached Figure Description

[0074] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings:

[0075] Figure 1 This invention relates to an inertial navigation error compensation method and system for multiple mobile intelligent agents in target tracking, which is applied to a target tracking scenario.

[0076] Figure 2 This is a flowchart of an inertial navigation error compensation method for multiple mobile intelligent agents in target tracking according to an embodiment of the present invention;

[0077] Figure 3 This is a comparison of the root mean square error of multi-agent localization in one embodiment of the present invention;

[0078] Figure 4 This is a comparison of the root mean square error of multi-agent target tracking according to an embodiment of the present invention.

[0079] In this array, 1 represents radar, 2 represents an intelligent agent, 3 represents an inertial measurement unit, 4 represents a camera, and 5 represents a target. Detailed Implementation

[0080] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention. These all fall within the scope of protection of the present invention.

[0081] Reference Figure 1The diagram illustrates an embodiment of the present invention of a multi-mobile intelligent agent inertial navigation error compensation method and system applied to a target tracking scenario. The diagram includes a radar 1, an intelligent agent 2, an inertial measurement unit 3, a camera 4, and a target 5. For the intelligent agent equipped with a camera: camera 4 is fixed to intelligent agent 2, and intelligent agent 2 has a built-in inertial measurement unit 3; the number of intelligent agents equipped with cameras is 2. For the intelligent agent equipped with radar: radar 1 is fixed to intelligent agent 2, and intelligent agent 2 has a built-in inertial measurement unit 3. The inertial measurement unit can measure the speed of the intelligent agent and calculate its current position based on the speed and time; the number of intelligent agents equipped with radar is 2. All intelligent agents track target 5 together and move towards target 5 simultaneously. There are no reference anchor points in the environment, and the movement process relies solely on the inertial measurement unit for positioning. In this embodiment, the inertial navigation error compensation method of the multi-intelligent agent system can suppress the accumulation of inertial navigation errors to a certain extent, achieving inertial navigation error compensation for the self-positioning of the multi-intelligent agent system, thereby improving target tracking accuracy.

[0082] In a preferred embodiment, a flowchart illustrating the application of the inertial navigation error compensation method and system for multi-mobile intelligent agents in target tracking to a target tracking scenario is shown below. Figure 2 As shown. The specific process is as follows:

[0083] All agents in the S100 use their respective sensors to track targets. Agents equipped with cameras acquire images of the targets, and through image preprocessing and target detection methods, detect the target's position in the image, thereby obtaining the target's angle relative to the camera; agents equipped with radar acquire point clouds of the targets, and through clustering processing of the point clouds, obtain the target distance and angle information measured by the radar.

[0084] The S200's camera-equipped agent utilizes pseudo-distance estimation to unify measurements into distance and angle forms. Specifically, this includes:

[0085] S201 communicates with other intelligent agents the target's measurement information, including at least one of the two types of measurements: camera and radar.

[0086] For an agent i equipped with a camera, based on its position and the angle measured by the camera, the following system of linear equations can be obtained after transformation:

[0087]

[0088] in These represent the azimuth and elevation angles of the target relative to itself, measured by the camera relative to agent i, respectively. i ,y i ,z i Let (x, y, z) represent the position of agent i, and (x, y, z) represent the target position. The system of equations representing all camera measurements is denoted as:

[0089] A C x = b C

[0090] For an agent j equipped with radar, based on its location and radar measurements, the following system of linear equations can be obtained.

[0091]

[0092] in These represent the distance, azimuth, and elevation angles of the target relative to itself as measured by the radar, respectively, for agent j. j ,y j ,z j Let (x, y, z) represent the position of agent j, and (x, y, z) represent the target position. The system of equations representing all radar measurements is denoted as:

[0093] A L x = b L

[0094] Combining the equations from the camera and radar measurements, we get:

[0095]

[0096] S202 uses the least squares method to calculate the target's estimated position:

[0097]

[0098] S203 calculates a pseudo-range measurement estimate of the target from the camera-equipped agent i.

[0099]

[0100] Where ||·|| represents the Euclidean norm, X i The position of agent i.

[0101] The method described in this embodiment solves the problem of poor target tracking visibility caused by the lack of distance information in camera measurements; by utilizing multi-agent cooperation, the measurement information is unified into distance and angle forms, which helps to further target tracking and information fusion.

[0102] Each agent in the S300 system tracks the target locally using an unscented Kalman filter, fusing predictive model data with camera or radar measurements. The target state (position and velocity in the x, y, and z directions) and inertial navigation error compensation values ​​are used as state variables to estimate the target state and inertial navigation error compensation values. Specifically, this includes:

[0103] Unscented Kalman filtering is a nonlinear Kalman filtering method. Its steps include sampling, prediction, and updating. At time k, the steps of unscented Kalman filtering are as follows:

[0104] a. Sampling process, for the estimated state variables at time k-1 Covariance P k-1|k-1 The Sigma point set is obtained through UT transformation. Where L is the number of sampling points. The weights of the corresponding sampling points;

[0105] b. Prediction process, given the state equation f(·), measurement equation h(·), and process noise variance matrix Q. t The formula is as follows:

[0106]

[0107]

[0108]

[0109]

[0110]

[0111] The predicted values ​​of the state variables at time k were obtained through the prediction process. State variable covariance P k|k-1 and prediction of measured values

[0112] c. Measurement update, given the measurement noise variance matrix R t The formula is as follows:

[0113]

[0114]

[0115]

[0116]

[0117]

[0118] Using the unscented Kalman filter method, the estimated values ​​of the state variables at time k were obtained. Covariance P k|k .

[0119] In this embodiment, the target state and the inertial navigation error compensation value are used together as the state variables of the Kalman filter, and the two are correlated in the system. Introducing and estimating the inertial navigation error compensation value into the state variables means that the estimation of the target state is also more accurate.

[0120] The S400 agent communicates target state information with other agents within its communication range, and performs distributed consensus fusion of this target state information to achieve cooperative target tracking within the cluster. Specifically, this includes:

[0121] S401 Each agent estimates the posterior state of the target at time k. Covariance Converting this into information vectors and information matrices, taking agent i as an example:

[0122]

[0123]

[0124] S402 agent i sends to agents in the communication neighborhood And perform consistent weighted fusion with the information vectors and information matrices transmitted from the communication neighborhood agents:

[0125]

[0126]

[0127] τ←τ+1

[0128] Where w is the consistency weight, N i Let τ be the set of indices of the neighboring agents of agent i, and let τ be the number of consensus iterations.

[0129] S403 yields the local state estimate and covariance:

[0130]

[0131]

[0132] The present invention provides a preferred embodiment to verify the effectiveness of the embodiments provided by the present invention and to evaluate the effect.

[0133] The evaluation metric for agent localization is the root mean square error of localization for all agents, i.e.:

[0134]

[0135] in This represents the true position of agent n in the m-th Monte Carlo simulation. The true position is generated by the simulation program. express The estimated value is given by M, which represents the total number of Monte Carlo simulations, and N, which represents the total number of agents.

[0136] The evaluation metric for target tracking is the root mean square error, i.e.:

[0137]

[0138] in This represents the true value of the target position in the m-th Monte Carlo simulation. The true value is generated by the simulation program. express The estimated value, M represents the total number of Monte Carlo simulations.

[0139] In an application embodiment of the present invention, 100 Monte Carlo simulation experiments were conducted, and the root mean square error of the mobile intelligent agent cluster localization was compared with that of the other two experiments. Figure 3 As shown, the root mean square error of target tracking in a mobile intelligent agent cluster is compared to... Figure 4 As shown in the figure. The results indicate that, in the context of this embodiment, the present invention can effectively reduce the root mean square error of localization and the root mean square error of target tracking in mobile intelligent agent clusters.

[0140] The inertial navigation error compensation method for multi-agent systems in target tracking applications according to the above embodiments of the present invention can improve the positioning accuracy of inertial navigation agent clusters, thereby improving target tracking accuracy, and has application prospects in many practical scenarios.

[0141] The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various modifications or variations within the scope of the claims, which do not affect the essence of the present invention.

Claims

1. A method for inertial navigation error compensation of multiple mobile agents in target tracking, characterized in that, The unified multi-agent measurement data is measurement information in the form of distance and angle; Each agent locally performs Kalman filtering; Fusing the measurement information and the prediction information of Kalman filtering, target tracking is performed to obtain target state estimation information, and an inertial navigation error compensation value is estimated; Each agent communicates the target state estimation information with agents within a communication range, and obtains target tracking information through consistent fusion, including: τ←τ+1 The state variable x of each agent includes a target state x A and an inertial navigation error compensation value x B Two parts, the target state x A is fused in consistency; Each agent will target the posterior state estimate of the target at time k and covariance Transformed into the form of information vector and information matrix: The intelligent agent i sends to the intelligent agents in the communication neighborhood and consistent weighted fusion with the information vector and information matrix from the communication neighborhood intelligent agents: Local state estimation, i.e., target tracking information, and covariance are obtained: where w is a consistency weight, N i is a set of indices of the neighborhood of agent i, and τ is the number of consistency iterations. The unified multi-agent measurement data is measurement information in the form of distance and angle, including:

2. The method of claim 1, wherein, Raw images collected by a camera-equipped agent are obtained, and angle information of a target relative to the camera is obtained by detecting the target in the images; Point clouds collected by a radar-equipped agent are obtained, and distance and angle information of the target relative to the radar is obtained by clustering processing of the original point cloud data; The camera-measured target information and the radar-measured target information at the same time are taken as inputs to obtain pseudo-distance estimation of angle measurement, and the measurement information of the camera-equipped agent and the radar-equipped agent is unified in the form of distance and angle. The raw images collected by the camera-equipped agent are obtained, and the angle information of the target relative to the camera is obtained by detecting the target in the images, including:

3. The method according to claim 2, wherein, The position of the target in the original image is detected by using an image target detection method; According to the relative position relationship between the position and the center point of the original image; The angle of the target relative to the camera is obtained. The clustering processing includes DBSCAN and K-means clustering algorithms to obtain the distance and angle information of the measured target relative to the radar.

4. The method of claim 2, wherein, The camera-measured target information and the radar-measured target information at the same time are taken as inputs to obtain pseudo-distance estimation of angle measurement, and the measurement information of the camera-equipped agent and the radar-equipped agent is unified in the form of distance and angle, which needs to communicate the measurement information of the target with other agents, including at least one of the two types of camera and radar measurements, and the least square method is used to calculate the position estimation value of the target.

5. The method of claim 2, wherein, The pseudo-distance estimation includes:

6. The method of claim 5, wherein, For the camera-equipped agent i, according to the position of the target and the angle measurement of the camera, a linear equation set is obtained through transformation: All camera measurements are combined into a linear equation set: wherein respectively represent the azimuth and elevation angles of the target relative to the agent i camera measurement, (x i ,y i ,z i ) represent the position of the agent i, and (x, y, z) represent the target position; For the radar-equipped agent j, according to the position and radar measurement, a linear equation set is obtained A C X = b C All radar measurements are combined into a linear equation set: wherein respectively denote the range, azimuth and elevation of the target relative to the ego of agent j measured by the radar of agent j, (x j ,y j ,z j ) denotes the position of agent j, (x,y,z) denotes the position of the target; The camera and radar measurement equation sets are combined to obtain: A L X = b L The least square method is used to solve the target position estimation value: The pseudo-distance measurement estimation of the target to the camera-equipped agent i is calculated The Kalman filtering includes linear and nonlinear Kalman filtering methods. where || · || denotes the Euclidean norm, X i Location of agent i.

7. The method of claim 1, wherein, The measurement information and the prediction information of Kalman filtering are fused, target tracking is performed to obtain target state estimation information, and an inertial navigation error compensation value is estimated; 8. The method of claim 7, wherein, Wherein, the nonlinear Kalman filter, i.e., the unscented Kalman filter, is selected as the Kalman filter, and the target state and the inertial navigation error compensation value are taken as state variables, including: including: Sampling process: for the state variable estimation value at time k-1 and covariance P k-1|k-1 Sigma point set is obtained by UT transformation i = 1, 2, …, L, where L is the number of sampling points, is the weight of the corresponding sampling point; Prediction process: Given the state equation f(·), the measurement equation h(·) and the process noise variance matrix Q t ; a predicted value of the state variable at time k is obtained by the prediction process state variable covariance P k|k-1 and a prediction of the measurement Measurement update: Given the measurement noise variance matrix R t ; updating the estimate of the state variable at time k by the measurement and the state variable covariance P k|k . 9.A system for inertial navigation error compensation of multiple mobile agents in target tracking, characterized in that, ​ An image data processing module acquires original image data collected by an agent equipped with a camera, detects a target in the image, and obtains angle information of the target relative to the camera; A point cloud data processing module acquires point cloud collected by an agent equipped with a radar, performs clustering processing on the original point cloud data, and obtains distance and angle information of the target relative to the radar; An angle measurement pseudo-range estimation module takes, as input, target information measured by the camera and target information measured by the radar at the same time, and obtains angle measurement pseudo-range estimation, which unifies the measurement information of the agent equipped with the camera and the radar into distance and angle forms; A target tracking module based on distributed Kalman filtering takes distance and angle of the target as input information, and each agent locally performs Kalman filtering; the target tracking module fuses prediction information of the Kalman filtering and the input information, performs target tracking to obtain target state estimation information, and estimates an inertial navigation error compensation value at the same time; The target state consistency fusion module, each agent only communicates the target state estimation information with agents within a communication range, and obtains target tracking information through consistency fusion, including: The state variable x of each agent includes a target state x A and an inertial navigation error compensation value x B Two parts, the target state x A are fused in consistency; Each agent will target the posterior state estimate of the target at time k and covariance Transformed into the form of information vector and information matrix: The intelligent agent i sends to the intelligent agents in the communication neighborhood and consistent weighted fusion with the information vector and information matrix from the communication neighborhood intelligent agents: τ←τ+1 where w is a consistency weight, N i is a set of indices of the neighborhood of agent i, and τ is the number of consistency iterations. obtains local state estimation, i.e., target tracking information, and covariance:

Images