A nonlinear motion estimation and trajectory planning method for security drones

Abstract

The present invention relates to a nonlinear motion estimation and trajectory planning method for a security drone, which models the problem of catching illegal drones at the terminal into a nonlinear motion estimation based on information gain and a trajectory planning problem with terminal constraints, and adopts an improved extended Kalman filter based on information gain to perform state estimation on illegal drones in approaching situations, thereby enhancing the stability of state estimation. The trajectory planning of the present invention adopts an offline state space constraint primitive sampling and online trajectory optimization method, so the generated flight trajectory must satisfy the drone's own kinematic constraints, and after obtaining the illegal drone's position information, it can be effectively captured and countered.

Description

Technical Field

[0001] The invention relates to a nonlinear motion estimation and trajectory planning method for a security unmanned aerial vehicle. Background Art

[0002] A drone, or unmanned aircraft, is an unmanned aircraft controlled by a radio remote control device and a self-contained program control device. In recent years, with the gradual maturity of the drone industry, the production cost of drones has been greatly reduced, and the demand for civilian drones in various industries has increased day by day. At the same time, the promotion of drone companies represented by DJI has made drones more and more popular in life. However, while people feel convenient, they are also facing the influence of various unstable factors. For example, illegal drones invading airports and air defense identification zones have had a serious impact on aerospace safety. Drones themselves have a certain load, and with a little modification, they can become offensive weapons, which seriously threaten public safety. Therefore, it is an urgent social need to carry out technical countermeasures against drones in low-altitude airspace.

[0003] At low altitudes, net guns can be used to entangle the propellers of captured drones to force them to land. Currently, there are two types of net guns, one is launched from the ground, and the other is mounted on drones. On the ground, it is usually held by a person or fixed on the ground with computer-assisted aiming to capture the drone. Due to the effective range of the net gun, its capture range is small. Most of the net guns mounted on drones on the market cannot be adjusted in angle, and the capture range is limited to a fixed angle. After firing, ammunition needs to be replenished, and the net gun needs to be disassembled from the drone.

[0004] Most illegal drones are small drones, which are highly maneuverable and can quickly arrive and launch attacks, and quickly withdraw after use. Due to the small size and high maneuverability of drones, it is difficult to effectively stop and attack illegal drones after they are discovered. Therefore, a method that can quickly counter illegal drones is currently needed. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide a nonlinear motion estimation and trajectory planning method for a security drone, which can capture illegal drones in low-altitude airspace.

[0006] The technical solution adopted by the present invention is:

[0007] A nonlinear motion estimation and trajectory planning method for a security drone, including a nonlinear motion estimation method based on information gain and a trajectory planning method with terminal constraints:

[0008] Terminal constraints include: ① Distance constraint: Illegal drones should fall within the cone-shaped operating range determined by the security drone’s net gun to ensure the success rate of capture, i.e. ||p p -p e ||≤d max ; ② Process constraints: including the saturation constraints of the rotor control force and speed regulation during the security UAV's execution of the maneuvering target capture mission, which are reflected in the kinematic constraints in trajectory planning, including the speed v∈[v min , v max ], acceleration a∈[a min , a max ], jerk j∈[j min , j max ];

[0009] Since there is a highly nonlinear relationship between the airborne sensor observation and the target position, an improved extended Kalman filter based on information gain is used to estimate the position and speed of illegal drones in approaching situations. The nonlinear observation model of the airborne sensor is:

[0010] z k =h(x k )+v k ;

[0011] In order to deal with the intermittent measurement problem of the airborne sensor on the target, an improved Gaussian noise is established.

[0012]

[0013] Among them, σ represents a value approaching infinity, the finite covariance R>0, and the discrete indicator function γ k , is the effective detection area of ​​the airborne sensor, which can be approximately expressed as a convex hull: Ax≤b.

[0014] After the target state is estimated by improving the extended Kalman filter, a two-point boundary value optimization problem is established according to the terminal state constraints of the security drone:

[0015] min J=||v q -v e || 2 +λtr(P k )

