Air refueling-oriented close formation path planning method

Abstract

The invention discloses a close formation path planning method for air refueling, and belongs to the technical field of unmanned aerial vehicle air refueling path planning. The method comprises the following steps: acquiring key points of a receiver plane path in two stages of aerial refueling based on wind tunnel test data of a meeting stage and a formation entering stage, defining an eddy current negative influence area as an obstacle, and building a three-dimensional obstacle model for path planning to obtain a path planning map; performing initial planning in a map by using a key point heuristic algorithm based on the path key points and set constraint conditions; establishing a mathematical model of the fixed-wing unmanned aerial vehicle and analyzing differential flatness characteristics of the mathematical model; and establishing a cost function of a path optimization process by using a differential flat characteristic, and optimizing a path obtained by initial planning by using a flat output minimum mutation algorithm in combination with a constraint condition to obtain a final air refueling path. According to the method, the influence of eddy current on the oil receiving machine is considered while the safety is guaranteed, and a smooth and feasible path is obtained.

Description

Technical Field

[0001] The invention belongs to the technical field of aerial refueling path planning for unmanned aerial vehicles, and in particular relates to a close formation path planning method for aerial refueling. Background Art

[0002] Aerial refueling of unmanned aerial vehicles refers to the autonomous completion of the refueling task by the tanker and the receiving aircraft without the operation of the pilot. It is necessary to autonomously complete the links including rendezvous, pre-docking, docking, refueling and changing out of formation, so as to realize the automation of the aerial refueling function of unmanned aircraft. Aerial refueling can significantly improve the endurance and mounting capacity of fighter jets, increase the range and combat radius of combat aircraft, and extend the time in the air. The path planning of aerial refueling is to plan the path from the starting point to the target point under certain constraints, so that the specified performance indicators are optimal. The constraints mainly refer to spatial coordination constraints, temporal coordination constraints, and UAV constraints; performance indicators include path length, path smoothness, path safety, task completion time, etc. When the tanker and the receiving aircraft fly in a close formation during the aerial refueling operation, the tanker will generate wingtip vortices, which will have a great impact on the receiving aircraft. At the same time, the receiving aircraft needs to fly with a precise trajectory relative to the tanker to complete the refueling operation. Therefore, real-time and safe trajectory planning during the aerial refueling process is particularly important.

[0003] At present, many methods have emerged for the path planning problem of UAVs. However, most traditional path planning methods and intelligent path optimization algorithms have high computational complexity and are prone to falling into local optimal solutions. Moreover, most of them do not consider the influence of eddy currents and only plan paths in a two-dimensional environment without considering the influence of eddy currents. At the same time, most studies only consider spatial constraints when planning paths, but not temporal constraints, and cannot achieve UAV refueling path planning that meets continuous temporal and spatial changes. Summary of the invention

[0004] To solve the above problems, the present invention provides a close formation path planning method for aerial refueling, which can be used for path planning for aerial refueling in a vortex environment. While ensuring safety, the present invention takes into account the impact of vortices on the receiving aircraft and obtains a smooth and feasible path.

[0005] The present invention calculates the influence of vortex on the receiving aircraft at different positions based on the wind tunnel test data of the rendezvous phase and the formation phase, obtains the key points of the receiving aircraft path in the two stages of aerial refueling, defines the vortex negative impact area as an obstacle, builds a three-dimensional obstacle model for path planning, and obtains an aerial refueling path planning map accordingly; for the set constraints, according to the obtained path key points, the key point heuristic algorithm is used to perform initial planning in the map; establishes a mathematical model of a fixed-wing UAV, analyzes the differential flatness characteristics of the fixed-wing UAV, and uses the position of the UAV and the derivatives of each position to describe other state quantities and control quantities of the UAV system; uses the differential flatness characteristics to establish the cost function of the path optimization process, combines the constraints and uses the flat output minimum mutation algorithm to optimize the path obtained by the initial planning, and obtains the final aerial refueling path. While ensuring safety, the present invention takes into account the influence of vortex on the receiving aircraft and obtains a smooth and feasible path.

[0006] The technical solution adopted by the present invention is: a close formation path planning method for aerial refueling, the method comprising the following steps:

[0007] Step 1, based on the wind tunnel test data of the rendezvous and formation phases of aerial refueling, according to the changes of the lift coefficient, drag coefficient, roll, pitch and yaw moment coefficients of the refueling aircraft at different positions in the vortex, obtain the key points of the path of the refueling aircraft in the rendezvous and formation phases;

[0008] Based on the selected obstacle (e.g., defining the vortex negative impact area as the obstacle), a three-dimensional obstacle model for path planning is built to obtain an aerial refueling path planning map;

[0009] Step 2: Determine the constraints of the two phases of aerial refueling. According to the key points of the receiving aircraft's path in the rendezvous phase and the formation phase, use the key-point heuristic A* algorithm (KHA*) to perform initial path planning in the aerial refueling path planning map to obtain the nominal planning path.

[0010] Step 3, establish a fixed-wing UAV model, analyze the differential flatness characteristics of the fixed-wing UAV, and use the position of the UAV and its derivatives to describe the state and control quantities of the UAV system;

[0011] Step 4: Based on the differential flatness characteristics of the fixed-wing UAV, the cost function of the path optimization process is established. Based on the constraints of the two stages of aerial refueling, the Flat-output MinimumSnap algorithm (FMS) is used to optimize the nominal planning path. Optimize and obtain the final aerial refueling path.

[0012] Furthermore, in step 1, the key points of the path of the receiving aircraft in the rendezvous phase and the formation phase are determined based on the minimum value of the cost function W;

[0013] Among them, the expression of the cost function W is:

[0014]

[0015] Where Δcmx represents the rolling moment coefficient, Δcmy represents the pitching moment coefficient, Δcmz represents the yaw moment coefficient, K represents the lift-to-drag ratio (lift / drag), and λ i ,i∈[1,2,…,4] represents the weight of the corresponding rolling moment coefficient, pitching moment coefficient, yaw moment coefficient or lift-to-drag ratio.

[0016] Preferably, 1 =λ 2 =λ 3 =1,λ 4 =0.001.

[0017] Furthermore, in step 1, the area where the negative impact of the eddy current exceeds a set threshold is defined as an obstacle.

