CN112241178B - Unmanned aerial vehicle autonomous safety control method - Google Patents

Unmanned aerial vehicle autonomous safety control method Download PDF

Info

Publication number
CN112241178B
CN112241178B CN202011060931.2A CN202011060931A CN112241178B CN 112241178 B CN112241178 B CN 112241178B CN 202011060931 A CN202011060931 A CN 202011060931A CN 112241178 B CN112241178 B CN 112241178B
Authority
CN
China
Prior art keywords
radius
path
point
protection domain
unmanned aerial
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.)
Active
Application number
CN202011060931.2A
Other languages
Chinese (zh)
Other versions
CN112241178A (en
Inventor
李照宏
王毅
唐瑞卿
刘宇佳
陈根浦
纪海成
刘海亮
张颂
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chengdu Aircraft Industrial Group Co Ltd
Original Assignee
Chengdu Aircraft Industrial Group Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Chengdu Aircraft Industrial Group Co Ltd filed Critical Chengdu Aircraft Industrial Group Co Ltd
Priority to CN202011060931.2A priority Critical patent/CN112241178B/en
Publication of CN112241178A publication Critical patent/CN112241178A/en
Application granted granted Critical
Publication of CN112241178B publication Critical patent/CN112241178B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • 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

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 an autonomous safety control method for an unmanned aerial vehicle, which comprises the steps that when a planned path of the unmanned aerial vehicle passes through a protection area, if a straight path section is intersected with the protection area, a middle route point method is adopted to generate a homodromous rotation solution and a counter-rotation solution, and path re-planning is carried out according to the position of the unmanned aerial vehicle, a middle route point and a point to be flown; if the arc path section is intersected with the protection domain, calculating a point-avoiding protection domain tangent to the protection domain by adopting a method of adjusting the curvature of the moving arc; if the re-planned path is intersected with the arc section or the straight line section, a path avoiding the protection domain is generated through iteration. The unmanned aerial vehicle has the autonomous safety control handling capacity, and the regional protection of the preset target in the flight area and the autonomous judgment and handling of the flight boundary are realized. Once the unmanned aerial vehicle crosses the safety area or flies to the protection area due to runaway or other reasons, the control signal is automatically output according to a preset strategy, and the safety of ground equipment and personnel is ensured to the maximum extent.

Description

Unmanned aerial vehicle autonomous safety control method
Technical Field
The invention belongs to the technical field of safety control of aircrafts, and particularly relates to an autonomous safety control method for an unmanned aerial vehicle.
Background
In the flight process of the unmanned aerial vehicle, once the unmanned aerial vehicle fails to control, the unmanned aerial vehicle can form a great threat to surrounding targets, and even the unmanned aerial vehicle can cross the international border to cause great political and foreign traffic influences. Therefore, when the unmanned aerial vehicle executes a flight mission, the flight range of the unmanned aerial vehicle is limited in a safety area planned in advance according to the mission execution area, the external environment and the air route requirements of the unmanned aerial vehicle, and the selected area not only comprises the flight boundary of the flight area of the unmanned aerial vehicle, but also avoids protective areas such as a population dense area, important economic and military targets and the like as far as possible.
Once unmanned aerial vehicle loses control at the flight in-process, the crash is on ground or marine, will constitute the threat to personnel's life and property safety in peripheral area, if cross the border of a country, then probably cause abominable political influence. The unmanned aerial vehicle autonomous safety control method enables the unmanned aerial vehicle to have autonomous safety control (safety control for short) handling capacity, and achieves regional protection of a preset target in a navigation area and autonomous judgment and handling of a flight boundary. Once the unmanned aerial vehicle crosses the safety area or flies to the protection area due to runaway or other reasons, the control signal is automatically output according to a preset strategy, and the safety of ground equipment and personnel is ensured to the maximum extent.
In 2019, the Dode Gui provides an autonomous safety controller module design method, and autonomous judgment of a convex area is achieved by calculating cross multiplication of adjacent vectors which are arranged in sequence. In an actual airspace, the boundary line is not completely a convex area, and aiming at the research of judging and controlling the number of independent out-of-range boundaries of an airspace with concave boundaries, the invention provides an unmanned aerial vehicle independent safety control method.
Disclosure of Invention
The invention aims to provide an autonomous safety control method for an unmanned aerial vehicle, which enables the unmanned aerial vehicle to have autonomous safety control handling capacity and realizes regional protection of a preset target in a navigation area and autonomous judgment and handling of a flight boundary. Once the unmanned aerial vehicle crosses the safety area or flies to the protection area due to runaway or other reasons, the control signal is automatically output according to a preset strategy, and the safety of ground equipment and personnel is ensured to the maximum extent.
On the basis, supplementary research is carried out on the path intersection detection and avoidance with the non-flyable region in the boundary region. The design method of the autonomous security control module is suitable for multi-type airspaces, and has an early warning function, wherein the multi-type airspaces comprise concave-convex boundary domains, single-connected domains and multi-connected domains.
The invention is mainly realized by the following technical scheme: an unmanned aerial vehicle autonomous safety control method comprises the steps that when a planned path of an unmanned aerial vehicle passes through a protection area, an intersected path is judged to be a straight path or an arc path, if a straight path section is intersected with the protection area, a middle waypoint method is adopted to generate a homodromous rotation solution and a counter-rotation solution, and path re-planning is carried out according to the position of the unmanned aerial vehicle, a middle waypoint and a point to be flown; if the arc path section is intersected with the protection domain, calculating a point-avoiding protection domain tangent to the protection domain by adopting a method of adjusting the curvature of the moving arc; if the re-planned path is intersected with the arc section or the straight line section, a path avoiding the protection domain is generated through iteration.
In order to better realize the method, further, when the position of the unmanned aerial vehicle is positioned in the height interval of the protection domain, if the type attribute of the avoided object is cylindrical, the maximum radius of the protection domain is taken as the radius of the protection circle, otherwise, the superposition safety distance of the radius of the horizontal projection circle of the protection domain on the height plane of the unmanned aerial vehicle is taken as the radius of the protection circle; and taking the protective circle radius as a safe circle radius for path re-planning.
In order to better implement the present invention, further, the criterion of intersection of the straight path and the protection domain is as follows: the distance from the central point of the protection domain to the straight line path section is smaller than the radius of the protection domain, and at least one point of the intersection point is on the straight line path section.
In order to better realize the method, an intermediate waypoint algorithm is further adopted to generate intermediate waypoints, if the arc radius of the intermediate waypoints is smaller than the minimum turning radius, the minimum turning radius is taken as the radius of the re-planned path arc segment, otherwise, the arc radius of the intermediate waypoints is taken as the radius of the re-planned path arc segment.
In order to better implement the present invention, further, the criterion of the intersection of the circular arc path and the protection domain is as follows: the distance between the protection center and the circle center of the arc path section is less than the protection circle radius and the arc path radius, the distance between the protection center and the circle center of the arc path section is more than the protection circle radius-the arc path radius, and at least one intersection point is in the arc path section.
In order to better realize the method, an inner tangent solution and an outer tangent solution are generated by a curvature adjusting method, if the radius of the arc corresponding to the inner tangent solution is larger than the minimum turning radius, the radius of the inner tangent solution is the radius of the arc segment of the re-planned path, otherwise, if the radius corresponding to the arc of the outer tangent solution is larger than the minimum turning radius, the radius of the outer tangent solution is the radius of the arc segment of the re-planned path, and otherwise, the radius of the minimum turning radius is the radius of the arc segment of the re-planned path.
To better implement the present invention, further, the center of the protection domain is (x)b,yb) Curvature of kbRadius rb＝1/kbThe distance from the central point of the protective domain to the starting point of the straight line segment is d0The vertical projection line from the center point of the protective domain to the straight line segment is dn(ii) a The criterion of the intersection of the straight line segment and the protection domain is dn≤rbAnd p isipAnd pimFalling within the straight path segment.
In order to better realize the method, further, if the straight path is intersected with the protection domain, a method for generating an intermediate route point is adopted to calculate a point tangent to a safety circle of the protection domain; the starting point is PsEnd point is Pf，CsafeA security circle that is a protection domain; from PsTo PfThe path parameters to are r (t), r (t) and CsafeIntersect at an intersection point X1,X2，X1Is an entry point, X2To exit, make a line with X1,X2Straight line connecting vertically, cross-CsafeM, N, M, N is the newly generated intermediate waypoint;
the point of entry of the linear path is (x)px,ypx) The cut-off point is (x)pn,ypn) The unit direction vector of the straight path is
Figure BDA0002712387850000021
(xpx,ypx) And (x)3,4,y3,4) The unit direction vector of (a) is:
Figure BDA0002712387850000031
let thetana＝txn×txa,θnm＝txn×txm，
If thetana＞0,θnm< 0, point p located above the straight pathupIs (x)3,y3) Point p located below the straight pathdownIs (x)4,y4)，
If thetana＜0,θnm> 0, point p above the straight pathupIs (x)4,y4) Point p located below the straight pathdownIs (x)3,y3)，
If the unmanned aerial vehicle rotates clockwise along the initial position, the same-direction rotation solution pSYNIs pupSolution p by reverse rotationINVIs pdown，
If the unmanned aerial vehicle rotates anticlockwise along the initial position, the same-direction rotation is solved for pSYNIs pdownSolution p by reverse rotationINVIs pup。
To better implement the present invention, further, the center of the protection domain is (x)b,yb) Curvature of kbRadius rb＝1/kb(ii) a The center coordinate of the circular arc path is (x)c,yc) Curvature of kcRadius rc＝1/kcThe angle of the arc path is psicThe distance between the circle center of the arc segment and the protection circle center is
Figure BDA0002712387850000032
Note dcm＝dc-(rc+rb)，dcp＝dc+rb-rcThe criterion for the intersection of the arc segment and the protective circle is dc＜rc+rb，dc＞|rc-rbL, i.e. dcm＜0，dcpIs greater than 0 and the protection domain falls within the arc of motion.
In order to better realize the method, further, if the circular arc path is intersected with the protection domain, a point regulation protection domain tangent to the protection domain is calculated by adopting a method of adjusting the curvature of the moving circular arc;
noting the curvature of the guard domain as kbRadius of
Figure BDA0002712387850000033
Curvature of path arc kfRadius of
Figure BDA0002712387850000034
The target point is Pf(xf,yf) Distance between the center of circle of arc segment and the center of circle of protection
Figure BDA0002712387850000035
Distance between end position and protective circle center
Figure BDA0002712387850000036
The included angle from the connecting line of the circle center and the termination position of the arc segment to the connecting line of the termination position and the circle center of the protection domain
Figure BDA0002712387850000037
The maximum curvature solution and the minimum curvature solution are:
Figure BDA0002712387850000038
if the two groups of path solutions both meet the flight constraint, the shortest path is taken; if one group does not meet the flight constraint, the other group is selected; if two sets of all unsatisfied flight restraint, then turn with unmanned aerial vehicle minimum turning radius.
In order to better realize the invention, further, in order to realize the protection of the protection domain, an avoidance type variable ω is introduced, wherein ω is 1, represents on different flight level planes, the radii of avoidance circles are different, and are equal to the radius of an ellipsoid projected on the current flight level plane plus a safety distance; and on different flight height planes, the radius of the evasion circle is the same and is equal to the maximum radius of the ellipsoid projection plus the safety distance.
In order to better implement the invention, the position of the no-fly center is recorded as Pprot(x, y, h, r) the current height of the unmanned aerial vehicle is hnowω 1, projection circleRadius of (2)
Figure BDA0002712387850000041
Omega is-1, and the radius of the projection circle is rcir＝r。
In order to better realize the method, a ray scanning method is further adopted to judge the relation between the position of the unmanned aerial vehicle and a boundary map, the boundary map is assumed to be formed by irregular n-polygons, the counterclockwise direction is defined as positive, vertexes are arranged in the counterclockwise direction and are sequentially Pi(xi,yi) And i is 0,1,2, …, n-1, making a horizontal ray right through P (x, y), calculating the intersection points with the polygon boundary, wherein if the number of the intersection points is odd, the point is in the polygon, and if the number of the intersection points is even, the point is out of the polygon.
To better implement the present invention, further, the ray method is calculated as follows:
firstly, quickly removing points outside a rectangle, recording that A is {0,1,2, …, n-1}, n is more than or equal to 3,
Figure BDA0002712387850000042
if P (X, y) does not satisfy Xmin＜x＜Xmax,Ymin＜y＜YmaxIf so, the unmanned aerial vehicle crosses the boundary; if not, judging whether P (x, y) is a control vertex set P or not according to the distance between the point P (x, y) and each edge for the points in the matrixi(xi,yi) I is 0,1,2, …, the edge points and vertices of n-1; to pair
Figure BDA0002712387850000043
The formula for P (x, y) and the distance of each side is as follows:
Figure BDA0002712387850000044
if d satisfies d < epsilon, P (x, y) is a boundary point, and the unmanned aerial vehicle is out of range;
if i is 0,1,2, …, n-1, satisfy
Figure BDA0002712387850000045
P (x, y) is a vertex, and the unmanned aerial vehicle is out of range;
if P (x, y) is not a vertex or edge point, then a horizontal ray is taken to the right through P (x, y), the intersection with the polygon boundary is calculated,
if the number of the intersection points is an odd number, the point is in the polygon, and if the number of the intersection points is an even number, the point is out of the polygon;
to pair
Figure BDA0002712387850000046
If epsilon is arbitrarily small, j is 1,2, …, n-1, n, and | y is satisfiedj-1-yjIf | < epsilon, the number of the crossing points is 0,
if | x is satisfiedj-1-xjIf | < epsilon, there is an intersection point with the coordinate of xp＝xj-1,yp＝y，
If y is satisfiedj-1-yj|＞ε,|xj-1-xjIf | is greater than epsilon, then there is an intersection point with the coordinates:
Figure BDA0002712387850000051
if P (x, y) satisfies x < xp,min(yj-1,yj)≤y≤max(yj-1,yj) And if the intersection points are not polygon vertexes, adding 1 to the number of the intersection points, if the above formula is satisfied and the P (x, y) intersection points are polygon vertexes, adding 2 to the number of the intersection points of the same side vertexes, and adding 1 to the number of the intersection points of the different side vertexes.
To better implement the present invention, further, the criterion of the same side and different side of the vertex is as follows:
note Pt＝(xp-x,yp-y),Ptt＝(xmod(j+1,n)-x,ymod(j+1,n)-y),Ptb＝(xj-1-x,yj-1-y)，
If satisfy (P)t×Ptt)(Pt×Ptb) If the cross point is more than 0, the cross point is on the same side, otherwise, the cross point is on the different side.
To better realizeNow, further, the present invention sets a central point of the protection domain as P (x, y), an aircraft position as P ' (x ', y '), a distance between the unmanned aerial vehicle and the protection domain as r, and a radius of the protection domain as r0The safety circle radius of the protection domain is r1；
If r-r1If the value is more than 0, the security circle does not enter the protection domain; if 0 < | r-r0|＜r1-r0Entering a safety circle of a protection domain and not entering the protection domain; if r-r0And (5) the number of the airplane entering a protection domain is less than or equal to 0.
The safety control steps are as follows:
firstly, automatically closing the area according to the bound waypoints, generating a warning line by the bound waypoints, and realizing the inward contraction and outward expansion of the warning line by adopting vector translation transformation or line segment translation transformation.
And secondly, realizing real-time intervention of a treatment program through ground station instructions.
And thirdly, equidistant inward contraction of warning lines of the unmanned aerial vehicle in any concave-convex polygonal area is performed to generate inward contracted boundaries, the position state of the unmanned aerial vehicle is judged by scanning the boundary lines, and corresponding state flag bits are set, so that the unmanned aerial vehicle has functions of boundary crossing warning and boundary crossing early warning. And according to the state flag, automatically outputting a control signal by a preset strategy, and controlling the unmanned aerial vehicle to execute a disposal program.
And fourthly, introducing an avoidance type variable to represent radius parameters of avoidance circles on planes with different flight heights, and realizing fixed parameter avoidance and variable parameter avoidance on the protected object.
And fifthly, realizing intersection detection and evasion path planning of different types of planned paths and protection domains according to the types of the original planned paths.
The invention comprises region management, strategy design and algorithm design. The area management divides the no-fly area, defines the concepts of internal no-fly and external no-fly, and realizes the variable parameter autonomous avoidance of planes with different heights by introducing avoidance type parameters for an internal no-fly area model. The strategy design provides a handling principle of out-of-range early warning or alarm. The algorithm design comprises an early warning line generation, border crossing judgment, intersection detection and avoidance algorithm. The early warning function is realized by the contraction of polygons, the boundary crossing judgment is realized by a ray scanning method, and the design method provided by the invention is reasonable and feasible for internal simulation verification, and basically meets the safety control requirements of most small unmanned aerial vehicles.
1. Area management
The unmanned aerial vehicle security control area comprises a flight navigation area and a no-fly area, the flight navigation area is the largest flyable area in a normal state, and the no-fly area comprises an internal no-fly area and an external no-fly area.
As shown in fig. 15, the safety control regions in the present invention are as follows:
a crash zone: a banded region formed by possible crash scatter points after the unmanned aerial vehicle executes the treatment procedure.
Warning lines: in the maximum safe flight area of the unmanned aerial vehicle, the vertex position information is completed by a ground control device through a bound route before flight, the route attribute is that the flight is forbidden in an external area, the route has 3-127 waypoints in total, each route has at least 3 waypoints, and the waypoint height is the absolute geographical height. The ground control equipment can maintain the air route data through remote control instructions, and the specific instructions comprise: binding waypoints, editing waypoints, inquiring routes and inquiring waypoints.
An early warning line: after binding warning line data, a closed warning line which shrinks for a certain distance towards the inner diameter is automatically generated so as to ensure the safe return flight of the unmanned aerial vehicle during the early warning.
Flight area: the maximum flyable region in the normal state.
Flight route: the airlines where the drone performs the mission, are stitched before the flight.
Internal no-fly domain: the radius of the ball covering the protected area is determined according to the specific conditions such as the distribution range of facilities to be avoided or the size of a residential area. The position information is completed by ground control equipment through a bound route before flying, the attribute of the route is that the internal area is prohibited from flying, the route has 1-127 route points, the height of the route point is absolute geographical height, each 1 route point represents 1 protection domain, and the longitude, the latitude, the height and the radius of the corresponding route point form a protection domain ball.
Protection domain: and taking the central point of the protection target as a sphere center, and obtaining the protection domain by the safety distance expanded outside the internal no-fly domain, wherein the radius of the sphere is larger than that of the internal no-fly domain.
In order to realize protection of the protection domain, the protection domain is usually wrapped by splicing a sphere or an ellipsoid or a plurality of spheres, and the protection of the protection domain is completed by collision avoidance of the unmanned aerial vehicle and a forbidden circle of the protection domain. In the actual flight process, because airspace restriction or unmanned aerial vehicle performance restriction, need fly at the projected circle that closes on of plane at the protection domain, to this kind of condition, introduce and avoid type variable ω, ω equals 1 and represents on different flight level planes, the radius of avoiding the circle is different, get the radius + safe distance of ellipsoid body projection on current flight level plane, ω equals-1 and represents on different flight level planes, the radius of avoiding the circle is the same, get the projected maximum radius + safe distance of ellipsoid body, safe distance receives unmanned aerial vehicle performance restriction.
The position of the flying-forbidden center is recorded as Pprot(x, y, h, r) the current height of the unmanned aerial vehicle is hnowω 1, radius of the projected circle
Figure BDA0002712387850000071
Omega is-1, and the radius of the projection circle is rcir＝r。
2. Control strategy design
The autonomous safety control judges the relation between the current position of the unmanned aerial vehicle and a safety control area in real time according to the positioning information, determines the safety control state, meets the execution condition or receives the instruction of a ground station, automatically outputs a control signal according to a preset strategy, and controls the unmanned aerial vehicle to crash in a crash area.
The design of the safety control strategy needs to meet a certain autonomous disposal principle, when the takeoff position of the unmanned aerial vehicle is usually located in a safe area, and aiming at the condition that the takeoff position may be located outside a protection area or a warning line, before the takeoff position of the unmanned aerial vehicle enters the safe area, autonomous safety control is not involved, and an executing mechanism is prevented from making wrong actions. In the flight process of the unmanned aerial vehicle, judging whether the unmanned aerial vehicle crosses a boundary line and the relation between the path from the unmanned aerial vehicle to a point to be flown and a protection target in real time, and if the unmanned aerial vehicle crosses an early warning line, setting an early warning line crossing warning state as warning; if the alarm line is crossed, setting the alarm state of crossing the alarm line as alarm; if the unmanned aerial vehicle path intersects with the protection area, an intersection detection state is set to be intersection.
In order to improve the reliability of the safety control module, the safety control module is monitored and interrupted in real time, a ground station sets a safety control instruction, when the safety control instruction is not started, the autonomous safety control only executes safety control judgment and downloads a marker bit, when the safety control instruction is started, safety control disposal logic is accessed, and the unmanned aerial vehicle is controlled to execute a safety control program according to a preset strategy after triggering a safety control area. The response condition of the safety control instruction is that a valid ' pre-instruction ' + ' safety control instruction is received, the ' safety control permission ' state bit is set to be permitted for odd times when the execution content is that the valid instruction is received, and the ' safety control permission ' state bit is set to be prohibited for even times.
As shown in fig. 6, the criterion when the unmanned aerial vehicle crosses the early warning line is that the control mode is in program control or manual correction, and the unmanned aerial vehicle crosses the early warning line. The disposal strategy depends on the security flag state. When the safety control mark is that safety control is forbidden, the treatment is not carried out; and when the safety control mark is autonomous safety control, if the flight stage is located in the navigation stage, the instruction flight stage and the emergency return stage, executing an emergency return program, and not disposing other stages. If the unmanned aerial vehicle is out of control, the pilot can be in attitude-to-attitude remote control according to the situation.
As shown in fig. 7, when the unmanned aerial vehicle crosses the warning line, the criterion is that the control mode is in program control or manual correction, and the unmanned aerial vehicle crosses the warning line. The disposal strategy depends on the security flag state. When the safety control mark is that safety control is forbidden, the treatment is not carried out; and when the safety control mark is autonomous safety control, if the flight stage is in a navigation stage, an instruction flight stage and an emergency return stage, executing a forced landing program, and not disposing other flight stages. If the unmanned aerial vehicle is out of control, the pilot can be in attitude-to-attitude remote control according to the situation.
When the planned path of the unmanned aerial vehicle passes through the protection area, path re-planning is carried out by judging whether the intersected path is a straight path or an arc path, and the unmanned aerial vehicle is controlled to fly by the re-planned path so as to avoid the protection area. If the straight line path section is intersected with the protection domain, a middle waypoint method is adopted to generate a homodromous rotation solution and a counter-rotation solution, and path re-planning is carried out according to the position of the unmanned aerial vehicle, the middle waypoint and the point to be flown. If the arc path segment intersects with the protection domain, a maximum curvature path solution and a minimum curvature path solution are generated by adjusting the curvature of the intersecting arc. If the re-planned path is intersected with the arc section or the straight line section, a path avoiding the protection domain is generated through iteration.
As shown in fig. 8 and 9, the intersection of the drone path and the protection domain is handled as follows:
a) criterion
The intersection criterion of the straight path and the protection domain is as follows: the position information is effective in a program control or manual correction mode; the distance from the central point of the protection domain to the straight line path section is smaller than the radius of the protection domain, and at least one point of the intersection point is on the straight line path section.
The criterion of the intersection of the arc path and the protection domain is as follows: the position information is effective in a program control or manual correction mode; the distance between the protection center and the circle center of the arc path section is less than the protection circle radius and the arc path radius, the distance between the protection center and the circle center of the arc path section is more than the protection circle radius-the arc path radius, and at least one intersection point is in the arc path section.
b) Disposal of
1) The position of the unmanned aerial vehicle does not fall within the height interval of the protection domain and is not dealt with;
2) unmanned aerial vehicle position is located within range of height of protection field
i. The unmanned aerial vehicle is used for determining the height of the intersection moment and level flying;
if the type attribute of the avoided object is cylindrical, taking the maximum radius of the protection domain as the radius of a protection circle, and otherwise, taking the superposition safety distance of the radius of the horizontal projection circle of the protection domain on the height plane of the unmanned aerial vehicle as the radius of the protection circle;
and iii, taking the radius of the protection circle as the radius of a safety circle used for re-planning the path, adopting an intermediate waypoint algorithm to generate an intermediate waypoint when the straight path intersects with the protection domain, if the radius of an arc where the intermediate waypoint is located is smaller than the minimum turning radius, taking the minimum turning radius as the radius of an arc section of the re-planned path, and if not, taking the radius of the arc where the intermediate waypoint is located as the radius of the arc section of the re-planned path. When the circular arc path is intersected with the protection domain, generating an inner tangent solution (maximum curvature solution) and an outer tangent solution (minimum curvature solution) by adjusting a curvature method, wherein the circular arc radius corresponding to the inner tangent solution is larger than the minimum turning radius, taking the inner tangent solution radius as the circular arc section radius of the re-planned path, otherwise, taking the outer tangent solution radius as the circular arc section radius of the re-planned path if the radius corresponding to the outer tangent solution circular arc is larger than the minimum turning radius, and otherwise, taking the minimum turning radius as the circular arc section radius of the re-planned path;
and iv, meeting the path re-planning window or receiving an access path re-planning instruction sent by the ground station to generate a re-planning path and control the unmanned aerial vehicle to fly along the re-planning path.
c) Exit conditions
And exiting the path re-planning after the flying points are switched.
3. Algorithm design
The design of the safety control algorithm comprises the steps of generating an early warning line, judging boundary crossing, detecting intersection with a protection domain and avoiding.
1.1 early warning line Generation
The number of the top points of the alarm ring line is n, and the set of the top points is Pi(xi,yi) I is 0,1,2, …, n-1, the starting point and the end point are connected to form a closed polygon, the warning line is generated by adopting a geometric transformation method, and each side P of the warning line is connected with the other side P of the warning lineiPmod(i+1,n)Translating along the normal vector of the inner diameter by a distance L, wherein the translated edge is Pi'P′mod(i+1,n)The intersection point of two adjacent sides is Pi'(xi',yi'),PiThe' is the peak set of the safety control early warning line.
For j 2,3, …, N +1, an in-unit normal vector N for each edge of the polygon is calculatedv(j-1). By judging Nv(j-1) direction if NvAnd (j-1) taking an inverted vector as an external normal vector. The normal vector N in each edge unit of the polygonv(j-1) translating the distance L to generate a new edge, and calculating the intersection point of adjacent straight lines to obtain an early warning line vertex set.
1.2 out-of-range determination
The boundary map is assumed to be formed of irregular n-sided polygons, and the vertices are arranged counterclockwise and are sequentially Pi(xi,yi) I is 0,1,2, …, n-1, the position of the unmanned plane is P (x, y), and the position of the unmanned plane is judged by adopting a ray scanning methodThe relationship between the position and the boundary map. Making a horizontal ray to the right through P (x, y), calculating the intersection points with the polygon boundary, if the number of the intersection points is odd, the point is in the polygon, if the number of the intersection points is the
Even numbers, the points are outside the polygon.
1.3 intersection detection
Setting the central point position of the protection domain as P (x, y), the aircraft position as P ' (x ', y '), the distance between the unmanned aerial vehicle and the protection domain as r, and the radius of the protection domain as r0The safety circle radius of the protection domain is r1。
If r-r1If the value is more than 0, the security circle does not enter the protection domain; if 0 < | r-r0|＜r1-r0Entering a safety circle of a protection domain and not entering the protection domain; if r-r0And (5) the number of the airplane entering a protection domain is less than or equal to 0.
The circle center of the protection domain is marked as (x)b,yb) Protective Domain curvature of kbRadius of the protection domain is rb＝1/kbThe distance from the central point of the protective domain to the starting point of the straight line segment is d0The vertical projection line from the center point of the protective domain to the straight line segment is dn. The criterion of the intersection of the straight line segment and the protection domain is dn≤rbAnd p isipAnd pimFalling within the straight path segment.
The center of the protection domain is (x)b,yb) Curvature of kbRadius rb＝1/kbThe center coordinate of the circular arc path is (x)c,yc) Curvature of kcRadius rc＝1/kcThe angle of the arc path is psicThe distance between the circle center of the arc segment and the protection circle center is
Figure BDA0002712387850000091
Note dcm＝dc-(rc+rb)，dcp＝dc+rb-rcThe criterion for the intersection of the arc segment and the protective circle is dc＜rc+rb，dc＞|rc-rbL, i.e. dcm＜0，dcp> 0, and a protection domainFalling within the arc of motion.
1.4 circumvention Algorithm design
If the straight path intersects with the protection domain, calculating a point tangent to a safety circle of the protection domain by adopting a method for generating an intermediate route point.
Not to mark the starting point as PsEnd point is Pf，CsafeIs a security circle of the protection domain. From PsTo PfThe path parameters to are r (t), r (t) and CsafeIntersect at an intersection point X1,X2，X1Is an entry point, X2To exit, make a line with X1,X2Straight line connecting vertically, cross-CsafeAnd M, N, M and N are the newly generated intermediate waypoints.
The point of entry of the linear path is (x)px,ypx) The cut-off point is (x)pn,ypn) The unit direction vector of the straight path is
Figure BDA0002712387850000101
(xpx,ypx) And (x)3,4,y3,4) Has a unit direction vector of
Figure BDA0002712387850000102
Let thetana＝txn×txa,θnm＝txn×txmIf thetana＞0,θnm< 0, point p located above the straight pathupIs (x)3,y3) Point p located below the straight pathdownIs (x)4,y4) If thetana＜0,θnm> 0, point p above the straight pathupIs (x)4,y4) Point p located below the straight pathdownIs (x)3,y3). If the unmanned aerial vehicle rotates clockwise along the initial position, the same-direction rotation solution pSYNIs pupSolution p by reverse rotationINVIs pdownIf the unmanned aerial vehicle rotates anticlockwise along the initial position, the same-direction rotation is solved by pSYNIs pdownSolution p by reverse rotationINVIs pup。
If the arc path intersects with the protection domain, calculating a point rule avoidance protection domain tangent with the protection domain by adopting a method of adjusting the curvature of the moving arc.
Noting the curvature of the guard domain as kbRadius of
Figure BDA0002712387850000103
Curvature of path arc kfRadius of
Figure BDA0002712387850000104
The target point is Pf(xf,yf) Distance between the center of circle of arc segment and the center of circle of protection
Figure BDA0002712387850000105
Distance between end position and protective circle center
Figure BDA0002712387850000106
The included angle from the connecting line of the circle center and the termination position of the arc segment to the connecting line of the termination position and the circle center of the protection domain
Figure BDA0002712387850000107
The maximum and minimum curvature solutions are
Figure BDA0002712387850000108
If the two groups of path solutions both meet the flight constraint, the shortest path is taken; if one group does not meet the flight constraint, the other group is selected; if two sets of all unsatisfied flight restraint, then turn with unmanned aerial vehicle minimum turning radius.
The invention has the beneficial effects that:
(1) the unmanned aerial vehicle has the autonomous safety control handling capacity, and the regional protection of the preset target in the flight area and the autonomous judgment and handling of the flight boundary are realized.
(2) Once the unmanned aerial vehicle crosses the safety area or flies to the protection area due to runaway or other reasons, the control signal is automatically output according to a preset strategy, and the safety of ground equipment and personnel can be ensured to the greatest extent.
(3) In the actual airspace, the boundary line is not completely a convex domain, the invention judges the relationship between the position of the unmanned aerial vehicle and the boundary map by the ray method, and solves the technical problem of autonomous boundary-crossing judgment of the airspace with a concave boundary.
Drawings
FIG. 1 is a schematic view of a ray method;
FIG. 2 is a schematic diagram of a cross-border algorithm simulation;
FIG. 3 is a schematic diagram of the intersection of a protection domain with a straight path;
FIG. 4 is a schematic diagram of the intersection of a circular arc segment and a protection domain;
FIG. 5 is a schematic diagram of an intermediate waypoint algorithm;
FIG. 6 is a logic diagram of the over-the-precaution-line handling;
FIG. 7 is a logic diagram of the over-the-wire handling;
fig. 8 is a logic diagram of handling intersection of a straight path of an unmanned aerial vehicle and a protection domain;
FIG. 9 is a logic diagram of handling intersection of the arc path of the UAV and the protection domain;
fig. 10 is a schematic diagram of the unmanned aerial vehicle path not intersecting the protection domain;
FIG. 11 is a path re-plan of a straight path intersecting a protection domain;
FIG. 12 is a path re-plan of the intersection of a circular arc path and a protection domain;
FIG. 13 is a path track combining out-of-range and evasive protection domains;
FIG. 14 is an iterative path re-planning in which the protection domain intersects both a straight path and a circular path;
FIG. 15 is a schematic view of a safety control area of the present invention.
Detailed Description
Example 1:
an unmanned aerial vehicle autonomous safety control method comprises the steps that when a planned path of an unmanned aerial vehicle passes through a protection area, an intersected path is judged to be a straight path or an arc path, if a straight path section is intersected with the protection area, a middle waypoint method is adopted to generate a homodromous rotation solution and a counter-rotation solution, and path re-planning is carried out according to the position of the unmanned aerial vehicle, a middle waypoint and a point to be flown; if the arc path section is intersected with the protection domain, calculating a point-avoiding protection domain tangent to the protection domain by adopting a method of adjusting the curvature of the moving arc; as shown in fig. 14, if the re-planned path intersects with a circular arc segment or a straight line segment, a path avoiding the protection domain is generated through iteration.
Example 2:
the method is optimized on the basis of embodiment 1, when the position of the unmanned aerial vehicle is located in the height interval of the protection domain, if the type attribute of the avoided object is cylindrical, the maximum radius of the protection domain is taken as the radius of the protection circle, otherwise, the horizontal projection circle radius superposition safety distance of the protection domain on the height plane of the unmanned aerial vehicle is taken as the radius of the protection circle; and taking the protective circle radius as a safe circle radius for path re-planning.
Other parts of this embodiment are the same as embodiment 1, and thus are not described again.
Example 3:
in this embodiment, optimization is performed on the basis of embodiment 1 or 2, and the criterion for intersection between the straight path and the protection domain is as follows: the distance from the central point of the protection domain to the straight line path section is smaller than the radius of the protection domain, and at least one point of the intersection point is on the straight line path section.
And further, generating an intermediate waypoint by adopting an intermediate waypoint algorithm, if the arc radius of the intermediate waypoint is smaller than the minimum turning radius, taking the minimum turning radius as the radius of the re-planned path arc segment, and if not, taking the arc radius of the intermediate waypoint as the radius of the re-planned path arc segment.
Further, the center of the protection domain is (x)b,yb) Curvature of kbRadius rb＝1/kbThe distance from the central point of the protective domain to the starting point of the straight line segment is d0The vertical projection line from the center point of the protective domain to the straight line segment is dn(ii) a The criterion of the intersection of the straight line segment and the protection domain is dn≤rbAnd p isipAnd pimFalling within the straight path segment.
Further, as shown in fig. 11, if the straight path intersects with the protection domain, a point tangent to the safety circle of the protection domain is calculated by using a method of generating an intermediate route point; the starting point is PsEnd point is Pf，CsafeA security circle that is a protection domain; from PsTo PfThe path parameters to are r (t), r (t) and CsafeIntersect at an intersection point X1,X2，X1Is an entry point, X2To exit, make a line with X1,X2Straight line connecting vertically, cross-CsafeM, N, M, N is the newly generated intermediate waypoint;
the point of entry of the linear path is (x)px,ypx) The cut-off point is (x)pn,ypn) The unit direction vector of the straight path is
Figure BDA0002712387850000121
(xpx,ypx) And (x)3,4,y3,4) The unit direction vector of (a) is:
Figure BDA0002712387850000122
let thetana＝txn×txa,θnm＝txn×txm，
If thetana＞0,θnm< 0, point p located above the straight pathupIs (x)3,y3) Point p located below the straight pathdownIs (x)4,y4)，
If thetana＜0,θnm> 0, point p above the straight pathupIs (x)4,y4) Point p located below the straight pathdownIs (x)3,y3)，
If the unmanned aerial vehicle rotates clockwise along the initial position, the same-direction rotation solution pSYNIs pupSolution p by reverse rotationINVIs pdownIf the unmanned aerial vehicle rotates anticlockwise along the initial position, then the same asSolution p to rotationSYNIs pdownSolution p by reverse rotationINVIs pup。
The rest of this embodiment is the same as embodiment 1 or 2, and therefore, the description thereof is omitted.
Example 4:
in this embodiment, optimization is performed on the basis of embodiment 1 or 2, and the criterion for intersection between the arc path and the protection domain is as follows: the distance between the protection center and the circle center of the arc path section is less than the protection circle radius and the arc path radius, the distance between the protection center and the circle center of the arc path section is more than the protection circle radius-the arc path radius, and at least one intersection point is in the arc path section.
And further, generating an inner tangent solution and an outer tangent solution by a curvature adjusting method, if the radius of the arc corresponding to the inner tangent solution is larger than the minimum turning radius, taking the inner tangent solution radius as the radius of the arc segment of the re-planned path, otherwise, if the radius corresponding to the outer tangent solution arc is larger than the minimum turning radius, taking the outer tangent solution radius as the radius of the arc segment of the re-planned path, and otherwise, taking the minimum turning radius as the radius of the arc segment of the re-planned path.
Further, the center of the protection domain is (x)b,yb) Curvature of kbRadius rb＝1/kb(ii) a The center coordinate of the circular arc path is (x)c,yc) Curvature of kcRadius rc＝1/kcThe angle of the arc path is psicThe distance between the circle center of the arc segment and the protection circle center is
Figure BDA0002712387850000131
Note dcm＝dc-(rc+rb)，dcp＝dc+rb-rcThe criterion for the intersection of the arc segment and the protective circle is dc＜rc+rb，dc＞|rc-rbL, i.e. dcm＜0，dcpIs greater than 0 and the protection domain falls within the arc of motion.
Further, as shown in fig. 12, if the arc path intersects with the protection domain, a method of adjusting the curvature of the moving arc is used to calculate a point-avoiding protection domain tangent to the protection domain;
noting the curvature of the guard domain as kbRadius of
Figure BDA0002712387850000132
Curvature of path arc kfRadius of
Figure BDA0002712387850000133
The target point is Pf(xf,yf) Distance between the center of circle of arc segment and the center of circle of protection
Figure BDA0002712387850000134
Distance between end position and protective circle center
Figure BDA0002712387850000135
The included angle from the connecting line of the circle center and the termination position of the arc segment to the connecting line of the termination position and the circle center of the protection domain
Figure BDA0002712387850000136
The maximum curvature solution and the minimum curvature solution are:
Figure BDA0002712387850000141
if the two groups of path solutions both meet the flight constraint, the shortest path is taken; if one group does not meet the flight constraint, the other group is selected; if two sets of all unsatisfied flight restraint, then turn with unmanned aerial vehicle minimum turning radius.
The rest of this embodiment is the same as embodiment 1 or 2, and therefore, the description thereof is omitted.
Example 5:
in this embodiment, optimization is performed on the basis of any one of embodiments 1 to 4, as shown in fig. 13, in order to implement protection of a protection domain, an avoidance type variable ω is introduced, where ω is 1, which indicates that on different flight height planes, radii of avoidance circles are different and equal to a radius + a safety distance of an ellipsoid projected on a current flight height plane; and on different flight height planes, the radius of the evasion circle is the same and is equal to the maximum radius of the ellipsoid projection plus the safety distance.
Further, the no-fly center position is recorded as Pprot(x, y, h, r) the current height of the unmanned aerial vehicle is hnowω 1, radius of the projected circle
Figure BDA0002712387850000142
Omega is-1, and the radius of the projection circle is rcir＝r。
Other parts of this embodiment are the same as any of embodiments 1 to 4, and thus are not described again.
Example 6:
in this embodiment, optimization is performed on the basis of any one of embodiments 1 to 5, and a ray scanning method is used to determine the relationship between the position of the unmanned aerial vehicle and a boundary map, and the boundary map is assumed to be formed by irregular n-polygons, wherein the boundary map is defined to be positive counterclockwise, vertexes are arranged in the counterclockwise direction, and the vertexes are sequentially Pi(xi,yi) And i is 0,1,2, …, n-1, making a horizontal ray right through P (x, y), calculating the intersection points with the polygon boundary, wherein if the number of the intersection points is odd, the point is in the polygon, and if the number of the intersection points is even, the point is out of the polygon.
Further, the ray method is calculated as follows:
firstly, quickly removing points outside a rectangle, recording that A is {0,1,2, …, n-1}, n is more than or equal to 3,
Figure BDA0002712387850000143
if P (X, y) does not satisfy Xmin＜x＜Xmax,Ymin＜y＜YmaxIf so, the unmanned aerial vehicle crosses the boundary; if not, judging whether P (x, y) is a control vertex set P or not according to the distance between the point P (x, y) and each edge for the points in the matrixi(xi,yi) I is 0,1,2, …, the edge points and vertices of n-1; to pair
Figure BDA0002712387850000144
Formula of P (x, y) and distance between each sideThe following were used:
Figure BDA0002712387850000151
if d satisfies d < epsilon, P (x, y) is a boundary point, and the unmanned aerial vehicle is out of range;
if i is 0,1,2, …, n-1, satisfy
Figure BDA0002712387850000152
P (x, y) is a vertex, and the unmanned aerial vehicle is out of range; if P (x, y) is not a vertex or an edge point, a horizontal ray is taken rightwards through P (x, y), intersection points with the boundaries of the polygon are calculated, if the number of the intersection points is an odd number, the point is inside the polygon, and if the number of the intersection points is an even number, the point is outside the polygon;
to pair
Figure BDA0002712387850000153
If epsilon is arbitrarily small, j is 1,2, …, n-1, n, and | y is satisfiedj-1-yjIf | < epsilon, the number of the crossing points is 0, if | x is satisfiedj-1-xjIf | < epsilon, there is an intersection point with the coordinate of xp＝xj-1,yp＝y，
If y is satisfiedj-1-yj|＞ε,|xj-1-xjIf | is greater than epsilon, then there is an intersection point with the coordinates:
Figure BDA0002712387850000154
if P (x, y) satisfies x < xp,min(yj-1,yj)≤y≤max(yj-1,yj) And if the intersection points are not polygon vertexes, adding 1 to the number of the intersection points, if the above formula is satisfied and the P (x, y) intersection points are polygon vertexes, adding 2 to the number of the intersection points of the same side vertexes, and adding 1 to the number of the intersection points of the different side vertexes.
Further, the criterion of the same side and different side of the vertex is as follows:
note Pt＝(xp-x,yp-y),Ptt＝(xmod(j+1,n)-x,ymod(j+1,n)-y),Ptb＝(xj-1-x,yj-1-y)，
If satisfy (P)t×Ptt)(Pt×Ptb) If the cross point is more than 0, the cross point is on the same side, otherwise, the cross point is on the different side.
Other parts of this embodiment are the same as any of embodiments 1 to 5, and thus are not described again.
Example 7:
the present embodiment is optimized on the basis of any one of embodiments 1 to 6, and the central point of the protection domain is set to be P (x, y), the aircraft position is set to be P ' (x ', y '), the distance from the unmanned aerial vehicle to the protection domain is set to be r, and the radius of the protection domain is set to be r0The safety circle radius of the protection domain is r1(ii) a If r-r1If the value is more than 0, the security circle does not enter the protection domain; if 0 < | r-r0|＜r1-r0Entering a safety circle of a protection domain and not entering the protection domain; if r-r0And (5) the number of the airplane entering a protection domain is less than or equal to 0.
Other parts of this embodiment are the same as any of embodiments 1 to 6, and thus are not described again.
Example 8:
an autonomous safety control method for an unmanned aerial vehicle, fig. 1 is a schematic diagram of a ray method, and the ray method is calculated as follows:
firstly, quickly removing points outside a rectangle, recording that A is {0,1,2, …, n-1}, n is more than or equal to 3,
Figure BDA0002712387850000161
if P (X, y) does not satisfy Xmin＜x＜Xmax,Ymin＜y＜YmaxIf so, the unmanned aerial vehicle crosses the boundary; if not, judging whether P (x, y) is a control vertex set P or not according to the distance between the point P (x, y) and each edge for the points in the matrixi(xi,yi) I is 0,1,2, …, and n-1. To pair
Figure BDA0002712387850000162
The formula for P (x, y) and the distance of each side is as follows:
Figure BDA0002712387850000163
if d satisfies d < epsilon, P (x, y) is a boundary point, and the unmanned aerial vehicle is out of range. If i is 0,1,2, …, n-1, satisfy
Figure BDA0002712387850000164
Then P (x, y) is the vertex and the drone is out of range. If P (x, y) is not a vertex or edge point, then a horizontal ray is taken through P (x, y) to the right to calculate the intersection points with the polygon boundary, if the number of intersection points is odd, the point is inside the polygon, if the number of intersection points is even, the point is outside the polygon. To pair
Figure BDA0002712387850000165
If epsilon is arbitrarily small, j is 1,2, …, n-1, n, and | y is satisfiedj-1-yjIf | < epsilon, the number of the crossing points is 0, if | x is satisfiedj-1-xjIf | < epsilon, there is an intersection point with the coordinate of xp＝xj-1,ypIf y satisfies | yj-1-yj|＞ε,|xj-1-xjIf | > epsilon, there is an intersection point with the coordinates of the intersection point being
Figure BDA0002712387850000166
If P (x, y) satisfies x < xp,min(yj-1,yj)≤y≤max(yj-1,yj) And if the intersection points are not polygon vertexes, adding 1 to the number of the intersection points, if the above formula is satisfied and the P (x, y) intersection points are polygon vertexes, adding 2 to the number of the intersection points of the same side vertexes, and adding 1 to the number of the intersection points of the different side vertexes.
The criterion of the same side and different side of the vertex is as follows.
Note Pt＝(xp-x,yp-y),Ptt＝(xmod(j+1,n)-x,ymod(j+1,n)-y),Ptb＝(xj-1-x,yj-1-y), if (P) is satisfiedt×Ptt)(Pt×Ptb) If greater than 0, thenThe intersection points are on the same side, otherwise, the intersection points are on the different side.
FIG. 2 is a schematic diagram of a simulation of the boundary crossing algorithm. The outer dotted line is a warning line, the inner dotted line is an early warning line, the black frame line is a normal air line, the position of the unmanned aerial vehicle in a random sequence is input, the black circle indicates that the unmanned aerial vehicle does not cross the boundary, the magenta circle indicates that the unmanned aerial vehicle is outside the early warning line, and the red circle indicates that the unmanned aerial vehicle is outside the warning line.
FIG. 3 is a schematic diagram of intersection of a protection domain and a straight line path, where the criterion of intersection of the straight line path and a no-fly circle is dn≤rbAnd the point of intersection pipAnd pimFalling within the straight path segment. The method of determining that the intersection point falls on the straight path is as follows.
Recording the coordinates of the secondary point of the linear path as Px:(xpx,ypx) To a point coordinate of Pn:(xpn,ypn) The circle center of the no-fly ring is Pb:(xb,yb)。
a) Calculating the distance from the center of the no-fly ring to the straight line segment
Figure BDA0002712387850000171
b) Calculating the intersection point of the no-fly ring and the straight line segment
1) Straight-line path slope does not exist
If dn≤rbThe coordinates of the intersection point are
Figure BDA0002712387850000172
If dn＞rbThe protection domain does not intersect the straight line.
2) Straight-line path slope exists
Let Δ be b2-4ac, wherein
b＝2bA-2Ayb-2xb,a＝1+A2,c＝xb 2-rb 2+(b-yb)2,
Figure BDA0002712387850000173
If Δ.gtoreq.0 and dn≤rbIf the protection domain has an intersection point with the straight line, then
u＝-b2-2Abxb+2byb+A2rb 2-A2xb 2+2Axbyb+rb 2-yb 2，
The coordinates of the intersection point are
Figure BDA0002712387850000174
y1,2＝Ax1,2+b
If Δ < 0 or dn＞rbThe protection domain does not intersect the straight line.
c) Intersection detection
XMIN＝min(x1,2),XMAX＝max(x1,2),xmin＝min(xpx,xpn),xmax＝max(xpx,xpn) If it satisfies
XMIN＞xmaxOr XMAX＜xminThe straight line path does not pass through the protection domain, if XMIN＞xmin，XMAX＜xmax，
And the flying height is positioned in the height interval of the protection domain, and the straight path is intersected with the protection domain.
FIG. 4 is a schematic diagram of the intersection of a circular arc segment and a protection domain, wherein a counterclockwise unit vector toaAnd clockwise unit vector tocRepresenting a straight line d from the centre of the arc segment to the intersection with the protection fieldoaAnd docDirection of (1), tapAnd tamRepresents the unit vector of the radius boundary of the arc segment, and let i equal to [ p m ]]，j＝[ac]The method comprises the following steps:
sinθij＝|tai×toj|，cosθij＝tai·toj，
Figure BDA0002712387850000181
the criterion of the intersection of the motion arc segment and the protection domain is dc＜rc+rb，dc＞|rc-rbL, i.e. dcm＜0，dcpIs greater than 0, and at least one intersection point vector falls within the motion arc segment. The method for judging that the protection domain falls in the motion arc segment is as follows.
a) Calculating the intersection point P of the protection domain and the circular arc path1，2
Figure BDA0002712387850000182
y1，2＝Ax1，2+b
Wherein
Figure BDA0002712387850000183
u＝-b2+rb 2+A2rb 2-2Abxb-A2xb 2+2byb+2Axbyb-yb 2
b) Calculating a unit vector toa、toc、tap、tam
Circle center of arc segment to P1，2Unit vector t ofoa、tocThe following were used:
Figure BDA0002712387850000184
the point of entry of the arc segment is recorded as (x)p，yp) The arc segment cut-off point is (x)m，ym) Radius boundary vector t of arc segmentop，tomIs composed of
Figure BDA0002712387850000185
c) Calculating the arc entry point (x)p，yp) Cutting-out point (x)m，ym) And point of intersection P1，2Angle of (2)
θpam＝θpa+θma，θpcm＝θpc+θmc，
Wherein
θpa＝arc cos(toa·top)，θma＝arc cos(toa·tom)
θpc＝arc cos(toc·top)，θmc＝arc cos(toc·tom)
Note the book
pa＝toa×top,ma＝toa×tom,pc＝toc×top,mc＝toc×tom
pm_ac＝(pa,ma,pc,mc),θ＝(θpa,θma,θpc,θmc) If the pm _ ac is determined to be positive when the unmanned aerial vehicle rotates anticlockwise around the movement arc and the unmanned aerial vehicle rotates anticlockwise around the movement arciTheta is less than or equal to 0i＝2π-θiWhen the unmanned aerial vehicle rotates clockwise around the motion arc, if pm _ aciGreater than or equal to 0, then thetai＝2π-θi。
d) Intersection detection
If thetapam＝ψcOr thetapcm＝ψcWhen the flying height is within the height interval of the protection domain, the protection domain
Intersecting the arc of motion.
FIG. 5 is a schematic view of the intermediate waypoint method. The intermediate waypoints are calculated as follows.
a) Calculating the intersection of the straight path and the safety circle
b) Calculating the intersection point of the straight line perpendicular to the intersection line and the safety circle
Note the book
Figure BDA0002712387850000191
If the slope of the straight line path does not exist, then there is
x3,4＝x0±rb,y3,4＝y0
If the slope of the straight line path is 0
x3,4＝x0,y3,4＝y0±rb
If the slope of the straight line path exists, the straight line X is recorded1X2Has a slope of k0The slope of the straight line MN
Figure BDA0002712387850000192
u＝k2rb 2-k2x0 2+2k2x0xb-k2xb 2+2kx0y0-2kx0yb-2kxby0+2kxbyb+rb 2-y0 2+2y0yb-yb 2Then
Figure BDA0002712387850000193
y3,4＝k(x3,4-x0)+y0
Fig. 10 is a schematic diagram of the unmanned aerial vehicle path not intersecting the protection domain, and for the case where the unmanned aerial vehicle path does not intersect the protection domain, path re-planning is not enabled.
The above description is only a preferred embodiment of the present invention, and is not intended to limit the present invention in any way, and all simple modifications and equivalent variations of the above embodiments according to the technical spirit of the present invention are included in the scope of the present invention.

