Method, System, Electronic Device and Storage Medium for Determining UAV Flight Trajectory
By establishing the coordinate system of obstacles and target areas, determining the relative coordinates and calculating the robustness score, the problem of low robustness of the drone's flight trajectory is solved, and accurate determination and intelligent evaluation of the drone's flight trajectory are achieved.
Patent Information
- Application Number
- CN202310508493.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-05-08
- Publication Date
- 2025-08-05
- Estimated Expiration
- 2043-05-08
AI Technical Summary
In the prior art, the preset flight trajectory of drones is relatively robust and lacks standard intelligent performance evaluation methods.
By obtaining the coordinates of the position points at the ground at each moment of the drone's multiple preset flight trajectories, establishing the coordinate system of obstacles and target areas, determining the relative coordinates of each vertex, and using the coordinate transfer matrix to calculate the robustness score, selecting the trajectory with the highest score as the target flight trajectory.
The determination of the drone's flight trajectory is achieved, improving the robustness of the flight trajectory and the accuracy of intelligent performance evaluation.
Smart Images
Figure CN116382347B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of unmanned aerial vehicle (UAV) trajectory planning, and particularly to a method, a system, an electronic device, and a storage medium for determining a flight trajectory of a UAV. Background Art
[0002] UAVs have advantages such as being flexible, easy to operate, and having low research and development costs, making them play an increasingly crucial role in both military and civilian fields. Research on UAV performance evaluation methods can clarify relevant technical indicators in this field, thereby guiding the rapid development of UAV technology. However, in the field of UAVs, there is still no relatively standard intelligent performance evaluation method for UAVs, which leads to the problem of low robustness in the preset flight trajectories of UAVs. Summary of the Invention
[0003] The purpose of the present invention is to provide a method, a system, an electronic device, and a storage medium for determining a flight trajectory of a UAV, which realizes the determination of the flight trajectory of a UAV.
[0004] To achieve the above purpose, the present invention provides the following solutions:
[0005] A method for determining a flight trajectory of a UAV includes:
[0006] Obtaining the ground coordinates of the position points of each moment of multiple preset flight trajectories of the UAV;
[0007] Determining the ground coordinates of each vertex of an obstacle and the ground coordinates of each vertex of a target area; the ground coordinate of a point is the coordinate of the point in the ground coordinate system; the point includes a vertex and a position point; the ground coordinate system is constituted based on the take-off point of the UAV as the origin, the geographic north pole as the x-axis, the direction vertically downward to the ground as the z-axis, and the y-axis in the horizontal plane determined by the right-hand rule; the starting point of all the preset flight trajectories is the take-off point;
[0008] Establishing an obstacle coordinate system based on the ground coordinates of each vertex of the obstacle and determining the farthest vertex of the obstacle;
[0009] Establishing a target area coordinate system based on the ground coordinates of each vertex of the target area and determining the farthest vertex of the target area;
[0010] Determining the first relative coordinates of each vertex of the obstacle and the second relative coordinates of each vertex of the target area; the first relative coordinate of a point is the coordinate of the point in the obstacle coordinate system, and the second relative coordinate of a point is the coordinate of the point in the target area coordinate system;
[0011] For any current preset flight trajectory:
[0012] Using the coordinate transformation matrix, determine the first relative coordinates and the second relative coordinates of the position points at each moment of the current preset flight trajectory;
[0013] Based on the first relative coordinates of the farthest vertex of the obstacle, the origin coordinates of the obstacle coordinate system, the second relative coordinates of the farthest vertex of the target area, the origin coordinates of the target area coordinate system, and the first relative coordinates and the second relative coordinates of the position points at each moment of the current preset flight trajectory, calculate the robustness score of the current preset flight trajectory;
[0014] Determine the target flight trajectory of the drone as the preset flight trajectory with the highest robustness score among all the preset flight trajectories.
[0015] Optionally, establish an obstacle coordinate system based on the ground coordinates of each vertex of the obstacle, specifically including:
[0016] Take the vertex of the obstacle closest to the take-off point as the origin of the obstacle coordinate system, and establish the obstacle coordinate system.
[0017] Optionally, establish a target area coordinate system based on the ground coordinates of each vertex of the target area, specifically including:
[0018] Take the vertex of the target area closest to the take-off point as the origin of the target area coordinate system, and establish the target area coordinate system.
[0019] Optionally, based on the first relative coordinates of the farthest vertex of the obstacle, the origin coordinates of the obstacle coordinate system, the second relative coordinates of the farthest vertex of the target area, the origin coordinates of the target area coordinate system, and the first relative coordinates and the second relative coordinates of the position points at each moment of the current preset flight trajectory, calculate the robustness score of the current preset flight trajectory, specifically including:
[0020] Based on the first relative coordinates of the farthest vertex of the obstacle, the origin coordinates of the obstacle coordinate system, and the first relative coordinates of the position points at each moment of the current preset flight trajectory, calculate the first robustness of the current preset flight trajectory;
[0021] Based on the second relative coordinates of the farthest vertex of the target area, the origin coordinates of the target area coordinate system, and the second relative coordinates of the position points at each moment of the current preset flight trajectory, calculate the second robustness of the current preset flight trajectory;
[0022] Based on the first robustness and the second robustness, determine the robustness score.
[0023] Optionally, calculate the first robustness of the current preset flight trajectory based on the first relative coordinates of the farthest vertex of the obstacle, the origin coordinates of the obstacle coordinate system, and the first relative coordinates of the position points at each moment of the current preset flight trajectory. Specifically, it includes:
[0024] For the position point at any moment:
[0025] Calculate the difference between the first component of the first relative coordinates of the position point and the first component of the origin coordinates of the obstacle coordinate system, calculate the difference between the second component of the first relative coordinates of the position point and the second component of the origin coordinates of the obstacle coordinate system, and calculate the difference between the third component of the first relative coordinates of the position point and the third component of the origin coordinates of the obstacle coordinate system; the first component is the component of the coordinate along the x-axis of the obstacle coordinate system, the second component is the component of the coordinate along the y-axis of the obstacle coordinate system, and the third component is the component of the coordinate along the z-axis of the obstacle coordinate system;
[0026] Calculate the difference between the first component of the first relative coordinates of the farthest vertex of the obstacle and the first component of the first relative coordinates of the position point, calculate the difference between the second component of the first relative coordinates of the farthest vertex of the obstacle and the second component of the first relative coordinates of the position point, and calculate the difference between the third component of the first relative coordinates of the farthest vertex of the obstacle and the third component of the first relative coordinates of the position point;
[0027] Determine the smallest difference value among all the differences of the position points at all moments as the first robustness.
[0028] Optionally, calculate the second robustness of the current preset flight trajectory based on the second relative coordinates of the farthest vertex of the target area, the origin coordinates of the target area coordinate system, and the second relative coordinates of the position points at each moment of the current preset flight trajectory. Specifically, it includes:
[0029] For the position point at any moment:
[0030] Calculate the difference between the fourth component of the origin coordinates of the target area coordinate system and the fourth component of the second relative coordinates of the position point, calculate the difference between the fifth component of the origin coordinates of the target area coordinate system and the fifth component of the second relative coordinates of the position point, and calculate the difference between the sixth component of the origin coordinates of the target area coordinate system and the sixth component of the second relative coordinates of the position point;
[0031] Calculate the difference between the fourth component of the second relative coordinate of the position point and the fourth component of the second relative coordinate of the farthest vertex of the target area, calculate the difference between the fifth component of the second relative coordinate of the position point and the second relative coordinate of the farthest vertex of the target area, and calculate the difference between the sixth component of the second relative coordinate of the position point and the sixth component of the second relative coordinate of the farthest vertex of the target area; the fourth component is the component of the coordinate along the x-axis of the target area coordinate system, the fifth component is the component of the coordinate along the y-axis of the target area coordinate system, and the sixth component is the component of the coordinate along the z-axis of the target area coordinate system;
[0032] Determine the smallest difference value among all the differences of the position points at all times as the second robustness.
[0033] Optionally, determining the robustness score based on the first robustness and the second robustness includes:
[0034] Judge whether the first robustness is greater than the second robustness;
[0035] If so, determine the second robustness as the robustness score;
[0036] If not, determine the first robustness as the robustness score.
[0037] A system for determining an unmanned aerial vehicle flight trajectory includes:
[0038] A first ground coordinate determination module, configured to obtain the ground coordinates of the position points at each moment of multiple preset flight trajectories of the unmanned aerial vehicle;
[0039] A second ground coordinate determination module, configured to determine the ground coordinates of each vertex of the obstacle and the ground coordinates of each vertex of the target area; the ground coordinate of a point is the coordinate of the point in the ground coordinate system; a point includes a vertex and a position point; the ground coordinate system is composed of the take-off point of the unmanned aerial vehicle as the origin, the geographic north pole as the x-axis, the vertical downward direction of the ground as the z-axis, and the y-axis in the horizontal plane determined by the right-hand rule; the starting points of all the preset flight trajectories are the take-off point;
[0040] An obstacle coordinate system establishment module, configured to establish an obstacle coordinate system based on the ground coordinates of each vertex of the obstacle and determine the farthest vertex of the obstacle;
[0041] A target area coordinate system establishment module, configured to establish a target area coordinate system based on the ground coordinates of each vertex of the target area and determine the farthest vertex of the target area;
[0042] A relative coordinate determination module, configured to determine the first relative coordinates of each vertex of the obstacle and the second relative coordinates of each vertex of the target area; the first relative coordinate of a point is the coordinate of the point in the obstacle coordinate system, and the second relative coordinate of a point is the coordinate of the point in the target area coordinate system;
[0043] A robustness score calculation module, configured to, for any current preset flight trajectory:
[0044] Use a coordinate transformation matrix to determine the first relative coordinates and the second relative coordinates of the position points at each moment of the current preset flight trajectory;
[0045] Based on the first relative coordinate of the farthest vertex of the obstacle, the origin coordinate of the obstacle coordinate system, the second relative coordinate of the farthest vertex of the target area, the origin coordinate of the target area coordinate system, and the first relative coordinates and the second relative coordinates of the position points at each moment of the current preset flight trajectory, calculate the robustness score of the current preset flight trajectory;
[0046] A target flight trajectory determination module, configured to determine the preset flight trajectory with the highest robustness score among all the preset flight trajectories as the target flight trajectory of the unmanned aerial vehicle.
[0047] An electronic device, comprising:
[0048] One or more processors;
[0049] A storage device, on which one or more programs are stored;
[0050] When the one or more programs are executed by the one or more processors, the one or more processors are caused to implement the method for determining the flight trajectory of the unmanned aerial vehicle as described above.
[0051] A storage medium, on which a computer program is stored, wherein when the computer program is executed by a processor, the method for determining the flight trajectory of the unmanned aerial vehicle as described above is implemented.
[0052] According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:
[0053] The present invention discloses a method, system, electronic device and storage medium for determining the flight trajectory of an unmanned aerial vehicle (UAV). First, the coordinates of the position points of each moment of multiple preset flight trajectories of the UAV, the vertices of each obstacle, and the vertices of each target area in the ground coordinate system are obtained, an obstacle coordinate system and a target area coordinate system are established, and the farthest vertices of the obstacle and the target area are determined. Secondly, the first relative coordinates of the vertices of the obstacle in the obstacle coordinate system and the second relative coordinates of the vertices of the target area in the target area coordinate system are determined. Thirdly, using the coordinate transformation matrix, the first relative coordinates and the second relative coordinates of the position points of each moment of the current preset flight trajectory are determined, and based on the first relative coordinates of the farthest vertex of the obstacle, the origin coordinates of the obstacle coordinate system, the second relative coordinates of the farthest vertex of the target area, the origin coordinates of the target area coordinate system, and the first relative coordinates and the second relative coordinates of the position points of each moment of the current preset flight trajectory, the robustness score of the current preset flight trajectory is calculated. Finally, the preset flight trajectory with the highest robustness score is determined as the target flight trajectory of the UAV, thereby realizing the determination of the flight trajectory of the UAV. BRIEF DESCRIPTION OF THE DRAWINGS
[0054] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.
[0055] Figure 1 Schematic flowchart of the method, system, electronic device and storage medium for determining the flight trajectory of the UAV provided in Embodiment 1 of the present invention;
[0056] Figure 2 Schematic diagram of the coordinate transformation matrix;
[0057] Figure 3 Schematic diagram of the flight trajectories of three UAVs. DETAILED DESCRIPTION OF THE EMBODIMENTS
[0058] The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the drawings in the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, rather than all embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts belong to the scope of protection of the present invention.
[0059] The object of the present invention is to provide a method, system, electronic device and storage medium for determining the flight trajectory of a UAV, aiming to realize the determination of the flight trajectory of the UAV.
[0060] To make the above objects, features, and advantages of the present invention more obvious and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.
[0061] Embodiment 1
[0062] Figure 1 It is a schematic flowchart of the method for determining the flight trajectory of the unmanned aerial vehicle provided in Embodiment 1 of the present invention. As Figure 1 shown, the method for determining the flight trajectory of the unmanned aerial vehicle in this embodiment includes:
[0063] Step 101: Obtain the ground coordinates of the position points of each moment of multiple preset flight trajectories of the unmanned aerial vehicle.
[0064] Step 102: Determine the ground coordinates of each vertex of the obstacle and the ground coordinates of each vertex of the target area.
[0065] Among them, the ground coordinate of a point is the coordinate of the point in the ground coordinate system; the point includes vertices and position points; the ground coordinate system is composed of the take-off point of the unmanned aerial vehicle as the origin, the geographic north pole as the x-axis, the vertical downward direction of the ground as the z-axis, and the y-axis in the horizontal plane determined by the right-hand rule; the starting points of all preset flight trajectories are the take-off point.
[0066] Step 103: Establish an obstacle coordinate system based on the ground coordinates of each vertex of the obstacle and determine the farthest vertex of the obstacle.
[0067] Step 104: Establish a target area coordinate system based on the ground coordinates of each vertex of the target area and determine the farthest vertex of the target area.
[0068] Step 105: Determine the first relative coordinates of each vertex of the obstacle and the second relative coordinates of each vertex of the target area.
[0069] Among them, the first relative coordinate of a point is the coordinate of the point in the obstacle coordinate system, and the second relative coordinate of a point is the coordinate of the point in the target area coordinate system.
[0070] Step 106: Determine the robustness scores of all preset flight trajectories.
[0071] Step 106 specifically includes:
[0072] For any current preset flight trajectory:
[0073] Step 1061: Use the coordinate transformation matrix to determine the first relative coordinates and the second relative coordinates of the position points of each moment of the current preset flight trajectory.
[0074] Step 1062: Calculate the robustness score of the current preset flight trajectory based on the first relative coordinates of the farthest vertex of the obstacle, the origin coordinates of the obstacle coordinate system, the second relative coordinates of the farthest vertex of the target area, the origin coordinates of the target area coordinate system, and the first and second relative coordinates of the position points at each moment of the current preset flight trajectory.
[0075] Step 107: Determine the target flight trajectory of the UAV as the preset flight trajectory with the highest robustness score among all preset flight trajectories.
[0076] As an optional implementation manner, establishing an obstacle coordinate system based on the ground coordinates of each vertex of the obstacle specifically includes:
[0077] Take the vertex of the obstacle closest to the take-off point as the origin of the obstacle coordinate system and establish the obstacle coordinate system.
[0078] As an optional implementation manner, establishing a target area coordinate system based on the ground coordinates of each vertex of the target area specifically includes:
[0079] Take the vertex of the target area closest to the take-off point as the origin of the target area coordinate system and establish the target area coordinate system.
[0080] As an optional implementation manner, Step 1062 specifically includes:
[0081] Step 10621: Calculate the first robustness of the current preset flight trajectory based on the first relative coordinates of the farthest vertex of the obstacle, the origin coordinates of the obstacle coordinate system, and the first relative coordinates of the position points at each moment of the current preset flight trajectory.
[0082] Step 10622: Calculate the second robustness of the current preset flight trajectory based on the second relative coordinates of the farthest vertex of the target area, the origin coordinates of the target area coordinate system, and the second relative coordinates of the position points at each moment of the current preset flight trajectory.
[0083] Step 10623: Determine the robustness score based on the first robustness and the second robustness.
[0084] As an optional implementation manner, Step 10621 specifically includes:
[0085] For the position point at any moment:
[0086] Calculate the difference between the first component of the first relative coordinate of the position point and the first component of the origin coordinate of the obstacle coordinate system, calculate the difference between the second component of the first relative coordinate of the position point and the second component of the origin coordinate of the obstacle coordinate system, and calculate the difference between the third component of the first relative coordinate of the position point and the third component of the origin coordinate of the obstacle coordinate system; the first component is the component of the coordinate along the x-axis of the obstacle coordinate system, the second component is the component of the coordinate along the y-axis of the obstacle coordinate system, and the third component is the component of the coordinate along the z-axis of the obstacle coordinate system.
[0087] Calculate the difference between the first component of the first relative coordinate of the farthest vertex of the obstacle and the first component of the first relative coordinate of the position point, calculate the difference between the second component of the first relative coordinate of the farthest vertex of the obstacle and the second component of the first relative coordinate of the position point, and calculate the difference between the third component of the first relative coordinate of the farthest vertex of the obstacle and the third component of the first relative coordinate of the position point.
[0088] Determine the smallest difference value among all the differences of the position points at all times as the first robustness.
[0089] As an optional implementation manner, step 10622 specifically includes:
[0090] For the position point at any moment:
[0091] Calculate the difference between the fourth component of the origin coordinate of the target area coordinate system and the fourth component of the second relative coordinate of the position point, calculate the difference between the fifth component of the origin coordinate of the target area coordinate system and the fifth component of the second relative coordinate of the position point, and calculate the difference between the sixth component of the origin coordinate of the target area coordinate system and the sixth component of the second relative coordinate of the position point.
[0092] Calculate the difference between the fourth component of the second relative coordinate of the position point and the fourth component of the second relative coordinate of the farthest vertex of the target area, calculate the difference between the fifth component of the second relative coordinate of the position point and the second relative coordinate of the farthest vertex of the target area, and calculate the difference between the sixth component of the second relative coordinate of the position point and the sixth component of the second relative coordinate of the farthest vertex of the target area; the fourth component is the component of the coordinate along the x-axis of the target area coordinate system, the fifth component is the component of the coordinate along the y-axis of the target area coordinate system, and the sixth component is the component of the coordinate along the z-axis of the target area coordinate system.
[0093] Determine the smallest difference value among all the differences of the position points at all times as the second robustness.
[0094] As an optional implementation manner, step 10623 includes:
[0095] Judge whether the first robustness is greater than the second robustness.
[0096] If so, determine the second robustness as the robustness score.
[0097] If not, determine the first robustness as the robustness score.
[0098] Specific embodiment: In actual operation, signal temporal logic can be introduced to calculate the robustness score of the UAV flight trajectory. Taking a quadrotor UAV avoiding static obstacles as an example, the above method will be described below.
[0099] The task requirements in this specific embodiment are as follows: The quadrotor UAV starts from the starting point and flies in the time interval t ∈ [0, T]. During this process, the UAV always avoids the cuboid obstacle B and is always located within the cube target area A in t ∈ [a, T]. This task requirement is described by the Signal Temporal Logic (STL) formula as:
[0100]
[0101] Where i ∈ {A, B}, a and T are time variables, 0 < T, l is the lower bound, and u is the upper bound. F [a,T] φ A means that the UAV is within the cube target area A in t ∈ [a, T], means that the UAV avoids the cuboid obstacle B within the time [0, T].
[0102] Step 1: Establish a ground coordinate system with the origin O e as the UAV take-off location. The x-axis (O e x e axis) of the ground coordinate system points to the geographic north pole, the z-axis (O e z e axis) is perpendicular to the ground and points downward, and the y-axis (O e y e axis) is in the horizontal plane and is determined by the right-hand rule. Collect the path conditions of each UAV within the specified time and obtain all position vectors (x t , y t , z t ) of each UAV when performing the task. (x t , y t , z t ) is the ground coordinate of the UAV at the position point at the t-th moment, t ∈ [0, T].
[0103] Step 2: Collect the boundary signals of the obstacle B in the ground coordinate system, obtain the boundary position information of the obstacle, and generate the coordinates of the eight vertices of the obstacle in the ground coordinate system is the i-th vertex of obstacle B (0 < i < 9). Establish an obstacle coordinate system, where the origin O of the obstacle coordinate system B is the vertex among the eight vertices of the obstacle that is closest to the origin O of the ground coordinate system e . Taking the three sides where O e is located as the O B x B axis, O B y B axis, and O B z B axis respectively. Denote the vertex among the eight vertices of obstacle B that is farthest from the origin O of the obstacle coordinate system B (i.e., the farthest vertex of the obstacle) as the origin O of the obstacle B coordinate system B and use to represent it.
[0104] Step 3: Collect the boundary signals of the target area A in the ground coordinate system, obtain the boundary position information of the target area, and generate the coordinates of the eight vertices of the target area in the ground coordinate system is the j-th vertex of the target area A (0 < j < 9). Establish a target area coordinate system, where the origin O of the target area coordinate system A is the vertex among the eight vertices of the target area that is closest to the origin O of the ground coordinate system e . Taking the three sides where O A is located as the O A x A axis, O A y A axis, and O A z A axis respectively. Denote the vertex among the eight vertices of the target area A that is farthest from the origin O of the target area A coordinate system A as the origin O of the target area A coordinate system A and use to represent it.
[0105] Step 4: By using the coordinate transformation matrix, the relative positions of the position vector of the UAV at any moment in the obstacle B coordinate system and the target area A coordinate system can be obtained as (i.e., the first relative coordinate of the position point) and (i.e., the second relative coordinate of the position point).
[0106] Specifically, the coordinate transformation matrix means that any two rectangular coordinate systems can be made to coincide by translation plus rotation. The specific calculation method is as follows:
[0107] Translation: In 3D space, assume that a point needs to be translated to another position. Assume a point P in space, whose coordinates are represented as (x1, y1, z1). Translate point P by t in the x-axis direction x , by t in the y-axis direction y , and by t in the z-axis direction z . Let the coordinates of the translated point be (x', y', z'). Then the above point translation operation can be summarized into the following formula:
[0108] Rotation: As Figure 2 shown, from the O n -X n Y n Z n coordinate system to the O b -X b Y b Z b coordinate system conversion can be decomposed into three rotations: The coordinate system O n -X n Y n Z n first rotates ψ degrees around its own Z-axis, then rotates degrees around the Y-axis, and finally, rotates γ degrees around the X-axis to obtain the coordinate system O b -X b Y b Z b . The matrices corresponding to the three rotations are:
[0109]
[0110] From the O n -X n Y n Z n coordinate system to the O b -X b Y b Z b coordinate system rotation matrix can be obtained through the matrices corresponding to the three rotations:
[0111]
[0112] Step Five: Set up an evaluation algorithm. The degree of satisfaction or deviation (i.e., the robustness score) of the trajectories of each UAV to be evaluated in this task from the specification STL formula describing this task can be expressed as:
[0113]
[0114] Through the relative coordinate information, the above equation can be used to obtain the final result.
[0115] Among them, It represents the situation of avoiding obstacle B. If the UAV bypasses obstacle B, then at any moment, the relative position of the UAV trajectory position vector in the obstacle coordinate system (i.e., the first relative coordinate of any position point) has a component less than the minimum coordinate of the obstacle or greater than the maximum coordinate of the cube, and this point is not within the cube. Select the minimum value among the 6 values. After traversing all times, select the minimum value among the minimum values at all times, which is the obstacle avoidance score (i.e., the first robustness).
[0116] It represents the situation of reaching the target area A. If the UAV reaches the target area A, then at any moment, the three component values of the relative position of the UAV trajectory position vector in the obstacle coordinate system (i.e., the second relative coordinate of any position point) are both greater than the origin value and less than the vertex value. Select the maximum value among the 6 values and take its opposite number. After traversing t∈[a,T], select the minimum value among the results at all times, which is the target area score situation (i.e., the second robustness).
[0117] Select the minimum value between the obstacle avoidance score and the target area score, which is the UAV robustness score. The higher the score, the better the robustness.
[0118] Step 6: Determine the target flight trajectory of the UAV as the flight trajectory with the highest robustness score, and control the UAV to fly according to this trajectory.
[0119] As Figure 3 shown, in a certain example, the preset flight trajectory data of 3 UAVs are given, which are used to evaluate the ability of the UAV to avoid static obstacles by using the robustness index in the signal temporal logic theory. From Figure 3 it can be seen that although the No. 1 UAV can be in the target area A within the specified time range and pass by the obstacle B closely, its robustness score ρ is 0, indicating that it meets the STL task formula but has poor robustness; the No. 2 UAV can be in the target area A within the specified time range and fly far away from the obstacle B, and its robustness score is ρ = 0.5, indicating that it meets the STL task formula and has good robustness; the No. 3 UAV is neither in the target area A within the specified time range nor flies inside the obstacle B, and its robustness score ρ is -0.48, indicating that it does not meet the STL task formula. From Figure 3 it can also be clearly seen that among the No. 1 and No. 2 UAVs that complete the task, the No. 2 UAV has stronger obstacle avoidance ability and a better position in the target area A, so its robustness score is higher.
[0120] Embodiment 2
[0121] The UAV flight trajectory determination system in this embodiment includes:
[0122] The first ground coordinate determination module is configured to obtain the ground coordinates of the position points at each moment of multiple preset flight trajectories of the UAV; the ground coordinates of a point are the coordinates of the point in the ground coordinate system; a point includes a vertex and a position point.
[0123] The second ground coordinate determination module is configured to determine the ground coordinates of each vertex of the obstacle and the ground coordinates of each vertex of the target area; the ground coordinate system is composed of the take-off point of the UAV as the origin, the geographic north pole as the x-axis, the direction vertically downward to the ground as the z-axis, and the y-axis in the horizontal plane determined by the right-hand rule; the starting point of all preset flight trajectories is the take-off point.
[0124] The obstacle coordinate system establishment module is configured to establish an obstacle coordinate system based on the ground coordinates of each vertex of the obstacle and determine the farthest vertex of the obstacle.
[0125] The target area coordinate system establishment module is configured to establish a target area coordinate system based on the ground coordinates of each vertex of the target area and determine the farthest vertex of the target area.
[0126] The relative coordinate determination module is configured to determine the first relative coordinates of each vertex of the obstacle and the second relative coordinates of each vertex of the target area; the first relative coordinate of a point is the coordinate of the point in the obstacle coordinate system, and the second relative coordinate of a point is the coordinate of the point in the target area coordinate system.
[0127] The robustness score calculation module is configured to, for any current preset flight trajectory:
[0128] Use the coordinate transformation matrix to determine the first relative coordinates and the second relative coordinates of the position points at each moment of the current preset flight trajectory.
[0129] Based on the first relative coordinate of the farthest vertex of the obstacle, the origin coordinate of the obstacle coordinate system, the second relative coordinate of the farthest vertex of the target area, the origin coordinate of the target area coordinate system, and the first relative coordinates and the second relative coordinates of the position points at each moment of the current preset flight trajectory, calculate the robustness score of the current preset flight trajectory.
[0130] The target flight trajectory determination module is configured to determine the preset flight trajectory with the highest robustness score among all preset flight trajectories as the target flight trajectory of the UAV.
[0131] Embodiment 3
[0132] An electronic device includes:
[0133] One or more processors.
[0134] A storage device on which one or more programs are stored.
[0135] When one or more programs are executed by one or more processors, the one or more processors implement the method for determining the flight trajectory of the drone as in Embodiment 1.
[0136] Embodiment 4
[0137] A storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the method for determining the flight trajectory of the drone as in Embodiment 1.
[0138] The embodiments in this specification are described in a progressive manner. Each embodiment focuses on the differences from other embodiments. For the same or similar parts among the embodiments, reference can be made to each other. For the system disclosed in the embodiments, since it corresponds to the method disclosed in the embodiments, the description is relatively simple. For the relevant parts, reference can be made to the description in the method part.
[0139] Specific examples are used in this article to elaborate on the principles and implementation manners of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea; at the same time, for those of ordinary skill in the art, based on the idea of the present invention, there will be changes in the specific implementation manners and application scopes. In summary, the content of this specification should not be construed as a limitation on the present invention.
Claims
1. A method for determining the flight trajectory of an unmanned aerial vehicle, characterized in that: The method comprises: Obtain the ground coordinates of the drone's position points at each moment along multiple preset flight paths; Determine the ground coordinates of each vertex of the obstacle and each vertex of the target area; the ground coordinates of a point are the coordinates of the point in a ground coordinate system; a point includes a vertex and a position point; the ground coordinate system is based on the take-off point of the UAV as an origin, the geographic North Pole as an x-axis, a vertical downward direction as a z-axis, and a y-axis in a horizontal plane determined by the right-hand rule; the starting point of all the preset flight paths is the take-off point; Establishing an obstacle coordinate system based on the ground coordinates of each vertex of the obstacle, and determining the farthest vertex of the obstacle; Establishing a target area coordinate system based on the ground coordinates of each vertex of the target area, and determining the farthest vertex of the target area; Determine the first relative coordinates of each vertex of the obstacle and the second relative coordinates of each vertex of the target area; the first relative coordinates of a point are the coordinates of the point in the obstacle coordinate system, and the second relative coordinates of a point are the coordinates of the point in the target area coordinate system; For any current preset flight path: Determine the first relative coordinates and the second relative coordinates of the position point at each moment of the current preset flight trajectory using the coordinate transfer matrix; Calculating a robustness score of the current preset flight trajectory based on the first relative coordinates of the farthest vertex of the obstacle, the coordinates of the origin of the obstacle coordinate system, the second relative coordinates of the farthest vertex of the target area, the coordinates of the origin of the target area coordinate system, and the first relative coordinates and the second relative coordinates of the position points at each moment of the current preset flight trajectory; The preset flight trajectory with the highest robustness score among all the preset flight trajectories is determined as the target flight trajectory of the UAV.
2. The method for determining the flight trajectory of a UAV according to claim 1, characterized in that: Establishing an obstacle coordinate system based on the ground coordinates of each vertex of the obstacle specifically includes: The obstacle coordinate system is established by taking the vertex of the obstacle closest to the take-off point as the origin of the obstacle coordinate system.
3. The method for determining the flight trajectory of a UAV according to claim 1, characterized in that: Establishing a target area coordinate system based on the ground coordinates of each vertex of the target area specifically includes: The target area coordinate system is established by taking the vertex of the target area closest to the take-off point as the origin of the target area coordinate system.
4. The method for determining the flight trajectory of a UAV according to claim 1, wherein: Calculating a robustness score of the current preset flight trajectory based on the first relative coordinates of the farthest vertex of the obstacle, the coordinates of the origin of the obstacle coordinate system, the second relative coordinates of the farthest vertex of the target area, the coordinates of the origin of the target area coordinate system, and the first relative coordinates and second relative coordinates of the position points at each moment of the current preset flight trajectory, specifically including: Calculating a first robustness of the current preset flight trajectory based on the first relative coordinates of the farthest vertex of the obstacle, the coordinates of the origin of the obstacle coordinate system, and the first relative coordinates of the position points at each moment of the current preset flight trajectory; Calculating a second robustness of the current preset flight trajectory based on the second relative coordinates of the farthest vertex of the target area, the coordinates of the origin of the target area coordinate system, and the second relative coordinates of the position points at each moment of the current preset flight trajectory; The robustness score is determined based on the first robustness and the second robustness.
5. The method for determining the flight trajectory of a UAV according to claim 4, characterized in that: Calculating a first robustness of the current preset flight trajectory based on the first relative coordinates of the farthest vertex of the obstacle, the coordinates of the origin of the obstacle coordinate system, and the first relative coordinates of the position points at each moment of the current preset flight trajectory, specifically comprising: For any position point at any moment: Calculate the difference between the first component of the first relative coordinate of the position point and the first component of the coordinates of the origin of the obstacle coordinate system, calculate the difference between the second component of the first relative coordinate of the position point and the second component of the coordinates of the origin of the obstacle coordinate system, and calculate the difference between the third component of the first relative coordinate of the position point and the third component of the coordinates of the origin of the obstacle coordinate system; the first component is the component of the coordinate along the x-axis of the obstacle coordinate system, the second component is the component of the coordinate along the y-axis of the obstacle coordinate system, and the third component is the component of the coordinate along the z-axis of the obstacle coordinate system; Calculate the difference between the first component of the first relative coordinate of the farthest vertex of the obstacle and the first component of the first relative coordinate of the location point, calculate the difference between the second component of the first relative coordinate of the farthest vertex of the obstacle and the second component of the first relative coordinate of the location point, and calculate the difference between the third component of the first relative coordinate of the farthest vertex of the obstacle and the third component of the first relative coordinate of the location point; The smallest difference among all differences of position points at all times is determined as the first robustness.
6. The method for determining the flight trajectory of a UAV according to claim 4, characterized in that: Calculating a second robustness of the current preset flight trajectory based on the second relative coordinates of the farthest vertex of the target area, the coordinates of the origin of the target area coordinate system, and the second relative coordinates of the position points at each moment of the current preset flight trajectory specifically includes: For any position point at any moment: Calculating the difference between the fourth component of the origin coordinates of the target area coordinate system and the fourth component of the second relative coordinates of the location point, calculating the difference between the fifth component of the origin coordinates of the target area coordinate system and the fifth component of the second relative coordinates of the location point, and calculating the difference between the sixth component of the origin coordinates of the target area coordinate system and the sixth component of the second relative coordinates of the location point; Calculate the difference between the fourth component of the second relative coordinate of the position point and the fourth component of the second relative coordinate of the farthest vertex of the target area, calculate the difference between the fifth component of the second relative coordinate of the position point and the second relative coordinate of the farthest vertex of the target area, and calculate the difference between the sixth component of the second relative coordinate of the position point and the sixth component of the second relative coordinate of the farthest vertex of the target area; the fourth component is the component of the coordinate along the x-axis of the target area coordinate system, the fifth component is the component of the coordinate along the y-axis of the target area coordinate system, and the sixth component is the component of the coordinate along the z-axis of the target area coordinate system; The difference with the smallest value among all the differences of the position points at all times is determined as the second robustness.
7. The method for determining the flight trajectory of a UAV according to claim 4, characterized in that: Determining the robustness score based on the first robustness and the second robustness includes: determining whether the first robustness is greater than the second robustness; If so, determining the second robustness as the robustness score; If not, the first robustness is determined as the robustness score.
8. A system for determining the flight trajectory of an unmanned aerial vehicle, characterized in that: The system comprises: The first ground coordinate determination module is used to obtain the ground coordinates of the position points of multiple preset flight paths of the UAV at each moment; a second ground coordinate determination module, configured to determine the ground coordinates of each vertex of the obstacle and each vertex of the target area; the ground coordinates of a point are the coordinates of the point in a ground coordinate system; a point includes a vertex and a position point; the ground coordinate system is based on the take-off point of the UAV as an origin, the geographic North Pole as an x-axis, a vertical downward direction as a z-axis, and a y-axis in a horizontal plane determined by the right-hand rule; the starting point of all the preset flight trajectories is the take-off point; An obstacle coordinate system establishment module, configured to establish an obstacle coordinate system based on the ground coordinates of each vertex of the obstacle and determine the farthest vertex of the obstacle; a target area coordinate system establishing module, configured to establish a target area coordinate system based on the ground coordinates of each vertex of the target area, and determine the farthest vertex of the target area; A relative coordinate determination module is used to determine the first relative coordinates of each vertex of the obstacle and the second relative coordinates of each vertex of the target area; the first relative coordinates of a point are the coordinates of the point in the obstacle coordinate system, and the second relative coordinates of a point are the coordinates of the point in the target area coordinate system; Robustness score calculation module, used for any current preset flight trajectory: Determine the first relative coordinates and the second relative coordinates of the position point at each moment of the current preset flight trajectory using the coordinate transfer matrix; Calculating a robustness score of the current preset flight trajectory based on the first relative coordinates of the farthest vertex of the obstacle, the coordinates of the origin of the obstacle coordinate system, the second relative coordinates of the farthest vertex of the target area, the coordinates of the origin of the target area coordinate system, and the first relative coordinates and the second relative coordinates of the position points at each moment of the current preset flight trajectory; The target flight trajectory determination module is used to determine the preset flight trajectory with the highest robustness score among all the preset flight trajectories as the target flight trajectory of the UAV.
9. An electronic device, characterized in that: include: one or more processors; a storage device having one or more programs stored thereon; When the one or more programs are executed by the one or more processors, the one or more processors implement the method for determining the flight trajectory of a drone as described in any one of claims 1 to 7.
10. A storage medium, characterized in that: A computer program is stored thereon, wherein when the computer program is executed by a processor, the method for determining the flight trajectory of a drone as described in any one of claims 1 to 7 is implemented.