An unmanned helicopter route planning method based on self-adjusting coulomb force method

Abstract

The application relates to an unmanned helicopter route planning method based on a self-adjusting Coulomb force method, and the steps are as follows: (1) constructing a comprehensive threat model, (2) calculating a comprehensive threat model influence threat field, (3) selecting a suitable flight area, constructing a flight route set, (4) performing route evaluation, selecting an optimal initial route, (5) adopting the self-adjusting Coulomb force method to improve the optimal initial route, and realizing route flyability optimization. The application is a route optimization problem considering comprehensive threats, flight path point spatial distribution characteristics, flight safety and aircraft performance and the like. The example result shows that the self-adjusting Coulomb force method can realize optimal route planning in a complex threat environment, and the method can provide effective flight service for the unmanned helicopter while improving flight safety and reliability.

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 a self-adjusting Coulomb force method. Background Technology

[0002] To date, research on flight path planning in complex threat environments, both domestically and internationally, is still in its early stages. A complete and systematic model and method for rapid flight path planning of unmanned helicopters has not yet been established. Rapid flight path planning for unmanned helicopters in complex threat environments remains largely unresolved. How to apply comprehensive threat analysis and the spatial distribution characteristics of waypoints to plan flight paths with maximum safety, avoid comprehensive threats, and achieve aviation safety rescue are urgent problems to be solved. Solving this problem has extremely important application value for both military and civilian applications. Summary of the Invention

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

[0004] Using formulas (1) to (4), the discrete values ​​of the planar coordinates x and y in the airspace are obtained, and the T of the integrated threat model is calculated based on these values. t Threat level:

[0005] T t (x, y)=z1(x, y)+z2(x, y)+z3(x, y) (1)

[0006]

[0007]

[0008] z3(x, y)=hc, x∈(x1, x2), y∈(y1, y2) (4)

[0009] In formulas (1) to (4), T t (x, y) represents the terrain threat value at the plane coordinates (x, y), in kilometers; e is an exponential function; z1, z2, and z3 are the non-flyable area model, terrain threat model, and building threat model, respectively, in meters; x1 and x2 are the start and end coordinates of the building threat on the x-axis, in kilometers; y1 and y2 are the start and end coordinates of the building threat on the y-axis, in kilometers; and hc is the height constant of the building threat, in meters.

[0010] (2) Calculate the impact of the integrated threat model on the threat field:

[0011] First, use formulas (5) to (6) to obtain the discrete matrices Mx and My, and then substitute Mx and My into formula (1) to calculate the discrete matrix MT of the corresponding terrain threat model:

[0012] [Mx,My]=meshgrid(1:wd) (5)

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

[0014] In formulas (5) to (6), wd represents the scale range of the study space, and meshgrid is the function that generates grid sampling points; then, using formulas (7) to (10), contour lines are obtained from MT to get the terrain threat boundary set c:

[0015] [c, h] = contour(MT) (7)

[0016] hv=h.LevelList(h.LevelList>0) (8)