Claims (14)

1. An unmanned aerial vehicle autonomous safety control method is characterized in that when a planned path of an unmanned aerial vehicle passes through a protection area, an intersected path is judged to be a straight path or an arc path, if a straight path section is intersected with the protection area, a middle route point method is adopted to generate a homodromous rotation solution and a counter-rotation solution, and path re-planning is carried out according to the position of the unmanned aerial vehicle, a middle route point and a point to be flown; if the arc path section is intersected with the protection domain, calculating a point-avoiding protection domain tangent to the protection domain by adopting a method of adjusting the curvature of the moving arc; if the re-planned path is intersected with the arc section or the straight line section, generating a path avoiding the protection domain through iteration;
adopting a ray scanning method to judge the relation between the position of the unmanned aerial vehicle and a boundary map, assuming that the boundary map is formed by irregular n polygons, wherein the specified anticlockwise direction is positive, the vertexes are arranged according to the anticlockwise direction and are sequentially Pi(xi,yi) I is 0,1,2, …, n-1, making a horizontal ray right through P (x, y), calculating the intersection point with the polygon boundary, if the number of the intersection points is odd, the point is inside the polygon, if the number of the intersection points is even, the point is outside the polygon;
the ray method is calculated as follows:
firstly, quickly removing points outside a rectangle, recording that A is {0,1,2, …, n-1}, n is more than or equal to 3,
Figure FDA0003332099540000011
if P (X, y) does not satisfy Xmin＜x＜Xmax,Ymin＜y＜YmaxIf so, the unmanned aerial vehicle crosses the boundary; if not, judging whether P (x, y) is a control vertex set P or not according to the distance between the point P (x, y) and each edge for the points in the matrixi(xi,yi) I is 0,1,2, …, the edge points and vertices of n-1; to pair
Figure FDA0003332099540000012
The formula for P (x, y) and the distance of each side is as follows:
Figure FDA0003332099540000013
if d satisfies d < epsilon, P (x, y) is a boundary point, and the unmanned aerial vehicle is out of range;
if i is 0,1,2, …, n-1, satisfy
Figure FDA0003332099540000014
P (x, y) is a vertex, and the unmanned aerial vehicle is out of range;
if P (x, y) is not a vertex or an edge point, a horizontal ray is taken rightwards through P (x, y), intersection points with the boundaries of the polygon are calculated, if the number of the intersection points is an odd number, the point is inside the polygon, and if the number of the intersection points is an even number, the point is outside the polygon;
to pair
Figure FDA0003332099540000022
If epsilon is arbitrarily small, j is 1,2, …, n-1, n, and | y is satisfiedj-1-yjIf | < epsilon, the number of the crossing points is 0,
if | x is satisfiedj-1-xjIf | < epsilon, there is an intersection point with the coordinate of xp＝xj-1,yp＝y，
If y is satisfiedj-1-yj|＞ε,|xj-1-xjIf | is greater than epsilon, then there is an intersection point with the coordinates:
Figure FDA0003332099540000021
if P (x, y) satisfies x < xp,min(yj-1,yj)≤y≤max(yj-1,yj) And if the intersection points are not polygon vertexes, adding 1 to the number of the intersection points, if the above formula is satisfied and the P (x, y) intersection points are polygon vertexes, adding 2 to the number of the intersection points of the same side vertexes, and adding 1 to the number of the intersection points of the different side vertexes.
2. The autonomous safety control method for the unmanned aerial vehicle according to claim 1, wherein when the position of the unmanned aerial vehicle is within the altitude interval of the protection domain, if the type attribute of the avoided object is cylindrical, the maximum radius of the protection domain is taken as the radius of the protection circle, otherwise, the radius of the protection circle is taken as the radius of the protection circle superimposed by the safe distance of the horizontal projection circle of the protection domain on the altitude plane of the unmanned aerial vehicle; and taking the protective circle radius as a safe circle radius for path re-planning.
3. The autonomous safety control method of the unmanned aerial vehicle according to claim 1 or 2, wherein the criterion that the straight path intersects with the protection domain is as follows: the distance from the central point of the protection domain to the straight line path section is smaller than the radius of the protection domain, and at least one point of the intersection point is on the straight line path section.
4. The autonomous safety control method for the unmanned aerial vehicle as claimed in claim 3, wherein an intermediate waypoint algorithm is used to generate the intermediate waypoint, and if the arc radius of the intermediate waypoint is smaller than the minimum turning radius, the minimum turning radius is used as the radius of the re-planned path arc segment, otherwise, the arc radius of the intermediate waypoint is used as the radius of the re-planned path arc segment.
5. The autonomous safety control method of the unmanned aerial vehicle according to claim 1 or 2, wherein the criterion that the arc path intersects with the protection domain is as follows: the distance between the protection center and the circle center of the arc path section is less than the protection circle radius and the arc path radius, the distance between the protection center and the circle center of the arc path section is more than the protection circle radius-the arc path radius, and at least one intersection point is in the arc path section.
6. The autonomous safety control method for the unmanned aerial vehicle as claimed in claim 5, wherein the inner tangent solution and the outer tangent solution are generated by adjusting a curvature method, if the radius of the arc corresponding to the inner tangent solution is larger than the minimum turning radius, the radius of the arc segment of the re-planned path is the radius of the inner tangent solution, otherwise, if the radius corresponding to the arc corresponding to the outer tangent solution is larger than the minimum turning radius, the radius of the arc segment of the re-planned path is the radius of the outer tangent solution, and otherwise, the radius of the arc segment of the re-planned path is the minimum turning radius.
7. The autonomous safety control method for unmanned aerial vehicle of claim 3, wherein the circle center of the protection domain is (x)b,yb) Curvature of kbRadius rb＝1/kbThe distance from the central point of the protective domain to the starting point of the straight line segment is d0The vertical projection line from the center point of the protective domain to the straight line segment is dn(ii) a The criterion of the intersection of the straight line segment and the protection domain is dn≤rbAnd p isipAnd pimFalling within the straight path segment.
8. The autonomous safety control method of the unmanned aerial vehicle of claim 7, wherein if the straight path intersects the protection domain, a point tangent to a safety circle of the protection domain is calculated by a method of generating an intermediate waypoint; the starting point is PsEnd point is Pf，CsafeA security circle that is a protection domain; from PsTo PfThe path parameters to are r (t), r (t) and CsafeIntersect at an intersection point X1,X2，X1Is an entry point, X2To exit, make a line with X1,X2Straight line connecting vertically, cross-CsafeM, N, M, N is the newly generated intermediate waypoint;
the point of entry of the linear path is (x)px,ypx) The cut-off point is (x)pn,ypn) The unit direction vector of the straight path is
Figure FDA0003332099540000031
(xpx,ypx) And (x)3,4,y3,4) The unit direction vector of (a) is:
Figure FDA0003332099540000032
let thetana＝txn×txa,θnm＝txn×txm，
If thetana＞0,θnm< 0, point p located above the straight pathupIs (x)3,y3) Point p located below the straight pathdownIs (x)4,y4)，
If thetana＜0,θnm> 0, point p above the straight pathupIs (x)4,y4) Point p located below the straight pathdownIs (x)3,y3)，
If notWhen the man-machine rotates clockwise along the initial position, the solution p is rotated in the same directionSYNIs pupSolution p by reverse rotationINVIs pdown，
If the unmanned aerial vehicle rotates anticlockwise along the initial position, the same-direction rotation is solved for pSYNIs pdownSolution p by reverse rotationINVIs pup。
9. The unmanned aerial vehicle autonomous safety control method of claim 5,
the center of the protection domain is (x)b,yb) Curvature of kbRadius rb＝1/kb(ii) a The center coordinate of the circular arc path is (x)c,yc) Curvature of kcRadius rc＝1/kcThe angle of the arc path is psicThe distance between the circle center of the arc segment and the protection circle center is
Figure FDA0003332099540000041
Note dcm＝dc-(rc+rb)，dcp＝dc+rb-rcThe criterion for the intersection of the arc segment and the protective circle is dc＜rc+rb，dc＞|rc-rbL, i.e. dcm＜0，dcpIs greater than 0 and the protection domain falls within the arc of motion.
10. The autonomous safety control method of the unmanned aerial vehicle according to claim 9, wherein if the arc path intersects with the protection domain, a point regulation protection domain tangent to the protection domain is calculated by adjusting the curvature of the moving arc;
noting the curvature of the guard domain as kbRadius of
Figure FDA0003332099540000042
Curvature of path arc kfRadius of
Figure FDA0003332099540000043
The target point is Pf(xf,yf) Distance between the center of circle of arc segment and the center of circle of protection
Figure FDA0003332099540000044
Distance between end position and protective circle center
Figure FDA0003332099540000045
The included angle from the connecting line of the circle center and the termination position of the arc segment to the connecting line of the termination position and the circle center of the protection domain
Figure FDA0003332099540000046
The maximum curvature solution and the minimum curvature solution are:
Figure FDA0003332099540000047
if the two groups of path solutions both meet the flight constraint, the shortest path is taken; if one group does not meet the flight constraint, the other group is selected; if two sets of all unsatisfied flight restraint, then turn with unmanned aerial vehicle minimum turning radius.
11. The autonomous safety control method for the unmanned aerial vehicle as claimed in claim 1, wherein to achieve protection of the protection domain, an avoidance type variable ω is introduced, where ω is 1, and represents different flight altitude planes, and radii of avoidance circles are different and equal to a radius + a safety distance of an ellipsoid projected on a current flight altitude plane; and on different flight height planes, the radius of the evasion circle is the same and is equal to the maximum radius of the ellipsoid projection plus the safety distance.
12. The autonomous safety control method for unmanned aerial vehicle of claim 11, wherein the no-fly center position is recorded as Pprot(x, y, h, r) the current height of the unmanned aerial vehicle is hnowω 1, radius of the projected circle
Figure FDA0003332099540000051
Omega is-1, and the radius of the projection circle is rcir＝r。
13. The autonomous safety control method for the unmanned aerial vehicle according to claim 1, wherein the criterion that the vertexes are on the same side and on the different side is as follows:
note Pt＝(xp-x,yp-y),Ptt＝(xmod(j+1,n)-x,ymod(j+1,n)-y),Ptb＝(xj-1-x,yj-1-y), if (P) is satisfiedt×Ptt)(Pt×Ptb) If the cross point is more than 0, the cross point is on the same side, otherwise, the cross point is on the different side.
14. The autonomous safety control method for unmanned aerial vehicle according to claim 1, wherein the central point of the protection domain is P (x, y), the aircraft position is P ' (x ', y '), the distance from the unmanned aerial vehicle to the protection domain is r, and the radius of the protection domain is r0The safety circle radius of the protection domain is r1(ii) a If r-r1If the value is more than 0, the security circle does not enter the protection domain; if 0 < | r-r0|＜r1-r0Entering a safety circle of a protection domain and not entering the protection domain; if r-r0And (5) the number of the airplane entering a protection domain is less than or equal to 0.
CN202011060931.2A 2020-09-30 2020-09-30 Unmanned aerial vehicle autonomous safety control method Active CN112241178B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011060931.2A CN112241178B (en) 2020-09-30 2020-09-30 Unmanned aerial vehicle autonomous safety control method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011060931.2A CN112241178B (en) 2020-09-30 2020-09-30 Unmanned aerial vehicle autonomous safety control method

