Method for Designing and Implementing a Cooperative Observation Simulation Platform Based on Swarm Movement

Abstract

The present invention discloses a method for designing and implementing a collaborative observation simulation platform based on flocking movement, comprising the following steps: S1, determining the observation matrix of each sensor and measuring the state of the target system; S2, using a consensus Kalman filter to correct the measurement values of each sensor to obtain the optimal estimated value of the target state; S3, designing a flocking movement algorithm for elliptical multi-agent to achieve the tracking of the target. The present invention combines a consensus Kalman filter with an elliptical multi-agent flocking movement algorithm to construct a collaborative observation platform, realizing the optimal observation and tracking of the target system.

Description

Technical Field

[0001] The present invention belongs to the field of collaborative observation, and particularly relates to a method for designing and implementing a collaborative observation simulation platform based on flocking movement. Background Art

[0002] The collaborative observation of the target state in the environment and the tracking thereof appear in various surveillance and security applications as well as intelligent transportation. Most of the previous research on target tracking has focused on static multi-sensor platforms and used centralized processing algorithms. In recent years, there has been great development in distributed estimation on mobile sensor networks.

[0003] Considering that there is always uncertainty in the real world, for the dynamic equation describing the system state, there is process noise and it cannot accurately describe the system state; in addition, there is also measurement noise when measuring the state of the target system through sensors. In order to obtain the optimal estimated value of the target system state, the consensus Kalman filtering algorithm is adopted to enable each node in the multi-agent network to obtain the optimal estimated value of the target state and the states are consistent. In addition, in order to better achieve collaborative observation, the flocking movement control algorithm is used for tracking among multi-agents, which can make the effect of collaborative observation better. The present invention can be applied to the interaction and cooperation between unmanned boats and unmanned aerial vehicles, realize the observation of the state of the unmanned aerial vehicle by the unmanned boat, and at the same time use the flocking movement algorithm to achieve tracking.

[0004] The present invention provides a method for designing and implementing a collaborative observation simulation platform based on flocking movement, which can not only better observe the state of the target system, but also realize the tracking of the target. Summary of the Invention

[0005] Object of the Invention: The present invention provides a method for designing and implementing a collaborative observation simulation platform based on flocking movement, which can reduce the noise error existing in the process of observing the target state and improve the observation accuracy.

[0006] Summary of the Invention: The present invention provides a method for designing and implementing a collaborative observation simulation platform based on flocking movement, including the following steps:

[0007] S1. Determine the observation matrix of each sensor and measure the state of the target system;

[0008] S2. Use the consensus Kalman filter to correct the measurement value of each sensor to obtain the optimal estimated value of the target state;

[0009] S3. Design the flocking movement algorithm of elliptical multi-agents to realize the tracking of the target.

[0010] Specifically, the step S1 includes the following steps:

[0011] S11. Consider a continuous-time linear system representing a target, whose dynamic equation is as follows:

[0012] \(\dot{x}=A(t)x + B(t)w\)

[0013] where \(A\in\mathbb{R}\) n×n , \(x\in\mathbb{R}\) n×1 is the state of the system, \(B\in\mathbb{R}\) n×n , \(w\in\mathbb{R}\) n×1 is the process noise of the system;

[0014] S12. The sensing model of the sensor is:

[0015] \(z = H(t)x + v\)

[0016] where \(H(t)\in\mathbb{R}\) p×n is the measurement matrix of the sensor, \(v\in\mathbb{R}\) p×1 is the measurement noise of the sensor.

[0017] Specifically, the step S2 includes the following steps:

[0018] S21. For each sensor node, use the local Kalman filter algorithm to calculate the covariance matrix information and measurement data information. Assume that node \(i\) has not received any information from its non-neighbor nodes. The iterative form of the local Kalman filter of node \(i\) is as follows:

[0019]

[0020] M i (k)=(P i (k) -1 +S i (k)) -1 ,

[0021]

[0022] where \(S i (k)\) represents the covariance matrix information of all neighbor nodes of node \(i\) at time \(k\), \(y i (k)\) represents the sensing data information of all neighbor nodes of node \(i\) at time \(k\). Node \(i\) locally calculates \(y i (k)\) and \(S i (k)\);

