Minimum-Time Trajectories for Steered Agent With Constraints on Speed, Lateral Acceleration, and Turning Rate

Minimum-time problems have long been a source of fascination for the mathematics community, reaching as far back as the brachistochrone problem that arguably led to the creation of optimal control theory and the calculus of variations [1]. The ongoing progress in the availability and capability of mobile robotic systems has seen an increase in interest in robotic minimum-time motion planning problems. In biology, solutions to minimum-time motion problems inform studies of predator-prey interactions. Optimal trajectories for minimum-time problems are closely related to optimal strategies in pursuit and evasion differential games, making them useful both for design and analysis. Further, the time-optimal solution for a single agent is an important building block for the design and analysis of multi-agent systems, such as the evasion of a group from a pursuer [2].

A popular model with which to study motion planning is the “steered agent” with state consisting of a point on the plane $r=(x,y)∈ℝ2$ with an associated heading angle $θ∈S$⁠. The motion of the agent is constrained such that its velocity is aligned with its heading at all times, with no side-slip. With this model, many different types of vehicles can be studied through the choice of control inputs and associated constraints on those inputs. The control inputs are typically the instantaneous speed v and the angular turning rate $ω=θ˙$⁠. For example, much work has been done to characterize time-optimal trajectories for steered agents with limited turning rates, notably the “Dubins vehicle,” which has a constant forward speed (⁠$v=v¯=$constant), and the Reeds-Shepp vehicle, which also allows for reverse motion (⁠$v=±v¯$⁠). See Ref. [3] for a review and Ref. [4] for detailed derivation of optimal paths with fixed terminal headings. The steered agent model can also be used to represent a two-wheeled differential-drive robot. A limit on the angular rate of each wheel leads to a set of linear inequalities on the speed and turning rate of the agent [5].

In this paper, we solve the problem of reaching a desired point on the plane in minimum time for a steered agent with inputs and constraints that make it applicable to the study of terrestrial (legged) animal motion as well as the design of robotic motion. The model is analytically tractable for a single agent and thus extendable to evasion and coverage problems for multi-agent systems. The constraints on the control inputs, instantaneous speed and angular turning rate, are as follows: (1) speed is positive and bounded by a maximum value, (2) the magnitude of the turning rate is bounded, and (3) the magnitude of the lateral acceleration (the product of speed and turning rate) is bounded.

For our novel set of constraints, we can still leverage Pontryagin's minimum principle as in the studies of the Dubins and differential drive agents. However, our model differs from both the Dubins and Reeds-Shepp models in that it allows for the agent to rotate in place with zero forward speed. In addition, the lateral acceleration constraint creates coupling between the two inputs of speed and turning rate, such that the agent must slow down to achieve a higher turning rate and vice versa. The angular acceleration constraint is chosen with regard to legged locomotion. A study of the kinematics of horses during polo games and track racing [6] indicates that grip strength and limb force limits constrain the maximum lateral acceleration during a turn, such that the horses must decrease their speed in anticipation of tight turns. By imposing a constraint on the lateral acceleration of our steered agent, we capture the tradeoff between speed and turning rate that is seen in nature.

Inherent in the choice of speed as a control input is the assumption that forward acceleration (thrust) is unbounded. Although legged animals and some robots can achieve large accelerations [7,8], there will be a limit in any physical system. Nonetheless, the assumption helps with analytical tractability and allows for real-time computation. It is of interest in future work to compare performance determined here with that in the case of bounds on forward acceleration.

Our analysis is based closely on the geometric methods of Balkcom and Mason, which were applied to differential drive vehicles with limited wheel speeds in Ref. [9] and to extremal trajectories for more general constraints in Ref. [5]. These methods have also been applied to minimum-time trajectories for omni-directional robots [10] and minimum wheel-rotation for the differential drive [11].

