Bioinspired Tracking Control of High Speed Nonholonomic Ground Vehicles

The behavior of nature’s predators is considered for designing a high speed tracking controller for nonholonomic vehicles, whose dynamics are represented using a unicycle model. To ensure that the vehicle behaves intuitively and mimics the biologically inspired predator-prey interaction, saturation constraints based on Ackermann steering kinematics are added. A new strategy for mapping commands back into a viable envelope is introduced, and the restrictions are accounted for using Lyapunov stability criteria. Following verification of the saturation constraints, the proposed algorithm was implemented on a testing platform. Stable trajectories of up to 9m/s were achieved. The results presented show that the algorithm demonstrates significant promise in high speed trajectory tracking with obstacle avoidance.


Introduction
With the development of driverless technologies, there is an increasing demand for high speed systems capable of dealing with unstructured environments.Many commonly used obstacle avoidance methods focus on generating a path and forcing the vehicle directly to the desired trajectory [1][2][3][4][5][6][7].Unfortunately, in many cases, such as potential field methods, the desired route is rarely smooth and does not sufficiently account for the desired velocity or vehicle capability [3,[8][9][10][11][12][13].
To find a more intuitive behavior, we look to biology for inspiration, specifically a generic predator-prey interaction.A cheetah chasing its prey, for example, does not directly mimic the path of its target.Instead, it creates a relatively smooth path, although its target may be moving somewhat chaotically [14].The smooth path, in turn, allows the cheetah to maintain much of its high speed and still meet the desired goal [14].As will be shown, the proposed algorithm mimics this behavior by introducing a reference system for the vehicle to follow, analogous to how a predator chases its prey.
Due to the high speed nature of our goal, this study focuses on Ackermann steering based platforms.Because of its simplicity and ability to capture nonholonomic constraints, we begin by considering a unicycle model, which will be shown to fit within the confines of an Ackermann platform by adding saturation constraints.While there exists extensive research in the control of unicycle type robotic systems [15][16][17][18][19][20][21], the study begins with the framework laid out in [21,22] as it introduces a relationship similar to the predator-prey interaction.In particular, when considering the path of the cheetah, its trajectory can be thought of as a filter to the prey's path; this is possible due to the cheetah maintaining a certain following distance from its prey.The larger the distance is, the more filtered the path becomes.In [22], the authors provide a control algorithm that mimics this behavior.By controlling velocity and angular rate, an algorithm is designed in [22] which theoretically tracks a virtual target at a time-varying distance.However, while capturing certain constraints, the unicycle model, and by consequence the work of [22], are not comprehensive.Much as the cheetah is unable to immediately turn independently of speed, neither are many ground vehicles.This paper focuses on extending preceding work to better satisfy the desired bioinspired model.This work is organized as follows.In Sections 2 and 3 we introduce the algorithm laid out in [22] along with additional restrictions placed on the system structure and following distance parameters, which are aimed at bolstering controller reliability.Moreover, we evaluate several limitations of the base algorithm in the context of the biologically inspired predator-prey relationship and high speed ground vehicles.In Section 4, we introduce constraints on angular rate and velocity commands, which are based on Ackermann steering platform kinematics.In this section, we develop a new strategy for mapping commands into a viable control space that reflects these kinematic restraints.We additionally provide constraints on the following distance parameters to ensure stability.The algorithm is shown to satisfy the Lyapunov stability criteria, while still meeting all of these requirements.
The proposed algorithm was ultimately implemented on a testing platform, the results of which are presented in Section 5. To satisfy high-level commands, low-level PI controllers were used to control commanded velocity and angular rate.The reference system was manipulated using a basic potential field methodology of strictly attractors and repulsors similar to that presented in [23][24][25].Using an Extended Kalman Filter to provide reliable state information, the full algorithm was used to achieve stable trajectories up to 9 m/s, the top speed of the platform.Further work will focus on accounting for actuator dynamics and extending the method to multiple platforms.

System Definition
We begin with a unicycle model as it adequately captures the nonholonomic constraints of our Ackermann steering platform.Moreover, the simplicity of the model directly lends itself to controller design.While typically used for differential drive vehicles, we note that this relationship is not exclusive, and by developing saturation constraints, we can force the unicycle model to operate in the confines of an Ackermann steering platform.The unicycle model used is as follows, where () ≜ [() ()] T ∈ R 2 is the vehicle's position, V() ∈ R is the longitudinal velocity, () ∈ R is the heading, and () ∈ R is the vehicle's angular velocity.A reference system is then introduced to represent a virtual target, where  r () ∈ R 2 is the reference system's position.In the interest of continuous controller commands, we must guarantee that the reference system is composed of class  1 functions in time.For convenience, the linear velocity of the reference system is defined as where V r () ∈ R.
In the following section, we will see that the controller design will guarantee the system described by ( 1) and ( 2) will converge to a sufficiently small neighborhood around the reference system, described by (3), such that ‖() −  r ()‖ →  * (), as  → ∞, where  * () > 0 is a user defined nominal following distance.By guaranteeing that the vehicle follows the reference system in this manner, we enforce our bioinspired predator-prey interaction mentioned previously.

Control Design
The overall controller is presented in two distinct steps.The first step involves the basic design, while the second step includes saturation algorithms based on Ackermann steering kinematics.In this first step, we assume that the reference system can be controlled to follow a desired trajectory.By ensuring that the actual system converges to a small neighborhood of the reference system, we ensure that the system follows the desired trajectory as well.At the same time, a sufficiently large separation between reference system and vehicle allows a buffer for the vehicle to track the reference, without violating its nonholonomic constraints.
In order to properly design the following distance, it is necessary to define the error between the vehicle and reference system.We start by assuming that the origins of the body fixed coordinate frame, B, and of the global inertial coordinate frame, U, coincide with the center of mass of the vehicle in the horizontal plane.An orthonormal transformation from B to U is then defined, Using this transformation along with (1) and ( 3), the position error can be expressed in the body coordinate frame as where () ∈ R 2 .This position error can be thought of as a vector expression of longitudinal and lateral error in the body frame.Next we introduce the commanded distance vector, where () > 0 is the commanded following distance.By guaranteeing that () →  as  → ∞, the longitudinal error will converge to () while the lateral error goes to zero.
To provide this behavior, we introduce the following control law ( [22]), where and  V > 0 and   > 0 are scalar tuning constants used to provide bounded signals to the commanded longitudinal and angular velocities, respectively.This control law, which is more thoroughly derived in [22], is based on Lyapunov stability theory.In [22], the authors show it to be the natural result that guarantees a negative definite Lyapunov time derivative, given the Lyapunov function (12) introduced in the following theorem.
In order to ensure that Δ −1 () is defined for all  ≥ 0, we must guarantee that () ̸ = 0 for all  ≥ 0. In practice, if () < 0, the vehicle will follow the reference system but in reverse, such that V() < 0. As such, we restrict the commanded following distance to () > 0. For design purposes, a more rigorous constraint of () > ( − ) > 0, for all  ≥ 0, is enforced, where  >  > 0 are tuning parameters used to place a minimum bound on ().The commanded following distance, (), is designed such that () →  * () as  → ∞, provided that  * () ≥  as well.
At this point, we note the intuitive form of (8).For instance, from the  T (()) ṗ r () term, we see that the reference velocity, projected along the vehicle's longitudinal axis, positively contributes to the commanded velocity, V().Likewise the reference velocity, projected along the lateral axis, contributes to the commanded angular rate, ().In examining the δ () term, we see that an increasing distance negatively impacts the commanded velocity, as would be expected.Lastly, with regard to the  tanh(() − ()) term, we see that the difference between longitudinal error and the commanded distance, (), contributes positively to the commanded velocity.Along the same lines, lateral error contributes positively to the angular rate command.
With regard to the tanh(⋅) term of (8), which is a componentwise operation, we note that while it is unnecessary for theoretical stability, it adds a layer of tuning to the controller.This layer proves particularly useful in experimentation.The function is chosen for its natural saturation capability.Based on the constants  V and   , the function directly contributes bounded signals to commanded longitudinal and angular velocities, which are based on the difference between the tracking error () and distance vector ().
Theorem 1.Consider the system described by ( 1) and ( 2), the reference system described by (3), and the feedback controller described by (8).If the nominal distance is restricted such that  * () ≥ , for all  ≥ 0, and the distance () is updated according to where with tuning constant  > 0, then the distance (), between the vehicle and reference system, converges to the desired distance,  * (), while guaranteeing that () > ( − ) > 0. Meanwhile, the tracking error () converges to the distance vector ().
Proof.Consider the Lyapunov function candidate where In order to examine the derivative of the Lyapunov function along the system trajectories, the time derivative of the tracking error, (), given by (6), is computed as follows, The time derivative of the orthonormal transformation, (()), is given by where By substituting (1) and ( 15) into ( 14), the error dynamics can be simplified to ė () =  T ( ())  T ( ()) ( r () −  ()) By substituting the control law given in (8), the error dynamics are then further simplified to The error dynamics given by ( 18) are then substituted into the time derivative of the Lyapunov function, If we consider the case in which () ≥ , we then substitute (10), (11), and ( 18) into ( 19) to obtain From (20), it is trivial to show that again V() ≤ 0 for all  ≥ 0, () ≥ .

Simulation Results.
The behavior of the control algorithm is examined prior to the introduction of saturation constraints on commanded longitudinal and angular velocities.Before simulation, it is necessary to choose a nominal following distance.By choosing for  > 0 and  > 0, we enforce a behavior that tends toward the bioinspired predator-prey model discussed earlier.For example, with higher reference velocities, we intuitively allow a greater distance to respond to instantaneous changes in reference direction.Likewise, (22) draws the vehicle closer to tightly follow slow trajectories.Analogously, large separation allows for a cheetah to easily maneuver while maintaining high speed, whereas a small separation lends itself to tighter tracking.To evaluate the performance of the controller, we consider the situation in which the reference system is defined as two decoupled first-order systems, where   () and   () are the desired reference trajectories in  and , respectively.Figures 1-3 show the results of simulation using the parameters given in Table 1.
Figure 1 shows the theoretical performance of the system.As intended, the vehicle converges tightly to the reference system's path, while maintaining a varying following distance, shown in Figure 2. Further inspection shows that the minimal distance coincides with the sinusoidal peaks in the trajectory.This relationship is a result of lower reference velocity in the  direction, which leads to close tracking at low speeds around the peaks of the sine wave.
With regard to the controller commands, seen in Figure 3, the trends are as expected.Since the controller is based on a unicycle model, there is no obvious correlation between velocity and angular rate commands.In fact, many of the largest angular rate commands, with regard to magnitude, correspond to the lowest velocity commands.This relationship is exemplified by the third graph which examines the ratio of angular rate to longitudinal velocity.At multiple instances throughout the simulation, this ratio experiences singularities.

Saturation Constraints
Since the control algorithm is geared toward high speed control and obstacle avoidance, we consider an Ackermann steering platform for testing and implementation.However, due to the behavior discussed in the previous section, where the ratio of angular to longitudinal velocity becomes singular, the control algorithm is not suitable for an Ackermann steering platform without modification.
Additionally, we observe in Figure 3 that the controller naturally commands negative velocities.Specifically, at the start of simulation, the vehicle immediately backs up to satisfy the error and distance requirements, which for this case are largely dictated by δ ().Further instances of negative velocity occur throughout the trajectory while undergoing tight cornering.In practice, however, we would like the behavior to better follow that of a predator chasing its prey.In the case of the cheetah, it is unlikely that the creature will stop, back up while turning, and then continue the chase.Instead, it is more intuitive to continue along its current path, preserving speed while correcting heading to the best of its ability.
In order to elicit the same behavior, the following saturation constraints are introduced based on an Ackermann steering platform, where  > 0 is the wheelbase length from rear to front axle,  max > 0 is the maximum steering angle deflection referenced from the longitudinal body axis, V min > 0 is a lower bound on the longitudinal velocity, and V max > 0 is an upper bound on longitudinal velocity.In this case it is assumed that the maximum steering angle is the same in both directions, allowing  max to be used for both minimum and maximum saturation of the angular rate.The envelope is graphically demonstrated via the shaded region seen in Figure 4.In Figure 4, we note the minimum bound on velocity, which excludes the origin.As a result, the algorithm is unable to achieve a stationary command.This choice is a product of our bioinspired cheetah model, which we want to continue on a forward trajectory at a minimum velocity bound, rather than stopping or reversing direction.Additionally, if commands were mapped to the origin during saturation, the vehicles position would remain unchanged, potentially giving it no way to exit the saturation condition.To prevent this stagnation, we again enforce a minimum velocity.

First Stage Saturation.
During operation, the control law given in (8) can viably produce a command anywhere in the V- plane; however, it is desired to map the commands to somewhere within the given envelope while still preserving directional intent.Moreover, any mapping of commands should be continuous, not allowing for instantaneous transitions from one region of the envelope to another.In order to address these constraints, we present a dual stage saturation algorithm.
Based on our choice of regions in the second stage of saturation, which will be discussed in the next section, it is possible for commands to instantaneously change locations on the envelope.Specifically, if the controller commands cross the negative V axis or pass through the origin, an instantaneous change will occur in the second stage mapping, which is undesirable.In order to prevent these potential problems and preserve directional intent, the first stage is used to prevent commands from entering into a small boundary around the negative V axis and the origin.It should be noted that this saturation stage does not result in commands mapped back to the envelope, which is instead achieved by the second stage.
The first stage ensures that controller commands cannot traverse the region shown in Figures 5 and 6, which is defined as where where  ∈ (0, V min ) defines the size of this small region around the negative V axis.In practice,  can be arbitrarily small as its only purpose is to provide a boundary around the negative V axis.In general, small values, such that  ≪ V min , are preferred to provide the least interference with controller intent.Figure 6 gives an example of the saturation implementation, which is detailed in the following discussion.During implementation, if at time   > 0 the controller issues a command such that (V(  ), (  )) ∈ , then   ≜ (  ) is recorded.If   ≥ 0, then for all subsequent controller commands such that (V(), ()) ∈  + the algorithm maps the commands to (V(),   ()) ∈  + ∩  + .Likewise in the case of   < 0, for all subsequent commands such that (V(), ()) ∈  − the algorithm maps the command to (V(),   ()) ∈  − ∩  − .We note that   (), the resultant angular rate command, is defined by the point on the boundary that corresponds to the original velocity command.
Figure 6 demonstrates a parabolic command evolution before and after the first saturation stage.In this case, we consider the commands evolving from right to left in the order they are numbered.The circles represent the original commands from the controller, and the crosses demonstrate the resulting command after the first saturation stage.Examining the figure, we see that commands 1-3 are left unaltered, as they are outside of the region .At some point between 3 and 4, the command crosses the boundary of , at which point   is recorded to be of some positive value.Since   ≥ 0, all subsequent commands such that (V(), ()) ∈  + are mapped to  + ∩  + , while preserving the velocity command, V().This trend is the case for commands 4-10 which are mapped to 4  -10  , respectively.Of particular interest are commands 6 and 7.Although these commands exit the region , they are still contained in  + .As such, the saturation algorithm continues to map these commands to  + using the same methodology.Lastly, we look at command 11, which occurs at some point after the commands have exited  + .In this case, since (V(), ()) ∉  + the saturation algorithm is no longer active.

Second Stage Saturation.
Once the commands are passed through the first stage of saturation, they are categorized to a specific region based on their location in the V- plane.Figure 7 shows four separate regions which use various methodologies to map the commands into the viable envelope.In Region 1, we focus on preserving the radius of curvature of the original commands as this directly relates to the intended path of the controller.Due to the limitations of the Ackermann platform, however, not all curvatures are achievable.As such, in Region 2, the second stage saturation algorithm maps commands to the nearest achievable radius of curvature while preserving lateral acceleration.We choose to preserve lateral acceleration so that the acceleration perceived by the vehicle and its sensors will remain unchanged.In the case of Regions 3 and 4, commands originate outside of any achievable radius of curvature or lateral acceleration.As a result, these commands are mapped to the closest achievable points that preserve intent.As expected, all commands originating within the envelope are left unaltered.For the remainder of this discussion, we only consider cases of () ≥ 0, as the other case is simply a reflection about the V axis.

Region 1 Methodology.
Ideally, during saturation it is desirable to reproduce the same trajectories that are commanded by the controller.In order to elicit this behavior, we examine the radius of curvature given by When the requested command lies within Region 1, which is given by the dark blue regions on the left and right sides of the envelope in Figure 7, the second stage saturation algorithm maps the new command to the nearest point on the envelope that provides the same radius of curvature.By maintaining the same radius of curvature, we ensure that the intended path is followed to the best of the vehicles capability.We note that the origin is included as part of Region 1, however, this presents a potential for discontinuous mapping depending on command evolution.For this reason, the first stage saturation method is used to prevent transitions through the origin.
Figure 8 provides several examples of Region 1 saturation.In Figure 8, the circles represent the command received from the first stage of saturation, and the crosses represent the command following the second saturation stage.In the first example, the command is mapped to the minimum velocity while still preserving the ()/V() ratio.The second and third examples exhibit the same behavior, mapping to the maximum velocity instead.Of particular interest is the third example, which maps along the V axis.In this case, the radius of curvature is infinite, but by using the inverse of curvature, the methodology is preserved.For this reason, the reciprocal of the radius is typically used to prevent singularities in computation.

Region 2 Methodology. Due to the constraints of an
Ackermann platform, there are a variety of conditions in which the desired radius of curvature is not achievable, as is the case of Region 2. To find a reasonable alternative, we start by considering the lateral acceleration of the vehicle given by Commands originating in Region 2 are mapped to the point on the envelope that is characterized by the same lateral acceleration.This point also corresponds to the nearest achievable radius of curvature.Figure 9 demonstrates saturation behavior for Region 2. The circle represents the command received from the first stage of saturation, and the cross represents the command following the second saturation stage.In this figure, the black dashed line represents the laws of constant lateral acceleration.As explained, both commands have the same lateral acceleration, with one being in the achievable command space.

Region 3 Methodology. While lateral acceleration offers
an intuitive choice for mapping commands to an obtainable radius of curvature, Ackermann platforms inherently have a maximum lateral acceleration, given by When the requested command exceeds the minimum radius of curvature and the maximum lateral acceleration, the command is mapped to the maximum velocity and maximum turning angle, represented by the corner of the envelope.Region 3 of Figure 7 depicts the area in question.By holding the command at the corner of the envelope in Region 3, we ensure a continuous mapping from Region 2 to Region 3 and from Region 3 to Region 1.

Region 4 Methodology.
Although lateral acceleration offers a good methodology for mapping commands, it does not always elicit optimal behavior.Recalling from Section 3.1, the controller can command high angular rates while also commanding little to no longitudinal velocity.In these cases, the lateral acceleration is small.If we were to map to the envelope maintaining this lateral acceleration, the result would yield a forward velocity, with a nearly zero angular rate, which would be opposite to desired.Instead, the vehicle should ideally turn as sharp as possible.
In Region 4, we examine the situation in which the radius of curvature is not obtainable and the command lies below the minimum lateral acceleration threshold given by In this case, the command is mapped to the minimal velocity and maximum angular rate, characterized by the left corner of the envelope given in Figure 7.For the situations in which the vehicle has a large requested angular rate with small velocity, the algorithm ensures that the vehicle turns as sharp as possible at a minimal velocity until the controller requests more achievable behavior.We extend Region 4 to encompass all negative velocity commands as well since the goal is to turn as much as possible until the controller requests positive velocities.We note that while Region 4 includes portions of the first stage of saturation, these portions will not be reached due to the effects of the first stage of saturation.If the controller commands were to cross the V axis while the requested velocity is negative, indicated by Figures 5 and 6, the second stage saturation algorithm would fully reverse the steering command.However, the first level of saturation restricts this potential chattering behavior by keeping the command in the same quadrant until the controller requests positive velocities.

Maintaining Lyapunov Criteria.
For the above cases, the controller commands are altered without regard to how it affects the tracking performance of the overall system.In order to ensure that the asymptotic stability of the error dynamics is still achieved, the control law is backsolved from (8) as ( [22]) where ṗ sat r () ∈ R 2 is the resulting reference time derivative, and, in this case, V sat () and  sat () are determined from the saturation algorithm.It is important to realize that  * () must be at least a class  1 function in time to guarantee both that ḋ() exists and also that the control law is continuous.To elicit this behavior, we modify the nominal following distance to be where   > 0,   > 0 are tuning constants and  ref () is the user desired reference distance.

Simulation Results with Saturation.
Prior to saturation, we chose  * () to follow the relationship given by (22).In order to obtain similar behavior during saturation and adhere  to the criteria of the previous section, we choose a desired reference distance of For comparison purposes, we introduce the same reference trajectory of (23).All simulation parameters are listed in Table 2.
Figure 10 demonstrates the resulting trajectory information with the addition of saturation constraints.Overall, the vehicle still follows the trajectory given by [  ()   ()] T of Table 2, which is desired.However, in the case of sharp turns, the vehicle continues forward, locking itself into a circular trajectory until it is able to reobtain the desired path.To facilitate this behavior, the reference system is pushed away from the trajectory detailed by [  ()   ()] T , overriding the dynamics described by (23).In addition to following the path, the saturation algorithm also eliminated the prior initial behavior, characterized by an immediate reversal of trajectory to satisfy distance requirements.Instead, we observe a situation in which the vehicle continues smoothly in a forward direction while steering back into the path.
From Figure 10 we can see the vehicle better adheres to our bioinspired model of the predator-prey interaction.Instead of reversing direction or undergoing unreasonably tight turns, the vehicle preserves speed and corrects heading to the best of its ability.In the same way, this behavior is largely what we would expect from a cheetah chasing its prey.
Further examination of the behavior is given in Figure 11.Clear limitations in the velocity are observed between 1 m/s and 10 m/s.Additionally, the implemented algorithm provides a smooth velocity command, as expected.This behavior is a result of a continuous mapping of saturation commands back to a region on or within the envelope.
Looking further at Figure 11, we see that the angular rate commands are significantly different from those obtained without saturation constraints.In general, the commanded angular rates achieve higher magnitudes than previously observed.From the third graph, however, it is obvious that there are clear bounds on the ratio of ()/V().As such, the vehicle achieves higher angular rates, while still following the criteria of an Ackermann based platform.
While the saturation constraints perform as desired, it is important to ensure that the overall distance following behavior is not sacrificed as this is a significant trait of our bioinspired desired behavior.Figure 12 demonstrates the comparison of the new following distance to the original.As expected, the added filter (34) between  * () and  ref () introduces a phase shift and attenuation of () when saturation constraints are introduced.Of some interest is the fact that the distance appears more irregular, as opposed to  the smooth original distance.This trend is a result of the fluctuating reference velocity during periods of saturation.Despite these differences, the desired trend is still present in regard to reference system behavior.Consequently, the saturation constraints are held as a valid addition to the overall control algorithm.

Experimental Testing and Results
To investigate the practicality of the controller, the algorithm was implemented on an experimental platform.This section discusses the experimental setup, the reference system manipulation, and the results.

Experimental Setup.
For the purpose of testing, the robotic platform is shown in Figure 13.This vehicle incorporates an Inertial Measurement Unit (IMU) that provides Euler angles at ±2 ∘ , acceleration on 3 axes at ±0.01 g, and angular rates at ±0.2 ∘ /sec.Additionally, the vehicle hosts an onboard GPS which provides position feedback information within ±1.5 m as well as velocity information accurate to within ±0.5 m/s.The sensor information was incorporated into an Extended Kalman Filter, designed specifically for this project, which provided feedback information on position, heading, and velocity, as well as the biases for heading and acceleration measurements.
The onboard actuators consist of a brushless motor for speed and standard hobby servo for steering, both of which were controlled using a generic servo controller.In order to implement the controller in the form presented by this paper, low-level PI controllers were experimentally designed and implemented to directly regulate speed and angular velocity.These controllers, the Kalman Filter, and the overall control algorithm were implemented through an onboard computer at an update rate of 40 Hz.

Potential Field Reference System Design.
For the purpose of experimental testing, the reference system was manipulated according to a simple potential field algorithm similar to that presented in [23][24][25].This method was chosen primarily because it offers a computationally simple manner to control the reference system.It also provides an intuitive velocity behavior, which is characterized by reducing velocity when obstacles are encountered.
To define the potential field algorithm, we begin by introducing distance between the reference system and the destination, where  des () ∈ R,  des, ∈ R is the  coordinate of the th destination waypoint, and  des, ∈ R is the  coordinate of the th destination waypoint.Furthermore, the distance between the reference system and surrounding obstacles is defined as where  obs, () ∈ R, where  ac > 0,  rc > 0,  > 0, and  > 1 are tuning constants,  a () ∈ R 2 is the attractive force vector,  r () ∈ R 2 is the repulsive force vector, and  obs is the number of obstacles within a given radius of the reference system.From (38), we can see that the attractive force,  a (), is essentially a unit vector multiplied by the tuning constant  ac .This vector points from the reference system toward the current destination at all times.Because of this relationship, the reference system will be pulled toward the goal with some constant force dictated by  ac .
Likewise, we note that (39) has a similar form to (38).This results in a force vector,  r (), that points from a particular obstacle toward the reference system, repelling it.Because of the exponential relationship caused by , the repulsive force increases as the reference system nears the obstacle.Consequently, the reference system is largely uninfluenced until it is within the vicinity of an obstacle, at which point it is pushed away.Ultimately, the combination of the attractive and repulsive forces,  a () and  r (), respectively, causes the reference system to avoid any obstacles while moving toward the goal.In order to utilize the force vectors given in (38) and (39), the reference system is given the following dynamics, where  > 0 and  > 0 are tuning constants for inertia and damping of the virtual system, respectively.To define these constants, we start by considering the maximum desired velocity in the presence of only attractive forces.When maximum velocity of the reference system is achieved, the reference acceleration reduces to p r () = 0, which yields  ṗ r () =  a () .
By taking the norm of both sides and considering the maximum reference velocity case, we can determine the damping as In regard to determining the inertia, we assume that the reference system persists at maximum velocity throughout the trajectory.If the kinetic energy is maintained along the path, then the path maintains the same characteristic shape, regardless of velocity.As such, the inertial term is defined as where  > 0 is the user defined kinetic energy term of the system.For all experimental tests, the potential field tuning constants used are listed in Table 3.We consider only cases with one or no obstacle presented.Furthermore, only obstacles within a 10 m radius are considered during force computation.For the testing results presented in this paper, V r max is varied accordingly to achieve the desired overall speeds.We do not consider the effect of the potential fields beyond the reference system as the controller is designed to drive the vehicle to the reference system, given that the reference system is composed of class  1 functions.To better understand the effects of potential fields, we refer the readers to [23][24][25], which better explore their behavior and usage.

Results
. Each experiment uses the tuning constants laid out in Table 4.In addition, the parameter   is varied to account for actuator dynamics in the steering.Moreover, V r max is altered to achieve varying speeds for each experiment.We begin by examining the basic tracking performance with no obstacles, presented below in Section 5.3.1.In the three scenarios that follow, the reference velocity is set to constant values of 2 m/s, 5 m/s, and 9 m/s.Each scenario uses the same set of waypoints, which the reference system follows based on the potential field setup presented earlier.Once the reference system is within a meter of the target waypoint, it switches its desired position to the subsequent point.In each of the three cases, the vehicle travels in a clockwise motion.The vehicle starts from rest in each case.We do not consider the exact coordinates of the initial position of the vehicle and reference system.Due to the nature of the experiment, this position is not repeatable and the effects of the initial conditions in these cases are inconsequential relative to the entirety of the path.
Following the tracking performance, we consider applications to obstacle avoidance in Section 5.3.2.Again, we consider three scenarios, a low, a medium, and a high speed case.For these tests, a specific set of waypoints is chosen as well as a single, constant obstacle.The target reference velocity is then varied to examine the behavior of the system.In these tests, data collection starts after the vehicle has reached a velocity relatively close to the intended reference velocity.This decision is to better focus on obstacle avoidance rather than the transient effects of accelerating to the target speed.

Experimental Tracking Performance.
For the low speed tracking test, the additional tuning parameters not given in Table 4 are set to   = 1 and V r max = 2 m/s.The low speed test results are given in Figure 14.As expected, the vehicle (solidred) tracks the reference trajectory (dashed-black) tightly.This is the typical behavior at low speeds, as the vehicle lags the reference system by a relatively small amount.While there are some slight deviations at the bottom and top of the graph, the overall performance is favorable in light of experimental results.
In Figure 15, we examine the response of the vehicle to a reference system that is moving at 5 m/s.All parameters are consistent with those in the previous example, with the exception of   = 2.5 and V r max = 5 m/s.At first glance, it is clear that the vehicle is cutting to the inside of the reference system's path, as seen in the bottom of the graph and in the top.This performance is ideal based on the derivation of the controller.As the vehicle moves faster, the following distance will increase.As a result, the vehicle begins correcting its trajectory earlier.Consequently, the vehicle moves to the inside of the reference system's path, which is corroborated in experimentation.While the vehicle stays largely within the reference system's path, in the upper left corner of both Figures 14 and 15 the vehicle travels outside the bound defined by the reference trajectory.These mild deviations are attributed to actuation dynamics.In derivation and simulation, we assume that the actuation of longitudinal and angular rates is instantaneous.In reality, there are dynamics that are crucial to the behavior of the system.When the controller commands a tight turn that is within the realm of reachable commands, the dynamics may still keep the vehicle from responding within the necessary time frame.In practice, these dynamic effects can be mitigated by adjusting the tuning parameters  V and   .By increasing   for the medium speed case, the deviation is substantially less than if the gains would be kept constant from the low speed case.
We refrain from holding   at high values for all tests because the dynamics are also influential at low speeds.With high values of   at low speeds, significant oscillation is seen in the commands, although tracking is largely unchanged.
Next, we examine the tracking performance at high speeds, the results of which are given in Figure 16.Again, all parameters are held constant according to Table 4, with the exception of   = 3.5 and V r max = 9 m/s.This test appears as a more extreme case of the medium speed result.In fact, the vehicle's path remains well within the bound laid out by the reference system aside from the initial straightaway and the last turn.As with the medium speed case, the deviation on the last turn is attributed to actuator dynamics, which are not fully compensated for but mitigated by higher values of   .

Application to Obstacle
Avoidance.The control algorithm presented can be conceptualized as a position filter, which smoothly follows a reference system despite sudden changes in direction.This behavior elicits the predator-prey interaction sought after.Potential fields, on the other hand, have natural obstacle avoidance properties, but they lack smoothness, which is particularly necessary for high speed situations.Using a combination of the two, we examine the performance of the given approach in the hope of finding more intuitive behavior.
Figures 17 and 18 examine the case of obstacle avoidance at low speed using the same parameters laid out in Table 4, with   = 1 and V r max = 2 m/s.In Figures 17,19

Figure 6 :
Figure 6: First stage representation of  with saturation example.

Figure 7 :
Figure 7: Region classification for second stage saturation.

Figure 9 :
Figure 9: Region 2 example for second stage saturation.

Figure 11 :
Figure 11: Controller commands for sinusoidal trajectory with saturation.

Figure 12 :
Figure 12: Comparison of following distance with (black) and without (red) saturation.

Figure 14 :Figure 15 :
Figure 14: Tracking performance at 2 m/s.The crosses are desired waypoints.

Figure 16 :
Figure 16: Tracking performance at 9 m/s.The crosses are desired waypoints.
, and 21, the triangles and circles denote the location of the reference

Table 1 :
Parameters for simulation without saturation.

Table 2 :
Parameters for simulation with saturation.
obs, ∈ R is the  coordinate of the th obstacle waypoint, and  obs, ∈ R is the  coordinate of the th obstacle waypoint.Based on these distance parameters, virtual force vectors are given such that

Table 3 :
Potential field tuning constants for experiments.

Table 4 :
Common controller tuning parameters for experiments.