An unmanned helicopter route planning method based on diffraction principle

Abstract

The application relates to a route planning method of an unmanned helicopter based on a diffraction principle, and the steps are as follows: (1) constructing a terrain threat model T t , (2) calculating a track point optimization data set B, (3) preliminarily selecting an optimal flight route fpb, (4) dynamically adjusting the flight route, constructing a real-time flight route set, (5) updating the initial optimal route, and generating a real-time optimal route fpn, (6) improving the optimal initial route by adopting a cubic B-spline curve method and a route optimization algorithm, and realizing flight route flyability optimization. The application is a route optimization problem considering the terrain threat characteristics, flight safety and aircraft performance. The example results show that the light diffraction principle can realize automatic route generation in a complex threat environment, the method can provide effective flight service for the unmanned helicopter, and the flight safety and reliability are improved.

Description

Technical Field

[0001] This invention belongs to the field of unmanned helicopter flight path planning, and involves a method for planning the optimal flight path using information such as the performance of unmanned helicopters and environmental threats. Specifically, it is an unmanned helicopter flight path planning method based on the principle of diffraction. Background Technology

[0002] To date, research on automatic flight path planning in complex terrain environments, both domestically and internationally, has not been systematically studied, and a complete and systematic model and method for autonomous flight path planning of unmanned helicopters has not yet been formed. Rapid flight path planning for unmanned helicopters in complex terrain environments has not yet been well resolved. How to utilize terrain threat characteristics to reduce planning space and design the fastest flight path with the highest safety performance is an urgent problem to be solved. Solving this problem has extremely important application value for both military and civilian applications. Summary of the Invention

[0003] (1) Constructing the terrain threat model T t The mathematical description is as follows:

[0004]

[0005] x∈(x min x max ), y∈(y min y max )

[0006] In the above formula, T t (x, y) represents the terrain threat model value at the x-coordinate and y-coordinate, where x min and x max y represents the minimum and maximum values ​​of the x-coordinate within the planned airspace, respectively; min and y max These represent the minimum and maximum values ​​of the ordinate within the planned airspace, respectively, in kilometers; It is a kind of (-x) 2 -(y+1) 2 ) power exponential function;

[0007] (2) Calculate the optimal set of waypoints B:

[0008] First, use formulas (2) to (3) to obtain the x-axis coordinate vector Vx and the y-axis coordinate vector Vy:

[0009] Vx=(x i ) = [x1, x2...x m (2)

[0010]

[0011]

[0012]

[0013] In formulas (2) to (3), [] represents the matrix symbol, x i y i are the i-th x-coordinate and y-coordinate components, respectively, and m and n are the total number of x-axis and y-axis coordinate vector elements, respectively.

[0014] Then, using formulas (4) to (6), we obtain the discrete matrices Mx and My of the x-axis coordinate vector Vx and the y-axis coordinate vector Vy, and substitute Mx and My into formula (1) to calculate the corresponding discrete matrix MT of the terrain threat model:

[0015]

[0016]

[0017] MT = T t (Mx, My) (6)

[0018] Finally, using formulas (7) to (10) to calculate the buffer zone of the shortest route, we can perform waypoint optimization on MT to obtain the waypoint optimization dataset B:

[0019] [xb i yb i ] = bufferm([x i x i +ins],[y s y e ], d bf ,opt),x i ∈(x s x e ), i = 1, 2, ..., n(7)

[0020] xbs = [xb1, xb2, ..., xb] n (8)

[0021] ybs = [yb1, yb2, ..., yb n (9)

[0022] [in i on i = inpolygon(Mx, My, xb) i yb i ), i = 1, 2, ... n (10)

[0023] xis=x(in i (11)

[0024] yis=y(in i (12)

[0025] zis=T t (xis,yis) (13)

[0026] B = [xis, yis, zis] (14)

[0027] In formulas (4) to (9), ones(n, 1) and ones(1, m) represent generating matrices of all 1s in rows of n and columns of 1 and in rows of m, respectively. i yb i Let x and y represent the x and y coordinates of the generated optimal route buffer boundary set, respectively, where ins and n are the x and y coordinates of the generated optimal route buffer boundary set, respectively. i The coordinate increment of the buffer and the total number of buffers, xs, ys, xs, ys are the x and y coordinates of the starting point and the target point, respectively; bufferm, dbf, opt are the function for generating the buffer, the distance for generating the buffer, and the parameter for selecting whether to generate an internal or external buffer, respectively; xbs and ybs are the x and y coordinates of the buffer boundary point set of the shortest route, respectively; in formulas (10) to (14), in i on i represents the index of the discrete points of terrain threats located inside and on the boundary of the i-th buffer, respectively; inpolygon represents the function to determine whether a point is inside the polygon; xis, yis, and zis represent the x, y, and z coordinates of the point set inside the buffer of the shortest route, respectively; and [] represents the matrix symbol.

[0028] (3) Preliminary selection of the optimal flight route fpb:

[0029] fpb=min(sum(zis(zis>0))) (15)

[0030] fpb = [fpx fpy fpz] (16)

[0031] In formulas (15) to (16), mmin and sum are functions for finding the minimum value and summation, respectively. The minimum value of the sum of terrain threats of the track points is calculated by fpb to obtain the preliminary optimal route with restricted route elevation. fpx, fpy, and fpz represent the x, y, and z coordinates of the preliminary preferred track point set, respectively.

[0032] (4) Dynamically adjust flight routes and construct real-time flight route sets.

[0033] First, when a sudden threat is generated using formulas (13) to (14), a new threat model Tj is constructed, and the terrain threat model is updated to Yn:

[0034]

[0035] Tn(x, y) = T t (x, y) + Tj(x, y) (18)

[0036] In formulas (17) to (18), Tj, Yn, and tm represent the new threat model, the updated threat model, and the total number of new threats, respectively. oi y oi ) and (x si y si ) represent the planar coordinates of the i-th new threat center point and the parameters decreasing along the x and y directions, respectively;

[0037] Then, using formulas (19) to (25), the affected route segments are selected based on the maximum impact boundary of the new threat model, generating the set of route segments TB that need to be adjusted:

[0038] [c, h] = contour(Tj) (19)

[0039] hv=h.LevelList(h.LevelList>0) (20)

[0040] [hmnv, hmnix]=min(hv) (2 1 )

[0041] [rw,cw]=find(abs(c(rr,cc)-hv(hmnix)<v ths ) (twenty two)

[0042] [xx,yy]=bufferm(x1,y1,d bf ,opt) (23)

[0043] [in1, on1]=inpolygon(fpx, fpy, xx, yy) (24)

[0044] TB=[fpx(in1); fpy(in1); hv(hmnix)] (25)

[0045] In formulas (19) to (23), c and h represent the generated terrain threat boundary set and its handle, respectively; contour, find, and abs are the functions for calculating contour lines, searching, and solving for absolute values, respectively; h.LevelList is the elevation value corresponding to the contour line level; hmnv and hmnix are the minimum elevation value and its index corresponding to the contour line of the new threat, respectively; hv, rw, and cw represent the handle of the terrain threat boundary set affecting flight safety, the set of rows and columns of the searched terrain threat boundary, respectively; v thsrr and cc refer to the threshold for comparing the similarity between the two, the row and column index values ​​of the terrain threat boundary set, respectively; xx, yy, x1, and y1 are the vertical and horizontal coordinates of the buffer boundary and the terrain threat boundary, respectively; bufferm, dbf, and optf are the function for generating the buffer, the distance for generating the buffer, and the parameter for selecting to generate the internal or external buffer, respectively; in formulas (24) to (25), in1 and on1 represent the sequence numbers of the track points of the initial shortest route on the inner and outer boundaries of the new threat's influence range, respectively; TB represents the set of track points affected by the new threat.

[0046] Using formulas (26) to (30), the affected route point set is dynamically adjusted to generate a new track point set TBN:

[0047] xs=linspace(fpx(in1(1)), fpx(in1(end)), N) (26)

[0048] ys=linspace(fpy(in1(1)), fpy(in1(end)), N) (27)

[0049] zs=linspace(fpz(in1(1)), fpz(in1(end)), N) (28)

[0050]

[0051] TBN = [xs; ys+1; zs] (30)

[0052] In formulas (26) to (30), xs, ys, and zs represent the x, y, and z coordinates of the optimized track point set, respectively. linspace and sin are the vector function and the function for calculating the sine value of the N-point linear line between fpx(in1(1)) and fpx(in1(end)), respectively. N and N0 represent the total number of discrete points and the total number of sudden threats that will divide the adjusted route segment, respectively. C0, C1, and C2 represent the parameters for controlling the direction, size, and shape of the adjusted route segment, respectively. end means last, and in in1(end) it represents the last element of the vector in1.

[0053] (5) Update the initial optimal route and generate the real-time optimal route fpn:

[0054] fpnx=[fpx(1)...fpx(in1-1)xs...fpx(end)] (31)

[0055] fpny=[fpy(1)...fpy(in1-1)ys+I...fpy(end)] (32)

[0056] fpnz=[fpz(1)...fpz(in1-1)zs...fpz(end)] (33)

[0057] fpn=[fpnx;fpny;fpnz] (34)

[0058] In formulas (31) to (34), fpn, fpnx, fpny, and fpnz represent the x, y, and z coordinates of the route and track point set after the initial optimal route is updated;

[0059] (6) The optimal initial route is improved by using the cubic B-spline curve method and route optimization algorithm, which combines route flightability optimization with flight safety, and provides an effective method to avoid aircraft crashes and reduce fuel consumption.

[0060] Compared with the prior art, the advantages of this invention are:

[0061] a. Achieving rapid and automated flight path planning in complex terrain environments. This method combines the principles of light diffraction with spatial analysis to provide an effective approach for dynamic planning of unmanned helicopters in complex environments.

[0062] b. Route safety optimization in complex environments using terrain features combined with buffer zone analysis. Utilizing terrain features for route selection significantly reduces planned airspace and enables rapid dynamic threat avoidance for higher safety. It plays a crucial role in the rapid generation of routes and timely avoidance of terrain threats in complex terrain environments. Detailed Implementation

[0063] The technical solution of the present invention will be further described below with reference to the embodiments. In the embodiment, an unmanned helicopter V750 is flying. The lower left corner of the planned airspace is [0km, 0km, 0km], and the upper right corner is [20km, 20km, 20km]. There are 6 terrain threats at (11km, 16km), (9km, 9km), (15km, 1km), (11km, 5km), (7km, 12km), and (12km, 12km). The V750 has a range of 150km, an endurance of 4 hours, a cruising speed of 145km / h, and a maximum service ceiling of 100km. The coordinates of the starting point and the target point are (1km, 1km, 2km) and (20km, 20km, 2km), respectively.

[0064] (1) Constructing the terrain threat model T t The mathematical description is as follows:

[0065]

[0066] x∈(1, 20), y∈(1, 20)

[0067] In the above formula, T t (x, y) represents the terrain threat model value at the x-coordinate and y-coordinate, where x min and x max y represents the minimum and maximum values ​​of the x-coordinate within the planned airspace, respectively; min and y max These represent the minimum and maximum values ​​of the ordinate within the planned airspace, respectively, in kilometers; It is a kind of (-x) 2 -(y+1) 2 ) power exponential function;

[0068] (2) Calculate the terrain threat model vector dataset and angle set:

[0069] First, use formulas (2) to (3) to obtain the x-axis coordinate vector Vx and the y-axis coordinate vector Vy:

[0070] Vx = [1, 2...20] (2)

[0071]

[0072]

[0073]

[0074] In formulas (2) to (3), [] represents the matrix symbol, x i y i are the i-th x-coordinate and y-coordinate components, respectively, and m and n are the total number of x-axis and y-axis coordinate vector elements, respectively.

[0075] Then, using formulas (4) to (6), we obtain the discrete matrices Mx and My of the x-axis coordinate vector Vx and the y-axis coordinate vector Vy, and substitute Mx and My into formula (1) to calculate the corresponding discrete matrix MT of the terrain threat model:

[0076]

[0077]

[0078]

[0079] Finally, the shortest route buffer is calculated using formulas (7) to (10) to perform path point optimization on MT and obtain path point optimization dataset B;

[0080] [xb i yb i ] = buffem([xi x i +1],

[1010] ,0.5,′out′),x i ∈(1,20), i=1,2,...10(7)

[0081] xbs = [xb1, xb2, ..., xb] n (8)

[0082] ybs = [yb1, yb2, ..., yb n (9)

[0083] [in i on i = inpolygon(Mx, My, xb) i yb i ), i = 1, 2, ... 10 (10)

[0084] xis=x(in i (11)

[0085] yis=y(in i (12)

[0086] zis=T t (xis,yis) (13)

[0087] B = [xis, yis, zis] (14)

[0088] In formulas (4) to (9), ones(n, 1) and ones(1, m) represent generating matrices of all 1s in rows of n and columns of 1 and in rows of m, respectively. i yb i Let x and y represent the x and y coordinates of the generated optimal route buffer boundary set, respectively, where ins and n are the x and y coordinates of the generated optimal route buffer boundary set, respectively. i The coordinate increments and total number of buffers, xs, ys, xs, ys are the x and y coordinates of the starting point and the target point, respectively, bufferm, d bf _opt_ and _opt_ are the functions that generate the buffer, the distance to generate the buffer, and the parameters for selecting whether to generate an internal or external buffer, respectively. _xbs_ and _ybs_ are the x and y coordinates of the buffer boundary point set of the shortest route, respectively. In formulas (10) to (14), in i on i represents the index of the discrete points of terrain threats located inside and on the boundary of the i-th buffer, respectively; inpolygon represents the function to determine whether a point is inside the polygon; xis, yis, and zis represent the x, y, and z coordinates of the point set inside the buffer of the shortest route, respectively; and [] represents the matrix symbol.

[0089] (3) Preliminary selection of the optimal flight route fpb:

[0090] fpb=min(sum(zis(zis>0))) (15)

[0091] fpb = [fpx fpy fpz] (16)

[0092] In formulas (15) to (16), mmin and sum are functions for finding the minimum value and summation, respectively. The minimum value of the sum of terrain threats of the track points is calculated by fpb to obtain the preliminary optimal route with restricted route elevation. fpx, fpy, and fpz represent the x, y, and z coordinates of the preliminary preferred track point set, respectively.

[0093] (4) Dynamically adjust flight routes and construct real-time flight route sets.

[0094] First, when a sudden threat is generated using formulas (13) to (14), a new threat model Tj is constructed, and the terrain threat model is updated to Yn:

[0095]

[0096] Tn(x, y) = T t (x, y) + Tj(x, y) (18) In formulas (17) to (18), Tj, Yn, and tm represent the new threat model, the updated threat model, and the total number of new threats, respectively. oi y oi ) and (x si y si ) represent the planar coordinates of the i-th new threat center point and the parameters decreasing along the x and y directions, respectively;

[0097] Then, using formulas (19) to (25), the affected route segments are selected based on the maximum impact boundary of the new threat model, generating the set of route segments TB that need to be adjusted:

[0098] [c, h] = contour(Tj) (19)

[0099] hv=h.LevelList(h.LevelList>0) (20)

[0100] [hmnv, hmnix]=min(hv) (2 1 )

[0101] [rw,cw]=find(abs(c(rr,cc)-hv(hmnix)<0.01) (22)

[0102] [xx, yy] = bufferm (x1, y1, 0.5, 'out') (23)

[0103] [in1, on1]=inpolygon(fpx, fpy, xx, yy) (24)

[0104] TB=[fpx(in1); fpy(in1); hv(hmnix)] (25)

[0105] In formulas (19) to (23), c and h represent the generated terrain threat boundary set and its handle, respectively; contour, find, and abs are the functions for calculating contour lines, searching, and solving for absolute values, respectively; h.LevelList is the elevation value corresponding to the contour line level; hmnv and hmnix are the minimum elevation value and its index corresponding to the contour line of the new threat, respectively; hv, rw, and cw represent the handle of the terrain threat boundary set affecting flight safety, the set of rows and columns of the searched terrain threat boundary, respectively; v ths rr, cc refer to the threshold for comparing the similarity between the two, and the row and column indices of the terrain threat boundary set, respectively. xx, yy, x1, y1 are the x and y coordinates of the buffer boundary and the terrain threat boundary, respectively. bufferm, d bf , optf are the functions for generating buffers, the distance for generating buffers, and the parameters for selecting whether to generate internal or external buffers, respectively; in formulas (24) to (25), in1 and on1 represent the sequence numbers of the initial shortest route points on the inner and outer boundaries of the new threat's influence area, respectively, and TB represents the set of route points affected by the new threat;

[0106] Using formulas (26) to (30), the affected route point set is dynamically adjusted to generate a new track point set TBN:

[0107] xs=linspace(fpx(4), fpx(5), 1000) (26)

[0108] ys=linspace(fpy(4), fpy(5), 1000) (27)

[0109] zs=linspace(fpz(4), fpz(5), 1000) (28)

[0110]

[0111] TBN = [xs; ys + I; zs] (30)

[0112] In formulas (26) to (30), xs, ys, and zs represent the x, y, and z coordinates of the optimized track point set, respectively. linspace and sin are the vector function and the function for calculating the sine value, respectively, used to generate the N-point linearity between fpx(in1(1)) and fpx(in1(end)). N and N0 represent the total number of discrete points and the total number of sudden threats to be divided into the adjusted route segment, respectively. C0, C1, and C2 represent the parameters for controlling the direction, size, and shape of the adjusted route segment, respectively. end represents the last value, and in in1(end) represents the last element of the vector in1.

[0113] (5) Update the initial optimal route and generate the real-time optimal route fpn:

[0114] fpnx=[fpx(1)...fpx(in1-1)xs...fpx(end)] (31)

[0115] fpny=[fpy(1)...fpy(in1-1)ys+I...fpy(end)] (32)

[0116] fpnz=[fpz(1)...fpz(in1-1)zs...fpz(end)] (33)

[0117] fpn=[fpnx;fpny;fpnz] (34)

[0118] In formulas (31) to (34), fpn, fpnx, fpny, and fpnz represent the x, y, and z coordinates of the route and track point set after the initial optimal route is updated;

[0119] (6) The optimal initial route is improved by using the cubic B-spline curve method and the route optimization algorithm to achieve route flightability optimization.

[0120] The experimental results are as follows: the planned flight distance is 43.9424 kilometers, which meets the 150-kilometer flight distance limit; the planning time is 0.8070 seconds; the route planned using this method can meet the online planning requirements (5 seconds); the cruise flight time is 0.3031 hours, which meets the limit of 4 hours; at the same time, the route selects a safe low-lying area for flight, which can realize online flight route planning and provide safety guarantee for emergency accident rescue.

Claims

1. A method for unmanned helicopter flight path planning based on the principle of diffraction, characterized by employing... The following are the implementation steps: (1) Constructing the terrain threat model T t The mathematical description is as follows: In formula (1), T t (x, y) represents the terrain threat model value at the x-coordinate and y-coordinate, where x min and x max y represents the minimum and maximum values ​​of the x-coordinate within the planned airspace, respectively; min and y max These represent the minimum and maximum values ​​of the ordinate within the planned airspace, respectively, in kilometers; It is a kind of (-x) 2 -(y+1) 2 ) power exponential function; (2) Calculate the optimal set of waypoints B: First, use formulas (2) to (3) to obtain the x-axis coordinate vector Vx and the y-axis coordinate vector Vy: Vx=(x i )=[x1,x2…x m ] (2) In formulas (2) to (3), [] represents the matrix symbol, x i y i are the i-th x-coordinate and y-coordinate components, respectively, and m and n are the total number of x-axis and y-axis coordinate vector elements, respectively. Then, using formulas (4) to (6), we obtain the discrete matrices Mx and My of the x-axis coordinate vector Vx and the y-axis coordinate vector Vy, and substitute Mx and My into formula (1) to calculate the corresponding discrete matrix MT of the terrain threat model: MT=T t (Mx,My) (6) Finally, the shortest route buffer is calculated using formulas (7) to (10) to perform path point optimization on MT and obtain path point optimization dataset B; [xb i ,yb i ]=bufferm([x i x i +ins],[y s y e ],d bf ,opt),x i ∈(x s ,x e ),i=1,2,…n (7) xbs=[xb j ,xb2,...,xb n ] (8) ybs=[yb1,yb2,...,yb n ] (9) [in i ,on i ]=inpolygon(Mx,My,xb i ,yb i ),i=1,2,...n (10) xis=x(in i ) (11) yis=y(in i ) (12) output=T t (yes, yes) (13) B = [xis, yis, zis] (14) In formulas (4) to (9), ones(n, 1) and ones(1, m) represent generating matrices of all 1s in rows of n and columns of 1 and in rows of m, respectively. i yb i Let x and y represent the x and y coordinates of the generated optimal route buffer boundary set, respectively, where ins and n are the x and y coordinates of the generated optimal route buffer boundary set, respectively. i The coordinate increments and total number of buffers, xs, ys, xs, ys are the x and y coordinates of the starting point and the target point, respectively, bufferm, d bf _opt_ and _opt_ are the functions that generate the buffer, the distance to generate the buffer, and the parameters for selecting whether to generate an internal or external buffer, respectively. _xbs_ and _ybs_ are the x and y coordinates of the buffer boundary point set of the shortest route, respectively. In formulas (10) to (14), in i on i represents the index of the discrete points of terrain threats located inside and on the boundary of the i-th buffer, respectively; inpolygon represents the function to determine whether a point is inside the polygon; xis, yis, and zis represent the x, y, and z coordinates of the point set inside the buffer of the shortest route, respectively; and [] represents the matrix symbol. (3) Preliminary selection of the optimal flight route fpb: fpb=min(sum(zis(zis>0))) (15) fpb=[fpx fpy fpz] (16) In formulas (15)~(16), min and sum are functions for finding the minimum value and summation, respectively. The minimum value of the sum of terrain threats of the track points is calculated by fpb to obtain the preliminary optimal route with restricted route elevation. fpx, fpy, and fpz represent the x, y, and z coordinates of the preliminary preferred track point set, respectively. (4) Dynamically adjust flight routes and construct real-time flight route sets. First, when a sudden threat is generated using formulas (13) to (14), a new threat model Tj is constructed, and the terrain threat model is updated to Tn: Tn(x,y)=T t (x,y)+Tj(x,y) (18) In formulas (17) to (18), Tj, Tn, and tm represent the new threat model, the updated threat model, and the total number of new threats, respectively. oi y oi ) and (x si y si ) represent the planar coordinates of the i-th new threat center point and the parameters decreasing along the x and y directions, respectively; Then, using formulas (19) to (25), the affected route segments are selected based on the maximum impact boundary of the new threat model, generating the set of route segments TB that need to be adjusted: [c, h] = contour(Tj) (19) hv=h.LevelList(h.LevelList>0) (20) [hmnv, hmnix]=min(hv) (2 1 ) [rwcw]=find(abs(c(rr,cc)-hv(hmnix)<v ths ) (22) [xx,yy]=bufferm(x1,y1,d bf ,opt) (23) [in1,on1]=inpolygon(fpx,fpy,xx,yy) (24) TB=[fpx(in1); fpy(in1); hv(hmnix)] (25) In formulas (19) to (23), c and h represent the generated terrain threat boundary set and its handle, respectively; contour, find, and abs are the functions for calculating contour lines, searching, and solving for absolute values, respectively; h.LevelList is the elevation value corresponding to the contour line level; hmnv and hmnix are the minimum elevation value and its index corresponding to the contour line of the new threat, respectively; hv, rw, and cw represent the handle of the terrain threat boundary set affecting flight safety, the set of rows and columns of the searched terrain threat boundary, respectively; v ths rr, cc refer to the threshold for comparing the similarity between the two, and the row and column indices of the terrain threat boundary set, respectively. xx, yy, x1, y1 are the x and y coordinates of the buffer boundary and the terrain threat boundary, respectively. bufferm, d bf , optf are the functions for generating buffers, the distance for generating buffers, and the parameters for selecting whether to generate internal or external buffers, respectively; in formulas (24) to (25), in1 and on1 represent the sequence numbers of the initial shortest route points on the inner and outer boundaries of the new threat's influence area, respectively, and TB represents the set of route points affected by the new threat; Using formulas (26) to (30), the affected route point set is dynamically adjusted to generate a new track point set TBN: zs=linspace(fpx(in1(1)), fpx(in1(end)), N) (26) ys=linspace(fpy(in1(1)), fpy(in1(end)), N) (27) zs=linspace(fpz(in1(1)), fpz(in1(end)), N) (28) TBN = [xs; ys + I; zs] (30) In formulas (26) to (30), xs, ys, and zs represent the x, y, and z coordinates of the optimized track point set, respectively. linspace and sin are the vector function and the function for calculating the sine value of the N-point linear line between fpx(in1(1)) and fpx(in1(end)), respectively. N and N0 represent the total number of discrete points and the total number of sudden threats that will divide the adjusted route segment, respectively. C0, C1, and C2 represent the parameters for controlling the direction, size, and shape of the adjusted route segment, respectively. end means last, and in in1(end) it represents the last element of the vector in1. (5) Update the initial optimal route and generate the real-time optimal route fpn: fpax=[fpx(1)…fpx(in1-1)xs…fpx(end)] (31) fpny=[fpy(1)…fpy(in1-1)ys+I…fpy(end)] (32) fpnz=[fpz(1)…fpz(in1-1)zs…fpz(end)] (33) fpn=[fpnx;fpny;fpnz] (34) In formulas (31) to (34), fpn, fpnx, fpny, and fpnz represent the x, y, and z coordinates of the route and track point set after the initial optimal route is updated; (6) The optimal initial route is improved by using the cubic B-spline curve method and the route optimization algorithm to achieve route flightability optimization.

Images