Urban low-altitude unmanned aerial vehicle autonomous route deviation detection method

Abstract

The invention relates to an urban low-altitude unmanned aerial vehicle autonomous route deviation detection method. The method comprises the following steps: detecting deviation of a plurality of fixed airways and forming a deviation coordinate mapping relation between the airways and different coordinates of the unmanned aerial vehicle; the method comprises the following steps: (1) initializing an unmanned aerial vehicle, an air route deviation algorithm, starting point and target end point coordinates of each air route, and establishing latitude and longitude coordinates and height coordinates of a starting point of a local coordinate system of the unmanned aerial vehicle in a space coordinate system; calculating an ith air route space coordinate system where the current unmanned aerial vehicle is located by taking the current air route starting point as a coordinate origin; (2) taking the starting point of the current air route as a coordinate origin, setting the unmanned aerial vehicle to coincide with the direction of the current air route in the X axis, the Y axis and the Z axis, and calculating the coordinate values of the other two axes deviating from i air routes; and finally, integrating pitching, rolling and yawing three-axis changes of the unmanned aerial vehicles, and establishing a coordinate space deviation value of the ith air route, thereby avoiding collision danger when multiple unmanned aerial vehicles work at the same time.

Description

Technical Field

[0001] The invention relates to the technical field of directional transportation, and in particular to an autonomous route deviation detection method for urban low-altitude unmanned aerial vehicles. Background Art

[0002] With the development of drone technology, it is widely used in industries such as smart agriculture, rescue operations and logistics transportation. Compared with traditional manual operations, it has the advantages of high operation efficiency and intelligent process.

[0003] With the development of UAV technology in logistics and transportation, when a UAV deviates greatly during route transportation, it can automatically correct the deviation and return to the route. This can not only save energy costs, but also improve the efficiency of logistics and transportation distribution.

[0004] When a drone flies on a fixed route and deviates from the route by a large distance, there will be a risk of collision. Therefore, there is an urgent need for a deviation detection method that can detect the deviation from the route in time and intervene to compensate, thereby avoiding the occurrence of collision danger. Summary of the invention

[0005] In order to solve the above-mentioned problems, the present invention proposes a deviation detection method for a UAV under a fixed route, which is suitable for operating a UAV under a fixed route.

[0006] The present invention is achieved through the following technical solutions:

[0007] A method for detecting autonomous route deviation of an urban low-altitude UAV comprises the following steps:

[0008] S1. Initialize the drone, initialize the route deviation algorithm, initialize the coordinates of the starting point and target end point of each route; establish the latitude and longitude coordinates and altitude coordinates of the starting point of the drone's local coordinate system in the spatial coordinate system;

[0009] S2. Calculate the i-th route corresponding to the current moment of the UAV among the n routes according to the time of the flight plan; calculate the spatial coordinate system of the i-th route where the UAV is currently located, taking the starting point of the current route as the coordinate origin;

[0010] S3. Set the X-axis of the drone to coincide with the current route direction, and calculate the deviation values ​​of the Y-axis and Z-axis; set the Y-axis of the drone to coincide with the current route direction, and calculate the deviation values ​​of the X-axis and Z-axis; set the Z-axis of the drone to coincide with the current route direction, and calculate the deviation values ​​of the X-axis and Y-axis; when the drone undergoes changes in pitch, roll and yaw, establish the spatial deviation value of the i-th route.

[0011] Furthermore, the method specifically comprises the following steps:

[0012] (1) The current route is the i-th route, and the local coordinates of the starting point of the current route are set to (0,0,0);

[0013] Condition 1: Set the X-axis to coincide with the current route direction, with the forward direction as the positive direction; the Y-axis to be perpendicular to the current route X-axis direction, with the upward direction as the positive direction; the Z-axis to be perpendicular to the current route X-axis direction, with the direction perpendicular to the paper and outward as the positive direction;

[0014] Condition 2: Set the Y axis to coincide with the current route direction, with the forward direction as the positive direction; the X axis to be perpendicular to the Y axis of the current route, with the outward direction perpendicular to the paper as the positive direction; the Z axis to be perpendicular to the X axis of the current route, with the upward direction as the positive direction;

[0015] Condition 3: Set the Z axis to coincide with the current route direction, with the forward direction as the positive direction; the X axis to be perpendicular to the current route X axis direction, with the upward direction as the positive direction; the Y axis to be perpendicular to the current route X axis direction, with the direction perpendicular to the paper and outward as the positive direction;

