Unmanned aerial vehicle obstacle avoidance flight control method based on relative motion state

Abstract

The invention discloses an unmanned aerial vehicle obstacle avoidance flight control method based on a relative motion state, and relates to the technical field of unmanned aerial vehicle flight control, and the method comprises the following steps: S1, constructing a three-dimensional speed obstacle space according to the position and speed of the unmanned aerial vehicle and the position and speed of an obstacle; s2, formulating an obstacle avoidance strategy and solving an optimization speed; and S3, realizing an obstacle avoidance strategy. According to the unmanned aerial vehicle obstacle avoidance flight control method based on the relative motion state, a two-dimensional relative speed obstacle avoidance method is expanded to a three-dimensional method, and the algorithm is closer to the actual application scene of the unmanned aerial vehicle; the environment priori information is not a necessary condition, and the optimal speed capable of avoiding the movement obstacle while the flight task is completed can be obtained only through the starting point position, the final point position and the obstacle movement state information in the current view field.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle flight control, and in particular to an unmanned aerial vehicle obstacle avoidance flight control method based on relative motion state. Background Art

[0002] As a flexible flying platform, drones have a wide range of application scenarios: in the civilian field, drones have been used on a large scale in emergency rescue, remote sensing and earth observation, light shows and other fields, and there is still huge room for development in the future.

[0003] In complex environments such as disaster areas, obstacles are dense, the drone's line of sight is easily blocked, and there are a large number of emerging threats with fast movement speeds, such as falling building material fragments and dense rubble piles. Therefore, in order to ensure that obstacles are avoided while shortening the mission execution time as much as possible, scholars have conducted a considerable amount of research. The most representative method is the Reciprocal Velocity Obstacle (RVO) method.

[0004] Defects of the prior art:

[0005] 1. Most existing methods require relatively complete prior environmental information. When the environmental scene changes in real time, the planned movement cannot be adjusted in time, which makes the drone easily shot down by sudden obstacles in actual application scenarios such as battlefields and ruins.

[0006] 2. The flight strategy is difficult to achieve the optimal trade-off between avoiding threats and reaching the mission destination. There is a high probability of crashing due to collision, or the flight strategy deviates significantly from the mission destination in order to avoid obstacles, and the flight takes too long to complete the mission.

[0007] 3. The response speed of the control algorithm of the existing open source flight control is significantly slower than that of the motion planning method, which makes it impossible to ensure a safe distance between the drone and obstacles, and the obstacle avoidance action is not smooth, and it is easy to need to perform large speed changes in a short period of time. Summary of the invention

[0008] The purpose of the present invention is to provide a UAV obstacle avoidance flight control method based on relative motion state, to solve the problems raised in the above background technology, to expand the two-dimensional relative speed obstacle avoidance method to three dimensions, and the algorithm is closer to the actual application scenario of the UAV.

[0009] To achieve the above object, the present invention provides a method for controlling an obstacle avoidance flight of a UAV based on a relative motion state, comprising the following steps:

[0010] Step S1, constructing a three-dimensional speed obstacle space according to its own position and speed and the position and speed of obstacles;

[0011] Step S2, formulate an obstacle avoidance strategy and solve the optimal speed;

[0012] Step S3: Implement obstacle avoidance strategy.

[0013] Preferably, the method for constructing the three-dimensional speed obstacle space in step S1 specifically includes: constructing a speed coordinate system, setting the position vector of the center of the drone itself as p in the speed coordinate system q =[x q y q z q ], the velocity vector is The collision radius is r q , the center position of the obstacle is o ob =[x ob y ob z ob ], the speed is The collision radius is r ob , the time margin for the drone to avoid obstacles is τ;

[0014] Then in the velocity coordinate system, the drone is at point The obstacle is located at the point By point is the center of the ball, The radius is used to make a sphere, and then a circumscribed conical surface is made through the origin. The part of the sphere protruding from the conical surface and the shadow area enclosed by the conical surface are the three-dimensional speed obstacle space 3D-VO Q|B .

[0015] Preferably, the velocity coordinate system is a three-dimensional Cartesian coordinate system, the origin represents the three-axis velocity (0, 0, 0), and the three coordinate axes represent the components of the velocity vector on the x, y, and z coordinate axes.

