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 9 m/s
were achieved. The results presented show that the algorithm demonstrates significant promise in high speed trajectory tracking with
obstacle avoidance.
1. 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–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–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–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–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.
2. 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,(1)p˙t=x˙ty˙t=vtcosθtvtsinθt,(2)θ˙t=ωt,where pt≜xtytT∈R2 is the vehicle’s position, v(t)∈R is the longitudinal velocity, θ(t)∈R is the heading, and ω(t)∈R is the vehicle’s angular velocity. A reference system is then introduced to represent a virtual target,(3)prt≜xrtyrt,where pr(t)∈R2 is the reference system’s position. In the interest of continuous controller commands, we must guarantee that the reference system is composed of class C1 functions in time. For convenience, the linear velocity of the reference system is defined as(4)vrt≜x˙rt2+y˙rt2,where vr(t)∈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 p(t)-pr(t)→d∗(t), as t→∞, where d∗(t)>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.
3. 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,(5)Rθt≜cosθt-sinθtsinθtcosθt.Using this transformation along with (1) and (3), the position error can be expressed in the body coordinate frame as(6)et≜RTθtprt-pt,where e(t)∈R2. 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,(7)δt≜dt0,where d(t)>0 is the commanded following distance. By guaranteeing that e(t)→δ as t→∞, the longitudinal error will converge to d(t) while the lateral error goes to zero.
To provide this behavior, we introduce the following control law ([22]),(8)vtωt=Δ-1t·Ktanhet-δt+RTθtp˙rt-δ˙t,where(9)Δt≜100dt,K≜kv00kω,and kv>0 and kω>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(t) is defined for all t≥0, we must guarantee that d(t)≠0 for all t≥0. In practice, if d(t)<0, the vehicle will follow the reference system but in reverse, such that v(t)<0. As such, we restrict the commanded following distance to d(t)>0. For design purposes, a more rigorous constraint of d(t)>(β-ε)>0, for all t≥0, is enforced, where β>ε>0 are tuning parameters used to place a minimum bound on d(t). The commanded following distance, d(t), is designed such that d(t)→d∗(t) as t→∞, provided that d∗(t)≥β as well.
At this point, we note the intuitive form of (8). For instance, from the RT(θ(t))p˙r(t) term, we see that the reference velocity, projected along the vehicle’s longitudinal axis, positively contributes to the commanded velocity, v(t). Likewise the reference velocity, projected along the lateral axis, contributes to the commanded angular rate, ω(t). In examining the δ˙(t) term, we see that an increasing distance negatively impacts the commanded velocity, as would be expected. Lastly, with regard to the Ktanh(e(t)-δ(t)) term, we see that the difference between longitudinal error and the commanded distance, d(t), 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 kv and kω, the function directly contributes bounded signals to commanded longitudinal and angular velocities, which are based on the difference between the tracking error e(t) and distance vector δ(t).
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 d∗(t)≥β, for all t≥0, and the distance d(t) is updated according to(10)d˙t=Γt,dt≥βΓt+β-dtdt-β-ε,dt<β,d0≥β,where(11)Γt≜d˙∗t-λdt-d∗t,with tuning constant λ>0, then the distance d(t), between the vehicle and reference system, converges to the desired distance, d∗(t), while guaranteeing that d(t)>(β-ε)>0. Meanwhile, the tracking error e(t) converges to the distance vector δ(t).
Proof.
Consider the Lyapunov function candidate(12)Vt=12e1Tte1t+12dt-d∗t2,where(13)e1t≜et-δt.
In order to examine the derivative of the Lyapunov function along the system trajectories, the time derivative of the tracking error, e(t), given by (6), is computed as follows,(14)e˙t=R˙Tθt,ωtprt-pt+RTθtp˙rt-p˙t.The time derivative of the orthonormal transformation, R(θ(t)), is given by(15)R˙θt,ωt=-ωtsinθt-ωtcosθtωtcosθt-ωtsinθt=RθtSωt,where(16)Sωt≜0-ωtωt0.By substituting (1) and (15) into (14), the error dynamics can be simplified to(17)e˙t=STωtRTθtprt-pt+RTθtp˙rt-RTθtvtcosθtvtsinθt=-Sωtet+RTθtp˙rt-vt0=-Sωtet+RTθtp˙rt-vtdtωt+0dtωt=-Sωte1t-Δtvtωt+RTθtp˙rt.By substituting the control law given in (8), the error dynamics are then further simplified to(18)e˙t=-Sωte1t-Ktanhe1t+δ˙t.
The error dynamics given by (18) are then substituted into the time derivative of the Lyapunov function,(19)V˙t=e1Tte˙1t+dt-d∗td˙t-d˙∗t.If we consider the case in which d(t)≥β, we then substitute (10), (11), and (18) into (19) to obtain(20)V˙t=e1Tt-Sωte1t-Ktanhe1t-λdt-d∗t2=-e1TtKtanhe1t-λdt-d∗t2.From (20), it is trivial to show that again V˙(t)≤0 for all t≥0, d(t)≥β.
The Lyapunov time derivative is again examined when d(t)<β. Again, we substitute (10), (11), and (18) into (19) to obtain(21)V˙t=-e1TtKtanhe1t+dt-d∗t·-λdt-d∗t+β-dtdt-β-ε=-e1TtKtanhe1t-λdt-d∗t2+dt-d∗tβ-dtdt-β-ε.
The commanded following distance, d(t), is restricted such that d(t)>(β-ε) by design. This behavior is the result of β-d(t)/d(t)-(β-ε)→∞ as d(t)→(β-ε). As d(t) decreases below β but still remains larger than (β-ε), the additional corrective term pushes d(t) back toward β, eventually guaranteeing d˙(t)>0 as d(t)→(β-ε). As such, the corrective term β-d(t)/d(t)-(β-ε)>0 for d(t)<β. Additionally, since d∗(t)≥β>d(t), we find that (d(t)-d∗(t))(β-d(t)/d(t)-(β-ε))<0. Since the remainder of the Lyapunov time derivative terms are the same as given in (20), we can conclude that V˙≤0 for all t≥0.
3.1. 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(22)d∗t=αvrt+β,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,(23)p˙rt=-10xrt+10rxt-10yrt+10ryt,where rx(t) and ry(t) are the desired reference trajectories in x and y, respectively. Figures 1–3 show the results of simulation using the parameters given in Table 1.
Parameters for simulation without saturation.
Desired Reference Trajectory
Tuning Parameters
Initial Conditions
rx(t)=0.5t
kv=1
xr(0)=0
ry(t)=10sin(0.5t)
kω=1
yr(0)=0
λ=1
x(0)=-0.1
α=0.5
y(0)=0
β=0.1
θ(0)=0
d(0)=0.1
Sinusoidal trajectory without saturation.
Following distance without saturation.
Controller commands for sinusoidal trajectory without saturation.
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 y 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.
4. 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 δ˙(t). 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,(24)ωtvt≤tanϕmaxL,(25)vmin≤vt≤vmax,where L>0 is the wheelbase length from rear to front axle, ϕmax>0 is the maximum steering angle deflection referenced from the longitudinal body axis, vmin>0 is a lower bound on the longitudinal velocity, and vmax>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.
Graphical representation of viable control envelope (shaded).
4.1. 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(26)S≜S+∩S-,where(27)S+≜v,ω∈R2:0≤ω≤s,v≤s2-ω2∪v,ω∈R2:v<s,ω<0,(28)S-≜v,ω∈R2:-s≤ω≤0,v≤s2-ω2∪v,ω∈R2:v<s,ω>0,where s∈(0,vmin) defines the size of this small region around the negative v axis. In practice, s can be arbitrarily small as its only purpose is to provide a boundary around the negative v axis. In general, small values, such that s≪vmin, 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.
Graphical representation of S+ (a) and S- (b).
First stage representation of S with saturation example.
During implementation, if at time te>0 the controller issues a command such that (v(te),ω(te))∈∂S, then ωe≜ω(te) is recorded. If ωe≥0, then for all subsequent controller commands such that (v(t),ω(t))∈S+ the algorithm maps the commands to (v(t),ωs(t))∈∂S+∩S+. Likewise in the case of ωe<0, for all subsequent commands such that (v(t),ω(t))∈S- the algorithm maps the command to (v(t),ωs(t))∈∂S-∩S-. We note that ωs(t), 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 S. At some point between 3 and 4, the command crosses the boundary of S, at which point ωe is recorded to be of some positive value. Since ωe≥0, all subsequent commands such that (v(t),ω(t))∈S+ are mapped to ∂S+∩S+, while preserving the velocity command, v(t). 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 S, they are still contained in S+. As such, the saturation algorithm continues to map these commands to ∂S+ using the same methodology. Lastly, we look at command 11, which occurs at some point after the commands have exited S+. In this case, since (v(t),ω(t))∉S+ the saturation algorithm is no longer active.
4.2. 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 ω(t)≥0, as the other case is simply a reflection about the v axis.
Region classification for second stage saturation.
4.2.1. 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(29)rt=vtωt.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 ω(t)/v(t) 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 1 examples for second stage saturation.
4.2.2. 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(30)aLt=vtωt.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 2 example for second stage saturation.
4.2.3. 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(31)aLmax≜vmax2tanϕmaxL.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.
4.2.4. 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(32)aLmin≜vmin2tanϕmaxL.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.
4.3. 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])(33)p˙rsatt=RθtΔtvsattωsatt-Ktanhet-δt+δ˙t,where p˙rsat(t)∈R2 is the resulting reference time derivative, and, in this case, vsat(t) and ωsat(t) are determined from the saturation algorithm. It is important to realize that d∗(t) must be at least a class C1 function in time to guarantee both that d˙(t) exists and also that the control law is continuous. To elicit this behavior, we modify the nominal following distance to be(34)d¨∗t+2ζdωdd˙∗t+ωd2d∗t=ωd2dreft,where ζd>0, ωd>0 are tuning constants and dref(t) is the user desired reference distance.
4.4. Simulation Results with Saturation
Prior to saturation, we chose d∗(t) 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(35)dreft=αvrt+β.
For comparison purposes, we introduce the same reference trajectory of (23). All simulation parameters are listed in Table 2.
Parameters for simulation with saturation.
Desired Reference Trajectory
Tuning Parameters
Initial Conditions
rx(t)=0.5t
kv=1
xr(0)=0
ry(t)=10sin(0.5t)
kω=1
yr(0)=0
λ=1
x(0)=-0.1
α=0.5
y(0)=0
β=0.1
θ(0)=0
L=0.3556
d(0)=0.1
γmax=25°
d∗(0)=0.1
vmin=1
d˙∗(0)=0
vmax=10
s=0.01
ωd=2.5
ζd=0.85
Figure 10 demonstrates the resulting trajectory information with the addition of saturation constraints. Overall, the vehicle still follows the trajectory given by rxtrytT 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 rxtrytT, 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.
Sinusoidal trajectory with saturation algorithm.
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 1m/s and 10m/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.
Controller commands for sinusoidal trajectory with saturation.
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 ω(t)/v(t). 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 d∗(t) and dref(t) introduces a phase shift and attenuation of d(t) 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.
Comparison of following distance with (black) and without (red) saturation.
5. 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.
5.1. 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.01g, and angular rates at ±0.2°/sec. Additionally, the vehicle hosts an onboard GPS which provides position feedback information within ±1.5m as well as velocity information accurate to within ±0.5m/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.
Experimental testing platform.
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.
5.2. 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–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,(36)ddest≜xdes,i-xrt2+ydes,i-yrt2,where ddes(t)∈R, xdes,i∈R is the x coordinate of the ith destination waypoint, and ydes,i∈R is the y coordinate of the ith destination waypoint. Furthermore, the distance between the reference system and surrounding obstacles is defined as(37)dobs,jt≜xobs,j-xrt2+yobs,j-yrt2,where dobs,j(t)∈R, xobs,j∈R is the x coordinate of the jth obstacle waypoint, and yobs,j∈R is the y coordinate of the jth obstacle waypoint. Based on these distance parameters, virtual force vectors are given such that(38)Fat≜Facddestxdes,i-xrtydes,i-yrt,(39)Frt≜∑j=1Nobs-FrcWdobs,jtnxobs,j-xrtyobs,j-yrt,where Fac>0, Frc>0, W>0, and n>1 are tuning constants, Fa(t)∈R2 is the attractive force vector, Fr(t)∈R2 is the repulsive force vector, and Nobs is the number of obstacles within a given radius of the reference system.
From (38), we can see that the attractive force, Fa(t), is essentially a unit vector multiplied by the tuning constant Fac. 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 Fac.
Likewise, we note that (39) has a similar form to (38). This results in a force vector, Fr(t), that points from a particular obstacle toward the reference system, repelling it. Because of the exponential relationship caused by n, 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, Fa(t) and Fr(t), 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,(40)mp¨rt+cp˙rt=Fat+Frt,where m>0 and c>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(t)=0, which yields(41)cp˙rt=Fat.By taking the norm of both sides and considering the maximum reference velocity case, we can determine the damping as(42)c=Facvrmax.
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(43)m=2Evrmax2,where E>0 is the user defined kinetic energy term of the reference 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 10m radius are considered during force computation. For the testing results presented in this paper, vrmax 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 C1 functions. To better understand the effects of potential fields, we refer the readers to [23–25], which better explore their behavior and usage.
Potential field tuning constants for experiments.
Potential Field Parameters
Fac=10
Frc=8
W=2
n=1.5
E=14
5.3. Results
Each experiment uses the tuning constants laid out in Table 4. In addition, the parameter kω is varied to account for actuator dynamics in the steering. Moreover, vrmax is altered to achieve varying speeds for each experiment.
Common controller tuning parameters for experiments.
Tuning Parameters
kv=1
α=1.3
β=1
λ=1
s=0.01
ωd=2.5
ζd=0.85
L=0.3556
ϕmax=25°
vmax=10
vmin=1
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 2m/s, 5m/s, and 9m/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.
5.3.1. Experimental Tracking Performance
For the low speed tracking test, the additional tuning parameters not given in Table 4 are set to kω=1 and vrmax=2m/s. The low speed test results are given in Figure 14. As expected, the vehicle (solid-red) 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.
Tracking performance at 2m/s. The crosses are desired waypoints.
In Figure 15, we examine the response of the vehicle to a reference system that is moving at 5m/s. All parameters are consistent with those in the previous example, with the exception of kω=2.5 and vrmax=5m/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.
Tracking performance at 5m/s. The crosses are desired waypoints.
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 kv and kω. By increasing kω 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 kω at high values for all tests because the dynamics are also influential at low speeds. With high values of kω 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 kω=3.5 and vrmax=9m/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 kω.
Tracking performance at 9m/s. The crosses are desired waypoints.
5.3.2. 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 kω=1 and vrmax=2m/s. In Figures 17, 19, and 21, the triangles and circles denote the location of the reference system and vehicle, respectively, at certain instances in time. Looking at Figure 17, the vehicle tightly tracks the reference system despite a sharp, awkward corner around the obstacle. Since the speed is small, seen in Figure 18, this behavior is acceptable. Of particular interest is the fact that the velocity drops to only 1m/s as the system avoids the obstacle. This is a natural result of the potential field system, which we utilize to obtain an intuitive response from the platform. Additionally, we note that this lower velocity is also bounded by the saturation algorithm which maintains at least a 1m/s speed. Consequently, by examining the time stamps of Figure 17, we see that the distance between the reference and vehicle reduces during the cornering, allowing the vehicle to better track the turn.
Obstacle avoidance at low speeds. Triangles and circles denote the location of reference system and vehicle, respectively, at certain instances in time.
Velocity of obstacle avoidance at low speeds.
Obstacle avoidance at medium speeds. Triangles and circles denote the location of reference system and vehicle, respectively, at certain instances in time.
The results of obstacle avoidance at medium speeds are given in Figures 19 and 20. Here the tuning gains are altered such that kω=2.5 and vrmax=6m/s. As with the medium speed case in the tracking study, the vehicle does not follow the reference system as tightly. This behavior, however, has distinct advantages in dealing with obstacles. At the 5-second mark, the vehicle begins to diverge from the reference path in order to avoid the obstacle, as opposed to waiting until the last instant. By steering earlier, the vehicle achieves a less severe curve during avoidance, facilitating higher speeds throughout. With regard to the predator-prey analogy, this behavior is more intuitive than basic potential fields. Rather than directly mimicking the prey’s path, the predator adjusts its path to easily close in on the prey. The intuition naturally follows for humans as well, as a human driver would avoid immediately upon the observation of an obstacle, rather than waiting. The human’s path of avoidance would likewise be based on speed. Looking at Figure 20, the speed decreases from 6m/s to 3m/s during the turn. Again, this is a natural behavior as the average driver would naturally slow down in order to safely avoid the obstacle. By slowing down, the distance decreases, which then facilitates better tracking of the reference system.
Velocity of obstacle avoidance at medium speeds.
Obstacle avoidance at high speeds. Triangles and circles denote the location of reference system and vehicle, respectively, at certain instances in time.
Lastly, we consider the case of obstacle avoidance at high speeds, seen in Figures 21 and 22. For this case, the tuning gains are altered such that kω=2.5 and vrmax=9m/s. Looking at Figure 21, the algorithm clearly smooths the trajectory of the reference system. In the same way a predator would make milder turns at high speeds, the vehicle departs from the reference path at approximately 3 seconds, at a point much earlier than the reference system begins to avoid. During this outward swerve, Figure 22 indicates that the velocity begins to drop off at 5 seconds. This drop results in a decrease from 9m/s to 6m/s, a 33% dip opposed to the 50% change in the previous scenario. With a smoother trajectory, the algorithm allows for more preservation of speed than previously seen. Lastly, we note that the distance between the reference system and vehicle decreases, allowing for a more aggressive turn after passing the obstacle.
Velocity of obstacle avoidance at high speeds.
6. Conclusions
Many control algorithms currently in existence are largely effective but lack the simplicity of intuitive behavior native to most people. With the increasing push for driverless technologies, however, it is becoming more important to develop algorithms that exploit the trends expected by everyday users in high speed situations. To this end, we consider nature’s predators for inspiration. Specifically, our bioinspired model references the cheetah on several occasions. We note that a variable distance to its prey allows it to better plan its path while maintaining a high speed. Additionally, we assume natural constraints on its motion that also hold favorable characteristics for fast maneuvering.
In an effort to reproduce this behavior, a control algorithm, based on a unicycle model, is introduced. This algorithm focuses on driving the current vehicle to within some variable following distance of a user defined reference system by commanding longitudinal and angular rates. This algorithm is proven stable through Lyapunov criteria.
Due to the limitations of the unicycle model, we are not able to directly achieve the sought after motion constraints of the predator. As a result, additional saturation constraints are implemented to enforce Ackermann steering kinematics on the system’s behavior. The saturation constraints are presented as a two stage algorithm. The first stage, which is not a sufficient method within itself, focuses on preventing chatter in the final command. The second stage receives commands from the first stage and maps them within an achievable envelope. From the saturation methodologies, the reference system is then manipulated to satisfy the Lyapunov criteria. Based on simulations, the dual stage saturation algorithm exhibits the desired intuitive behavior.
Using potential fields to control the reference system, the controller is implemented on a small scale robotic vehicle. Testing results thus far indicate that the proposed controller, coupled with a potential field algorithm, exhibits strong intuitive behaviors similar to those described by the predator-prey relationship. At low speeds, the vehicle tracks the reference system within a close tolerance of the reference system’s path. This trend holds in the case of obstacle avoidance. However, at high speeds, the vehicle diverges from the set path and instead smooths the overall trajectory, allowing some conservation of speed without extremely aggressive steering, much as would be expected from a human controller. Overall, the algorithm was used to successfully navigate trajectories at up to 9m/s.
There are several instances in which the dynamics prevent the vehicle from consistently following the idealized path. Consequently, future work will focus on including these effects through backstepping or other related methods. Also, the algorithm will be refined for applicability to multiple platforms, most likely through adaptive methods.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
This material is based upon the work performed while Dr. Alexander Leonessa was serving at the National Science Foundation of the United States. The authors would also like to thank Virginia Tech’s Open Access Subvention Fund for their support in publishing this work.
ChuK.LeeM.SunwooM.Local path planning for off-road autonomous driving with avoidance of static obstacles20121341599161610.1109/tits.2012.21982142-s2.0-84870559556MoonS.LeeU.ShimD. H.Study on real-time obstacle avoidance for unmanned ground vehiclesProceedings of the International Conference on Control, Automation and Systems (ICCAS '10)October 2010133213352-s2.0-78751536334Fu-guangD.PengJ.Xin-qianB.Hong-jianW.AUV local path planning based on virtual potential field4Proceedings of the IEEE International Conference on Mechatronics and Automation (ICMA '05)August 2005171117162-s2.0-27744451378DailyR.BevlyD. M.Harmonic potential field path planning for high speed vehiclesProceedings of the American Control ConferenceJune 20084609461410.1109/acc.2008.45872222-s2.0-52449106466ZhaoP.ChenJ.MeiT.LiangH.Dynamic motion planning for autonomous vehicle in unknown environmentsProceedings of the IEEE Intelligent Vehicles Symposium (IV '11)June 2011Baden-Baden, Germany28428910.1109/ivs.2011.59405062-s2.0-79960828476GlaserS.VanholmeB.MammarS.GruyerD.NouvelièreL.Maneuver-based trajectory planning for highly autonomous vehicles on real road with traffic and driver interaction201011358960610.1109/tits.2010.20460372-s2.0-77956344419HäselichM.HandzhiyskiN.WinkensC.PaulusD.Spline templates for fast path planning in unstructured environmentsProceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '11)September 2011San Francisco, Calif, USAIEEE3545355010.1109/IROS.2011.60947562-s2.0-84455200426FulgenziC.SpalanzaniA.LaugierC.Dynamic obstacle avoidance in uncertain environment combining PVOs and occupancy gridProceedings of the IEEE International Conference on Robotics and Automation (ICRA '07)April 20071610161610.1109/robot.2007.3635542-s2.0-36348965932RichardsonA.OlsonE.Iterative path optimization for practical robot planningProceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '11)September 20113881388610.1109/iros.2011.60485212-s2.0-84455195572BhattacharyaP.GavrilovaM. L.Voronoi diagram in optimal path planningProceedings of the 4th International Symposium on Voronoi Diagrams in Science and Engineering (ISVD '07)July 2007384710.1109/isvd.2007.432-s2.0-47849090248ValavanisK. P.HebertT.KolluruR.TsourveloudisN.Mobile robot navigation in 2-D dynamic environments using an electrostatic potential field200030218719610.1109/3468.8331002-s2.0-0033872805ParkM.LeeM.Experimental evaluation of robot path planning by artificial potential field approach with simulated annealing4Proceedings of the 41st SICE Annual ConferenceerenceAugust 2002Osaka, Japan2190219510.1109/sice.2002.1195739HuX.Clonal selection based mobile robot path planningProceedings of the IEEE International Conference on Automation and Logistics (ICAL '08)September 200843744210.1109/ical.2008.46361912-s2.0-56449117424WilsonA. M.LoweJ. C.RoskillyK.HudsonP. E.GolabekK. A.Mc-NuttJ. W.Locomotion dynamics of hunting in wild cheetahs2013498745318518910.1038/nature122952-s2.0-84878963299FierroR.LewisF. L.Control of a nonholonomic mobile robot: backstepping kinematics into dynamics199714314916310.1002/(sici)1097-4563(199703)14:360;149::aid-rob162;3.0.co;2-r2-s2.0-0031103696DeVonD.BretlT.Kinematic and dynamic control of a wheeled mobile robotProceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS ’07)November 2007San Diego, Calif, USA4065407010.1109/iros.2007.43995992-s2.0-51349098828PathakK.AgrawalS. K.Planning and control of a nonholonomic unicycle using ring shaped local potential fieldsProceedings of the American Control Conference (AAC '04)July 2004236823732-s2.0-8744288404WangQ.TianY.-P.Formation control of multiple nonholonomic unicycles using adaptive perturbation methodProceedings of the 51st IEEE Conference on Decision and Control (CDC '12)December 20122453245810.1109/cdc.2012.64264662-s2.0-84874223545FigueiredoL. C.JotaF. G.Nonholonomic systems stabilization and tracking control using discontinuous controlProceedings of the IEEE Conference on Automation Science and Engineering (CASE '05)August 200514214710.1109/coase.2005.15067592-s2.0-33745965070AicardiM.CasalinoG.BicchiA.BalestrinoA.Closed loop steering of unicycle like vehicles via Lyapunov techniques199521273510.1109/100.3882942-s2.0-0029271815CaronaR.AguiarA. P.GasparJ.Control of unicycle type robotsIV Jornadas de Engenharia Electronica e Telecomunicacoes e de ComputadoresNovember 2008180185Medina-GarciadiegoV.LeonessaA.Tracking control of a nonholonomic ground vehicleProceedings of the American Control Conference (ACC '11)June-July 2011San Francisco, Calif, USAIEEE1710171310.1109/ACC.2011.5991169BorensteinJ.KorenY.High-speed obstacle avoidance for mobile robotsProceedings of the 3rd International Symposium on Intelligent ControlAugust 19883823842-s2.0-0024183716BorensteinJ.KorenY.Real-time obstacle avoidance for fast mobile robots19891951179118710.1109/21.440332-s2.0-0024733382KorenY.BorensteinJ.Potential field methods and their inherent limitations for mobile robot navigationProceedings of the IEEE Conference on Robotics and AutomationApril 199113981404