[0018] Furthermore, in step 2, the constraints include, but are not limited to: path length constraint, yaw angle constraint, pitch angle constraint, relative distance constraint between two aircrafts, etc.

[0019] Furthermore, step 2 includes:

[0020] The aerial refueling path planning map is divided into multiple three-dimensional elements according to a certain step length, and obstacle blocks and non-obstacle blocks are marked. Each three-dimensional element represents a path node (referred to as node);

[0021] Then use the KHA* algorithm to trigger from the set starting point, gradually pass through all the key points, and each node found between two key points must satisfy the minimum movement cost function. Then find each path node on the path in turn, and then connect it with the key points to form a nominal planning path.

[0022] The movement cost function includes the movement cost from the starting point to the path node and the estimated movement cost from the path node to the end point.

[0023] Furthermore, step 3 includes:

[0024] Define the coordinate system G i ,G b and G s They represent the inertial reference frame, the body reference frame and the stable reference frame respectively;

[0025] Among them, the inertial reference system G i The origin of x is located at a point on the ground. i The axis lies in the ground plane and points in a particular direction, y i Axis and x i The axes are in the same plane and perpendicular to x i Axis, z i The axis is vertically downward, pointing toward the center of the Earth;

[0026] Body reference frame G b The origin is located at the center of mass of the drone, x b The y axis is in the symmetry plane of the drone, parallel to the fuselage axis of the drone or the average aerodynamic chord of the wing, and points to the direction of the drone's nose. b The axis is perpendicular to the symmetry plane of the drone and points to the right side of the fuselage. b With x b The axis is vertical and points downwards from the drone;

[0027] Stable reference frame G s The origin is located at the center of mass of the drone, x s The axis coincides with the projection of the drone's airspeed in the symmetry plane, y s Axis and body coordinate axis y b Overlap, z s In the symmetry plane of the aircraft s Vertical and pointing down the fuselage;

[0028] Define the regular basis vector in the three-dimensional coordinate system as e 1 ,e 2 and e 3 , the fixed-wing UAV model is established as:

[0029]

[0030] Among them, O, v, m, ω are the position, velocity, mass and angular velocity of the receiving aircraft respectively, g is the acceleration of gravity, ω c is the specified angular rate, α is the angle of attack, and D is the velocity of the drone along the negative x b The drag force of the UAV along the x axis, T is b The thrust generated by the axis, L is the thrust generated by the UAV along the negative z axis. b The lift generated by the shaft; Denotes the stable coordinate system G s To the inertial coordinate system G i The stability-inertia selection matrix, Represents the coordinate system G b The position O in the coordinate system G i In the expression The first derivative of is the coordinate system G b The velocity in the coordinate system G i In the expression, for The first derivative of In the coordinate system G i Measurement coordinate system G b The angular velocity in the coordinate system G b In the statement, Represents the coordinate system G b To coordinate system G i The body-inertia selection matrix, for The first derivative of .

[0031] Furthermore, the drag in the fixed-wing drone model is set to:

[0032]

[0033] Where ρ is the air density, V a is the airspeed of the drone, S is the plane area of ​​the wing, c is the chord length of the wing, and is the aerodynamic coefficient.

[0034] Furthermore, in step 3, the airspeed of the drone in the absence of wind is defined as And define the state x and control input u of the drone system as:

[0035]

[0036] The state x and control input u are described by the flat output z and its higher-order derivatives as:

[0037]

[0038] Furthermore, in step 4, the cost function of the path optimization process is established based on the differential flatness characteristics of the fixed-wing UAV, including:

[0039] According to the differential flatness characteristics of fixed-wing UAVs, based on the current coordinate position x of the UAV in space T (t),y T (t),z T (t) The nominal planning path corresponding to time t is described as:

[0040]

[0041] Among them, the superscript T represents the transposition, and the subscript T is used to represent the current;

[0042] The path is divided into several segments based on the key points of the receiving aircraft path, and t is defined.i (i=1,2,…,j) represents the key point number, t i represents the time corresponding to the i-th key point, j represents the total number of key points, t 0 Indicates the start time, then Described as:

[0043]

[0044] Minimize the fourth-order derivative of the drone’s position to obtain a smooth path for each segment (i.e., a path segment, which is a section of road between two path nodes):

[0045]

[0046] Among them, the drone position r = [x T (t),y T (t),z T (t)] T .

[0047] Furthermore, step 4 also includes time allocation optimization:

[0048] Let the time allocation vector τ = [T 1 ,T 2 ,…,T j ], where T i (i=1,2,…,j) represents the time length allocated to the i-th segment;

[0049] An initial value is set for the time allocation vector τ, and then the gradient descent method is used to iteratively optimize τ until the preset iterative convergence condition is reached, and the final time allocation vector τ is obtained.

[0050] Furthermore, in step 4, the flat output minimum mutation algorithm is used to calculate the nominal planning path When optimizing, set the safety corridor constraint and define the distance from the point on the path at time t to the corresponding segmented straight line segment as:

[0051] d i (t)=(r T (t)-r i )-((r T (t)-r i )·s i )s i (9)

[0052] Among them, r T (t) is the path at time t, r i is the i-th key point, s i For along r i Point to r i+1The unit vector of

[0053] Define the width of the safety corridor as ε i , requiring that the distance that the path deviates from the straight line segment does not exceed the width of the safety corridor, then the safety corridor constraint is described as:

[0054] |d i (t)|≤ε i ,t i ≤t≤t i+1 (10)

[0055] All the key points of the path are taken from the straight line path. Therefore, the path at the key points must satisfy the safety corridor constraint.

[0056] The denser the key points are, the closer the path is to the straight line path. Therefore, the path can be made closer to the straight line by introducing the middle point as a new key point, avoiding the collision of the drone with obstacles due to deviation from the straight line.

[0057] By introducing intermediate points in the segments that do not meet the constraints and re-optimizing the path, after several iterations, it can be ensured that the path is within the safe corridor. The path after introducing the intermediate points needs to meet the following requirements:

[0058]

[0059] Among them, n c To introduce the number of intermediate points, the final path generated after optimization is obtained.

[0060] The technical solution provided by the present invention brings at least the following beneficial effects:

[0061] (1) For UAV aerial refueling, the present invention can plan a safe, smooth and optimal path under the influence of vortex.