Publications (2)

Publication Number Publication Date
CN112241178A CN112241178A (en) 2021-01-19
CN112241178B true CN112241178B (en) 2022-01-25

Family

ID=74171827

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011060931.2A Active CN112241178B (en) 2020-09-30 2020-09-30 Unmanned aerial vehicle autonomous safety control method

Country Status (1)

Country Link
CN (1) CN112241178B (en)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113112873B (en) * 2021-03-29 2022-12-09 中琪华安(北京)科技有限公司 Airspace alarm method and device
CN113342045B (en) * 2021-06-20 2022-09-02 西北工业大学 Unmanned aerial vehicle autonomous avoidance navigation control method for any no-fly zone
CN113784284B (en) * 2021-09-01 2024-07-19 中国航空工业集团公司西安飞行自动控制研究所 Fixed wing unmanned aerial vehicle electronic fence avoidance method
CN116534278B (en) * 2023-07-07 2023-11-07 四川腾盾科技有限公司 Test flight planning method for verifying minimum spiral radius index of low-speed unmanned aerial vehicle
CN117130393B (en) * 2023-10-26 2024-01-26 成都时代星光科技有限公司 Unmanned aerial vehicle no-fly zone around-the-fly analysis method and system

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR2789771B1 (en) * 1999-02-12 2001-06-08 Sextant Avionique METHOD FOR GENERATING A HORIZONTAL PATH TO AVOID HAZARDOUS AREAS FOR AN AIRCRAFT
CN105717942B (en) * 2016-01-31 2018-07-06 中国人民解放军海军航空大学 A kind of unmanned vehicle space barrier-avoiding method and the online planing method of introductory path
CN106289233B (en) * 2016-07-25 2019-04-19 中国船舶重工集团公司第七0九研究所 The unmanned plane paths planning method and system of polymorphic obstacle
CN107085438B (en) * 2017-04-28 2020-02-07 中国船舶重工集团公司第七0九研究所 Unmanned aerial vehicle path correction method and system based on quasi-uniform spline curve
CN108387140B (en) * 2018-01-19 2020-05-05 北京航空航天大学 Analytic reentry guidance method considering multiple no-fly zone constraints
CN109814455A (en) * 2019-01-31 2019-05-28 拓攻（南京）机器人有限公司 A kind of no-fly control method, device, equipment and the storage medium of unmanned plane
CN109947123B (en) * 2019-02-27 2021-06-22 南京航空航天大学 Unmanned aerial vehicle path tracking and autonomous obstacle avoidance method based on sight guidance law
CN109828607B (en) * 2019-04-03 2020-07-07 南京航空航天大学 Unmanned aerial vehicle path planning method and system for irregular obstacles