[0016] in and Represent the speed of security drones and illegal drones, tr(P k ) represents the covariance matrix P k The constraints in the optimization problem J include the terminal constraints of the security drone and the position constraints of the illegal drone pe ∈S, S represents the effective capture area, for the binary variable γ k Do approximate processing, when the illegal drone position p e Distance p p The closer and The closer to the center axis of the airborne sensor, γ k →0, otherwise γ k →0;

[0017] After obtaining the location information of the illegal drone, the security drone is adjusted to the appropriate position through the algorithm, and the illegal drone is captured using a net gun.

[0018] Furthermore, the security drone includes a drone body, a top plate installed on the drone body through a flat hinge, a net gun arranged on the top plate, and an excitation device for activating the net gun, a push rod is arranged between the top plate and the drone body, and an airborne sensor is arranged in front of the top plate.

[0019] Furthermore, two side panels are relatively arranged on the top of the top panel, a gun slide is fixedly installed on the left and right sides of the net gun, a slide groove matched with the gun slide is arranged on the side panel, the gun slide is installed in the slide groove, and a limiting device is arranged between the gun slide and the slide groove.

[0020] Furthermore, the limiting device includes a ball screw fixedly mounted on the gun slide, and a limiting hole corresponding to the ball screw is arranged on the side wall of the slide groove.

[0021] Furthermore, a guide groove is provided in front of the slide groove.

[0022] Furthermore, the excitation device includes a fixing plate fixedly mounted on the top plate and a servo mounted on the fixing plate, a rocker arm is provided on the output shaft of the servo arm, and the excitation button of the net gun faces and corresponds to the rocker arm.

[0023] Furthermore, the airborne sensor is a binocular camera, and the drone body is a rotary-wing drone.

[0024] The positive effects of the present invention are:

[0025] The present invention models the problem of catching illegal drones at the terminal as a nonlinear motion estimation based on information gain and a trajectory planning problem with terminal constraints and process constraints. The improved extended Kalman filter based on information gain is used to perform state estimation (including position and speed) of illegal drones in approaching situations, thereby enhancing the stability of state estimation. The trajectory planning of the present invention adopts the method of offline state space constraint primitive sampling and online trajectory optimization, so the generated flight trajectory must satisfy the kinematic constraints of the drone itself. After obtaining the position information of the illegal drone, it can be effectively captured and countered. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1 This is the control strategy roadmap for the terminal capture drone of the present invention;

[0027] Figure 2 This is a flow chart of the present invention for capturing a drone;

[0028] Figure 3 This is a schematic diagram of the structure of the security drone of the present invention;

[0029] Figure 4 It is a schematic diagram of the structure of the excitation device of the present invention;

[0030] Figure 5 It is a schematic diagram of the chute structure of the present invention. DETAILED DESCRIPTION

[0031] In order to further explain the technical means and effects adopted by the present invention to achieve the predetermined invention purpose, the specific implementation mode, structure, characteristics and effects of the present invention are described in detail below in combination with the accompanying drawings and preferred embodiments.

[0032] Example 1

[0033] As attached Figure 1 , 2 As shown, the present invention discloses a nonlinear motion estimation and trajectory planning method for a security drone. In the present invention, the terminal non-anti-drone capture problem is modeled as a nonlinear motion estimation based on information gain and a trajectory planning problem with terminal constraints. The terminal constraints include: ① Distance constraint: The illegal drone should fall within the conical operating range determined by the security drone's net gun to ensure the success rate of capture, that is, ||p p -p e ||≤d max ; ② Process constraints: including the saturation constraints of the rotor control force and speed regulation during the security UAV's execution of the maneuvering target capture mission, which are reflected in the kinematic constraints in trajectory planning, including the speed v∈[v min , v max ], acceleration a∈[a min , a max ], jerk j∈[j min , j max ];

[0034] Since there is a highly nonlinear relationship between the airborne sensor observation and the target position, the present invention adopts an improved extended Kalman filter (Extended Kalman Filter, EKF) based on information gain to perform state estimation (including position and speed) of illegal drones in close situations to enhance the stability of state estimation. In the present invention, the nonlinear observation model of the airborne sensor is:

[0035] z k =h(x k )+v k .

