# High-Speed Accurate Robot Control using Learned Forward Kinodynamics and
Non-linear Least Squares Optimization

Pranav Atreya1, Haresh Karnan2, Kavan Singh Sikand1,
Xuesu Xiao1, Sadegh Rabiee1, and Joydeep Biswas1 1The University of Texas at
Austin, Department of Computer Science, {pranavatreya<EMAIL_ADDRESS>{xiao, srabiee<EMAIL_ADDRESS>University of Texas at Austin,
Department of Mechanical Engineering<EMAIL_ADDRESS>
###### Abstract

Accurate control of robots at high speeds requires a control system that can
take into account the kinodynamic interactions of the robot with the
environment. Prior works on learning inverse kinodynamic (IKD) models of
robots have shown success in capturing the complex kinodynamic effects.
However, the types of control problems these approaches can be applied to are
limited only to that of following pre-computed kinodynamically feasible
trajectories. In this paper we present Optim-FKD, a new formulation for
accurate, high-speed robot control that makes use of a learned forward
kinodynamic (FKD) model and non-linear least squares optimization. Optim-FKD
can be used for accurate, high speed control on _any_ control task specifiable
by a non-linear least squares objective. Optim-FKD can solve for control
objectives such as path following and time-optimal control in real time,
without needing access to pre-computed kinodynamically feasible trajectories.
We empirically demonstrate these abilities of our approach through experiments
on a scale one-tenth autonomous car. Our results show that Optim-FKD can
follow desired trajectories more accurately and can find better solutions to
optimal control problems than baseline approaches.

##  I Introduction and Related Work

At moderate speeds, pure kinematic models or simplified kinodynamic models are
sufficient for point to point motion control, for example, in model predictive
control [1, 2, 3] schemes. Such simplified models assume that robots only
operate in a limited subspace of their entire state space, such as low
acceleration and speed, minimum wheel slip, negligible tire deformation, and
perfect non-holonomic constraints. However, real-world robotic missions such
as high-speed off-road driving may entail violations of such simplified
kinodynamic models.

Since accurate kinodynamic models are hard to construct analytically, there
has recently been considerable interest in applying learning to motion control
[4]. End-to-end learning is the most straightforward way to encapsulate both
the model and controller in one function approximator and to train it with
data, e.g., learning a neural network using imitation learning [5, 6, 7, 8, 9,
10, 11, 12] or reinforcement learning [13, 14, 15, 16]. Such end-to-end
approaches suffer from drawbacks such as requiring a significant volume of
training data, poor generalizability to unseen scenarios, and most
importantly, a lack of safety assurance for navigation, which is commonly
provided by classical planning and control algorithms.

To address such disadvantages of end-to-end learning, hybrid approaches have
been introduced that leverage existing robot planning and control methods. For
example, Wigness et al. [17] and Sikand et al. [18] used imitation learning to
learn a cost function for existing navigation controllers to enable adaptive
behaviors for semantics. Similar techniques have been applied to learn
socially compliant navigation [19, 20, 21]. Xiao et al. [22] introduced
Adaptive Planner Parameter Learning, which learns to dynamically adjust
existing motion planners’ parameters to efficiently navigate through different
obstacle configurations using teleoperated demonstration [23], corrective
interventions [24], evaluative feedback [25], and reinforcement learning [26].
These approaches focus on using machine learning to enable custom behaviors,
such as semantic awareness, social compliance, or smooth obstacle-avoidance.

Machine learning has also been used for high-speed robot control [27, 9, 28].
Model Predictive Path Integral control [29] utilizes a classical sampling
based controller in an online learning fashion: it learns a sample
distribution during online deployment that is likely to generate good samples
[30]. However, it uses a simple unicycle forward model to predict future paths
based on the samples and compensates for such an over-simplified model by a
massive number of samples evaluated in parallel on GPUs [31]. Brunnbauer et
al. [32] reported that model-based deep reinforcement learning substantially
outperforms model-free agents with respect to performance, sample efficiency,
successful task completion, and generalization in autonomous racing. Learning
inverse kinodynamics conditioned on inertial observations [33] has also been
shown to be effective for accurate, high-speed, off-road navigation on
unstructured terrain.

Another class of approaches apply machine learning to model predictive control
[34, 35, 36]. These approaches however either use simple learned primitives
for modelling dynamics, or are geared primarily towards process control, and
have not been demonstrated to be capable of controlling complex robotic
systems at high speed in real time.

Leveraging a hybrid paradigm to address high-speed robot control problems, our
approach, which we call Optim-FKD, utilizes non-linear least squares
optimization to find optimal control sequences based on the learned model in
order to enable high-speed, accurate robot motions. The contributions of this
paper are summarized as follows:

  1. 1.

A novel formulation for robotic control using a learned forward kinodynamic
function and non-linear least squares optimization. We demonstrate that this
formulation is easily extensible to a range of control tasks without requiring
the retraining of a new forward kinodynamic model.

  2. 2.

A novel learning formulation that enables a highly accurate forward
kinodynamic model to be learned.

  3. 3.

A detailed description of the system architecture required to enable the
presented approach to run on real robot hardware in real time.

  4. 4.

Empirical results demonstrating that the presented approach outperforms
baselines for various robot control tasks.

##  II Optim-FKD Mathematical Formulation

We first present a general mathematical formulation for optimal control stated
as a non-linear least squares problem using a learned forward kinodynamic
function. Next, we demonstrate how such a formulation can be applied to
different objectives, including trajectory following and time-optimal control.

###  II-A Preliminaries

Let $X$ represent the state space of the robot. $X$ consists of configuration
space variables (such as position and orientation) and dynamics variables
(such as linear and angular velocity). Let $U$ represent the control space of
the robot. Consider a period of time of operation of the robot $\Delta t$. It
is assumed that controls are executed on the robot in a piecewise constant
manner. Let $\tau$ be the duration for which a particular constant control is
executed. In the time period of operation $\Delta t$, the robot will execute
$n=\frac{\Delta t}{\tau}$ constant controls.

To model the response of the robot from the executed controls, we introduce a
state transition likelihood function $\rho:X\times U^{n}\times
X^{n}\rightarrow[0,1]$. $\rho$ takes as input the initial state of the robot
$x_{0}\in X$, a length-$n$ piecewise constant control sequence $u_{1:n}$, and
a length-$n$ state sequence $x_{1:n}$. Each $x_{i}\in x_{1:n}$ represents the
state of the robot at time $i\cdot\tau$. The output of $\rho$ is the
probability that the state sequence $x_{1:n}$ is observed after executing
$u_{1:n}$ beginning from $x_{0}$.

We assume that the motion of the robot obeys the Markov property, that is, the
probability of reaching a state $x_{i}$ depends only on the previous state
$x_{i-1}$ and the constant control executed beginning at that previous state
$u_{i}$. This induces a local state transition likelihood function
$\rho_{i}(x_{i-1},u_{i},x_{i})$ for every $i\in 1...n$. The probability that
$x_{1:n}$ is observed after executing $u_{1:n}$ from $x_{0}$ can be written in
terms of $\rho_{i}$ as

$\rho(x_{0},u_{1:n},x_{1:n})=\prod_{i=1}^{n}{\rho_{i}(x_{i-1},u_{i},x_{i})}\quad.$
(1)

Moreover, the maximum likelihood state sequence $\widehat{x_{1:n}}$ after
executing $u_{1:n}$ from $x_{0}$ is

$\displaystyle\widehat{x_{1:n}}$
$\displaystyle=\operatorname*{arg\,max}_{x_{1:n}}{\rho(x_{0},u_{1:n},x_{1:n})}$
(2)
$\displaystyle=\operatorname*{arg\,max}_{x_{1:n}}{\prod_{i=1}^{n}{\rho_{i}(x_{i-1},u_{i},x_{i})}}\quad.$
(3)

Breaking down each generative probability $\rho_{i}$ into discriminative
probabilities yields

$\displaystyle\rho_{i}(x_{i-1},u_{i},x_{i})$
$\displaystyle=p(x_{i}|u_{i},x_{i-1})p(x_{i-1}|u_{i})p(u_{i})$ (4)
$\displaystyle=p(x_{i}|u_{i},x_{i-1})p(x_{i-1})p(u_{i})\quad,$ (5)

where Eq. 5 is obtained by applying the fact that the previous state $x_{i-1}$
is independent from the next control. Plugging Eq. 5 in Eq. 3 yields
$\widehat{x_{1:n}}=\operatorname*{arg\,max}_{x_{1:n}}{\prod_{i=1}^{n}{p(x_{i}|u_{i},x_{i-1})}}$.
We assume $p(x_{i}|u_{i},x_{i-1})$ follows a normal distribution:
$p(x_{i}|u_{i},x_{i-1})\sim\mathcal{N}(\bar{x_{i}},\sigma_{x_{i}})$, and
represent the maximum likelihood estimate of $p(x_{i}|u_{i},x_{i-1})$ as the
forward kinodynamic function $\pi(u_{i},x_{i-1})=\bar{x_{i}}$. With this
definition of $\pi$, equation 3 can be rewritten as

$\widehat{x_{1:n}}=(\pi(u_{1},x_{0}),...,\pi(u_{n},\widehat{x_{n-1}}))$ (6)

Next, we show that various robot control problems can be expressed as
nonlinear least squares optimizations that use the forward kinodynamic
function $\pi$.

###  II-B Objective 1: Path Following

The problem we consider here is that of following a predefined path as closely
as possible. This problem becomes noteworthy at high speeds where accurate
control of the robot becomes increasingly more difficult.

We are given $x_{1:n}^{*}$ which describes a path to follow. Following this
path as closely as possible amounts to solving

$u_{1:n}^{*}=\operatorname*{arg\,max}_{u_{1:n}}{\rho(x_{0},u_{1:n},x_{1:n}^{*})}\quad.$
(7)

From Eq. 6, this is equivalent to solving

$u_{1:n}^{*}=\operatorname*{arg\,min}_{u_{1:n}}{||x_{1:n}^{*}-\widehat{x_{1:n}}||_{2}^{2}}\quad,$
(8)

which is a nonlinear least squares problem where each
$\widehat{x_{i}}\in\widehat{x_{1:n}}$ is determined from the forward
kinodynamic function $\pi$.

###  II-C Objective 2: Optimal Connectivity

Another variant of the robot control problem that we consider is reaching a
goal state $x_{f}$ from a start state $x_{i}$ in the shortest amount of time
possible. Problems of this type appear frequently in optimal sampling-based
motion planning, where algorithms like RRT* [37] and BIT* [38] require a
steering function that can time-optimally connect arbitrary states. Learned
kinodynamic models for such problems have so far only been studied in
simulation [39].

Consider the maximum likelihood state sequence $\widehat{x_{1:n}}$ from
earlier. If we wanted the final state of the robot to be as close as possible
to the goal state $x_{f}$, we would optimize

$u_{1:n}^{*}=\operatorname*{arg\,min}_{u_{1:n}}{||x_{f}-\widehat{x_{n}}||_{2}^{2}}$
(9)

This formulation however keeps the time that the goal state $x_{f}$ is reached
fixed. Specifically, the state $\widehat{x_{n}}$ is reached after time
$n\cdot\tau$. To also minimize the time taken to reach the goal, $n$ is
introduced as an optimization parameter. This results in the objective
function

$u_{1:n}^{*},n^{*}=\operatorname*{arg\,min}_{u_{1:n},n}{||x_{f}-\widehat{x_{n}}||_{2}^{2}+(\alpha(n\cdot\tau))^{2}}$
(10)

where $\alpha$ is a scaling parameter that trades off time to reach the goal
and the distance to the goal. Like the path following formulation, this
formulation is a nonlinear least squares optimization where $\widehat{x_{n}}$
is determined from the forward kinodynamic function $\pi$.

##  III Forward Kinodynamic Model Learning

In this section we present how the forward kinodynamic model $\pi$ is learned.
Since $\pi$ is an integral component to the nonlinear least squares
optimizations introduced earlier, it is key that $\pi$ models the true forward
kinodynamics effectively. We learn $\pi$ for a scale one-tenth autonomous
robot car.

###  III-A Dataset Generation

The FKD model $\pi$ is trained in a supervised manner. We obtain the labeled
training data by teleoperating the robot at various speeds and recording at
every timestep the state estimates of the robot and the joysticked control
commands. This results in a dataset $D$ of trajectories $T_{1},...,T_{m}$
where each trajectory $T_{i}\in D$ is a tuple of the form
$(v_{x}(t),v_{y}(t),\omega(t),x(t),y(t),\theta(t),\delta(t),\psi(t))$. Here,
$v_{x}(t)$ is the velocity in the $x$-direction, $v_{y}(t)$ is the velocity in
the $y$-direction, $\omega(t)$ is the angular velocity, $x(t)$ is the
$x$-position, $y(t)$ is the $y$-position, $\theta(t)$ is the orientation,
$\delta(t)$ is the commanded forward velocity, and $\psi(t)$ is the commanded
angular velocity. All of these functions are time-dependent and are defined in
the domain $[0,t_{f}^{(i)}]$ where $t_{f}^{(i)}$ is the termination time for
trajectory $T_{i}$.

###  III-B Learning Formulation

The formulation presented earlier for $\pi$ maps an initial state of the robot
and a constant control to the most likely immediate next state of the robot.
Training of an FKD model with such short prediction horizon is challenging in
practice. If the model is tasked with predicting the state after $\tau$ units
of time, where $\tau$ is a small positive number, it can get away with simply
predicting the current state without incurring much loss. In order to ensure
the model’s predictions are of high quality, the model needs to learn to model
the state of the robot after a time period much greater than $\tau$.

Simply increasing $\tau$ would not suffice for achieving a longer prediction
horizon since that would forego the fine-grained prediction capabilities of
the model. We address this problem by training $\pi$ in a recurrent fashion.
The basic structure of the recurrence formulation is as follows. The model
predicts the next state $x_{i}=\pi(u_{i},x_{i-1})$. For timestep $i+1$,
instead of being given access to the ground truth value of $x_{i}$, the model
uses its previous prediction as the starting state:
$x_{i+1}=\pi(u_{i+1},x_{i})$. This process continues for the number of
timesteps in the prediction horizon.

This simple recurrent approach has a few limitations however. The base
timestep duration $\tau$ is selected to be small so as to capture minute
changes in the state of the robot. In our experiments we set $\tau$ to $0.05$
seconds. For the model to predict the state of the robot after time
$t_{\mathrm{pred}}$, $\frac{t_{\mathrm{pred}}}{\tau}$ forward passes through
the model are needed. In our experiments, we set $t_{\mathrm{pred}}$ to $3.0$
seconds, requiring $60$ forward passes through the model. Since $\pi$ is to be
used in a real-time optimization framework, the number of forward passes
through $\pi$ need to be limited to maintain computational efficiency. We
achieve this by introducing a model prediction time $t_{\mathrm{model}}$. The
FKD model $\pi$, in one forward pass, outputs the next
$\frac{t_{\mathrm{model}}}{\tau}$ states given the next
$\frac{t_{\mathrm{model}}}{\tau}$ controls. It also takes in as input the
previous $\frac{t_{\mathrm{model}}}{\tau}$ states, enabling recurrence. In our
experiments $t_{\mathrm{model}}$ was set to $0.5$ seconds. This approach
enables both computational efficiency and fine-grained prediction.

Having motivated the model formulation, we now present the learning objective.
For each trajectory $T_{i}$ in our dataset $D$, we evenly sample $k$ starting
times $(t_{1},...,t_{k})$ from the range
$[t_{\mathrm{model}},t_{f}^{(i)}-t_{\mathrm{pred}}]$. For each starting time
$t_{a}\in(t_{1},...,t_{k})$, we use the model to predict the states at times
$t_{a}+b\tau$ for $b\in[0,1,...,\frac{t_{\mathrm{pred}}}{\tau}]$. For brevity,
let $S_{t_{a}}$ be the state variables
$v_{x}(t),v_{y}(t),\omega(t),x(t),y(t)$, and $\theta(t)$ sampled evenly with
spacing $\tau$ from the time period $[t_{a},t_{a}+t_{\mathrm{model}}]$.
Additionally let $M_{t_{a}}$ be the control variables $\delta(t)$ and
$\psi(t)$ sampled in the same manner. The model $\pi$ takes in as input
$M_{t_{a}}$ and $S_{t_{a}-t_{\mathrm{model}}}$ and produces as output
$\tilde{S}_{t_{a}}$. $\tilde{S}_{t_{a}}$ differs from $S_{t_{a}}$ in that the
former is the model’s prediction whereas the latter is the ground truth.
Taking everything into account, we obtain the following learning objective

$\displaystyle\operatorname*{arg\,min}_{\Theta}\sum_{T_{i}\in
D}\sum_{t_{a}\in(t_{1},...,t_{k})}\sum_{i=0}^{\frac{t_{\mathrm{pred}}}{t_{\mathrm{model}}}-1}{||\pi(\tilde{S}_{t_{a}+t_{\mathrm{model}}\cdot(i-1)},}$
(11) $\displaystyle{M_{t_{a}+i\cdot t_{\mathrm{model}}})-S_{t_{a}+i\cdot
t_{\mathrm{model}}}||_{2}^{2}}$

where $\Theta$ is the parameter set of $\pi$ and
$\tilde{S}_{t_{a}-t_{\mathrm{model}}}=S_{t_{a}-t_{\mathrm{model}}}$ for $i=0$.

##  IV Optimization System Architecture

In this section, we describe the system architecture of the proposed approach.
We discuss how to integrate the optimization procedure and calls to the FKD
model in a manner that enables real-time control on real robot hardware.
Figure 1 shows a block diagram of the system components. There are four key
components that all operate asynchronously: the state estimator, optimizer,
updater, and executor.

###  IV-A State Estimator

Figure 1: System architecture block diagram.

In order for a predefined path to be followed accurately or for a goal state
to be reached as fast as possible, accurate state estimates of the robot are
essential. These state estimates define the robot’s position, orientation,
velocity, and angular velocity with respect to some coordinate frame. The
state estimates themselves can come from a variety of sources such as a LIDAR
based localization algorithm or visual odometry. It is assumed that there
exists some delay $\epsilon$ between the actual, real-world state of the robot
and output of the state estimator. The state estimator operates
asynchronously, continuously updating a fixed sized buffer of the most recent
state estimates. It is critical that each estimated state in the buffer is
timestamped.

###  IV-B Optimizer

The optimizer begins by obtaining from the state buffer with the most recent
estimated state of the robot $\xi$. This state is used as the start state in
the optimization procedure. Depending on the optimization objective, different
steps are taken. For the path following objective, it is critical to first
localize the robot on the map that the robot is desired to follow. Let $P$ be
the path that the robot is assigned to follow at speed $v_{\mathrm{desired}}$.
$P$ consists of a series of robot positions $x,y,\theta$ ordered in increasing
order by which each position is to be reached by the robot. Each optimization
will plan the next $\Delta t$ controls for the robot. To do this, the goal
state $g\in P$ the robot needs to reach after time $\Delta t$ needs to be
determined. Localizing the robot in $P$ amounts to finding the position $s\in
P$ that minimizes $||\xi-s||_{2}^{2}$. The goal state $g$ can then be obtained
by computing $g=P\rightsquigarrow v_{\mathrm{desired}}\cdot\Delta t$ where the
$a\rightsquigarrow b$ operator looks ahead in $P$ from position $a$ by $b$
distance.

Next, the optimizer prepares the input required by the forward kinodynamic
model, namely the past $t_{\mathrm{model}}$ units time of robot state
information. This is done by running time synchronization on the states in the
state buffer by making use of the states’ timestamps to sample
$\frac{t_{\mathrm{model}}}{\tau}$ evenly spaced states. Finally the
optimization procedure is called, which optimizes over the next $\Delta t$ of
controls by making $\frac{\Delta t}{t_{\mathrm{model}}}$ calls to the FKD
model. The result $u^{*}$ along with $\xi$ is stored in a concurrency-safe
data structure. Note that the optimization horizon $\Delta t$ differs from the
prediction horizon $t_{\mathrm{pred}}$ in equation 11.

###  IV-C Updater

The main role of the updater is dealing with latencies that are characterstic
of real robot systems. We consider the two most impactful latencies:
$\epsilon$, the previously introduced latency in the state estimators
measurements, and $\gamma_{i}$, the time required for optimization $i$ to
complete. Since $\gamma_{i}$ is different for every run of the optimizer, it
needs to first be computed. This is done by computing the difference between
the current system time and the timestamp of $\xi$ that was used as the
starting state of the optimization procedure. The first $\gamma_{i}+\epsilon$
time units of controls are discarded from $u^{*}$, and the remainder replaces
the contents of the control buffer.

###  IV-D Executor

Finally, the executor asynchronously executes the commands stored in the
control queue one at a time on the robot. Given a well-trained and accurate
forward kinodynamic model $\pi$ the evolution of the state of the robot after
executing the controls will closely match the predicted state sequence by the
model, meaning the controls output by the optimization procedure are the
desired ones.

##  V Experiments

Figure 2: UT-Automata F1Tenth Robot Car

To evaluate the performance of our proposed Optim-FKD approach, we perform two
sets of experiments, each involving a different variant of the robot control
problem. We demonstrate the ability of Optim-FKD to successfully complete
both, and show improved performance over an optimization-free, IKD model
baseline [33].

###  V-A Experimental Setup

(a) Optim-FKD on rounded rectangle

(b) IKD on rounded rectangle

(c) Optim-FKD on figure 8

(d) IKD on figure 8

Figure 3: Superposition of execution traces of both the Optim-FKD and IKD
algorithms running at different speeds on the rounded rectangle and figure 8
paths. In (a) and (c), which correspond to Optim-FKD, the execution trace
matches the desired path (black) very closely. In contrast, there are
significant errors in the baseline IKD model shown in (b) and (d).

(a) Rounded rectangle path

(b) Figure 8 path

Figure 4: Average Hausdorff distances between the executed paths and the
desired paths across different speeds. Each speed’s average is computed for
five rollouts.

The robot platform used for our experimentation is the UT-Automata F1tenth Car
depicted in figure 2. We make use of the car’s Intel Realsense T265 tracking
camera for localization and its Nvidia Jetson TX2 for compute.

We consider two different experimental setups, each involving a different form
of the robot control problem and thus a different optimization objective. For
the first task we consider having the robot follow two different predefined
paths as accurately as possible at speeds varying from
$1.0\leavevmode\nobreak\ m/s$ to $3.0\leavevmode\nobreak\ m/s$. This task
corresponds to the optimization objective presented in equation 8. For the
second task we have the robot traverse from an initial starting state to a
goal state in as little time as possible, without explicitly constraining
which path it needs to take to reach the goal state. This task corresponds to
the optimization objective presented in Eq. 10.

The baseline algorithm we compare against in all of the experiments is an
inverse kinodynamic model. In contrary to the forward kinodynamic model, which
maps a sequence of controls to the most likely next sequence of states, the
inverse kinodynamic model attempts to infer what controls lead to a particular
state sequence. To ensure fairness in the comparison, the underlying
feedforward neural network used in both the FKD and IKD models was identical,
with the same architecture ($6$ layers, $256$ neurons per layer) and
activations (ReLU); the dataset that both were trained on was identical; and
finally the number of iterations each was trained for was identical.

###  V-B Experiment 1: Path Following

Here we assess the control capabilities of the Optim-FKD model on the task of
accurately following a path. Figure 3 shows a visualization in black of the
two paths used: a rounded rectangle path and a figure 8 path. Both paths were
generated by joysticking the robot at a slow speed and recording only the
position estimates. Each algorithm is assessed on how accurately it can follow
the paths both at slower and higher speeds. For each speed, five full
traversals through the desired path are completed. We measure an algorithm’s
ability to closely follow a path at a particular speed by computing the
Hausdorff distance between the executed path and the desired path.

Figure 4 shows the evaluation of the Hausdorff distance metric for (a) the
rounded rectangle path and (b) the figure 8 path. Optim-FKD produces execution
traces that are significantly closer to the desired trajectories than the IKD
model. For both paths, the Optim-FKD algorithm is strictly under $0.2m$
Hausdorff distance, while the IKD algorithm is strictly greater than $0.2m$.
Figure 3 provides a visual argument for these results: in (a) and (c) the
executed path hugs the desired path much more closely than in (b) and (d). For
both algorithms and across both paths, we observe that increases in velocity
tend to result in higher Hausdorff distances. This is expected, since at high
velocities the kinodynamic responses of the robot with respect to the terrain
become more pronounced, making accurate control more difficult.

We posit that the better performance of the Optim-FKD approach is explained by
an inherent advantage in FKD models over IKD models: lesser impact of domain
shift. Most robots where kinodynamic interactions with the environment are
important are underactuated, i.e., the dimensionality of the control space is
smaller than that of the state space. For a FKD model and an IKD model trained
on the same dataset, the FKD model will learn a mapping from a lower
dimensional space (the control space) to a higher dimensional space (the state
space), whereas the IKD model learns the opposite mapping. During training,
both the FKD model and the IKD model will be exposed to a limited subset of
their input spaces - namely the space of teleoperable control inputs and the
space of kinodynamically feasible state sequences. However when deployed, it
is not guaranteed that the FKD model will receive control inputs
characteristic of those during training, or the IKD model will be given
kinodynamically feasible trajectories. Both models will suffer a domain shift,
but the impact will be less for the FKD model. With the control space’s lower
dimensionality, the same quantity of training data will allow for the FKD
model to have higher input space coverage than the IKD model, meaning there is
less of a domain difference between train and test data.

###  V-C Experiment 2: Optimal Connectivity

Figure 5: Paths produced by Optim-FKD, a human, and the IKD model following
the human’s path. Objective is to reach from the start (green) state at
$(0,0)$ to the goal (red) state at $(-5,0)$ in minimum time. The start and end
velocities are constrained to be $0\leavevmode\nobreak\ \frac{m}{s}$, the
start heading is $\frac{\pi}{2}\leavevmode\nobreak\ rad$, and the end heading
must be $\frac{3\pi}{2}\leavevmode\nobreak\ rad.$

In this experiment, we assess the generalizability of Optim-FKD compared to
the IKD approach. As demonstrated in section II-C, the optimal connectivity
problem can be solved by Optim-FKD simply by altering the optimization
objective. However, solving the same problem with the IKD approach requires
the retraining of a new IKD model. This is because the IKD model has a fixed
prediction horizon. Given a nearby state, it can output what controls to
execute during this prediction horizon to reach the goal state. However in the
general optimal connectivity problem, it may be possible that the goal state
cannot be reached within the IKD model’s prediction horizon, simply because it
is too far away. Thus multiple evaluations of the IKD model would be
necessary. But to do any one evaluation, the IKD model must know what the
desired state is at time equal to its prediction horizon. This effectively
means that the IKD model needs to be supplied with the so called racing lines,
i.e., a user-specified path to take to reach the goal state. However, user-
specified racing lines are prone to being suboptimal.

Figure 5 depicts the problem of reaching the goal position $(-5,0)$ from the
start position $(0,0)$ in as little time as possible. Since, this task cannot
be completed within the duration of the IKD model’s prediction horizon, racing
lines shown in yellow are provided by a human demonstrator. The IKD model
attempts to trace the racing line as fast as possible shown. In contrast,
Optim-FKD runs the optimization described in Eq. 10, and is thus not dependent
on racing lines nor requires a new FKD model to be trained.

Using Optim-FKD, the robot is able to reach the goal state in
$3.3\leavevmode\nobreak\ s$, whereas it takes $6.3\leavevmode\nobreak\ s$ for
the robot to execute the path when using the IKD model. Thus Optim-FKD is able
to find a more optimal path than the human provided one. Figure 5 depicts the
path taken by the robot under each navigation algorithm. Using Optim-FKD the
robot makes a wider turn, which allows it to reach velocities higher than
$3\leavevmode\nobreak\ \frac{m}{s}$ whereas the maximum linear velocity of the
robot when using the IKD model is $2.6\frac{m}{s}$

##  VI Conclusion

In this work we presented Optim-FKD, a new approach for accurate, high-speed
robot control. We showed that solutions to various formulations of the robot
control problem can be naturally expressed as a nonlinear least squares
optimization with a FKD model. We demonstrate how such an FKD model can be
learned effectively and integrated with the optimization on real robot
hardware. Finally we evaluate our proposed approach on two robotic control
tasks at high speeds and show that it outperforms the baseline.

## ACKNOWLEDGMENT

This work has taken place in the Autonomous Mobile Robotics Laboratory (AMRL)
at UT Austin. AMRL research is supported in part by NSF (CAREER-2046955,
IIS-1954778, SHF-2006404), ARO (W911NF-19-2-0333, W911NF-21-20217), DARPA
(HR001120C0031), Amazon, JP Morgan, and Northrop Grumman Mission Systems. The
views and conclusions contained in this document are those of the authors
alone.

## References

  * [1] A. Tahirovic, G. Magnani, and P. Rocco, “Mobile robot navigation using passivity-based mpc,” in _2010 IEEE/ASME International Conference on Advanced Intelligent Mechatronics_. IEEE, 2010, pp. 483–488.
  * [2] J. J. Park, C. Johnson, and B. Kuipers, “Robot navigation with model predictive equilibrium point control,” in _2012 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2012, pp. 4945–4952.
  * [3] J. Lorenzetti, B. Landry, S. Singh, and M. Pavone, “Reduced order model predictive control for setpoint tracking,” in _2019 18th European Control Conference (ECC)_. IEEE, 2019, pp. 299–306.
  * [4] X. Xiao, B. Liu, G. Warnell, and P. Stone, “Motion control for mobile robot navigation using machine learning: a survey,” _Autonomous Robots_ , 2022\.
  * [5] M. Bojarski, D. Del Testa, D. Dworakowski, B. Firner, B. Flepp, P. Goyal, L. D. Jackel, M. Monfort, U. Muller, J. Zhang _et al._ , “End to end learning for self-driving cars,” _arXiv preprint arXiv:1604.07316_ , 2016.
  * [6] L. Tai, S. Li, and M. Liu, “A deep-network solution towards model-less obstacle avoidance,” in _2016 IEEE/RSJ international conference on intelligent robots and systems (IROS)_. IEEE, 2016, pp. 2759–2764.
  * [7] M. Pfeiffer, M. Schaeuble, J. Nieto, R. Siegwart, and C. Cadena, “From perception to decision: A data-driven approach to end-to-end motion planning for autonomous ground robots,” in _2017 ieee international conference on robotics and automation (icra)_. IEEE, 2017, pp. 1527–1533.
  * [8] Y. Pan, C.-A. Cheng, K. Saigol, K. Lee, X. Yan, E. A. Theodorou, and B. Boots, “Imitation learning for agile autonomous driving,” _The International Journal of Robotics Research_ , vol. 39, no. 2-3, pp. 286–302, 2020.
  * [9] X. Xiao, B. Liu, G. Warnell, and P. Stone, “Toward agile maneuvers in highly constrained spaces: Learning from hallucination,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 1503–1510, 2021.
  * [10] X. Xiao, B. Liu, and P. Stone, “Agile robot navigation through hallucinated learning and sober deployment,” in _2021 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2021, pp. 7316–7322.
  * [11] Z. Wang, X. Xiao, A. J. Nettekoven, K. Umasankar, A. Singh, S. Bommakanti, U. Topcu, and P. Stone, “From agile ground to aerial navigation: Learning from learned hallucination,” in _2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2021, pp. 148–153.
  * [12] H. Karnan, G. Warnell, X. Xiao, and P. Stone, “Voila: Visual-observation-only imitation learning for autonomous navigation,” 2021. [Online]. Available: https://arxiv.org/abs/2105.09371
  * [13] L. Tai, G. Paolo, and M. Liu, “Virtual-to-real deep reinforcement learning: Continuous control of mobile robots for mapless navigation,” in _2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2017, pp. 31–36.
  * [14] M. Pfeiffer, S. Shukla, M. Turchetta, C. Cadena, A. Krause, R. Siegwart, and J. Nieto, “Reinforced imitation: Sample efficient deep reinforcement learning for mapless navigation by leveraging prior demonstrations,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 4, pp. 4423–4430, 2018\.
  * [15] H.-T. L. Chiang, A. Faust, M. Fiser, and A. Francis, “Learning navigation behaviors end-to-end with autorl,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 2, pp. 2007–2014, 2019.
  * [16] Y. F. Chen, M. Liu, M. Everett, and J. P. How, “Decentralized non-communicating multiagent collision avoidance with deep reinforcement learning,” in _2017 IEEE international conference on robotics and automation (ICRA)_. IEEE, 2017, pp. 285–292.
  * [17] M. Wigness, J. G. Rogers, and L. E. Navarro-Serment, “Robot navigation from human demonstration: Learning control behaviors,” in _2018 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2018, pp. 1150–1157.
  * [18] K. S. Sikand, S. Rabiee, A. Uccello, X. Xiao, G. Warnell, and J. Biswas, “Visual representation learning for preference-aware path planning,” in _2022 ieee international conference on robotics and automation (icra)_. IEEE, 2022.
  * [19] B. Kim and J. Pineau, “Socially adaptive path planning in human environments using inverse reinforcement learning,” _International Journal of Social Robotics_ , vol. 8, no. 1, pp. 51–66, 2016.
  * [20] H. Kretzschmar, M. Spies, C. Sprunk, and W. Burgard, “Socially compliant mobile robot navigation via inverse reinforcement learning,” _The International Journal of Robotics Research_ , vol. 35, no. 11, pp. 1289–1307, 2016\.
  * [21] H. Karnan, A. Nair, X. Xiao, G. Warnell, S. Pirk, A. Toshev, J. Hart, J. Biswas, and P. Stone, “Socially compliant navigation dataset (scand): A large-scale dataset of demonstrations for social navigation,” 2022. [Online]. Available: https://arxiv.org/abs/2203.15041
  * [22] X. Xiao, Z. Wang, Z. Xu, B. Liu, G. Warnell, G. Dhamankar, A. Nair, and P. Stone, “Appl: Adaptive planner parameter learning,” _arXiv preprint arXiv:2105.07620_ , 2021.
  * [23] X. Xiao, B. Liu, G. Warnell, J. Fink, and P. Stone, “Appld: Adaptive planner parameter learning from demonstration,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 3, pp. 4541–4547, 2020.
  * [24] Z. Wang, X. Xiao, B. Liu, G. Warnell, and P. Stone, “Appli: Adaptive planner parameter learning from interventions,” in _2021 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2021, pp. 6079–6085.
  * [25] Z. Wang, X. Xiao, G. Warnell, and P. Stone, “Apple: Adaptive planner parameter learning from evaluative feedback,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 4, pp. 7744–7749, 2021.
  * [26] Z. Xu, G. Dhamankar, A. Nair, X. Xiao, G. Warnell, B. Liu, Z. Wang, and P. Stone, “Applr: Adaptive planner parameter learning from reinforcement,” in _2021 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2021, pp. 6086–6092.
  * [27] J. Betz, H. Zheng, A. Liniger, U. Rosolia, P. Karle, M. Behl, V. Krovi, and R. Mangharam, “Autonomous vehicles on the edge: A survey on autonomous vehicle racing,” _arXiv preprint arXiv:2202.07008_ , 2022.
  * [28] H. Karnan, K. S. Sikand, P. Atreya, S. Rabiee, X. Xiao, G. Warnell, P. Stone, and J. Biswas, “Vi-ikd: High-speed accurate off-road navigation using learned visual-inertial inverse kinodynamics,” 2022. [Online]. Available: https://arxiv.org/abs/2203.15983
  * [29] G. Williams, A. Aldrich, and E. A. Theodorou, “Model predictive path integral control: From theory to parallel computation,” _Journal of Guidance, Control, and Dynamics_ , vol. 40, no. 2, pp. 344–357, 2017.
  * [30] G. Williams, N. Wagener, B. Goldfain, P. Drews, J. M. Rehg, B. Boots, and E. A. Theodorou, “Information theoretic mpc for model-based reinforcement learning,” in _2017 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2017, pp. 1714–1721.
  * [31] G. Williams, P. Drews, B. Goldfain, J. M. Rehg, and E. A. Theodorou, “Aggressive driving with model predictive path integral control,” in _2016 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2016, pp. 1433–1440.
  * [32] A. Brunnbauer, L. Berducci, A. Brandstätter, M. Lechner, R. Hasani, D. Rus, and R. Grosu, “Model-based versus model-free deep reinforcement learning for autonomous racing cars,” _arXiv preprint arXiv:2103.04909_ , 2021.
  * [33] X. Xiao, J. Biswas, and P. Stone, “Learning inverse kinodynamics for accurate high-speed off-road navigation on unstructured terrain,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 3, pp. 6054–6060, 2021.
  * [34] S. Piche, B. Sayyar-Rodsari, D. Johnson, and M. Gerules, “Nonlinear model predictive control using neural networks,” _IEEE Control Systems Magazine_ , vol. 20, no. 3, pp. 53–62, 2000.
  * [35] J. Kabzan, L. Hewing, A. Liniger, and M. N. Zeilinger, “Learning-based model predictive control for autonomous racing,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 4, pp. 3363–3370, 2019.
  * [36] L. Hewing, K. P. Wabersich, M. Menner, and M. N. Zeilinger, “Learning-based model predictive control: Toward safe learning in control,” _Annual Review of Control, Robotics, and Autonomous Systems_ , vol. 3, pp. 269–296, 2020\.
  * [37] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” _The international journal of robotics research_ , vol. 30, no. 7, pp. 846–894, 2011.
  * [38] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Batch informed trees (bit*): Sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs,” in _2015 IEEE international conference on robotics and automation (ICRA)_. IEEE, 2015, pp. 3067–3074.
  * [39] P. Atreya and J. Biswas, “State supervised steering function for sampling-based kinodynamic planning.” in _AAMAS_ , 2022, pp. 35–43.