[0017] [rw,cw]=find(abs(c(rr,cc)-hv<v ths (9)

[0018] [xx,yy]=bufferm(x1,y1,d bf ,opt) (10)

[0019] [in, on] = inpolygon (xx, yyy, x1, y1) (11)

[0020] B=[xx(~in); yy(~in); hv] (12)

[0021] In formulas (7) to (10), 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 and h.LevelList are the contour level number and the elevation value corresponding to the contour level, 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 boundaries, 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 an internal or external buffer, respectively; in formulas (11) to (12), in and on represent the serial numbers of the buffer boundary points inside the terrain threat and on the boundary, respectively; inpolygon represents the function for determining whether a point is inside a polygon; and ~ is the sign for taking the opposite number.

[0022] (3) Select suitable flight areas and construct flight route clusters

[0023] First, the shortest route pool boundary L under no terrain threat is generated using formulas (13) to (14):

[0024] [xx1, yy1] = bufferm ([xs xe], [ys ye], dl bf ,opt) (13)

[0025] L = [xx1; yy1; hv] (14)

[0026] In formulas (13) to (14), xs, ys, xs, and ys are the x and y coordinates of the starting point and the target point, respectively, and dl bf This represents the buffer distance of the shortest route.

[0027] Then, using formula (15), the shortest route pool boundary is used to select the area after removing terrain threats, generating a suitable flight zone that meets distance and safety constraints:

[0028] [in1,on1]=inpolygon(xx(~in),yy(~in),xx1,yy1) (15)

[0029] [in2,on2]=inpolygon(xr,yr,xx1,yy1) (16)

[0030] xr=ceil(rand(xrc,yrl)*(xe-xs+1)

[0031] yr=ceil(rand(xrc,yrl)*(ye-ys+1)

[0032] [in3,on3]=inpolygon(xr,yr,xx(in),yy(in)) (17)

[0033] P=[xr(in2&~in3)yr(in2&~in3)] (18)

[0034] In formulas (15) to (18), in1 and on1 represent the serial numbers of discrete points inside and on the shortest route pool set, respectively, excluding terrain threats; xr and yr represent the x and y coordinates of the randomly generated route set, respectively; xrc and yrl represent the number of rows and columns of the randomly generated route set, respectively; nin, non, and nnin represent the number of discrete points of V1 inside, on, and outside the distance ellipse, respectively; in2 and on2 represent the serial numbers of discrete points inside and on the boundary of the shortest route pool set, respectively, nin and on3 represent the serial numbers of discrete points inside and on the boundary of the randomly generated route set, respectively; and P represents the route set excluding terrain threats and located within the shortest route pool set.

[0035] The dynamic combination of route sets frs is achieved using formulas (20) to (23):

[0036] [xfd, si] = sort(xf) (20)

[0037] yfd=yf(si) (21)

[0038] zfd=zf(si) (22)

[0039] pfs = [xfd; yfd; zfd] (23)

[0040] In formulas (20) to (23), xfd and si represent the x coordinate values ​​after sorting in ascending order and their indices in the original xf, respectively; yfd and zfd represent the y and z coordinate values ​​corresponding to the sorted values, respectively; and frs represents the dynamic combination of the route set.

[0041] 4) Perform route evaluation and select the optimal initial route fp:

[0042] [fpv, fid]=min(sum(dist([xfd;yfd;zfd] T ))) (twenty four)

[0043] fp=[xfd(fid);yfd(fid);zfd(fid)] T

[0044] In formula (24), min, sum, and dist are functions for finding the minimum value, summing, and calculating the distance, respectively. The superscript T indicates the function for finding the matrix transpose. The minimum value fpv and the index fid of the sum of the comprehensive threat models of the track points are calculated by formula (24). fp obtains the optimal initial route with restricted route elevation.

[0045] (5) The self-adjusting Coulomb force method is used to improve the optimal initial flight path, combining flight path flightability optimization with altitude adaptability methods, providing an effective method to avoid aircraft crashes and reduce fuel consumption:

[0046]

[0047] zp=kz*dh+fpz (26)

[0048]

[0049]

[0050] dx(i)=x(i+1)-x(i), i=1, 2,..., fn-1

[0051] dy(i)=y(i+1)-y(i), i=1, 2,..., fn-1

[0052] dh(i)=fpz(i+1)-fpz(i), i=1, 2,..., fn-1

[0053] mdx=mean([dx(i),dx(i+1),dx(i-1)])

[0054] mdy=mean([dy(i), dy(i+1), dy(i-1)])

[0055] mdz=dh

[0056] kx = -sign(dx)*cx / (mdx+eps)

[0057] ky = -sign(dy)*cy / (mdy+eps)

[0058] kz = -sign(dz)*cz / (mdz+eps)

[0059] cx = c1 / (mdx + eps)

[0060] cy = c² / (mdy + eps)

[0061] cz = c3 / (mdz + eps)

[0062] In formulas (25) to (27), kx, ky, and kz represent the parameters corresponding to the route optimization adjustment, d, dx, dy, and dh represent the length of the route segment, the increment of the horizontal coordinate, the increment of the vertical coordinate, and the increment of the elevation, respectively. fn represents the number of route points in the route. xp, yp, and zp represent the x, y, and z coordinate values ​​of each track point after route optimization, respectively. fpx, fpy, and fpz represent the x, y, and z coordinate values ​​of each track point of the optimal initial route, respectively. mdx, mdy, and mdz represent the adjustment coefficients of the x, y, and z coordinate values ​​used for route optimization, respectively. cx, cy, and cz represent the adjustment constants of the x, y, and z coordinate values ​​used for route optimization, respectively. mean, sign, and eps represent the average value function, the sign function, and the floating-point relative precision function, respectively.

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

[0064] a. Enables rapid planning of globally optimal flight routes under complex threat environments. A combination of selecting suitable flight areas and using comprehensive threat value-based route elimination methods provides an effective approach to prevent aircraft safety incidents.

[0065] b. Rapid route optimization in complex environments using the spatial relationship characteristics of waypoints. The route planning algorithm utilizing the spatial relationship characteristics of waypoints, through the self-adjusting Coulomb force method to obtain the safest flight waypoints, plays a crucial role in rapidly generating routes and promptly avoiding comprehensive threats in complex terrain environments. Detailed Implementation

[0066] The technical solution of the present invention will be further described below with reference to the embodiments. In the embodiment, a Mi-28 attack helicopter is flying. The planned airspace has the coordinates of the lower left corner [0km, 0km, 0km] and the upper right corner [20km, 20km, 20km]. There are 6 terrain threats at (11km, 16km), (9km, 9km), (15km, 1km), (11km, 5km), (7km, 12km), and (12km, 12km). There are 3 no-fly zones at (8km, 12km), (13km, 7km), and (8km, 7km). There is 1 building threat at (7.5km, 6km). The performance parameters of the UAV are: maximum level flight speed ≥ 60km / h, endurance time ≥ 45min, and range ≥ 50km. The coordinates of the starting point and the target point are (1km, 1km, 3m) and (20km, 20km, 3m), respectively.

[0067] (1) Constructing a comprehensive threat model T t The mathematical description is as follows:

[0068] Using formulas (1) to (4), the discrete values ​​of the planar coordinates x and y in the airspace are obtained, and the T of the integrated threat model is calculated based on these values. t Threat level:

[0069] T t (x, y)=z1(x, y)+z2(x, y)+z3(x, y) (1)

[0070]

[0071]

[0072] z3(x, y)=10, x∈(10,15), y∈(4,8) (4)

[0073] In formulas (1) to (4), T t (x, y) represents the terrain threat value at the plane coordinates (x, y), in kilometers; e is an exponential function; z1, z2, and z3 are the non-flyable area model, terrain threat model, and building threat model, respectively, in meters; x1 and x2 are the start and end coordinates of the building threat on the x-axis, in kilometers; y1 and y2 are the start and end coordinates of the building threat on the y-axis, in kilometers; and hc is the height constant of the building threat, in meters.

[0074] (2) Calculate the impact of the integrated threat model on the threat field:

[0075] First, use formulas (5) to (6) to obtain the discrete matrices Mx and My, and then substitute Mx and My into formula (1) to calculate the discrete matrix MT of the corresponding terrain threat model:

[0076] [Mx,My]=meshgrid(1:wd) (5)

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

[0078] In formulas (5) to (6), wd represents the scale range of the study space, and meshgrid is the function that generates grid sampling points;

[0079] Then, using formulas (7) to (10), contour lines are obtained from MT to get the terrain threat boundary set c:

[0080] [c, h] = contour(MT) (7)

[0081] hv=h.LevelList(h.LevelList>0) (8)

[0082] [rw,cw]=find(abs(c(1,:)-hv(1)<0.01 (9)

[0083] [xx, yy] = bufferm (x1, y1, 0.5, 'out') (10)

[0084] [in, on]=inpolygon(xx, yy, x1, y1) (11)

[0085] B=[xx(~in); yy(~in); hv] (12)

[0086] In formulas (4) to (10), ones(n, 1) and ones(1, m) represent generating n-row, 1-column and 1-row, m-column matrices, respectively, all of which are 1s. 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 and h.LevelList are the contour level number and the elevation value corresponding to the contour level, 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. ths ,rr, andcc 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 (11) to (12), in and on represent the serial numbers of the buffer boundary points inside the terrain threat and on the boundary, respectively; inpolygon represents the function for determining whether a point is inside a polygon; ~ is the sign for taking the opposite number; and the colon (:) is the sign for taking all values.

[0087] (3) Select suitable flight areas and construct flight route clusters

[0088] First, the shortest route pool boundary L under no terrain threat is generated using formulas (13) to (14):

[0089] [xx1, yy1]=bufferm([1 2 0]], [1 2 0], 6, 'out') (13)

[0090] L = [xx1; yy1; hv] (14)

[0091] In formulas (13) to (14), xs, ys, xs, and ys are the x and y coordinates of the starting point and the target point, respectively, and dl bf This represents the buffer distance of the shortest route.

[0092] Then, using formula (15), the shortest route pool boundary is used to select the area after removing terrain threats, generating a suitable flight zone that meets distance and safety constraints:

[0093] [in1,on1]=inpolygon(xx(~in),yy(~in),xx1,yy1) (15)

[0094] [in2,on2]=inpolygon(xr,yr,xx1,yy1) (16)

[0095] xr=ceil(rand(xrc,yrl)*(xe-xs+1)

[0096] yr=ceil(rand(xrc,yrl)*(ye-ys+1)

[0097] [in3,on3]=inpolygon(xr,yr,xx(in),yy(in)) (17)

[0098] P=[xr(in2&~in3)yr(in2&~in3)] (18)

[0099] In formulas (15) to (18), in1 and on1 represent the serial numbers of discrete points inside and on the shortest route pool set, respectively, excluding terrain threats; xr and yf represent the x and y coordinates of the randomly generated route set, respectively; xrc and yrl represent the number of rows and columns of the randomly generated route set, respectively; nin, non, and nnin represent the number of discrete points of V1 inside, on, and outside the distance ellipse, respectively; in2 and on2 represent the serial numbers of discrete points inside and on the boundary of the shortest route pool set, respectively, nin and on2 represent the serial numbers of discrete points inside and on the boundary of the randomly generated route set, respectively; in3 and on3 represent the serial numbers of discrete points inside and on the boundary of the randomly generated route set, respectively; and P represents the route set excluding terrain threats and located within the shortest route pool set.

[0100] The dynamic combination of route sets frs is achieved using formulas (20) to (23):

[0101] [xfd, si] = sort(xf) (20)

[0102] yfd=yf(si) (21)

[0103] zfd=zf(si) (22)

[0104] pfs = [xfd; yfd; zfd] (23)

[0105] In formulas (20) to (23), xfd and si represent the x coordinate values ​​after sorting in ascending order and their indices in the original xf, respectively; yfd and zfd represent the y and z coordinate values ​​corresponding to the sorted values, respectively; and frs represents the dynamic combination of the route set.

[0106] (4) Perform route evaluation and select the optimal initial route fp:

[0107] [fpv, fid]=min(sum(dist([xfd;yfd;zfd] T ))) (twenty four)

[0108] fp=[xfd(fid);yfd(fid);zfd(fid)] T

[0109] In formula (24), min, sum, and dist are functions for finding the minimum value, summing, and calculating the distance, respectively. The superscript T indicates the function for finding the matrix transpose. The minimum value fpv and the index fid of the sum of the comprehensive threat models of the track points are calculated by formula (24). fp obtains the optimal initial route with restricted route elevation.

[0110] (5) The optimal initial route is improved by using the self-adjusting Coulomb force method to achieve route flightability optimization:

[0111]

[0112] zp=kz*dh+fpz (26)

[0113]

[0114]

[0115] dx(i)=x(i+1)-x(i), i=1, 2,..., 6

[0116] dy(i)=y(i+1)-y(i), i=1, 2,..., 6

[0117] dh(i)=fpz(i+1)-fpz(i), i=1, 2,..., 6

[0118] mdx=mean([dx(i),dx(i+1),dx(i-1)])

[0119] mdy=mean([dy(i), dy(i+1), dy(i-1)])

[0120] mdz=dh

[0121] kx = -sign(dx)*cx / (mdx+eps)

[0122] ky = -sign(dy)*cy / (mdy+eps)

[0123] kz = -sign(dz)*cz / (mdz+eps)

[0124] cx = 0.789 / (mdx + eps)

[0125] cy = 0.789 / (mdy + eps)

[0126] cz = 1 / (mdz + eps)

[0127] In formulas (25) to (27), kx, ky, and kz represent the parameters corresponding to the route optimization adjustment, d, dx, dy, and dh represent the length of the route segment, the increment of the horizontal coordinate, the increment of the vertical coordinate, and the increment of the elevation, respectively. fn represents the number of route points in the route. xp, yp, and zp represent the x, y, and z coordinate values ​​of each track point after route optimization, respectively. fpx, fpy, and fpz represent the x, y, and z coordinate values ​​of each track point of the optimal initial route, respectively. mdx, mdy, and mdz represent the adjustment coefficients of the x, y, and z coordinate values ​​used for route optimization, respectively. cx, cy, and cz represent the adjustment constants of the x, y, and z coordinate values ​​used for route optimization, respectively. mean, sign, and eps represent the average value function, the sign function, and the floating-point relative precision function, respectively.

[0128] The experimental results are as follows: the planned flight distance is 34.4691 kilometers, which meets the requirement of a range of ≥50km. The planning time is 0.1891 seconds. The route planned using this method can meet the requirements of online planning (5 seconds). At the maximum level flight speed of 60km / h, the flight time of the route is 34.47 minutes, which meets the requirement of a range of ≥45 minutes. At the same time, the route selects a safe area for flight, which can realize rapid flight route planning and provide safety guarantee for emergency accident rescue.

Claims

1. A self-adjusting Coulomb force method for unmanned helicopter flight path planning, characterized by employing... The following are the implementation steps: (1) Constructing a comprehensive threat model T t The mathematical description is as follows: Using formulas (1) to (4), the discrete values ​​of the planar coordinates x and y in the airspace are obtained, and the T of the integrated threat model is calculated based on these values. t Threat level: T t (x,y)=z1(x,y)+z2(x,y)+z3(x,y) (1) z3(x, y)=hc, x∈(x1, x2), y∈(y1, y2) (4) In formulas (1) to (4), T t (x, y) represents the terrain threat value at the plane coordinates (x, y), in kilometers; e is an exponential function; z1, z2, and z3 are the non-flyable area model, terrain threat model, and building threat model, respectively, in meters; x1 and x2 are the start and end coordinates of the building threat on the x-axis, in kilometers; y1 and y2 are the start and end coordinates of the building threat on the y-axis, in kilometers; and hc is the height of the building threat, in meters. (2) Calculate the impact of the integrated threat model on the threat field: First, use formulas (5) to (6) to obtain the discrete matrices Mx and My, and then substitute Mx and My into formula (1) to calculate the discrete matrix MT of the corresponding terrain threat model: [Mx,My]=meshgrid(1:wd) (5) MT=T t (Mx,My) (6) In formulas (5) to (6), wd represents the scale range of the study space, and meshgrid is the function that generates grid sampling points; MT=T t (Mx,My) (6) Then, using formulas (7) to (10), contour lines are obtained from MT to get the terrain threat boundary set c: [c, h] = contour(MT) (7) hv=h.LevelList(h.LevelList>0) (8) [rw,cw]=find(abs(c(rr,cc)-hv<v ths ) (9) [xx,yy]=bufferm(x1,y1,d bf ,opt) (10) [in, on] = inpolygon (xx, yy, x1, y1) (11) B=[xx(~in); yy(~in); hv] (12) In formulas (7) to (10), c and h represent the generated terrain threat boundary set and its handle, respectively; contour, find, and abs represent the contour line function, search function, and absolute value function, respectively; h.LevelList represents the elevation values ​​corresponding to the contour line levels; 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 boundaries, 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 that generate the buffer, the distance to generate the buffer, and the parameters for selecting whether to generate an internal or external buffer, respectively; in formulas (11) to (12), in and on represent the serial numbers of the buffer boundary points within the terrain threat and on the boundary, respectively, inpolygon represents the function to determine whether a point is inside a polygon, and ~ is the sign for taking the opposite number; (3) Select suitable flight areas and construct flight route clusters First, the shortest route pool boundary L under no terrain threat is generated using formulas (13) to (14): [xx1,yy1]=bufferm([xs xe],[ys ye],dl bf ,opt) (13) L = [xx1; yy1; hv] (14) In formulas (13) to (14), xs, ys, xs, and ys are the x and y coordinates of the starting point and the target point, respectively, and dl bf This represents the buffer distance of the shortest route; Then, using formula (15), the shortest route pool boundary is used to select the area after removing terrain threats, generating a suitable flight zone that meets distance and safety constraints: [in1,on1]=inpolygon(xx(~in),yy(~in),xx1,yy1) (15) [in2,on2]=inpolygon(xr,yr,xx1,yy1) (16) xr=ceil(rand(xrc,yrl)*(xe-xs+1) yr=ceil(rand(xrc,yrl)*(ye-ys+1) [in3,on3]=inpolygon(xr,yr,xx(in),yy(in)) (17) P=[xr(in2&~in3)yr(in2&~in3)] (18) In formulas (15) to (18), in1 and on1 represent the serial numbers of discrete points inside and on the shortest route pool set, respectively, excluding terrain threats; xr and yr represent the x and y coordinates of the randomly generated route set, respectively; xrc and yrl represent the number of rows and columns of the randomly generated route set, respectively; nib, non, and nnin represent the number of discrete points of V1 inside, on, and outside the distance ellipse, respectively; in2 and on2 represent the serial numbers of discrete points inside and on the boundary of the shortest route pool set, respectively, in3 and on3 represent the serial numbers of discrete points inside and on the boundary of the randomly generated route set, respectively; and P represents the route set excluding terrain threats and located within the shortest route pool set. The dynamic combination frs of the route set is established using formulas (20) to (23): [xfd,si]=sort(xf) (20) yfd=yf(si) (21) zfd=zf(si) (22) pfs = [xfd; yfd; zfd] (23) In formulas (20) to (23), xfd and si represent the x coordinate values ​​after sorting in ascending order and their indices in the original xf, respectively; yfd and zfd represent the y and z coordinate values ​​corresponding to the sorted values, respectively; and frs represents the dynamic combination of the route set. (4) Perform route evaluation and select the optimal initial route fp: In formula (24), min, sum, and dist are functions for finding the minimum value, summing, and calculating the distance, respectively. The superscript T indicates the function for finding the matrix transpose. The minimum value fpv and the index fid of the sum of the comprehensive threat models of the track points are calculated by formula (24). fp obtains the optimal initial route with restricted route elevation. (5) The optimal initial route is improved by using the self-adjusting Coulomb force method to achieve route flightability optimization: zp=kz*dh+fpz (26) dx(i)=x(i+1)-x(i),i=1,2,…,fn-1 dy(i)=y(i+1)-y(i),i=1,2,…,fn-1 dh(i)=fpz(i+1)-fpz(i),i=1,2,…,fn-1 mdx=mean([dx(i),dx(i+1),dx(i-1)]) mdy=mean([dy(i), dy(i+1), dy(i-1)]) mdz=dh kx = -sign(dx)*cx / (mdx+eps) ky = -sign(dy)*cy / (mdy+eps) kz = -sign(dz)*cz / (mdz+eps) cx = c1 / (mdx + eps) cy = c² / (mdy + eps) cz = c3 / (mdz + eps) In formulas (25) to (27), kx, ky, and kz represent the parameters corresponding to the route optimization adjustment, d, dx, dy, and dh represent the length of the route segment, the increment of the horizontal coordinate, the increment of the vertical coordinate, and the increment of the elevation, respectively. fn represents the number of route points in the route. xp, yp, and zp represent the x, y, and z coordinate values ​​of each track point after route optimization, respectively. fpx, fpy, and fpz represent the x, y, and z coordinate values ​​of each track point of the optimal initial route, respectively. mdx, mdy, and mdz represent the adjustment coefficients of the x, y, and z coordinate values ​​used for route optimization, respectively. cx, cy, and cz represent the adjustment constants of the x, y, and z coordinate values ​​used for route optimization, respectively. mean, sign, and eps represent the average value function, the sign function, and the floating-point relative precision function, respectively.

Images