[0062] (2) The present invention uses two-layer planning of the front end and the back end. The front end uses the KHA* algorithm combined with the aerodynamic analysis results to plan an initial path. The back end uses the FMS algorithm to add safety corridor constraints and optimize the time and smoothness of path planning to obtain the optimal path, thereby ensuring safety and improving planning efficiency.

[0063] (3) The present invention is simple and easy to use, and is convenient for engineering implementation. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0065] Figure 1 The distribution diagram of Δcmx, Δcmy, Δcmz and K of the receiving machine when the extension is 1.5 times, and the black dots indicate the corresponding extreme values.

[0066] Figure 2 The distribution diagram of Δcmx, Δcmy, Δcmz and K of the receiving machine when the extension is 3 times, and the black dots indicate the corresponding extreme values.

[0067] Figure 3 The distribution diagram of Δcmx, Δcmy, Δcmz and K of the receiving machine when the extension is 5 times, and the black dots indicate the corresponding extreme values.

[0068] Figure 4 The distribution diagram of Δcmx, Δcmy, Δcmz and K of the receiving machine when the extension is 10 times, and the black dots indicate the corresponding extreme values.

[0069] Figure 5 The distribution diagram of Δcmx, Δcmy, Δcmz and K of the receiving machine when the extension is 15 times, and the black dots indicate the corresponding extreme values.

[0070] Figure 6 Schematic diagram of the vortex distribution generated by the tanker wing.

[0071] Figure 7 To customize the 3D environment map based on the eddy distribution, the eddies are modeled as cylinders in the figure.

[0072] Figure 8 This is the force analysis diagram of the fixed-wing UAV.

[0073] Fig. 9 This is the initial path diagram planned in the vortex map using the KHA* algorithm. The path is represented by a solid line in the figure, and the key points are represented by “*” points in the figure.

[0074] Fig.10 This is the path map after the initial path in the vortex map is optimized by the FMS algorithm.

[0075] Fig.11 This is the collision diagram between the path and the obstacle when there is no safety corridor constraint.

[0076] Fig.12 This is the path diagram after adding the safety corridor constraint.

[0077] Fig.13 This is a graph showing the change in pitch angle during path planning.

[0078] Fig.14 This is a graph showing the change in roll angle during path planning. DETAILED DESCRIPTION

[0079] To make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be described in detail and completely in conjunction with the drawings in the implementation of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings can be arranged and designed using different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the present application for protection, but merely represents selected embodiments of the present invention.

[0080] The embodiment of the present invention proposes a close formation path planning method for aerial refueling, which is used to reduce the adverse effects of the tanker vortex during the aerial refueling process and ensure the safety, smoothness and economy of the path of the UAV during the aerial refueling process. The concept of the embodiment of the present invention is: first, analyze the two stages of forming a formation, including the rendezvous stage and the stage of entering the formation; then, calculate the influence of vortices on the receiving aircraft at different positions based on wind tunnel test data, thereby obtaining the key points of the receiving aircraft path in the two stages of aerial refueling, and define the vortex negative impact area as an obstacle body, build a three-dimensional obstacle model for path planning, and obtain an aerial refueling path planning map based on this; determine different constraints for the situation of being affected by vortices in the rendezvous stage and the stage of entering the formation, and use the key point heuristic A* algorithm (KHA*) to perform initial planning in the map according to the obtained path key points; establish a mathematical model of a fixed-wing UAV, analyze the differential flatness characteristics of the fixed-wing UAV, and use the position of the UAV and the derivatives of each position to describe other state quantities and control quantities of the UAV system; use the differential flatness characteristics to establish a cost function of the path optimization process, use the flat output minimum mutation algorithm (FMS) in combination with the constraints, optimize the path obtained by the initial planning, and obtain the final aerial refueling path.

[0081] As a possible implementation, the close formation path planning method for aerial refueling provided in an embodiment of the present invention specifically includes the following steps:

[0082] Step 1: Analyze the wind tunnel test data, obtain the key points of the path during aerial refueling, and build a map for path planning;

[0083] Based on the wind tunnel test data, the aerodynamic data of the receiving aircraft in the two stages of aerial refueling (rendezvous stage and formation stage) were analyzed. The aerodynamic data were used to calculate the impact of vortices on the receiving aircraft at different positions, including the changes in the lift coefficient, drag coefficient, roll, pitch and yaw moment coefficients of the receiving aircraft at different positions in the vortex. The key points of the receiving aircraft's path in the two stages of aerial refueling were obtained, and the areas with greater negative impacts of vortices were defined as obstacles. A three-dimensional model for path planning was built and defined as a map for path planning.

[0084] The two stages of aerial refueling are analyzed, and the influence of vortex exists in both stages:

[0085] 1) Meeting stage:

[0086] Before entering the formation, the tanker and the receiving aircraft need to prepare for rendezvous. First, the tanker should arrive and cruise along the waiting route at the lowest fuel consumption rate. At this time, the receiving aircraft needs to gradually approach from behind the left wing of the tanker, always maintaining a vertical spacing at a standard height (such as at least 305 meters). Until the longitudinal spacing between the two sides is reduced to the standard spacing (such as 1852 meters), the two aircraft should achieve visual contact to verify each other's identity. Subsequently, the receiving aircraft joins the refueling route and climbs steadily, gradually approaching the tanker, and reaches the rendezvous stage target point (also known as the rendezvous stage end point).

[0087] 2) Formation phase:

[0088] When the longitudinal distance between the two sides is reduced to within the target distance (such as 500 meters), the receiving aircraft starts from the end of the rendezvous phase, and joins the formation from the left after confirming that there are no other aircraft behind the tanker. After reaching the desired position behind the tanker, the formation phase is completed.

[0089] Based on the wind tunnel test data, the aerodynamic analysis of the receiving aircraft in the two stages of aerial refueling (rendezvous stage and formation stage) is carried out. The wingspan of the receiving aircraft is defined as b. The key points of the path are selected based on the data of five cross-sections with the longitudinal distance X between the two aircraft being 1.5, 3.0, 5.0, 10.0 and 15.0 times the span. The key points are the positions where the lift-to-drag ratio is the largest and the moment coefficient changes the least. The data analysis is as follows: Figures 1 to 5 As shown. The data in the formation phase is within five times the span of the longitudinal spacing, and the data in the rendezvous phase is greater than five times the span. As shown in the following table, K is the lift-to-drag ratio, Δcmx, Δcmy and Δcmz are the coefficient changes of the rolling, pitching and yaw moments, respectively, and X, Y, and Z are the relative distances of the two aircraft in the longitudinal, lateral and vertical directions, respectively, and the unit is b = 50m.

