# Image-Based Trajectory Tracking through Unknown Environments without
Absolute Positioning

Shiyu Feng1,†, Zixuan Wu2,†, Yipu Zhao3 and Patricio A. Vela2 *This work
supported in part by NSF Award #1849333.$\dagger$ Equal contribution1S. Feng
is with the School of Mechanical Engineering and the School of Electrical and
Computer Engineering, Georgia Institute of Technology, Atlanta, GA 30308, USA.
<EMAIL_ADDRESS>Wu and P.A. Vela are with the School of Electrical and
Computer Engineering and the Institute for Robotics and Intelligent Machines,
Georgia Institute of Technology, Atlanta, GA 30308, USA. {zwu380,
<EMAIL_ADDRESS>Zhao is with Meta Reality Lab, Redmond, USA.
<EMAIL_ADDRESS>
###### Abstract

This paper describes a stereo image-based visual servoing system for
trajectory tracking by a non-holonomic robot without externally derived pose
information nor a known visual map of the environment. It is called trajectory
servoing. The critical component is a feature-based, indirect Simultaneous
Localization And Mapping (SLAM) method to provide a pool of available features
with estimated depth, so that they may be propagated forward in time to
generate image feature trajectories for visual servoing. Short and long
distance experiments show the benefits of trajectory servoing for navigating
unknown areas without absolute positioning. Empirically, trajectory servoing
has better trajectory tracking performance than pose-based feedback when both
rely on the same underlying SLAM system.

##  I Introduction

Navigation systems with real-time needs often employ hierarchical schemes that
decompose navigation across multiple spatial and temporal scales. Doing so
improves real-time responsiveness to novel information gained from sensors,
while being guided by the more slowly evolving global path. At the lowest
level of the hierarchy lies trajectory tracking to realize the planned paths
or synthesized trajectories. In the absence of an absolute reference (such as
GPS) and of an accurate map of the environment, only on-board mechanisms can
support trajectory-tracking. These include odometry through proprioceptive
sensors (wheel encoders, IMUs, etc.) or visual sensors. Pose estimation from
proprioceptive sensors is not observable, thus visual sensors provide the best
mechanism to anchor the robot’s pose estimate to external, static reference
locations. Indeed visual odometry (VO) or visual SLAM (V-SLAM) solutions are
essential in these circumstances. However, VO/V-SLAM estimation error and pose
drift degrade trajectory tracking performance. Is it possible to do better?

The hypothesis explored in this paper is that performing trajectory tracking
in the image domain reduces the trajectory tracking error of systems reliant
on VO or V-SLAM pose estimates for feedback. Trajectory tracking feedback
shifts from pose space to perception space. Perception space approaches have
several favorable properties when used for navigation [1, 2]. Shifting the
representation from being world-centric to being viewer-centric reduces
computational demands and improves run-time properties. For trajectory
tracking without reliable absolute pose information, simplifying the feedback
pathway by skipping processes that degrade performance of–or are not essential
to–the local tracking task may have positive benefits. Using image
measurements to control motion relative to visual landmarks is known as visual
servoing. Thus, the objective is to explore the use of image-based visual
servoing for long distance trajectory tracking with a stereo camera as the
primary sensor and without absolute positioning. The technique, which we call
trajectory servoing, will be shown to improve trajectory tracking over systems
reliant on V-SLAM for pose-based feedback. In this paper, a trajectory is a
time parametrized curve in Cartesian space.

###  I-A Related Work

####  I-A1 Visual Servoing

Visual servoing (VS) has a rich history and a diverse set of strategies for
stabilizing a camera to a target pose described visually. VS algorithms fit
into one of two categories: image-based visual servoing (IBVS) and position-
based visual servoing (PBVS)[3, 4]. IBVS implementations include both feature
stabilization and feature trajectory tracking [4, 5]. IBVS emphasizes point-
to-point reconfiguration given a terminal image state [6]. It requires
artificial markers and co-visibility of image measurements during motion [7],
which may not be satisfied for long-distance displacements. Furthermore, there
is no guarantee on the path taken since the feature space trajectory has a
nonlinear relationship with the Cartesian space trajectory. Identifying a
feature path to track based on a target Cartesian space trajectory requires
mapping the target positions into the image frame over time to generate the
feature trajectory [5].

The target application is trajectory tracking for a mobile robot with IBVS.
Both holonomic [8] and non-holonomic [9, 10, 11, 12, 13, 14] mobile robots
have been studied as candidates for visual servoing. Most approaches do not
use the full IBVS equations involving the image Jacobian. The centroid of the
features [9, 13], the most frequent horizontal displacement of the matched
feature pairs [10] or other qualitative cost functions [8] are used to
generate [9] or correct [10] the feedforward angular velocity of mobile robot.
These simplifications work well in circumstances tolerant to high tracking
inaccuracy (e.g. an outdoor, open field navigation). In this paper, we use
more precise velocity relations between the robot and feature motion to
generate a feedback control signal for exact tracking similar to [11]. That
work studied the path following problem with a visible path marker line, which
does not hold here.

####  I-A2 Visual Teach and Repeat

Evidence that visual features can support trajectory tracking or consistent
navigation through space lies in the Visual Teach and Repeat (VTR) navigation
problem in robotics [9, 10]. Given data or recordings of prior paths through
an environment, robots can reliably retrace past trajectories. The teaching
phase of VTR creates a visual map that contains features associated with robot
poses obtained from visual odometry [9, 15, 16, 17, 18]. Extensions include
real-time construction of the VTR data structure during the teaching process,
and the maintenance and updating of the VTR data during repeat runs [16, 15].
Feature descriptor improvements make the feature matching more robust to the
environment changes [18, 19]. Visual data in the form of feature points can
have task relevant and irrelevant features, which provide VTR algorithms an
opportunity to select a subset that best contributes to the localization or
path following task [15, 17]. It is difficult to construct or update a visual
map in real-time while in motion due to the separation of the teach and repeat
phases. In addition, VTR focuses more on local map consistency and does not
work toward global pose estimation [17] compared with SLAM since the
navigation problems it solves are usually defined in the local frame.

Another type of VTR uses the optical flow [10, 20] or feature sequences [21,
22, 23] along the trajectory, which is then encoded into a VTR data structure
and control algorithm in the teaching phase. Although similar to visual
servoing, the system is largely over-determined. It can tolerate feature
tracking failure, compared with traditional visual servo system, but may lead
to discontinuities [24]. Though this method handles long trajectories, and may
be supplemented from new teach recordings, it can only track paths through
visited space.

####  I-A3 Navigation Using Visual SLAM

Visual Simultaneous Localization And Mapping (V-SLAM) systems estimate the
robot’s trajectory and world structure as the robot moves through space [25].
SLAM derived pose estimation naturally supports navigation or path following
[26, 27], and PBVS [28, 29]. Most of these methods still need to initialize
and maintain topological or metric visual maps for path planning [26, 27] or
for localizing the robot for PBVS [28, 29]. These works do not involve
Cartesian trajectories [26, 28, 29] nor provide solutions to occlusion and
tracking loss problems [26, 27, 28, 29]. The shortcomings imply the inability
to track long Cartesian trajectories through unknown environments.

Pose estimation accuracy of SLAM is a major area of study [30, 31, 32, 33,
34]. However, most studies only test under open-loop conditions [30, 31, 32,
34], i.e., they analyze the pose estimation difference with the ground truth
trajectory and do not consider the error induced when the estimated pose
informs feedback control. More recently, closed-loop evaluation of V-SLAM
algorithms as part of trajectory tracking feedback control or navigation were
tested for individual SLAM systems [35, 36, 37] or across different systems
[38]. The studies exposed the influence of V-SLAM estimation drift and latency
on pose-based feedback control performance. This paper builds upon an existing
closed-loop benchmarking framework [38] and shows improved tracking
performance by trajectory servoing.

###  I-B Contribution

Both IBVS and VTR depend on reliably tracked, known features within the field-
of-view, which can only be achieved by artificial, task-oriented scenarios [5,
11] or a pre-built trajectory map [9, 15, 16, 17, 18]. This limitation does
not permit travel through unknown environments to an unseen terminal state,
and motivates the use of feature-based V-SLAM systems with online map saving
[26, 27]. Compared with PBVS based on SLAM [28, 29], trajectory servoing uses
IBVS to less frequently query the pose from a stereo V-SLAM system [30],
thereby attenuating the impact of estimation drift and error on trajectory
tracking performance.

Trajectory servoing bypasses the explicit use of VO/V-SLAM pose, whose
estimates are vulnerable to image-driven uncertainty which manifests as pose
error or drift [3, 4], by relying on the V-SLAM feature maintenance components
that provide accurate and robust feature tracking and mapping. We present
evidence for the assertion that the coupling between V-SLAM and IBVS combines
their advantages to provide more effective feedback signals relative to V-SLAM
pose-based control. Simulation and real experiment performance benchmarking
show that trajectory servoing improves trajectory tracking performance over
pose-based feedback using SLAM estimates, thereby mitigating the effect of
pose estimation error or drift, even though the same visual information is
used for closing the loop. The beneficial coupling is the motivation behind
the trajectory servoing system design. It is a promising approach to
trajectory tracking through unknown environments in the absence of absolute
positioning signals.

###  I-C Image-Based Visual Servoing Rate Equations

The core algorithm builds on IBVS [39]. This section covers IBVS with an
emphasis on how it relates the velocity of image features to the robot
velocities via the image Jacobian [3, 4]. These equations will inform the
trajectory tracking problem under non-holonomic robot motion. We use the more
modern notation from geometric mechanics [40] since it provides equations that
better connect to contemporary geometric control and to SLAM formulations for
moving rigid bodies.

####  I-C1 Non-Holonomic Robot and Camera Kinematic Models

Let the motion model of the robot be a kinematic Hilare robot model where the
pose state ${g}^{{\mathcal{W}}}_{{\mathcal{R}}}\in SE(2)$ evolves under the
control $\boldsymbol{u}=[\nu,\omega]^{T}$ as

${\dot{g}}^{{\mathcal{W}}}_{{\mathcal{R}}}={g}^{{\mathcal{W}}}_{{\mathcal{R}}}\cdot\begin{bmatrix}1&0\\\
0&0\\\ 0&1\end{bmatrix}\begin{bmatrix}\nu\\\
\omega\end{bmatrix}={g}^{{\mathcal{W}}}_{{\mathcal{R}}}\cdot\xi_{\boldsymbol{u}},$
(1)

for $\nu$ a forward linear velocity and $\omega$ an angular velocity, and
$\xi_{\boldsymbol{u}}\in\mathfrak{se}(2)$. The state is the robot frame
${\mathcal{R}}$ relative to the world frame ${\mathcal{W}}$. The camera frame
${\mathcal{C}}$ is presumed to be described as
${h}^{{\mathcal{R}}}_{{\mathcal{C}}}$ relative to the robot frame.
Consequently, camera kinematics relative to the world frame are

${\dot{h}}^{{\mathcal{W}}}_{{\mathcal{C}}}={g}^{{\mathcal{W}}}_{{\mathcal{R}}}\cdot{h}^{{\mathcal{R}}}_{{\mathcal{C}}}\cdot{\textrm{Ad}}^{-1}_{{h}^{{\mathcal{R}}}_{{\mathcal{C}}}}\cdot\begin{bmatrix}1&0\\\
0&0\\\ 0&1\end{bmatrix}\begin{bmatrix}\nu\\\
\omega\end{bmatrix}={g}^{{\mathcal{W}}}_{{\mathcal{R}}}\cdot{h}^{{\mathcal{R}}}_{{\mathcal{C}}}\cdot\zeta_{\boldsymbol{u}},$
(2)

with $\zeta_{\boldsymbol{u}}\in\mathfrak{se}(2)$. Now, let the camera
projection equations be given by the function
$\boldsymbol{H}:\mathbb{R}^{3}\rightarrow\mathbb{R}^{2}$ such that a point
${q}^{{\mathcal{W}}}$ projects to the camera point
$r=\boldsymbol{H}\circ{h}^{{\mathcal{C}}}_{{\mathcal{W}}}({q}^{{\mathcal{W}}})$.
Under camera motion, the differential equation relating the projected point to
the camera velocity for a static point ${q}^{{\mathcal{W}}}$ is

$\dot{r}=\text{D}\boldsymbol{H}({q}^{{\mathcal{C}}})\cdot\left(\zeta_{\boldsymbol{u}}\cdot{q}^{{\mathcal{C}}}\right),\quad\text{for}\
{q}^{{\mathcal{C}}}={h}^{{\mathcal{C}}}_{{\mathcal{W}}}{q}^{{\mathcal{W}}},$
(3)

where D is the differential operator. Since the operation $\zeta\cdot{q}$ is
linear for $\zeta\in\mathfrak{se}(2)$, ${q}\in\mathbb{R}^{3}$, it can be
written as a matrix-vector product $\boldsymbol{M}({q})\zeta$ leading to,

$\dot{r}=\text{D}\boldsymbol{H}({q}^{{\mathcal{C}}})\cdot\boldsymbol{M}({q}^{{\mathcal{C}}})\zeta_{\boldsymbol{u}}=\boldsymbol{\mathcal{L}}({q}^{{\mathcal{C}}})\zeta_{\boldsymbol{u}},$
(4)

where $\boldsymbol{\mathcal{L}}:\mathbb{R}^{3}\times\mathfrak{se}(2)$ is the
Image Jacobian. Given the point and projection pair
$({q},r)\in\mathbb{R}^{3}\times\mathbb{R}^{2}$, $\boldsymbol{\mathcal{L}}$
works out to be

$\boldsymbol{\mathcal{L}}({q})=\boldsymbol{\mathcal{L}}({q},r)=\begin{bmatrix}-\frac{f}{{q}^{3}}&0&r^{2}\\\\[1.0pt]
0&-\frac{f}{{q}^{3}}&-r^{1}\end{bmatrix},$ (5)

where $f$ is the focal length. Recall that $r=\boldsymbol{H}({q})$. Re-
expressing $\boldsymbol{\mathcal{L}}$ as a function of $({q},r)$ simplifies
its written form, and exposes what information is available from the image
directly $r\in\mathbb{R}^{2}$ and what additional information must be known to
compute it: coordinate ${q}^{3}$ from ${q}^{{\mathcal{C}}}\in\mathbb{R}^{3}$
in the camera frame, which is also called depth. With a stereo camera, the
depth value is triangulated. The next section will use these equations for
trajectory tracking with image features.

V-SLAM System Time-varying Trajectory Feature Trajectory Generation Trajectory
Servoing Core Feature Trajectory Regeneration Robot When the number of
features is too low Run once at the start Tracked Features Estimated Poses
Commands Short Distance Trajectory Servoing Long Distance Addition High
Frequency Low Frequency

Figure 1: A trajectory servoing system has two major components. One (red)
steers the robot to track short paths, while the other (blue) ensures the
sufficiency of features to use by querying a V-SLAM module. The entire system
is used when tracking long distance trajectories. Solid arrows indicate high
frequency data passing, and dashed arrows low frequency. All blue arrows
represent the information flow related to long distance addition.

##  II Trajectory Servoing

###  II-A System Overview

The algorithmic components and information flow of a trajectory servoing
system are depicted in Fig. 1, and consist of two major components. The first
one, described in §II, is a trajectory servoing system for a set of world
points and specified trajectories. These points are obtained from the V-SLAM
system as well as tracked over time. It is capable of guiding a mobile robot
along short paths. The second component, described in §III, supervises the
core trajectory servoing system to confirm that it has sufficient features
from the feature pool to operate. Should this quantity dip too low, the
supervisor queries the V-SLAM module for additional features and the robot
pose to build new feature tracks.

###  II-B Trajectory Servoing

This section describes the basic trajectory servoing implementation.
Traditional controllers are designed in Cartesian space where V-SLAM is a pose
observer (also called visual odometry). For short distance navigation, where
sufficient image features remain within the field-of-view (FoV) over the
trajectory, we shift the problem to image space and solve using IBVS by
synthesizing a desired feature trajectory that defines what the camera should
see over time from an initial view.

$\boldsymbol{u}(t_{i})$$\boldsymbol{u}(t_{i+1})$$t_{0}$$t_{i}$$t_{i+1}$${g}^{{\mathcal{W,*}}}_{{\mathcal{R}}}(t)$$\boldsymbol{S}^{*}(t)$$\boldsymbol{S}(t)$${g}^{{\mathcal{W}}}_{{\mathcal{R}}}(t)$$\boldsymbol{H}\circ{h}^{{\mathcal{C}}}_{{\mathcal{W}}}(t_{i})$$\boldsymbol{H}\circ{h}^{{\mathcal{C}}}_{{\mathcal{W}}}(t_{i+1})$$\boldsymbol{Q}^{{\mathcal{W}}}$
Desired Actual Pose Feature Feature Pose

Figure 2: The trajectory servoing process uses matches from
$\boldsymbol{S}^{*}$ to $\boldsymbol{S}$ to define the control
$\boldsymbol{u}$, where $\boldsymbol{S}^{*}(t)$ is computed from the desired
trajectory ${g}^{{\mathcal{W,*}}}_{{\mathcal{R}}}(t)$.

The standard IBVS equations presented in §I-C typically apply to tracked
features with known static positions in the world (relative to some frame
attached to these positions). As described in §I-A1 and §I-A2, a visual map or
visible targets are usually necessary. The prerequisites needed for stable
tracking and depth recovery of features are major challenges regarding the use
of visual servoing in unknown environments. Fortunately, they are all
realizable based on information and modules available within mobile robot
autonomy stacks.

Trajectory servoing requirements condense down to the following: 1) A set of
image points, $\boldsymbol{S}^{*}(t_{0})$, with known (relative) positions; 2)
A given trajectory and control signal for the robot starting at the robot’s
current pose or nearby, ${g}^{{\mathcal{W}}}_{{\mathcal{R}}}(t_{0})$; and 3)
The ability to index and associate the image points across future image
measurements, $\boldsymbol{S}^{*}(t)\leftrightarrow\boldsymbol{S}(t)$, when
tracking the trajectory. In other words, it requires a mechanism to temporally
associate measured features along the entire trajectory. The trajectory
servoing process and variables are depicted in Fig. 2. The autonomy modules
contributing this information are the navigation and V-SLAM stacks. The
navigation stack generates a trajectory to follow. An indirect, feature-based
V-SLAM stack keeps track of points in the local environment, links them to
previously observed visual features, and estimates their actual positions
relative to the robot.