[0016] (2) Save the latitude, longitude and altitude of the current route starting point and convert them into the coordinates corresponding to the local coordinate system (0,0,0);

[0017] (3) According to the coordinate position of the current route starting point, convert the current UAV latitude, longitude and altitude coordinate position.

[0018] Furthermore, based on condition 1, the drone moving forward in the positive direction of the X axis determines whether the current Y-axis and Z-axis headings deviate from the values; there are the following four situations:

[0019] The first case: when the current i routes are flying along the X axis, neither the Y axis nor the Z axis deviates;

[0020] The second situation: when the current i routes are flying along the X axis, only the Y axis deviates;

[0021] The third case: when the X axis of the current i routes is flying, only the Z axis deviates;

[0022] The fourth situation: when the X-axis of the current i routes is flying, both the Y-axis and the Z-axis deviate.

[0023] (1) Based on the second case of condition 1, the maximum MAX deviation among the following three methods is used as the final Y-axis deviation value:

[0024] Method 1: According to the Y-axis deviation, take the absolute value to obtain the deviation latitude and longitude / altitude coordinates, that is, read the average deviation of n points at the same time as:

[0025] Method 2: According to the deviation of the Y axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0026] Method 3: According to the Y-axis deviation, the distance from the point to the straight line is calculated to obtain the deviation from the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as: Based on the third case of condition one, the maximum MAX deviation among the following three methods is used as the final Z-axis deviation value:

[0027] Method 1: According to the deviation of the Z axis, take the absolute value to obtain the longitude and latitude / altitude coordinates, that is, read the average deviation of n points at the same time as:

[0028] Method 2: According to the deviation of the Z axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0029] Method 3: According to the deviation of the Z axis, the distance from the point to the straight line is calculated to obtain the deviation of the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as: Based on the fourth case of condition one, the maximum MAX deviation among the following three methods is used as the final Y and Z axis deviation value:

[0030] Method 1: According to the deviation of Y-axis and Z-axis, take the absolute value and use the Pythagorean theorem to calculate the deviation coordinates, that is, read the mean deviation of n points at the same time as:

[0031] Method 2: According to the deviation of the Y-axis and Z-axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0032] Method 3: According to the Y-axis and Z-axis offset, calculate the deviation value by the distance from the space point to the straight line, that is, read the mean deviation of n points at the same time as:

[0033] Among them, |y i | represents the absolute value of the deviation error of the position of the i-th flight coordinate point on the Y axis, Lxy represents the distance between the i-th point Y axis deviation and the take-off origin when flying in the X-axis direction, α represents the angle between the i-th point Y axis deviation and the take-off origin when flying in the X-axis direction, x i Represents the deviation value of the position of the i-th flight coordinate point on the X axis, y i Represents the deviation value of the i-th flight coordinate point position on the Y axis, A and B represent the parameters on the X axis and Y axis respectively, and C represents a constant.

[0034] |z i| represents the absolute value of the deviation error of the i-th flight coordinate point on the Z axis, Lxz represents the distance between the i-th point Z axis deviation and the takeoff origin when flying in the X axis direction, β represents the angle between the i-th point Z axis deviation and the takeoff origin when flying in the X axis direction, x i Represents the deviation value of the position of the i-th flight coordinate point on the X axis, z i Represents the deviation value of the i-th flight coordinate point position on the Z axis, A and C represent the parameters on the X axis and Z axis respectively, and D represents a constant.

[0035] Lxyz represents the distance between the Y-axis and Z-axis deviation of the i-th point and the take-off origin when flying in the X-axis direction.

[0036] Furthermore, based on condition 2, the drone moving forward in the positive direction of the Y axis determines whether the current X-axis and Z-axis headings deviate from the values; there are the following four situations:

[0037] The first case: when the Y axis of the current route i is flying, neither the X axis nor the Z axis deviates;

[0038] The second situation: when the Y axis of the current route i is flying, only the X axis deviates;

[0039] The third case: when the Y axis of the current i routes is flying, only the Z axis deviates;

[0040] The fourth situation: When the Y axis of the current i routes is flying, both the X axis and the Z axis deviate.

[0041] (1) Based on the second case of condition 2, the maximum MAX deviation among the following three methods is used as the final X-axis deviation value:

[0042] Method 1: According to the deviation of the X-axis, take the absolute value to obtain the deviation longitude / latitude / altitude coordinates, that is, read the average deviation of n points at the same time as:

[0043] Method 2: According to the deviation of the X-axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0044] Method 3: According to the X-axis deviation, the distance from the point to the straight line is calculated to obtain the deviation from the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as:

[0045] (2) Based on the third case of condition 2, the maximum MAX deviation among the following three methods is used as the final Z-axis deviation value:

[0046] Method 1: According to the deviation of the Z axis, take the absolute value to obtain the longitude and latitude / altitude coordinates, that is, read the average deviation of n points at the same time as:

[0047] Method 2: According to the deviation of the Z axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0048] Method 3: According to the deviation of the Z axis, the distance from the point to the straight line is calculated to obtain the deviation of the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as:

[0049] (3) Based on the fourth case of condition 2, the maximum MAX deviation among the following three methods is used as the final X and Z axis deviation value:

[0050] Method 1: According to the deviation of X-axis and Z-axis, take the absolute value and use the Pythagorean theorem to calculate the deviation coordinates, that is, read the mean deviation of n points at the same time as:

[0051] Method 2: According to the deviation of the X-axis and Z-axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0052] Method 3: According to the X-axis and Z-axis offset, calculate the deviation value by the distance from the space point to the straight line, that is, read the mean deviation of n points at the same time as:

[0053] Wherein, Lyx represents the distance between the X-axis deviation of the i-th point and the take-off origin when flying in the Y-axis direction, and γ represents the angle between the X-axis deviation of the i-th point and the take-off origin when flying in the Y-axis direction.

[0054] Lyz represents the distance between the Z-axis deviation of the i-th point and the take-off origin when flying in the Y-axis direction, and δ represents the angle between the Z-axis deviation of the i-th point and the take-off origin when flying in the Y-axis direction.

[0055] Lyxz represents the distance between the X-axis and Z-axis deviation of the i-th point and the take-off origin when flying in the Y-axis direction.

[0056] Furthermore, based on condition three, the drone moving forward in the positive direction of the Z axis determines whether the current X-axis and Y-axis headings deviate from the values; there are the following four situations:

[0057] The first case: when the current i routes are flying along the Z axis, neither the X axis nor the Y axis deviates;

[0058] The second situation: when the current i routes are flying along the Z axis, only the X axis deviates;

[0059] The third case: when the Z axis of the current i routes is flying, only the Y axis deviates;

[0060] The fourth situation: When the current i routes are flying along the Z axis, both the X axis and the Y axis deviate.

[0061] (1) Based on the second case of condition three, the maximum MAX deviation among the following three methods is used as the final X-axis deviation value:

[0062] Method 1: According to the deviation of the X-axis, take the absolute value to obtain the deviation longitude / latitude / altitude coordinates, that is, read the average deviation of n points at the same time as:

[0063] Method 2: According to the deviation of the X-axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0064] Method 3: According to the X-axis deviation, the distance from the point to the straight line is calculated to obtain the deviation from the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as:

[0065] (2) Based on the third case of condition three, the maximum MAX deviation among the following three methods is used as the final Y-axis deviation value:

[0066] Method 1: According to the deviation of the Y axis, take the absolute value to obtain the longitude and latitude / altitude coordinates, that is, read the average deviation of n points at the same time as:

[0067] Method 2: According to the deviation of the Y axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0068] Method 3: According to the Y-axis deviation, the distance from the point to the straight line is calculated to obtain the deviation from the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as:

[0069] (2) Based on the fourth case of condition three, the maximum MAX deviation among the following three methods is used as the final X-axis and Y-axis deviation value:

[0070] Method 1: According to the deviation of X-axis and Y-axis, take the absolute value and use the Pythagorean theorem to calculate the deviation coordinates, that is, read the mean deviation of n points at the same time as:

[0071] Method 2: According to the deviation of the X-axis and Y-axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0072] Method 3: According to the X-axis and Y-axis offset, calculate the deviation value by the distance from the space point to the straight line, that is, read the mean deviation of n points at the same time as:

[0073] Wherein, Lzx represents the distance between the X-axis deviation of the i-th point and the take-off origin when flying in the Z-axis direction, and η represents the angle between the X-axis deviation of the i-th point and the take-off origin when flying in the Z-axis direction.

[0074] Lzy represents the distance between the Y-axis deviation of the i-th point and the take-off origin when flying in the Z-axis direction, and θ represents the angle between the Y-axis deviation of the i-th point and the take-off origin when flying in the Z-axis direction.

[0075] Lzxy represents the distance between the X-axis and Y-axis deviation of the i-th point and the take-off origin when flying in the Z-axis direction.

[0076] Compared with the prior art, the present invention has the following beneficial effects:

[0077] The present invention is an autonomous route deviation detection method for urban low-altitude unmanned aerial vehicles. The method comprises the following steps: 100 initializing the unmanned aerial vehicle, initializing the route deviation algorithm, and initializing the coordinates of the starting point and the target end point of each route; 101 establishing the latitude and longitude coordinates and the altitude coordinates corresponding to the starting point of the local coordinate system of the unmanned aerial vehicle in the spatial coordinate system; 102 calculating the ith route corresponding to the unmanned aerial vehicle at the current moment among the n routes according to the time of the flight plan; 103 taking the starting point of the current route as the coordinate origin, calculating the spatial coordinate system of the ith route where the current unmanned aerial vehicle is located; 104 setting the unmanned aerial vehicle X-axis to coincide with the current route direction, and calculating the route deviation values ​​of the Y-axis and the Z-axis; 105 setting the unmanned aerial vehicle Y-axis to coincide with the current route direction, and calculating the route deviation values ​​of the X-axis and the Z-axis; 106 setting the unmanned aerial vehicle Z-axis to coincide with the current route direction, and calculating the route deviation values ​​of the X-axis and the Y-axis; and 107 establishing the spatial deviation value of the ith route when the unmanned aerial vehicle undergoes three-axis changes in pitch, roll and yaw. BRIEF DESCRIPTION OF THE DRAWINGS

[0078] Figure 1 It is a general diagram of the fixed route deviation detection framework of the present invention.

[0079] Figure 2 It is a mapping relationship diagram between the local coordinates of the route and the human-machine space coordinates of the present invention.

[0080] Figure 3 It is the design diagram of the route and the X-axis direction coincidence deviation and solution of the present invention.

[0081] Figure 4 It is the design diagram of the route and Y-axis direction coincidence deviation and solution of the present invention.

[0082] Figure 5It is the design diagram of the route and Z-axis direction coincidence deviation and solution of the present invention.

[0083] Figure 6 It is a top view of the UAV route operation of the present invention.

[0084] Figure 7 It is a deviation diagram of the UAV of the present invention when operating on a single route.

[0085] Figure 8 It is a deviation diagram of the UAV of the present invention when operating on multiple routes. DETAILED DESCRIPTION

[0086] The present invention will be further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0087] like Figure 1 As shown, the present invention provides a method for detecting autonomous route deviation of an urban low-altitude UAV, comprising the following steps:

[0088] 100. Initialize the UAV, initialize the route deviation algorithm, and initialize the coordinates of each route starting point and target end point;

[0089] 101. Establish the latitude, longitude and altitude coordinates of the starting point of the local coordinate system of the drone in the spatial coordinate system;

[0090] 102. Calculate the i-th route corresponding to the current moment of the drone among the n routes according to the time of the flight plan;

[0091] 103. Taking the starting point of the current route as the coordinate origin, calculate the spatial coordinate system of the i-th route where the current UAV is located;

[0092] 104. Set the X axis of the drone to coincide with the current route direction, and calculate the deviation values ​​of the Y and Z axes;

[0093] 105. Set the Y axis of the drone to coincide with the current route direction, and calculate the deviation values ​​of the X and Z axes;

[0094] 106. Set the Z axis of the drone to coincide with the current route direction, and calculate the deviation values ​​of the X and Y axes;

[0095] 107. When the UAV undergoes pitch, roll and yaw changes, the spatial deviation value of the i-th route is established.

[0096] like Figure 2 As shown, the specific steps are as follows:

[0097] 200. The current route is the i-th route, and the local coordinates of the starting point of the current route are set to (0,0,0);

[0098] 201. Set the X-axis to coincide with the current route direction, with the forward direction as the positive direction; the Y-axis to be perpendicular to the current route X-axis direction, with the upward direction as the positive direction; the Z-axis to be perpendicular to the current route X-axis direction, with the direction perpendicular to the paper and outward as the positive direction;

[0099] 202. Set the Y axis to coincide with the current route direction, with the forward direction as the positive direction; the X axis to be perpendicular to the Y axis of the current route, with the outward direction perpendicular to the paper as the positive direction; the Z axis to be perpendicular to the X axis of the current route, with the upward direction as the positive direction;

[0100] 203. Set the Z axis to coincide with the current route direction, with the forward direction as the positive direction; the X axis to be perpendicular to the current route X axis direction, with the upward direction as the positive direction; the Y axis to be perpendicular to the current route X axis direction, with the direction perpendicular to the paper and outward as the positive direction;