[0023] S22. The form of the continuous-time Kalman filter is as follows:

[0024]

[0025] K = PH T R -1

[0026]

[0027] S23. Consider a sensor network with a sensing model in continuous time. Assume that each node uses the following distributed estimation algorithm:

[0028]

[0029] where the initial condition \(P\) i (0) = \(P_0\),

[0030] S24. Iterative algorithm of the consensus Kalman filter:

[0031] ①: Initialization:

[0032] ②: When new data appears, calculate the following formula

[0033] ③: Locally aggregate the sensed data and the covariance matrix:

[0034] \(J = N_Y\{i\}\)

[0035]

[0036] ④: Calculate the consensus Kalman filter estimate:

[0037]

[0038] ⑤: Update the state of the consensus Kalman filter:

[0039] \(P\) i ← \(A_M\) i \(A\) T + \(B_Q B\) T

[0040]

[0041] ⑥: End the loop;

[0042] In the above algorithm, the information broadcast by node \(i\) to all its neighbor nodes is as follows:

[0043]

[0044] Specifically, step S3 includes the following steps:

[0045] S31. First, determine the separation condition between two ellipses: Consider two ellipses \(i, j\), where the semi-axes of ellipse \(i\) are \((a\) i , \(b\) i ), and the center point coordinates are \((x\) i , \(y\)i ), with a deflection angle of φ i ; The semi-major axis of the ellipse j is (a j , b j ), and the center point coordinates are (x j , y j ), with a deflection angle of φ j ; Define the generalized distance between the ellipses i and j as D ij ;

[0046]

[0047] Where:

[0048]

[0049] S32. Construct the potential function between the elliptical agents and use it in the Lyapunov function for the flocking control algorithm;

[0050] S33. Utilize the potential function constructed in S32 and the position error p i -p od and the angle error to construct the Lyapunov function and give the control algorithm for the flocking movement of the elliptical agents;

[0051] Lyapunov function:

[0052] Compared with the prior art, the beneficial effects of the present invention are as follows: The present invention combines the consensus Kalman filter with the elliptical multi-agent flocking movement algorithm to construct a collaborative observation platform, achieving optimal observation and tracking of the target system. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following will briefly introduce the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0054] Figure 1 is the flowchart of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0055] In order to make the objectives, technical solutions, and advantages of the present invention clearer, the following further details the present invention with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0056] To illustrate the technical solution of the present invention, the following will be described through specific embodiments.

[0057] Embodiment

[0058] As Figure 1 shown, this embodiment proposes a method for the design and implementation of a collaborative observation simulation platform based on flocking movement, including the following steps:

[0059] Step S1, determine the observation matrix of each sensor and measure the state of the target system, specifically including the following steps:

[0060] S11. Consider a continuous-time linear system representing the target, whose dynamic equation is as follows:

[0061]

[0062] where, A ∈ R n×n , x ∈ R n×1 is the state of the system, B ∈ R n×n , w ∈ R n×1 is the process noise of the system, and the process noise satisfies Gaussian distribution;

[0063] S12. The perception model of the sensor is:

[0064] z = H(t)x + v

[0065] where, H(t) ∈ R p×n is the measurement matrix of the sensor, v ∈ R p×1 is the measurement noise of the sensor, and the measurement noise satisfies Gaussian distribution;

[0066] Step S2, use the consensus Kalman filter to correct the measurement values of each sensor to obtain the optimal estimated value of the target state, specifically including the following steps:

[0067] S21. For each sensor node, use the local Kalman filtering algorithm to calculate the covariance matrix information and measurement data information, assuming that node i has not received any information from its non-neighbor nodes. The iterative form of the local Kalman filter of node i is as follows:

[0068]

[0069]

[0070] M i (k) = (P i (k) -1 + S i (k)) -1 ,

[0071]

[0072] Among them, S i (k) represents the covariance matrix information of all neighbor nodes of node i at time k, and y i (k) represents the sensed data information of all neighbor nodes of node i at time k. Node i locally calculates y i (k) and S i (k);

[0073] S22. The continuous-time Kalman filter is in the following form:

[0074]

[0075] K = PH T R -1