####  II-B1 Trajectory and control signals

Define
$\boldsymbol{S}=\left\\{\,r_{i}\,\right\\}_{1}^{n_{\text{F}}}\subset\mathbb{R}^{2}$
as a set of image points in the current camera image, sourced from the set
$\boldsymbol{Q}=\left\\{\,{q}_{i}^{{\mathcal{W}}}\,\right\\}_{1}^{n_{\text{F}}}\subset\mathbb{R}^{3}$
of points in the world frame. Suppose that the robot should attain a future
pose given by $g^{*}$, for which the points in $\boldsymbol{Q}$ will project
to the image coordinates
$\boldsymbol{S}^{*}=\boldsymbol{H}\circ(g^{*}{h}^{{\mathcal{R}}}_{{\mathcal{C}}})^{-1}(\boldsymbol{Q})$.
For simplicity, ignore field of view issues and occlusions between points.
Their effect would be such that only a subset of the points in
$\boldsymbol{Q}$ would contribute to visual servoing.

Assume that a specific short-duration path has been established as the one to
follow, and has been converted into a path relative to the robot’s local
frame. It either contains the current robot pose in it, or has a nearby pose.
Contemporary navigation stacks have a means to synthesize both a time-varying
trajectory and an associated control signal from the paths. Here, we apply a
standard trajectory tracking controller [41] to generate
$\xi_{\boldsymbol{u}}^{*}(t)$ and ${g}^{{\mathcal{W,*}}}_{{\mathcal{R}}}(t)$
by forward simulating (1); note that $\xi_{\boldsymbol{u}}^{*}$ contains the
linear velocity $\nu^{*}$ and angular velocity $\omega^{*}$. Some navigation
stacks use optimal control synthesis to build the trajectory. Either way, the
generated trajectory is achievable by the robot.