[0016] Preferably, the specific steps of step S2 include:

[0017] Step S21, using a determination method to determine whether there is a collision risk after maintaining the current relative motion state for a period of time τ;

[0018] Step S22, completing the solution of the optimization speed.

[0019] Preferably, the determination method in step S21 includes:

[0020] Calculate the obstacle relative to the speed v of the drone ob -v q , this relative velocity corresponds to the point in the velocity coordinate system If point J is located in the speed obstacle space VO Q|BIf the UAV continues to move along the line connecting the current position and the mission endpoint, otherwise, a three-dimensional obstacle avoidance auxiliary space 3D-ORCA is constructed. Q|B , and select the appropriate speed based on this.

[0021] Preferably, the specific operations of step S21 include:

[0022] Solve the 3D-VO Q|B The vector u with the shortest modulus formed by a boundary point and point J:

[0023]

[0024] Among them, u=[u x u y u z ] represents the minimum speed change required to avoid a collision;

[0025] From this we get the velocity u+v in the velocity coordinate system q The corresponding point Draw a plane through point Q' with u as the normal vector, which divides the 3D_ORCA Q|B .

[0026] Preferably, step S22 specifically includes:

[0027] Let the preferred speed be v pref , v pref It is proportional to the direction vector g connecting the drone and the mission endpoint. Let the proportional coefficient be k, then:

[0028]

[0029] Finally, in 3D_ORCA Q|B Internal selection point Q new , let Q new The corresponding velocity vector and v pref The modulus of the difference between the two reaches the minimum value, thus solving the optimal speed v of the drone new :

[0030]

[0031] Continuing with the above formula, we can find v in a multi-obstacle environment. new .

[0032] Preferably, step S3 specifically comprises: obtaining v new As the input of the control system, the control system adopts an improved incremental nonlinear dynamic inverse control method to output the motor speed signal to complete the implementation of the obstacle avoidance strategy.

[0033] Therefore, the present invention adopts the above-mentioned unmanned aerial vehicle obstacle avoidance flight control method based on relative motion state, which has the following beneficial effects:

[0034] (1) The two-dimensional relative speed obstacle avoidance method is expanded to three dimensions, and the algorithm is closer to the actual application scenarios of drones.

[0035] (2) Environmental prior information is not a necessary condition. Only the starting and ending positions and the obstacle motion state information in the current field of view are needed to obtain the optimal speed that can complete the flight mission while avoiding moving obstacles.

[0036] (3) The control system responds faster and has higher control accuracy, which effectively alleviates the problems of reduced flight safety, poor continuity of the aircraft's obstacle avoidance movement, and sudden speed changes caused by the traditional method's untimely response.

[0037] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 A schematic diagram of a three-dimensional speed obstacle space of an embodiment of a UAV obstacle avoidance flight control method based on relative motion state of the present invention;

[0039] Figure 2 A schematic diagram of a three-dimensional obstacle avoidance auxiliary space of an embodiment of a UAV obstacle avoidance flight control method based on relative motion state of the present invention;

[0040] Figure 3 It is a multi-obstacle three-dimensional obstacle avoidance auxiliary space of an embodiment of a UAV obstacle avoidance flight control method based on relative motion state of the present invention;

[0041] Figure 4 A control system block diagram of an embodiment of a method for controlling an obstacle avoidance flight of a UAV based on a relative motion state according to the present invention;

[0042] Figure 5 This is a diagram of the internal structure of a UAV frame according to an embodiment of the present invention of a UAV obstacle avoidance flight control method based on relative motion state. DETAILED DESCRIPTION

[0043] The technical solution of the present invention is further described below through the accompanying drawings and embodiments.

[0044] Unless otherwise defined, the technical terms or scientific terms used in the present invention should be understood by people with ordinary skills in the field to which the present invention belongs. The words "first", "second" and similar words used in the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. "Include" or "comprise" and similar words mean that the elements or objects appearing before the word include the elements or objects listed after the word and their equivalents, without excluding other elements or objects. "Connect" or "connected" and similar words are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "Up", "down", "left", "right" and the like are only used to indicate relative positional relationships. When the absolute position of the described object changes, the relative positional relationship may also change accordingly.

[0045] Example

[0046] See also Figure 1-5 The present invention provides a method for controlling an unmanned aerial vehicle's obstacle avoidance flight based on a relative motion state, comprising the following steps:

[0047] Step S1: construct a three-dimensional speed obstacle space according to its own position and speed as well as the position and speed of the obstacle. The construction method is as follows:

[0048] In the velocity coordinate system, let the position vector of the drone's own center be p q =x q y q z q ], the velocity vector is The collision radius is r q , the center position of the obstacle is p ob =[x ob y ob z ob ], the speed is The collision radius is r ob , the time margin of the UAV to avoid obstacles is τ, then the three-dimensional speed obstacle space 3D_VO marked by the shadow can be obtained Q|B ;

[0049] like Figure 1 As shown in the figure, the coordinate system is a three-dimensional Cartesian coordinate system. The origin represents the three-axis velocity (0, 0, 0), and the three coordinate axes represent the components of the velocity vector on the x, y, and z axes. This coordinate system is hereinafter referred to as the velocity coordinate system. In this velocity coordinate system, the drone is located at point The obstacle is located at the point By point is the center of the ball, The radius is used to make a sphere, and then a circumscribed cone is made through the origin. The part of the sphere protruding from the cone and the shadow area enclosed by the cone are the three-dimensional speed obstacle space 3D-VO. Q|B .

[0050] Step S2: formulate an obstacle avoidance strategy and solve the optimal speed. The ultimate goal of this strategy is to minimize the change in the speed of the drone and the offset between the drone and the end point. Determine whether there is a risk of collision after the current relative motion state is maintained for a period of time τ. The determination method is to calculate the speed v of the obstacle relative to the drone. ob -v q , this relative velocity corresponds to the point in the velocity coordinate system If point J is located in the speed obstacle space VO Q|B If the UAV continues to move along the line connecting the current position and the mission endpoint, otherwise, a three-dimensional obstacle avoidance auxiliary space 3D-ORCA is constructed. Q|B , and select the appropriate speed based on this. The specific construction method is as follows Figure 2 As shown:

[0051] Step S21: First, solve the three-dimensional speed obstacle space 3D-VO Q|B The vector u with the shortest modulus formed by a boundary point and point J:

[0052]

[0053] Among them, u=[u x u y u z ] represents the minimum velocity change required to avoid a collision.

[0054] From this we get the velocity u+v in the velocity coordinate system q The corresponding point Draw a plane through point Q' with u as the normal vector, which divides the 3D_ORCA Q|B .

[0055] Step S22, complete the solution of the optimized speed. First, introduce the concept of physical quantity "preferred speed", which is a vector. And the preferred speed is recorded as v pref , v pref It is proportional to the direction vector g connecting the drone and the mission endpoint. Let the proportional coefficient be k, then:

[0056]

[0057] Finally, in 3D_ORCA Q|B Internal selection point Q new , let Q new The corresponding velocity vector and vpref The modulus of the difference is as small as possible, so as to solve the optimal speed v of the drone new :

[0058]

[0059] When there are multiple obstacles B1, B2, ..., B N When , the three-dimensional obstacle avoidance auxiliary space is obtained for each obstacle using the same method The final auxiliary space is the intersection of these independent spaces, such as Figure 3 As shown, that is:

[0060]

[0061]

[0062] Continuing with the above formula (3), we can find v in a multi-obstacle environment: new .

[0063] Step S3: Implement obstacle avoidance strategy. new As the input of the control system, the control system adopts the improved incremental nonlinear dynamic inverse control method to output the motor speed signal and complete the implementation of the obstacle avoidance strategy. The control system block diagram is shown in Figure 4 As shown:

[0064] Among them, A(z) represents the dynamic model of the actuator propeller, H(z) is a second-order filter, ω is the vector of motor speed, Ω is the attitude angular velocity of the drone, d is G1, G2 is the coefficient matrix:

[0065]

[0066] Where l is the length of the drone, b is the width of the drone, k1 and k2 are constants, and I v is the inertia matrix of the UAV, I r is the inertia matrix of the UAV rotor, T s is the sampling period of the controller.

[0067] The specific structure of the module with the word "UAV" is as follows Figure 5 shown.

[0068] Thus, a fast and accurate control system design was completed.

