Optimal acceleration thresholds for non-holonomic agents

Abstract

Finding optimal trajectories for non-accelerating, non-holonomic agents is a well-understood problem. However, in video games, robotics, and crowd simulations non-holonomic agents start and stop frequently. With the vision of improving crowd simulation, we find optimal paths for virtual agents accelerating from a standstill. These paths are designed for the “ideal”, initial stage of planning when obstacles are ignored. We analytically derive paths and arrival times using arbitrary acceleration angle thresholds. We use these paths and arrival times to find an agent’s optimal ideal path. We then numerically calculate the decision surface that can be used by an application at run-time to quickly choose the optimal path. Finally, we use quantitative error analysis to validate the accuracy of our approach.

1 Introduction

The fields of computer games, robotics, and simulation often use point-based, virtual agents to represent real entities. Frequently, these agents are non-holonomic, meaning they accelerate in the direction they are facing (see [2]). For non-holonomic agents that cannot accelerate, it has been shown that optimal trajectories are composed of lines and circular arcs [5].

However, it is non-trivial to find optimal paths for accelerating non-holonomic agents. Consider the superimposed paths in Fig. 1. All agents started at the origin looking right. The red agent turns in place until it sees its destination before accelerating. The green agent turns until there are $\pi /2$ radians between its heading and the destination before accelerating. The blue agent accelerates without turning in place. For this particular destination, $(-8,0)$, the green agent arrives first, the blue agent arrives second, and the red agent arrives last. However, there exist destinations where the red agent would be the fastest and others where the blue agent would be the fastest. As we demonstrate later, there is no trivial method for choosing the shortest path.

Fig. 1

Superimposed paths of agents using different acceleration angle thresholds. Agents started at the origin (right) looking right. A threshold of $\pi /2$ (green) reaches the destination first, followed by $\pi$ (blue), followed by $0$ (red, who reaches the destination 1.33 s after the first agent). An agent with a threshold of $\sim$2 radians is optimal here for this destination

In order to improve non-holonomic agent movement, we:

• 1. We derive equations for arrival time and paths for agents with arbitrary destinations. We do this for non-holonomic agents starting from a standstill.

• 2. We present an off-line method for numerically calculating a decision surface that finds the optimal path to any destination.

• 3. We find a close approximation to this surface that can be used at run time to find the optimal path for non-holonomic agents.

We provide rigorous validation for these contributions. Combined, we believe this will lead to meaningful reductions in agent travel time and computation time for games, robotics, and crowd simulations.

2 Previous work

The most relevant previous work in terms of our results can be divided into three main parts: modeling human motion, optimal trajectories, and collision avoidance.

2.1 Reproducing human trajectories

One branch of virtual agent research finds patterns in human movement and reproduces them in virtual agents. The hope is that believable agent motion can be created by reproducing these patterns. This research includes studies on how we move our eyes [7], hands [7, 10], and arms [15]. Often it is assumed that humans optimize their movement over some function [1], including minimizing variance [7] or maximize smoothness [15]. Researchers do the same analysis on human trajectories. Arechavaleta et al. [2] asserted that the movement of humans can be modeled non-holonomically while minimizing the $L_2$ norm. Hicheur et al. [9] argued that locomotion planning is done at a higher level than footstep planning. Thus, they assert that people plan trajectories first and footfalls second.

This research has an appropriate place in virtual agent movement, but should not be the sole foundation for choosing paths for agents. Unlike the human subjects in these studies, virtual agents often start and stop frequently. Also, non-holonomic agents are also used to model non-human entities. In such cases, a more generic model is required.

2.2 Optimal trajectories

Recognizing the importance of improved trajectories for non-holonomic agents, researchers have studied path planning from a mathematical perspective. One of the key themes in this area is optimizing with respect to an agent’s arrival time. Researchers have looked at fastest paths for robots moving at constant speeds. These are called Dubins paths after the work of Dubin [5]. Cockayne and Hall [4] found the set of all reachable points using Dubins paths in a fixed amount of time. Soueres and Laumond [13] have studied how to optimally link any two configurations. However, similar to previous work, all agents travel at constant speeds.

Our work deviates from Dubins paths we do not assume that agents have a constant velocity. On the contrary, we are interested in the more complex scenario where agents can accelerate from a standstill. Thus, we seek a more expressive set of proofs and algorithms for choosing paths for non-holonomic agents.

2.3 Collision avoidance

A common use of non-holonomic agents is in crowd simulation and it is related to our current and future work.

Collision avoidance has roots in Reynolds’ [12] instantaneous forces collision avoidance methods. Helbing and Molnár [8] proposed a 2D method called social forces. Fiorini and Shiller [6] introduced velocity obstacles and created very believable crowds with less stalling than when using social forces. This was further extended by van den Berg et al. [3]. Moving from velocity-based collision avoidance to accelerations-based collision avoidance, Ondrej et al. [11] proposed a synthetic-vision crowd simulation algorithm. They also assert that an agent’s movement can be judged on their arrival time to destinations. For a survey of this area see [14].

Most crowd simulation algorithms avoid collisions by finding a collision-free velocity for each agent. When there are multiple options, algorithms choose a collision-free velocity that is closest to the agent’s ideal path to its destination. This ideal path is usually the straightest line from the agent’s current location to the destination without regard to obstacles. Crowd simulation is highly relevant to this paper since we show that a straight line from the agent to the destination is not always the optimal path for agents. Also, since our work has direct application to crowd simulation, we do calculations in a similar ideal, obstacle-free environment.

3 Problem definition

A non-holonomic agent’s motion can be defined using the equation: $\dot{x}=v\cos \theta , \dot{y}=v\sin \theta , \dot{v}=a$. Without loss of generality, we start the agent at the origin, i.e., $x_0=y_0=0$ and $\theta _0=0$. We also assume that $v_0=0$.