[0101] 204. Save the longitude, latitude and altitude of the current route starting point and convert them into coordinates corresponding to the local coordinate system (0,0,0); 205. Convert the longitude, latitude and altitude coordinate positions of the current drone according to the coordinate position of the current route starting point.

[0102] like Figure 3 As shown in the figure, the route and the X-axis direction coincidence deviation and solution include:

[0103] 300. The drone moves forward in the positive direction of the X axis, and determines whether the current Y axis and Z axis headings deviate from the values;

[0104] 301. The first case: when the current i routes are flying along the X axis, neither the Y axis nor the Z axis deviates;

[0105] 302. The second case: When the current i routes are flying along the X axis and only the Y axis deviates:

[0106] Method 1: According to the Y-axis deviation, take the absolute value to obtain the deviation latitude and longitude / altitude coordinates, that is, read the average deviation of n points at the same time as:

[0107] Method 2: According to the deviation of the Y axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0108] Method 3: According to the Y-axis deviation, the distance from the point to the straight line is calculated to obtain the deviation from the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as:

[0109] 303. For the second case above, the maximum MAX deviations of the three methods are collected as the final Y-axis deviation value;

[0110] 304. The third case: When the current i routes are flying along the X axis and only the Z axis deviates:

[0111] Method 1: According to the deviation of the Z axis, take the absolute value to obtain the longitude and latitude / altitude coordinates, that is, read the average deviation of n points at the same time as:

[0112] Method 2: According to the deviation of the Z axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0113] Method 3: According to the deviation of the Z axis, the distance from the point to the straight line is calculated to obtain the deviation of the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as:

[0114] 305. For the third case above, the maximum MAX deviations of the three methods are collected as the final Z-axis deviation value;

[0115] 306. The fourth case: When the current i routes are flying along the X axis. When both the Y axis and the Z axis deviate:

[0116] Method 1: According to the deviation of Y-axis and Z-axis, take the absolute value and use the Pythagorean theorem to calculate the deviation coordinates, that is, read the mean deviation of n points at the same time as:

[0117] Method 2: According to the deviation of the Y-axis and Z-axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0118] Method 3: According to the Y-axis and Z-axis offset, calculate the deviation value by the distance from the space point to the straight line, that is, read the mean deviation of n points at the same time as:

[0119] 307. For the fourth case above, the maximum MAX deviations of the three methods are collected as the final Y and Z axis deviation values.

[0120] like Figure 4 As shown in the figure, the route and Y-axis overlap deviation and solution include:

[0121] 310. The drone moves forward in the positive direction of the Y axis, and determines whether the current X-axis and Z-axis headings deviate from the values;

[0122] 311. The first case: when the Y axis of the current i routes is flying, neither the X axis nor the Z axis deviates;

[0123] 312. The second case: When the Y axis of the current i routes is flying. When only the X axis deviates:

[0124] Method 1: According to the deviation of the X-axis, take the absolute value to obtain the deviation longitude / latitude / altitude coordinates, that is, read the average deviation of n points at the same time as:

[0125] Method 2: According to the deviation of the X-axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0126] Method 3: According to the X-axis deviation, the distance from the point to the straight line is calculated to obtain the deviation from the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as:

[0127] 313. For the second case above, the maximum MAX deviation of the three methods is collected as the final X-axis deviation value;

[0128] 314. The third case: When the Y axis of the current i routes is flying. When only the Z axis deviates:

[0129] Method 1: According to the deviation of the Z axis, take the absolute value to obtain the longitude and latitude / altitude coordinates, that is, read the average deviation of n points at the same time as:

[0130] Method 2: According to the deviation of the Z axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0131] Method 3: According to the deviation of the Z axis, the distance from the point to the straight line is calculated to obtain the deviation of the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as:

[0132] 315. For the third case above, the maximum MAX deviations of the three methods are collected as the final Z-axis deviation value;

[0133] 316. The fourth case: When the Y axis of the current i routes is flying. When both the X axis and the Z axis deviate:

[0134] Method 1: According to the deviation of X-axis and Z-axis, take the absolute value and use the Pythagorean theorem to calculate the deviation coordinates, that is, read the mean deviation of n points at the same time as:

[0135] Method 2: According to the deviation of the X-axis and Z-axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0136] Method 3: According to the X-axis and Z-axis offset, calculate the deviation value by the distance from the space point to the straight line, that is, read the mean deviation of n points at the same time as:

[0137] 317. For the fourth case above, the maximum MAX deviations of the three methods are collected as the final X and Z axis deviation values.