In the time-varying trajectory tracking case, we assume that a trajectory
reference ${h}^{{\mathcal{W}}}_{{\mathcal{C}}}(t)$ exists along with a control
signal $\boldsymbol{u}^{*}(t)$ satisfying (2). It would typically be derived
from a robot trajectory reference ${g}^{{\mathcal{W}}}_{{\mathcal{R}}}(t)$ and
control signal $\boldsymbol{u}^{*}(t)$ satisfying (1). Using those time-
varying functions, the equations in (2) are solved to obtain the image
coordinate trajectories. Written in short-hand to expose only the main
variables, the forward integrated feature trajectory $\boldsymbol{S}^{*}$ is:

$\begin{split}\dot{\boldsymbol{S}}^{*}&=\boldsymbol{\mathcal{L}}\circ{h}^{{\mathcal{C}}}_{{\mathcal{W}}}(t)(\boldsymbol{Q}^{{\mathcal{W}}})\cdot\zeta_{\boldsymbol{u}^{*}(t)},\
\text{with}\\\
\boldsymbol{S}^{*}(0)&=\boldsymbol{H}\circ{h}^{{\mathcal{C}}}_{{\mathcal{W}}}(0)(\boldsymbol{Q}^{{\mathcal{W}}}).\end{split}$
(6)

It will lead to a realizable visual servoing problem where $\nu^{*}$,
$\omega^{*}$, and $\boldsymbol{S}^{*}(t)$ are consistent with each other. The
equations will require converting the reference robot trajectory to a camera
trajectory ${h}^{{\mathcal{W,*}}}_{{\mathcal{C}}}(t)$ using
${\textrm{Ad}}^{-1}_{{h}^{{\mathcal{R}}}_{{\mathcal{C}}}}$.

####  II-B2 Features and feature paths

The V-SLAM module provides a pool of visible features with known relative
position for the current stereo frame, plus a means to assess future
visibility if desired. Taking this pool to define the feature set
$\boldsymbol{S}^{*}(0)$ gives the final piece of information needed to forward
integrate (6) and generate feature trajectories $\boldsymbol{S}^{*}(t)$ in the
left camera frame. This process acts like a short-term teach and repeat
feature trajectory planner but is simulate and repeat, for on-the-fly
generation of the repeat data.

A less involved module could be used besides a fully realized V-SLAM system,
however doing so would require creating many of the fundamental building
blocks of an indirect, feature-based V-SLAM system. Given the availability of
strong performing open-source, real-time V-SLAM methods, there is little need
to create a custom module. In addition, an extra benefit to tracked features
through V-SLAM system is that a feature map is maintained to retrieve same
reappeared features. As will be shown, this significantly improves the average
lifetime of features, especially compared to a simple frame by frame tracking
system without the feature map.

After the V-SLAM feature tracking process, we are already working with this
feasible set whereby the indexed elements in $\boldsymbol{S}$ correspond
exactly to their counterpart in $\boldsymbol{S}^{*}$ with the same index,
i.e., the sets are in correspondence.

####  II-B3 Trajectory Servoing Control

Define the error to be $\boldsymbol{E}=\boldsymbol{S}-\boldsymbol{S}^{*}$
where elements with matched indices are subtracted. The error dynamics of the
points are:

$\dot{\boldsymbol{E}}=\dot{\boldsymbol{S}}-\dot{\boldsymbol{S}}^{*}=\boldsymbol{\mathcal{L}}_{\boldsymbol{u}}({h}^{{\mathcal{C}}}_{{\mathcal{W}}}(\boldsymbol{Q}),\boldsymbol{S};{h}^{{\mathcal{R}}}_{{\mathcal{C}}})\cdot\boldsymbol{u}-\dot{\boldsymbol{S}}^{*},$
(7)

where we apply the same argument adjustment as in (5) so that dependence is on
image features then point coordinates as needed. Further, functions or
operations applied to indexed sets will return an indexed set whose elements
correspond to the input elements from the input indexed set. Since the desired
image coordinates $\boldsymbol{S}^{*}$ are not with respect to a static goal
pose but a dynamic feature trajectory, $\dot{\boldsymbol{S}}^{*}\neq 0$, see
(6). Define $\boldsymbol{e}$, $\boldsymbol{s}$, $\boldsymbol{s}^{*}$,
$\boldsymbol{q}$ and $\boldsymbol{L}$ to be the vectorized versions of
$\boldsymbol{E}$, $\boldsymbol{S}$, $\boldsymbol{S}^{*}$, $\boldsymbol{Q}$ and
$\boldsymbol{\mathcal{L}}$. Then,

$\dot{\boldsymbol{e}}=\boldsymbol{L}({h}^{{\mathcal{C}}}_{{\mathcal{W}}}(\boldsymbol{q}),\boldsymbol{s};{h}^{{\mathcal{R}}}_{{\mathcal{C}}})\cdot\boldsymbol{u}-\dot{\boldsymbol{s}}^{*}$
(8)

is an overdetermined set of equations for $\boldsymbol{u}$ when
$n_{\text{F}}\geq 2$.
$\boldsymbol{L}=[\boldsymbol{L}^{1},\boldsymbol{L}^{2}]\in\mathbb{R}^{2n_{\text{F}}\times
2}$. Removing the functional dependence and breaking apart the different
control contributions, the objective is to satisfy,

