A UAV formation control and target tracking method

Abstract

The invention discloses a method for controlling a formation of unmanned aerial vehicles (UAVs) and tracking a target, comprising the following steps: S1, forming and maintaining an equilateral triangle formation of UAVs; S2, tracking target information processing, first obtaining the position under the atmospheric coordinates at a place where the starting position signal is good, performing formation control to establish an equilateral triangle formation system of UAVs, then establishing the coordinates of the equilateral triangle formation system of UAVs, setting the target position of each UAV, obtaining the current position of the UAV, and controlling the lateral and longitudinal speeds of the UAVs to keep the UAVs moving toward the target position, ensuring that they form a stable equilateral triangle formation; determining the position information and motion information of the target, keeping the center of the formation system at the target point, ensuring that the UAV monitoring area covers the target, and maintaining both the monitoring stability and the formation structure stability.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles (UAVs), and in particular to a UAV formation control and target tracking method. Background Art

[0002] In the fields of search and rescue, agricultural protection, security monitoring, etc., a single drone cannot meet the needs of large-scale operations in terms of efficiency and endurance. Using multiple drones in formation to operate can improve the efficiency of operations and effectively solve the needs of large-scale monitoring operation application scenarios.

[0003] At present, most methods of UAV formation movement control first obtain the coordinates of all individual UAVs, and then perform formation control based on the tracking path and excitation of each UAV and the target. When passing through areas with poor signals, this formation control method is prone to control confusion. Summary of the invention

[0004] The purpose of the present invention is to provide a method for controlling a formation of unmanned aerial vehicles and tracking a target in view of the deficiencies in the prior art.

[0005] The technical solution of the present invention to solve the above problem is: a method for controlling a formation of unmanned aerial vehicles and tracking a target, comprising the following steps:

[0006] S1, form and maintain an equilateral triangle formation of drones;

[0007] S2, tracking target information processing.

[0008] Furthermore, the specific process of step S1 is as follows:

[0009] Step 1: Set M (M≥3) quad-rotor drones to form a drone swarm. Each quad-rotor drone communicates with each other via Bluetooth. The quad-rotor drone obtains its own position coordinates through GPS positioning. The position coordinates of each quad-rotor drone can be expressed as

[0010] Q i =[Q (i,X) ,Q (i,Y) ,Q (i,Z) ]

[0011] i is the label of each quadrotor drone, 1≤i≤M.

[0012] Step 2: Each quadrotor drone obtains the coordinate positions of all adjacent quadrotor drones through Bluetooth communication, sets the distance d between adjacent quadrotor drones, keeps all quadrotor drones at the same flight altitude, and establishes an equilateral triangle formation system for drones.

[0013] The position relationship expression between two adjacent quadrotor drones is:

[0014] (Q Xi+1 -Q Xi ) 2 +(Q Yi+1 -Q Yi ) 2 =d 2

[0015] Step 3: Take the center of the drone equilateral triangle formation system as the origin of the formation system coordinate system, and take the current position as the target position of the quadrotor drone in the formation system coordinate system. The target position vector of each quadrotor drone can be expressed as

[0016] P i =[P (i,X) ,P (i,Y) ]

[0017] The current position vector of the quadrotor drone can be expressed as

[0018] P j =[P (j,X) ,P (j,Y) ]

[0019] In the subsequent movement, the expected speed of the adjacent quadrotor drones is calculated through the distributed control algorithm, and the speed control is performed so that the adjacent quadrotor drones maintain a given distance.

[0020] U x =V x +(X j -X i ) / s

[0021] U y =V y +(Y j -Y i ) / s

[0022] U x It represents the desired lateral velocity vector control quantity of the UAV relative to the formation system coordinate system, U y represents the desired longitudinal velocity vector control quantity of the UAV relative to the formation system coordinate system, V x V is the expected lateral motion velocity vector of the UAV when the formation is stable; y is the expected longitudinal motion velocity vector of the UAV when the formation is stable, X j -X i is the horizontal distance between the current position and the target position, Y j -Y i It is the vertical distance between the current position and the target position.

[0023] Furthermore, step S2 specifically includes the following steps:

[0024] Step 1, receiving the target position sent externally, including the target's position information and motion information;

[0025] The target’s position information is A(a1, b1), and the target’s motion information includes V Ax 、V Ay , V Ax is the lateral velocity vector of the target motion, V Ay is the target motion longitudinal velocity vector.