[0090] Table 1: Longitudinal spacing is 1.5 times the extension length

[0091]

[0092] Table 2 The longitudinal spacing is 3 times the extension length

[0093]

[0094] Table 3 The longitudinal spacing is 5 times the extension length

[0095]

[0096]

[0097] Table 4: Longitudinal spacing is 10 times the extension length

[0098]

[0099] Table 5: Longitudinal spacing is 15 times the extension length

[0100]

[0101] It can be seen from the above table that when the longitudinal spacing between the two machines is 1.5 times the span, Y=-0.375b, Z=0.14b is the position with the maximum gain and the position with relatively small change in moment coefficient; when the longitudinal spacing between the two machines is 3 times the span, the position with the maximum gain is also the position with the smallest change in Δcmx. For Δcmy and Δcmz, the positions with the smallest changes are respectively around z=-0.25b and z=0.043b; when the longitudinal spacing between the two machines is 5 times the span, the position with the maximum gain is z=-0.21b, the positions with the smallest changes in Δcmx and Δcmy are almost the same at z=-0.14b, and the position with the smallest change in Δcmz is around z=-0.04b.

[0102] When the two aircraft were between 1.5b and 5b apart, it was found that when Y=-0.375b and Z=-0.12b, the three situations all achieved the maximum gain at this time. At the same time, with the increase of the longitudinal spacing, the rolling moment coefficient reached the minimum value at 3 times the span, the pitch moment gradually decreased, and the yaw moment had no obvious change.

[0103] When the longitudinal spacing between the two machines is 10 times the span, the position with the maximum gain is also the position with the minimum disturbance of Δcmy. For Δcmx and Δcmz, the positions with the maximum gain and the minimum disturbance are conflicting, especially for Δcmx, which is completely opposite.

[0104] When the longitudinal distance between the two machines is 15 times the span, the position with the maximum gain is also the position where the disturbances of Δcmy and Δcmx are the smallest, so the position with the maximum gain can be used directly.

[0105] From this, the cost function W can be established as:

[0106]

[0107] where λ i , i∈[1,2,…,4] represents the weight corresponding to each item, and the parameters are taken as λ according to the degree of change of the four variables. 1 =λ 2 =λ 3 =1,λ 4 =0.001, minimum cost W min The corresponding position is the path key point.

[0108] According to the aerodynamic data at different cross-sectional positions, the corresponding minimum cost positions at different cross-sectional positions are obtained, as shown in Table 6, where the unit is span b.

[0109] Table 6 Key point positions

[0110]

[0111] At the same time, the area with greater negative impact of vortex is obtained and modeled as an obstacle. The schematic diagram of the vortex distribution generated by the wing of the tanker is shown in Figure 6 As shown in the figure, we can see the distribution of vortices behind the wing. By analyzing the vortices, we can define the area with the greatest impact as a three-dimensional cylindrical obstacle, and build a three-dimensional model for path planning as shown in the figure below. Figure 7 As shown, this model is defined as a map for path planning.

[0112] Step 2: determine the constraints of the two phases of aerial refueling and use the KHA* algorithm for initial path planning;

[0113] In the process of path planning, it is necessary to consider the path length constraint, yaw angle constraint, pitch angle constraint, relative distance constraint between the two aircrafts, etc.; combined with the key path points obtained in step 1, without considering its own kinematic and dynamic constraints, the KHA* algorithm is used in the constructed 3D environment model for initial path planning to obtain an initial path without considering the constraints.

[0114] In the path planning process, the constraints that need to be considered are:

[0115] 1) Path length constraint:

[0116] The path length is used to represent the flight distance of the drone. Let L max is the maximum flight distance of the UAV. Then, the path length constraint is calculated as

[0117]

[0118] Among them, (x i ,y i ,z i),i=1,2,...,N,is the coordinate of the i-th path point in the three-dimensional planning environment, and N is the total number of path points.

[0119] 2) Yaw angle constraint:

[0120] Considering the smoothness of the path, the maximum turning angle is introduced to constrain the turning angle, that is, the yaw angle ψ. Assume that vector a i is equal to [x i -x i-1 ,y i -y i-1 ,] T ,||a i || is the vector a i The yaw angle constraint of the UAV is

[0121]

[0122] Among them, ψ max Indicates the maximum yaw angle of the drone.

[0123] 3) Pitch angle constraint:

[0124] Similar to the turning angle, the climbing angle is the pitch angle, and the pitch angle θ can be calculated as:

[0125]

[0126] Among them, θ max is the maximum pitch angle, -θ max is the minimum pitch angle.

[0127] 4) Relative distance constraints between two machines:

[0128] The drone must maintain a relative distance after changing its flight attitude or turning, and will not turn frequently. Therefore, it is necessary to limit the path segment between adjacent path points to be greater than the shortest path segment length L min . The path segment constraint can be expressed as

[0129] L min -||p i+1 -p i ||≤0 (16)

[0130] Among them, p i+1 Yes(x i+1 ,y i+1 ,z i+1 ), p i Yes(x i ,y i ,z i ), p i+1 and p i are two adjacent waypoints.

[0131] The KHA* algorithm considers the cost of moving from the starting point to the node and the estimated cost of moving from the node to the end point, and considers the key path points obtained in step 1, without considering its own constraints. Before path planning, the map needs to be simplified into a set of quantifiable nodes. The map area is divided into many (three-dimensional) elements according to a certain step size. In order to further improve the accuracy of path planning, the divided elements are usually small, that is, divided according to a smaller step size (which can be set based on the actual scenario so that the divided elements are sufficient), and the divided elements can be called small elements. Each element records the position of the center of mass, and marks the element as an obstacle element or a non-obstacle element (manual marking) according to whether the center of mass is in the obstacle. Considering the volume of the element and the radius of the drone, the obstacle is widened so that more elements are recorded as being in the obstacle, to ensure that when the center of mass of the drone enters the element marked as a non-obstacle, it will not collide with the obstacle due to the influence of the drone volume and the element volume.