$\dot{\boldsymbol{e}}=\boldsymbol{L}\cdot\boldsymbol{u}-\dot{\boldsymbol{s}}^{*}=\boldsymbol{L}^{1}\nu+\boldsymbol{L}^{2}\omega-\dot{\boldsymbol{s}}^{*}=-\lambda\boldsymbol{e}.$
(9)

A least-squares solution establishes the angular rate feedback,

$\omega={\left(\boldsymbol{L}^{2}\right)}^{\dagger}\left(-\boldsymbol{L}^{1}\nu-\lambda\boldsymbol{e}+\dot{\boldsymbol{s}}^{*}\right),$
(10)

so that

$\dot{\boldsymbol{e}}=-\lambda\boldsymbol{e}+\Delta\boldsymbol{e},$ (11)

where $\Delta\boldsymbol{e}$ is mismatch between the true solution and the
computed pseudo-inverse solution. If the overdetermined linear system
equalities (9) are compatible and have a unique solution (10), then
$\Delta\boldsymbol{e}$ will vanish and the robot will achieve the target pose.
If $\Delta\boldsymbol{e}$ does not vanish, then there will be an error
(usually some fixed point $\boldsymbol{e}_{ss}\neq 0$). For mobile robots, it
is common to use the linear velocities from $\nu^{*}(t)$ of the given
trajectory [9, 10] for angular control (10). The decoupling provides
robustness to the motion imperceptibility problem that can affect
translational motion control [4, 42].

The vectorized form of (6) for $\boldsymbol{S}^{*}(t)$ is

$\displaystyle\dot{\boldsymbol{s}}^{*}=\boldsymbol{L}^{1}(\boldsymbol{q}^{{\mathcal{C}}^{*}}(t),\boldsymbol{s}^{*}(t))\nu^{*}{(t)}+\boldsymbol{L}^{2}(\boldsymbol{q}^{{\mathcal{C}}^{*}}(t),\boldsymbol{s}^{*}(t))\omega^{*}{(t)}.$
(12)

Continuing, the vectorized steering equation (10) leads to

$\omega={\left(\boldsymbol{L}^{2}(\boldsymbol{q}^{{\mathcal{C}}},\boldsymbol{s})\right)}^{\dagger}\bigg{(}\boldsymbol{L}^{1}(\boldsymbol{q}^{{\mathcal{C}}^{*}}(t),\boldsymbol{s}^{*}(t))\nu^{*}{(t)}-\boldsymbol{L}^{1}(\boldsymbol{q}^{{\mathcal{C}}},\boldsymbol{s})\nu\\\
+\boldsymbol{L}^{2}(\boldsymbol{q}^{{\mathcal{C}}^{*}}(t),\boldsymbol{s}^{*}(t))\omega^{*}{(t)}-\lambda\boldsymbol{e}\bigg{)}.$
(13)

They consist of feedforward terms derived from the desired trajectory and
feedback terms to drive the error to zero. The feedforward terms should cancel
out the $\dot{\boldsymbol{S}}^{*}$ term in (7), or equivalently the now non-
vanishing $\dot{\boldsymbol{s}}^{*}$ term in (8). When traveling along the
feature trajectory $\boldsymbol{S}^{*}(t)$, the angular velocity $\omega$ is
computed from (13), where starred terms are known, and $\nu=\nu^{*}(t)$ from
discussion after (10). As far as we know, no general IBVS tracking equations
have been derived that combine feedforward and feedback signals.

Figure 3: Gazebo environment top view and robot view with SLAM features.

SSSWTSSTSTSSTT Figure 4: Short distance template trajectories.

Figure 5: Real experiment top view and robot view with SLAM features. Blue box
is the robot’s start pose. Red box shows the end poses region of short
trajectories. The green curve is a sample trajectory to track.

###  II-C Simulation Experiments and Results

This section runs several short distance trajectory servoing experiments to
evaluate the accuracy of the image-based feedback strategy supplemented by
stereo V-SLAM. The hypothesis is that short distance trajectory tracking in
image space will improve over tracking in pose space.

####  II-C1 Experimental Setup

For quantifiable and reproducible outcomes, the ROS/Gazebo SLAM evaluation
environment from [38] is used for the tests, on an Intel i7-8700 workstation.
Fig. 5 shows a top down view of the world plus a robot view. The simulated
robot is a Turtlebot. It is tasked to follow a given short distance
trajectory, whose desired linear velocity is $0.3m/s$ and which is generated
to be dynamically feasible [41]. A total of five paths were designed, loosely
based on Dubins paths. They are denoted as short: straight (SS), weak turn
(SWT), straight+turn (SST), turn+straight (STS), turn+turn (STT), and are
depicted in Fig. 5. The average trajectory lengths are $\sim$4m. Longer paths
would consist of multiple short segment reflecting variations on this path
set. They are designed to ensure that sufficient feature points, visible in
the first frame, remain visible along the entirety of the path. Five trials
per trajectory are run. The desired and actual robot poses are recorded for
performance scoring.

Two metrics quantify tracking performance: a) Average lateral error (ALE) is
the 2-norm of the perpendicular distance to the robot heading direction
averaged over time. It measures robot deviation from the desired trajectory
and has been used to evaluate steering controllers [43]; b) Terminal error
(TE) measures the robot’s distance to the final stopped position of the
desired trajectory after tracking ends. Implementation details are provided in
the public Github repository [44].

####  II-C2 SLAM Stack

Part of the robot’s software stack includes the Good Feature (GF) ORB-SLAM
system [30] for estimating camera poses. It is configured to work with a
stereo camera and integrated into a loosely coupled, visual-inertial (VI)
system [38, 45] based on a multi-rate filter to form a VI-SLAM system. The
trajectory servoing system will interface with the GF-ORB-SLAM system to have
access to tracked features for servoing.

TABLE I:  Short distance trajectory benchmark and real experiment results.

Seq. | PO | SLAM | TS | VS+
---|---|---|---|---
SS | 0.70 | 0.84 | 0.88 | x
SWT | 0.71 | 1.35 | 1.23 | x
SST | 0.55 | 1.64 | 0.82 | x
STS | 0.86 | 1.68 | 0.91 | x
STT | 0.75 | 1.21 | 0.96 | x
Avg. | 0.71 | 1.34 | 0.96 | x
(a) Sim ALE (cm) Seq. | PO | SLAM | TS
---|---|---|---
SS | 1.06 | 1.53 | 2.05
SWT | 1.24 | 2.23 | 2.87
SST | 2.07 | 3.33 | 2.47
STS | 1.97 | 4.14 | 2.18
STT | 4.34 | 4.99 | 2.42
Avg. | 2.14 | 3.24 | 2.40
(b) Sim Terminal Error (cm) Seq. | RO | SLAM | TS
---|---|---|---
SS | 4.5 | 8.9 | 4.9
SWT | 6.8 | 10.4 | 4.9
SST | 8.0 | 13.1 | 5.7
STS | 13.3 | 11.9 | 5.0
STT | 9.5 | 10.5 | 3.5
Avg. | 8.4 | 10.9 | 4.8
(c) Real Terminal Error (cm)

Sim ALE (cm)

Sim Terminal Error (cm)

Real Terminal Error (cm)

Figure 6: 95% confidence intervals for short distance outcomes.

####  II-C3 Methods Tested

The baseline performance standard is pose-based control using perfect odometry
(PO) as obtained from the actual robot pose in the Gazebo simulator. PO is
used for performance comparison of the tested methods. Two comparison methods
are implemented. The first replaces PO with the V-SLAM estimated pose (SLAM).
The second is an implementation of IBVS based on (13), which is effectively
trajectory servoing without the V-SLAM system. It is called VS+ to
differentiate from trajectory servoing (TS), and uses a frame-by-frame stereo
feature tracking system [46]. Pose-based trajectory tracking [38] uses a
geometric controller with feedforward $[\nu^{*},\omega^{*}]^{T}$ and feedback
signals:

$\begin{split}\nu_{cmd}&=\nu^{*},\\\
w_{cmd}&=k_{\theta}*\widetilde{\theta}+k_{y}*\widetilde{y}+\omega^{*}.\\\
\end{split}$ (14)

where feedback uses only $\widetilde{y}$ and $\widetilde{\theta}$ from the
pose error,