[0076]

[0077] S23. (Consensus Kalman filter) Consider a sensor network with a sensing model in continuous time. Assume that each node uses the following distributed estimation algorithm:

[0078]

[0079] Among them, the initial condition P i (0) = P0, The dynamic equation of the estimation error (without noise) is a stable linear system, and its Lyapunov function is In addition, And all the estimators asymptotically reach consensus

[0080] S24. The iterative algorithm of the consensus Kalman filter:

[0081] ①: Initialization: P i = P0,

[0082] ②: When new data appears, calculate the following formulas

[0083] ③: Locally aggregate the sensed data and the covariance matrix:

[0084] J = NY{i}

[0085]

[0086] ④: Calculate the consensus Kalman filter estimate:

[0087] M i = (P i-1 +S i ) -1

[0088]

[0089] ⑤: Update the consistent Kalman filter state:

[0090] P i ←AM i A T +BQB T

[0091]

[0092] ⑥: End the loop;

[0093] In the above algorithm, the information broadcast by node i to all neighbor nodes is as follows:

[0094]

[0095] Step S3. Using the optimal estimated value of the target state obtained in S2, design a flocking movement algorithm for the elliptical multi-agent to achieve the tracking of the target, which specifically includes the following steps:

[0096] S31. First, determine the separation condition between two ellipses: Consider two ellipses i, j, where the semi-major axis of ellipse i is (a i , b i ), the center point coordinates are (x i , y i ), and the deflection angle is φ i ; the semi-major axis of ellipse j is (a j , b j ), the center point coordinates are (x j , y j ), and the deflection angle is φ j ; define the generalized distance between ellipses i, j as D ij ;

[0097]

[0098] Where:

[0099]

[0100] θ ij is the largest root of the following equation, that is, the rightmost root on the coordinate axis:

[0101]

[0102] All variables in here It is calculated by the following formula:

[0103]

[0104] x ij = x i - x j , y ij = y i - y j , φ ij = φ i - φ j

[0105]

[0106] Q i = -Q -1 (φ i )

[0107] where Q(·) is a rotation matrix, According to the generalized distance between the ellipsoids defined above, the separation condition between ellipsoids i and j can be obtained as:

[0108] D ij > 1

[0109] That is, when the above conditions are satisfied, the two ellipsoids are separated and have no contact with each other;

[0110] S32. Construct the potential function between the elliptical agents, which will be used in the Lyapunov function for the flocking control algorithm;

[0111] First, define the scalar function h(x, a, b) as follows:

[0112]

[0113] where a and b are constants and satisfy a < b, and the function f(y) is defined as

[0114] f(y) = 0, if y ≤ 0

[0115] f(y) = g(y), if y > 0

[0116] where g(y) has the following properties:

[0117] 1) g(t - a)g(b - t) > 0

[0118] 2) g(y) is a p-times differentiable function.

[0119]

[0120] where p is a positive integer. It follows that the function h(x,a,b) is a step function differentiable p times. Design the potential function between elliptical agents i and j which is defined as follows:

[0121]

[0122] where τ is a positive constant. Additionally, the positive constants a ij , b ij satisfy the following conditions:

[0123] a ij = δ ijd ,

[0124] where δ ijd , is defined as follows:

[0125]

[0126] Select the positive constant g ij to satisfy

[0127] Furthermore, the potential function between elliptical agents i and j that has been constructed By summing up the pairwise potential functions between all elliptical agents, the potential function is obtained

[0128]

[0129] For the convenience of calculating the derivative of the Lyapunov function later, the potential function between the given elliptical agents needs to be differentiated:

[0130]

[0131] For convenience of calculation, define

[0132] Then the following formula can be obtained:

[0133]

[0134] where:

[0135]

[0136] From the above calculation process, the derivative of the potential function can be obtained as follows:

[0137]

[0138] where:

[0139]

[0140] S33. Utilize the potential function constructed in S32 and the position error p between the agent and the target i -p od and the angular error to construct a Lyapunov function and give the control algorithm for the flocking movement of elliptical agents;

[0141] Lyapunov function:

[0142] where c1 and c2 are positive constants. Differentiating both sides of this function yields the following:

[0143]