The main contributions of this paper are as follows: First, we solve the minimum-time problem for a single steered agent with a novel set of constraints chosen to provide a tractable model for the study of terrestrial animal motion as well as the design of robotic motion. We solve for open-loop optimal trajectories analytically and derive the optimal control input as a state-feedback control law. Optimal trajectories have piecewise-constant control inputs, such that the trajectories consist of up to four discrete phases: rotate in place, slow moving turn at maximum turning rate but reduced speed, fast moving turn at maximum speed but lower turning rate, and forward motion at maximum speed. Additionally, we show that as the bound on lateral acceleration approaches zero, the optimal trajectories approach those for a differential drive agent. Second, we demonstrate how the results for a single agent can be used to analyze the minimum-time-to-reach coverage problem for a system of N mobile agents with the same inputs and constraints as the single agent. We prove a lower bound on achievable performance and introduce an iterative coverage algorithm.

In Sec. 2, we present the formal problem statement and system equations of motion. Section 3 derives extremal control inputs according to Pontryagin's minimum principle. In Sec. 4, we prove conditions on the possible families of optimal trajectories, and in Sec. 5, we solve for open-loop control switching times for all cases. Section 6 presents a state-feedback formulation of the optimal control based on the relative position of the destination in a body-fixed frame. We examine special limiting cases of the lateral acceleration parameter in Sec. 7. Multi-agent coverage is presented in Sec. 8. We conclude in Sec. 9.

The control inputs $u(t)=(v(t),ω(t))T$ at time t consist of the forward speed $v∈ℝ$ and the turning rate $ω∈ℝ$⁠. We impose the following constraints on the control input:

1. (1)

   Limited speed: Let $v¯>0$ be the maximum speed. The speed control must satisfy $v(t)≤v¯$ for all time t.

2. (2)

   No reverse motion: Speed must satisfy $v(t)≥0$ for all time t, such that the agent never moves in reverse.

3. (3)

   Limited turning rate: Let $ω¯>0$ be the maximum turning rate. Then, the turning control must satisfy $|ω(t)|≤ω¯$ for all time t.

4. (4)

   Limited lateral acceleration: Let $μ>0$ represent the maximum lateral acceleration (turning traction limit). The inputs v(t) and $ω(t)$ must satisfy $|v(t) ω(t)|≤μ$ for all time t. We assume that $μ<v¯ ω¯$ so that the lateral acceleration constraint is active on part of the boundary of the control domain.

We define the admissible control region $Ω={u=(v,ω)∈ℝ2 | 0≤v≤v¯, |ω|≤ω¯, |vω|≤μ<v¯ω¯}$ as illustrated in Fig. 1. Admissible controls $U$ for the agent are bounded Lebesgue measurable functions from $ℝ+$ to Ω.

To solve for the optimal trajectories, we begin by using Pontryagin's minimum principle to find families of extremal trajectories that satisfy necessary conditions on optimality. We follow the method used in Ref. [9], which solved for optimal trajectories for differential drive robots with the same equations of motion as the current system, but with different constraints on the inputs. This leads to different switching functions and extremal controls. In the subsequent section, we use boundary constraints to characterize which of the extremal trajectories are optimal under different conditions.

where $q=(x,y,θ)T$⁠, $f(q,u)=(v cos θ,v sin θ,ω)T$⁠, and $ψ(t)=(x(t)−x1)2+(y(t)−y1)2$⁠, with initial conditions $q(0)=(x0,y0,θ0)T$⁠.

where the adjoint vector $λ=(λx,λy,λθ)∈ℝ3$ represents the partial derivative of the value of the cost function (in this case, the minimum time remaining to reach the destination) with respect to the system state.

The level set η = 0 describes a line in the x-y plane passing through the destination point $(x1,y1)T$⁠. β describes the agent's heading relative to the direction along the η = 0 line.

This makes it straightforward to apply Pontryagin's minimum principle to find extremal controls as a function of the state. Since this is a minimum-time problem with the dynamics linear in the control inputs, the optimal control will be of a bang–bang type, always taking values along the control constraint surfaces shown in Fig. 1.

Given our constraints on the control inputs, we need to determine which value of u will minimize the Hamiltonian for each point in the state space. We follow the same procedure as in Ref. [12], except that the additional constraint on lateral acceleration prompts a third switching function.

On time intervals for which the switching functions are nonzero, the corresponding extremal controls are called generic controls. These fall into three categories based on the signs of the switching functions. The generic control inputs along with their corresponding extremal trajectories are as follows, for initial state $q(0)=(x0,y0,θ0)T$⁠. In each case, $θ(t)=θ0+ω*t$⁠.