$[\widetilde{x},\widetilde{y},\raisebox{-1.5pt}{$\widetilde{\theta}$}\makebox[20.0pt]{$]$}^{T}\simeq\widetilde{g}=g^{-1}g^{*}=\left({g}^{{\mathcal{W}}}_{{\mathcal{R}}}\right)^{-1}(t)\left({g}^{{\mathcal{{W},*}}}_{{\mathcal{R}}}\right)(t).$
(15)

For experimental consistency with trajectory servoing, the pose-based control
directly uses feedforward linear velocity terms from the given trajectory and
only regulates heading. The controller gains were empirically tuned to give
good performance for the PO case and extensively used in prior work [38, 1,
2]. The TS gains were also tuned [44].

####  II-C4 Results and Analysis

Tables I(a,b) quantify the outcomes of all methods tested. Fig. 6 consists of
95% confidence intervals of ALE and TE for the different template trajectories
and methods (minus VS+) in simulations and real experiments. The first outcome
to note is that VS+ fails for all paths. The average length of successful
servoing is 0.4m ($\sim$10% of the path length). Inconsistent data association
rapidly degrades the feature pool and prevents consistent use of features for
servoing feedback. Without the feature map in V-SLAM, redetected features are
treated as new and assigned unique indices, which violates the correspondence
rule from §II-B2; as noted, any effort to improve this would increasingly
approach the computations found in V-SLAM. Maintaining stable feature tracking
through V-SLAM is critical to trajectory servoing.

Using PO as the standard, the tables show a smaller gap for TS than for SLAM
as seen by the lower metric scores. For ALE, TS experiences a 35.2%
degradation versus PO, while SLAM experiences an 88% degradation. The ALE
statistics in Fig. 6 indicate that TS is expected to outperform SLAM.
Comparing PO to TS and to SLAM, the p-values are 8e-4 and 1e-5. For TS to SLAM
they are 1.9e-4. All indicate statistically significant performance
differences. For TE, similar results hold except that there is overlap in the
PO and TS confidence intervals. Consequently the p-value comparing PO and TS
is 0.18, which is not significant. Thus TS performs close to PO with regards
to achieved terminal error, while SLAM does not (p-value: 2e-3).

Trajectory servoing uses the same control effort as pose-based control; the
angular control efforts of the methods were not significantly different [44].
However, the rate of change of the control does differ. The time
differentiated control signal norm is an indicator of control smoothness
(larger means less smooth). They are 0.174, 0.174 and 1.300 for PO, SLAM and
TS respectively. TS control is computed directly from the tracked features
without temporal regularization applied to the applied controls. The SLAM
estimation process smooths the orientation estimates, which translates to the
control signal.

The first overall finding here is that TS outperforms SLAM and visual servoing
(VS+), though it is based on both. This confirms that TS combines the
advantages of them to enhance performance over both. Secondly, implementing a
purely image-based approach to trajectory tracking through unknown
environments is not only possible, but can work better than SLAM pose-based
controls over short segments, in the absence of global positioning
information. The results validate the system design proposed at the beginning
of §II-B. The robust feature tracking of V-SLAM prevents the loss of
trajectory tracking stability seen in VS+. The V-SLAM feature map maintenance,
feature culling, and feature retrieval modules contribute to robust visual
servoing.

###  II-D Real Experiments and Results

To confirm that trajectory servoing outcomes translate to practice, the short
trajectory experiment is run on a LoCoBot equipped with a RealSense stereo
camera and an Intel NUC (i5-7260U). Fig. 5 presents a top-view of the
experimental environment (left) and a robot view with SLAM features (right).
Based on the environment and how long features can be tracked within it, the
template trajectories are scaled down to $\sim$2.4m. For pose-based control,
two sources of robot pose estimates are tested: robot odometry (RO) and SLAM.
Robot odometry is generated from onboard wheel encoders and an IMU. RO will
have imperfect odometry due to measurement noise and uncertainty. Five trials
were run for each trajectory and each tracking strategy. Only terminal error
is measured. The continuous robot ground truth pose signal is unavailable.

####  II-D1 Results and Analysis

Table I(c) and Fig. 6 give the outcomes of the tested methods. SLAM has the
highest average terminal error. TS has the lowest TE average, being 42.9%
lower than RO and 56.0% lower than SLAM (p-values less than 1e-2 for both).
The outcomes are consistent with those from simulation: trajectory servoing
achieves better performance than pose-based tracking strategies using V-SLAM
over short segments.

In one instance RO outperformed TS, the straight trajectory. RO pose
estimation is more accurate for straight trajectories, such that odometry
drift is comparable to the image-based motion imperceptibility effects that
impact trajectory servoing performance under zero desired angular velocities.
This observation will be seen again in real experiments involving long
distance trajectory servoing in the next section.

####  II-D2 Process Timing

Image to control timing for trajectory servoing is $\sim\\!\\!26ms$. The major
time cost is V-SLAM feature tracking, with tracked feature control
calculations taking $\sim\\!\\!1.5ms$. V-SLAM image to pose estimate timing is
$\sim\\!28ms$.

##  III Long Distance Trajectory Servoing

Short distance trajectory servoing cannot extend to long trajectories due to
feature impoverishment. When moving beyond the initially visible scene, a more
comprehensive trajectory servoing system should augment the feature pool
$\boldsymbol{S}^{*}$ with new features. Likewise, if navigation consists of
multiple short distance trajectories, then the system must have a regeneration
mechanism for synthesizing entirely new desired feature tracks for the new
segment. The overlapping needs for these two events inform the creation of a
module for feature replenishment and trajectory extension.

$\boldsymbol{Q}^{{\mathcal{W}}}(t_{i-2,e})$$\boldsymbol{Q}^{{\mathcal{W}}}(t_{i-1,e})$$\boldsymbol{Q}^{{\mathcal{W}}}(t_{i,e})$$t_{i-2,e}$$t_{i-1,s}$$t_{i-1,e}$$t_{i,s}$$t_{i,e}$$t_{i+1,s}$$\boldsymbol{S}^{*}_{i-1}(t)|_{t_{i-1,s}}^{t_{i-1,e}}$$\boldsymbol{S}^{*}_{i}(t)|_{t_{i,s}}^{t_{i,e}}$$\boldsymbol{S}^{*}_{i+1}(t)|_{t_{i+1,s}}^{t_{i+1,e}}$

Figure 7: Feature replenishment process. There are three segments of feature
trajectories. Stars are observed point sets at corresponding time. Each circle
is the start or end time of next or this segment of feature trajectory. Three
feature trajectories are generated by the feature replenishment equation (16).

Feature Replenishment $+$$-$ Trajectory Servoing Core Plant V-SLAM Tracked
Features V-SLAM Pose

$\boldsymbol{e}$

$\dot{\boldsymbol{s}},\boldsymbol{u}$

$\boldsymbol{u}^{*}$

$\boldsymbol{s},{g}^{{\mathcal{W}}}_{{\mathcal{R}}}$

$\hat{\boldsymbol{s}}$

$\boldsymbol{s}^{*}$

Ref Traj

${g}^{{\mathcal{W,*}}}_{{\mathcal{R}}}$

$\hat{{g}}^{{\mathcal{W}}}_{{\mathcal{R}}}$

Figure 8: Block diagram for long distance trajectory servoing. Notations
follow §I and §II. $\boldsymbol{s}^{*}$, $\boldsymbol{u}^{*}$ and
$\hat{\boldsymbol{s}}$, $\hat{{g}}^{{\mathcal{W}}}_{{\mathcal{R}}}$ are the
corresponding desired and measured values.

LRULLULSTLZZLSLT(a)(b)

Figure 9: Long distance trajectories. (a) Trajectories in simulation; (b)
Trajectories in real experiments.

###  III-A Feature Replenishment

The number of feature correspondences $n_{\text{F}}$ in $\boldsymbol{S}$ and
$\boldsymbol{S}^{*}$ indicates whether trajectory servoing can be performed
without concern. Let the threshold $\tau_{fr}$ determine when feature
replenishment should be triggered. Define
$\boldsymbol{S}^{*}_{i}(t)|_{t_{i,s}}^{t_{i,e}}$ as the $i^{\rm th}$ feature
trajectory starting from $t_{i,s}$ and ending at $t_{i,e}$. The case $i=0$
represents the first feature trajectory segment generated by (6) for
$t_{i,s}=0$, integrated up to the maximum time $t_{\text{end}}$ of the given
trajectory. The time varying function $n_{\text{F}}(t)$ is the actual number
of feature correspondences between $\boldsymbol{S}(t)$ and
$\boldsymbol{S}^{*}_{i}(t)$ as the robot proceeds.

