Path Following and Obstacle Avoidance for Unmanned Aerial Vehicles Using a Virtual-Force-Based Guidance Law

: This paper presents a virtual-force-based guidance law (VFGL) for path following and obstacle avoidance in unmanned aerial vehicles. First, a virtual spring force and a virtual drag force are designed for straight-line following; then, the dynamic of the cross-track-error is equivalent to a spring mass system, which is easy to tune to acquire stability and non-overshoot convergence. Sec-ondly, an additional virtual centripetal force is designed to counteract the inﬂuence of the curvature of the planned path so that the guidance law can accurately track a curve with a time-varying curvature. Thirdly, an extra virtual repulsive force is designed directly according to the sensor inputs; the virtual repulsive force pushes the vehicle away to move around obstacles. The use of artiﬁcial physics means the guidance law is founded on solid physical theory and is computationally simple. The physical meanings of the parameters are deﬁnite, and the VFGL has a large parameter adaptation. These make the guidance law easy to tune in application. Both the numerical and hardware-in-the-loop simulation results demonstrated the effectiveness of the proposed guidance law for path following and obstacle avoidance in unmanned aerial vehicles.


Introduction
In the application of unmanned aerial vehicles (UAVs), one of the basic requirements is for the UAVs to autonomously follow a planned path. In most cases, the reference path given by planning algorithms [1] is a curve. This requires that the UAVs can accurately track the reference curve. In addition, obstacle avoidance is another problem that has to be considered during path following. Although preknown static obstacles can be considered in path planning [2,3], unknown obstacles may be detected when the UAVs follow the planned path in a complex dynamic environment [4,5]. It is required that the UAVs are able to avoid the newly discovered obstacles and come back to the planned path. In this paper, we focus on the problems of path following and obstacle avoidance in UAVs.
Path following and obstacle avoidance have been hot topics in the field of UAVs for a few years. The following is a brief overview of the existing methods in the literature.
A. Path following Various strategies have been proposed in the literature for path following. Two main categories are control-theory-based approaches and geometry-based approaches. Several types of control-theoretic techniques have been developed for path following. Some of the well-known techniques are proportional integral differential (PID) control [6], model predictive control (MPC) [7][8][9], adaptive control [10][11][12], sliding mode control [13,14], and linear quadratic regulator (LQR) [15,16]. Other novel techniques, such as iterative learning control [17], were developed for path following. The main advantage of these control-theory-based approaches is the guarantee of the stability and the convenience for performance analysis in theory. One drawback is the longer man-hours associated with the controller's implementation when compared with the geometric methods.
Many control-theory-based methods rely on mathematical models of the UAVs, and it is time consuming to obtain an accurate model of a UAV, especially for a low-cost small fixed-wing UAV. If the model is not sufficiently accurate, more time may be required to adjust the parameters of the method in real applications. In addition, in terms of theoretical complexity, it takes more time for many engineers to understand and apply complex control theories than it does for geometric-based approaches.
Many geometric-based guidance laws were proposed for UAV path following, they are the line-of-sight (LOS) [18] method, pure pursuit (PP) [19] method, nonlinear guidance law (NLGL) [20], vector field (VF)-based guidance laws [21,22], and the combination of LOS and pure pursuit [23]. In PP, LOS, and NLGL, a virtual target point is defined on the reference path. The guidance laws direct the UAV to track the virtual target point, so that the UAV eventually converges to the reference path. All of the four kinds of guidance laws are applicable when the reference path is a straight-line or a circle.
Although the NLGL can be used to follow a curved path, the optimal parameters are different when the radius of the reference circle changes [24], so the cross-track error is still considerable when the reference curvature varies [25]. Since all these techniques use the geometric approach, computing the desired heading angle (or rate) is quick, and they are easy to implement. The performance of these techniques was evaluated and compared in [24] using straight-line following and loitering. A reference path with varying curvature was not considered. Accurately tracking a curved trajectory with a time-varying curvature remains a challenge.
B. Obstacle avoidance Another hot topic in this research area is obstacle avoidance during path following. One of the most popular techniques that combines path following and obstacle avoidance is the MPC-based approach [26], for convenience to tackle the constraints of the obstacles. A limitation of the MPC-based approaches is the problem of computation complexity. The authors in [27] addressed these two problems together using a mixed H 2 /H ∞ approach; however, a smooth trajectory was approximated by a set of piece-wise lines.
The potential field (PF) [28][29][30][31] and virtual force field (VFF) [22,32] methods were designed for obstacle avoidance guidance using heading direction vector fields. The PFbased method uses potential flow elements that result in a velocity vector; the velocity vector can be used to guide a vehicle to the desired goal while avoiding obstacles. VFFbased methods use artificial attractive forces to pull a UAV toward its goal, and use artificial repulsive forces to push the UAV away from obstacles. Neither PF and VFF can direct a UAV to converge back to the planned path after avoiding an obstacle [29,30].
In [33], a gradient vector field (GVF) method was proposed for the path following and circular obstacle avoidance of UAVs. The center and size of the newly detected obstacle is required. The user has to evaluate this information from the sensor inputs. A neural network based method is presented in [34]; the control commands were calculated directly from the sensor inputs without the knowledge of the obstacle center and size. The application of the neural-network-based method is limited by the requirement of training samples.
Another idea for obstacle avoidance during path following is replanning when new obstacles are discovered [2,[35][36][37]. Compared with the PF and VFF methods, replanning requires more calculation time. It is suitable for ground vehicles or quadrotors that can stop or hover during replanning. It is difficult to meet the real-time requirement of a fixed-wing UAV. In addition, the performance of the two-layer "replanning-following" structure still depends on the accuracy of the path following algorithms.
Otherwise, there may be a large deviation between the actual and the planned path, which means that the generated path cannot be accurately tracked. The idea of replanning is often used in the exploration of an unknown environment. For example, a dynamic path planner was designed in [37] for aerial robots (typically multicopters) to explore underground mines.
In the dynamic path planner, a sector-based obstacle avoidance algorithm was designed to avoid collisions and keep the robot more centered inside the tunnel. A similar obstacle avoidance algorithm based on open sectors [38] was proposed for aerial robot navigation through unstructured urban/suburban environments. These methods are successful in obstacle avoidance in unknown environments; however, they do not take the requirement of accurate path following into account.
For the problem of path following and obstacle avoidance, in addition to cross-track error and the man-hours associated with the controller's implementation, other considerations, such as stability, computational costs, and execution times are some of the determining factors that a designer has to take into account at the moment of choosing or implementing a controller [39].
Considering these factors in this paper, we propose a novel virtual-force-based guidance law (VFGL) for path following and obstacle avoidance in UAVs inspired by a spring mass system. First, we designed a virtual spring force and a virtual drag force for straightline following; the dynamic of the closed-loop of the cross-track error is equivalent to a spring mass system, which is easy to tune and to acquire stability and non-overshoot convergence. Then, an additional virtual centripetal force was designed to counteract the influence of the curvature of the planned path so that a general curve with time-varying curvature could be accurately tracked.
To avoid the obstacles detected in the planned path, an extra virtual repulsive force was designed to push the vehicle away from the planned path to move around the obstacles. The virtual repulsive force directly relies on the sensor inputs; there is no need to estimate the central position and size of the newly detected obstacle. The use of artificial physics (virtual forces) makes the guidance law founded on solid physical theory and computationally simple. The physical meanings of the parameters are definite, and the VFGL has a large parameter adaptation. These factors make the guidance law easy to tune in application.
To demonstrate the performance of the proposed guidance law, different parameters were evaluated in the simulation. The results show that the VFGL is easy to tune in application, and has a large parameter adaptation. Several methods were implemented in the simulation as comparison. The obtained simulation results demonstrated the superiority of the proposed guidance law. The VFGL performed over the NLGL, PLOS, and PID in curve following, and performed over GVF and VFF in obstacle avoidance during straight-line following.
To further evaluate the VFGL, a highly realistic hardware-in-the-loop (HIL) simulation system was developed by combining an autopilot with a heavily developed and commercially available flight simulator, X-Plane. Then, the VFGL was applied to the problem of path following and obstacle avoidance for a fixed-wing UAV. The results demonstrated the effectiveness of the VFGL. This paper is organized as follows. In Section 2, we formulate the problem. In Section 3, the idea and the procedure of the VFGL is presented for path following and obstacle avoidance. In Section 4, both numerical simulation and the HIL experiment are described to demonstrate the effectiveness of the proposed guidance law. Finally, we conclude the paper in Section 5.

Vehicle Model
We consider a fixed-wing UAV equipped with a low-level controller. Accordingly, the dynamics of the UAV is approximate to a unicycle model, which is given bẏ where (x, y) is the position; ψ is the course; v is the ground velocity; and ω is the course rate. The model is widely used in the guidance of UAVs equipped with a low-level controller [22,24,33,40,41].

Problem Definition
In this paper, we focus on the problem of path following and obstacle avoidance for a UAV. In path following, we consider the reference path as a continuous curve generated by path planning algorithms. The position and curvature of all the points on the reference path are known. Our first goal is to design a guidance law that directs a UAV to follow the planned path.
In obstacle avoidance, we assume that the UAV is equipped with LIDAR, which can detect obstacles on the reference path. The points on the surface of an obstacle can be detected within a certain range. Our second goal is to guide the UAV to avoid obstacles according to the onboard LIDAR inputs during path following; once the UAV moves around the obstacle, it returns to the original planned path.

The Virtual-Force-Based Guidance Law
In this section, we first describe a spring mass system, which is the source of our idea. Then, virtual forces are designed step by step for the path following and obstacle avoidance in UAVs. Finally, the guidance law is derived from the virtual forces.

Spring Mass System: The Source of the Idea
Consider a spring mass system as shown in Figure 1: before the unit mass (m = 1) reaches its equilibrium position, the deviation (d) is controlled by the resultant of the spring force (F t ) and the frictional resistance (F r ), where and k s and c d denote the spring constant and coefficient of resistance.
From (2), we obtain the dynamic of d Define X = [d,ḋ] , we obtain the state space description of (3) It is easy to find the only equilibrium state X = 0. Let |λI − A| = 0, we obtain the eigenvalues of (4) When k s > 0, c d > 0, real(λ) < 0,the spring mass system (3) is asymptotically stable, which means the deviation globally converges to 0. If c d ≥ 2 √ k s , d converges to 0 without overshoot or oscillation. From (5), we know that: • When 0 < c < 2 √ k, the spring mass system is a second-order weakly damped control system. Then, there will be oscillation before convergence. • When c = 2 √ k, the system is a second-order critical damped control system. In this case, d converges to 0 without overshoot. • When c > 2 √ k, the system is a second-order over damped system. d converges to 0 without overshoot, and the convergence rate is slower with a larger c.

The Virtual Forces for Straight-Line Following
Inspired by the spring mass system, a virtual spring force (F vt ) and a virtual drag force (F vd ) are designed to govern a UAV to track a reference line. F vt points to the reference point, which is the nearest point on the reference straight line to the UAV. The direction of F vd is opposite to the relative velocity v VP , which is the velocity of the UAV relative to the moving reference point as shown in Figure 2. The magnitude of the virtual spring force and the virtual drag force are where k v is the constant of the virtual spring, c v is the coefficient of the virtual drag, and the cross-track error d is the distance between the UAV and P r . In decomposing F vt and F vd along VP r and the normal direction of VP r , the motion of the UAV can be decomposed in the two directions.
In the VP r direction, there are two forces. One is the virtual spring force F vt , and the other is the component of the virtual drag force in the VP r direction, Then, the dynamics of the cross-track error d arë This is same as the dynamics of the mass spring system (3). The conclusions of the mass spring system are also valid here: if c v ≥ 2 √ k v , the cross-track error d converges to 0 without overshoot or oscillation.
In the normal direction of VP r , the initial velocity of the UAV relative to the moving reference point is v n (0) = v n0 , the resultant force is the component of the virtual drag force in this direction, F vdn = c v v n . The motion of the UAV in this direction is given bẏ The analytical solution of (9) is For the drag coefficient c v > 0, the relative velocity v n exponentially converges to 0. Using the two virtual forces, both d and v n converge to 0, which means that the UAV converges to the reference point and follows the reference line with the reference velocity v r .

Virtual Centripetal Force for Curve Following
For curve following, the reference point is not in an equilibrium state. The virtual spring force and the virtual drag force are insufficient to guide a UAV to accurately track a curve. Then, a virtual centripetal force (F vc ) is designed to enable a UAV to track the maneuver of the reference point. F vc points to the center (O) of the curvature circle at the reference point as shown in Figure 3. The magnitude of the force F vc is where l is the distance from the UAV to the center O; and v T is the velocity component of the UAV along the tangent direction at the point V.
The virtual centripetal force pulls the UAV to circle the point O at a radius of the current distance l. It counteracts the influence of P r 's maneuver on d, so that the virtual spring force and the virtual drag force guide the UAV to track a curve like a straight-line. Then, the dynamics of d are same as those of the straight-line case. After the UAV converges to the reference curve, l = r, the virtual centripetal force keeps the UAV moving along the reference curve.

Virtual Repulsive Force for Obstacle Avoidance
This method is based on the concept of virtual forces. A virtual repulsive force can be designed to avoid the obstacle detected in the reference path. Supposing that the UAV is equipped with LIDAR. The LIDAR can detect a series of points (P o ) on the obstacle's surface within a certain range as shown in Figure 4.  The leftmost point p 1 = (d o1 , θ o1 ) and the rightmost point p 2 = (d o2 , θ o2 ) can be found in P o . Then, the virtual repulsive force F vr is designed perpendicular to v, the positive direction of F vr is to the left of v. F vr is given by where k rep is the coefficient of the virtual repulsive force. d s is a parameter related to the safe clearance between the UAV and the obstacle. The virtual repulsive force is designed to repel the vehicle to its lateral; this means that the virtual repulsive force only affects the course of the UAV and does not change the speed of the UAV; when |θ o1 | > |θ o2 |, F vr repels the UAV to turn to its right; when |θ o1 | ≤ |θ o2 |, F vr repels the UAV to turn to its left. The UAV is always repelled to turn to the side with a smaller angle of view to move around of the obstacle.

Driven of the Virtual-Force-Based Guidance Law
According to the analysis above, the UAV is governed by four virtual forces: a virtual centripetal force, a virtual spring force, a virtual drag force, and a virtual repulsive force. The guidance law is calculated by the combination of four virtual forces. The procedure is given below:

•
Step 1: Obtain the state (x, y, ψ) of the UAV. • Step 2: Determine the reference point P r = (x r , y r , ψ r , ω r ) and calculate the reference center O. The reference center O = (x o , y o ) is determined by the reference point P r and the reference radius r = |v r /ω r | at point P r .
x o = x r + r · cos(ψ r + sign + (ω r ) · π 2 ) y o = y r + r · sin(ψ r + sign + (ω r ) · π 2 ) where • Step 3: Calculate the four virtual forces according to (6), (7), (11), and (12). • Step 4: Obtain the resultant virtual force in the forward and lateral direction by decomposing the virtual forces in the two directions where the subscripts f and l are used to denote the components of forces in the UAV's forward and lateral direction. • Step 5: Obtain the input command of the VFGL by where dt is the control interval in practical application. v min and v max are the minimum and the maximum velocity of the UAV. ω max is the maximum course rate. The

Numerical Simulation
The UAV model given in (1) is used in the numerical simulation. The saturation constraints of the course rate and the velocity are given in each simulation scenario. A more realistic fixed-wing UAV model will be considered in the hardware-in-the-loop simulation.

Path Following with Different Parameters
In this scenario, the velocity of the UAV is limited to 20 m/s. The heading rate is limited within [−0.2, 0.2] rad/s. Different k v and c v are considered in the simulation to evaluate the VFGL method. The results are given in Figure 5. Figure 5a shows the simulation results with constant k v = 0.5 and changing c v . When c v = √ k v , there are oscillations before the trajectory of the UAV converges to the reference. When c v = 2 √ k v and c v = 3 √ k v , the trajectory of the UAV converges to the reference without overshoot or oscillation. This is consistent with the theoretical results that, if the parameters satisfy c v ≥ 2 √ k v , d converges to 0 without any overshoot or oscillation. We also find that the convergence is slower with the increase of c v . Figure 5b is the simulation results with different k v and c v . It shows that the convergence obtains faster with the increase of k v (c v = 2 √ k v ). When k v increases to 2, there is an overshoot of more than 10 m before the UAV converges to the reference. That is because the control input (course rate and the velocity) of UAV is constrained. The overshoot can be reduced by adding c v . The result shows that when c v increases to 3 √ k v , the overshoot is suppressed to 0.

Comparison of the VFGL with Other Methods
To illustrate the superiority of the VFGL, several methods were implemented in the simulation as comparison. They are the NLGL, PLOS, PID, GVF, and VFF. Guidance laws for path following, such as the NLGL, PLOS, and PID, did not consider obstacle avoidance. For fair comparison, there are no obstacles in the comparison with these methods. In comparison with the GVF and VFF, a circular obstacle is set on the reference path to evaluate the performance of the methods for obstacle avoidance.
A. Curve following using different methods In this scenario, the velocity of the UAV is limited to 20 m/s. The heading rate is limited within [−0.2, 0.2] rad/s. The parameters of the VFGL are k v = 0.5, c v = 2 √ k v ; the guidance distance of the NLGL is 120; the parameters for the PLOS are k e = 2.5 and k d = 0.05; and the parameters for the PID are k P = 0.1, k I = 0.005, and k D = 0.64. The reference path includes a straight-line segment and a part of "8" shaped curve with a variable curvature. Figure 6 shows the simulation results using the four kinds of methods.  Figure 6a shows that all of the four methods can guide the UAV to track the reference curve. The differences lie in the speed of convergence and the tracking accuracyparticularly when the curvature of the reference path changes. The cross-track error using these methods is shown in Figure 6b. This shows that the convergence of the VFGL and PLOS is faster than the NLGL and PID. Using NLGL, PLOS, and PID, there are overshoots before the UAV converges to the reference. The cross-track error of VFGL is the lowest among these methods, especially when the curvature of the reference path changes.
The comparison in this scenario shows that the proposed VFGL method has better performance on path following than NLGL, PLOS, and PID, especially when the curvature of the reference curve changes. B. Obstacle avoidance using different methods In this scenario, a UAV is initially following a straight line at a constant speed of 25 m/s. A circular obstacle is set on the reference path. The VFGL, VFF, and GVF guidance were simulated in this scenario. The parameters for the VFGL are k v = 0.5, c v = 2 √ k v , k rep = 10, R d = 100 m, and d s = 10 m. The parameters of the simulation in [33] were used for VFF and GVF in this scenario. Details on the VFF and GVF guidance can be found in [29,33].
The results of the three guidance methods are shown in Figure 7. Using the VFGL, the UAV is pushed away from the planned path to avoid the obstacle, and after that it converges back to the planned path without any overshoot. Using the VFF method, the UAV moves away from the planned path to avoid the obstacle; after the obstacle avoidance, it can not return to the planned path but takes the shortest distance directly to its goal. GVF leaves the planned path before VFF and VFGL, tracks the outside of the obstacle, and then quickly converges back to the planned path.
The deviation from the planned path of the GVF is larger than the VFGL. There is overshoot before the UAV converges back to the planned path using the GVF. By comparison, we can find that the VFGL method achieved better path following and obstacle avoidance results. Compared with GVF, another advantage of the proposed VFGL is that it can be used for curve following and obstacle avoidance (see the results of the hardware-in-the-loop simulation).

Hardware-in-the-Loop Simulation
A hardware-in-the-loop (HIL) simulation is a bridge between numerical simulation and a field experiment. A HIL simulation is more realistic compared with numerical simulation for the participation of hardware and a more realistic flight simulator. Compared with field experiment, HIL simulation is more efficient, safe, and cost effective. HIL simulation was conducted to validate the proposed method.

System Setup
The HIL testbed was developed by combining an autopilot with a heavily developed and commercially available flight simulator, X-Plane. The diagram of the HIL testbed is shown in Figure 8. The flight simulator X-Plane was used to simulate the UAV platform, the sensors, and the external environment in a real system. The switch was used to substitute the real digital radio. Both the autopilot and the ground control station in the HIL testbed were the same as with the actual system. The developed HIL testbed is shown in Figure 9. Using the HIL testbed, new approaches and algorithms can be fully tested and evaluated. The HIL testbed has been used in previous work [41][42][43][44][45]. The approaches that passed the X-Plane-based HIL evaluation can be easily applied in real flight experiments. In this paper, we use the fixed-wing aircraft model PT60 in the X-Plane simulator to validate the effectiveness of the proposed guidance law for the problem of path following and obstacle avoidance. The plane and its main parameters are shown in Figure 10.  The altitude of the reference path was assumed to be constant. The ADRC-based low-level controller we designed in [46] was used to obtain the desired attitude and velocity. The effectiveness of the low-level controller was validated in simulation and field experiments [41][42][43][44][45][46] Table 1 were evaluated.  Figure 11 shows the cross-track errors in the HIL simulation. As shown with the red dashed line, there is an overshoot of 10 m when k

Label Parameters
It shows that the UAV converges to the reference line without overshoot. The green dotted line shows that the UAV converges to the reference line without overshoot when k v = 1, c v = 2 √ k v , and the convergence is faster than The result coincides with the numerical simulation ( Figure 5). This shows that the proposed VFGL is easy to tune and has a large parameter adaption.
The trajectory of the UAV in this scenario is shown in Figure 12. The reference heading rate switches between 0.1 rad/s and −0.1 rad/s at the tangent point of the reference "8" path. The result shows that the UAV followed the reference path well; however, the reference curvature varies. The cross-track error in the HIL simulation of curve following is shown in Figure 13. The maximum of the cross-track error after t = 20 s is 3.97 m, which emerges shortly after the first switch. The cross-track error at the second switch is within 1.95 m. These errors were mainly produced by the model error between a real fixed-wing UAV PT60 and the simplified UAV model expressed by (1).
The result is shown in Figure 14. First, the UAV converges to the reference curve and begins to fly along the reference path. Once the obstacle is detected, the UAV is repelled by the virtual repulsive force to move around the obstacle. After that, the UAV is pulled back to the reference path by the virtual spring force and the virtual centripetal force. During the process, there is no overshoot or oscillation; the UAV avoids the obstacle smoothly. Using the VFGL, a fixed-wing UAV can avoid obstacles on the reference curve during path following.
The HIL simulation results demonstrate that the VFGL was effective in path following and obstacle avoidance with a fixed-wing UAV. The UAV could accurately follow a predefined line or curve. When an obstacle was detected in the reference path, the UAV could smoothly avoid the obstacle; after that, it converged back to the reference.

Conclusions
In this paper, a virtual-force-based guidance law was proposed for path following and obstacle avoidance in UAVs. The guidance law was inspired by a spring mass system. In order to follow a planned path and avoid the newly detected obstacles in the planned path, four virtual forces were designed to guide the UAV. The virtual repulsive force pushed the UAV away from the planned path to move around the obstacles. The virtual centripetal force counteracted the influence of the curvature of the planned curve. Then, the virtual spring force and the virtual drag force guided the UAV to accurately track a curve in the same way as tracking a straight-line.
The use of artificial physics means the guidance law is founded in solid physical theory and is computationally simple. The proposed guidance law was evaluated by both numerical and HIL simulations. The numerical simulation results show that the VFGL has a large parameter adaptation. It performed over the NLGL, PLOS, and PID in path following and performed over GVF and VFF in obstacle avoidance during path following.
In the HIL simulation, the VFGL was applied to the problem of path following and obstacle avoidance for a fixed-wing UAV. The results show that the VFGL was easy to tune in application. The UAV accurately followed the predefined "8" shape path and successfully avoid the obstacle in the path. Although the VFGL was evaluated using a fixed-wing UAV in the HIL simulation, we considered that this is also applicable for other unmanned vehicles that can be modeled as unicycles.
Author Contributions: Methodology: X.W. and L.C.; software: L.K. and B.W.; resources: L.C.; validation: X.W., S.H. and C.L.; writing-original draft preparation: X.W.; writing-review and editing: X.W. All authors have read and agreed to the published version of the manuscript.