[0132] After completing the map division, the path planning can be carried out. The KHA* algorithm starts from the rendezvous starting point s to the docking end point. Each search evaluates the current search position through the cost function C(n). The cost function C(n) is:

[0133] C(n)=p(n)+q(n) (17)

[0134] Among them, n represents the nth node, p(n) is the actual cost function from the initial node s to the current node n, q(n) is the estimated cost function from the current node n to the terminal node o (that is, the moving cost from the current node to the terminal node o, such as the moving cost measured by the Manhattan distance between the current node and the terminal node o), combined with the key points of the aerodynamic constraints, together serve as the heuristic function of the A* algorithm.

[0135] Then, the accumulated cost function p(n e ) and q(n e ):

[0136] p(n e )=p(n)+Cnn e (18)

[0137]

[0138] Among them, x ,o y ,o z ,n x ,n y ,n z are the coordinates of the target point and the current point, respectively, Cnn e From node n to node ne Starting from the starting point, all key frames and key points constitute path nodes. Each round of search between adjacent nodes is based on the minimum cost function C(n). Each point is connected in sequence to obtain a nominal planning path from the meeting starting point to the docking end point, which is recorded as

[0139] In this embodiment, the processing process of the heuristic A* algorithm includes:

[0140] The input parameters are the initial node s (starting point) and the terminal node o (target point), and the output result is a nominal planning path connecting the initial node s and the terminal node o.

[0141] First, initialize the priority queue as a container for storing the nodes to be expanded, and set the actual cost function value of the initial node s to 0, and the actual cost function values ​​of the remaining nodes to infinity.

[0142] When the priority queue is not empty, the loop process begins. In the loop, the node n with the smallest C(n) value in the queue is first removed and marked as expanded. If node n is exactly the target node o, the loop terminates; for all unexpanded neighbor nodes n of node n, e If n is found e The actual cost function value is still infinite, indicating that n e It has not been expanded yet. At this time, based on the actual cost function value of node n (if node n is the initial node s, its value is 0; otherwise it is the value after the most recent update), p(n) is calculated according to formulas (18) and (19). e ) and q(n e ), that is, to calculate the cumulative cost function p(n e ) and q(n e ), and n e Join the queue. If a neighbor node n is found e p(n e ) and q(n e ) is greater than the sum of p(n) and Cnn e The sum of n e Has been expanded, now needs to be updated e The current actual cost function value is p(n)+Cnn e .

[0143] When all nodes in the queue have been expanded, the sequence of nodes traversed by the algorithm is arranged in reverse order to obtain the nominal planning path.

[0144] Step 3, establish a mathematical model of the fixed-wing UAV and analyze the differential flatness characteristics of the fixed-wing UAV;

[0145] A mathematical model of fixed-wing UAV is established, and the differential flatness characteristics of the fixed-wing UAV are analyzed. The state and control quantities represented by flat outputs are obtained, that is, the state and control quantities of the UAV system can be described by the position of the UAV and its derivatives of various orders.

[0146] Define the coordinate system as a right-handed coordinate system, represented by G a ={i a ,j a ,k a}, where i a ,i a and k a is the three-dimensional (x, y, z) basis vector. The expression In the coordinate system G b Measured in coordinate system G a The vector x in the coordinate system G c In the statement, Indicates that the coordinate system G a The coordinates in are converted to coordinate system G b The rotation matrix of the coordinates in . The regular basis vectors in (three-dimensional real number field) are denoted by e 1 ,e 2 and e 3 . Coordinate system G i ,G b and G s They represent the inertial reference system, the body reference system, and the stable reference system. The inertial reference system can also be called the ground coordinate system, which is used for orientation indication and navigation control. Its origin is located at a point on the ground, and the x-axis (i.e., i a ) is located in the ground plane and points in a specific direction. The y-axis (i.e., j a ) is in the same plane as the x-axis and is perpendicular to the x-axis. Its direction follows the right-hand rule. The z-axis (i.e., k a ) vertically downward, pointing to the center of the earth. The body reference system is used to describe the inherent motion of the drone, and its origin is located at the center of mass of the drone, the x-axis (i.e., the i b ) is in the symmetry plane of the UAV, parallel to the fuselage axis of the UAV or the average aerodynamic chord of the wing, and points to the direction of the nose of the UAV. The y-axis (i.e., j b ) axis is perpendicular to the symmetry plane of the drone and points to the right side of the fuselage. The z axis (i.e. k b ) is perpendicular to the x-axis and points downwards. The stable reference frame is used to describe the motion characteristics of the drone after a small disturbance. Its origin is located at the center of mass of the drone, and the x-axis (i.e., i s ) coincides with the projection of the UAV airspeed in the symmetry plane, and the y-axis (i.e. j s ) and the body coordinate axis j b Coincident, z axis (ie k s) in the symmetry plane of the aircraft with i s Vertically and pointing down the fuselage.

[0147] set up In the generalized vector The skew-symmetric matrix operator is defined on

[0148]

[0149] The mathematical model of the fixed-wing UAV is established as follows, and the force analysis of the UAV is as follows Figure 8 As shown:

[0150]

[0151] Where O is the position of the "unmanned aerial system", and the i, b, and s in the subscripts represent the variables defined in the inertial coordinate system, the body coordinate system, and the stable coordinate system, respectively. Denotes the stable coordinate system G s To the inertial coordinate system G i The stability-inertia selection matrix is: v is the velocity of the drone, g is the acceleration of gravity, m is the mass of the system, and the angular velocity of the drone is ω. Represents the coordinate system G b The position O in the coordinate system G i In the expression The first derivative of is the coordinate system G b The velocity in the coordinate system G i In the expression, for The first derivative of In the coordinate system G i Measurement coordinate system G b The angular velocity in the coordinate system G b In the statement, Represents the coordinate system G b To coordinate system G i The body-inertia selection matrix, for The first derivative of . That is, the symbol "" on the parameter head represents its first-order derivative, and the symbol "" represents its second-order derivative. ω c is the specified angular rate, α is the angle of attack, The drone is along the negative body axis i b resistance, is the UAV along the body axis i b The thrust generated, is the UAV along the negative body axis k b To simplify the model, assume that the rudder deflection, sideslip angle and angle of attack α 0The deviations from are relatively small and the drag due to the control surfaces can be neglected. Using these assumptions, the drag term simplifies to