1. (1)

   Rotation: When $ϕ1<0$⁠, the agent rotates in place at maximum turning rate: $v*=0$ and $ω*=ω¯sgn(ϕ2)$⁠. Here, $q(t)=(x0,y0,θ0+ω*t)T$⁠.

2. (2)
   Slow turn: When $ϕ1>0$ and $ϕ3>0$⁠, the agent moves forward with low speed while turning at the maximum rate: $v*=μ/ω¯$ and $ω*=ω¯sgn(ϕ2)$⁠. The agent moves on a circular arc with radius $Rs=μ/ω¯2$⁠, with

   $(x(t)y(t))=(x0y0)+B(θ0)(Rs sin(ω¯t)sgn(ϕ2)Rs(1−cos(ω¯t)))$

   (5)

   where $B(θ)$ is the standard rotation matrix

   $B(θ)=( cos θ−sin θ sin θ cos θ)$
3. (3)
   Fast turn: When $ϕ1>0$ and $ϕ3<0$⁠, the agent moves forward at maximum speed while turning at a lower rate: $v*=v¯$ and $ω*=sgn(ϕ2)μ/v¯$⁠. The agent moves on a circular arc with radius $Rf=v¯2/μ$⁠, with

   $(x(t)y(t))=(x0y0)+B(θ0)(Rf sin(μt/v¯)sgn(ϕ2)Rf(1−cos(μt/v¯)))$

   (6)

At times where some $ϕi=0$⁠, there exist multiple inputs that minimize the Hamiltonian. When the state arrives at such a switching surface, the control may have an instantaneous switching if the state instantaneously traverses the switching surface, or an interval of singular control where the state remains on the switching surface for some time interval. We must examine each switching surface separately.

When $ϕ2=0$ with $ϕ1>0$ at some time $t′$⁠, the agent is on the switching surface between fast turn left and fast turn right. Geometrically, this places the agent on the η = 0 line with $cos β>0$⁠, and the Hamiltonian is minimized by $v=v¯$ with ω taking any value in $[−μ/v¯,μ/v¯]$⁠. For $cos β≠1$⁠, the agent is not aligned with the η = 0 line. So, the positive speed will bring it off of the line at the next moment, which would manifest as an instantaneous switching from fast turn left to fast turn right or vice versa without an extended singular control. In the case that $cos β=1$⁠, the agent is arriving on the η = 0 line and tangent to it. Forward motion with ω = 0 would keep it on the line for further time $t>t′$⁠.

This represents an interval of singular control, which as we show is part of many time-optimal trajectories. It is straightforward to show that if the agent starts with its heading in the direction of the destination point, then the minimum-time trajectory to reach that point is forward motion at maximum speed.

When $ϕ3=0$ and $ϕ1>0$ at some time $t′$⁠, the agent is on the switching surface between fast turn and slow turn with direction determined by the sign of $ϕ2$⁠. On this switching surface, the Hamiltonian is minimized by two possible values of the control input: a fast turn $u=(v¯,±μ/v¯)T$ or a slow turn $u=(μ/ω¯,±ω¯)T$ (positive for left turn).

Thus, on the switching surface, the derivative is always nonzero for the minimizing control $u=u*$⁠, so there can be no singular interval for the $ϕ1=0$ switching surface.

We consider the situation that the state lies on multiple switching surfaces simultaneously. If both $ϕ1$ and $ϕ2$ (and subsequently $ϕ3$⁠) are zero for a given state, then H = 1. One of the conditions for the application of the minimum principle in a minimum-time problem is that the Hamiltonian is constant with H = 0. So, we can conclude that a minimum-time trajectory can never reach a state that lies on multiple switching surface in this system. The trivial case corresponds to the agent starting at the destination point.

Similarly, consider the case that $ϕ2=0$ with $ϕ1<0$⁠. Here, the ω term does not appear in H, and the minimizing control has v = 0, leading to the situation that H = 1. The surface $ϕ2=0$ and $ϕ1<0$ is invariant, since the only minimizing control when $ϕ1<0$ is rotation in place. This is another example of a trivial trajectory in which the agent starts at the destination.