Using these values, the initial configuration of any agent can be uniquely defined by a 5-tuple: $\langle D_x,D_y,r_{m},r^{\prime }_{m},\theta _{m}^{\prime }\rangle$ where $D_x$ and $D_y$ are values of the destination relative to the agent. (Equally, we use polar coordinates $D_r$ and $D_\theta$ to denote the destination.) Further, $r_{m}$ is the agent’s maximum velocity, $r^{\prime }_{m}$ is the agent’s maximum acceleration, and $\theta _{m}^{\prime }$ is the agent’s rate of rotation. Without loss of generality, we assume that $D_y$ is positive, resulting in a counter-clockwise turn by the agent to reach its destination.

When an agent plans its path, it often starts by guessing the shortest path without respect to dynamic obstacles (consider velocity obstacles or social forces). Our work focuses on this first part of agent path planning in an ideal, obstacle-free environment. Specifically, we want to take out the guessing and put in provably best choice.

The path of an agent to its destination can be thought of as having three distinct steps. In the first step, the agent turns in place at rate $\theta _{m}^{\prime }$ without any linear velocity or acceleration. We call this the turn in place step. Next, the agent accelerates linearly while turning. If the agent reaches its maximum linear velocity $r_{m}$ during this phase, it continues to turn while moving forward at this velocity. We call this the turning with velocity step. Lastly, when the agent is looking directly at its destination, it stops turning and continues to accelerate (or if it has already reached $r_{m}$, it continues at that speed), until it reaches its destination. We call this the straight motion step.

It is simple for an algorithm to transition an agent from the turning with velocity step to the straight motion step: the agent needs only stop turning when it faces the destination.

The moment at which an agent transitions between the turning in place and turning with velocity steps is chosen arbitrarily by the underlying motion algorithm. We call the angle between the agent’s current heading and the agent’s destination when it transitions between the first two steps the acceleration angle threshold.

As an example, consider an agent whose destination is at $(D_x,D_y) = (-8,0)$ (see the green/thicker path in Fig. 1). This agent’s acceleration angle threshold is $\pi /2$. Thus, the agent will turn in place without any linear velocity or acceleration until the difference between its heading and the destination is $\pi /2$. This happens when it is looking down the positive $y$-axis. As the agent accelerates and turns, it will move in an arc toward the destination. When the agent is looking directly at $(-8,0)$, the agent will stop turning. It will then proceed with the straight motion step until it reaches its destination.

The goal of this work is to answer the following question: Given a non-holonomic agent, how far should the agent turn in place to reach its destination in the least amount of time? To help answer this question, we introduce the function $ArrivalT$ that gives the arrival time of an agent based on its choice of acceleration angle threshold. Formally $ArrivalT:at\in [0,\pi ],D_x,D_y,r_{m},r^{\prime }_{m},\theta _{m}^{\prime }\rightarrow t$ where $at$ is the chosen acceleration angle threshold and $t$ is the amount of time it takes an agent to reach its destination using that threshold. As we discuss later, not all angle thresholds will reach all destinations, in which case $ArrivalT$ is $t=+\infty$. We derive $ArrivalT$ in Sects. 4 and 5.

We use $ArrivalT$ to find the $at$ that results in the lowest arrival time (and hence the optimal path). To do this, we use function $BestAT$. Formally, $BestAT: D_x,D_y,r_{m},r^{\prime }_{m},\theta _{m}^{\prime }\rightarrow at\ s.t.\ \forall q\in [0,\pi ], ArrivalT(at,D_x,D_y,r_{m},r^{\prime }_{m},\theta _{m}^{\prime })\le ArrivalT(q,D_x,D_y,r_{m},r^{\prime }_{m},\theta _{m}^{\prime })$. We derive a very close approximation to $BestAT$ in Sect. 6.

4 Acceleration angle threshold = $\pi$

In order to derive the equation for $ArrivalT$ for an arbitrary $at$, we begin by solving $ArrivalT$ when $at=\pi$. We show in the next section that we can derive $ArrivalT$ for any $at$ with only slight modification to the following equations.

Setting the acceleration angle threshold to $\pi$ means an agent will never turn in place when heading toward a destination. Instead, since $D_\theta \le \pi$, the agent will skip the turn in place step and immediately proceed to the turning with velocity step. The blue agent in Fig. 1 gives an example of paths generated by $at=\pi$.

4.1 Three areas of the surface

Deriving the equation for $ArrivalT$ for $at=\pi$ depends on whether or not an agent reaches full velocity before transitioning from the turning with velocity step to the straight motion step. We call destinations that an agent will look at directly before reaching full velocity pre-full velocity destinations and destinations that the agent will not see until after reaching full velocity post-full velocity destinations. We begin by discussing how to determine whether a point is a pre- or post-full velocity destination, then how to find the arrival time to each of these types of destinations in turn (see Fig. 2).

Fig. 2

Three surface subdivisions for $at=\pi$ (shown with $\theta _{m}^{\prime }=1,r_{m}=1,r^{\prime }_{m}=1$). The light green and medium green areas show pre- and post-full velocity destinations, respectively. The dark green area in the middle is unreachable by an infinitely small agent. The solid dark green line shows the trajectory of the agent up to time $t=\frac{r_{m}}{r^{\prime }_{m}}$

Until an agent reaches its maximum velocity, its acceleration is constant at $r^{\prime }_{m}$ and it will turn at a constant rate of $\theta _{m}^{\prime }$. Thus, the agent’s velocity at time $t$ is $r^{\prime }_{m}\cdot t$ and its heading is $\theta _{m}^{\prime }\cdot t$. Converting to $x$ and $y$ velocity:

$$\begin{aligned} x'(t)=\cos (\theta _{m}^{\prime }\cdot t)\cdot r^{\prime }_{m}\cdot t,\ \mathrm{and} \ y'(t)=\sin (\theta _{m}^{\prime }\cdot t)\cdot r^{\prime }_{m}\cdot t\nonumber \\ \end{aligned}$$

(1)

Taking the definite integral between $0$ and $t$:

$$\begin{aligned} x(t)&= \frac{\cos (\theta _{m}^{\prime }\cdot t)}{\theta _{m}^{\prime 2}} + \frac{t\cdot \sin (\theta _{m}^{\prime }\cdot t)}{\theta _{m}^{\prime }} - \frac{1}{\theta _{m}^{\prime 2}}\nonumber \\ y(t)&= \frac{\sin (\theta _{m}^{\prime }\cdot t)}{\theta _{m}^{\prime 2}} - \frac{t\cdot \cos (\theta _{m}^{\prime }\cdot t)}{\theta _{m}^{\prime }} \end{aligned}$$

(2)

We are interested in the position of the agent when it reaches its maximum velocity. This happens at time $t=\frac{r_{m}}{r^{\prime }_{m}}$. We can find the location of the agent when it reaches its full velocity by replacing $t$ with $\frac{r_{m}}{r^{\prime }_{m}}$ in Eq. 2. We call this point the full velocity point, the $x$ and $y$ coordinates of which are:

$$\begin{aligned} FVX&= r^{\prime }_{m}\cdot \left( \frac{\cos \left( \frac{r_{m}}{r^{\prime }_{m}}\cdot \theta _{m}^{\prime }\right) }{ (\theta _{m}^{\prime 2})}+\frac{\frac{r_{m}}{r^{\prime }_{m}}\cdot \sin \left( \frac{r_{m}}{r^{\prime }_{m}}\cdot \theta _{m}^{\prime }\right) }{\theta _{m}^{\prime }}\right) -1\nonumber \\ FVY&= r^{\prime }_{m}\cdot \left( \frac{\sin \left( \frac{r_{m}}{r^{\prime }_{m}}\cdot \theta _{m}^{\prime }\right) }{(\theta _{m}^{\prime 2})}-\frac{\frac{r_{m}}{r^{\prime }_{m}}\cdot \cos \left( \frac{r_{m}}{r^{\prime }_{m}}\cdot \theta _{m}^{\prime }\right) }{\theta _{m}^{\prime }}\right) \end{aligned}$$

(3)

In order to determine which destinations are pre- and post-full velocity destinations, we can think of an agent walking from the origin to the full velocity point. Every point the agent looks at directly is a pre-full velocity destination. If we draw a line through the agent at time $\frac{\theta _{m}^{\prime }\cdot r_{m}}{r^{\prime }_{m}}$ parallel to the agent’s heading, it is easy to prove that all points to the right of that line (or returning a negative distance) will be pre-velocity destinations. This is true since all these points are visible to the agent as it moves from the origin to the full velocity point. We thus use this line to segregate pre-full velocity destinations and post-full velocity destinations.

In order to find this segregating line, we need a point and a tangent. We know the full velocity point is on this line and we derived it in Eq. 3. We also know the vector of the tangent of this segregating line, since it is the direction the agent is facing at time $\frac{r_{m}}{r^{\prime }_{m}}$. The non-unit vector version of this tangent can be found by taking the velocity of the agent at time $\frac{r_{m}}{r^{\prime }_{m}}$ using Eq. 1. If we drop the scaling factor in Eq. 1, we have the unit length tangent: $tangent=(\cos (\frac{r_{m}}{r^{\prime }_{m}}\cdot \theta _{m}^{\prime }),\sin (\frac{r_{m}}{r^{\prime }_{m}}\cdot \theta _{m}^{\prime }))$. An orthogonal vector to this is $(OrthX=-\sin (\frac{r_{m}}{r^{\prime }_{m}}\cdot \theta _{m}^{\prime }),OrthY=\cos (\frac{r_{m}}{r^{\prime }_{m}}\cdot \theta _{m}^{\prime }))$. Thus, the equation of the segregating line running through the agent at time $t=\frac{r_{m}}{r^{\prime }_{m}}$ is:

$$\begin{aligned}&-\sin \left( \frac{\theta _{m}^{\prime }\cdot r_{m}}{r^{\prime }_{m}}\right) x+\cos \left( \frac{\theta _{m}^{\prime }\cdot r_{m}}{r^{\prime }_{m}}\right) y\nonumber \\&\quad +\frac{r_{m}\theta _{m}^{\prime }- r^{\prime }_{m}\sin \left( \frac{\theta _{m}^{\prime }\cdot r_{m}}{r^{\prime }_{m}}\right) }{r^{\prime }_{m}\theta _{m}^{\prime 2}} = 0 \end{aligned}$$

(4)

To determine if a point is a pre-velocity destination, we can plug $D_x$ and $D_y$ into Eq. 4. If the result is less than 0, we know that the point is a pre-velocity destination. Interestingly, this line does not guarantee that points on the other side are post-velocity destinations (as we discuss shortly). Additionally, the trajectory of the agent forms a curve below this dividing line that contains pre-velocity destinations. Using this segregating line, these points are incorrectly identified as post-full velocity points. However, as all these points have low $D_\theta$ and $D_r$, the arrival time to these points is low and this mislabeling is inconsequential (see Sect. 4.4).

$ArrivalT$ can be found by first finding whether the destination is a pre- or a post-full velocity destination. Once we know which type of destination we have, we can use the appropriate equations to find arrival times. We first describe how to find $ArrivalT$ for post-full velocity destinations and then for pre-full velocity destinations.

4.2 Post-full velocity destinations

If we know a destination is a post-full velocity destination, we know the agent will reach the full velocity point before it transitions to the straight motion step (see Sect. 3). Since it takes $\frac{r_{m}}{r^{\prime }_{m}}$ time for the agent to reach this point, we already know one part of the arrival time.