[0138] like Figure 5 As shown in the figure, the route and Z-axis direction coincidence deviation and solution include:

[0139] 320. The drone moves forward in the positive direction of the Z axis, and determines whether the current X-axis and Y-axis headings deviate from the values;

[0140] 321. The first case: when the current i routes are flying along the Z axis, neither the X axis nor the Y axis deviates;

[0141] 322. The second case: When the current i routes are flying along the Z axis. When only the X axis deviates:

[0142] Method 1: According to the deviation of the X-axis, take the absolute value to obtain the deviation longitude / latitude / altitude coordinates, that is, read the average deviation of n points at the same time as:

[0143] Method 2: According to the deviation of the X-axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0144] Method 3: According to the X-axis deviation, the distance from the point to the straight line is calculated to obtain the deviation from the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as:

[0145] 323. For the second case above, the maximum MAX deviation of the three methods is collected as the final X-axis deviation value;

[0146] 234. The third case: When the current i routes are flying along the Z axis and only the Y axis deviates:

[0147] Method 1: According to the deviation of the Y axis, take the absolute value to obtain the longitude and latitude / altitude coordinates, that is, read the average deviation of n points at the same time as:

[0148] Method 2: According to the deviation of the Y axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0149] Method 3: According to the Y-axis deviation, the distance from the point to the straight line is calculated to obtain the deviation from the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as:

[0150] 325. For the third case above, the maximum MAX deviation of the three methods is collected as the final Y-axis deviation value;

[0151] 326. The fourth case: When the current i routes are flying along the Z axis. When both the X axis and the Y axis deviate:

[0152] Method 1: According to the deviation of X-axis and Y-axis, take the absolute value and use the Pythagorean theorem to calculate the deviation coordinates, that is, read the mean deviation of n points at the same time as:

[0153] Method 2: According to the deviation of the X-axis and Y-axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as:

[0154] Method 3: According to the X-axis and Y-axis offset, calculate the deviation value by the distance from the space point to the straight line, that is, read the mean deviation of n points at the same time as:

[0155] 327. For the fourth case above, the maximum MAX deviations of the three methods are collected as the final X and Y axis deviation values.

[0156] The above describes the specific embodiments of the invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art may make various modifications or variations within the scope of the claims, which do not affect the essence of the present invention.

Claims

1. A method for detecting autonomous route deviation of low-altitude urban UAVs, characterized in that: The following steps are included: S1. Initialize the drone, initialize the route deviation algorithm, initialize the coordinates of the starting point and target end point of each route; establish the latitude and longitude coordinates and altitude coordinates of the starting point of the drone's local coordinate system in the spatial coordinate system; S2. Calculate the i-th route corresponding to the current moment of the UAV among the n routes according to the time of the flight plan; calculate the spatial coordinate system of the i-th route where the UAV is currently located, taking the starting point of the current route as the coordinate origin; S3. Set the X-axis of the drone to coincide with the current route direction, and calculate the deviation values ​​of the Y-axis and Z-axis; set the Y-axis of the drone to coincide with the current route direction, and calculate the deviation values ​​of the X-axis and Z-axis; set the Z-axis of the drone to coincide with the current route direction, and calculate the deviation values ​​of the X-axis and Y-axis; when the drone undergoes changes in pitch, roll and yaw, establish the spatial deviation value of the i-th route.

2. The method for detecting autonomous route deviation of an urban low-altitude UAV according to claim 1 is characterized in that: The specific steps include: (1) The current route is the i-th route, and the local coordinates of the starting point of the current route are set to (0,0,0); Condition 1: Set the X-axis to coincide with the current route direction, with the forward direction as the positive direction; the Y-axis to be perpendicular to the current route X-axis direction, with the upward direction as the positive direction; The Z axis is perpendicular to the X axis of the current route, and the positive direction is perpendicular to the paper and outward; Condition 2: Set the Y axis to coincide with the current route direction, with the forward direction as the positive direction; the X axis to be perpendicular to the Y axis of the current route, with the direction perpendicular to the paper and outward as the positive direction; The Z axis is perpendicular to the X axis of the current route, and the upward direction is the positive direction; Condition 3: Set the Z axis to coincide with the current route direction, with the forward direction as the positive direction; the X axis to be perpendicular to the current route X axis direction, with the upward direction as the positive direction; The Y axis is perpendicular to the X axis of the current route, and the positive direction is perpendicular to the paper and outward; (2) Save the latitude, longitude and altitude of the current route starting point and convert them into the coordinates corresponding to the local coordinate system (0,0,0); (3) According to the coordinate position of the current route starting point, convert the current UAV latitude, longitude and altitude coordinate position.