[0152]

[0153] Where ρ is the air density, is the airspeed of the drone, S is the plane area of ​​the wing, c is the chord length of the wing, and is the aerodynamic coefficient.

[0154] Assuming there is no wind, the airspeed of the drone is

[0155] Assume that there is no wind during the flight of the drone, and the angle of attack is constant and equal to the trim angle of attack α 0 And the sideslip angle is zero, the state and input of the drone system are defined as:

[0156]

[0157] Let the stability-inertia rotation matrix Expressed as

[0158]

[0159] Among them, r j is the jth stable coordinate system axis of the fixed wing, expressed in an inertial coordinate system. According to the above two assumptions, the speed of the drone is related to the stable axis i s Alignment, i.e.

[0160]

[0161] in, is the z-axis of the body reference system (z b ) direction vector z b The first derivative of .

[0162] Accordingly, we can get

[0163]

[0164] Take r 1 The dot product of

[0165]

[0166] Accordingly, we can get

[0167]

[0168] Because ||r 3 ||=1, then we can conclude

[0169]

[0170] There must be

[0171] r 2 =r 3 × 1 (30)

[0172] Then the body-inertia rotation matrix is ​​calculated using the stability-inertia rotation matrix as

[0173]

[0174] in

[0175]

[0176] Rotation Matrix Can be written as flat output Function of , angular velocity vector:

[0177]

[0178] Define the “∨” operator as

[0179]

[0180] The angular velocity can be expressed in terms of the plane output as

[0181]

[0182] The thrust T can be obtained as

[0183]

[0184] Using V a and Is defined, and Give available

[0185]

[0186] It can be concluded that all states x and control inputs u of the system can be described by flat output z and its derivatives. That is, and

[0187] Then the system is differentially flat, and the flat output can be obtained as:

[0188]

[0189] It can be seen that all states and inputs of the fixed-wing UAV can be defined by the position of the UAV and its derivatives.

[0190] Step 4, using the differential flatness characteristics of fixed-wing UAVs, establish the path function of the path optimization process, use the FMS algorithm to optimize the initial path, and obtain the final path;

[0191] Combined with the differential flatness characteristics of the fixed-wing UAV in step 3, it can be obtained that the state quantity and control quantity of the UAV system can be described by the position of the UAV and the derivatives of each position. This feature is used to establish the cost function of the path optimization process. Considering the path smoothness, flight time, safety and the constraints in step 2, the FMS algorithm is used to optimize the initial path generated in step 2 to obtain the final path.

[0192] Based on the analysis of the differential flatness characteristics of fixed-wing UAVs, the path of fixed-wing UAVs It can be described by the following formula:

[0193]

[0194] Among them, x T (t),y T (t),z T (t) is the coordinate position of the drone in space. In order to facilitate the optimization of the path, a set of key points is used to divide the path into several segments, i represents the key point number, t i represents the time corresponding to the i-th key point, and j represents the total number of key points.

[0195]

[0196] In the process of fixed-wing UAV path planning, in order to make the path of the UAV as smooth as possible, the fourth-order derivative of the position should be minimized to obtain a smooth path. Specifically for a certain segment i, that is:

[0197]

[0198] Where r = [x(t), y(t), z(t)] T is the coordinate position of the drone in space.

[0199] The three parameters of the flat output are decoupled from each other. Therefore, the three parameters (three-dimensional coordinates) of the flat output can be optimized separately to achieve flat optimization.

[0200] Take parameter x as an example to optimize the path. Use an n-order polynomial with time t as the independent variable to describe the path of a certain segment i of a certain parameter of the drone, that is:

[0201] p i (t) = p i0 +p i1 t+pi2 t 2 +…+p in t n ,t i-1 ≤t≤t i (42)

[0202] Define the polynomial coefficient vector P i as follows:

[0203] P i =[p i0 ,p i1 ,p i2 ,...,p in ] T (43)

[0204] Then the polynomial path can be expressed as:

[0205] p i (t)=[0,t,t 2 ,…,t n ]P,t i-1 ≤t≤t i (44)

[0206] P is the polynomial coefficient matrix. If the position p(t) of the drone is known, the speed, acceleration, jerk and snap of the fixed-wing drone can be obtained by calculating the derivatives of each order (jerk is the third-order derivative of the position with respect to time, and snap is the fourth-order derivative of the position with respect to time). The overall path is divided into j segments according to the key points, which can be written as:

[0207]

[0208] P i =[p i0 ,p i1 ,p i2 ,…,p in ] T ,i=1,2,…,j (46)

[0209] You can Rewritten in matrix form in the direction of the parameters currently being solved:

[0210]

[0211] Among them, the matrix Matrix Q i Lower right corner block Q i,right The elements satisfy: Where r' is the row index, c' is the column index, and r'≥5,c'≤n+1.

[0212] According to the matrix of the entire path, the form of the entire path can be obtained:

[0213]

[0214] Among them, the matrix

[0215] The optimization problem can be written as follows:

[0216] minP T QP (49)

[0217] stAP=b (50)

[0218] Vector b stores the values ​​of various parameters at the key frame, which correspond to the constraints in matrix A one by one. AP = b is the optimized constraint. Taking the i-th segment as an example, the constraints include:

[0219] 1) The drone is at the end of the segment t i At time, its position p i , speed v i , acceleration a i , jerk i 、snap i is a specific value, forming 5 equality constraints;

[0220] [1,t i ,t i 2 ,…,t i n ]P i =p i (51)

[0221] [0,1,2t i ,3t i 2 ,4t i 3 …,nt i n ]P i =v i (52)

[0222] [0,0,2,6t i ,12t i 2 ,…,n(n-1)t i n-2 ]P i =a i (53)

[0223]

[0224] 2) The parameters of the drone should not change suddenly at the segment boundary, that is, the difference between the starting time of the i-th segment and the end time of the i-1-th segment is 0, forming 5 equality constraints, called continuity constraints;

[0225] [1,t i-1 ,t i-1 2 ,…,t i-1 n ]P i -[1,t i-1 ,t i-1 2 ,…,t i-1 n ]P i-1 =0 (56)