Once an agent reaches full velocity it stops accelerating and continues at the same speed while turning. Thus, the agent follows a perfect circle from the time it reaches full velocity to the time it is looking directly at its destination (see the dotted circle in Fig. 2). Finding the point where the agent is looking directly at the destination requires finding the equation for this circle and then finding the point on this circle where the agent changes from the turning with velocity step to the straight motion step. Since this is the point where the agent leaves the perfect circle trajectory, we call this the departure point.

To find the departure point, we first have to find the circle the agent follows after it reaches full velocity. Since the agent reaches its full velocity at the full velocity point, we know this point is on the circle. We can calculate the radius of this circle, since it takes $\frac{2\cdot \pi }{\theta _{m}^{\prime }}$ seconds for an agent to turn $2\pi$ radians, in which time the agent will go around this circle exactly once. Since the time to go around the circle once is $t=\frac{2\cdot \pi }{\theta _{m}^{\prime }}$ and the agent’s velocity is $v=r_{m}$, the agent will travel $\frac{2\cdot \pi }{\theta _{m}^{\prime }}\cdot r_{m}=\frac{2\cdot \pi \cdot r_{m}}{\theta _{m}^{\prime }}$ meters if it followed this circle all the way around. Thus, the circumference of the circle is $\frac{2\cdot \pi \cdot r_{m}}{\theta _{m}^{\prime }}$. Using this, we can compute the radius of the circle: $\frac{2\cdot \pi \cdot r_{m}}{\theta _{m}^{\prime }}=2\cdot \pi Radius.$ It follows that $R=\frac{r_{m}}{\theta _{m}^{\prime }}$.

We find the center of this circle by moving in the opposite direction of $(OrthX, OrthY)$ a distance of $R$ starting from the full velocity point. The circle center $(CCX,CCY)$ in terms of equation Eq. 3 is:

$$\begin{aligned} CCX = FVX + OrthX\cdot R\nonumber \\ CCY = FVY + OrthY\cdot R. \end{aligned}$$

(5)

We can now find the exact departure point since the destination, circle center, and departure point form a right triangle. The hypotenuse of the triangle runs from the destination to the circle center. Using this information, we can find the angle between the vectors running from the destination to the circle center and departure point $AngleDepDestCC$. The angle from the destination the center of the circle, $AngleDestCC$ is trivial to find. Using these we can find the angle of the vector running from the circle center to the departure point, $AngleDep = ASin(Radius / DistDestCC) + AngleDepDestCC$.

Finding the departure point is a matter of finding the distance from the destination to the departure point $DistDest$ $Dep$ and using that to find the exact location of the departure point $(DPX,DPY)$. The points are $DPX = \cos (AngleDep)\cdot DistDestDep + DX$ and $DPY = \sin (AngleDep)\cdot DistDestDep + DY$.

Now that we have the location of the departure point, we can find how long the agent is on the circle. We do this by taking the angles from the circle center to the full velocity point and then from the circle center to the departure point. The angle difference between these over $2\pi$ will give us the percentage of the circumference that the agent follows between the full velocity point and the departure point. We call this time $TCircle$. To find the time the agent spends on the straight motion step, we take the distance from the departure point to the destination we found earlier ($DistDestDep$) and divide by $r_{m}$: $TDest = DistDestDep / r_{m}$. Combining $\frac{r_{m}}{r^{\prime }_{m}}$, $TCircle$ and $TDepDest$ gives us the total time to reach the post-full velocity destination. More Formally:

$$\begin{aligned}&AT_{Post}(\pi ,D_x,D_y,r_{m},r^{\prime }_{m},\theta _{m}^{\prime })\nonumber \\&\quad =\frac{r_{m}}{r^{\prime }_{m}}+TCircle+TDepDest \end{aligned}$$

(6)

Notice that the trajectory from the full velocity point to the destination is composed of straight lines and circles. This Dubins path (see Sect. 2) is expected since the agent’s velocity does not change between these points.

4.3 Pre-full velocity destinations

Thus far we have talked about how to find $AT_{Post}$, or the time required for an agent to reach its post-full velocity destination where $at=\pi$. Now, we address the issue of reaching pre-full velocity destinations.

By definition, pre-full velocity paths are composed of two parts. In the first part, the agent turns while accelerating. In the second part, the agent heads straight while accelerating with the possibility of continuing straight at maximum speed. The key to deriving these paths and their lengths lies in finding the transition point between the turning portion and the straight portion. In order to find this transition point, we need to find the time when the angle of the vector running from the agent to the destination has the same heading as the agent. The heading of the agent at time $t$ is simply $t\cdot \theta _{m}^{\prime }$. The angle of the vector running from the agent at time $t$ to the destination is: $Tan(\frac{D_y-y(t)}{D_x-x(t)})$ where $x(t)$ and $y(t)$ are defined in Eq. 2. Thus, we can find the transition point if we can solve the following for $t$:

$$\begin{aligned} \theta (t)=Tan\left( \frac{D_y-\left( \frac{\sin (\theta _{m}^{\prime }\cdot t)}{\theta _{m}^{\prime 2}} - \frac{t\cdot \cos (\theta _{m}^{\prime }\cdot t)}{\theta _{m}^{\prime }}\right) }{D_x-\left( \frac{\cos (\theta _{m}^{\prime }\cdot t)}{\theta _{m}^{\prime 2}} + \frac{t\cdot \sin (\theta _{m}^{\prime }\cdot t)}{\theta _{m}^{\prime }} - \frac{1}{\theta _{m}^{\prime 2}}\right) }\right) \end{aligned}$$

(7)