[0026] Step 2: Select one of the drones in the drone equilateral triangle formation system as the tracking drone, and obtain the current position B (a2, b2) of the tracking drone in the atmospheric coordinate system.

[0027] To ensure that the center of the UAV equilateral triangle formation system moves to the target position, the tracking UAV is located at (X a , Y a ), then the target position of the tracking drone is (a1+X a , b1+Y a ), calculate the expected lateral motion velocity vector V of the tracking drone based on the current position and target position of the tracking drone x1 and the desired longitudinal velocity vector V y1 , the lateral velocity vector of the final motion of the tracking drone is V x＝ V x1 +V Ax , the transverse velocity vector is V y＝ V y1 +V Ay .

[0028] The present invention has the beneficial effects:

[0029] The present invention provides a method for controlling a formation of unmanned aerial vehicles (UAVs) and tracking a target. The method comprises the following steps: firstly obtaining a position in atmospheric coordinates at a place where a starting position signal is relatively good, performing formation control to establish an equilateral triangle formation system of UAVs, then establishing the coordinates of the equilateral triangle formation system of UAVs, setting a target position of each UAV, obtaining the current position of the UAVs, and controlling the lateral and longitudinal speeds of the UAVs to keep the UAVs moving toward the target position, ensuring that they form a stable equilateral triangle formation; determining the position information and motion information of the target, keeping the center of the formation system at the target point, ensuring that the UAV monitoring area covers the target, and maintaining both monitoring stability and formation structure stability. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 It is the front view of the present invention. DETAILED DESCRIPTION

[0031] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood by specific circumstances.

[0032] As shown in the figure, a method for UAV formation control and target tracking includes the following steps:

[0033] S1, form and maintain an equilateral triangle formation of drones. The specific process is:

[0034] Step 1: Set M (M≥3) quad-rotor drones to form a drone swarm. Each quad-rotor drone communicates with each other via Bluetooth. The quad-rotor drone obtains its own position coordinates through GPS positioning. The position coordinates of each quad-rotor drone can be expressed as

[0035] Q i =[Q( i,X ),Q( i,Y ),Q( i,Z )]

[0036] i is the label of each quadrotor drone, 1≤i≤M.

[0037] Step 2: Each quadrotor drone obtains the coordinate positions of all adjacent quadrotor drones through Bluetooth communication, sets the distance d between adjacent quadrotor drones, keeps all quadrotor drones at the same flight altitude, and establishes an equilateral triangle formation system for drones.

[0038] The position relationship expression between two adjacent quadrotor drones is:

[0039] (Q Xi+1 -Q Xi ) 2 +(Q Yi+1 -Q Yi ) 2 =d 2

[0040] Step 3: Take the center of the drone equilateral triangle formation system as the origin of the formation system coordinate system, and take the current position as the target position of the quadrotor drone in the formation system coordinate system. The target position vector of each quadrotor drone can be expressed as

[0041] P i =[P( i,X ),P( i,Y )]

[0042] The current position vector of the quadrotor drone can be expressed as

[0043] P j =[P( j,X ),P( j,Y )]

[0044] In the subsequent movement, the expected speed of the adjacent quadrotor drones is calculated through the distributed control algorithm, and the speed control is performed so that the adjacent quadrotor drones maintain a given distance.

[0045] U x =V x +(X j -X i ) / s

[0046] U y =V y +(Y j -Y i ) / s

[0047] U x It represents the desired lateral velocity vector control quantity of the UAV relative to the formation system coordinate system, U y represents the desired longitudinal velocity vector control quantity of the UAV relative to the formation system coordinate system, V x V is the expected lateral motion velocity vector of the UAV when the formation is stable; y is the expected longitudinal motion velocity vector of the UAV when the formation is stable, X j -X i is the horizontal distance between the current position and the target position, Y j -Y i It is the vertical distance between the current position and the target position.

[0048] First, obtain the position in atmospheric coordinates at a place with good signal at the starting position, perform formation control to establish an equilateral triangle formation system for UAVs, then establish the coordinates of the equilateral triangle formation system for UAVs, set the target position of each UAV, obtain the current position of the UAV, and control the lateral and longitudinal speed of the UAV to keep the UAV moving towards the target position, ensuring that they form a stable equilateral triangle formation.

[0049] S2, tracking target information processing. Specifically includes the following steps:

[0050] Step 1, receiving the target position sent externally, including the target's position information and motion information;