Now that we have enumerated the types of extremal trajectory segments, the task is to show which combination of extremal segments make up the minimum-time trajectory to a given destination point. We examine the possible terminal conditions and integrate backward in time to find switching conditions compatible with the terminal constraints. In Sec. 5, we show how to reach any point in the plane by one of these “nominal trajectories.”

Theorem 1. Nontrivial minimum-time trajectories must end in either a fast turn or forward motion segment.

Proof. The terminal condition states that $ϕ2(t1)=0$ at the final time t₁. If we disregard the trivial trajectories discussed in Sec. 3.3 for $ϕ1≤0$⁠, this implies $ϕ1(t1)>0$ as an additional terminal condition. Noting that $ϕ3=ω¯|ϕ2|−v¯ϕ1$⁠, we see that $ϕ3(t1)<0$⁠. There are two control types consistent with $ϕ1>0$ and $ϕ3<0$⁠: fast turn (in either direction) and forward motion. Under forward motion, $ϕ2=0=$ constant, thus satisfying the terminal conditions. So, a minimum-time trajectory may end in a forward motion segment.

Under fast turning motion, $ϕ2≠0$⁠, but the state can reach the terminal conditions from a fast turn segment as follows. Taking the derivative of $ϕ2$ with respect to time and substituting for fast turn control inputs, we find $ϕ˙2(t)=−v¯ cos(θ(t)−γ)$⁠, which can be positive or negative depending on the value of the parameter γ. Thus, with an appropriate value of γ, a fast turn trajectory can reach the switching surface $ϕ2(t1)=0$ at the final time t₁, implying that a minimum-time trajectory may end in a fast turn segment. $◻$

For a trajectory to end with a forward motion segment, it must have $η(t1)=0$ and $cos β(t1)=1$ at the terminal time t₁. These conditions will hold for the duration of the forward motion segment, no matter how long it lasts. From Sec. 3.2.1, we know that a forward motion segment can only be preceded by a fast turn. Suppose that at some time, the control switches from fast turn to forward motion. To compute the maximum duration of a fast turn leading to forward motion, we integrate backward in time to find the switching times corresponding to $ϕ3=0$ (see Fig. 2, left).

Continuing further back in time, we have a rotation segment. Figure 2 on the left shows the state of the agent relative to the η = 0 line at the times of control switching for the backward-in-time trajectory described earlier.

From Bellman's principle of optimality, we know that subsets of a trajectory at different starting points (but sharing an endpoint) will also be optimal trajectories themselves. So, these switching intervals $τ¯f$ and $τ¯s$ allow us to define the family of all trajectories that end with a forward motion segment of nonzero length.

This family of trajectories consists of all trajectories of the following types (for both left and right turns):

F: Forward motion only;

T_fF: Fast turn of up to $τ¯f$ duration followed by some forward motion;

$TsTfF$⁠: Slow turn of up to $τ¯s$ duration, followed by fast turn of $τ¯f$ duration, followed by some forward motion;

$RTsTfF$⁠: Rotation, followed by slow and fast turns of duration $τ¯s$ and $τ¯f$⁠, respectively, followed by some forward motion.

Increasing the duration of the initial rotation segment will cause the endpoint to rotate about the origin, and a duration of $τ¯r$ as defined earlier brings the destination to the negative x-axis. By symmetry, a right-turning trajectory with the same segment durations would bring the agent to that same point in the same amount of time. From this, we can determine that a left-turning trajectory with rotation longer than $τ¯r$ would put the destination at a point that can be reached in less time with a right-turning trajectory. Thus, a minimum-time trajectory of type $RTsTfF$ cannot include a rotation segment of duration greater than $τ¯r$⁠. Trajectories with rotation segments of duration exactly $τ¯r$ correspond to destinations lying directly behind the initial position of the agent. $◻$

Trajectories with $η(t1)=0$ and $cosβ(t1)≠1$ end in a fast turn. We can again integrate backward in time to find families of optimal trajectories, but in this case the switching angles will be a function of the terminal value of $β(t1)$⁠. The state of the agent relative to the η = 0 line at the times of control switching for this backward-in-time trajectory is illustrated in Fig. 2, right.

