\documentclass[twocolumn,letter]{article}

\usepackage[margin=0.5in]{geometry}

\usepackage[english]{babel}
\usepackage[utf8]{inputenc}
\usepackage{amsmath}
\usepackage{color}
\usepackage{graphicx}

\usepackage{hyperref}

\renewcommand{\arctan}[1]{\mathrm{atan}#1}
\title{Kinematic trajectories for vehicle agents}

\author{Rasmus Fonseca \& Tim Caldwell}

\date{\today}

\begin{document}
\maketitle

\begin{abstract}
Kinematic trajectories are used to move agents forward along a predicted route frame. Here we describe two underlying kinematic models based on bicycle models and how they're translated from cartesian into route coordinates. 
\end{abstract}


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Kinematic bicycle models}

The commonly used simplification of a car is the rear-axle centered bicycle model:

\begin{figure}[!hb]
\centering
\def\svgwidth{\columnwidth}
\input{BicycleCartesian.pdf_tex}
\end{figure}

Our vehicle model uses lateral accelleration ($u_1$) and curvature of the front-wheel turning ($u_2$) as controls. The geometry of the rear-axle centered model (left) can be described as 
\begin{align}
\dot{V} &= u_1 \\
\dot{x} &= V \cos \theta \\
\dot{y} &= V \sin \theta \\
\dot{\theta} &= V \frac{\tan \delta}{l} \\
             &= V u_2
\end{align}
Where $(x,y)$ are the coordinates of the rear-axle and $V$ the velocity. Derivatives of $x$, $y$, and $V$ are straightforward, and $\dot{\theta}$ can be found by observing that the rear-axle will travel by the distance $V\mathrm{d}t$ along the circle which correponds to an angular change of $\frac{V\mathrm{d}t}{R} = Vu_2\mathrm{d}t$.
\begin{figure}[!hb]
\centering
\def\svgwidth{0.7\columnwidth}
\input{dTheta.pdf_tex}
\end{figure}

Polack et al.~\cite{Polack17} outline a similar model which is instead centered somewhere on the middle of the vehicles axis (right side of first figure). Here, the cartesian geometry can be described using the slip-angle of the center-of-mass: 
\begin{align}
\beta (\delta) &= \arctan \left( \tan(\delta)\frac{l_r}{l_r+l_f}\right) \\
\dot{V} &= u_1 \\
\dot{x} &= V \cos(\theta + \beta(u_2)) \\
\dot{y} &= V \sin(\theta + \beta(u_2)) \\
\dot{\theta} &= V \frac{\sin \beta(u_2)}{l_r} 
\end{align}
By substituting $u_2 = \frac{\tan\delta}{l_r + l_f}$ into $\beta$ and simplifying using trigonometric equations~\cite{WikiInvTrig,WikiAngleSum} all calls to $\arctan$ can be eliminated:

\begin{align}
\beta (u_2)  &= \arctan \left( u_2 l_r\right) \\
\dot{\theta} &= V \frac{\sin \beta(u_2)}{l_r} \\
        &= V \frac{\sin \arctan(u_2 l_r)}{l_r}  \\
        &= V \frac{u_2}{\sqrt{1+u_2^2l_r^2}}\\
        &= V \psi u_2 \label{eqn:CoM_tdot}\\
\dot{x} &= V \cos(\theta + \beta(u_2)) \\
        &= V \left(\cos(\theta)\cos(\arctan(u_2l_r)) - \sin(\theta)\sin(\arctan(u_2l_r)) \right)\\
        &= V \left(\frac{\cos(\theta)}{\sqrt{1+u_2^2l_r^2}} - \frac{\sin(\theta)u_2l_r}{\sqrt{1+u_2^2l_r^2}} \right)\\
        &= V \frac{\left(\cos(\theta) - \sin(\theta)u_2l_r\right)}{\sqrt{1+u_2^2l_r^2}} \\
        &= V \psi \left(\cos(\theta) - \sin(\theta)u_2l_r\right) \label{eqn:CoM_xdot}\\
\dot{y} &= V \sin(\theta + \beta(u_2)) \\
        &= V \left(\sin(\theta)\cos(\arctan(u_2l_r)) + \cos(\theta)\sin(\arctan(u_2l_r)) \right)\\
        &= V \left(\frac{\sin(\theta)}{\sqrt{1+u_2^2l_r^2}} + \frac{\cos(\theta)u_2l_r}{\sqrt{1+u_2^2l_r^2}} \right)\\
        &= V \frac{\left(\sin(\theta) + \cos(\theta)u_2l_r\right)}{\sqrt{1+u_2^2l_r^2}} \\
        &= V \psi \left(\sin(\theta) + \cos(\theta)u_2l_r\right) \label{eqn:CoM_ydot}
\end{align}
where

\begin{align}
\psi = \frac{1}{\sqrt{1+u_2^2l_r^2}} 
\end{align}



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Transformation to road-reference coordinates}