[0036] In order to deal with the intermittent measurement problem of the airborne sensor on the target, an improved Gaussian noise is established.

[0037]

[0038] Among them, σ represents a value approaching infinity, the finite covariance R>0, and the discrete indicator function γ k . is the effective detection area of ​​the airborne sensor, which can be approximately expressed as a convex hull: Ax≤b.

[0039] After the target state is estimated by improving the extended Kalman filter, a two-point boundary value optimization problem is established according to the terminal state constraints of the security drone:

[0040] min J=||v q -v e || 2 +λtr(P k )

[0041] in and Represent the speed of security drones and illegal drones, tr(P k ) represents the covariance matrix P k The constraints in the optimization problem J include the terminal constraints of the security drone and the position constraints of the illegal drone p e ∈S, S represents the effective capture area, for the binary variable γ k Do approximate processing, when the illegal drone position p e Distance p p The closer and The closer to the center axis of the airborne sensor, γ k →1, conversely γ k →0(such as Figure 3 The trajectory planning of the present invention adopts the method of offline state space constraint primitive sampling and online trajectory optimization, so the generated flight trajectory must satisfy the kinematic constraints of the UAV itself.

[0042] After obtaining the position information of the illegal drone, the security drone is adjusted to the appropriate position, and then the push rod is adjusted to the appropriate angle. The net gun is fired through the rotation angle of the servo to capture the illegal drone.

[0043] Example 2

[0044] This embodiment discloses a security drone, as shown in the attached Figure 3-5 As shown, the drone includes a drone body 1, a top plate 2, a push rod 3, a side plate 4, a net gun 5, a gun slide 6 and an airborne sensor 7. An excitation device is also provided on the top plate 2 to excite the net gun 5.

[0045] The drone body 1 is a rotary-wing drone, and the top plate 2 is installed on the drone body 1 through a flat hinge. The tail of the top plate 2 is connected and fixed to one side of the flat hinge through two M4 screws, and the other side of the flat hinge can be connected and fixed to the drone body 1. A push rod 3 is arranged between the top plate 2 and the drone body 1. The push rod 3 can be an electric push rod, and its two ends are respectively hinged to the front end of the top plate 2 and the drone body 1. The angle of the top plate 2 is adjusted by retracting the push rod 3, thereby adjusting the angle of the net gun 5, and the airborne sensor 7 is fixedly installed at the front end of the top plate 2.

[0046] The two sides of the net gun 5 are fixed to the gun slide 6 by two M4 screws respectively. There are two side panels 4 and they are arranged opposite to each other. The net gun 5 is installed between the two side panels 4. A slide groove 10 is arranged on the side panel 4. The slide groove 10 and the gun slide 6 are dovetail-shaped and have corresponding sizes. The gun slide 6 can be slidably installed in the slide groove 10, and a limiting device is arranged between the gun slide 6 and the slide groove 10. The limiting device includes a ball screw fixedly installed on the gun slide 6, and a limiting hole 11 corresponding to the ball screw is arranged on the side wall of the slide groove 10. After being installed in place, the ball screw cooperates with the limiting hole 11 to achieve limiting. Preferably, in order to enable the gun slide 6 to be installed in the slide groove 10 more smoothly, a guide groove 12 is arranged at the front end of the slide groove 10, and the position of the guide groove 12 corresponds to the ball screw.

[0047] The excitation device is located below the net gun 5, and includes a fixing plate 13 fixedly mounted on the top plate 2 and a steering gear 8 mounted on the fixing plate 13. A rocker arm 9 is provided on the output shaft of the steering gear 8, and the excitation button of the net gun 5 faces and corresponds to the rocker arm 9. The steering gear 8 can control the rocker arm 9 to rotate through the drone, so that it can touch the excitation button of the net gun 5 connected to the side plate 4 in a locked state through the ball screw, so as to achieve the goal of firing the net gun 5 to spray the net.

[0048] The airborne sensor 7 is a binocular camera, which is used to calculate the position information of the illegal drone.

[0049] The security drone of the present invention has a top plate 2 with adjustable angle, and a net gun 5 is installed on the top plate 2. The angle of the net gun 5 can be adjusted according to actual conditions, so that the capture range is larger. The net gun 5 is detachably connected to the side plate 4 through a gun slide 6, and can be quickly disassembled to replenish ammunition after firing.

[0050] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features thereof may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A nonlinear motion estimation and trajectory planning method for a security drone, characterized in that Includes nonlinear motion estimation based on information gain and trajectory planning methods with terminal constraints: Terminal constraints include: ① Distance constraint: Illegal drones should fall within the cone-shaped operating range determined by the security drone’s net gun to ensure the success rate of capture, i.e. ||p p -p e ||≤d max ; ② Process constraints: including the saturation constraints of the rotor control force and speed regulation during the security UAV's execution of the maneuvering target capture mission, which are reflected in the kinematic constraints in trajectory planning, including the speed v∈[v min , v max ], acceleration a∈[a min ,a max ], jerk j∈[j min ,j max ]; Since there is a nonlinear relationship between the airborne sensor observation and the target position, an improved extended Kalman filter based on information gain is used to estimate the position and speed of illegal UAVs in approaching situations. The nonlinear observation model of the airborne sensor is: z k =h(x k )+v k ; In order to deal with the intermittent measurement problem of the airborne sensor on the target, an improved Gaussian noise is established. Among them, σ represents a value approaching infinity, the finite covariance R>0, and the discrete indicator function γ k , Z k is the effective detection area of ​​the airborne sensor, which can be approximately expressed as a convex hull: Ax≤b; After the target state is estimated by improving the extended Kalman filter, a two-point boundary value optimization problem is established according to the terminal constraints of the security drone: min J=|v q -v e || 2 +λtr(P k ) in and Represent the speed of security drones and illegal drones, tr(P k ) represents the covariance matrix P k The constraints in the optimization problem J include the terminal constraints of the security drone and the position constraints of the illegal drone p e ∈S, S represents the effective capture area, for the binary variable γ k Do approximate processing, when the illegal drone position p e Distance p p The closer and The closer to the center axis of the airborne sensor, γ k →1, conversely γ k →0; After obtaining the location information of the illegal drone, the security drone is adjusted to the appropriate position and the illegal drone is captured using a net gun.

2. The nonlinear motion estimation and trajectory planning method of a security drone according to claim 1 is characterized in that The security drone comprises a drone body (1), a top plate (2) mounted on the drone body (1) via a flat hinge, a net gun (5) arranged on the top plate (2), and an excitation device for exciting the net gun (5), a push rod (3) is arranged between the top plate (2) and the drone body (1), and an airborne sensor (7) is arranged in front of the top plate (2).

3. The nonlinear motion estimation and trajectory planning method of a security drone according to claim 2 is characterized in that Two side plates (4) are arranged opposite to each other on the top of the top plate (2); a gun slide (6) is fixedly installed on the left and right sides of the net gun (5); a slide groove (10) adapted to the gun slide (6) is arranged on the side plate (4); the gun slide (6) is installed in the slide groove (10); and a limiting device is arranged between the gun slide (6) and the slide groove (10).

4. The nonlinear motion estimation and trajectory planning method of a security drone according to claim 3 is characterized in that The limiting device comprises a ball screw fixedly mounted on the gun slide (6), and a limiting hole (11) corresponding to the ball screw is arranged on the side wall of the slide groove (10).

5. The nonlinear motion estimation and trajectory planning method of a security drone according to claim 4 is characterized in that A guide groove (12) is provided in front of the slide groove (10).

6. The nonlinear motion estimation and trajectory planning method of a security drone according to claim 2 is characterized in that The excitation device comprises a fixing plate (13) fixedly mounted on the top plate (2) and a steering gear (8) mounted on the fixing plate (13); a rocker arm (9) is provided on the output shaft of the steering gear (8); and an excitation button of the net gun (5) faces and corresponds to the rocker arm (9).

7. The nonlinear motion estimation and trajectory planning method of a security drone according to claim 2 is characterized in that The airborne sensor (7) is a binocular camera, and the drone body (1) is a rotary-wing drone.

Images