When $n_{\text{F}}(t)\geq\tau_{fr}$, the feature trajectory
$\boldsymbol{S}^{*}_{i}(t)$ may be used for trajectory servoing. When
$n_{\text{F}}(t)<\tau_{fr}$, the feature replenishment process will be
triggered at the current time and noted as $t_{i,e}$. The old feature
trajectory $\boldsymbol{S}^{*}_{i}(t)|_{t_{i,s}}^{t_{i,e}}$ is finished. A new
feature trajectory is generated with

$\boldsymbol{S}^{*}_{i+1}(t)|_{t_{i+1,s}}^{t_{i+1,e}}=\boldsymbol{H}\circ(g^{*}(t_{i,e},t){h}^{{\mathcal{R}}}_{{\mathcal{C}}})^{-1}(\boldsymbol{Q}^{{\mathcal{W}}}(t_{i,e})),$
(16)

where $g^{*}(t_{i,e},t)$ is the transformation between the current robot pose
and a future desired pose ($t>t_{i,e}$) on the trajectory. The poses behind
the robot are not included. The set $\boldsymbol{Q}^{{\mathcal{W}}}(t_{i,e})$
consists of observed points at the current time $t_{i,e}$. The feature pool is
augmented by these current features. When this regeneration step is finished,
the exact time will be assigned as the $t_{i+1,s}$. Trajectory servoing is
performed on this new feature trajectory until the regeneration is triggered
again or the arriving at the end of the trajectory. The process of
regenerating new feature tracks is equivalent to dividing a long trajectory
into a set of shorter segments pertaining to the generated feature trajectory
segments. An depiction of feature replenishment is shown in Fig. 9.

TABLE II: Long distance trajectory benchmark and real experiment results.

Seq. | PO | SLAM | TS | TS+PO
---|---|---|---|---
LRU | 0.53 | 3.88 | 4.00 | 1.47
LLU | 0.86 | 8.21 | 5.18 | 1.61
LST | 1.13 | 5.03 | 3.00 | 2.03
LZZ | 1.06 | 7.54 | 5.90 | 5.04
Avg. | 0.90 | 6.17 | 4.52 | 2.54
(a) Sim ALE (cm) Seq. | PO | SLAM | TS | TS+PO
---|---|---|---|---
LRU | 8.57 | 10.66 | 6.42 | 4.15
LLU | 5.54 | 29.48 | 15.75 | 4.02
LST | 6.01 | 7.60 | 1.76 | 6.61
LZZ | 7.83 | 9.28 | 9.00 | 12.21
Avg. | 6.99 | 14.26 | 8.23 | 6.75
(b) Sim Terminal Error (cm) Seq. | RO | SLAM | TS
---|---|---|---
LS | 8.2 | 14.1 | 10.9
LT | 12.8 | 11.8 | 6.8
Avg. | 10.5 | 13.0 | 8.9
(c) Real Terminal Error (cm)

Sim ALE (cm)

Sim Terminal Error (cm)

Real Terminal Error (cm)

Figure 10: 95% confidence intervals for long distance outcomes.

During navigation, (16) requires the current robot pose relative to the
initial pose to be known. In the absence of an absolute reference or position
measurement system, the only option available is to use the estimated robot
pose from V-SLAM, or some equivalent process. Although there are some
drawbacks to relying on V-SLAM, it attempts to keep pose estimation as
accurate as possible over long periods through feature mapping, bundle
adjustment, loop closure, etc. To further couple V-SLAM and trajectory
servoing, we design a multi-loop scheme, see Fig. 9. The inner loop is
governed by trajectory servoing with V-SLAM tracked features. The V-SLAM
estimated pose is only explicitly used in the outer loop, during feature
replenishment. Though doing so may introduce bias into the feature
trajectories, the lower frequency reliance on SLAM avoids accumulating SLAM
pose estimation uncertainty as would higher frequency use in the inner loop
[44].

###  III-B Simulation Experiments and Results

This section modifies the experiments in §II-C to involve longer trajectories
that trigger feature replenishment and synthesize new feature trajectory
segments. The set of trajectories to track is depicted in Fig. 9(a). They are
denoted as long: right u-turn (LRU), left u-turn (LLU), straight+turn (LST),
and zig-zag (LZZ). Each trajectory is around 20m or longer. Testing and
evaluation follows as before (minus VS+). A new tracking method is added:
TS+PO, which uses perfect odometry instead of SLAM odometry in the feature
replenishment stage. The parameter was tuned for performance, giving
$\tau_{fr}=10$ [44].

####  III-B1 Results and Analysis

Tables II(a,b) give outcomes for the two error metrics. Though TS continues to
outperform SLAM and is closer to PO, the gap relative to PO is larger than the
gap for short trajectories. As hypothesized, longer trajectory error is
affected by the need to use SLAM pose estimates for regeneration. SLAM pose
drift impacts trajectory tracking error for both methods, but is attenuated
when using TS. The TS+PO outcomes confirm the impact of SLAM drift on TS
performance as the TS+PO outcomes are lower than the TS outcomes. Tighter
coupling of the trajectory servoing and V-SLAM systems (e.g., using TS
tracking to assist with pose estimation) should improve the accuracy of pose
estimation, and further improve tracking performance.

Fig. 10 depicts the 95% confidence intervals of the outcomes for the template
trajectories. For ALE, the intervals and p-values (<5e-3) imply that all
pairwise comparisons are significant. For TE, SLAM compared to any method
indicates statistical significance (p-values: <8e-3). The TE scores amongst
PO, TS, and TS+PO are not significant (p-values: $>$0.16). TS and TS+PO
performance is close to PO.

For long distance trajectories, trajectory servoing again has similar control
effort to pose-based feedback. However, the norms of the time differentiated
control signal are 0.151, 0.148, 0.565 and 0.518 for PO, SLAM, TS and TS+PO
respectively. TS-based control signals remain less smooth.

###  III-C Real Experiments and Results

The last experiment evaluates long trajectory performance on a real robot. The
experimental setup is similar to §II-D. Two long trajectories, LS and LT, in
Fig. 9(b) are used, of lengths $\sim$13m and $\sim$8m, respectively.

####  III-C1 Results and Analysis

In Table II(c), TS ranks first for the average TE metric. Average TE decreases
31.5% from SLAM to TS and 15.2% from RO to TS. Importantly TS outperforms SLAM
in all cases, with lower variance (see Fig. 10). These outcomes are consistent
with the simulation results and support the premise behind the trajectory
servoing system design described at the end of §III-A.

For the LS trajectory with long straight segments, RO outperforms SLAM because
it has better pose estimation. Using TS reduces the terminal error by 22.7%,
compared to SLAM. Trajectory servoing is more robust to pose estimation
errors. Yet, it is still impacted by the SLAM pose uncertainty arising from
forward motion imperceptibility due to TS’ use of SLAM state information for
feature regeneration. These results reproduce the observations in §II-D1.

####  III-C2 Feature Replenishment Frequency

Trajectory servoing feature replenishment calls queried SLAM poses 1.5 times
per meter in simulation and experiment. The SLAM pose-based tracking method
equivalent is 100 times per meter.

##  IV Conclusion

The paper presented an image-based trajectory tracking approach for a non-
holonomic mobile ground robot. Called trajectory servoing, it combines IBVS
and V-SLAM to achieve tracking through unknown environments without externally
derived absolute positioning information. Trajectory servoing successfully
follows short trajectories. Using estimated robot poses from the V-SLAM module
extends trajectory tracking to longer trajectories. Experiments demonstrate
improved accuracy over pose-based trajectory tracking using estimated SLAM
poses or robot odometry. Trajectory servoing is less impacted by pose
estimation error, by virtue of directly using for feedback the features from
which pose is inferred. However, the trade-off for relying on visual features
is that feature poor environments may not be traversable using trajectory
servoing.

Real-world uncertainty and disturbances as well as non-smooth trajectories may
degrade tracking performance. Future work intends to improve robustness of
trajectory servoing by adding linear velocity control, temporal smoothness
constraints, and performing uncertainty analysis on the feedback equations. In
addition, more tightly coupling trajectory servoing with V-SLAM may benefit
the pose estimation process and further improve tracking performance.

##  Appendix A Study of using pose with higher frequency

In this study, we are investigating the effect that pose estimation
uncertainty brings to tracking performance. In SLAM pose-based control, the
pose estimation errors immediately cause worse tracking performance. The
design of trajectory servoing intends to improve the performance by less
frequently requesting robot poses and shifting to the image domain. We already
have the comparison of SLAM and TS in the paper. To further show the effect of
pose estimation uncertainty, we conduct ablation studies about tracking
performance when trajectory servoing more frequently using SLAM poses.