Also Published As

Publication number Publication date
CN112241178A (en) 2021-01-19

Similar Documents

Publication Publication Date Title
CN112241178B (en) Unmanned aerial vehicle autonomous safety control method
CN109557942B (en) Unmanned aerial vehicle geo-fencing algorithm for autonomous flight
US10217369B2 (en) Method and system to improve safety concerning drones
CN107409174B (en) System and method for regulating operation of an unmanned aerial vehicle
CN107615785B (en) System and method for displaying geofence device information
CN113345271B (en) Method for detecting flight deviation of unmanned aerial vehicle
CN107533331B (en) Geo-fencing device with dynamic characteristics
CN107409051B (en) Authentication system and method for generating flight controls
CN107430403B (en) System and method with geo-fencing device level
CN107407915B (en) Authentication system and method for generating flight controls
CN107531324B (en) System and method for mobile geofencing
US11619953B2 (en) Three dimensional aircraft autonomous navigation under constraints
JP2018505089A (en) Supervisory safety system for control and restriction of unmanned aerial vehicle (UAS) maneuvers
JP2018509676A (en) System and method for managing flight paths of an autonomous airplane
US20230237917A1 (en) A Method and System for Controlling Flight Movements of Air Vehicles
US20180373243A1 (en) System and Method for Safe Utilization of Unmanned Automated Vehicles in Entertainment Venues
JP7146834B2 (en) Method and system for determining level of authorization for unmanned aerial vehicle (UAV) operation
CN114355967B (en) Aircraft, method for controlling an aircraft, and computer-aided system
US20240069575A1 (en) Adaptive anti-laser system
Ma et al. A Fast path re-planning method for UAV based on improved A* algorithm
Sláma et al. Risk-aware trajectory planning in urban environments with safe emergency landing guarantee
CN113701762A (en) Unmanned aerial vehicle search route establishing method and device
Pierpaoli et al. Altering UAV flight path by threatening collision
US20210025716A1 (en) Navigation based on multi-agent interest diffusion
Malaek et al. Optimal maneuvers for aircraft conflict resolution based on efficient genetic webs

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
GR01 Patent grant
GR01 Patent grant