CN116382347A - Unmanned aerial vehicle flight trajectory determination method, unmanned aerial vehicle flight trajectory determination system, electronic equipment and storage medium - Google Patents
Unmanned aerial vehicle flight trajectory determination method, unmanned aerial vehicle flight trajectory determination system, electronic equipment and storage medium Download PDFInfo
- Publication number
- CN116382347A CN116382347A CN202310508493.9A CN202310508493A CN116382347A CN 116382347 A CN116382347 A CN 116382347A CN 202310508493 A CN202310508493 A CN 202310508493A CN 116382347 A CN116382347 A CN 116382347A
- Authority
- CN
- China
- Prior art keywords
- coordinates
- coordinate system
- target area
- obstacle
- relative
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 32
- 239000011159 matrix material Substances 0.000 claims abstract description 14
- 238000012546 transfer Methods 0.000 claims abstract description 11
- 238000004590 computer program Methods 0.000 claims description 5
- 238000004364 calculation method Methods 0.000 claims description 4
- 238000013519 translation Methods 0.000 description 5
- 239000004973 liquid crystal related substance Substances 0.000 description 4
- 239000013598 vector Substances 0.000 description 4
- 238000011156 evaluation Methods 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
- G05D1/106—Change initiated in response to external conditions, e.g. avoidance of elevated terrain or of no-fly zones
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a method, a system, electronic equipment and a storage medium for determining a flight track of an unmanned aerial vehicle, and relates to the technical field of unmanned aerial vehicle track planning, wherein the method comprises the following steps: 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; determining first relative coordinates of each vertex of the obstacle and second relative coordinates of each vertex of the target area; determining the relative coordinates of position points at all moments of a preset flight track by using a coordinate transfer matrix; calculating a robustness score based on the first relative coordinates of the farthest vertex of the obstacle, the origin coordinates of the coordinate system of the obstacle and the coordinate system of the target area, the second relative coordinates of the farthest vertex of the target area and all the relative coordinates of the preset flight track; and determining the preset flight path with the highest robustness score in all the preset flight paths as a target flight path, thereby realizing the determination of the unmanned aerial vehicle flight path.
Description
Technical Field
The invention relates to the technical field of unmanned aerial vehicle track planning, in particular to a method and a system for determining a flight track of an unmanned aerial vehicle, electronic equipment and a storage medium.
Background
The unmanned aerial vehicle has advantages of flexible maneuvering, strong operability, low development cost and the like, so that the unmanned aerial vehicle plays an increasingly critical role in various fields of army and civilian. The research on the unmanned aerial vehicle performance evaluation method can determine relevant technical indexes in the field, so that the rapid development of unmanned aerial vehicle technology is guided. However, in the field of unmanned aerial vehicles, a relatively standard unmanned aerial vehicle intelligent performance evaluation method is not available so far, and therefore, the problem of low robustness of a preset flight track of the unmanned aerial vehicle exists.
Disclosure of Invention
The invention aims to provide a method, a system, electronic equipment and a storage medium for determining the flight path of an unmanned aerial vehicle, which realize the determination of the flight path of the unmanned aerial vehicle.
In order to achieve the above object, the present invention provides the following solutions:
a method for determining the flight path of an unmanned aerial vehicle comprises the following steps:
acquiring ground coordinates of position points of a plurality of preset flight trajectories of the unmanned aerial vehicle at all moments;
determining the ground coordinates of each vertex of the obstacle and the ground coordinates of each vertex of the target area; the ground coordinates of the points are the coordinates of the points in a ground coordinate system; the points include vertices and position points; the ground coordinate system is composed based on a flying spot of the unmanned aerial vehicle as an origin, a geographic north pole as an x-axis, a vertical ground down direction as a z-axis, and a y-axis in a horizontal plane determined by a right-hand rule; the starting points of all the preset flight trajectories are all the departure points;
establishing an obstacle coordinate system based on 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;
determining first relative coordinates of each vertex of the obstacle and second relative coordinates of each vertex of the target area; the first relative coordinates of the points are the coordinates of the points in the obstacle coordinate system, and the second relative coordinates of the points are the coordinates of the points in the target area coordinate system;
for any current preset flight trajectory:
determining a first relative coordinate and a second relative coordinate of a position point at each moment of a current preset flight track by utilizing a 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 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;
and determining the preset flight trajectory with the highest robustness score in all the preset flight trajectories as a target flight trajectory of the unmanned aerial vehicle.
Optionally, establishing an obstacle coordinate system based on the ground coordinates of each vertex of the obstacle specifically includes:
and establishing the obstacle coordinate system by taking the vertex of the obstacle closest to the departure point as the origin of the obstacle coordinate system.
Optionally, establishing a target area coordinate system based on the ground coordinates of each vertex of the target area specifically includes:
and taking the vertex of the target area closest to the flying spot as the origin of the target area coordinate system, and establishing the target area coordinate system.
Optionally, calculating the robustness score of the current preset flight trajectory 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 coordinate and the second relative coordinate of the position point at each moment of the current preset flight trajectory specifically includes:
calculating a first robustness of the current preset flight trajectory based on a first relative coordinate of the farthest vertex of the obstacle, an origin coordinate of the obstacle coordinate system and a first relative coordinate of a position point of each moment of the current preset flight trajectory;
calculating a second robustness of the current preset flight trajectory based on a second relative coordinate of the farthest vertex of the target area, an origin coordinate of the target area coordinate system and a second relative coordinate of a position point of each moment of the current preset flight trajectory;
the robustness score is determined based on the first robustness and the second robustness.
Optionally, calculating 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 includes:
for any point in time, the location point:
calculating a difference between a first component of a first relative coordinate of a location point and a first component of an origin coordinate of the obstacle coordinate system, calculating a difference between a second component of the first relative coordinate of the location point and a second component of the origin coordinate of the obstacle coordinate system, and calculating a difference between a third component of the first relative coordinate of the location point and a third component of the origin coordinate of the obstacle coordinate system; the first component is a component of coordinates along an x-axis of the obstacle coordinate system, the second component is a component of coordinates along a y-axis of the obstacle coordinate system, and the third component is a component of coordinates along a z-axis of the obstacle coordinate system;
calculating a difference between a first component of a first relative coordinate of a farthest vertex of the obstacle and a first component of a first relative coordinate of a location point, calculating a difference between a second component of the first relative coordinate of the farthest vertex of the obstacle and a second component of the first relative coordinate of the location point, and calculating a difference between a third component of the first relative coordinate of the farthest vertex of the obstacle and a third component of the first relative coordinate of the location point;
and determining the difference value with the smallest numerical value in all difference values of the position points at all moments as the first robustness.
Optionally, calculating 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 includes:
for any point in time, the location point:
calculating a difference value of a fourth component of the origin coordinates of the target area coordinate system and a fourth component of the second relative coordinates of the position points, calculating a difference value of a fifth component of the origin coordinates of the target area coordinate system and a fifth component of the second relative coordinates of the position points, and calculating a difference value of a sixth component of the origin coordinates of the target area coordinate system and a sixth component of the second relative coordinates of the position points;
calculating a difference between a fourth component of the second relative coordinates of the location point and a fourth component of the second relative coordinates of the farthest vertex of the target area, calculating a difference between a fifth component of the second relative coordinates of the location point and the second relative coordinates of the farthest vertex of the target area, and calculating a difference between a sixth component of the second relative coordinates of the location point and a sixth component of the second relative coordinates of the farthest vertex of the target area; the fourth component is a component of coordinates along an x-axis of the target area coordinate system, the fifth component is a component of coordinates along a y-axis of the target area coordinate system, and the sixth component is a component of coordinates along a z-axis of the target area coordinate system;
and determining the difference value with the smallest numerical value in all the difference values of the position points at all the moments as the second robustness.
Optionally, determining the robustness score based on the first robustness and the second robustness includes:
judging whether the first robustness is larger than the second robustness;
if yes, determining the second robustness as the robustness fraction;
and if not, determining the first robustness as the robustness fraction.
A system for determining a flight trajectory of an unmanned aerial vehicle, comprising:
the first ground coordinate determining module is used for acquiring the ground coordinates of the position points of the unmanned aerial vehicle at each moment of a plurality of preset flight tracks;
the second ground coordinate determining module is used for determining the ground coordinates of each vertex of the obstacle and the ground coordinates of each vertex of the target area; the ground coordinates of the points are the coordinates of the points in a ground coordinate system; the points include vertices and position points; the ground coordinate system is composed based on a flying spot of the unmanned aerial vehicle as an origin, a geographic north pole as an x-axis, a vertical ground down direction as a z-axis, and a y-axis in a horizontal plane determined by a right-hand rule; the starting points of all the preset flight trajectories are all the departure points;
the obstacle coordinate system establishing module is used for establishing an obstacle coordinate system based on the ground coordinates of each vertex of the obstacle and determining the farthest vertex of the obstacle;
the system comprises a target area coordinate system establishing module, a target area coordinate system determining module and a target area coordinate system determining module, wherein the target area coordinate system establishing module is used for establishing a target area coordinate system based on the ground coordinates of all vertexes of the target area and determining the farthest vertexes of the target area;
the relative coordinate determining module is used for determining first relative coordinates of each vertex of the obstacle and second relative coordinates of each vertex of the target area; the first relative coordinates of the points are the coordinates of the points in the obstacle coordinate system, and the second relative coordinates of the points are the coordinates of the points in the target area coordinate system;
the robustness score calculation module is used for carrying out the following steps on any current preset flight track:
determining a first relative coordinate and a second relative coordinate of a position point at each moment of a current preset flight track by utilizing a 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 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;
the target flight trajectory determining module is used for determining the preset flight trajectory with the highest robustness score in all the preset flight trajectories as the target flight trajectory of the unmanned aerial vehicle.
An electronic device, comprising:
one or more processors;
a storage device having one or more programs stored thereon;
the one or more programs, when executed by the one or more processors, cause the one or more processors to implement the method of determining a flight trajectory of a drone as described above.
A storage medium having stored thereon a computer program which, when executed by a processor, implements a method of determining a flight trajectory of a drone as described above.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects:
the invention discloses a method, a system, electronic equipment and a storage medium for determining a flight trajectory of an unmanned aerial vehicle, wherein the method comprises the steps of firstly obtaining coordinates of position points of a plurality of preset flight trajectories of the unmanned aerial vehicle at all times, vertexes of an obstacle and vertexes of a target area in a ground coordinate system, establishing the coordinate system of the obstacle and the coordinate system of the target area, and determining the farthest vertexes of the obstacle and the target area; secondly, determining a first relative coordinate of each vertex of the obstacle in an obstacle coordinate system and a second relative coordinate of each vertex of the target area in a target area coordinate system; thirdly, determining a first relative coordinate and a second relative coordinate of a position point at each moment of the current preset flight trajectory by utilizing a coordinate transfer matrix, and calculating a robustness score of the current preset flight trajectory 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 coordinate and the second relative coordinate of the position point at each moment of the current preset flight trajectory; and finally, determining the preset flight trajectory with the highest robustness score as the target flight trajectory of the unmanned aerial vehicle, thereby realizing the determination of the flight trajectory of the unmanned aerial vehicle.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions of the prior art, the drawings that are needed in the embodiments will be briefly described below, it being obvious that the drawings in the following description are only some embodiments of the present invention, and that other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a schematic flow chart of a method, a system, an electronic device and a storage medium for determining a flight trajectory of an unmanned aerial vehicle according to embodiment 1 of the present invention;
FIG. 2 is a schematic diagram of a coordinate transfer matrix;
fig. 3 is a schematic diagram of flight trajectories of three unmanned aerial vehicles.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The invention aims to provide a method, a system, electronic equipment and a storage medium for determining the flight trajectory of an unmanned aerial vehicle, and aims to determine the flight trajectory of the unmanned aerial vehicle.
In order that the above-recited objects, features and advantages of the present invention will become more readily apparent, a more particular description of the invention will be rendered by reference to the appended drawings and appended detailed description.
Example 1
Fig. 1 is a flow chart of a method for determining a flight path of an unmanned aerial vehicle according to embodiment 1 of the present invention. As shown in fig. 1, the method for determining the flight trajectory of the unmanned aerial vehicle in this embodiment includes:
step 101: and acquiring the ground coordinates of the position points of the unmanned aerial vehicle at each moment of a plurality of preset flight tracks.
Step 102: and determining the ground coordinates of each vertex of the obstacle and the ground coordinates of each vertex of the target area.
Wherein, the ground coordinates of the points are the coordinates of the points in a ground coordinate system; the points include vertices and position points; the ground coordinate system is formed based on a flying spot of the unmanned aerial vehicle serving as an origin, a geographic north pole serving as an x axis, a vertical ground downward direction serving as a z axis and a y axis which is determined by a right hand rule and is in a horizontal plane; the starting points of all the preset flight trajectories are all departure points.
Step 103: and establishing an obstacle coordinate system based on the ground coordinates of each vertex of the obstacle, and determining the farthest vertex of the obstacle.
Step 104: and 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.
Step 105: first relative coordinates of each vertex of the obstacle and second relative coordinates of each vertex of the target area are determined.
Wherein the first relative coordinates of the points are the coordinates of the points in the obstacle coordinate system, and the second relative coordinates of the points are the coordinates of the points in the target area coordinate system.
Step 106: and determining the robustness scores of all the preset flight trajectories.
for any current preset flight trajectory:
step 1061: and determining a first relative coordinate and a second relative coordinate of a position point at each moment of the current preset flight track by utilizing the coordinate transfer matrix.
Step 1062: and calculating the robustness score of the current preset flight trajectory based on the first relative coordinates of the farthest vertex of the obstacle, the original point coordinates of the obstacle coordinate system, the second relative coordinates of the farthest vertex of the target area, the original point 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.
Step 107: and determining the preset flight trajectory with the highest robustness score in all the preset flight trajectories as a target flight trajectory of the unmanned aerial vehicle.
As an alternative embodiment, the establishing an obstacle coordinate system based on the ground coordinates of each vertex of the obstacle specifically includes:
and taking the vertex of the obstacle closest to the flying spot as the origin of the obstacle coordinate system, and establishing the obstacle coordinate system.
As an alternative embodiment, the establishing the target area coordinate system based on the ground coordinates of each vertex of the target area specifically includes:
and taking the vertex of the target area closest to the flying spot as the origin of the target area coordinate system, and establishing the target area coordinate system.
As an alternative embodiment, step 1062 specifically includes:
step 10621: and calculating 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.
Step 10622: and calculating the second robustness of the current preset flight trajectory based on the second relative coordinates of the farthest vertex of the target area, the original point coordinates of the target area coordinate system and the second relative coordinates of the position points of each moment of the current preset flight trajectory.
Step 10623: a robustness score is determined based on the first robustness and the second robustness.
As an alternative embodiment, step 10621 specifically includes:
for any point in time, the location point:
calculating a difference value between a first component of the first relative coordinates of the position points and a first component of the origin coordinates of the obstacle coordinate system, calculating a difference value between a second component of the first relative coordinates of the position points and a second component of the origin coordinates of the obstacle coordinate system, and calculating a difference value between a third component of the first relative coordinates of the position points and a third component of the origin coordinates of the obstacle coordinate system; the first component is a component of the coordinates along the x-axis of the obstacle coordinate system, the second component is a component of the coordinates along the y-axis of the obstacle coordinate system, and the third component is a component of the coordinates along the z-axis of the obstacle coordinate system.
Calculating a difference between a first component of a first relative coordinate of a farthest vertex of the obstacle and a first component of a first relative coordinate of a location point, calculating a difference between a second component of the first relative coordinate of the farthest vertex of the obstacle and a second component of the first relative coordinate of the location point, and calculating a difference between a third component of the first relative coordinate of the farthest vertex of the obstacle and a third component of the first relative coordinate of the location point.
And determining the difference value with the smallest numerical value in all the difference values of the position points at all the moments as a first robustness.
As an alternative embodiment, step 10622 specifically includes:
for any point in time, the location point:
and calculating a difference value of a fourth component of the origin coordinates of the target area coordinate system and a fourth component of the second relative coordinates of the position points, calculating a difference value of a fifth component of the origin coordinates of the target area coordinate system and a fifth component of the second relative coordinates of the position points, and calculating a difference value of a sixth component of the origin coordinates of the target area coordinate system and a sixth component of the second relative coordinates of the position points.
Calculating a difference value of a fourth component of the second relative coordinates of the location point and a fourth component of the second relative coordinates of the farthest vertex of the target area, calculating a difference value of a fifth component of the second relative coordinates of the location point and a second relative coordinates of the farthest vertex of the target area, and calculating a difference value of a sixth component of the second relative coordinates of the location point and a sixth component of the second relative coordinates of the farthest vertex of the target area; the fourth component is a component of the coordinates along the x-axis of the target area coordinate system, the fifth component is a component of the coordinates along the y-axis of the target area coordinate system, and the sixth component is a component of the coordinates along the z-axis of the target area coordinate system.
And determining the difference value with the smallest numerical value in all the difference values of the position points at all the moments as a second robustness.
As an alternative embodiment, step 10623 includes:
and judging whether the first robustness is larger than the second robustness.
If so, the second robustness is determined as a robustness fraction.
If not, the first robustness is determined as a robustness fraction.
Specific examples: in actual operation, signal sequential logic can be introduced to calculate the robustness score of the flight track of the unmanned aerial vehicle, and the method is described below by taking a four-rotor unmanned aerial vehicle as an example to avoid static obstacles.
The task requirements in this embodiment are: the four-rotor unmanned aerial vehicle starts from a starting point, and the flight time interval T epsilon [0, T ], in the process, the unmanned aerial vehicle always avoids a cuboid barrier B and is always positioned in a cube target area A in T epsilon [ a, T ]. The task requirement is described by a signal sequential logic (Signal Temporal Logic, STL) formula:
wherein, the liquid crystal display device comprises a liquid crystal display device,
i ε { A, B }, a and T are time variables, 0 < T, l is the lower boundary, and u is the upper boundary. F (F) [a,T] φ A Indicating that the unmanned aerial vehicle is at T E [ a, T]Is positioned in the cube target area A +.>
Indicating that unmanned plane is in [0, T ]]Avoiding the cuboid barrier B in time.
Step one: establishing a ground coordinate system and an origin O e For the unmanned plane take-off site, the x-axis (O e x e Axis) pointing to geographic north, z-axis (O) of the ground coordinate system e z e Axis) is oriented vertically to the ground, the y-axis (O) of the ground coordinate system e y e Axis) is in the horizontal plane, as determined by the right hand rule. Acquiring the path condition of each unmanned aerial vehicle in a specified time, and acquiring each unmanned aerial vehicleAll position vectors (x t ,y t ,z t )。(x t ,y t ,z t ) Is the ground coordinate of the position point of the unmanned plane at the t moment, t is E [0, T]。
Step two: acquiring boundary signals of the obstacle B in a ground coordinate system, acquiring boundary position information of the obstacle, and generating coordinates of eight vertexes of the obstacle in the ground coordinate system
Is the ith vertex of the obstacle B (0 < i < 9). Establishing an obstacle coordinate system, wherein the origin O of the obstacle coordinate system B Is the distance from the origin O of the ground coordinate system in eight vertexes of the obstacle e The nearest vertex, O e The three sides are respectively taken as O B x B Shaft, O B y B Shaft and O B z B A shaft. Distance between eight vertexes of obstacle B and origin O of obstacle coordinate system B The furthest vertex (i.e. the furthest vertex of the obstacle) is noted as
Obstacle B coordinate system origin O B Use->
And (3) representing.
Step three: acquiring boundary signals of a target area A in a ground coordinate system, acquiring boundary position information of the target area, and generating coordinates of eight vertexes of the target area in the ground coordinate system
The j-th vertex of the target area A (0 < j < 9). Establishing a target area coordinate system, wherein the origin O of the target area coordinate system A For the origin O of the ground coordinate system in the eight vertexes of the target area e The nearest vertex, O A The three sides are respectively taken as O A x A Shaft, O A y A Shaft and O A z A A shaft. Eight tops of the target area AOrigin O of a coordinate system of a target area a in a point A The furthest vertex is noted as
Origin O of coordinate system of target area A A Use->
And (3) representing.
Step four: the relative positions of the position vector of the unmanned aerial vehicle at any moment in the obstacle B coordinate system and the target area A coordinate system can be obtained by using the coordinate transfer matrix
(i.e., the first relative coordinates of the location point) and
(i.e., the second relative coordinate of the location point).
Specifically, the coordinate transfer matrix, that is, any two rectangular coordinate systems, can be overlapped by using a translation and rotation mode. The specific calculation mode is as follows:
translation: in 3D space, it is assumed that one point needs to be translated to another position. Assuming a point P in space, which is represented by coordinates (x 1, y1, z 1), the point P is translated t in the x-axis direction x Translation t in y-axis direction y Translation t in the z-axis direction z Assuming that the coordinates of the translated point are (x ', y ', z '), the translation operation of the above point can be summarized as the following formula:
and (3) rotation: as shown in FIG. 2, from O n -X n Y n Z n Coordinate system to O b -X b Y b Z b The transformation of the coordinate system can be broken down into three rotations: coordinate system O n -X n Y n Z n First rotate psi around Z axis of the motor and then rotate around Y axis
Finally, rotating gamma degrees around the X axis to obtain a coordinate system O b -X b Y b Z b . The matrix corresponding to the three rotations is:
from O n -X n Y n Z n Coordinate system to O b -X b Y b Z b Rotation matrix of coordinate system
The matrix corresponding to the three rotations can be obtained:
step five: setting an evaluation algorithm, wherein the satisfaction or departure degree (i.e. robustness score) of the task described by the track and the standard STL formula of each unmanned aerial vehicle to be evaluated in the task can be expressed as follows:
the final result can be obtained by applying the equation of the above equation through the relative coordinate information.
Wherein, the liquid crystal display device comprises a liquid crystal display device,
indicating the avoidance of obstacle B, if the drone bypasses obstacle B, then at any time the relative position of the drone trajectory position vector in the obstacle coordinate system (i.e., the first relative coordinate of any point in position) has a component that is less than the minimum obstacle coordinate or greater than the maximum cube coordinate, and this point is not within the cube. Selecting the minimum value of the 6 values, traversing all times, and selecting the minimum value from the minimum values at all times to obtain an obstacle avoidance score (i.e. a first robustness degree)。
And selecting the minimum value from the obstacle avoidance score and the target area score to obtain the unmanned aerial vehicle robustness score, wherein the higher the score is, the better the robustness is.
Step six: and determining the flight track with the highest robustness score as the target flight track of the unmanned aerial vehicle, and controlling the unmanned aerial vehicle to fly according to the track.
As shown in fig. 3, in a certain example, preset flight trajectory data of 3 unmanned aerial vehicles are given, and the preset flight trajectory data are used for evaluating the capability of the unmanned aerial vehicles to avoid static obstacles by adopting a robustness index in a signal sequential logic theory. As can be seen from fig. 3, although the unmanned aerial vehicle No. 1 can be located in the target area a within a predetermined time range and is in close contact with the obstacle B, the robustness score ρ is 0, which indicates that the STL task formula is satisfied, but the robustness is poor; the unmanned aerial vehicle No. 2 can be located in a target area A in a specified time range and fly away from an obstacle B, the robustness score of the unmanned aerial vehicle is rho is 0.5, the STL task formula is met, and the robustness is good; the unmanned aerial vehicle No. 3 is not in the target area A in the specified time range and flies through the inside of the obstacle B, and the robustness fraction rho is-0.48, which indicates that the unmanned aerial vehicle does not meet the STL task formula. As can be clearly seen from fig. 3, in the unmanned aerial vehicle No. 1 and No. 2 which complete the task, the unmanned aerial vehicle No. 2 has strong obstacle avoidance capability, and is better in the position of the target area a, so that the robustness score is higher.
Example 2
The unmanned aerial vehicle flight path determining system in this embodiment includes:
the first ground coordinate determining module is used for acquiring the ground coordinates of the position points of the unmanned aerial vehicle at each moment of a plurality of preset flight tracks; the ground coordinates of the points are the coordinates of the points in a ground coordinate system; points include vertices and position points.
The second ground coordinate determining module is used for determining 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 formed based on a flying spot of the unmanned aerial vehicle serving as an origin, a geographic north pole serving as an x axis, a vertical ground downward direction serving as a z axis and a y axis which is determined by a right hand rule and is in a horizontal plane; the starting points of all the preset flight trajectories are all departure points.
And the obstacle coordinate system establishing module is used for establishing an obstacle coordinate system based on the ground coordinates of each vertex of the obstacle and determining the farthest vertex of the obstacle.
And the target area coordinate system establishing module is used for 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.
The relative coordinate determining module is used for 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 coordinates of the points are coordinates of the points in the obstacle coordinate system, and the second relative coordinates of the points are coordinates of the points in the target area coordinate system.
The robustness score calculation module is used for carrying out the following steps on any current preset flight track:
and determining a first relative coordinate and a second relative coordinate of a position point at each moment of the current preset flight track by utilizing the coordinate transfer matrix.
And calculating the robustness score of the current preset flight trajectory based on the first relative coordinates of the farthest vertex of the obstacle, the original point coordinates of the obstacle coordinate system, the second relative coordinates of the farthest vertex of the target area, the original point 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.
The target flight trajectory determining module is used for determining the preset flight trajectory with the highest robustness score in all the preset flight trajectories as the target flight trajectory of the unmanned aerial vehicle.
Example 3
An electronic device, comprising:
one or more processors.
A storage device having one or more programs stored thereon.
The one or more programs, when executed by the one or more processors, cause the one or more processors to implement a method of determining a flight trajectory of a drone as in embodiment 1.
Example 4
A storage medium having stored thereon a computer program, wherein the computer program when executed by a processor implements the method of determining a flight trajectory of a drone as in embodiment 1.
In this specification, each embodiment is described in a progressive manner, and each embodiment is mainly described in a different manner from other embodiments, so that identical and similar parts of each embodiment are mutually referred to. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant points refer to the description of the method section.
The principles and embodiments of the present invention have been described herein with reference to specific examples, the description of which is intended only to assist in understanding the methods of the present invention and the core ideas thereof; also, it is within the scope of the present invention to be modified by those of ordinary skill in the art in light of the present teachings. In view of the foregoing, this description should not be construed as limiting the invention.
Claims (10)
1. A method for determining a flight trajectory of an unmanned aerial vehicle, the method comprising:
acquiring ground coordinates of position points of a plurality of preset flight trajectories of the unmanned aerial vehicle at all moments;
determining the ground coordinates of each vertex of the obstacle and the ground coordinates of each vertex of the target area; the ground coordinates of the points are the coordinates of the points in a ground coordinate system; the points include vertices and position points; the ground coordinate system is composed based on a flying spot of the unmanned aerial vehicle as an origin, a geographic north pole as an x-axis, a vertical ground down direction as a z-axis, and a y-axis in a horizontal plane determined by a right-hand rule; the starting points of all the preset flight trajectories are all the departure points;
establishing an obstacle coordinate system based on 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;
determining first relative coordinates of each vertex of the obstacle and second relative coordinates of each vertex of the target area; the first relative coordinates of the points are the coordinates of the points in the obstacle coordinate system, and the second relative coordinates of the points are the coordinates of the points in the target area coordinate system;
for any current preset flight trajectory:
determining a first relative coordinate and a second relative coordinate of a position point at each moment of a current preset flight track by utilizing a 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 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;
and determining the preset flight trajectory with the highest robustness score in all the preset flight trajectories as a target flight trajectory of the unmanned aerial vehicle.
2. The method for determining the flight trajectory of the unmanned aerial vehicle according to claim 1, wherein the establishing of the obstacle coordinate system based on the ground coordinates of each vertex of the obstacle specifically comprises:
and establishing the obstacle coordinate system by taking the vertex of the obstacle closest to the departure point as the origin of the obstacle coordinate system.
3. The method for determining the flight trajectory of the unmanned aerial vehicle according to claim 1, wherein the establishing a target area coordinate system based on the ground coordinates of each vertex of the target area specifically comprises:
and taking the vertex of the target area closest to the flying spot as the origin of the target area coordinate system, and establishing the target area coordinate system.
4. The method according to claim 1, wherein calculating 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 relative coordinates and the second relative coordinates of the position point at each moment of the current preset flight trajectory, specifically comprises:
calculating a first robustness of the current preset flight trajectory based on a first relative coordinate of the farthest vertex of the obstacle, an origin coordinate of the obstacle coordinate system and a first relative coordinate of a position point of each moment of the current preset flight trajectory;
calculating a second robustness of the current preset flight trajectory based on a second relative coordinate of the farthest vertex of the target area, an origin coordinate of the target area coordinate system and a second relative coordinate of a position point of 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 according to claim 4, wherein calculating 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 point at each time of the current preset flight trajectory, specifically comprises:
for any point in time, the location point:
calculating a difference between a first component of a first relative coordinate of a location point and a first component of an origin coordinate of the obstacle coordinate system, calculating a difference between a second component of the first relative coordinate of the location point and a second component of the origin coordinate of the obstacle coordinate system, and calculating a difference between a third component of the first relative coordinate of the location point and a third component of the origin coordinate of the obstacle coordinate system; the first component is a component of coordinates along an x-axis of the obstacle coordinate system, the second component is a component of coordinates along a y-axis of the obstacle coordinate system, and the third component is a component of coordinates along a z-axis of the obstacle coordinate system;
calculating a difference between a first component of a first relative coordinate of a farthest vertex of the obstacle and a first component of a first relative coordinate of a location point, calculating a difference between a second component of the first relative coordinate of the farthest vertex of the obstacle and a second component of the first relative coordinate of the location point, and calculating a difference between a third component of the first relative coordinate of the farthest vertex of the obstacle and a third component of the first relative coordinate of the location point;
and determining the difference value with the smallest numerical value in all difference values of the position points at all moments as the first robustness.
6. The method according to claim 4, wherein calculating the second robustness of the current preset flight path 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 point at each time of the current preset flight path, specifically comprises:
for any point in time, the location point:
calculating a difference value of a fourth component of the origin coordinates of the target area coordinate system and a fourth component of the second relative coordinates of the position points, calculating a difference value of a fifth component of the origin coordinates of the target area coordinate system and a fifth component of the second relative coordinates of the position points, and calculating a difference value of a sixth component of the origin coordinates of the target area coordinate system and a sixth component of the second relative coordinates of the position points;
calculating a difference between a fourth component of the second relative coordinates of the location point and a fourth component of the second relative coordinates of the farthest vertex of the target area, calculating a difference between a fifth component of the second relative coordinates of the location point and the second relative coordinates of the farthest vertex of the target area, and calculating a difference between a sixth component of the second relative coordinates of the location point and a sixth component of the second relative coordinates of the farthest vertex of the target area; the fourth component is a component of coordinates along an x-axis of the target area coordinate system, the fifth component is a component of coordinates along a y-axis of the target area coordinate system, and the sixth component is a component of coordinates along a z-axis of the target area coordinate system;
and determining the difference value with the smallest numerical value in all the difference values of the position points at all the moments as the second robustness.
7. The method of determining a flight trajectory of a drone of claim 4, wherein determining the robustness score based on the first robustness and the second robustness comprises:
judging whether the first robustness is larger than the second robustness;
if yes, determining the second robustness as the robustness fraction;
and if not, determining the first robustness as the robustness fraction.
8. A system for determining a flight trajectory of an unmanned aerial vehicle, the system comprising:
the first ground coordinate determining module is used for acquiring the ground coordinates of the position points of the unmanned aerial vehicle at each moment of a plurality of preset flight tracks;
the second ground coordinate determining module is used for determining the ground coordinates of each vertex of the obstacle and the ground coordinates of each vertex of the target area; the ground coordinates of the points are the coordinates of the points in a ground coordinate system; the points include vertices and position points; the ground coordinate system is composed based on a flying spot of the unmanned aerial vehicle as an origin, a geographic north pole as an x-axis, a vertical ground down direction as a z-axis, and a y-axis in a horizontal plane determined by a right-hand rule; the starting points of all the preset flight trajectories are all the departure points;
the obstacle coordinate system establishing module is used for establishing an obstacle coordinate system based on the ground coordinates of each vertex of the obstacle and determining the farthest vertex of the obstacle;
the system comprises a target area coordinate system establishing module, a target area coordinate system determining module and a target area coordinate system determining module, wherein the target area coordinate system establishing module is used for establishing a target area coordinate system based on the ground coordinates of all vertexes of the target area and determining the farthest vertexes of the target area;
the relative coordinate determining module is used for determining first relative coordinates of each vertex of the obstacle and second relative coordinates of each vertex of the target area; the first relative coordinates of the points are the coordinates of the points in the obstacle coordinate system, and the second relative coordinates of the points are the coordinates of the points in the target area coordinate system;
the robustness score calculation module is used for carrying out the following steps on any current preset flight track:
determining a first relative coordinate and a second relative coordinate of a position point at each moment of a current preset flight track by utilizing a 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 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;
the target flight trajectory determining module is used for determining the preset flight trajectory with the highest robustness score in all the preset flight trajectories as the target flight trajectory of the unmanned aerial vehicle.
9. An electronic device, comprising:
one or more processors;
a storage device having one or more programs stored thereon;
the one or more programs, when executed by the one or more processors, cause the one or more processors to implement the method of determining a flight trajectory of a drone as claimed in any one of claims 1 to 7.
10. A storage medium having stored thereon a computer program, wherein the computer program when executed by a processor implements the method of determining a flight trajectory of a drone according to any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310508493.9A CN116382347A (en) | 2023-05-08 | 2023-05-08 | Unmanned aerial vehicle flight trajectory determination method, unmanned aerial vehicle flight trajectory determination system, electronic equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310508493.9A CN116382347A (en) | 2023-05-08 | 2023-05-08 | Unmanned aerial vehicle flight trajectory determination method, unmanned aerial vehicle flight trajectory determination system, electronic equipment and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116382347A true CN116382347A (en) | 2023-07-04 |
Family
ID=86969517
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310508493.9A Pending CN116382347A (en) | 2023-05-08 | 2023-05-08 | Unmanned aerial vehicle flight trajectory determination method, unmanned aerial vehicle flight trajectory determination system, electronic equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116382347A (en) |
-
2023
- 2023-05-08 CN CN202310508493.9A patent/CN116382347A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20190138029A1 (en) | Collision avoidance system, depth imaging system, vehicle, map generator and methods thereof | |
| CN112884894B (en) | Scene reconstruction data acquisition method and device, computer equipment and storage medium | |
| Betge-Brezetz et al. | Uncertain map making in natural environments | |
| CN110362098A (en) | Unmanned plane vision method of servo-controlling, device and unmanned plane | |
| Carloni et al. | Robot vision: Obstacle-avoidance techniques for unmanned aerial vehicles | |
| CN112184812B (en) | Method for improving identification and positioning precision of unmanned aerial vehicle camera to april tag and positioning method and system | |
| Eynard et al. | Real time UAV altitude, attitude and motion estimation from hybrid stereovision | |
| Call et al. | Obstacle avoidance for unmanned air vehicles using image feature tracking | |
| Brockers et al. | Autonomous landing and ingress of micro-air-vehicles in urban environments based on monocular vision | |
| CN112789672A (en) | Control and navigation system, attitude optimization, mapping and positioning technology | |
| Wang et al. | Obstacle avoidance for quadrotor using improved method based on optical flow | |
| US20190324448A1 (en) | Remote steering of an unmanned aerial vehicle | |
| Nashashibi et al. | 3-d autonomous navigation in a natural environment | |
| Choi et al. | Efficient simultaneous localization and mapping based on ceiling-view: ceiling boundary feature map approach | |
| CN112419403A (en) | Indoor unmanned aerial vehicle positioning method based on two-dimensional code array | |
| Fragoso et al. | Dynamically feasible motion planning for micro air vehicles using an egocylinder | |
| Liu et al. | A new approach for the estimation of non-cooperative satellites based on circular feature extraction | |
| CN113961013A (en) | Unmanned aerial vehicle path planning method based on RGB-D SLAM | |
| Singh et al. | Autonomous cross-country navigation using stereo vision | |
| CN116382347A (en) | Unmanned aerial vehicle flight trajectory determination method, unmanned aerial vehicle flight trajectory determination system, electronic equipment and storage medium | |
| Duan et al. | Image digital zoom based single target apriltag recognition algorithm in large scale changes on the distance | |
| Bai et al. | Research on obstacles avoidance technology for UAV based on improved PTAM algorithm | |
| Cui et al. | Landmark extraction and state estimation for UAV operation in forest | |
| Yang et al. | A semantic SLAM-based method for navigation and landing of UAVs in indoor environments | |
| CN114923477A (en) | Multi-dimensional space-ground collaborative map building system and method based on vision and laser SLAM technology |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |