METHOD FOR MULTI-UNMANNED AERIAL VEHICLE FORMATION TO PASS THROUGH FRAME-SHAPED OBSTACLE

Abstract

A method for a multi-UAV formation to pass through a frame-shaped obstacle, includes: obtaining in real time distance between the frame-shaped obstacle and the UAV formation and displacement of the UAV formation during a flight process; constructing a relative position model between the frame-shaped obstacle and the UAV formation; constructing a UAV velocity control model; constructing a parameter model based on the distance, the displacement, and the relative position model; constructing a position estimation model of the frame-shaped obstacle and a relative position estimation model among the UAVs based on the parameter model; and designing, based on the constructed position estimation model, the relative position estimation model, and the UAV velocity control model, a controller for the UAV formation to pass through the obstacle, and controlling a flight velocity of the UAV formation through the controller to enable the UAV formation to pass through the frame-shaped obstacle.

Description

CROSS-REFERENCE TO RELATED APPLICATIONS

This application claims priority to Chinese Patent Application No. 202210344428.2 with a filing date of Apr. 2, 2022. The content of the aforementioned application, including any intervening amendments thereto, is incorporated herein by reference.

TECHNICAL FIELD

The present disclosure relates to the field of unmanned aerial vehicle (UAV) technologies, and specifically, to a method for a multi-UAV formation to pass through a frame-shaped obstacle, which is applicable to rescue, firefighting, detection and other scenarios that require UAVs to collaboratively work during passing through obstacles.

BACKGROUND

In the last decade, UAVs have been widely used in civil, industrial and military applications. Cooperation among multiple UAVs during executing tasks such as passing through, rescue and resupply has been a research hotspot. Multiple UAVs form a UAV cluster and realize information exchange and sharing through aerial networking. The UAV cluster is capable of executing collaborative tasks through flight control, intelligent learning and other technologies, which give full play to the advantages of each UAV, so as to meet complex task requirements. Compared with a single UAV, the multi-UAV cluster has higher robustness, redundancy, scalability and efficiency when executing tasks, and the viability of the UAVs is also enhanced. When a node in the cluster fails or is attacked offline, the task undertaken by this node will be replaced by another node, which ensures the completion of tasks.

Regarding the cooperation among UAVs, it is the research emphasis on how the multi-UAV formation passes through obstacles. In order to realize the cooperation among UAVs, each UAV needs to obtain relative positions of neighboring UAVs which are usually obtained by external positioning systems such as the Global Position System (GPS) or the Global Navigation Satellite System (GNSS). When executing the pass-through task, the UAV usually carries a visual odometer to identify and calculate the center of the frame-shaped obstacle. However, it is difficult for the visual odometer to function since the UAVs usually work in some scenarios where the GPS or GNSS signals are weak, especially with dim light, low visibility, and occlusion.

In the prior art, there are the following solutions for the UAV formation collaboratively passing through the frame-shaped obstacle:

If the GPS is available, positioning modules such as GPS modules are installed on the UAVs and the obstacle, to obtain the positions of the UAVs and the obstacle, and the formation passes through the obstacle through position control. Alternatively, if indoor positioning based on Ultra Wide Band (UWB) base stations is available, the positions of the UAVs and the obstacle can also be obtained.

If monocular, binocular or infrared cameras (RGB-D) are available, map construction and positioning are performed through the simultaneous localization and mapping (SLAM) algorithm, and the obstacle is recognized based on feature points. The UAV formation may share positions through transmission protocols such as Bluetooth or WIFI. This solution has high requirements on hardware and involves more complex computation.

If both the GPS and the cameras are available, positioning modules such as GPS modules are installed on the UAVs to obtain positions, while monocular or binocular cameras are installed on the UAVs to identify the obstacle, which enables the formation to pass through the obstacle.

Without exception, all of the above solutions require at least one GPS or camera for the formation to pass through the obstacle, and they usually fail in some unknown environments where the GPS is limited and cameras are limited due to dim light.

SUMMARY OF PRESENT INVENTION

The purpose of the present disclosure is to provide a method for a multi-UAV formation to pass through a frame-shaped obstacle, which uses only the on-board sensor of the UAV and the UWB distance sensor. This method has the advantages of strong adaptability and no dependence on the light and GPS.

To achieve the above objective, the following technical solutions are provided:

A method for a multi-UAV formation to pass through a frame-shaped obstacle includes:

• • obtaining in real time distance between the frame-shaped obstacle and the UAV formation and displacement of the UAV formation during a flight process;
  • constructing a relative position model between the frame-shaped obstacle and the UAV formation;
  • constructing a UAV velocity control model;
  • constructing a parameter model based on the distance, the displacement, and the relative position model;
  • constructing a position estimation model of the frame-shaped obstacle and a relative position estimation model among UAVs in the UAV formation based on the parameter model; and
  • designing, based on the position estimation model, the relative position estimation model, and the UAV velocity control model, a controller for the UAV formation to pass through the obstacle, and controlling a flight velocity of the UAV formation through the controller to enable the UAV formation to pass through the frame-shaped obstacle.

Further, the relative position model between the frame-shaped obstacle and the UAV formation is constructed by using the following formula:

$\begin{array}{cc}\{\begin{array}{c}{q}_0\left(k\right)=p_0\left(k\right)-p_s\\ q_{j0}\left(k\right)=p_j\left(k\right)-p_0\left(k\right)\end{array}& \left(1.1\right)\end{array}$

where p₀(k) and p_j(k) respectively represent positions of a primary UAV and a UAV j in a global coordinate system at a moment k, p_s represents a position of the frame-shaped obstacle in the global coordinate system, q₀(k) represents a relative position between a primary UAV and the frame-shaped obstacle, and q_j0(k) represents a relative position between the UAV j and the primary UAV.

Further, the constructing the UAV velocity control model may include:

modeling each of the UAVs as a discrete-time integrator with bounded velocities by using the following formula:

$\begin{array}{cc}\{\begin{array}{c}{p}_0\left(k+1\right)=p_0\left(k\right)+{\overline{v}}_0\left(k\right)T,{\overline{v}}_0\left(k\right)\le V_0\\ p_j\left(k+1\right)=p_j\left(k\right)+{\overline{v}}_j\left(k\right)T,{\overline{v}}_j\left(k\right)\le V_j\end{array}& \left(1.2\right)\end{array}$

where T is a sampling period, v₀(k) and v_j(k) are velocity bounds to the primary UAV and the UAV j respectively, V₀ and V_j are maximum velocities of the primary UAV and the UAV j respectively, and V_j>V₀.

Further, the constructing the parameter model based on the distance, the displacement, and the relative position model may include:

• • expressing the distance and the displacement obtained in real time by relative positions among the UAVs as follows:

d ₀(k)=∥q₀(k)∥,d_j0(k)=∥q_j0(k)∥  (1.3)

φ₀(k)=q₀(k+1)−q₀(k)

φ_j0(k)=q_j0(k+1)−q_j0(k)=φ_j(k)−φ₀(k)  (1.4)

• • constructing the parameter model based on the formulas (1.3) and (1.4):

$\begin{array}{cc}\{\begin{array}{c}{\stackrel{‵}{{\rm O}}}_s\left(k\right)={{\phi }_0\left(k\right)}^T{p}_0\left(k\right)-\frac{1}{2}\left(d_0^2\left(k\right)-d_0^2\left(k-1\right)+{{\phi }_0\left(k\right)}^2\right)\\ ={{\phi }_0\left(k\right)}^T\left(p_0\left(0\right)+\sum _{i=1}^k{\phi }_0\left(i\right)\right)-\frac{1}{2}(d_0^2\left(k\right)-d_0^2\left(k-1\right)+\\ {{\phi }_0\left(k\right)}^2)={{\phi }_0\left(k\right)}^T{p}_s\left(k\right)\\ {\stackrel{‵}{{\rm O}}}_{j0}\left(k\right)=\frac{1}{2}\left(d_{j0}^2\left(k\right)-d_{j0}^2\left(k-1\right)+{{\phi }_{j0}\left(k\right)}^2\right)\\ ={{\phi }_{j0}\left(k\right)}^T{q}_{j0}\left(k\right)\end{array}& \left(1.5\right)\end{array}$

where p₀(0) represents a position of the primary UAV at an initial moment, d₀(k) represents a distance between the primary UAV and the frame-shaped obstacle, d_j0(k) represents a distance between the primary UAV and the a UAV j, φ₀(k) represents a displacement of the primary UAV, φ_j(k) represents a displacement of the UAV j, φ_j0(k) represents a relative displacement between the primary UAV and the UAV j, as ò_s(k) is a constructed parameter model related to the position p_s(k) of the frame-shaped obstacle, ò_j0(k) is a constructed parameter model related to the relative position q_j0(k) between the UAV j and the primary UAV, and the superscript T represents transposition.

Further, the constructing a position estimation model of the frame-shaped obstacle may specifically include:

$\begin{array}{cc}\{\begin{array}{c}{R}_0\left(k+1\right)={\left(\mu I+{\Phi }_0\left(k\right)M\left(k\right){{\Phi }_0\left(k\right)}^T\right)}^{-1}\\ ={\left(\lambda {R_0\left(k\right)}^{-1}+\mu \left(1-\lambda \right)I+{\phi }_0\left(k\right){{\phi }_0\left(k\right)}^T\right)}^{-1}\\ =\frac{1}{\lambda }\left(I-\frac{{\chi }_0\left(k\right){\phi }_0\left(k\right){{\phi }_0\left(k\right)}^T}{{{\phi }_0\left(k\right)}^T{\chi }_0\left(k\right){\phi }_0\left(k\right)+\lambda }\right){\chi }_0\left(k\right)\\ {\chi }_0\left(k\right)={\left({R_0\left(k\right)}^{-1}+\frac{\left(1-\lambda \right)\mu I}{\lambda }\right)}^{-1}\\ {\hat{p}}_s\left(k+1\right)={\hat{p}}_s\left(k\right)+\lambda \mu R_0\left(k+1\right)\left({\hat{p}}_s\left(k\right)-{\hat{p}}_s\left(k-1\right)\right)+\\ R_0\left(k+1\right){\phi }_0\left(k\right)\left({\stackrel{‵}{{\rm O}}}_s\left(k\right)-{{\phi }_0\left(k\right)}^T{\hat{p}}_s\left(k\right)\right)\end{array}& \left(1.6\right)\end{array}$

where I is a unit matrix, R₀(k) is a covariance matrix at the moment k, {circumflex over (p)}_s(k) is an estimation value of the position p_s(k), λ is a forgetting factor, μ is a damping term, and Φ₀(k) is a set matrix of φ₀(k) and satisfies:

$\sum _{i=k+1}^{k+N}{\phi }_0\left(i\right){{\phi }_0\left(i\right)}^T\ge {\delta }_{1}I>0,$

δ₁ is a minimum eigenvalue of the set matrix, N represents a dimensionality of the method, and M (k) is a forgetting operator matrix of λ and satisfies:

$M\left(k\right)=\left[\begin{array}{cc}\lambda M\left(k-1\right)& 0\\ 0& 1\end{array}\right].$

Further, the relative position estimation model among the UAVs is expressed as follows:

$\begin{array}{cc}\{\begin{array}{c}{R}_{j0}\left(k+1\right)={\left({\Phi }_{j0}\left(k\right)M\left(k\right){{\Phi }_{j0}\left(k\right)}^T\right)}^{-1}\\ ={\left(\lambda {R_{j0}\left(k\right)}^{-1}+{\phi }_{j0}\left(k\right){{\phi }_{j0}\left(k\right)}^T\right)}^{-1}\\ =\frac{1}{\lambda }\left(I-\frac{R_{j0}\left(k\right){\phi }_{j0}\left(k\right){{\phi }_{j0}\left(k\right)}^T}{{{\phi }_{j0}\left(k\right)}^T{R}_{j0}\left(k\right){\phi }_{j0}\left(k\right)+\lambda }\right)R_{j0}\left(k\right)\\ {\hat{q}}_{j0}\left(k+1\right)={\hat{q}}_{j0}\left(k\right)+{\phi }_{j0}\left(k\right)+\\ R_{j0}\left(k\right){\phi }_{j0}\left(k\right)\left({\stackrel{‵}{{\rm O}}}_{j0}\left(k\right)-{{\phi }_{j0}\left(k\right)}^T{\hat{q}}_{j0}\left(k\right)\right)\end{array}& \left(1.7\right)\end{array}$

where Φ_j0(k) is a set matrix of φ_j0(k), R_j0(k) is a covariance matrix, and {circumflex over (q)}_j0(k) is an estimated position of the UAV j relative to the primary UAV.

Further, the controller for the UAV formation to pass through the obstacle may be designed as follows:

$\begin{array}{cc}\{\begin{array}{c}{v}_0\left(k\right)=A_0\left(H_0\left(k\right)-p_0\left(0\right)-\sum _{i=0}^{k-1}{\phi }_0\left(i\right)+{\hat{p}}_s\left(k\right)+\varsigma \left(k\right)\right)\\ {\stackrel{\_}{v}}_0\left(k\right)=\frac{\min \left(V_0,v_0\left(k\right)\right)}{v_0\left(k\right)}{v}_0\left(k\right),V_0>0\\ v_j\left(k\right)=A_j\left(H_{j0}\left(k\right)-{\hat{q}}_{j0}\left(k\right)\right)\\ {\stackrel{\_}{v}}_j\left(k\right)=\frac{\min \left(V_j,v_j\left(k\right)\right)}{v_j\left(k\right)}{v}_j\left(k\right),V_j>V_0>0\end{array},& \left(1.8\right)\end{array}$

where φ₀(i) represents a displacement of the primary UAV at a moment i, {circumflex over (p)}_s(k) is an estimation value of the position p_s(k) of the frame-shaped obstacle at the moment k, v₀(k) and v_j(k) are velocities of the primary UAV and the UAV j obtained by event-triggering the controller for the formation to pass through the obstacle, v₀(k) and v_j(k) are obtained by limiting v₀(k) and v_j(k) to enable final output velocities to be less than preset maximum velocities V₀ and V_j, A₀ and A; are designed controller coefficients, and ζ(k) is a continuous excitation signal; and H₀(k) and H_j0(k) each is a switching function that is a segmented constant function, and H₀(k) and H_j0(k) represent a signals switching between displaying formation and passing through the obstacle.

Further, H₀(k) and H_j0(k) are specifically expressed as follows:

$\begin{array}{cc}{H}_0\left(k\right)=\{\begin{array}{cc}{q}_0^s& k\in \left[k_1,k_2\right]\\ \mathrm{Rot}*\left({\hat{q}}_0\left(k\right)+\alpha \right)& k\in \left(k_2,k_3\right)\\ q_0^e& k\in \left[k_3,k_4\right]\end{array}& \left(1.9\right)\end{array}$ $H_{j0}\left(k\right)=\{\begin{array}{cc}\mathrm{Rot}*q_{j0}^{\mathrm{pass}}& k\in \left(k_2,k_3\right)\\ q_{j0}^{\mathrm{init}}& k\in \left[k_1,k_2\right]\bigcup \left[k_3,k_3\right]\end{array}$

where k₁ is an initial time, k₄ is an operation time, k₂=sup{k>k₁|{circumflex over (q)}₀(k)<q₀^s} k₃=sup{k>k₃|{circumflex over (q)}₀(k)>q₀^e}, {circumflex over (q)}₀(k) represents an estimation value of a position of the primary UAV at the moment k, sup represents an upper bound, q₀^s and q₀^e are start and end points artificially set for an event trigger respectively, Rot is a rotation matrix of the frame-shaped obstacle relative to a coordinate system; in a two-dimensional case, the rotation matrix is:

$\mathrm{Rot}=\left(\begin{array}{cc}\cos \left(w\right)& -\sin \left(w\right)\\ \sin \left(w\right)& \cos \left(w\right)\end{array}\right),$

w is an angle of the obstacle relative to the UAV in the coordinate system; α is a pass-through step during passing through the obstacle; q_j0^init is a formation of the UAVs before and after passing through the obstacle, and q_j0^pass is a formation of the UAVs during passing through the obstacle.

A computer device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, and the processor executes the computer program to perform the steps of the method described above.

A computer-readable storage medium is provided. The computer-readable storage medium stores a computer program, and the steps of the method described above are performed when the computer program is executed by a processor.

Compared with the prior art, technical solutions provided in the present disclosure have the following advantages:

• • 1. In the present disclosure, there is neither dependence on the GPS to obtain accurate global position information, nor dependence on the cameras to obtain the visual information. The multi-UAV formation passes through the frame-shaped obstacle using the distance and displacement data obtained by the UWB sensor and the Inertia Measurement Unit (IMU) sensor. The method provided in the present disclosure is applicable to numerous special scenarios since the UWB sensor and the IMU sensor have strong anti-interference ability in long distance scenarios, and are hardly influenced by the environment. Further, in some cases, the UWB sensor and the IMU sensor can be used for precise positioning, and precise calculation of the relative distances between the UAVs which further ensures coverage search of the UAV formation in special environments, especially in GPS-constrained situations.
  • 2. In the present disclosure, the computation complexity is very low. Compared with positioning and navigation through the existing simultaneous localization and mapping (SLAM) technology, the computation volume in the technical solutions of the present disclosure is very low and the required data can be processed very fast with high precision but low requirements on hardware environment. Therefore, the present disclosure requires lower computation volume and simpler hardware configuration, which can greatly reduce the costs.
  • 3. The present disclosure innovatively connects and integrates the relative fixed formation and obstacle pass-through, to propose a new solution in this field.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a two-dimensional view of the simulation effect of the method in MATLAB according to the present disclosure;

FIG. 2 is a three-dimensional view of the simulation effect of the method in MATLAB according to the present disclosure; and

FIG. 3 to FIG. 5 are schematic diagrams of simulation effects at different positions in gazebo according to the present disclosure.

DETAILED DESCRIPTION OF THE EMBODIMENTS

In the prior at, the GPS is needed to obtain global absolute positions of UAVs and relative positions among the UAVS for the UAV formation to pass through the obstacle. The data volume is very large and hence the processing speed is slowed down. In addition, good light environment is necessary for passing through the obstacle. In view of this, the present disclosure proposes a method for a multi-UAV formation to pass through a frame-shaped obstacle, in which the GPS is not needed, and the method hence has strong environment applicability.

The method for the multi-UAV formation to pass through the frame-shaped obstacle, includes the following steps.

Step 1: Obtain in real time a distance between the frame-shaped obstacle and the UAV formation and a displacement of the UAV formation during flight.

The distance d₀ between the frame-shaped obstacle and the UAV formation and the distance d_j0 between a UAV j and a primary UAV 0 are obtained.

In addition, the UAVs obtain velocity information in real time and perform integration processing to obtain the displacement φ₀ and φ_j. φ₀ represents a displacement of the primary UAV (leader) in the formation, φ_j represents a displacement of the UAV j (follower) in the formation, and j represents the serial number of the UAV.

Step 2: Construct a relative position model between the frame-shaped obstacle and the UAV formation by using the following formula:

$\begin{array}{cc}\{\begin{array}{c}{q}_0\left(k\right)=p_0\left(k\right)-p_s\\ q_{j0}\left(k\right)=p_j\left(k\right)-p_0\left(k\right)\end{array}& \left(1.1\right)\end{array}$

p₀(k) and p_j(k) respectively represent positions of the primary UAV and the UAV j in a global coordinate system at a moment k, p_s represents a position of the frame-shaped obstacle in the global coordinate system, q₀(k) represents a relative position between the primary UAV and the frame-shaped obstacle, and q_j0(k) represents a relative position between the UAV j and the primary UAV.

Step 3: Construct a UAV velocity control model, including:

modeling each of the UAVs as a discrete-time integrator with bounded velocities by using the following formula:

$\begin{array}{cc}\{\begin{array}{c}{p}_0\left(k+1\right)=p_0\left(k\right)+{\stackrel{\_}{v}}_0\left(k\right)T,{\stackrel{\_}{v}}_0\left(k\right)\le V_0\\ p_j\left(k+1\right)=p_j\left(k\right)+{\stackrel{\_}{v}}_j\left(k\right)T,{\stackrel{\_}{v}}_j\left(k\right)\le V_j\end{array}& \left(1.2\right)\end{array}$

where T is a sampling period, v₀(k) and v_j(k) are velocity bounds to the primary UAV and the UAV j respectively, V₀ and V_j are maximum velocities of the primary UAV and the UAV j respectively, and V_j>V₀. k and k+1 represent moments k and k+1, similarly hereinafter. Details are not repeated again.

Step 4: Construct a parameter model based on the distance, the displacement obtained in the step 1, and the relative position model obtained in the step 2. Details are as follows.

The distance and the displacement obtained in real time are expressed by relative positions among the UAVs as follows:

d ₀(k)=∥q₀(k)∥,d_j0(k)=∥q_j0(k)∥  (1.3)

φ₀(k)=q₀(k+1)−q₀(k)

φ_j0(k)=q_j0(k+1)−q_j0(k)=φ_j(k)−φ₀(k)  (1.4)

Then, the parameter model is constructed based on the formulas (1.3) and (1.4):

$\begin{array}{cc}\{\begin{array}{c}{\stackrel{‵}{{\rm O}}}_s\left(k\right)={{\phi }_0\left(k\right)}^T{p}_0\left(k\right)-\frac{1}{2}\left(d_0^2\left(k\right)-d_0^2\left(k-1\right)+{{\phi }_0\left(k\right)}^2\right)\\ ={{\phi }_0\left(k\right)}^T\left(p_0\left(0\right)+\sum _{i=1}^k{\phi }_0\left(i\right)\right)-\frac{1}{2}(d_0^2\left(k\right)-d_0^2\left(k-1\right)+\\ {{\phi }_0\left(k\right)}^2)={{\phi }_0\left(k\right)}^T{p}_s\left(k\right)\\ {\stackrel{‵}{{\rm O}}}_{j0}\left(k\right)=\frac{1}{2}\left(d_{j0}^2\left(k\right)-d_{j0}^2\left(k-1\right)+{{\phi }_{j0}\left(k\right)}^2\right)\\ ={{\phi }_{j0}\left(k\right)}^T{q}_{j0}\left(k\right)\end{array}& \left(1.5\right)\end{array}$

In the formula (1.5), p₀(0) represents a position of the primary UAV at an initial moment, ò_s(k) is a constructed parameter model related to the position p_s(k) of the frame-shaped obstacle, ò_j0(k) is a constructed parameter model related to a relative position q_j0(k) between the UAV j and the primary UAV, and the superscript T represents transposition.

Step 5: Construct a position estimation model of the frame-shaped obstacle and a relative position estimation model among the UAVs based on the parameter model in the step 4, including the following derivations:

Step 5.1: Construct the position estimation model of the frame-shaped obstacle.

According to formula (1.5), a forgetting factor λ is added in this scheme to prevent new data from being overwhelmed by old data. In addition, a damping term μ is designed in this scheme to increase the stability of the algorithm in order to prevent unexpected cases during recognition of {circumflex over (p)}_s(k).

$\begin{array}{cc}\{\begin{array}{c}{R}_0\left(k+1\right)={\left(\mu I+{\Phi }_0\left(k\right)M\left(k\right){{\Phi }_0\left(k\right)}^T\right)}^{-1}\\ ={\left(\lambda {R_0\left(k\right)}^{-1}+\mu \left(1-\lambda \right)I+{\phi }_0\left(k\right){{\phi }_0\left(k\right)}^T\right)}^{-1}\\ =\frac{1}{\lambda }\left(I-\frac{{\chi }_0\left(k\right){\phi }_0\left(k\right){{\phi }_0\left(k\right)}^T}{{{\phi }_0\left(k\right)}^T{\chi }_0\left(k\right){\phi }_0\left(k\right)+\lambda }\right){\chi }_0\left(k\right)\\ {\chi }_0\left(k\right)={\left({R_0\left(k\right)}^{-1}+\frac{\left(1-\lambda \right)\mu I}{\lambda }\right)}^{-1}\\ {\hat{p}}_s\left(k+1\right)={\hat{p}}_s\left(k\right)+\lambda \mu R_0\left(k+1\right)\left({\hat{p}}_s\left(k\right)-{\hat{p}}_s\left(k-1\right)\right)+\\ R_0\left(k+1\right){\phi }_0\left(k\right)\left({\stackrel{‵}{{\rm O}}}_s\left(k\right)-{{\phi }_0\left(k\right)}^T{\hat{p}}_s\left(k\right)\right)\end{array}& \left(1.6\right)\end{array}$

I is a unit matrix, R₀(k) is a covariance matrix at the moment k, {circumflex over (p)}_s (k) is an estimation value of the position p_s(k) of the frame-shaped obstacle at a moment k. In this scheme, the parameter marked with A represents an estimation of the parameter, similarly hereinafter. Φ₀(k) is a set matrix of φ₀(k) and satisfies:

$\sum _{i=k+1}^{k+N}{\phi }_0\left(i\right){{\phi }_0\left(i\right)}^T\ge {\delta }_{1}I>0,$

δ₁ is a minimum eigenvalue of the set matrix, and N represents a dimension of the method. N is 2 if the position is estimated in the two-dimensional case, and N is 3 if the position is estimated in the three-dimensional case. M(k) is a forgetting operator matrix of λ, and satisfies:

$M\left(k\right)=\mathrm{diag}\left(\begin{array}{cccc}{\lambda }^{k-1}& {\lambda }^{k-2}& \dots & {\lambda }^0\end{array}\right)=\left[\begin{array}{cc}\lambda M\left(k-1\right)& 0\\ 0& 1\end{array}\right].$

Therefore, when 0<λ≤1, μ²≤δ₁²λ^2N×3, the primary UAV can accurately estimate the position of the frame-shaped obstacle by using the formula (1.6).

Step 5.2: Construct the relative position estimation model among the UAVs, including the following derivations:

The following relative position estimator is designed for the UAV j:

$\begin{array}{cc}\{\begin{array}{c}{R}_{j0}\left(k+1\right)={\left({\Phi }_{j0}\left(k\right)M\left(k\right){{\Phi }_{j0}\left(k\right)}^T\right)}^{-1}\\ ={\left(\lambda {R_{j0}\left(k\right)}^{-1}+{\phi }_{j0}\left(k\right){{\phi }_{j0}\left(k\right)}^T\right)}^{-1}\\ =\frac{1}{\lambda }\left(I-\frac{R_{j0}\left(k\right){\phi }_{j0}\left(k\right){{\phi }_{j0}\left(k\right)}^T}{{{\phi }_{j0}\left(k\right)}^T{R}_{j0}\left(k\right){\phi }_{j0}\left(k\right)+\lambda }\right)R_{j0}\left(k\right)\\ {\hat{q}}_{j0}\left(k+1\right)={\hat{q}}_{j0}\left(k\right)+{\phi }_{j0}\left(k\right)+\\ R_{j0}\left(k\right){\phi }_{j0}\left(k\right)\left({\stackrel{‵}{{\rm O}}}_{j0}\left(k\right)-{{\phi }_{j0}\left(k\right)}^T{\hat{q}}_{j0}\left(k\right)\right)\end{array}& \left(1.7\right)\end{array}$

Φ_j0(k) is a set matrix of φ_j0(k), R_j0(k) is a covariance matrix, and {circumflex over (q)}_j0(k) is an estimated position of the UAV j relative to the primary UAV. When 0<λ≤1, the relative position between the UAV j and the primary UAV can be accurately estimated by using the formula (1.7).

Step 6: Design, based on the position estimation model (1.6) and the relative position estimation model (1.7) constructed in the step 5, and the UAV velocity control model in the step 3, a controller for the UAV formation to pass through the obstacle, and control a flight velocity of the UAV formation through the controller to enable the UAV formation to pass through the frame-shaped obstacle.

The controller for the UAV formation to pass through the obstacle may be specifically designed as follows:

An event is designed as follows based on the formulas (1.6) and (1.7) and the UAV velocity control model (1.2) to trigger the controller for the formation to pass through.

$\begin{array}{cc}\{\begin{array}{c}{v}_0\left(k\right)=A_0\left(H_0\left(k\right)-p_0\left(0\right)-\sum _{i=0}^{k-1}{\phi }_0\left(i\right)+{\hat{p}}_s\left(k\right)+\varsigma \left(k\right)\right),\\ {\overline{v}}_0\left(k\right)=\frac{\min \left(V_0,v_0\left(k\right)\right)}{v_0\left(k\right)}{v}_0\left(k\right),V_0>0\\ v_j\left(k\right)=A_j\left(H_{j0}\left(k\right)-{\hat{q}}_{j0}\left(k\right)\right)\\ {\overline{v}}_j\left(k\right)=\frac{\min \left(V_j,v_j\left(k\right)\right)}{v_j\left(k\right)}{v}_j\left(k\right),V_j>V_0>0\end{array}& \left(1.8\right)\end{array}$

{circumflex over (p)}_s(k) is an estimation of the position p_s(k) of the frame-shaped obstacle at the moment k, v₀(k) and v_j(k) are velocities of the primary UAV and the UAV j obtained by event-triggering the controller for the formation to pass through the obstacle, and v₀(k) and v_j(k) are obtained by limiting v₀(k) and v_j(k) to enable final output velocities to be less than preset maximum velocities V₀ and V_j. Designing is based on the UAV control model in the formula (1.2). A₀ and A_j are designed controller coefficients, the range of which is defined by the formula (1.10). ζ(k) is a continuous excitation signal, which is used to avoid a singular solution.

H₀(k) and H_j0(k) each are a switching function that is a segmented constant function, which represents a switching signal between displaying formation and passing through the obstacle. The details are as follows:

$\begin{array}{cc}{H}_0\left(k\right)=\{\begin{array}{cc}{q}_0^s& k\in \left[k_1,k_2\right]\\ Ro{t}^{*}\left({\hat{q}}_0\left(k\right)+\alpha \right)& k\in \left(k_2,k_3\right)\\ q_0^e& k\in \left[k_3,k_4\right]\end{array}& \left(1.9\right)\end{array}$ $H_{j0}\left(k\right)=\{\begin{array}{cc}Ro{t}^{*}{q}_{j0}^{\mathrm{pass}}& k\in \left(k_2,k_3\right)\\ q_{j0}^{\mathrm{init}}& k\in \left[k_1,k_2\right]\bigcup \left[k_3,k_4\right]\end{array}$

k₁ is an initial time, k₄ is an operation time, k₂=sup{k>k₁|{circumflex over (q)}₀(k)<q₀^s}, k₃=sup{k>k₃|{circumflex over (q)}₀(k)>q₀^e}, {circumflex over (q)}₀(k) represents an estimation of the position of the primary UAV at the moment k, and sup represents an upper bound. q₀^s and q₀^e are start and end points artificially set for the event trigger, expressing that formation pass-through starts when the relative position between the primary UAV and the frame-shaped obstacle reaches q₀^s, and ends when the relative position reaches q₀^e. Rot is a rotation matrix of the frame-shaped obstacle relative to the coordinate system. In a two-dimensional case, the rotation matrix is:

$Rot=\left(\begin{array}{cc}\cos \left(w\right)& -\sin \left(w\right)\\ \sin \left(w\right)& \cos \left(w\right)\end{array}\right).$

w is an angle of the obstacle relative to the UAV in the coordinate system, and the angle may be artificially specified, or be measured by the odometer on the UWB sensor. α is a pass-through step during passing through the obstacle. q_j0^init is a formation of the UAVs before and after passing through the obstacle, and q_j0^pass is a formation of the UAVs during passing through the obstacle.

It should be noted that the formation pass-through error is exponentially convergent when the controller (1.8) and (1.9) for the UAV formation to pass through the obstacle satisfies the following conditions:

$\begin{array}{cc}0<-{\left(1-A_{0}T{s}_0\left(k\right)\right)}^2+\frac{4}{3}<1& \left(1.1\right)\end{array}$ $0<-{\left(1-A_{j}T{s}_j\left(k\right)\right)}^2+\frac{3}{2}<1$

T represents a sampling time,

$s_0\left(k\right)=\frac{\min \left(V_0,v_0\left(k\right)\right)}{v_0\left(k\right)}<1,$ $\mathrm{and}$ $s_j\left(k\right)=\frac{\min \left(V_j,v_J\left(k\right)\right)}{v_j\left(k\right)}<1.$

When the method of the present disclosure is applied in practice, firstly, the distance d₀ and d_j0 and the displacement φ₀ and φ_j that are acquired in the step 1 are substituted into the parameter model in the step 4 for solving, to obtain ò_s(k) and ò_j0(k), which are then substituted into the position estimation model of the obstacle and the relative position estimation model among the UAVs in the step 5, to obtain {circumflex over (p)}_s(k) and {circumflex over (q)}^j0(k). Further, {circumflex over (p)}_s(k) and {circumflex over (q)}_j0(k) are substituted into the controller for the UAV formation to pass through the obstacle in the step 6, and the UAV formation is controlled to fly according to the preset formula (1.9). In addition, the vector velocity of the UAV formation at each moment is calculated by using the formula (1.8), and the UAV formation passes through the frame-shaped obstacle through speed control. This can solve the formation problem in the limited GPS environment and the pass-through problem in the limited light environment.

Simulation Experiments

The simulation effect of the method according to the present disclosure in MATLAB is shown in FIG. 1 and FIG. 2. The distance and displacement required for the algorithms is obtained by mathematical operations on coordinates. It should be noted that the distance is 2-Norm of a difference between two coordinates, and the displacement is the difference between two coordinates. The global coordinate values are not used in the numerical verification of the algorithms.

Further, the method of the present disclosure is applied to GAZEBO software for verification, which depends on the powerful UAV dynamics model of GAZEBO. This aims to ensure that the mathematical model designed in the algorithms can be applied in a real UAV model. The simulation effects in GAZEBO are shown in FIG. 3 to FIG. 5.

It can be learned that the multiple UAVs, as envisioned, maintain a formation before passing through, then change the formation to meet the requirements for passing through the frame-shaped obstacle, and resume the original formation afterwards. This verifies the reasonableness of this method in the real UAV model.

It should be noted that the above embodiments are merely intended to explain the technical solutions of the present application, rather than to limit the present application. Although the present application is described in detail with reference to the above embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the above embodiments or make equivalent substitutions on some technical features therein without departing from the spirit of the technical solutions. However, these modifications or substitutions should fall within the protection scope of the present application.

Claims

1. A method for a multi-unmanned aerial vehicle (UAV) formation to pass through a frame-shaped obstacle, comprising: obtaining in real time a distance between the frame-shaped obstacle and the UAV formation and a displacement of the UAV formation during a flight process;constructing a relative position model between the frame-shaped obstacle and the UAV formation;constructing a UAV velocity control model;constructing a parameter model based on the distance, the displacement, and the relative position model;constructing a position estimation model of the frame-shaped obstacle and a relative position estimation model among UAVs in the UAV formation based on the parameter model; anddesigning, based on the position estimation model, the relative position estimation model, and the UAV velocity control model, a controller for the UAV formation to pass through the obstacle, and controlling a flight velocity of the UAV formation through the controller to enable the UAV formation to pass through the frame-shaped obstacle.

2. The method according to claim 1, wherein the relative position model between the frame-shaped obstacle and the UAV formation is constructed by using the following formula:

3. The method according to claim 1, wherein the constructing the UAV velocity control model comprises: modeling each of the UAVs as a discrete-time integrator with bounded velocities by using the following formula:

4. The method according to claim 1, wherein the constructing the parameter model based on the distance, the displacement, and the relative position model comprises: expressing the distance and the displacement obtained in real time by relative positions among the UAVs as follows: d0(k)=∥q0(k)∥,dj0(k)=∥qj0(k)∥  (1.3)φ0(k)=q0(k+1)−q0(k)φj0(k)=qj0(k+1)−qj0(k)=φj(k)−φ0(k)  (1.4)constructing the parameter model based on the formulas (1.3) and (1.4):

5. The method according to claim 4, wherein the constructing the position estimation model of the frame-shaped obstacle specifically comprises:

6. The method according to claim 5, wherein the relative position estimation model among the UAVs is expressed as follows:

7. The method according to claim 1, wherein the controller for the UAV formation to pass through the obstacle is designed as follows:

8. The method according to claim 7, wherein H0(k) and Hj0(k) are specifically expressed as follows:

9. A terminal device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to perform the steps of the method according claim 1.

10. A non-transitory computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and the steps of the method according claim 1 are performed when the computer program is executed by a processor.