We present time-optimal trajectories for a steered agent with constraints on speed, lateral acceleration, and turning rate for the problem of reaching a point on the plane in minimum time with free terminal heading angle. Both open-loop and state-feedback forms of optimal controls are derived through application of Pontryagin's minimum principle. We apply our results for the single agent to solve a multi-agent coverage problem in which each agent has 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 . 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 .
A popular model with which to study motion planning is the “steered agent” with state consisting of a point on the plane with an associated heading angle . 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 (constant), and the Reeds-Shepp vehicle, which also allows for reverse motion (). See Ref.  for a review and Ref.  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 .
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  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.  and to extremal trajectories for more general constraints in Ref. . These methods have also been applied to minimum-time trajectories for omni-directional robots  and minimum wheel-rotation for the differential drive .
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.
Problem Statement and System Dynamics
The control inputs at time t consist of the forward speed and the turning rate . We impose the following constraints on the control input:
Limited speed: Let be the maximum speed. The speed control must satisfy for all time t.
No reverse motion: Speed must satisfy for all time t, such that the agent never moves in reverse.
Limited turning rate: Let be the maximum turning rate. Then, the turning control must satisfy for all time t.
Limited lateral acceleration: Let represent the maximum lateral acceleration (turning traction limit). The inputs v(t) and must satisfy for all time t. We assume that so that the lateral acceleration constraint is active on part of the boundary of the control domain.
We define the admissible control region as illustrated in Fig. 1. Admissible controls for the agent are bounded Lebesgue measurable functions from to Ω.
Extremal Trajectories From Pontryagin's Minimum Principle
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. , 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 , , and , with initial conditions .
where the adjoint vector 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 . β 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.
Switching Functions and Generic Control Inputs.
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. , 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 . In each case, .
Rotation: When , the agent rotates in place at maximum turning rate: and . Here, .
- (2)Slow turn: When and , the agent moves forward with low speed while turning at the maximum rate: and . The agent moves on a circular arc with radius , withwhere is the standard rotation matrix(5)
- (3)Fast turn: When and , the agent moves forward at maximum speed while turning at a lower rate: and . The agent moves on a circular arc with radius , with(6)
Singular Control Inputs.
At times where some , 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 with at some time , 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 , and the Hamiltonian is minimized by with ω taking any value in . For , 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 , 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 .
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.
Slow Turn and Fast Turn.
When and at some time , the agent is on the switching surface between fast turn and slow turn with direction determined by the sign of . On this switching surface, the Hamiltonian is minimized by two possible values of the control input: a fast turn or a slow turn (positive for left turn).
Rotation and Slow Turn.
Thus, on the switching surface, the derivative is always nonzero for the minimizing control , so there can be no singular interval for the switching surface.
Multiply Singular Control and Trivial Trajectories.
We consider the situation that the state lies on multiple switching surfaces simultaneously. If both and (and subsequently ) 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 with . Here, the ω term does not appear in H, and the minimizing control has v = 0, leading to the situation that H = 1. The surface and is invariant, since the only minimizing control when is rotation in place. This is another example of a trivial trajectory in which the agent starts at the destination.
Families of Optimal Trajectories
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 at the final time t1. If we disregard the trivial trajectories discussed in Sec. 3.3 for , this implies as an additional terminal condition. Noting that , we see that . There are two control types consistent with and : fast turn (in either direction) and forward motion. Under forward motion, constant, thus satisfying the terminal conditions. So, a minimum-time trajectory may end in a forward motion segment.
Under fast turning motion, , but the state can reach the terminal conditions from a fast turn segment as follows. Taking the derivative of with respect to time and substituting for fast turn control inputs, we find , 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 at the final time t1, implying that a minimum-time trajectory may end in a fast turn segment.
Trajectories Ending in Forward Motion.
For a trajectory to end with a forward motion segment, it must have and at the terminal time t1. 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 (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 and 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;
TfF: Fast turn of up to duration followed by some forward motion;
: Slow turn of up to duration, followed by fast turn of duration, followed by some forward motion;
: Rotation, followed by slow and fast turns of duration and , 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 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 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 cannot include a rotation segment of duration greater than . Trajectories with rotation segments of duration exactly correspond to destinations lying directly behind the initial position of the agent.
Trajectories Ending in Fast Turn.
Trajectories with and 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 . 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 β1.
This family of trajectories comprises all trajectories of the following types (for both left and right turns):
Tf: Fast turn of up to duration only;
: Slow turn of up to duration, followed by fast turn of duration, for some ;
: Rotation, followed by slow and fast turns of duration and , respectively, for some .
Proof. Follows similar to the proof of Theorem 2.
By Theorems 2 and 3, we have determined seven minimum-time trajectory types: F, TfF, , Tf, , and , 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.
The Optimal Trajectory
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.
Trajectory Parameterized by Switching Times.
Note that the time to reach the destination is given by the sum of the four segment durations:
For convenience, we define the headings at switching times as 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” and as the translation due to slow turn and fast turn segments of duration t that start from the origin , as defined in Eqs. (5) and (6). Superscripts + and – denote left and right turns, respectively. Also, define the “forward motion vector” F(t) as
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 , 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 . 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.
Optimal Switching Times for Each Compound Trajectory Type.
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 , and the destination lies in the upper half-plane so that left turning controls () 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 β1 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 , and .
where atan2(y, x) is the standard two-input inverse tangent function with range .
This is the single trajectory type which eludes an explicit analytical solution, although the equations involved are well behaved and simple to solve numerically.
and θs, θf are found according to the solution for a trajectory presented in Sec. 5.3.3, using as the destination.
Figure 4 shows minimum-time trajectories to various destination points under different values of the lateral acceleration limit μ, including examples for different trajectory types.
By calculating the optimal switching times at every destination point on the plane, we can build a map of the time-to-reach under minimum-time control as a function of the destination. In Fig. 5, we illustrate how the minimum time-to-reach is affected by varying the μ parameter of the acceleration constraint. As the value of μ decreases, the time-to-reach increases for all destinations except those reachable by a forward-motion-only trajectory.
State-Feedback Formulation of Optimal Control Law
The optimal control consists of the following rules:
If destination is on the positive xrel-axis, go forward.
Else if destination is in a trajectory-type region with fast turn as the initial segment, go in a fast turn in the appropriate direction.
Else if destination is in a trajectory-type region with slow turn as the initial segment, go in a slow turn in the appropriate direction.
Else, rotate in the appropriate direction.
Figure 6 illustrates the state-feedback control-type regions under different values of the lateral acceleration constraint μ. As μ decreases, the edges separating the fast turn and slow turn control regions approach the positive xrel-axis.
Special Cases for Large and Small Values of μ
Here, we examine two limiting cases in the minimum-time problem. We first consider relaxing the constraint on lateral acceleration. We then look at the limiting case for very low μ, which has parallels to the problem of a forward-only differential drive vehicle. Control-type regions and optimal trajectories for both cases are shown in Fig. 7.
Relaxed Acceleration Constraint.
For , the lateral acceleration constraint does not affect the boundary of the permissible control space. Here, the controls are limited to the rectangle . In this case, the extremal control is determined through only two switching functions, namely, and , from the general system. In effect, the slow turn and fast turn extremals merge into a single turn trajectory with , and radius .
For a more detailed discussion of this system without the acceleration constraint, see Ref. .
Highly Constrained Lateral Acceleration.
For μ = 0, the acceleration constraint is equivalent to constraining either v or ω to be zero at any given time. We can interpret this as the slow turn merging with rotation and the fast turn merging with forward motion. The admissible control space becomes a “T” shape, such that the agent can either rotate or move forward but not both at the same time. Under those constraints, the extremal controls are specified by the signs of two switching functions, and , from the general system. We find that time optimal trajectories consist of rotating in place until facing the destination, then moving forward at full speed.
Interestingly, this control scheme is also optimal for a differential-drive robot constrained to only go forward, with input constraints and . For that system, the switching functions and extremal trajectories are the same. The only difference is that the extreme corners of the control space are connected by a straight line in the differential drive case, rather than a concave curve for the limited acceleration system.
In this section, we apply our solution of the minimum-time problem for a single agent, with constraints on speed, lateral acceleration, and turning rate, to the analysis of a coverage problem in a multi-agent system. Suppose there is a domain on the plane where events can occur at any point, uniformly randomly distributed within the domain. The problem of coverage as applied to multi-agent robotic systems asks how to distribute a collection of N agents within the domain so that some performance metric that describes the ability of the group to respond to events, such as average or worst-case time-to-reach, is optimal.
Coverage problems have been studied extensively for various agent dynamics and performance metrics. Notably, Ref.  shows that optimal arrangements for omni-directional agents under a weighted-average performance metric will take on the form of a centroidal-Voronoi configuration, which can be reached under decentralized control using Lloyd's algorithm. Ref.  considers the worst-case time-to-reach problem for a system of Dubins vehicles. Because Dubins vehicles have constant forward speed, no static configuration can be achieved. So, performance is considered on a time-averaged basis. The authors develop heuristic strategies for the cases of high and low vehicle density and show that performance is within a constant factor of optimal. In Ref. , coverage is studied using a metric that mixes time-to-reach with a measure of the energy needed to reach a point.
We seek to characterize bounds on V(Q) for a given domain and number of agents. To aid in the analysis, we define the dominance region for agent i as the set of points within the domain that can be reached by agent i before any other agent: . The set of dominance regions comprises a generalized Voronoi partition under the time-to-reach metric. Additionally, we define the t-reachable region as the set of points reachable within time t for an agent with state q: . Let refer to the area enclosed in a plane region , such that . Note that translates and rotates with the agent's state q, so that the area of the t-reachable region for an agent is not a function of its state. Let for any q.
Lower Bound on Worst-Case Time-to-Reach.
We observe that A(t) increases monotonically with t: any point reachable up to time t is also reachable up to time for any . So, . Also note that , since the only point reachable in zero time is the agent's current position. From the definitions of dominance region and worst-case time-to-reach V(Q), for a set of agents with configuration Q in domain , for any point . This implies that .
In the following, we prove a lower bound on V(Q):
Theorem 4. , whereis the unique solution tofor a system with N steered agents with maximum speed, maximum turning rate, and maximum lateral acceleration μ operating in a domain of interest.
Proof. We prove the theorem by contradiction. Note that from the definition of the dominance region partition. By monotonicity of A(t) and exists and is unique. Suppose . Then, . So , and , since A is monotonic in t and . From the definition of . Thus, , which is a contradiction, proving that is a lower bound for V(Q) in domain .
We now propose an algorithm for coverage in the multi-agent system such that each agent aims to decrease the maximum time-to-reach within its own dominance region at each time-step, and we compare numerical results with the theoretical lower bound found in Theorem 4. We consider a system with N agents moving within rectangular domain .
Algorithm: Let the initial configuration be and the timestep. At time , the time-to-reach partition is computed based on the configuration . Each agent chooses a point in its dominance region with the maximum time-to-reach, and sets that point as its next destination. A new configuration is calculated corresponding to the state of each agent after traveling along the minimum-time trajectory toward its destination for one timestep. If , the agents move to the new state and begin a new step. Otherwise, the algorithm stops at configuration .
The performance index of the worst-case time-to-reach improves with each step, with . Since V(Q) has a lower bound from Theorem 4 and is monotonically decreasing at each step, the algorithm is guaranteed to converge, although there is no guarantee that the final configuration is a global minimum.
In practice, the algorithm performs quite well, converging quickly to a solution that is close to the lower bound. Figure 8 illustrates a typical solution, which converges to a V(Q) that is only about 50% higher than the lower bound. As can be seen in the right panel of Fig. 8, the speed, turning rate, and lateral acceleration constraints have a strong influence on the domains of dominance and therefore the coverage dynamics.
We note that the coverage algorithm and lower bound on coverage performance presented earlier can be easily generalized for application to systems with different types of agent dynamics and input constraints, as long as the time-to-reach metric is well defined. Comparisons in coverage performance for different dynamics and constraints may yield new insights.
We have derived optimal control laws for an agent with constraints on speed, lateral acceleration, and turning rate in the problem of reaching a destination point in minimum time with free terminal heading. The optimal control laws were presented in both open-loop and feedback control formulations, with analytic expressions for the optimal trajectories.
These control laws and the related time-to-reach surfaces can be used as a building block for problems involving multiple agents. We apply our results to a coverage problem where the goal is to distribute a group of agents over a rectangular region such that the worst-case time-to-reach for a point in the domain is minimized. Each agent is assumed to have constraints on speed, lateral acceleration, and turning rate. We prove a theoretical lower bound on performance and develop an iterative algorithm based on a generalized Voronoi partition with time-to-reach as a metric.
The minimum time problem with free terminal heading is also closely related to the two-player differential game of pursuit and evasion. The evader aims to avoid capture for as long as possible, which is achieved in some cases by using a minimum-time trajectory to reach a point in the space with a lower time-to-reach for the evader than the pursuer. We explore the problem of a single pursuer facing multiple evasive agents without turning constraints in Ref. .
Directorate for Engineering NSF grant (Grant No. ECCS-1135724).