[0226] [0,1,2t i-1 ,3t i-1 2 ,4t i-1 3 …,nt i-1 n ]P i -[0,1,2t i-1 ,3t i-1 2 ,4t i-1 3 …,nt i-1 n ]P i-1 =0(57)

[0227] [0,0,2,6t i-1 ,12t i-1 2 ,…,n(n-1)t i-1 n-2 ]P i -[0,0,2,6t i-1 ,12t i-1 2 ,…,n(n-1)t i-1 n-2 ]P i-1 =0 (58)

[0228]

[0229] These 10 equality constraints are written in matrix form as AP=b, which means that the polynomial coefficients of the target path can be solved to obtain a path described by a polynomial.

[0230] Optimizing the time allocation during the planning process can prevent the paths from overlapping. The time allocation vector τ = [T1 ,T 2 ,…,T j ], where T i , (i=1,2,…,j) represents the time length assigned to the i-th segment. Then the time t corresponding to each key point i There are the following relationships:

[0231]

[0232] First, an initial value is set for τ, and then the gradient descent method is used to iteratively optimize τ. There are several ways to set the initial value: ① Give each segment the same time length as the starting value. ② According to the proportion of the straight path length in each segment, the initial time value is allocated accordingly. ③ On the basis of considering the acceleration and deceleration process, the time is allocated according to the length of each segment path to reflect the actual flight speed change requirements.

[0233] Taking the time length of each segment as the independent variable, define the cost function J:

[0234]

[0235] Find the gradient of J Optimize τ using the gradient descent algorithm:

[0236]

[0237] Among them, β represents the step size of each iteration, τ k represents the time allocation vector for the kth iteration.

[0238] Repeat the above process iteratively until the cost function meets certain indicators, such as the cost function is less than a given value, or the change in the cost function between two iterations is less than a certain proportion, then the optimal segmentation time optimization is considered to be completed.

[0239] In order to prevent collisions during the planning process, it is necessary to add safety corridor constraints, as shown in formulas (10) and (11).

[0240] The steps are verified through the implementation examples. After data analysis, the key points of the path of the receiving aircraft during aerial refueling and the three-dimensional model for path planning are obtained; the KHA* algorithm is used to perform initial planning in the map to obtain the initial path; the differential flatness characteristics of the fixed-wing UAV are analyzed, and it can be obtained that the state quantity and control quantity of the UAV system can be described by the position of the UAV and the various derivatives of the position; this feature is used to establish the cost function of the path optimization process, and the FMS algorithm is used to optimize the path obtained by the initial planning in combination with the constraint conditions to obtain the final path.

[0241] Example

[0242] In the MATLAB simulation, the KHA* algorithm is used for unconstrained initial path planning. The map is modeled according to the existence of eddy currents. Figure 7 The basic parameters are shown in Table 7, and the obstacle is the cylinder in the figure.

[0243] Table 7 Basic parameters of planning map

[0244]

[0245] Input the map and the starting and ending point information into the KHA* algorithm module to obtain an initial path such as Figure 8 As shown, each point on the path records the three-dimensional coordinates of the point. Figure 8 It can be seen that the path generated by KHA* has a large turning angle at the corner and is not smooth enough. The path is represented by a solid line in the figure. The starting point is the first key point, and then the next point is retrieved in sequence. Check whether the line connecting this key point and the next target point collides with an obstacle. If not, check the next point. If yes, use the previous point as the new key point, and then use the same method to find the next key point. The key point is represented by a "*" point in the figure. The coordinates of the 6 key points are shown in Table 8:

[0246] Table 8 Key point numbers and coordinates

[0247]

[0248] After obtaining the key points, a polyline path is generated. The polyline path is not smooth enough and has a large turning angle, so FMS optimization is required. The square of the norm of the fourth-order derivative of the position is used as the cost function. At the same time, the constraints at different stages are considered. The constraint item parameter settings are shown in Table 9. On the basis of satisfying the constraints, the cost function is minimized as the goal to optimize, and a segmented path can be obtained. Each segment is described by a polynomial. After obtaining the path, it is necessary to check whether each segment of the path has collided with the obstacle. A certain number of points are taken from the path at a certain sampling rate, the position of each point on the path is calculated, and then the point is determined whether it is located in the obstacle element according to the previously loaded map information. If any point on a path is located in the obstacle element, the path is marked as needing a safe corridor constraint and the constraint is added. After adding the constraint, the path is generated again, and then the collision detection is performed again. If there is still a collision, a narrower safe corridor width is added until no collision occurs. In order to avoid path overlap, the path is optimized in time to obtain the final path.

[0249] Table 9 Smoothing optimization constraint parameter settings

[0250]

[0251] The generated initial path is optimized to obtain the optimized path as follows: Fig.10 As shown, it can be seen Fig.10 and Fig. 9 It is smoother than in the corners; Fig.11 , Fig.12 This is a detailed diagram of adding a safety corridor constraint after a path segment collides with an obstacle. It can be seen that when the safety corridor constraint is not added ( Fig.11 ) The planned path collides with the obstacle. The collision path segment is between the second key point and the third key point. After adding the safety corridor constraint optimization, the collision is avoided. Fig.12 shown. Fig.13 , Fig.14 They are the changes of pitch angle and roll angle during the path planning process. It can be seen from the figure that the maximum values ​​of pitch angle and roll angle do not exceed the given constraints and are both within the constraints.

[0252] Experimental results show that by optimizing the initial path while satisfying basic constraints and adding safety corridor constraints, a safe and smooth optimal path can be obtained.

[0253] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

[0254] The above are only some embodiments of the present invention. For those skilled in the art, several modifications and improvements can be made without departing from the creative concept of the present invention, which all belong to the protection scope of the present invention.

Claims

