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.

## Introduction

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)\u2208\mathbb{R}2$ with an associated heading angle $\theta \u2208S$. 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 $\omega =\theta \u02d9$. 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\xaf=$constant), and the Reeds-Shepp vehicle, which also allows for reverse motion ($v=\xb1v\xaf$). 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.

## Problem Statement and System Dynamics

The control inputs $u(t)=(v(t),\omega (t))T$ at time *t* consist of the forward speed $v\u2208\mathbb{R}$ and the turning rate $\omega \u2208\mathbb{R}$. We impose the following constraints on the control input:

- (1)
*Limited speed*: Let $v\xaf>0$ be the maximum speed. The speed control must satisfy $v(t)\u2264v\xaf$ for all time t. - (2)
*No reverse motion*: Speed must satisfy $v(t)\u22650$ for all time t, such that the agent never moves in reverse. - (3)
*Limited turning rate*: Let $\omega \xaf>0$ be the maximum turning rate. Then, the turning control must satisfy $|\omega (t)|\u2264\omega \xaf$ for all time t. - (4)
*Limited lateral acceleration*: Let $\mu >0$ represent the maximum lateral acceleration (turning traction limit). The inputs*v*(*t*) and $\omega (t)$ must satisfy $|v(t)\u2009\omega (t)|\u2264\mu $ for all time t. We assume that $\mu <v\xaf\u2009\omega \xaf$ so that the lateral acceleration constraint is active on part of the boundary of the control domain.

We define the admissible control region $\Omega ={u=(v,\omega )\u2208\mathbb{R}2\u2009|\u20090\u2264v\u2264v\xaf,\u2009|\omega |\u2264\omega \xaf,\u2009|v\omega |\u2264\mu <v\xaf\omega \xaf}$ as illustrated in Fig. 1. Admissible controls $U$ for the agent are bounded Lebesgue measurable functions from $\mathbb{R}+$ 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. [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,\theta )T$, $f(q,u)=(v\u2009cos\u2009\theta ,v\u2009sin\u2009\theta ,\omega )T$, and $\psi (t)=(x(t)\u2212x1)2+(y(t)\u2212y1)2$, with initial conditions $q(0)=(x0,y0,\theta 0)T$.

where the adjoint vector $\lambda =(\lambda x,\lambda y,\lambda \theta )\u2208\mathbb{R}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.

*t*:

*λ*and

_{x}*λ*are constant over time. Thus, as noted in Ref. [9], since $\lambda \u02d9\theta =\lambda xy\u02d9\u2212\lambda yx\u02d9,$$\lambda \theta $ can be directly integrated as $\lambda \theta =\lambda xy\u2212\lambda yx\u2212\rho ,$ for some constant of integration

_{y}*ρ*. Since the terminal heading

*θ*is free, we must impose that $\lambda \theta (t1)=0$. This allows us to solve for

_{f}*ρ*: $\lambda \theta (t1)=\lambda xy1\u2212\lambda yx1\u2212\rho =0$. Thus

*λ*and

_{x}*λ*cannot both be zero. Without loss of generality, let $\lambda x=\lambda 0\u2009sin\u2009\gamma $ and $\lambda y=\u2212\lambda 0\u2009cos\u2009\gamma $ for some unknown angle

_{y}*γ*, with the constant $\lambda 0\u22600$ computed from the terminal condition

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.

### 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. [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,\theta 0)T$. In each case, $\theta (t)=\theta 0+\omega *t$.