Route reference lines are composed of geometric primitives that can be slices of either piecewise clothoid curves, circles, or lines. Positions in the route frame are represented using ($s$, $e_y$)-coordinates corresponding to distance along the curve and lateral offset. Vehicle orientation is specified using the yaw $\theta_e$ with respect to the tangent of the reference line at the $s$ coordinate. 
\begin{figure}[!hb]
\centering
\def\svgwidth{\columnwidth}
\input{BicycleRoad.pdf_tex}
\end{figure}
A model of the rear-centered bicycle in this coordinate system can be defined by extending on the cartesian set of equations where the $x$-axis is aligned with the tangent of the curve (left side of figure). This ensures that $\dot{s}$ is proportional to $\dot{x}$, $\dot{e_y}$ to $\dot{y}$, and that $\theta = \theta_e$. An assumption is made that for small steps the curve is well-approximated by a circle with the same curvature, $\kappa$, as the curve. 
\begin{align}
\dot{V}        &= u_1 \\
\dot{s}        &= \frac{\dot{x}}{1-e_y \kappa} \label{eqn:x-s}\\
               &= V \omega \cos{\theta_e} \\
\dot{e_y}      &= \dot{y} \label{eqn:y-ey}\\
               &= V \sin{\theta_e} \\
\dot{\theta_e} &= \dot{\theta} - \Delta_\theta\\
               &= \dot{\theta} - \frac{\dot{x}}{e_y - 1/\kappa} \label{eqn:t-t1}\\
               &= \dot{\theta} - \kappa \omega \dot{x} \label{eqn:t-t}\\
               &= V\left(u_2 - \kappa \omega \cos{e_y}\right)
\end{align}
where
\begin{align}
\omega         = \frac{1}{1-e_y\kappa} 
\end{align}

Equation \ref{eqn:x-s} comes from interpreting the triangle spanned by $\dot{x}$ from the rear-axle to the center of the reference lines curvature center (right side of above figure). The relationship between $\dot{x}$ and the full baseline roughly\footnote{More accurately, $\dot{s}$ would be the extent along the circle arc, but this introduces an expensive $\arctan$ computation which we'd like to avoid.} equals the relationship between $\dot{s}$ and the radius of the curvature-circle, $-1/\kappa$. Rewriting we get:
\begin{align}
\frac{\dot{s}}{-1/\kappa} &= \frac{\dot{x}}{e_y - 1/\kappa} \Leftrightarrow \\
\dot{s}\kappa &= \frac{\dot{x}}{1/\kappa-e_y} \Leftrightarrow \\
\dot{s} &= \frac{\dot{x}}{1 - e_y \kappa}
\end{align}

The $\Delta_\theta$ term in Equation~\ref{eqn:t-t1} is the compensation to $\theta_e$ required to keep the same heading when stepping the distance $\dot{s}$ along the route frame. Like in the derivation of $\dot{s}$, this term is determined by observing the curvature circle, but we now assume that at small values, $\dot{s}$ divided by the full base-line roughly\footnote{Again, this is only true in the limit of small steps, but avoids an expensive $\arctan$ computation.} equals the angular change around the curvature center. 

The transformation from rear-axle centered vehicle to CoM can be performed by substituting Equations~\ref{eqn:CoM_tdot}, \ref{eqn:CoM_xdot}, and \ref{eqn:CoM_ydot} into \ref{eqn:x-s}, \ref{eqn:y-ey}, and \ref{eqn:t-t}.
\begin{align}
\dot{V}        &= u_1 \\
\dot{s}        &= V \psi \omega \left(\cos(\theta_e) - \sin(\theta_e) u_2 l_r\right)\\
\dot{e_y}      &= V \psi \left(\sin(\theta_e) + \cos(\theta_e)u_2l_r\right)\\
\dot{\theta_e} &= V \psi \left(u_2 - \kappa \omega \left(\cos(\theta_e) - \sin(\theta_e)u_2l_r\right)\right)
\end{align}
where, to summarize
\begin{align}
\psi    &= \frac{1}{\sqrt{1+u_2^2l_r^2}} \\
\omega  &= \frac{1}{1-e_y\kappa} 
\end{align}




\begin{thebibliography}{9}
\bibitem{Polack17}
  P. Polack, F. Altche, B. D'Andrea-Novel, and A. De La Fortelle
  \emph{The Kinematic Bicycle Model: a Consistent Model for Planning Feasible Trajectories for Autonomous Vehicles}.
  HAL archives-ouvertes, 2017. \url{https://hal-polytechnique.archives-ouvertes.fr/hal-01520869}

\bibitem{WikiInvTrig}
  Wikipedia
  \emph{Inverse trigonometric functions}.
  \url{https://en.wikipedia.org/wiki/Inverse\_trigonometric\_functions}

\bibitem{WikiAngleSum}
  Wikipedia
  \emph{Angle sum and difference identities}.
  \url{https://en.wikipedia.org/wiki/List\_of\_trigonometric\_identities\#Angle\_sum\_and\_difference\_identities}

\end{thebibliography}
\end{document}