And again, there can be a rotation segment prior to full-length slow turn and fast turn segments for a given value of β₁.

This family of trajectories comprises all trajectories of the following types (for both left and right turns):

T_f: Fast turn of up to $τ¯f$ duration only;

$TsTf$⁠: Slow turn of up to $τ¯s(β1)$ duration, followed by fast turn of $τ¯f(β1)$ duration, for some $β1∈[0,π/2]$⁠;

$RTsTf$⁠: Rotation, followed by slow and fast turns of duration $τ¯s(β1)$ and $τ¯f(β1)$⁠, respectively, for some $β1∈[0,π/2]$⁠.

Proof. Follows similar to the proof of Theorem 2. $◻$

By Theorems 2 and 3, we have determined seven minimum-time trajectory types: F, T_fF, $TsTfF, RTsTfF$⁠, T_f, $TsTf$⁠, and $RTsTf$⁠, which we illustrate in Table 1. Next, we show that these seven trajectory types cover the space of destinations. We show how to determine the optimal trajectory type and switching times given a destination point.

Here, we present the explicit form of the minimum-time trajectories. The seven types of trajectory (in each turning direction) from Theorems 2 and 3 correspond to different possible combinations of rotation, slow turn, fast turn, and forward trajectory segments (Table 1). We first describe the partition of the plane into regions for the different trajectory types. We then present the explicit form of the optimal trajectory for each type individually.

Note that the time to reach the destination is given by the sum of the four segment durations: $t1−t0=τr+τs+τf+τd.$

For convenience, we define the headings at switching times as $θrs=θ(trs)$ and so on.

We can write the agent's position at a given time as the sum of vectors for each segment. Define the “turning vectors” $Ts±(t)$ and $Tf±(t)$ as the translation due to slow turn and fast turn segments of duration t that start from the origin $q(0)=(0,0,0)T$⁠, as defined in Eqs. (5) and (6). Superscripts + and – denote left and right turns, respectively. Also, define the “forward motion vector” F(t) as $F(t)=(v¯t,0)T.$

To calculate switching times for an open-loop optimal trajectory, we must first determine which trajectory type can be used to reach the destination. For a given set of initial conditions $q(t0)=(x0,y0,θ0)T$⁠, the plane can be partitioned into regions according to which trajectory type can be used to reach destination points in a given region. The set of trajectory types in Table 1 together covers the plane for all possible destinations.

An example of the trajectory type partition is shown in Fig. 3 for initial condition $q(t0)=(0,0,0)T$⁠. Here, the x-axis separates left turning from right turning trajectories. The positive x-axis is itself a trajectory-type region corresponding to the forward-only trajectory type. The negative x-axis also separates left turning from right turning trajectories, but in this case destinations lying there can be reached in equal time from either left or right turning trajectories with the same segment durations.

Here, we derive the open-loop optimal control segment durations τ_r, τ_s, τ_f, and τ_d for trajectories in each of the regions defined earlier. For all, we assume that the agent starts at the origin $q(t0)=(0,0,0)T$⁠, and the destination lies in the upper half-plane so that left turning controls (⁠$ω≥0$⁠) are used.

“Compound” trajectories are those that feature more than one control segment. For each compound trajectory type, there are two unknown values to solve for, as indicated in Table 1. Compound trajectories ending in a forward motion segment have unknown durations for the initial and final segments. Compound trajectories ending in a fast turn segment have the initial segment duration and the parameter β₁ as unknowns.

The general strategy to solve for the control segment durations is to first write the equation for the destination in terms of the segment durations as in Eq. (10). The equation is rearranged such that the initial segment duration appears only in a rotation matrix premultiplying one of the sides. Taking the two-norm thus removes the initial unknown angle, allowing us to solve for the duration of the final segment. We then use substitution to solve for the other unknown.

For convenience, we parameterize segment durations according to the change in heading or distance traveled, letting $θr=ω¯τr, θs=ω¯τs, θf=(μ/v¯)τf$⁠, and $d=v¯τd$⁠.