- (1)
*Rotation:*When $\varphi 1<0$, the agent rotates in place at maximum turning rate: $v*=0$ and $\omega *=\omega \xafsgn(\varphi 2)$. Here, $q(t)=(x0,y0,\theta 0+\omega *t)T$. - (2)
*Slow turn:*When $\varphi 1>0$ and $\varphi 3>0$, the agent moves forward with low speed while turning at the maximum rate: $v*=\mu /\omega \xaf$ and $\omega *=\omega \xafsgn(\varphi 2)$. The agent moves on a circular arc with radius $Rs=\mu /\omega \xaf2$, withwhere $B(\theta )$ is the standard rotation matrix$(x(t)y(t))=(x0y0)+B(\theta 0)(Rs\u2009sin(\omega \xaft)sgn(\varphi 2)Rs(1\u2212cos(\omega \xaft)))$(5)$B(\theta )=(\u2009cos\u2009\theta \u2212sin\u2009\theta \u2009sin\u2009\theta \u2009cos\u2009\theta )$ - (3)
*Fast turn:*When $\varphi 1>0$ and $\varphi 3<0$, the agent moves forward at maximum speed while turning at a lower rate: $v*=v\xaf$ and $\omega *=sgn(\varphi 2)\mu /v\xaf$. The agent moves on a circular arc with radius $Rf=v\xaf2/\mu $, with$(x(t)y(t))=(x0y0)+B(\theta 0)(Rf\u2009sin(\mu t/v\xaf)sgn(\varphi 2)Rf(1\u2212cos(\mu t/v\xaf)))$(6)

### Singular Control Inputs.

At times where some $\varphi 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.

#### Forward Motion.

When $\varphi 2=0$ with $\varphi 1>0$ at some time $t\u2032$, 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\u2009\beta >0$, and the Hamiltonian is minimized by $v=v\xaf$ with *ω* taking any value in $[\u2212\mu /v\xaf,\mu /v\xaf]$. For $cos\u2009\beta \u22601$, 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\u2009\beta =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\u2032$.

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 $\varphi 3=0$ and $\varphi 1>0$ at some time $t\u2032$, the agent is on the switching surface between fast turn and slow turn with direction determined by the sign of $\varphi 2$. On this switching surface, the Hamiltonian is minimized by two possible values of the control input: a fast turn $u=(v\xaf,\xb1\mu /v\xaf)T$ or a slow turn $u=(\mu /\omega \xaf,\xb1\omega \xaf)T$ (positive for left turn).

#### Rotation and Slow Turn.

*v*freely chosen as any value in the range $[0,\mu /\omega \xaf]$. Taking the derivative of $\varphi 1$ with respect to time

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 $\varphi 1=0$ switching surface.

### Multiply Singular Control and Trivial Trajectories.