[0144] where:

[0145]

[0146] Given the dynamic equation of the elliptical agent:

[0147]

[0148] where Ν is the set of all elliptical agents, and u i =[u xi , u yi , u φi T is the control input vector of agent i.

[0149] According to the given Lyapunov function, design the following control law:

[0150]

[0151] where s1, s2, and s3 are positive constants. The state (x od , y od , φ od ) of the target system is the optimal estimated value obtained through the consensus Kalman filter by the sensors mounted on the elliptical agent.

[0152] In addition, the function Ψ(x) is a scalar, differentiable, and bounded function that satisfies the following requirements:

[0153] 1) |Ψ(x)| ≤ M1

[0154] 2) Ψ(x) = 0 if x = 0, xΨ(x) > 0 if x ≠ 0

[0155] 3) Ψ(-x) = -Ψ(x), (x - y)[Ψ(x) - Ψ(y)] ≥ 0​

[0156] 4)

[0157] The arctan(·) function can be selected as the Ψ(·) function.

[0158] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, and improvements made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. A method for the design and implementation of a collaborative observation simulation platform based on flocking movement, characterized in that Including the following steps: S1. Determine the observation matrix of each sensor and measure the state of the target system; S2. Use the consensus Kalman filter to correct the measurement values of each sensor to obtain the optimal estimated value of the target state; S3. Design an elliptical multi-agent flocking movement algorithm to achieve the tracking of the target; The step S3 includes the following steps: S31. First, determine the separation condition between two ellipses: Consider two ellipses i and j, where the semi-axes of ellipse i are (a i , b i ), the center point coordinates are (x i , y i ), and the deflection angle is φ i ; the semi-axes of ellipse j are (a j , b j ), the center point coordinates are (x j , y j ), and the deflection angle is φ j ; define the generalized distance between ellipses i and j as D ij ; Where: S32. Construct the potential function between elliptical agents and incorporate it into the Lyapunov function for the flocking control algorithm; S33. Using the potential function constructed in S32 and the position error p between the agent and the target i -p od and the angular error construct a Lyapunov function and give a control algorithm for the flocking movement of the elliptical agent; Lyapunov function:

2. The method for designing and implementing a collaborative observation simulation platform based on flocking movement according to claim 1, wherein The step S1 includes the following steps: S11. Consider a continuous-time linear system representing the target, and its dynamic equation is as follows: where \(A\in R\) n×n , \(x\in R\) n×1 is the state of the system, \(B\in R\) n×n , \(w\in R\) n×1 is the process noise of the system; S12. The perception model of the sensor is: z = H(t)x + v where \(H(t)\in\mathbb{R}\) p×n is the measurement matrix of the sensor, and \(v\in\mathbb{R}\) p×1 is the measurement noise of the sensor.

3. The method for the design and implementation of a collaborative observation simulation platform based on flocking movement according to claim 1, characterized in that, The step S2 includes the following steps: S21. For each sensor node, use the local Kalman filtering algorithm to calculate the covariance matrix information and measurement data information. Assume that node i has not received any information from its non-neighbor nodes. The iterative form of the local Kalman filter of node i is as follows: M i (k) = (P i (k) -1 + S i (k)) -1 , Among them, S i (k) represents the covariance matrix information of all neighbor nodes of node i at time k, and y i (k) represents the sensed data information of all neighbor nodes of node i at time k. Node i locally calculates y i (k) and S i (k); S22. The continuous-time Kalman filter form is as follows: K = PH T R -1 S23. Consider a sensor network with a continuous-time perception model. Assume that each node uses the following distributed estimation algorithm: where the initial condition P i (0) = P0, S24. The iterative algorithm of the consensus Kalman filter: ①: Initialization: ②: Calculate the following formula when new data appears ③: Locally aggregate the perceived data and covariance matrix: J = N ∪ {i} ④: Calculate the consensus Kalman filter estimate: M i = (P i -1 + S i ) -1 ⑤: Update the state of the consensus Kalman filter: P i ←AM i A T +BQB T ⑥: End the loop; In the above algorithm, the information broadcast by node i to all its neighbor nodes is as follows: msg i =(u i ,U i ,x i )。

Images