There is no analytical solution to this equation and the Taylor Series approximation has too much error to be useful. However, we can reframe the problem to significantly reduce the error. Instead of thinking of an agent moving towards its destination over time, we think of the destination moving around and toward the agent over time. This frame of reference is best defined using polar coordinates where the polar coordinates give the distance and angle offset to the destination. In this frame of reference, the agent will look at its destination when the $y$ coordinate of the destination is 0. We can define the evolution of the destination’s position using differential equations as follows, with $\Delta$ being the timestep:

$$\begin{aligned} \!\!\!\!&D_r(0) = D_r\ \mathrm{and} \ D_\theta (0) = D_\theta \nonumber \\ \!\!\!\!\!&D_r(t) = D_r(t-\Delta ) - r^{\prime }_{m}\cdot \Delta \nonumber \\ \!\!\!\!\!&D_\theta (t) = D_\theta (t - \Delta ) - \theta _{m}^{\prime }\cdot \Delta \end{aligned}$$

(8)

Converting Eq. 8 to Euclidean values, the $y$ value of the rotating destination with respect to time is:

$$\begin{aligned} r^{\prime }_{m}\cdot t \cdot \theta _{m}^{\prime }+ D_y\cdot \theta _{m}^{\prime 2}\cos (t\cdot \theta _{m}^{\prime })- (r^{\prime }_{m}+ D_x\cdot \theta _{m}^{\prime 2}) \sin (t\cdot \theta _{m}^{\prime })\nonumber \\ \end{aligned}$$

(9)

There is no analytical way of finding $t$ when Eq. 9 equals 0. However, if we replace the Sine and Cosine functions in Eq. 9 with their second-degree Taylor Series approximations, Eq. 9 becomes tractable (Sect. 4.4 shows that the error of this and our other equations is minute). The new function with the Taylor Series approximations is:

$$\begin{aligned} r^{\prime }_{m}\cdot t\cdot \theta _{m}^{\prime }-t\cdot \theta _{m}^{\prime }(r^{\prime }_{m}+D_x\theta _{m}^{\prime 2})+D_y\theta _{m}^{\prime 2}\left( 1-\frac{t^2\cdot \theta _{m}^{\prime 2}}{2}\right) \nonumber \\ \end{aligned}$$

(10)

Solving for $t=0$, we know a pre-full velocity agent will look at its destination when $t=TStraight$:

$$\begin{aligned} TStraight=\dfrac{-D_x\cdot \theta _{m}^{\prime }+\sqrt{D_x^2\cdot \theta _{m}^{\prime 2}+2Dy^2\theta _{m}^{\prime 2}}}{D_y\theta _{m}^{\prime 2}} \end{aligned}$$

(11)

Starting from here, we can find the total time for the agent to reach the destination. Equation 11 gives us the time for the agent to look at is destination. It is a simple matter of integrating to find the agent’s location at the point, and hence the remaining distance to be traveled. The total time therefore becomes the time from Eq. 11 plus the remaining time the agent travels straight toward its destination (which we call $RT$). Combined, we can find the arrival time for an agent with a pre-full velocity destination:

$$\begin{aligned} AT_{Pre}(\pi ,D_x,D_y,r_{m},r^{\prime }_{m},\theta _{m}^{\prime })= TStraight+RT \end{aligned}$$

(12)

We now have the equations for deriving $ArrivalT$ where $at=\pi$ for any destination. If a destination is a post-full velocity destination, the equations outlined in Sect. 4.2 will give the arrival time of an agent. If a destination is a pre-full velocity destination, the equations outlined in Sect. 4.3 will give the arrival time of the agent. If the destination in unreachable, $ArrivalT$ returns $+\infty$.

We can combine Eqs. 6 and 12 to get the full definition of $ArrivalT$ where $at=\pi$. For this equation, we introduce a new function $Pre:x,y\rightarrow {true,false}$ which returns $true$ if the destination is to the right of the segregating line defined in Eq. 4 and $false$ otherwise. We also define a new function $Post:x,y\rightarrow {true,false}$ that returns $true$ if a destination is to the left of the segregating line defined in Eq. 4 and not in the unreachable area defined in Eq. 5.