We consider the situation that the state lies on multiple switching surfaces simultaneously. If both $\varphi 1$ and $\varphi 2$ (and subsequently $\varphi 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 $\varphi 2=0$ with $\varphi 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 $\varphi 2=0$ and $\varphi 1<0$ is invariant, since the only minimizing control when $\varphi 1<0$ 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 $\varphi 2(t1)=0$ at the final time *t*_{1}. If we disregard the trivial trajectories discussed in Sec. 3.3 for $\varphi 1\u22640$, this implies $\varphi 1(t1)>0$ as an additional terminal condition. Noting that $\varphi 3=\omega \xaf|\varphi 2|\u2212v\xaf\varphi 1$, we see that $\varphi 3(t1)<0$. There are two control types consistent with $\varphi 1>0$ and $\varphi 3<0$: fast turn (in either direction) and forward motion. Under forward motion, $\varphi 2=0=$ constant, thus satisfying the terminal conditions. So, a minimum-time trajectory may end in a forward motion segment.

Under fast turning motion, $\varphi 2\u22600$, but the state can reach the terminal conditions from a fast turn segment as follows. Taking the derivative of $\varphi 2$ with respect to time and substituting for fast turn control inputs, we find $\varphi \u02d92(t)=\u2212v\xaf\u2009cos(\theta (t)\u2212\gamma )$, 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 $\varphi 2(t1)=0$ at the final time *t*_{1}, implying that a minimum-time trajectory may end in a fast turn segment. $\u25fb$

### Trajectories Ending in Forward Motion.

For a trajectory to end with a forward motion segment, it must have $\eta (t1)=0$ and $cos\u2009\beta (t1)=1$ at the terminal time *t*_{1}. 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 $\varphi 3=0$ (see Fig. 2, left).

*η*= 0 and $cos\u2009\beta =1$, we find that $\varphi 3=0$ for a fast turn of duration $\tau \xaff=\theta \xaffv\xaf/\mu $, where

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 $\tau \xaff$ and $\tau \xafs$ 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 _{f}F*: Fast turn of up to $\tau \xaff$ duration followed by some forward motion;

$TsTfF$: Slow turn of up to $\tau \xafs$ duration, followed by fast turn of $\tau \xaff$ duration, followed by some forward motion;

$RTsTfF$: Rotation, followed by slow and fast turns of duration $\tau \xafs$ and $\tau \xaff$, respectively, followed by some forward motion.

*The maximum rotation duration for a minimum-time*$RTsTfF$

*trajectory with forward motion duration of τ*$\tau \xafr=\theta \xafr/\omega \xaf$

_{d}is given by*, where*

*Proof.*Consider a left-turning trajectory of type $RTsTfF$ with a rotation duration of $\tau r=0$ and forward motion duration

*τ*. Starting from the origin $q(0)=(0,0,0)T$, we use Eqs. (5) and (6) to calculate the endpoint of the trajectory at time $t1=\tau \xafs+\tau \xaff+\tau d$ as

_{d}Increasing the duration of the initial rotation segment will cause the endpoint to rotate about the origin, and a duration of $\tau \xafr$ 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 $\tau \xafr$ 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 $\tau \xafr$. Trajectories with rotation segments of duration exactly $\tau \xafr$ correspond to destinations lying directly behind the initial position of the agent. $\u25fb$

### Trajectories Ending in Fast Turn.

Trajectories with $\eta (t1)=0$ and $cos\beta (t1)\u22601$ 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 $\beta (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.

*η*= 0 and $cos\u2009\beta 1\u22601$, we find that the maximum duration of a fast turn segment (corresponding to $\varphi 3=0$ at the start) is $\tau \xaff(\beta 1)=\theta \xaff(\beta 1)v\xaf/\mu $ with

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):

*T _{f}*: Fast turn of up to $\tau \xaff$ duration only;

$TsTf$: Slow turn of up to $\tau \xafs(\beta 1)$ duration, followed by fast turn of $\tau \xaff(\beta 1)$ duration, for some $\beta 1\u2208[0,\pi /2]$;

$RTsTf$: Rotation, followed by slow and fast turns of duration $\tau \xafs(\beta 1)$ and $\tau \xaff(\beta 1)$, respectively, for some $\beta 1\u2208[0,\pi /2]$.

*The maximum rotation duration for a time-optimal*$RTsTf$

*trajectory with*$\beta (t1)=\beta 1$

*is given by*$\tau \xafr=\theta \xafr/\omega \xaf$

*, where*

*with*$\theta \xaff=\theta \xaff(\beta 1)$

*and*$\theta \xafs=\theta \xafs(\beta 1)$

*as defined in*

*Eqs. (8) and (9*

*).*

*Proof.* Follows similar to the proof of Theorem 2. $\u25fb$

By Theorems 2 and 3, we have determined seven minimum-time trajectory types: *F*, *T _{f}F*, $TsTfF,\u2009RTsTfF$,

*T*, $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.

_{f}Type | τ_{r} | τ_{s} | τ_{f} | τ_{d} |
---|---|---|---|---|

F | 0 | 0 | 0 | $[0,\u221e]$ |

T_{f}F | 0 | 0 | $[0,\tau \xaff]$ | $[0,\u221e]$ |

$TsTfF$ | 0 | $[0,\tau \xafs]$ | $\tau \xaff$ | $[0,\u221e]$ |

$RTsTfF$ | $[0,\tau \xafr(\tau d)]$ | $\tau \xafs$ | $\tau \xaff$ | $[0,\u221e]$ |

T_{f} | 0 | 0 | $[0,\tau \xaff]$ | 0 |

$TsTf$ | 0 | $[0,\tau \xafs(\beta 1)]$ | $\tau \xaff(\beta 1)$ | 0 |

$RTsTf$ | $[0,\tau \xafr(\beta 1)]$ | $\tau \xafs(\beta 1)$ | $\tau \xaff(\beta 1)$ | 0 |

Type | τ_{r} | τ_{s} | τ_{f} | τ_{d} |
---|---|---|---|---|

F | 0 | 0 | 0 | $[0,\u221e]$ |

T_{f}F | 0 | 0 | $[0,\tau \xaff]$ | $[0,\u221e]$ |

$TsTfF$ | 0 | $[0,\tau \xafs]$ | $\tau \xaff$ | $[0,\u221e]$ |

$RTsTfF$ | $[0,\tau \xafr(\tau d)]$ | $\tau \xafs$ | $\tau \xaff$ | $[0,\u221e]$ |

T_{f} | 0 | 0 | $[0,\tau \xaff]$ | 0 |

$TsTf$ | 0 | $[0,\tau \xafs(\beta 1)]$ | $\tau \xaff(\beta 1)$ | 0 |

$RTsTf$ | $[0,\tau \xafr(\beta 1)]$ | $\tau \xafs(\beta 1)$ | $\tau \xaff(\beta 1)$ | 0 |

Note: For the entries containing *β*_{1}, the range is $\beta 1\u2208[0,\pi /2]$.

## 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.

*θ*

_{0}at time

*t*

_{0}and a set of time durations spent in each control segment, we can write the trajectory explicitly as a sequence of translations and rotations about different points. Let $\tau r,\tau s,\tau f,$ and

*τ*represent the time spent in rotation, slow turn, fast turn, and forward segments, respectively. The switching times and the final time are denoted

_{d}Note that the time to reach the destination is given by the sum of the four segment durations: $t1\u2212t0=\tau r+\tau s+\tau f+\tau d.$

*t*, for initial heading $\theta (0)=\theta 0$, is given by the following expression, with + for left turn, – for right turn:

For convenience, we define the headings at switching times as $\theta rs=\theta (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\xb1(t)$ and $Tf\xb1(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\xaft,0)T.$

*t*is given by the following expression (with $+/\u2212$ on

*θ*and

*T*omitted):

### Trajectory-Type Partition.

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,\theta 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.

Boundary | τ_{r} | τ_{s} | τ_{f} | τ_{d} |
---|---|---|---|---|

$RTsTfF$–$RTsTf$ | $[0,\tau \xafr(0)]$ | $\tau \xafs$ | $\tau \xaff$ | 0 |

$TsTfF$–$TsTf$ | 0 | $[0,\tau \xafs]$ | $\tau \xaff$ | 0 |

$TsTf$–T_{f}F | 0 | 0 | $[0,\tau \xaff]$ | 0 |

$RTsTfF$–$TsTfF$ | 0 | $\tau \xafs$ | $\tau \xaff$ | $[0,\u221e]$ |

$TsTfF$–T_{f}F | 0 | 0 | $\tau \xaff$ | $[0,\u221e]$ |

$RTsTf$–$TsTf$ | 0 | $\tau \xafs(\beta 1)$ | $\tau \xaff(\beta 1)$ | 0 |

Left T– right _{f}FT_{f}F | 0 | 0 | 0 | $[0,\u221e]$ |

Left $RTsTf$– right $RTsTf$ | $\tau \xafr(\beta 1)$ | $\tau \xafs(\beta 1)$ | $\tau \xaff(\beta 1)$ | 0 |

Left $RTsTfF$– right $RTsTfF$ | $\tau \xafr(\tau d)$ | $\tau \xafs$ | $\tau \xaff$ | $[0,\u221e]$ |

Boundary | τ_{r} | τ_{s} | τ_{f} | τ_{d} |
---|---|---|---|---|

$RTsTfF$–$RTsTf$ | $[0,\tau \xafr(0)]$ | $\tau \xafs$ | $\tau \xaff$ | 0 |

$TsTfF$–$TsTf$ | 0 | $[0,\tau \xafs]$ | $\tau \xaff$ | 0 |

$TsTf$–T_{f}F | 0 | 0 | $[0,\tau \xaff]$ | 0 |

$RTsTfF$–$TsTfF$ | 0 | $\tau \xafs$ | $\tau \xaff$ | $[0,\u221e]$ |

$TsTfF$–T_{f}F | 0 | 0 | $\tau \xaff$ | $[0,\u221e]$ |

$RTsTf$–$TsTf$ | 0 | $\tau \xafs(\beta 1)$ | $\tau \xaff(\beta 1)$ | 0 |

Left T– right _{f}FT_{f}F | 0 | 0 | 0 | $[0,\u221e]$ |

Left $RTsTf$– right $RTsTf$ | $\tau \xafr(\beta 1)$ | $\tau \xafs(\beta 1)$ | $\tau \xaff(\beta 1)$ | 0 |

Left $RTsTfF$– right $RTsTfF$ | $\tau \xafr(\tau d)$ | $\tau \xafs$ | $\tau \xaff$ | $[0,\u221e]$ |

Note: For the entries containing *β*_{1}, the range is $\beta 1\u2208[0,\pi /2]$.

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.

### Optimal Switching Times for Each Compound Trajectory Type.

Here, we derive the open-loop optimal control segment durations *τ _{r}*,

*τ*,

_{s}*τ*, and

_{f}*τ*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 ($\omega \u22650$) are used.

_{d}“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 $\theta r=\omega \xaf\tau r,\u2009\theta s=\omega \xaf\tau s,\u2009\theta f=(\mu /v\xaf)\tau f$, and $d=v\xaf\tau d$.

#### $TsTf$ Trajectory.

*T*can be decomposed into

_{s}*θ*only appearing within a rotation matrix

_{s}*θ*, and we can solve for

_{s}*θ*is the clockwise angle about point $(0,Rs)T$ from $Tf(\theta f)$ to our destination

_{s}where atan2(*y*, *x*) is the standard two-input inverse tangent function with range $[\u2212\pi ,\pi ]$.

#### T_{f}F Trajectory.

#### $TsTfF$ Trajectory.

*θ*into a single rotation matrix

_{s}*d*with

*d*. Then to calculate the angle

*θ*, we can simply subtract the angle of the vector on the right-hand side of Eq. (12) from the angle of the vector on the left-hand side of Eq. (12)

_{s}#### $RTsTfF$ Trajectory.

*g*from Eq. (11). Then

*d*

#### $RTsTf$ Trajectory.

This is the single trajectory type which eludes an explicit analytical solution, although the equations involved are well behaved and simple to solve numerically.

*g*from Eq. (11). Geometrically, $(xc,yc)T$ is the point on the $RTsTf$–$TsTf$ boundary surface which is the same distance from the origin as the destination point. The distance is smoothly and monotonically increasing with

*x*, so it should not pose any difficulties to calculate numerically. Once found, the rotation angle is given by

_{c}and *θ _{s}*,

*θ*are found according to the solution for a $TsTf$ trajectory presented in Sec. 5.3.3, using $(xc,yc)T$ as the destination.

_{f}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

*x*-axis. The coordinate transformation from world to body-fixed is given by

The optimal control consists of the following rules:

- (1)
If destination is on the positive

*x*_{rel}-axis, go forward. - (2)
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.

- (3)
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.

- (4)
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 *x*_{rel}-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 $\mu \u2265v\xaf\omega \xaf$, the lateral acceleration constraint does not affect the boundary of the permissible control space. Here, the controls are limited to the rectangle $v\u2208[0,b\xaf],\u2009\omega \u2208[\u2212\omega \xaf,\omega \xaf]$. In this case, the extremal control is determined through only two switching functions, namely, $\varphi 1$ and $\varphi 2$, from the general system. In effect, the slow turn and fast turn extremals merge into a single turn trajectory with $v=v\xaf,\u2009\omega =\xb1\omega \xaf$, and radius $R=v\xaf/\omega \xaf$.

*F*), turn and forward (

*TF*), rotate, turn, and forward (

*RTF*), turn (

*T*), and rotate and turn (

*RT*). Destinations on the positive x

*-axis can be reached by*

_{rel}*F*trajectories. Destinations on the quarter circle

*TF*and

*RT*trajectory types. The boundary between

*TF*and

*RTF*trajectory types is a vertical line $xrel=R,\u2009yrel\u2265R$. Finally, the boundary between

*RT*and

*RTF*trajectory types is the circular arc

For a more detailed discussion of this system without the acceleration constraint, see Ref. [12].

### 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, $\varphi 2$ and $\varphi 3$, 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 $v\u22650$ and $v/v\xaf+|\omega |/\omega \xaf\u22641$. 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.

## Multi-Agent Coverage

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. [13] 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. [14] 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. [15], coverage is studied using a metric that mixes time-to-reach with a measure of the energy needed to reach a point.

*L*> 0 and width

*W*> 0, $D={(x,y)\u2208\mathbb{R}2\u2223\u2212W/2\u2264x\u2264W/2,\u2212L/2\u2264y\u2264L/2}$. Given a set of

*N*identical agents as defined earlier with states $qi=(xi,yi,\theta i),$ for $i=1,\u2026,N$ and identical speed and turning constraints $v\xaf,\omega \xaf,\mu $, we would like to find the configuration of all agents $Q=(q1,\u2026,qN)$ that minimizes the time necessary for an agent to reach any point in $D$. Let

*V*(

*Q*) be the worst-case time-to-reach for agents with configuration

*Q*

*q*using the minimum-time trajectory defined in Sec. 5.

_{i}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 $Di$ for agent *i* as the set of points within the domain that can be reached by agent *i* before any other agent: $Di={p\u2208D|tmin(qi,p)\u2264tmin(qj,p)\u2200j\u2260i}$. The set of dominance regions comprises a generalized Voronoi partition under the time-to-reach metric. Additionally, we define the *t*-reachable region $R(t,q)$ as the set of points reachable within time *t* for an agent with state *q*: $R(t,q)={p\u2208\mathbb{R}2|tmin(q,p)\u2264t}$. Let $|B|$ refer to the area enclosed in a plane region $B$, such that $|B|=\u222bBdA$. Note that $R(t,q)$ 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 $A(t)=|R(t,q)|$ 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 $t+\u03f5$ for any $\u03f5>0$. So, $R(t,q)\u2286R(t+\u03f5,q)$. Also note that $A(0)=0$, since the only point reachable in zero time is the agent's current position. From the definitions of dominance region $Di$ and worst-case time-to-reach *V*(*Q*), for a set of agents with configuration *Q* in domain $D$, for any point $p\u2208Di,\u2009tmin(qi,p)\u2264V(Q)$. This implies that $Di\u2286R(V(Q),qi),\u2200i=1\u2026N$.

In the following, we prove a lower bound on *V*(*Q*):

Theorem 4. $V(Q)\u2265t*$*, where*$t*$*is the unique solution to*$A(t*)=|D|/N$*for a system with N steered agents with maximum speed*$v\xaf$*, maximum turning rate*$\omega \xaf$*, and maximum lateral acceleration μ operating in a domain of interest*$D$.

*Proof*. We prove the theorem by contradiction. Note that $\u2211i=1N|Di|=|D|$ from the definition of the dominance region partition. By monotonicity of *A*(*t*) and $A(0)=0,\u2009t*>0$ exists and is unique. Suppose $V(Q)=t\u2032<t*$. Then, $Di\u2286R(t\u2032,qi),\u2009\u2200i=1,\u2026,N$. So $|Di|\u2264A(t\u2032),\u2009\u2200i$, and $\u2211i=1N|Di|<N\u2009A(t\u2032)<N\u2009A(t*)$, since *A* is monotonic in *t* and $t*>t\u2032$. From the definition of $t*,\u2009N\u2009A(t*)=|D|$. Thus, $\u2211i=1N|Di|<|D|$, which is a contradiction, proving that $t*$ is a lower bound for *V*(*Q*) in domain $D$. $\u25fb$

### Coverage Algorithm.

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 $D$.

*Algorithm:* Let the initial configuration be $Q(t0)$ and $\Delta t>0$ the timestep. At time $tk=t0+k\Delta t$, the time-to-reach partition is computed based on the configuration $Q(tk)$. Each agent chooses a point in its dominance region with the maximum time-to-reach, $pik=arg\u2009maxp\u2208Ditmin(qi(tk),p),$ and sets that point as its next destination. A new configuration $Q\u2032$ is calculated corresponding to the state of each agent after traveling along the minimum-time trajectory toward its destination for one timestep. If $V(Q\u2032)<V(Q)$, the agents move to the new state $Q(tk+1)=Q\u2032$ and begin a new step. Otherwise, the algorithm stops at configuration $Q(tk)$.

The performance index of the worst-case time-to-reach improves with each step, with $V(Q(tk+1))<V(Q(tk))$. 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.

## Final Remarks

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. [2].

## Funding Data

Directorate for Engineering NSF grant (Grant No. ECCS-1135724).