[0051] The target’s position information is A(a1, b1), and the target’s motion information includes V Ax 、V Ay , V Axis the lateral velocity vector of the target motion, V Ay is the target motion longitudinal velocity vector.

[0052] Step 2: Select one of the drones in the drone equilateral triangle formation system as the tracking drone, and obtain the current position B (a2, b2) of the tracking drone in the atmospheric coordinate system.

[0053] To ensure that the center of the UAV equilateral triangle formation system moves to the target position, the tracking UAV is located at (X a , Y a ), then the target position of the tracking drone is (a1+X a , b1+Y a ), calculate the expected lateral motion velocity vector V of the tracking drone based on the current position and target position of the tracking drone x1 and the desired longitudinal velocity vector V y1 , the lateral velocity vector of the final motion of the tracking drone is V x＝ V x1 +V Ax , the transverse velocity vector is V y＝ V y1 +V Ay .

[0054] The above description is only a preferred embodiment of the present invention and does not limit the present invention in other forms. Any technician familiar with the profession may use the technical content disclosed above to change or modify it into an equivalent embodiment with equivalent changes and apply it to other fields. However, any simple modification, equivalent change and modification made to the above embodiment based on the technical essence of the present invention without departing from the content of the technical solution of the present invention still falls within the protection scope of the technical solution of the present invention.

Claims

1. A method for controlling a formation of unmanned aerial vehicles and tracking a target, characterized in that: The following steps are involved: S1, form and maintain an equilateral triangle formation of drones; The specific process of step S1 is: Step 1: Set up M quad-rotor drones to form a drone swarm, where M ≥ 3; each quad-rotor drone communicates with each other via Bluetooth, and the quad-rotor drone obtains its own position coordinates through GPS positioning. The position coordinates of each quad-rotor drone can be expressed as Q i =[Q( i,X ),Q( i,Y ),Q( i,Z )] i is the number of each quadrotor drone, 1≤i≤M; Step 2: Each quadrotor drone obtains the coordinate positions of all adjacent quadrotor drones through Bluetooth communication, sets the distance d between adjacent quadrotor drones, and sets the flight height of all quadrotor drones to be consistent, thus establishing an equilateral triangle formation system for drones. The position relationship expression between two adjacent quadrotor drones is: (Q Xi+1 -Q Xi ) 2 +(Q Yi+1 -Q Yi ) 2 =d 2 Step 3: Take the center of the UAV equilateral triangle formation system as the origin of the formation system coordinate system, and take the current position as the target position of the quadrotor UAV in the formation system coordinate system; the target position vector of each quadrotor UAV can be expressed as P i =[P( i,X ),P( i,Y )] The current position vector of the quadrotor drone can be expressed as P j =[P( j,X ),P( j,Y )] In the subsequent movement, the desired speed of the adjacent quadrotor drones is calculated through the distributed control algorithm, and the speed is controlled so that the adjacent quadrotor drones maintain a given distance; U x =V x +(X j -X i ) / s OR y =V y +(And j -AND i ) / s U x It represents the desired lateral velocity vector control quantity of the UAV relative to the formation system coordinate system, U y represents the desired longitudinal velocity vector control quantity of the UAV relative to the formation system coordinate system, V x V is the expected lateral motion velocity vector of the UAV when the formation is stable; y is the expected longitudinal motion velocity vector of the UAV when the formation is stable, X j -X i is the horizontal distance between the current position and the target position, Y j -Y i is the longitudinal distance between the current position and the target position; S2, tracking target information processing; S2 specifically includes the following steps: Step 1, receiving the target position sent externally, including the target's position information and motion information; The target’s position information is A(a1, b1), and the target’s motion information includes V Ax 、V Ay , V Ax is the lateral velocity vector of the target motion, V Ay is the target motion longitudinal velocity vector; Step 2: Select one of the drones in the drone equilateral triangle formation system as the tracking drone, and obtain the current position B (a2, b2) of the tracking drone in the atmospheric coordinate system. To ensure that the center of the UAV equilateral triangle formation system moves to the target position, the tracking UAV is located at (X a , Y a ), then the target position of the tracking drone is (a1+X a , b1+Y a ), calculate the expected lateral motion velocity vector V of the tracking drone based on the current position and target position of the tracking drone x1 and the desired longitudinal velocity vector V y1 , the lateral velocity vector of the final motion of the tracking drone is V x＝ V x1 +V Ax , the transverse velocity vector is V y＝ V y1 +V Ay .

Images