[0069] Therefore, the present invention adopts the above-mentioned UAV obstacle avoidance flight control method based on relative motion state, expands the two-dimensional relative speed obstacle avoidance method to three dimensions, and the algorithm is closer to the actual application scenario of the UAV; environmental prior information is not a necessary condition, only the starting point, end point position and obstacle motion state information in the current field of view are needed to obtain the optimal speed that can avoid motion obstacles while completing the flight mission.

[0070] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solution of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solution to deviate from the spirit and scope of the technical solution of the present invention.

Claims

1. A method for controlling an unmanned aerial vehicle's obstacle avoidance flight based on relative motion state, characterized in that: The following steps are involved: Step S1, constructing a three-dimensional speed obstacle space according to its own position and speed and the position and speed of obstacles; Step S2, formulate an obstacle avoidance strategy and solve the optimal speed; Step S3: Implement obstacle avoidance strategy.

2. The method for controlling an unmanned aerial vehicle's obstacle avoidance flight based on relative motion state according to claim 1, characterized in that: The method for constructing the three-dimensional speed obstacle space in step S1 specifically includes: constructing a speed coordinate system, in which the position vector of the center of the drone itself is set as p q =[x q y q z q ], the velocity vector is The collision radius is r q , the center position of the obstacle is p ob =[x ob y ob z ob ], the speed is The collision radius is r ob , the time margin for the drone to avoid obstacles is τ; Then in the velocity coordinate system, the drone is at point Q(v qx v qy v qz ), the obstacle is located at point By point is the center of the ball, The radius is used to make a sphere, and then a circumscribed conical surface is made through the origin. The part of the sphere protruding from the conical surface and the shadow area enclosed by the conical surface are the three-dimensional speed obstacle space 3D-VO Q|B .

3. The method for controlling an unmanned aerial vehicle's obstacle avoidance flight based on relative motion state according to claim 2, characterized in that: The velocity coordinate system is a three-dimensional Cartesian coordinate system, where the origin represents the three-axis velocity (0, 0, 0), and the three coordinate axes represent the components of the velocity vector on the x, y, and z coordinate axes.

4. The method for controlling an unmanned aerial vehicle's obstacle avoidance flight based on relative motion state according to claim 3, characterized in that: The specific steps of step S2 include: Step S21, using a determination method to determine whether there is a collision risk after maintaining the current relative motion state for a period of time τ; Step S22, completing the solution of the optimization speed.

5. The method for controlling an unmanned aerial vehicle's obstacle avoidance flight based on relative motion state according to claim 4, characterized in that: The determination method in step S21 includes: Calculate the speed v of the obstacle relative to the drone ob -v q , this relative velocity corresponds to the point in the velocity coordinate system If point J is located in the speed obstacle space VO Q|B If the UAV continues to move along the line connecting the current position and the mission endpoint, otherwise, a three-dimensional obstacle avoidance auxiliary space 3D-ORCA is constructed. Q|B , and select the appropriate speed based on this.

6. The method for controlling an unmanned aerial vehicle's obstacle avoidance flight based on relative motion state according to claim 5, characterized in that: The specific operations of step S21 include: Solve the 3D-VO Q|B The vector u with the shortest modulus formed by a boundary point and point J: Among them, u=[u x u y u z ] represents the minimum speed change required to avoid a collision; From this we get the velocity u+v in the velocity coordinate system q The corresponding point Draw a plane through point Q' with u as the normal vector, which divides the 3D_ORCA Q|B .

7. The method for controlling an unmanned aerial vehicle's obstacle avoidance flight based on relative motion state according to claim 6, characterized in that: Step S22 specifically includes: Let the preferred speed be v pref , v pref It is proportional to the direction vector g connecting the drone and the mission endpoint. Let the proportional coefficient be k, then: Finally, in 3D_ORCA Q|B Internal selection point Q new , let Q new The corresponding velocity vector and v pref The modulus of the difference between the two reaches the minimum value, thus solving the optimal speed v of the drone new : Continuing with the above formula, we can find v in a multi-obstacle environment. new .

8. The method for controlling an unmanned aerial vehicle's obstacle avoidance flight based on relative motion state according to claim 7, characterized in that: Step S3 specifically includes: new As the input of the control system, the control system adopts an improved incremental nonlinear dynamic inverse control method to output the motor speed signal to complete the implementation of the obstacle avoidance strategy.

Images