$$\begin{aligned}&ArrivalT(\pi ,D_x,D_y,r_{m},r^{\prime }_{m},\theta _{m}^{\prime })\nonumber \\&=\,{\left\{ \begin{array}{ll} AT_{Pre}(\pi ,D_x,D_y,r_{m},r^{\prime }_{m},\theta _{m}^{\prime }),&{}\mathrm{if}\ Pre(D_x,D_y)\\ AT_{Post}(\pi ,D_x,D_y,r_{m},r^{\prime }_{m},\theta _{m}^{\prime }),&{}\mathrm{if}\ Post(D_x,D_y)\\ +\infty ,&{}\mathrm{otherwise} \end{array}\right. } \nonumber \\ \end{aligned}$$

(13)

4.4 Validation and error analysis

In order to define $ArrivalT$ with $at=\pi$, we broke down destinations into two sets, pre-full velocity destinations and post-full velocity destinations. For the post-full velocity destinations, we were able to derive arrival time analytically. For pre-full velocity destinations, we derived the formula for when the agent sees its destination using a Taylor Series approximation. To verify our equations and claims of low error, we compared our equations for agent arrival time $ArrivalT$ to actual simulations of non-holonomic agent movement. In doing so, we show that there is almost no error and that the simulation converges to our equations as the simulation’s timestep converges to 0.

Figure 3 shows the surface $ArrivalT$ with $at=\pi$ across a large set of destinations. Notice that this surface has several key features we would expect. First, arrival times grow as destinations move away from the origin. We would expect this since destinations further from the origin will take longer to reach. Second, the surface is almost radially symmetric, but not quite. You can tell by counting contour lines that destinations with higher $D_\theta$ values take longer to reach. We expect this since these destinations require the agent to turn in order to reach them, thus increasing the arrival time. Third, there are no seams or jumps as our equations change from using the post-full velocity equations and pre-full velocity equations. This is because the equations are accurate enough that slight changes in the destination around the segregating line in Eq. 4 do not create any perceptible change the calculated arrival time. Note also that there is a gap in the surface to the left of the origin. This corresponds to the unreachable circle shown in Fig. 2. Since agents cannot reach these points when $at=\pi$, they have an infinite arrival time and are culled from the surface.

Fig. 3

Plot of how long it will take for an agent ($r_{m}=r^{\prime }_{m}=\theta _{m}^{\prime }=1$) to arrive at different destinations when $at=\pi$. The $x$ and $y$ axes correspond to the $x$ and $y$ values of destinations. The height of the curve shows time required to reach destinations. Contour lines are placed every 1 s on the surface to help discern the surface’s height

For a quantitative validation of $ArrivalT$ with $at=\pi$, we generated a similar arrival time surface using the agent simulator from our crowd simulation algorithm. This simulator took time steps, moving the agent forward based on its current linear velocity and heading, and then updating the heading to turn toward the destination. The simulator continued to take steps until the agent came within a small threshold of the destination, at which time it reported the accumulated time. The closer the results of these simulations were similar to those predicted by $ArrivalT$, the more accurate we knew our equations were.

Figure 4 shows the error difference between our calculated surface using $ArrivalT$ and our simulated agent with a timestep of 0.1 s. The average difference between calculated and simulated arrival times was $-0.0015$ s. while the average arrival time for the samples on our surface was 8.93 s. Thus, the average difference was only 1/5,953 the size of our average arrival time.

Fig. 4

Difference between arrival times calculated using $ArrivalT$ with $at=\pi$ and simulation using 3,300 samples. We used a timestep of 0.1 s in this test. The average difference was a minimal $-0.0015$ s

Note also that when we run a discrete simulation, it is almost impossible for the agent to arrive exactly at the destination. In the simulation, we treat the destination as a circle with a radius of the timestep times $r_{m}$. Thus, in this error analysis, we would expect the simulated arrival times of an agent to a destination to be within $\pm .1$ s of the true arrival time. Note that in over 99 % of the comparisons in Fig. 4, the difference between our calculated arrival time and the simulated arrival time was within this $\pm 0.1$ s threshold. Also, almost 98 % of the comparisons are within half of that threshold. This result gives further evidence that our derivation of $ArrivalT$ is highly accurate.

We further looked at the error as the timestep decreases. Since many of our equations find the exact arrival time of an agent, we would expect the difference between our equations and simulated arrival times to decrease as the simulation time step approaches one over infinity. Figure 5 shows this difference as the simulation timestep approaches 0. Notice that the error decreases exponentially as expected, or approximately linearly on a log scale.

Fig. 5

Log of absolute average difference between $ArrivalT$ and simulated arrival times of agents as the timestep of the simulated agents decreases exponentially. Over 3,300 samples were used in each comparison. We would expect this error to drop to zero as the timestep approaches zero, which it appears to do

5 Arbitrary angle thresholds

We have derived $ArrivalT$ for an agent where $at=\pi$. We can now use this to derive the solution for $ArrivalT$ for arbitrary values of $at$.

Recall from our section on definitions (Sect. 3) that the movement of agents can be broken down into three steps: the turn in place, the turn with velocity, and the straight motion steps. When $at=\pi$, agents skip the turn in place step since all destinations have a $D_\theta \le at=\pi$. For most destinations, however, the optimal acceleration angle threshold is not $at=\pi$. Thus, we need to expand our solution to $ArrivalT$ to include all values of $at$ before we can solve $BestAT$.

Consider an agent whose acceleration angle threshold is $\pi /2$. For all destinations where $D_\theta <\pi /2$, the path of the agent to the destination would be the same as an agent with an acceleration angle threshold of $\pi$, since in both cases $D_\theta$ falls underneath these thresholds. The only difference is how the agent reaches destinations with $D_\theta >\pi /2$. In these cases, the $at=\pi /2$ agent would turn in place until the difference between its heading and the destination is $\pi /2$. It would then proceed to the turn with velocity step.

When $at=\pi /2$, an agent would turn until destinations with $D_\theta >\pi /2$ are $\pi /2$ radians away. For simplicity, we can think about the destination rotating about the agent instead of the agent rotating in place. Figure 6 shows how these destinations would rotate. Notice that some destinations that were unreachable to an agent with $at=\pi$ are reachable to an agent with $at=\pi /2$ agent and vice versa. Once the destination has been rotated, the agent proceeds as if $at=\pi$.

Fig. 6

How destinations with $D_\theta >\pi /2$ rotate during the turn in place step when $at=\pi /2$. Notice that this changes which destinations are reachable when compared to $at=\pi$

After rotating destinations, we can use $ArrivalT$ with $at=\pi$ as the key component of our solution to $ArrivalT$ for any $at$. First, if $D_\theta >at$, we rotate destinations about the origin until they have a polar angle of $at$. We then proceed as if the acceleration angle threshold was $\pi$, using the newly rotated destination. Formally:

$$\begin{aligned}&\!\!\!\!ArrivalT:at,D_x,D_y,r_{m},r^{\prime }_{m},\theta _{m}^{\prime }\rightarrow Time\nonumber \\&\!\!\!\!DestATDiff = D_\theta -at\nonumber \\&\!\!\!\!TurnTime = {\left\{ \begin{array}{ll} (D_\theta -at)/\theta _{m}^{\prime },&{}\mathrm{if}\ DestATDiff>0\\ 0,&{}\mathrm{otherwise} \end{array}\right. }\nonumber \\&\!\!\!\!D'_x = {\left\{ \begin{array}{ll} \cos (at)D_r,&{}\mathrm{if}\ DestATDiff>0\\ D_x,&{}\mathrm{otherwise} \end{array}\right. }\nonumber \\&\!\!\!\!D'_y = {\left\{ \begin{array}{ll} \sin (at)D_r,&{}\mathrm{if}\ DestATDiff>0\\ D_y,&{}\mathrm{otherwise} \end{array}\right. }\nonumber \\&\!\!\!\!RemainingTime = ArrivalT(\pi ,D'_x,D'_y,r_{m},r^{\prime }_{m},\theta _{m}^{\prime })\nonumber \\&\!\!\!\!Time = TurnTime+RemainingTime \end{aligned}$$

(14)

Equation 14 satisfies our first main contribution (see Sect. 1): we have derived the arrival time for a non-holonomic agent to any destination with arbitrary movement constraints. This gives us the mathematical background to find $BestAT$, or the best acceleration angle threshold for a given destination.

Additionally, this equation gives us the exact path of a non-holonomic agent without any simulation. By using these integrals and equations, we can derive the exact location of an agent at any time. This is an important contribution, since it allows applications of non-holonomic agents to analytically check for collisions without doing any relatively expensive discrete simulation. We discuss this further in our future work section (Sect. 7).

6 Optimal angle thresholds

The primary goal of this work is to find optimal paths for non-holonomic agents without doing simulation. We do this by deriving a function $BestAT$ that finds the optimal acceleration angle threshold for an agent in terms of its arrival time. In the previous section we arrived at a definition of $ArrivalT$, the function that gives the arrival time for any acceleration angle threshold. In this section we use $ArrivalT$ to find a low error, functional approximation to $BestAT$. First, note that given a destination with a polar angle of $D_\theta$, all angle thresholds whose values are greater than $D_\theta$ will produce the same path since the agents will all immediately start accelerating while turning. Thus, if the best angle threshold for a destination is $D_\theta$, $BestAT$ could return any value in the range $[D_\theta ,\pi ]$. For clarity and to make $BestAT$ a clearly defined function, we say that $BestAT$ never returns an angle higher than $D_\theta$.

Since we have a definition of $ArrivalT$, we can do off-line computation to find the numeric approximation of the decision surface for finding the best acceleration angle threshold for a set of destinations. To do this, we iterate across a dense set of destinations in the area $x\in [-10,10],y\in [0,10]$. For each destination, we then iterate across acceleration angle thresholds starting with 0 and ending at $D_\theta$. This process returns the best acceleration angle threshold for each sample in our area, which we show in Fig. 7. Similar surfaces can be shown for other values of $r_{m}, r^{\prime }_{m},$ and $\theta _{m}^{\prime }$. Areas of this surface where $BestAT=D_\theta$ is highlighted in green. Notice that this decision surface is not perfectly flat, i.e., there is no one, hard-coded acceleration angle threshold that will result in the best arrival time for all destinations. This again validates our assertion that finding optimal paths for non-holonomic agents is non-trivial.

Fig. 7

$BestAT$, or the optimal acceleration angle decision surface ($r_{m},r^{\prime }_{m},$ and $\theta _{m}^{\prime }$=1). This surface has two distinct regions: one where $BestAT$ equals $D_\theta$ (the plateau region, highlighted green) and the sub-plateau region, where the contour lines form circles

Unfortunately, this numerical approach to finding the optimal acceleration angle threshold for a given destination is computationally expensive and will not work in real-time as a way of choosing paths. Instead, we found a functional approximation of $BestAT$ that will produce this same decision surface.

One approach to finding $BestAT$ would be to take Eq. 14 and find its derivative with respect to $at$. The zero-crossings of the derivatives that correspond to local minima would determine which angle thresholds produce the shortest arrival times. Equation 14 is composed of a series of closed form equations, therefore we were able to find the derivatives for each branch case using a computational mathematics package. Unfortunately, the resulting derivatives are so long that we estimate that transcribing them into this paper would take approximately ten pages. In addition, these derivatives do not have closed-form solutions for their zero-crossings. We therefore have derived a functional approximation to $BestAT$ based on empirical observation.

6.1 Plateau region

Based on our observations, we have found several key features of this surface that hold regardless of the underlying values of $r_{m},r^{\prime }_{m},$ and $\theta _{m}^{\prime }$. We leave it to the future work to show mathematically that these features are true for all values of $r_{m},r^{\prime }_{m},$ and $\theta _{m}^{\prime }$.

The primary features of this set of curves are best described using the polar definition of destinations $(D_r,D_\theta )$. Using this polar form of the $BestAT$ function, we have observed that $\forall r_g > r, BestAT(r_g,D_\theta ,r_{m},r^{\prime }_{m},\theta _{m}^{\prime }) >= BestAT(r,D_\theta r_{m},r^{\prime }_{m},\theta _{m}^{\prime })$. In other words, for a fixed destination angle, the optimal acceleration angle threshold monotonically increases as the polar radius of the destination increases. At some radius, the optimal acceleration angle threshold will reach $D_\theta$, which is the maximum acceleration angle threshold that an agent can choose, and all values of $BestAT$ for radii after that point will all have the same value ($D_\theta$). Thus, the optimal acceleration angle threshold always reaches a plateau. We call the minimum polar radius for which destinations of a given polar angle have an optimal acceleration angle threshold of $D_\theta$ the plateau point, or simply $plateau$. Figure 7 shows $BestAT$ with the plateau region highlighted in green.

Our second observation is that the polar radius grows monotonically as the polar angle increases. The highlighted region in Fig. 7 gives evidence of this observation.

The location of these plateau points is key to approximating $BestAT$. Once we have a function for finding the radius of a plateau point given a radius, we can define half of the optimal acceleration angle threshold decision surface precisely. This is true since we know that any destination point whose $D_r$ is greater than that of the plateau point will always have an optimal acceleration angle threshold of $D_\theta$. As noted earlier, the derivative of Eq. 14 is too unwieldy to provide guidance as to where these points are. Instead, we have numerically found plateau point pairs $(r,\theta )$ for set values of $r_{m},r^{\prime }_{m},$ and $\theta _{m}^{\prime }$. We then did a curve fit across these points to find an approximation to where these plateau points are located.

For example, consider the case where $r_{m},r^{\prime }_{m},$ and $\theta _{m}^{\prime }$ all equal 1, which have been the movement constraints we have used in all our figures. Fitting a fifth-degree polynomial to the plateau points in Fig. 8, we get:

$$\begin{aligned}&-4.68+30.72D_\theta -73.08D_\theta ^2+84D_\theta ^3-45.23D_\theta ^4\nonumber \\&\quad +9.26D_\theta ^5 \end{aligned}$$

(15)

To find the optimal acceleration angle threshold for a destination $(D_r,D_\theta )$, we plug $D_\theta$ into Eq. 15 to get the plateau point for this angle. If $D_r$ is greater than or equal to the value returned by Eq. 15, then we know the optimal acceleration angle threshold is $D_\theta$.

Fig. 8

Plot of $D_\theta$ vs. $D_r$ of the plateau points the define the plateau region in Fig. 7. These points are key to our functional approximation to $BestAT$

6.2 Sub-plateau region

We call the region of surface where the polar radius is less than the plateau point, the sub-plateau region. Again, we return to our observations of surfaces with different $r_{m},r^{\prime }_{m},$ and $\theta _{m}^{\prime }$ values with an example shown in Fig. 7. Notice that in the sub-plateau region, the contour lines on the surface form perfect circles. In other words, if we find the optimal acceleration angle threshold for a point not on the plateau region, all destinations with the same $D_r$ and higher $D_\theta$s will have the same optimal acceleration angle threshold. This optimal acceleration threshold value is the value of the plateau point whose radius is $D_r$. Another way to think about this is that each plateau point “gives” its value to all destinations whose $D_r$ is the same and whose $D_\theta$ is greater. This results in the perfectly circular contour lines on the optimal acceleration angle threshold surfaces. More formally if $BestAT(r,D_\theta ,r_{m},r^{\prime }_{m},\theta _{m}^{\prime }) < D_\theta$ (i.e., the pair $(D_r,D_\theta )$ is not in the plateau region), then $\forall \, \theta > D_\theta ,BestAT(r,\theta ,r_{m},r^{\prime }_{m},\theta _{m}^{\prime })=BestAT(r,D_\theta ,r_{m},r^{\prime }_{m},\theta _{m}^{\prime })$.

Thus, to find the optimal acceleration angle threshold for a destination in the sub-plateau region, we can find its value by rotating around the origin until we find the plateau point whose radius matches the current $D_\theta$. We could do this by incrementally decreasing $D_\theta$ and checking for plateau points, but it is easier to simply invert the plateau point function (e.g., Eq. 15). Based on our second observation above (that the radius of plateau points increase monotonically with their angles), we know that we can invert these functions. Returning to the case where $r_{m},r^{\prime }_{m},\theta _{m}^{\prime }$ all equal 1, the fifth-degree polynomial fit to the points is:

$$\begin{aligned}&0.12+1.18D_r-0.31D_r^2+0.041D_r^3-0.002D_r^4\nonumber \\&\quad + 0.00006D_r^5 \end{aligned}$$

(16)

Thus, given a destination $(D_r,D_\theta )$, our functional approximation to $BestAT$ is:

$$\begin{aligned} P&= PlateauPoint(D_r,D_\theta )\nonumber \\ SP&= SubPlateauPoint(D_r,D_\theta )\nonumber \\ BestAT(D_r,D_\theta )&= {\left\{ \begin{array}{ll} D_\theta ,&{}\mathrm{if}\ D_r\ge P\\ SP,&{}\mathrm{otherwise} \end{array}\right. } \end{aligned}$$

(17)

This approximate functional solution to $BestAT$ has very low error. The drawback is that it requires fitting a fifth-degree polynomial to the plateau points for different values of $r_{m},r^{\prime }_{m},$ and $\theta _{m}^{\prime }$. Fortunately, in most virtual agent applications, there is a limited set of possibilities for $r_{m},r^{\prime }_{m},$ and $\theta _{m}^{\prime }$. Thus, if these fits are done off-line, a simulation algorithm can quickly approximate $BestAT$ using this baked information.

6.3 Error

We quantitatively validate our functional approximation to $BestAT$ in Eq. 17 by comparing our calculated decision surface to the one we generated numerically (Fig. 7). To generate the numerical surface, we iterated over thousands of possible $at$ values across 3,300 possible destinations. These destinations were in the polar region $D_r\in [.38, 12], D_\theta \in [0,\pi ]$ to avoid numerical issues close to the origin.

The average error was 0.009 radians with a standard deviation of 0.017 (see Fig. 9). Note that 0.009 radians is less than half a percent of $\pi /2$. This low error gives us high confidence in our approach.

Fig. 9

Distribution of error (in radians) between the numerical solution to $BestAT$ and our approximation from Eq. 17

7 Conclusion and future work

In this paper, we derived the arrival time of non-holonomic agents to arbitrary destinations with arbitrary movement constraints. We also showed how to find the location of an agent at any time without having to do any discrete simulation. We also derived a numerical, off-line method for finding the best arrival time surface. We analyzed this surface and showed how to define this surface with two curves for use in real-time applications. This means that any application that uses non-holonomic virtual agents can quickly find optimal paths for agents that start with no velocity.

Our future work is focused in several areas. First, we want to solve these equations for arbitrary starting velocities. Second, we want to see how this affects the crowd simulation since we can find precise ideal paths to destinations. This could could lead to reduced arrival times for crowd simulation agents.