3. The method for detecting autonomous route deviation of an urban low-altitude UAV according to claim 2 is characterized in that: Based on condition 1, the drone moving forward in the positive direction of the X axis determines whether the current Y-axis and Z-axis headings deviate from the values; There are four situations: The first case: when the current i routes are flying along the X axis, neither the Y axis nor the Z axis deviates; The second situation: when the current i routes are flying along the X axis, only the Y axis deviates; The third case: when the X axis of the current i routes is flying, only the Z axis deviates; The fourth situation: when the X-axis of the current i routes is flying, both the Y-axis and the Z-axis deviate.

4. The method for detecting autonomous route deviation of an urban low-altitude UAV according to claim 3 is characterized in that: (1) Based on the second case of condition 1, the maximum MAX deviation among the following three methods is used as the final Y-axis deviation value: Method 1: According to the Y-axis deviation, take the absolute value to obtain the deviation latitude and longitude / altitude coordinates, that is, read the average deviation of n points at the same time as: Method 2: According to the deviation of the Y axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as: Method 3: According to the Y-axis deviation, the distance from the point to the straight line is calculated to obtain the deviation from the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as: (2) Based on the third case of condition 1, the maximum MAX deviation among the following three methods is used as the final Z-axis deviation value: Method 1: According to the deviation of the Z axis, take the absolute value to obtain the longitude and latitude / altitude coordinates, that is, read the average deviation of n points at the same time as: Method 2: According to the deviation of the Z axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as: Method 3: According to the deviation of the Z axis, the distance from the point to the straight line is calculated to obtain the deviation of the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as: (3) Based on the fourth case of condition 1, the maximum MAX deviation among the following three methods is used as the final Y and Z axis deviation value: Method 1: According to the deviation of Y-axis and Z-axis, take the absolute value and use the Pythagorean theorem to calculate the deviation coordinates, that is, read the mean deviation of n points at the same time as: Method 2: According to the deviation of the Y-axis and Z-axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time: Method 3: According to the Y-axis and Z-axis offset, calculate the deviation value by the distance from the space point to the straight line, that is, read the mean deviation of n points at the same time as: Among them, |y i | represents the absolute value of the deviation error of the position of the i-th flight coordinate point on the Y axis, Lxy represents the distance between the i-th point Y axis deviation and the take-off origin when flying in the X-axis direction, α represents the angle between the i-th point Y axis deviation and the take-off origin when flying in the X-axis direction, x i Represents the deviation value of the position of the i-th flight coordinate point on the X axis, y i represents the deviation value of the position of the i-th flight coordinate point on the Y axis, A and B represent the parameters on the X axis and Y axis respectively, and C represents a constant; |z i | represents the absolute value of the deviation error of the i-th flight coordinate point on the Z axis, Lxz represents the distance between the i-th point Z axis deviation and the take-off origin when flying in the X-axis direction, β represents the angle between the i-th point Z axis deviation and the take-off origin when flying in the X-axis direction, x i Represents the deviation value of the position of the i-th flight coordinate point on the X axis, z i represents the deviation value of the position of the i-th flight coordinate point on the Z axis, A and C represent the parameters on the X axis and Z axis respectively, and D represents a constant; Lxyz represents the distance between the Y-axis and Z-axis deviation of the i-th point and the take-off origin when flying in the X-axis direction.

5. The method for detecting autonomous route deviation of an urban low-altitude UAV according to claim 4 is characterized in that: Based on condition 2, the drone moving forward in the positive direction of the Y axis determines whether the current X-axis and Z-axis headings deviate from the values; There are four situations: The first case: when the Y axis of the current route i is flying, neither the X axis nor the Z axis deviates; The second situation: when the Y axis of the current route i is flying, only the X axis deviates; The third case: when the Y axis of the current i routes is flying, only the Z axis deviates; The fourth situation: When the Y axis of the current i routes is flying, both the X axis and the Z axis deviate.