###  A-A Feature replenishment threshold $\tau_{fr}$

From §III.A in the paper, the feature replenishment threshold $\tau_{fr}$
controls the frequency of feature trajectory regeneration and robot pose
requesting with long distance trajectories. In the simulation, we conduct an
ablation study about the influence of varying $\tau_{fr}$. The same two
trajectory tracking performance metrics are used: average lateral error (ALE)
and terminal error (TE). The number of feature trajectory regeneration per
length is also recorded. Outcomes of averages over all trajectory templates
are included in Table III and Fig. 11

With smaller $\tau_{fr}$, trajectory servoing uses less number of tracked
features, and works similar to pure IBVS. The computed control could be
unstable and have oscillations, which reduces the tracking performance. In
contrast, if $\tau_{fr}$ is significantly large, trajectory servoing will more
frequently trigger feature replenishment. The performance will be more related
to pose estimation accuracy, and be close to SLAM pose-based control. More
pose estimation errors cause worse performance.

TABLE III:  Raw data from ablation study of feature replenishment $\tau_{fr}$

$\tau_{fr}$ | 4 | 6 | 10 | 16 | 22 | 36 | 50
---|---|---|---|---|---|---|---
ALE | 7.01 | 5.11 | 4.52 | 4.03 | 4.67 | 4.67 | 4.57
TE | 14.08 | 9.97 | 8.23 | 8.53 | 9.73 | 9.53 | 10.47
# of Reg/m | 1.4 | 1.5 | 1.6 | 1.7 | 1.8 | 2.0 | 2.7

Figure 11: Ablation study plots of feature replenishment $\tau_{fr}$

###  A-B Incremental Trajectory Servoing

In order to further investigate the effect that pose estimation uncertainty
brings to tracking performance. The trajectory servoing system is modified to
use robot pose information in every control loop and recomputes the desired
feature set for the next pose on the given trajectory. The feature trajectory
will not be precomputed at the beginning. This version is called Incremental
Trajectory Servoing (I-TS). Unlike the regular feature replenishment in §III.A
that is triggered by $\tau_{fr}$, desired feature regeneration is triggered by
time. By doing this modification, trajectory servoing works similar to SLAM
pose-based control, but shifting feedback to the image domain.

We applied the same simulation benchmark and metrics (i.e. ALE and TE) to
compare tracking performance. The benchmark is only tested with long distance
trajectories, because SLAM pose estimation uncertainty is not explicit with
short distance trajectories. Tables IV(a,b) gives outcomes for the two
metrics. I-TS has worse ALE and TE than TS and is similar to SLAM. It shows
that frequently using SLAM pose information will introduce more uncertainty
that potentially do harm to the tracking.

TABLE IV:  Incremental Trajectory Servoing Outcomes.

Seq. | PO | SLAM | TS | I-TS
---|---|---|---|---
LRU | 0.53 | 3.88 | 4.00 | 3.56
LLU | 0.86 | 8.21 | 5.18 | 7.68
LST | 1.13 | 5.03 | 3.00 | 4.19
LZZ | 1.06 | 7.54 | 5.90 | 9.72
Avg. | 0.90 | 6.17 | 4.52 | 6.29
(a) Sim ALE (cm) Seq. | PO | SLAM | TS | I-TS
---|---|---|---|---
LRU | 8.57 | 10.66 | 6.42 | 8.82
LLU | 5.54 | 29.48 | 15.75 | 24.3
LST | 6.01 | 7.60 | 1.76 | 10.45
LZZ | 7.83 | 9.28 | 9.00 | 9.91
Avg. | 6.99 | 14.26 | 8.23 | 13.37
(b) Sim TE (cm)

###  A-C Conclusion

The above two studies shows the effect of pose estimation uncertainty when
using SLAM poses with higher frequency. In addition, the results of TS+PO in
the paper also indicate the improved performance with more accurate pose
information. Overall, pose estimation uncertainty can lead to worse trajectory
tracking performance. Our trajectory servoing design can reduce the effect of
this uncertainty and have better tracking results.