1. A close formation path planning method for aerial refueling, characterized in that: The method comprises the following steps: Step 1, based on the wind tunnel test data of the rendezvous and formation phases of aerial refueling, according to the changes of the lift coefficient, drag coefficient, roll, pitch and yaw moment coefficients of the refueling aircraft at different positions in the vortex, obtain the key points of the path of the refueling aircraft in the rendezvous and formation phases; A three-dimensional obstacle model for path planning is built based on the selected obstacle body to obtain an aerial refueling path planning map; Step 2: Determine the constraints of the two phases of aerial refueling. According to the key points of the receiving aircraft's path in the rendezvous phase and the formation phase, use the key point heuristic A* algorithm to perform initial path planning in the aerial refueling path planning map to obtain the nominal planning path. Step 3, establish a fixed-wing UAV model, analyze the differential flatness characteristics of the fixed-wing UAV, and use the position of the UAV and its derivatives to describe the state and control quantities of the UAV system; Step 4: Based on the differential flatness characteristics of the fixed-wing UAV, the cost function of the path optimization process is established. Based on the constraints of the two stages of aerial refueling, the flat output minimum mutation algorithm is used to optimize the nominal planning path. Optimize and obtain the final aerial refueling path.

2. The method according to claim 1, characterized in that In step 1, the key points of the receiving aircraft path in the rendezvous phase and the formation phase are determined based on the minimum value of the cost function W; Among them, the expression of the cost function W is: Among them, Δcmx represents the rolling moment coefficient, Δcmy represents the pitching moment coefficient, Δcmz represents the yaw moment coefficient, K represents the lift-to-drag ratio, and λ1~λ4 represent the weights of the rolling moment coefficient, the pitching moment coefficient, the yaw moment coefficient or the lift-to-drag ratio respectively.

3. The method according to claim 1, characterized in that In step 2, the constraints include: path length constraint, yaw angle constraint, pitch angle constraint and / or relative distance constraint between two aircrafts.

4. The method according to claim 1 or 3, characterized in that Step 2 includes: Divide the aerial refueling path planning map into multiple three-dimensional elements according to a certain step length, and mark obstacle blocks and non-obstacle blocks. Each three-dimensional element represents a path node. Then use the KHA* algorithm to trigger from the set starting point, gradually pass through all the key points, and each node found between two key points must satisfy the minimum movement cost function. Then find each path node on the path in turn, and then connect it with the key points to form a nominal planning path. The movement cost function includes the movement cost from the starting point to the path node and the estimated movement cost from the path node to the end point.

5. The method according to claim 1, characterized in that Step 3 includes: Define the coordinate system G i ,G b and G s They represent the inertial reference frame, the body reference frame and the stable reference frame respectively; Among them, the inertial reference system G i The origin of x is located at a point on the ground. i The axis lies in the ground plane and points in a particular direction, y i Axis and x i The axes are in the same plane and perpendicular to x i Axis, z i The axis is vertically downward, pointing toward the center of the Earth; Body reference frame G b The origin is located at the center of mass of the drone, x b The y axis is in the symmetry plane of the drone, parallel to the fuselage axis of the drone or the average aerodynamic chord of the wing, and points to the direction of the drone's nose. b The axis is perpendicular to the symmetry plane of the drone and points to the right side of the fuselage. b With x b The axis is vertical and points downwards from the drone; Stable reference frame G s The origin is located at the center of mass of the drone, x s The axis coincides with the projection of the drone's airspeed in the symmetry plane, y s Axis and body coordinate axis y b Overlap, z s In the symmetry plane of the aircraft s Vertical and pointing down the fuselage; Define the regular basis vectors in the three-dimensional coordinate system as e1, e2 and e3, and establish the fixed-wing UAV model as: The fixed-wing UAV model is established as: in, Represents the coordinate system G b The position O in the coordinate system G i In the expression The first derivative of is the coordinate system G b The velocity in the coordinate system G i In the expression, for The first derivative of , g and m are the gravity acceleration and the mass of the receiving aircraft respectively, Denotes the stable coordinate system G s To the inertial coordinate system G i The stability-inertia selection matrix is ​​α, where α is the angle of attack and D is the velocity of the drone along the negative x axis. b The drag force of the UAV along the x axis, T is b The thrust generated by the axis, L is the thrust generated by the UAV along the negative z axis. b The lift force generated by the shaft, In the coordinate system G i Measurement coordinate system G b The angular velocity in the coordinate system G b In the statement, Represents the coordinate system G b To coordinate system G i The body-inertia selection matrix, for The first derivative of c is the specified angular velocity.

6. The method according to claim 5, characterized in that Set the drag in the fixed-wing drone model to: Where ρ is the air density, V a is the airspeed of the drone, S is the plane area of ​​the wing, c is the chord length of the wing, and is the aerodynamic coefficient.

7. The method according to claim 5 or 6, characterized in that In step 3, the state x and control input u of the drone system are defined as: The state x and control input u are described by the flat output z and its higher-order derivatives as:

8. The method according to claim 1, characterized in that In step 4, the cost function of the path optimization process is established based on the differential flatness characteristics of the fixed-wing UAV, including: According to the differential flatness characteristics of fixed-wing UAVs, based on the current coordinate position x of the UAV in space T (t),y T (t),z T (t) The nominal planning path corresponding to time t is described as: Among them, the superscript T represents transposition; The path is divided into several segments based on the key points of the receiving aircraft path, and t is defined. i (i=1,2,…,j) represents the key point number, t i represents the time corresponding to the i-th key point, j represents the total number of key points, t0 represents the starting time, Described as: Minimize the fourth-order derivative of the drone's position to obtain a smooth path for each segment: Among them, the drone position r = [x T (t),y T (t),z T (t)] T .

9. The method according to claim 8, characterized in that Step 4 also includes time allocation optimization: Let the time allocation vector τ = [T1, T2, ..., T j ], where T i (i=1,2,…,j) represents the time length allocated to the i-th segment; An initial value is set for the time allocation vector τ, and then the gradient descent method is used to iteratively optimize τ until the preset iterative convergence condition is reached, and the final time allocation vector τ is obtained.

10. The method according to claim 8, characterized in that In step 4, the flat output minimum mutation algorithm is used to calculate the nominal planning path When optimizing, set the safety corridor constraint as: |d i (t)|≤ε i ,t i ≤t≤t i+1 Among them, d i (t) indicates the distance from the point on the path to the corresponding segmented straight line segment at time t, d i (t)=(r T (t)-r i )-((r T (t)-r i )·s i )s i , r T (t) is the path at time t, r i is the i-th key point, s i For along r i Point to r i+1 The unit vector, ε i Indicates the preset safety corridor width.

Images