6. The method for detecting autonomous route deviation of an urban low-altitude UAV according to claim 5 is characterized in that: (1) Based on the second case of condition 2, the maximum MAX deviation among the following three methods is used as the final X-axis deviation value: Method 1: According to the deviation of the X-axis, take the absolute value to obtain the deviation longitude / latitude / altitude coordinates, that is, read the average deviation of n points at the same time as: Method 2: According to the deviation of the X-axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as: Method 3: According to the X-axis deviation, the distance from the point to the straight line is calculated to obtain the deviation from the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as: (2) Based on the third case of condition 2, the maximum MAX deviation among the following three methods is used as the final Z-axis deviation value: Method 1: According to the deviation of the Z axis, take the absolute value to obtain the longitude and latitude / altitude coordinates, that is, read the average deviation of n points at the same time as: Method 2: According to the deviation of the Z axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as: Method 3: According to the deviation of the Z axis, the distance from the point to the straight line is calculated to obtain the deviation of the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as: (3) Based on the fourth case of condition 2, the maximum MAX deviation among the following three methods is used as the final X and Z axis deviation value: Method 1: According to the deviation of X-axis and Z-axis, take the absolute value and use the Pythagorean theorem to calculate the deviation coordinates, that is, read the mean deviation of n points at the same time as: Method 2: According to the deviation of the X-axis and Z-axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as: Method 3: According to the X-axis and Z-axis offset, calculate the deviation value by the distance from the space point to the straight line, that is, read the mean deviation of n points at the same time as: Wherein, Lyx represents the distance between the X-axis deviation of the i-th point and the take-off origin when flying in the Y-axis direction, and γ represents the angle between the X-axis deviation of the i-th point and the take-off origin when flying in the Y-axis direction; Lyz represents the distance between the Z-axis deviation of the i-th point and the take-off origin when flying in the Y-axis direction, and δ represents the angle between the Z-axis deviation of the i-th point and the take-off origin when flying in the Y-axis direction; Lyxz represents the distance between the X-axis and Z-axis deviation of the i-th point and the take-off origin when flying in the Y-axis direction.

7. The method for detecting autonomous route deviation of an urban low-altitude UAV according to claim 6 is characterized in that: Based on condition three, the drone moving forward in the positive direction of the Z axis determines whether the current X-axis and Y-axis headings deviate from the values; There are four situations: The first case: when the current i routes are flying along the Z axis, neither the X axis nor the Y axis deviates; The second situation: when the current i routes are flying along the Z axis, only the X axis deviates; The third case: when the Z axis of the current i routes is flying, only the Y axis deviates; The fourth situation: When the current i routes are flying along the Z axis, both the X axis and the Y axis deviate.

8. The method for detecting autonomous route deviation of an urban low-altitude UAV according to claim 7, characterized in that: (1) Based on the second case of condition three, the maximum MAX deviation among the following three methods is used as the final X-axis deviation value: Method 1: According to the deviation of the X-axis, take the absolute value to obtain the deviation longitude / latitude / altitude coordinates, that is, read the average deviation of n points at the same time as: Method 2: According to the deviation of the X-axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as: Method 3: According to the X-axis deviation, the distance from the point to the straight line is calculated to obtain the deviation from the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as: (2) Based on the third case of condition three, the maximum MAX deviation among the following three methods is used as the final Y-axis deviation value: Method 1: According to the deviation of the Y axis, take the absolute value to obtain the longitude and latitude / altitude coordinates, that is, read the average deviation of n points at the same time as: Method 2: According to the deviation of the Y axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as: Method 3: According to the Y-axis deviation, the distance from the point to the straight line is calculated to obtain the deviation from the longitude and latitude / altitude coordinates, that is, the average deviation of n points at the same time is read as: (2) Based on the fourth case of condition three, the maximum MAX deviation among the following three methods is used as the final X-axis and Y-axis deviation value: Method 1: According to the deviation of X-axis and Y-axis, take the absolute value and use the Pythagorean theorem to calculate the deviation coordinates, that is, read the mean deviation of n points at the same time as: Method 2: According to the deviation of the X-axis and Y-axis, obtain the sine value of the angle between the current coordinate and the origin coordinate, that is, read the mean deviation of n points at the same time as: Method 3: According to the X-axis and Y-axis offset, calculate the deviation value by the distance from the space point to the straight line, that is, read the mean deviation of n points at the same time as: Where, Lzx represents the distance between the X-axis deviation of the i-th point and the take-off origin when flying in the Z-axis direction, and η represents the angle between the X-axis deviation of the i-th point and the take-off origin when flying in the Z-axis direction; Among them, Lzy represents the distance between the Y-axis deviation of the i-th point and the take-off origin when flying in the Z-axis direction, and θ represents the angle between the Y-axis deviation of the i-th point and the take-off origin when flying in the Z-axis direction; Lzxy represents the distance between the X-axis and Y-axis deviation of the i-th point and the take-off origin when flying in the Z-axis direction.

Images