## References

  * [1] J. S. Smith and P. A.Vela, “PiPS: Planning in perception space,” in _ICRA_ , May 2017, pp. 6204–6209.
  * [2] J. S. Smith, S. Feng, F. Lyu, and P. A. Vela, _Real-Time Egocentric Navigation Using 3D Sensing_. Cham: Springer International Publishing, 2020, pp. 431–484.
  * [3] F. Chaumette and S. Hutchinson, “Visual servo control. I. basic approaches,” _RAM_ , vol. 13, no. 4, pp. 82–90, 2006.
  * [4] ——, “Visual servo control. II. advanced approaches,” _RAM_ , vol. 14, no. 1, pp. 109–118, 2007.
  * [5] F. Fahimi and K. Thakur, “An alternative closed-loop vision-based control approach for unmanned aircraft systems with application to a quadrotor,” in _ICUAS_ , 2013, pp. 353–358.
  * [6] D. Zheng, H. Wang, J. Wang, S. Chen, W. Chen, and X. Liang, “Image-based visual servoing of a quadrotor using virtual camera approach,” _IEEE/ASME Transactions on Mechatronics_ , vol. 22, no. 2, pp. 972–982, 2017\.
  * [7] D. Zheng, H. Wang, J. Wang, X. Zhang, and W. Chen, “Toward visibility guaranteed visual servoing control of quadrotor uavs,” _IEEE/ASME Transactions on Mechatronics_ , vol. 24, no. 3, pp. 1087–1095, 2019.
  * [8] A. Remazeilles, F. Chaumette, and P. Gros, “3D navigation based on a visual memory,” in _ICRA_ , 2006, pp. 2719–2725.
  * [9] S. Šegvić, A. Remazeilles, A. Diosi, and F. Chaumette, “A mapping and localization framework for scalable appearance-based navigation,” _CVIU_ , vol. 113, no. 2, pp. 172 – 187, 2009.
  * [10] T. Krajník, F. Majer, L. Halodová, and T. Vintr, “Navigation without localisation: reliable teach and repeat based on the convergence theorem,” in _IROS_ , 2018, pp. 1657–1664.
  * [11] A. Cherubini, F. Chaumette, and G. Oriolo, “Visual servoing for path reaching with nonholonomic robots,” _Robotica_ , vol. 29, no. 7, pp. 1037–1048, 2011.
  * [12] A. Ahmadi, L. Nardi, N. Chebrolu, and C. Stachniss, “Visual servoing-based navigation for monitoring row-crop fields,” in _ICRA_ , May 2020, pp. 4920–4926.
  * [13] A. Diosi, S. Šegvić, A. Remazeilles, and F. Chaumette, “Experimental evaluation of autonomous driving based on visual memory and image-based visual servoing,” _T-ITS_ , vol. 12, no. 3, pp. 870–883, 2011\.
  * [14] G. Blanc, Y. Mezouar, and P. Martinet, “Indoor navigation of a wheeled mobile robot along visual routes,” in _ICRA_ , 2005, pp. 3354–3359.
  * [15] L. Halodová, E. Dvořráková, F. Majer, T. Vintr, O. M. Mozos, F. Dayoub, and T. Krajník, “Predictive and adaptive maps for long-term visual navigation in changing environments,” in _IROS_ , 2019, pp. 7033–7039.
  * [16] T. Do, L. C. Carrillo-Arce, and S. I. Roumeliotis, “High-speed autonomous quadrotor navigation through visual and inertial paths,” _IJRR_ , vol. 38, no. 4, pp. 486–504, 2019.
  * [17] P. Furgale and T. Barfoot, “Visual teach and repeat for long-range rover autonomy,” _J. Field Robot._ , vol. 27, pp. 534–560, 09 2010.
  * [18] T. Krajník, P. Cristóforis, K. Kusumam, P. Neubert, and T. Duckett, “Image features for visual teach-and-repeat navigation in changing environments,” _Robotics and Autonomous Systems_ , vol. 88, pp. 127 – 141, 2017.
  * [19] N. Zhang, M. Warren, and T. D. Barfoot, “Learning place-and-time-dependent binary descriptors for long-term visual localization,” in _ICRA_ , 2018, pp. 828–835.
  * [20] T. Nguyen, G. K. Mann, R. G. Gosine, and A. Vardy, “Appearance-based visual-teach-and-repeat navigation technique for micro aerial vehicle,” _J. Intell. Robotics Syst._ , vol. 84, no. 1–4, p. 217–240, 2016.
  * [21] K. Kidono, J. Miura, and Y. Shirai, “Autonomous visual navigation of a mobile robot using a human-guided experience,” _Robotics and Autonomous Systems_ , vol. 40, no. 2, pp. 121 – 130, 2002.
  * [22] A. Pfrunder, A. P. Schoellig, and T. D. Barfoot, “A proof-of-concept demonstration of visual teach and repeat on a quadrocopter using an altitude sensor and a monocular camera,” in _Canadian Conference on Computer and Robot Vision_ , 2014, pp. 238–245.
  * [23] A. Vardy, “Using feature scale change for robot localization along a route,” in _IROS_ , 2010, pp. 4830–4835.
  * [24] N. M. Garcia and E. Malis, “Preserving the continuity of visual servoing despite changing image features,” in _IROS_ , vol. 2, 2004, pp. 1383–1388 vol.2.
  * [25] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. Reid, and J. J. Leonard, “Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age,” _T-RO_ , vol. 32, no. 6, pp. 1309–1332, 2016.
  * [26] D. Fontanelli, “Mobile robot control in unknown indoor environments-the visual slam for servoing,” Ph.D. dissertation, Dipartimento di Sistemi Elettrici e Automazione, Università degli Studi di Pisa, Italy, 2006.
  * [27] M. Milford and G. Wyeth, “Hybrid robot control and slam for persistent navigation and mapping,” _Robotics and Autonomous Systems_ , vol. 58, no. 9, pp. 1096–1104, 2010.
  * [28] C. Li, X. Zhang, and H. Gao, “A general monocular visual servoing structure for mobile robots in natural scene using slam,” in _ICCSIP_. Springer, 2018, pp. 465–476.
  * [29] C. Li, X. Zhang, H. Gao, R. Wang, and Y. Fang, “Bridging the gap between visual servoing and visual slam: A novel integrated interactive framework,” _T-ASE_ , pp. 1–11, 2021.
  * [30] Y. Zhao and P. A. Vela, “Good feature matching: Toward accurate, robust VO/VSLAM with low latency,” _T-RO_ , vol. 36, no. 3, pp. 657–675, 2020\.
  * [31] L. Nardi, B. Bodin, M. Z. Zia, J. Mawer, A. Nisbet, P. H. J. Kelly, A. J. Davison, M. Luján, M. F. P. O’Boyle, G. Riley, N. Topham, and S. Furber, “Introducing SLAMBench, a performance and accuracy benchmarking methodology for SLAM,” in _ICRA_ , 2015, pp. 5783–5790.
  * [32] S. Saeedi _et al._ , “Navigating the landscape for real-time localization and mapping for robotics and virtual and augmented reality,” _Proceedings of the IEEE_ , vol. 106, no. 11, pp. 2020–2039, 2018.
  * [33] M. Bujanca, P. Gafton, S. Saeedi, A. Nisbet, B. Bodin, M. F. O’Boyle, A. J. Davison, P. H. Kelly, G. Riley, B. Lennox, M. Luján, and S. Furber, “SLAMBench 3.0: Systematic automated reproducible evaluation of SLAM systems for robot vision challenges and scene understanding,” in _ICRA_ , 2019, pp. 6351–6358.
  * [34] J. Delmerico and D. Scaramuzza, “A benchmark comparison of monocular visual-inertial odometry algorithms for flying robots,” in _ICRA_ , 2018, pp. 2502–2509.
  * [35] I. Cvišić, J. Ćesić, I. Marković, and I. Petrović, “SOFT-SLAM: Computationally efficient stereo visual simultaneous localization and mapping for autonomous unmanned aerial vehicles,” _J. Field Robot._ , vol. 35, no. 4, pp. 578–595, 2018.
  * [36] A. Weinstein, A. Cho, G. Loianno, and V. Kumar, “Visual inertial odometry swarm: An autonomous swarm of vision-based quadrotors,” _RA-L_ , vol. 3, no. 3, pp. 1801–1807, 2018.
  * [37] Y. Lin, F. Gao, T. Qin, W. Gao, T. Liu, W. Wu, Z. Yang, and S. Shen, “Autonomous aerial navigation using monocular visual-inertial fusion,” _J. Field Robot._ , vol. 35, no. 1, pp. 23–51, 2018.
  * [38] Y. Zhao, J. S. Smith, S. H. Karumanchi, and P. A. Vela, “Closed-loop benchmarking of stereo visual-inertial SLAM systems: Understanding the impact of drift and latency on tracking accuracy,” _ICRA_ , pp. 1105–1112, 2020.
  * [39] S. Hutchinson, G. D. Hager, and P. I. Corke, “A tutorial on visual servo control,” _IEEE Transactions on Robotics and Automation_ , vol. 12, no. 5, pp. 651–670, 1996.
  * [40] R. Murray, Z. Li, and S. Sastry, _A Mathematical Introduction to Robotic Manipulation_. CRC Press, 1994.
  * [41] R. Olfati-Saber, “Near-identity diffeomorphisms and exponential $\epsilon$-tracking and $\epsilon$-stabilization of first-order nonholonomic SE(2) vehicles,” in _ACC_ , vol. 6, May 2002, pp. 4690–4695 vol.6.
  * [42] R. Sharma and S. Hutchinson, “Motion perceptibility and its application to active vision-based servo control,” _IEEE Transactions on Robotics and Automation_ , vol. 13, no. 4, pp. 607–617, 1997.
  * [43] S. Dominguez, A. Ali, G. Garcia, and P. Martinet, “Comparison of lateral controllers for autonomous vehicle: Experimental results,” in _ITSC_. IEEE, 2016, pp. 1418–1423.
  * [44] S. Feng, Z. Wu, Y. Zhao, and P. Vela, “Trajectory servoing,” 2022, https://github.com/ivaROS/TrajectoryServoing.
  * [45] S. M. Weiss, “Vision based navigation for micro helicopters,” Ph.D. dissertation, Dep. of Mechanical and Process Eng., ETH Zurich, Zürich, 2012.
  * [46] S. Feng. (2020) Frame-by-frame stereo feature tracking system. [Online]. Available: https://github.com/ivaROS/stereoFeatureTracking.git

|  Shiyu Feng is a Ph.D. candidate in the School of Mechanical Engineering, at
the Georgia Institute of Technology. He is a member of Intelligent Vision and
Automation Laboratory (IVALab) supervised by Dr. Patricio A. Vela. His
research focuses on vision-based hierarchical navigation using sparse
representation to improve the computational efficiency and scalability.
Previously, he earned his B.Eng and M.Eng in Mechanical Engineering from
Chongqing University, China (2015) and University of California, Berkeley
(2016). Shiyu Feng is a student member of IEEE.
---|---
|  Zixuan Wu received his B.Eng. degree in automation from Harbin Institute of
Technology, China in 2019. After that, he went to Georgia Institute of
Technology, USA, obtained his M.Sc. degree at School of Electrical and
Computer Engineering, and started the Ph.D. at the same department in 2021.
His research interests lie in reinforcement learning, visual servoing and
visual navigation. Recently, he works on the experience replay optimization
for heterogeneous robot teaming.
---|---
|  Yipu Zhao is a Research Scientist at Meta Reality Lab. Prior to joining
Meta, he obtained his Ph.D. in 2019, under the supervision of Patricio A.
Vela, at the School of Electrical and Computer Engineering, Georgia Institute
of Technology, USA. Previously he received his B.Sc. degree in 2010 and M.Sc.
degree in 2013, at the Institute of Artificial Intelligence, Peking
University, China. His research interests include visual odometry/SLAM, 3D
reconstruction, and multi-object tracking. Dr. Zhao is a member of IEEE.
---|---
|  Patricio A. Vela is an associate professor in the School of Electrical and
Computer Engineering, and the Institute of Robotics and Intelligent Machines,
at Georgia Institute of Technology, USA. His research interests lie in the
geometric perspectives to control theory and computer vision. Recently, he has
been interested in the role that computer vision can play for achieving
control-theoretic objectives of (semi-)autonomous systems. His research also
covers control of nonlinear systems, typically robotic systems. Prof. Vela
earned his B.Sc. degree in 1998 and his Ph.D. degree in control and dynamical
systems in 2003, both from the California Institute of Technology, where he
did his graduate research on geometric nonlinear control and robotics. In
2004, Dr. Vela was as a post-doctoral researcher on computer vision with
School of ECE, Georgia Tech. He join the ECE faculty at Georgia Tech in 2005.
Prof. Vela is a member of IEEE.
---|---