# Deep Learning of Near Field Beam Focusing in Terahertz Wideband Massive MIMO
Systems

Yu Zhang and Ahmed Alkhateeb Yu Zhang and Ahmed Alkhateeb are with Arizona
State University (Email: y.zhang, alkhateeb@asu.edu). This work is supported
by the National Science Foundation under Grant No. 1923676.

###### Abstract

Employing large antenna arrays and utilizing large bandwidth have the
potential of bringing very high data rates to future wireless communication
systems. However, this brings the system into the near-field regime and also
makes the conventional transceiver architectures suffer from the wideband
effects. To address these problems, in this paper, we propose a low-complexity
frequency-aware beamforming solution that is designed for hybrid time-delay
and phase-shifter based RF architectures. To reduce the complexity, the joint
design problem of the time delays and phase shifts is decomposed into two
subproblems, where a signal model inspired online learning framework is
proposed to learn the shifts of the quantized analog phase shifters, and a
low-complexity geometry-assisted method is leveraged to configure the delay
settings of the time-delay units. Simulation results highlight the efficacy of
the proposed solution in achieving robust performance across a wide frequency
range for large antenna array systems.

##  I Introduction

Employing large antenna arrays and utilizing the large bandwidth available at
millimeter wave and terahertz (THz) bands are two key trends in current and
future wireless communication systems. However, these new characteristics also
bring new design challenges. Two particular challenges are: (i) With a large
antenna array aperture, the system is more likely to operate in the near-field
region. This makes the conventional far-field planar wave propagation
assumption invalid and challenges the beamforming design problems. (ii) With
the wide bandwidth, the classical phase shifter based analog-only beamforming
experiences beam squinting/mis-focusing, leading to degradation in the
beamforming gain and increase in the introduced network interference. These
problems motivate adopting new transceiver architectures and developing novel
beamforming solutions for near-field wideband communication systems, which is
the focus of this paper.

Prior Work: A common approach to improve the performance of a wideband system
is by equipping the antenna arrays with the time delay (TD) units which can
introduce a constant group delay to the different frequency components of the
transmit/receive signals. For instance, in [1, 2, 3], the authors demonstrate
the potential of such TD units based arrays in achieving robust beamforming
gain performance and in facilitating other operations (such as beam training
and alignment) in the wideband systems. However, the TD-based arrays come with
a higher manufacture cost and they also consume more power than the
conventional phase-shifter (PS) based antenna arrays. With the motivation to
address this increased cost and power consumption, [4, 5] studied the near-
field wideband beam focusing problem using PS-only architectures. Although the
average performance within the considered bandwidth is optimized, the peak
gains are still majorly sacrificed. To strike a balance between performance
and hardware cost/power consumption, the hybrid TD-PS based architecture is
introduced, where the wideband beamforming problem is studied under both far-
field [6] and near-field [7] channel models. However, the achieved performance
in the existing work relies on accurate channel knowledge, which incurs high
training and acquisition overhead given the large number of antennas.

Contribution: In this paper, we develop a low-complexity beamforming approach
for TD-PS architecture that is capable of learning and optimizing the near-
field wideband beams without requiring any channel knowledge. Specifically, an
online learning framework that is inspired by the signal model is developed to
learn the phase shifts of the quantized analog PSs and achieve fast
convergence despite the large numbers of antennas. Further, a low-complexity
geometry-assisted method is devised to configure the delay settings of the TD
units. The proposed framework relies only on the power measurements of the
received signal at different subcarriers, and does not require any explicit
channel or location information. Simulation results demonstrate the capability
of the proposed solution in learning optimized near-field beams that achieve
robust performance in practical wideband large-antenna array systems.

##  II System and Channel Models

###  II-A System Model

We consider a system where a THz massive MIMO base station (BS) with $M$
antennas is communicating with a single-antenna user equipment (UE). The
system adopts OFDM transmission/reception with $K$ subcarriers and operates at
a center frequency $f_{c}$ over a bandwidth $B$. In the uplink, if the UE
transmits a symbol $s_{k}\in\mathbb{C}$ at the $k$-th subcarrier, then the
received signal at the BS after combining can be expressed as

$y_{k}={\bf w}^{H}{\bf h}_{k}s_{k}+{\bf w}^{H}{\bf n}_{k},~{}\forall
k=1,\dots,K,$ (1)

where we assume that the transmitted symbol $s_{k}$ satisfies the average
power constraint $\mathbb{E}\\{|s_{k}|^{2}\\}=\frac{P_{\mathrm{T}}}{K}$ with
$P_{\mathrm{T}}$ denoting the total transmit power. ${\bf w}$ is the combining
vector, ${\bf h}_{k}\in\mathbb{C}^{M\times 1}$ is the uplink channel vector
between the UE and the BS at the $k$-th subcarrier, and ${\bf
n}_{k}\sim\mathcal{CN}(0,\sigma_{k}^{2}{\bf I})$ is the receive noise vector
at the BS with $\sigma_{k}^{2}$ being the noise power.

The combining vector ${\bf w}$ in (1) takes different forms in different
transceiver architectures. In this paper, we consider a practical system where
the BS has only one RF chain and employs analog-only beamforming using a
network of $r$-bit quantized PSs. Further, to mitigate the severe beam split
effect in the wideband systems, we assume that the system is equipped with
time-delay units. These TD units are RF circuit blocks that introduce a
constant group delay to the different frequency components of the received
signal [3]. For example, if the $n$-th TD unit is configured to introduce a
delay of $\tau_{n}$ seconds, then the phase shift incurred for the signal at
the $k$-th subcarrier is $-2\pi f_{k}\tau_{n}$. As shown in Fig. 1, we assume
that every $P$ PSs are connected to one TD unit, i.e., forming a partially
connected architecture. Without loss of generality, we assume that the system
adopts a total number of $N$ TD units such that $M=NP$. With this
architecture, and unlike the conventional PS-only beamforming, the beamformer
becomes frequency-dependent. Specifically, the effective combining vector at
the frequency $f_{k}$ is given by

${\bf w}_{k}=\frac{1}{\sqrt{M}}e^{j\left(-2\pi
f_{k}\boldsymbol{\tau}\otimes\mathbf{1}_{P}+\boldsymbol{\theta}\right)},$ (2)

where $\otimes$ is Kronecker product, $\boldsymbol{\tau}\in\mathbb{R}^{N\times
1}$ is the delay vector, $\boldsymbol{\theta}\in\mathbb{R}^{M\times 1}$ is the
phase vector (of the phase shifters), and $\mathbf{1}_{P}$ denotes the
$P\times 1$ all one vector. In the rest of the paper, we will use the
approximation

${\bf w}(f)\approx{\bf w}_{k},~{}\forall
f\in\left[f_{k}-\frac{B}{2K},f_{k}+\frac{B}{2K}\right],$ (3)

i.e., we will assume that the effective beamforming is constant across each
subcarrier.

Figure 1: The considered system architecture where we assume a general non-
uniform linear array that adopts analog PSs and TD units. The user equipment
is assumed to be within the near field region of the large-dimensional array.

###  II-B Near Field Wideband Channel Model

Without loss of generality, we assume a 2-D geometry, where both the BS
antenna array and the UE’s antenna are at the x-y plane. The BS is assumed to
employ a linear array that is aligned with the y axis and that has a half
aperture of $\frac{D}{2}$. The center of the BS array is assumed to be at the
origin. Based on that, the positions of the $M$ BS antenna elements can be
written as ${\bf p}_{m}=[0,\frac{D}{2}\alpha_{m}]^{T},\forall m=1,2,\dots,M$,
where $\alpha_{m}$ takes a value in $[-1,1]$, i.e.,

$-1\leq\alpha_{M}<\alpha_{M-1}<\cdots<\alpha_{2}<\alpha_{1}\leq 1.$ (4)

It is worth noting that we do not assume that the BS adopts a uniform linear
array (ULA), meaning that $\alpha_{m},\forall m$ can be any values in $[-1,1]$
and only need to satisfy the sequential constraint, i.e.,
$\alpha_{M}<\cdots<\alpha_{2}<\alpha_{1}$. Finally, we denote the UE’s
position as ${\bf q}=[q_{x},q_{y}]^{T}\in\mathbb{R}^{2}$. Hence, the distance
between the UE’s antenna and the $m$-th BS antenna element is given by
$d_{m}=\|{\bf p}_{m}-{\bf q}\|_{2}$.

Given the large aperture of the array adopted by the BS, we consider a
practical scenario where the UE is within the near field region of the BS.
Therefore, and based on the above geometry, the channel coefficient between
the UE and the $m$-th BS antenna at the $k$-th subcarrier is given by

$[{\bf h}_{k}]_{m}=\frac{\rho_{k}\lambda_{k}}{4\pi
d_{m}}e^{-j\frac{2\pi}{\lambda_{k}}d_{m}},~{}\forall
k=1,\dots,K,~{}m=1,\dots,M,$ (5)

where $\lambda_{k}$ is the wavelength of the $k$-th subcarrier and $\rho_{k}$
subsumes all the other factors such as antenna pattern of the BS array,
pathloss, and channel gain at the $k$-th subcarrier.

##  III Problem Formulation

Given the system and channel models described in Section II, we cast the
problem as a joint design of phase shifts of the quantized phase shifters and
the delay configurations of the TD units. The overall objective is to maximize
the average beamforming gain, or equivalently, the signal-to-noise ratio
(SNR), of the system across all the subcarriers. Formally, this objective can
be written as

$\displaystyle\max\limits_{\boldsymbol{\tau},\boldsymbol{\theta}}\hskip 2.0pt$
$\displaystyle\hskip
2.0pt\frac{1}{K}\sum_{k=1}^{K}\left|\mathbf{w}_{k}^{H}\mathbf{h}_{k}\right|,$
(6) $\displaystyle\operatorname{s.t.}\hskip 2.0pt$ $\displaystyle\hskip
2.0pt\tau_{n}\in\left[0,\tau_{\max}\right],~{}\forall n=1,2,\dots,N,$ (7)
$\displaystyle\hskip 2.0pt\theta_{m}\in\boldsymbol{\Psi},~{}\forall
m=1,2,\dots,M,$ (8)

where $\mathbf{w}_{k}$ is given by (2), $\tau_{\max}\in\mathbb{R}_{+}$ denotes
the maximum delay that the TD unit supports, and $\boldsymbol{\Psi}$ is a
finite set with $2^{r}$ possible phase values drawn uniformly from
$(-\pi,\pi]$. In addition to the constraints (7) and (8), we also assume that
the channels, $\mathbf{h}_{k},\forall k$, are unknown, as acquiring the
channel state information (CSI) is typically challenging in systems with fully
analog transceiver architectures [8].

##  IV Proposed Solution

In this section, we present our proposed near field wideband beam focusing
solution. The proposed approach decomposes the original joint design problem
(6) into two subproblems: (i) The quantized analog phase shifter design
subproblem, which focuses on maximizing the beamforming gain at the center
frequency, while setting all the TD units to have zero delays; and (ii) the TD
unit design subproblem, which focuses on maximizing the average beamforming
gain achieved across all the subcarriers by properly configuring the TD units.
Despite this decomposition, we will show in Section V that the performance
achieved by the proposed solution approaches that of the prior work [7] which
(different than our solution) requires full channel knowledge and uniform
array geometry. The motivation of this decomposition is partially originating
from the observation that unless the number of elements in each sub-array is
very large, then the performance degradation caused by the wideband effect for
this sub-array is negligible.

###  IV-A Decomposition of the Original Problem

The combining vector in (2) can be expressed as
$\mathbf{w}_{k}=\mathbf{w}_{\mathrm{TD},k}\odot\mathbf{w}_{\mathrm{PS}}$,
where $\mathbf{w}_{\mathrm{TD},k}=e^{-j2\pi
f_{k}\boldsymbol{\tau}\otimes\mathbf{1}_{P}}$ is the TD-based frequency-
dependent combining vector and
$\mathbf{w}_{\mathrm{PS}}=\frac{1}{\sqrt{M}}e^{j\boldsymbol{\theta}}$ is the
phase-shifter based frequency-flat combining vector. The symbol $\odot$
denotes the element-wise product. Correspondingly, the design of the quantized
analog phase shifters can be formulated as the following optimization problem

$\displaystyle\boldsymbol{\theta}^{\star}=\mathop{\arg\max}\limits_{\boldsymbol{\theta}}\hskip
2.0pt$ $\displaystyle\hskip
2.0pt\left|\mathbf{w}_{\mathrm{PS}}^{H}\mathbf{h}_{f_{c}}\right|,$ (9)
$\displaystyle\operatorname{s.t.}\hskip 4.0pt$ $\displaystyle\hskip
5.0pt[\mathbf{w}_{\mathrm{PS}}]_{m}=\frac{1}{\sqrt{M}}e^{j\theta_{m}},$ (10)
$\displaystyle\hskip 5.0pt\theta_{m}\in\boldsymbol{\Psi},~{}\forall
m=1,2,\dots,M,$ (11)

where $\mathbf{h}_{f_{c}}$ denotes the channel at the center frequency. After
obtaining $\boldsymbol{\theta}^{\star}$, the design problem of the TD units
can be expressed as follows

$\displaystyle\boldsymbol{\tau}^{\star}=\mathop{\arg\max}\limits_{\boldsymbol{\tau}}\hskip
2.0pt$ $\displaystyle\hskip
2.0pt\frac{1}{K}\sum_{k=1}^{K}\left|\left(\mathbf{w}_{\mathrm{TD},k}\odot\widehat{\mathbf{w}}_{\mathrm{PS}}^{\star}\right)^{H}\mathbf{h}_{k}\right|,$
(12) $\displaystyle\operatorname{s.t.}\hskip 2.0pt$
$\displaystyle\mathbf{w}_{\mathrm{TD},k}=e^{-j2\pi
f_{k}\boldsymbol{\tau}\otimes\mathbf{1}_{P}},$ (13) $\displaystyle\hskip
2.0pt\tau_{n}\in\left[0,\tau_{\max}\right],~{}\forall n=1,2,\dots,N,$ (14)
$\displaystyle\hskip
2.0pt[\widehat{\boldsymbol{\theta}}^{\star}]_{m}=\operatornamewithlimits{arg\min}_{\theta\in\boldsymbol{\Psi}}\left|\theta-\left([\boldsymbol{\theta}^{\star}]_{m}+2\pi
f_{c}\tau_{n}\right)\right|,$ $\displaystyle\hskip 2.0pt\forall n,\forall
m\in\\{(n-1)P+1,\dots,nP\\},$ (15)

where
$\widehat{\mathbf{w}}_{\mathrm{PS}}^{\star}=\frac{1}{\sqrt{M}}e^{j\widehat{\boldsymbol{\theta}}^{\star}}$.
The constraint (15) is to ensure that the gain at the center frequency, i.e.,
the objective function of (9), will not be influenced after the introduction
of the TD units. Next, in Sections IV-B and IV-C, we present the proposed
solutions for solving (9) and (12).

###  IV-B The Design of the Analog Phase Shifters

Figure 2: The adopted actor-critic learning architecture where the actor
network is implemented using the fully-connected neural network and the critic
network is designed to reflect the underlying signal model.

As mentioned before, the design of the quantized analog PSs focuses on
maximizing the beamforming/combining gain at the center frequency $f_{c}$. We
adopt the similar reinforcement learning (RL) formulation as in the previous
work [9]. Specifically, we define the state ${\bf s}_{t}$ as a vector that
consists of the phases of all the phase shifters at the $t$-th iteration, that
is, ${\bf s}_{t}=\left[\theta_{1},\theta_{2},\dots,\theta_{M}\right]^{T}$. We
define the action ${\bf a}_{t}$ as the element-wise changes to all the phases
in ${\bf s}_{t}$. Since the phases can only take values in
$\boldsymbol{\Psi}$, a change of a phase represents the action that a phase
shifter selects a value from $\boldsymbol{\Psi}$. Therefore, the action is
directly specified as the next state, i.e., ${\bf a}_{t}={\bf s}_{t+1}$, which
can be viewed as a deterministic transition in the Markov Decision Process
(MDP). We define the reward as the achieved beamforming gain difference
between the action (i.e., new beam, denoted as $\mathbf{w}_{\mathrm{PS},t}$)
and the state (i.e., the previous beam, denoted as
$\mathbf{w}_{\mathrm{PS},t-1}$). Formally,
$r_{t}=|\mathbf{w}_{\mathrm{PS},t}^{H}\mathbf{h}_{f_{c}}|^{2}-|\mathbf{w}_{\mathrm{PS},t-1}^{H}\mathbf{h}_{f_{c}}|^{2}$.

Moreover, to improve the sample efficiency of the proposed algorithm in
learning over large antenna arrays, we develop a signal model inspired online
RL-based search algorithm. Specifically, we improve the existing actor-critic
architecture based beam learning algorithm [9] by designing a special critic
network that better utilizes the underlying signal model. Formally, the critic
network takes the following form

$f({\bf s}_{t},{\bf a}_{t})=\mathbf{w}_{{\bf
a}_{t}}^{H}\mathbf{Q}\mathbf{Q}^{H}\mathbf{w}_{{\bf a}_{t}}-\mathbf{w}_{{\bf
s}_{t}}^{H}\mathbf{Q}\mathbf{Q}^{H}\mathbf{w}_{{\bf s}_{t}},$ (16)

where $\mathbf{Q}\in\mathbb{C}^{M\times v}$ is the model parameters with
$v\in\mathbb{N}_{+}$ being a hyperparameter, $\mathbf{w}_{{\bf
a}_{t}}=\frac{1}{\sqrt{M}}e^{j{\bf a}_{t}}$ and $\mathbf{w}_{{\bf
s}_{t}}=\frac{1}{\sqrt{M}}e^{j{\bf s}_{t}}$. The complete beam learning
framework is illustrated in Fig. 2.

###  IV-C The Design of the Time Delay Units

In this subsection, we introduce the proposed low complexity geometry-assisted
TD unit design algorithm. Before we delve into the details, it is worth
mentioning that the motivation of leveraging geometry to design the delay
configuration of the TD units can be found in the following observations.
First, the TD units are mainly adopted to compensate for the different delays
experienced at the different parts of the large array. This implies that the
design can be determined based on the distance differences between the
elements on the antenna array and the UE. Second, the absolute distance
difference between any two antenna elements on the array and the UE is upper
bounded by the array aperture, i.e., $D$. Third, depending on the relative
positions of the BS and UE, the delay profile on the BS’s array (i.e., the
experienced delay value as a function of antenna element) presents different
monotonicity properties.

These observations provide useful insights and guidance for the considered TD
units design as they help constrain the search space of the delay
configurations. Moreover, as indicated by the first observation, the
introduction of the TD units is to compensate for the differences in the
propagation delays experienced at the different receive antenna elements,
which are directly related to the different propagation distances. Therefore,
understanding and finding such distance differences could facilitate the
design of the TD units. Based on this, we introduce the concept of the
distance difference function (DDF), which directly determines the design of
the delay units. To be more specific, we set the point ${\bf
p}_{\mathrm{ref}}=[0,\frac{D}{2}]^{T}$ as the _reference point_ of the BS
array. Further, we define $\Delta=1-\alpha$ as the _relative coefficient_ with
respect to the _reference point_ 111As described in Section II-B, the position
of an antenna is determined via its coefficient $-1\leq\alpha\leq 1$., which
implies that $\Delta\in[0,2]$. Therefore, the _reference distance_ , defined
as the distance between ${\bf p}_{\mathrm{ref}}$ and the considered UE
position ${\bf q}$, is given by $d_{\mathrm{ref}}=\|{\bf
p}_{\mathrm{ref}}-{\bf q}\|_{2}$. Based on the reference distance, we define
the DDF $f_{\mathrm{d}}$ for any point on the array
${\mathbf{p}}_{\Delta}=[0,(1-\Delta)\frac{D}{2}]^{T}$ and the UE as

$f_{\mathrm{d}}(\Delta,{\bf q})=d(\Delta)-d_{\mathrm{ref}},$ (17)

where $d(\Delta)=\|{\mathbf{p}}_{\Delta}-{\bf q}\|_{2}$. As can be seen, the
DDF depends only on $\Delta$ and ${\bf q}$. Besides, $d_{\mathrm{ref}}$
corresponds to $\Delta=0$, and $f_{\mathrm{d}}=0$ when $\Delta=0$. Moreover,
an important observation is that the monotonicity property of $f_{\mathrm{d}}$
depends on the UE position ${\bf q}$ relative to the BS array. Without loss of
generality, we assume $q_{x}>0$. Then, the UE position space can be divided
into three regimes based on the value of $q_{y}$: (i) $q_{y}>\frac{D}{2}$;
(ii) $q_{y}<-\frac{D}{2}$; and (iii) $|q_{y}|<\frac{D}{2}$. For these regimes,
$f_{\mathrm{d}}$ is monotonically increasing when $q_{y}>\frac{D}{2}$ and is
monotonically decreasing when $q_{y}<-\frac{D}{2}$. When
$|q_{y}|<\frac{D}{2}$, $f_{\mathrm{d}}$ first decreases and then increases. It
is worth noting that the delay configuration of the TD units is closely
related to $f_{\mathrm{d}}$ which essentially characterizes the additional
propagation distance at different parts of the array. Specifically, the ideal
delay configuration of the $m$-th antenna is given by
$\frac{f_{\mathrm{d}}(\Delta_{m},{\bf q})}{c}$, where
$\Delta_{m}=1-\alpha_{m}$. However, as can be seen from (17), in order to
obtain the exact expression of $f_{\mathrm{d}}$, the UE position ${\bf q}$
should be known. Since in practice systems, however, the UE position is either
unavailable or noisy, it is important to develop search algorithms that are
able to find an approximation of $f_{\mathrm{d}}$ , and hence design the delay
configurations, without requiring the UE position. Next, we describe the
proposed low complexity delay search method. Each search cycle in this method
consists of the following three steps:

####  IV-C1 Select a Linear Approximation

Inspired by the behavior of the DDF $f_{\mathrm{d}}$, we propose a low
complexity linear approximation based search algorithm to design the delay
configurations of the TD units. Specifically, we use the following piecewise
linear function to approximate $f_{\mathrm{d}}$, that is

$\widehat{f}_{\mathrm{d}}(\Delta;\\{a_{x},a_{y},b\\})=\left\\{\begin{array}[]{ll}\frac{a_{y}}{a_{x}}\Delta,&0\leq\Delta\leq
a_{x},\\\ \frac{b-a_{y}}{2-a_{x}}(\Delta-a_{x})+a_{y},&a_{x}<\Delta\leq
2,\end{array}\right.$ (18)

where the approximation function $\widehat{f}_{\mathrm{d}}$ is parameterized
by $a_{x},a_{y},b$, with $(a_{x},a_{y})$ modelling the coordinate of the only
breakpoint of (18) and $b$ modelling the intersection of the second part of
(18) with $\Delta=2$. Therefore, the approximation problem essentially becomes
a joint search problem of $a_{x}$, $a_{y}$ and $b$. Since $a_{x}$, $a_{y}$ and
$b$ should satisfy the conditions determined by the properties of
$f_{\mathrm{d}}$, $0\leq a_{x}\leq 2$, $-\frac{D}{2}a_{x}\leq
a_{y}\leq\frac{D}{2}a_{x}$, $-D\leq b\leq D$, we discretize the original
continuous search space into a finite size discrete grid. Specifically, we
denote $\mathcal{A}_{x}\subseteq[0,2]$ as the finite search set that contains
the search points of $a_{x}$,
$\mathcal{A}_{y|a_{x}}\subseteq[-\frac{D}{2}a_{x},\frac{D}{2}a_{x}]$ as the
finite search set of $a_{y}$ given $a_{x}$, and $\mathcal{B}\subseteq[-D,D]$
as the finite search set of $b$. In each search cycle, one combination of the
possible values of $a_{x}$, $a_{y}$, and $b$ are selected, which gives one
candidate approximation of $f_{\mathrm{d}}$.

####  IV-C2 Compute the Delay Configurations

After obtaining $\widehat{f}_{\mathrm{d}}$, the delay search is then conducted
based on sampling of the corresponding _delay difference function_ , defined
as $\widehat{f}_{\tau}=\frac{\widehat{f}_{\mathrm{d}}}{c}$. If we set one
sampling point at the center of each sub-array to have the set
$\\{\Delta_{1},\Delta_{2},\dots,\Delta_{N}\\}$, with
$0\leq\Delta_{1}<\Delta_{2}<\cdots<\Delta_{N}\leq 2$, then, the delay
configuration vector will be

$\boldsymbol{\tau}=\left[\widehat{f}_{\tau}(\Delta_{1}),\widehat{f}_{\tau}(\Delta_{2}),\dots,\widehat{f}_{\tau}(\Delta_{N})\right]^{T}.$
(19)

####  IV-C3 Evaluate the Configured Delays

In this step, the analog phase shifters are first reconfigured to compensate
for the additional phase shifts introduced by the TD units, according to (15).
Then, the system measures the receive beamforming gain at the different
subcarriers to determine the performance of the current delay configurations.
This completes one cycle of the proposed delay search algorithm.

###  IV-D Complexity Analysis

For the analog PS design problem, the proposed signal model-based learning
architecture reduces the computational complexity when comparing with the
signal model-unaware architecture [9]. Specifically, the complexity reduction
of the proposed solution mainly comes from the following aspects. First, the
signal model-based critic network design reduces its complexity from
$\mathcal{O}(M^{2}N_{\mathrm{b}}N_{\mathrm{iter}})$, where we assume a fully-
connected neural network-based critic network architecture as in [9], to
$\mathcal{O}(MN_{\mathrm{b}}N_{\mathrm{iter}})$, with $N_{\mathrm{b}}$
denoting the mini-batch size and $N_{\mathrm{iter}}$ denoting the total number
of iterations. Second, from training perspective, the convergence of the
proposed solution is much faster than the signal model-unaware architectures,
meaning that $N_{\mathrm{iter}}$ is normally also small. To be more specific,
from the simulation result, it indicates that, for a $256$-antenna BS with
$3$-bit quantized PSs, the training of the critic network, depending on the
number of real power measurements, is able to converge within $1000\sim 5000$
iterations. And with a well-trained critic network, the actor network is
typically able to converge within only $20\sim 50$ iterations. Finally, due to
the decoupling of the original joint TD-PS design problem (6), the search
space becomes much smaller, which generally leads to less search complexity.

##  V Simulation Results

In this section, we evaluate the performance of the proposed near field
wideband beamforming design algorithm.

###  V-A Simulation Setup

In this simulation, we consider the scenario where a BS receiver adopts a
linear array with $256$ elements. While the positions of the antenna elements
within the array are randomly sampled, giving rise to an unknown and non-
uniform array geometry, we assume that the aperture of the array is given by
$(M-1)\frac{\lambda_{c}}{2}$, with $\lambda_{c}$ being the wavelength of the
center frequency and $M=256$. Furthermore, we assume that the system is
operating at a center frequency of $100$ GHz (hence $\lambda_{c}=0.3$
centimeter) and has a bandwidth of $10$ GHz. Moreover, each antenna of the
array is followed by a $3$-bit analog PS. The signals from the $P$ adjacent
PSs are first combined and then processed by the same TD unit, as illustrated
in Fig. 1. The value of $P$ depends on the number of TD units in the specific
simulations.

###  V-B Numerical Results

(a) Beam focusing at different frequencies

(b) Wideband performance

Figure 3: The evaluation of the proposed solution. (a) shows the beam focusing
performance at different frequencies (lowest, center and highest frequencies
of the considered bandwidth) to a user at ${\bf q}=[2,-2]^{T}$ that is marked
by the red square, where the figure to the left shows the performance of PS-
only architecture and the figure to the right shows the performance of the TD-
PS hybrid architecture with the proposed beam focusing algorithm. (b) shows
the beamforming gain performance with different numbers of TD units.

In Fig. 3(a), we visualize the beam focusing performance of the proposed
solution at different frequencies to a user at ${\bf q}=[2,-2]^{T}$, where we
show the lowest, center and highest frequencies of the considered bandwidth.
The average beamforming gain performance at these frequencies is plotted on
the considered user grid. Specifically, the figure to the left in Fig. 3(a)
shows the performance of the PS-only architecture with the phase shifts
designed based on the center frequency. As can be seen, due to the frequency-
flatness of the analog phase-shifters, the beam can only focus on the
considered user at the center frequency, while being completely deviated at
the other two frequencies, an effect that is oftentimes referred to as beam
split. By leveraging the TD-PS hybrid architecture and the proposed beam
focusing solution, the beamforming vector becomes frequency-dependent. As a
result, the beams at different frequencies are all able to focus on the user,
as shown in the figure to the right in Fig. 3(a).

The wideband performance of the proposed algorithm when utilizing different
numbers of TD units is then evaluated. As can be seen from Fig. 3(b), if the
BS adopts a PS-only architecture (without TD units), the system experiences
quite severe performance degradation in the considered bandwidth.
Specifically, although the system has a bandwidth of $10$ GHz, the 3 dB
bandwidth achieved by the PS-only architecture is only around $1$ GHz.
Moreover, most of the subcarriers experience less than $-10$ dB beamforming
gain with considerable subcarriers having even over $-20$ dB beamforming gain
loss compared to the center frequency. However, by leveraging only $8$ TD
units, meaning that every $32$ antennas share one TD unit, the performance
gets improved quite significantly, where the 3 dB bandwidth is widened to
almost $6$ GHz. If the number of the TD units is allowed to be further
increased to 16 TD units, the system is able to achieve almost frequency-flat
performance. To better understand the achieved performance by the proposed
solution, we compare it with the prior work [7], where the authors propose a
phase-delay focusing (PDF) method that achieves beam focusing with perfect
channel knowledge. As can be seen in Fig. 3(b), the proposed solution is able
to approach the PDF performance with negligible difference. It is worth noting
that the proposed solution achieves such performance while not requiring any
channel knowledge and while being generic to arrays with arbitrary geometries.
This reduces the system operation overhead and enhances the robustness of the
beam focusing solution against various hardware imperfections.

##  VI Conclusions

In this paper, we developed a low-complexity wideband beamforming solution for
hybrid TD-PS based RF architectures operating in the near-field regime. To
perform beamforming, the proposed solution does not require any explicit
channel knowledge and relies only on the power measurements of the received
signal at different subcarriers. The sample efficiency of the proposed
solution is enhanced by designing a signal model inspired actor-critic
architecture to learn the phase shifts, and by leveraging a geometry-assisted
linear approximation model to configure the TD units. Important future
extensions of this work include near-field wideband beam codebook learning,
interference aware near-field beam pattern learning, and beam/codebook
learning under user mobility.

## References

  * [1] M. Longbrake, “True time-delay beamsteering for radar,” in _2012 IEEE National Aerospace and Electronics Conference (NAECON)_ , 2012, pp. 246–249.
  * [2] E. Ghaderi _et al._ , “An Integrated Discrete-Time Delay-Compensating Technique for Large-Array Beamformers,” _IEEE Transactions on Circuits and Systems I: Regular Papers_ , vol. 66, no. 9, pp. 3296–3306, 2019.
  * [3] V. Boljanovic _et al._ , “Fast Beam Training With True-Time-Delay Arrays in Wideband Millimeter-Wave Systems,” _IEEE Transactions on Circuits and Systems I: Regular Papers_ , vol. 68, no. 4, pp. 1727–1739, 2021.
  * [4] X. Liu and D. Qiao, “Space-Time Block Coding-Based Beamforming for Beam Squint Compensation,” _IEEE Wireless Communications Letters_ , vol. 8, no. 1, pp. 241–244, 2019.
  * [5] N. J. Myers and R. W. Heath, “InFocus: A Spatial Coding Technique to Mitigate Misfocus in Near-Field LoS Beamforming,” _IEEE Transactions on Wireless Communications_ , vol. 21, no. 4, pp. 2193–2209, 2022.
  * [6] L. Dai _et al._ , “Delay-Phase Precoding for Wideband THz Massive MIMO,” _IEEE Transactions on Wireless Communications_ , 2022.
  * [7] M. Cui, L. Dai, R. Schober, and L. Hanzo, “Near-Field Wideband Beamforming for Extremely Large Antenna Arrays,” 2021. [Online]. Available: https://arxiv.org/abs/2109.10054
  * [8] A. Alkhateeb _et al._ , “Channel Estimation and Hybrid Precoding for Millimeter Wave Cellular Systems,” _IEEE Journal of Selected Topics in Signal Processing_ , vol. 8, no. 5, pp. 831–846, Oct. 2014\.
  * [9] Y. Zhang _et al._ , “Reinforcement Learning of Beam Codebooks in Millimeter Wave and Terahertz MIMO Systems,” _IEEE Transactions on Communications_ , vol. 70, no. 2, pp. 904–919, 2022.


# Deep 6-DoF Tracking of Unknown Objects for Reactive Grasping

Marc Tuscher1,2 Julian Hörz2 Danny Driess2,3 Marc Toussaint3,4 1 sereact.2
Machine Learning and Robotics Lab, University of Stuttgart.3 Max-Planck
Institute for Intelligent Systems, Stuttgart.4 Learning and Intelligent
Systems, TU Berlin.

###### Abstract

Robotic manipulation of unknown objects is an important field of research.
Practical applications occur in many real-world settings where robots need to
interact with an unknown environment. We tackle the problem of reactive
grasping by proposing a method for unknown object tracking, grasp point
sampling and dynamic trajectory planning. Our object tracking method combines
Siamese Networks with an Iterative Closest Point approach for pointcloud
registration into a method for 6-DoF unknown object tracking. The method does
not require further training and is robust to noise and occlusion. We propose
a robotic manipulation system, which is able to grasp a wide variety of
formerly unseen objects and is robust against object perturbations and
inferior grasping points.

##  I Introduction

Aiming towards a wide-spread application of robots in natural or unstructured
industrial environments, methods for robotic perception need to incorporate
more general approaches. Robots manipulating in noisy and cluttered real-world
scenarios require the ability to interact with arbitrary, unknown objects.
Interacting with objects in the real world requires the ability to efficiently
perceive the target object. Target objects need to be tracked in real-time.
Versatile and robust tracking algorithms are required. Further, in order to
act in a dynamically changing environment, robots need to react to changes and
collaborate with other agents.

This work tackles the problem of reactive grasping of unknown objects. That
is, grasping of unknown objects under perturbation and changes of the
environment. We divide the problem of reactive grasping into three
subproblems: detection and real-time tracking of unknown objects, computation
of grasp configurations and reactive trajectory planning.

(a) Yellow cuboid

(b) Pringles can

(c) Measuring tape

(d) Lashing strap

Figure 1: Tracking and grasping unknown objects.

Object tracking methods are often trained on specific classes of objects [1,
2] or require a 3D model of the target object a priori [3, 4, 5, 6]. This
heavily limits their usability for practical applications. Existing methods
for unknown object tracking either target coarse grained tracking [7, 8, 9,
10], are prone to pose drifting [11] or limited to a specific environment
[12]. Existing methods for grasp point detection are too slow for for real-
time evaluation [13] or do not take the current robot configuration into
account [14, 15]. Common methods for trajectory planning are designed for
static scenes. Specifically, standard Rapidly Random Trees for trajectory
planning are designed for static scenes only.

We propose a method for 6-DoF tracking of unknown objects based on Siamese
Networks for 2D tracking and Iterative Closest Point (ICP) for pointcloud
registration. This method is able to track the 6-DoF pose of a wide variety of
objects. That is, given a set of RGB templates captured during tracking and a
pointcloud template of the object captured at an initialization phase, the
method outputs the subsequent poses of the target object in real-time.
Further, we propose a method for grasp candidate computation and evaluation,
taking the depth of the target object and the current robot state into
account. We also propose a method for reactive trajectory planning in the
presence of dynamic changes and collisions. This system is able to efficiently
manipulate a diverse set of objects. We demonstrate this ability in our
accompanying video111https://youtu.be/Hew00rMw8qg. We summarize our
contributions as follows:

  1. 1.

Our approach combines Siamese Networks for RGB tracking with ICP to realize a
conceptually simple, but powerful 6-DoF tracking method: this method is able
to track a wide variety of unknown objects under occlusion in real-time, while
being class-agnostic and model-free.

  2. 2.

We propose a novel method for reactive grasping, able to grasp dynamic objects
even under perturbation of the robot or the target object.

  3. 3.

We combine both approaches with a method for reactive trajectory planning
accounting for dynamic changes of the environment.

The paper is structured as follows: in section II related work is presented,
section III provides background on the methods we built upon, section IV
explains our object tracking methond and section V describes the approach to
reactive grasping. In section VI experiments are conducted in simulation and
on a real robot.

##  II Related Work

###  II-A Object Tracking

#### Model-based tracking

One predominant paradigm for 6-DoF object tracking is model-based object
tracking. That is, a 3 dimensional description of the target object is
available a priori. A large body of work has been evaluated on this topic.
Particle Filtering [16] proves to be useful in model-based object tracking
[17]. Particle filter based tracking models often operate directly on
pointclouds. Incorporating control inputs from the robot, to give a good
estimate on the current object is also successfully applied to particle filter
trackers [3]. Other appraoches extend particle filter tracking to multi-target
tracking using a mixture in particles [17] and learning the correct
correspondence to a certain target object via AdaBoost [18]. [19] uses a
particle filter algorithm operating on RGB-pointclouds, thus, integrating
texture information into tracking.

Fully integrating the robot kinematics in object tracking helps to reduce the
degrees of freedom of the target object and serves as a good prior for vision
based object tracking in robotic manipulation [5, 20]. However, such
approaches rely on both, an object model and a robot model being present
beforehand, and reduce the general applicability of the system.

Rendering multiple simulated views from the 3D model of the target object can
be used to leverage deep transfer learning for pose estimation [21]. Garon et.
al [4] use convolutional neural networks to directly operate on RGBD data, by
training from scratch for each individual object. SegICP [22] successfully
combines deep neural networks for semantic segmentation with ICP for known
object tracking.

#### Pose estimation

Pose estimation and Object Detection are related fields of research: in [23] a
deep learning method for object detection is extended by an object database
yielding an object tracking system. Many approaches extend deep learning
models for visual object detection to 6-DoF pose estimation from RGB data [24,
25, 26]. Applying deep learning models directly on pointcloud data seems
promising since the rise of PointNet feature extractors [27, 28]. VoteNet [29]
combines deep learning with Hough voting [30] to predict 3D bounding boxes on
pointcloud input data.

However, most approaches to pose estimation lack the ability to run in real-
time, and therefore render impractical for robotic manipulation. Tremblay et.
al [1] train a deep neural network for pose estimation on synthetic RGB data
using domain randomization. The method is suited as a real-time system for
real-world robotic grasping of known objects. All approaches in this
paragraph, however, are only able to track objects from a set of predefined
classes.

#### Unknown objects

Despite the practical importance, only little research is done on tracking of
unknown objects in robotics applications. Early work uses optical flow
tracking and an integrated eye-in-hand vision system to grasp arbitrary
objects with a visual servoing appraoch [31]. Experiments show that the
approach only works in a specific environment. Other approaches rely on
detecting and segmenting unkown objects in a known environment [12] or do not
take the shape of the object into account [32]. Methods based on a large
number of different algorithms, each tuned for a specific application, are
usually brittle and sensitive to changing environments. A more recent approach
to unknown object tracking applies multiple deep learning models to first
segment the scene into model segments, then predict the position and
orientation relative to the last frame [11]. Therefore, the method bootstraps
the current object pose on earlier estimations. Experiments show that this
approach is particularly prone to tracking drift: stable tracking of real-
world objects is only possible for approximately one second.

###  II-B Grasp Candidate Detection

Deep learning for robotic perception has changed the field of computer vision
based grasp candidate detection. Existing approaches show that using neural
networks on RGBD input successfully discriminate grasp candidates to evaluate
the best grasp to manipulate objects [33, 34]. Neural nets are also used to
generate 6-DoF grasps enabling more informed grasping and manipulation of
unknown objects [35]. Recent work concentrates on the generation of dense
pixel-wise quality maps and pixel-wise grasp information to generate multiple
grasps at once [14]. These approaches use small neural networks to achieve
real-time generation of grasp candidates and repeatedly set a new state-of-
the-art on international grasping benchmarks [15]. However, such methods do
not take the robot configuration into account.

###  II-C Dynamic Trajectory Planning

Reactive planning is formerly shown to perform well in complex manipulation
tasks [36]. Dynamic changes of the environment can also be efficiently handled
by sampling based approaches [37].

Our method for object tracking combines Siamese Networks for template-matching
in RGB frames with ICP-based pointcloud registration. Using this object
tracking method we disentangle perception from grasp point calculation,
providing the flexibilty to exectute complex queries on the scene and taking
the current robot configuration into account. We further implement a variant
of the conceptually simple bug algorithm in configuration space able to plan
collision-free trajectories in a dynamic environment.

##  III SiamMask & THOR

The method for 6-DoF tracking proposed in this work is built on SiamMask [38]
and THOR [39] for template-matching based RGB object tracking. This section
provides background on these methods. For more details please consider the
original papers.

Given a template $T$ and an input $x$, SiamMask computes a cross-correlated
feature map

$g_{\theta}(T,~{}x)=f_{\theta}(T)\star f_{\theta}(x),$ (1)

where $f_{\theta}$ denotes the feature extraction model used to process both,
the novel input and the template. Equation 1 yields a bounding box in image
coordinates and a tracking score. Computing a segmentation mask of the target
object in the RGB observation is done using a separate branch in the neural
network for mask refinement.

THOR consists of a long-term and a short-term module of RGB templates which
can be used alongside SiamMask. In each iteration of tracking a subregion $x$,
corresponding to the estimated position and size of the target object, is
cropped from the current frame and fed through the SiamMask model alongside
each individual RGB template from the THOR long-term module (LTM)
$T_{l1},\dots,T_{l5}$ and short-term module (STM) $T_{s1},\dots,T_{s5}$

$g_{\theta}(T_{l1},~{}\dots,~{}T_{l5};T_{s1},~{}\dots,~{}T_{s5};x).$ (2)

Templates in the STM are updated in a first-in first-out manner. Every $10$
iterations a new template is generated from the current crop and appended to
the STM, while the oldest template in the STM is removed. $T_{l1}$, the ground
truth template, remains in the LTM.

Let $z_{i}=f_{\theta}(T_{i})$ denote the features extracted from template
$T_{i}$. A Gram Matrix

$\displaystyle\begin{bmatrix}z_{1}\star z_{1}&z_{1}\star
z_{2}&\cdots&z_{1}\star z_{m}\\\ \vdots&\vdots&\ddots&\vdots\\\ z_{m}\star
z_{1}&z_{m}\star z_{2}&\cdots&z_{m}\star z_{m}\end{bmatrix}$

is constructed, where each entry $G_{ij}$ expresses the similarity of
templates $T_{i}$ and $T_{j}$ by cross-correlating the features $z_{i}$ and
$z_{j}$. If a novel template $T_{c}$ is similar enough to the ground-truth
template $T_{l1}$ and replacing an existing template in the LTM increases the
volume of the parallelotope $\Gamma\left(z_{1},~{}\dots~{},z_{5}\right)$
spanned by the feature vectors of each template, it is added to the LTM. A new
template $T_{c}$ is similar enough, if its features $z_{c}$ satisfy the
inequality

$z_{c}\star z_{1}>l\cdot G_{11}-\gamma,$ (3)

where $l$ is a hyperparameter to trade-off tracking performance against
drifting robustness. $\gamma$ is computed using the templates in the STM

$\gamma=1-\frac{2}{N(N+1)G_{st,max}}\sum^{N}_{i<j}{G_{st,ij}}$ (4)

where $G_{st}$ is a Gram Matrix for the STM similar to $G$.

##  IV 6-DoF Tracking

This section describes the approach this work takes on tracking arbitrary
objects in 6 degrees of freedom. Since this approach is model-free, only
information from a single RGBD camera at initialization time is used to
describe the target object. Initialization of the tracking method requires an
initial (arbitrarily defined) object pose in world coordinates and a 2D
bounding box in image coordinates. We assume that we are able to detect an
initial pose and a 2D bounding box of an object.

Briefly, our method works as follows:

  1. 1.

In an initialization phase, 3D bounding boxes and cartesian poses of arbitrary
objects on a table are detected using background subtraction in depth images
from a top down view.

  2. 2.

SiamMask + THOR is started to track the target object in RGB frames. An
initial pointcloud is saved for 3D pose estimation via pointcloud
registration.

  3. 3.

SiamMask tracks the target object in new frames in real-time, a pointcloud of
this observation is segmented from the depth image. The initial pointcloud is
fit to the new observation using Generalized-ICP [40] to retrieve the 6-DoF
transform.

Figure 2 shows an overview of the tracking system. The system maps an RGBD
input alongside an initial guess (gained from the previous iteration or a
kinematic map) of the target object pose to an estimate of the current object
pose. We call our object tracking method THOR + G-ICP.

Figure 2: Overview of our object tracking component.

#### Initialization

RGB tracking is initialized by supplying an initial frame and a bounding box
containing the target object to the tracker. The bounding box is used to crop
the frame to an initial RGB template $T_{init}$. The THOR long-term module
(LTM)

$T_{l1},~{}\dots,~{}T_{l5}\leftarrow T_{init}$ (5)

and short-term module (STM)

$T_{s1},~{}\dots,~{}T_{s5}\leftarrow T_{init}$ (6)

are filled with this initial template of the object.

In order to find a template pointcloud of the target object, the segmentation
mask provided by SiamMask in the first iteration of RGB tracking is used to
retrieve the pixels $d_{ij}\in{\mathbb{R}}$ in the depth image, which
correspond to points on the target object’s surface. We project the depth
values $d_{ij}$ to points $x^{k}\in{\mathbb{R}}^{3}$ in world coordinates
using the inverse camera projection $\hat{P}\in{\mathbb{R}}^{3\times 4}$ via

$x^{k}=\hat{P}\cdot\left(i\cdot d_{ij}~{}~{}j\cdot
d_{ij}~{}~{}d_{ij}~{}~{}1\right)^{T}.$ (7)

The set of these points $\mathcal{P}_{temp}=\\{x^{k}\\}_{k=1}^{N}$ is the
template pointcloud, which is registered to a novel observation to retrieve
the transformation of the target object in world coordinates.

#### Tracking

RGB tracking is performed according to SiamMask with THOR as template module.
In every step, SiamMask relies on cropping the correct subwindow from the
image to perform template matching on a smaller region of the image. Cropping
the correct subwindow, depends on prior information on the target object’s
location and its size in the image frame. If the robot is not moving the
object, the position and size of the object in the previous frame is used. If
the robot is moving the object, a kinematic map
$\phi:~{}{\mathbb{R}}^{n}\rightarrow{\mathbb{R}}^{3}$ is used to compute an
estimate of the current object pose and a 3D bounding box containing the
object. A 2D bounding box in image coordinates is then found by projecting the
corners of the 3D bounding box to image coordinates and computing the bounding
rectangle containing all corners. A step of RGB tracking yields a segmentation
mask containing the target object in the current frame. This segmentation mask
is used to select the corresponding pixels in the depth image. These pixels
are then projected to points on the object’s surface in world coordinates
using equation 7. This set of points $\mathcal{P}_{obs}$ is used as an
observation to fit the template pointcloud $\mathcal{P}_{temp}$.

To reduce the computational cost of pointcloud registration, both pointclouds
are preprocessed by voxel grid filtering, reducing the dimesionality of the
pointcloud while keeping the geometrical properties.

Pointcloud fitting is done using the Generalized-ICP algorithm, setting either
the last known pose of the object or the pose computed using a kinematic map,
in case the robot is handling the target object, as an initial guess.
Formally, this can be seen as a function mapping from a set of pointclouds and
an initial guess $X$ to final transformation $Y$

$\begin{split}h_{\text{G-ICP}}&:~{}{\mathbb{R}}^{n\times
3}\times{\mathbb{R}}^{m\times 3}\times{\mathbb{R}}^{4\times
4}\rightarrow{\mathbb{R}}^{4\times 4}\\\
h_{\text{G-ICP}}&:~{}(\mathcal{P}_{obs},\mathcal{P}_{temp},X)\mapsto
Y.\end{split}$ (8)

We use this 6-DoF tracking method to perform reactive grasping on a real
robot.

##  V Reactive Grasping

In this section we develop a method to grasp (cuboid-shaped) objects, which is
able to dynamically react to disturbances, both in the environment and the
robot configuration. While our tracking method is able to process arbitrarily
defined poses, this method requires the pose to be sensibly defined. That is,
the coordinate system axes should be aligned with the edges of a bounding box
surrounding the target object.

(a) Linearly interpolating between $q_{t}$ and $q^{\ast}$ would lead to
collisions with the object.

(b) After backstepping two steps, the shortest path would still end in
collisions.

(c) After four backsteps we can take the path to our target trajectory.

(d) We follow the target trajectory towards $q^{\ast}$.

Figure 3: Procedure of backstepping if collisions are encountered on the
linear interpolation between $q_{t}$ and $q^{\ast}$.

###  V-A Grasp Configuration Observer

We propose a dynamic grasp configuration observer to find a variety of $N$
grasp candidates for a cuboid-shaped object. Since our tracking module outputs
the pose of a cuboid-shaped bounding box around an object, this approach is
also used to manipulate arbitrarily shaped objects.

Let $q\in{\mathbb{R}}^{n}$ be the robot configuration. A grasp configuration
candidate $g=(q,c,a)$ is defined by a joint configuration $q$, a cost
$c\in{\mathbb{R}}$, and an age $a\in{\mathbb{N}}$. Grasp candidates are
determined by minimizing an inverse kinematics problem

$\displaystyle
q^{*}=\operatorname*{arg\,min}_{q\in\mathbb{R}^{n}}\|q\|^{2}_{W}+\sum_{i=1}^{M}\lambda_{i}\left\|\phi_{i}(q)\right\|^{2},$
(9)

where
$\phi_{i}:~{}{\mathbb{R}}^{n}\rightarrow{\mathbb{R}}^{d_{i}},~{}~{}y_{i}=\phi_{i}(q)$
are task maps that map a robot configuration to a $d_{i}$-dimensional space
and $\lambda_{i}\in{\mathbb{R}}$ their weighting factors. $W$ is a positive
definite matrix providing regularization.

The following cost terms are considered during optimization.

#### Position

The position cost is defined as

$\phi_{pos}(q)=p_{o}(q)-p_{e}(q),$ (10)

where $p_{o}:{\mathbb{R}}^{n}\rightarrow{\mathbb{R}}^{3}$ and
$p_{e}:{\mathbb{R}}^{n}\rightarrow{\mathbb{R}}^{3}$ are functions mapping the
configuration $q$ to the position of the target object and the end-effector,
respectively.

#### Alignment

Performing successful grasipng requires the end-effector to be aligned with
the object. We formulate the alignment cost as

$\displaystyle\phi_{align}(q)=$
$\displaystyle\left(1-\left(v_{o,x}(q)^{T}v_{e,x}(q)\right)^{2}\right)$ (11)
$\displaystyle\left(1-\left(v_{o,y}(q)^{T}v_{e,x}(q)\right)^{2}\right)$ (12)
$\displaystyle\left(1-\left(v_{o,z}(q)^{T}v_{e,x}(q)\right)^{2}\right),$ (13)

where $v_{o,x}:{\mathbb{R}}^{n}\rightarrow{\mathbb{R}}^{3}$ maps the
configuration to the unit vector described by the $x$-axis of the target
object coordinate system, analogously $v_{o,y}(q)$ and $v_{o,z}(q)$ for the
$y$\- and $z$-axis. $v_{e,x}(q)$ maps to the unit vector describing $x$-axis
of the end-effector coordinate system. Minimizing this cost term aligns
$v_{e,x}$ to one of the axes of the target object coordinate system.

#### Collision

In order to successfully grasp the object, we need to avoid collisions with
the object itself. The collision cost term is defined as

$\phi_{coll}(q)=(d(q)-m)[d(q)<m]$ (14)

where $d$ maps $q$ to the pairwise distances of all shapes of the robot and
the target object. $m$ is an upper threshold on the minimum distance. If the
distance of two shapes is less than $m$ this cost term becomes active.

#### Joint Limits & Homing

We further add a cost term $\phi(q)_{limit}$ increasing when joint limits of
the robot are violated and a homing cost term $\phi(q)_{home}$.

$q^{\ast}$ is the configuration of a feasible grasp. However, computing $N$
grasp candidates starting at the same configuration $q_{0}$ results in all
candidates defining the same configuration $q^{\ast}$. This is especially
relevant for the alignment term $\phi_{align}$, which has multiple local
minima. Instead of explicitly enumerating possible different alignments as in
[41, 42], we diversify grasp candidate computation by introducing a random
alignment term, effectively randomizing the approach axis of a grasp
candidate. That is, at initialization we compute a random orthonomal basis

$R=\begin{pmatrix}v_{1}&v_{2}&v_{3}\end{pmatrix}\in{\mathbb{R}}^{3\times 3}.$
(15)

We use $R$ to introduce an additional cost term

$\displaystyle\begin{split}\phi_{rand}(q)=\begin{pmatrix}1-\left(v_{1}(q)^{T}v_{x,e}(q)\right)\left(v_{1}(q)^{T}v_{x,e}(q)\right)\\\
1-\left(v_{2}(q)^{T}v_{y,e}(q)\right)\left(v_{2}(q)^{T}v_{y,e}(q)\right)\\\
1-\left(v_{3}(q)^{T}v_{z,e}(q)\right)\left(v_{3}(q)^{T}v_{z,e}(q)\right)\end{pmatrix}.\end{split}$

We further introduce two additional weighting functions

$\displaystyle f_{inc}(a)$
$\displaystyle=\frac{1}{1+\exp(\frac{\alpha}{2}-a)}$ (16) $\displaystyle
f_{dec}(a)$ $\displaystyle=\frac{1}{1+\exp(a-\frac{\alpha}{2})}$ (17)

depending on the age $a$ of a grasp candidate. $\alpha$ is a constant
threshold. During optimization we weight
$\phi_{pos}(q),~{}\phi_{align}(q),~{}\phi_{coll}(q)$ with $f_{inc}(a)$ and
$\phi_{rand}(q)$ with $f_{dec}(a)$ ensuring that only the approach axis is
randomized and no force balance arises between $\phi_{align}(q)$ and
$\phi_{rand}(q)$.

###  V-B Grasp Candidate Ranking

Choosing the best grasp from $N$ grasp candidates is non-trivial. The
essential and hard requirement for a good grasp candidate is whether the grasp
configuration ends in an end-effector position inside the target cuboid. We
compute this using the end-effector position relative to the cuboids
coordinate system and the shape of the cuboid. We assume that success of
grasping increases with decreasing distance to the center of the cuboid. If
two candidates are both inside the cuboid and also share the same distance to
the center, we choose the one with lower homing costs.

###  V-C Updating Grasp Candidates

Since an online reactive grasping approach needs to take the changing
environment into account, we continously update the grasp candidates. At
initialization all grasp candidates have the same cost vector $c$. In each
iteration the age $a$ of each grasp is incremented and the cost vector of each
grasp is evaluated using the current configuration $q_{t}$.

###  V-D Trajectory Planning

Our goal is to follow a trajectory starting in the current configuration
$q_{t}$ and ending in the configuration $q^{\ast}$ of the best grasp candidate
$q^{\ast}$. Due to the possibilty of a changing environment, and more
specifically the change of the target object pose, we cannot plan a full
trajectory in advance. Movements of the target object can lead to collisions
during a grasp approach following a pre-planned trajectory. We mitigate this
problem by using a technique we call backstepping.

Our goal is to follow a target trajectory $T$ defined by the linear
interpolation with stepsize $\tau$ between the homing configuration $q_{0}$
and the configuration $q^{\ast}$ of the best grasp candidate. At each step we
find the closest configuration $q_{s}$ on the target trajectory $T$ to our
current configuration $q_{t}$. If a step of stepsize $\tau$ ends up in a
collision with an object, we take a step towards $q_{0}$. The procedure of
backstepping is depicted in figure 3.

##  VI Experiments

This section describes experiments conducted on object tracking using THOR +
G-ICP in simulation and experiments using the complete system for reactive
grasping on a real robot.

###  VI-A Object Tracking

For quantitive comparison to an existing method we conduct experiments in
simulation. We render a camera view on the scene which is located at the same
pose as our camera on the real world setup. We compare THOR + G-ICP to a
particle-filter based object tracking method from the PCL library [43] (named
particle in the plots). Errors are reported in terms of position and rotation.
Rotational errors are reported in terms of the geodesic loss.

#### Occlusion

Occlusions occur frequently during robotic manipulation. When grasping an
object with a parallel jaw gripper, parts of the object are already occluded
by the gripper fingers. In this experiment a small box moving into the scene
from the top occludes the target object, as seen in figure 4(a). Results, seen
in figure 4(c), show that occlusion has almost no impact on the performance of
THOR + G-ICP. The accuracy of the particle tracker, however, is reduced
proportional to the amount of occlusion. The stability of THOR + G-ICP relies
on the fact that SiamMask is able to accurately segment the tracked object
from the occluding object. As seen in figure 4(b), the segmentation mask of
SiamMask contains parts of the target object only. G-ICP is able to fit the
template pointcloud to the observations seamlessly, since the observation
pointcloud omits the occluded part.

(a) A small rectangle occludes the target object.

(b) Camera view on the scene. The object, which occludes the target object, is
not contained in the segmentation mask.

(c) Results of object tracking during occlusion. THOR + G-ICP remains stable
during the complete tracking sequence.

Figure 4: Object tracking during occlusion.

#### Manipulation

Supporting robotic manipulation is the main purpose of our object tracking
method and therefore an important experiment. It incorporates many different
possibilites which can occur during object tracking, e.g. translational and
orientational movements as well as partial occlusion due to the gripper
fingers or the second robot. Figure 5 shows the manipulation process and
results of object tracking during manipulation. Results of object tracking are
depicted in figure 5(e). These show that both methods are able to track the
target object during manipulation. However, the particle filter tracker loses
the object once completely. This corresponds to the moment of the handover
(figure 5(b)) and is due to the occlusion both gripper fingers generate on the
object. Tracking with THOR + G-ICP is stable during the whole manipulation
process and also generates almost no rotational error.

(a)

(b)

(c)

(d)

(e) Results of tracking during manipulation.

Figure 5: Object tracking during manipulation. In a) the first robot picks up
the target object and hands it over to the second robot in b). The second
robot moves the object to an intermediate position in c) and finally moves to
the goal position d) while keeping the object in hand.

###  VI-B Reactive Grasping

We perform reactive grasping with a Franka Emika Panda Robot. Objects on the
table are detected from a top down view using a ASUS Xtion PRO LIVE RGBD
camera. Once an object on the table is detected, THOR + G-ICP is initialized
with an Intel RealSense D415 camera from a side view. These experiments are
also shown in our accompanying video.

#### Unknown Objects

Experiments on grasping of unknown objects are conducted in this section.
Figure 1 shows the objects which are manipulated in our experiments. THOR +
G-ICP is able to track each of these objects during the complete grasping
process. Further, the approach to reactive grasping presented in this work is
able to grasp each object even under disturbances. The pringles can is moved
by a human during the manipulation process. However, our approach is able to
grasp the object successfully. Notably, the gripper finger occludes the
measuring tape by more than half of its size in the camera view. The lashing
strap is surrounded by a plastic firm, exposing heavy reflections. Although
reflections impose difficulties on classical computer vision techniques, THOR
+ G-ICP is able to track the lashing strap effortlessly.

#### Collaborative Manipulation

Experiments on grasping under heavy disturbances are conducted. During theses
experiment the robot and the target object are moved and re-oriented by a
human during grasp approaches. The robot is also prevented from grasping the
object and moved to a position beside the object, likely leading to collisions
with the object. Target objects are successfully tracked throughout the
process and the path planning component is able to recover from each state,
leading to a successful grasp in each experiment.

##  VII Conclusion

We present a system to track and dynamically manipulate unknown objects. Our
experiments show that our approach to reactive grasping is successful even
under occlusion of the target object and under perturbation of either the
robot or the object.

## References

  * [1] J. Tremblay, T. To, B. Sundaralingam, Y. Xiang, D. Fox, and S. Birchfield, “Deep Object Pose Estimation for Semantic Robotic Grasping of Household Objects,” _arXiv:1809.10790 [cs]_ , Sept. 2018.
  * [2] S. Giancola, J. Zarzar, and B. Ghanem, “Leveraging Shape Completion for 3D Siamese Tracking,” _arXiv:1903.01784 [cs]_ , Mar. 2019.
  * [3] M. Wüthrich, P. Pastor, M. Kalakrishnan, J. Bohg, and S. Schaal, “Probabilistic Object Tracking using a Range Camera,” _2013 IEEE/RSJ International Conference on Intelligent Robots and Systems_ , pp. 3195–3202, Nov. 2013.
  * [4] M. Garon and J.-F. Lalonde, “Deep 6-DOF Tracking,” _IEEE Transactions on Visualization and Computer Graphics_ , vol. 23, no. 11, pp. 2410–2418, Nov. 2017\.
  * [5] T. Schmidt, R. Newcombe, and D. Fox, “DART: Dense Articulated Real-Time Tracking,” in _Robotics: Science and Systems X_. Robotics: Science and Systems Foundation, July 2014.
  * [6] J. Schulman, A. Lee, J. Ho, and P. Abbeel, “Tracking deformable objects with point clouds,” in _2013 IEEE International Conference on Robotics and Automation_. Karlsruhe, Germany: IEEE, May 2013, pp. 1130–1137.
  * [7] A. Feldman, T. Balch, M. Hybinette, and R. Cavallaro, “The Multi-ICP Tracker: An Online Algorithm for Tracking Multiple Interacting Targets,” p. 22.
  * [8] J. Cho, S. Jin, X. Pham, J. Jeon, J. Byun, and H. Kang, “A Real-Time Object Tracking System Using a Particle Filter,” in _2006 IEEE/RSJ International Conference on Intelligent Robots and Systems_. Beijing, China: IEEE, Oct. 2006, pp. 2822–2827.
  * [9] A. Almeida, J. Almeida, and Rui Araujo, “Real-Time Tracking of Moving Objects Using Particle Filters,” in _Proceedings of the IEEE International Symposium on Industrial Electronics, 2005. ISIE 2005._ Dubrovnik, Croatia: IEEE, 2005, pp. 1327–1332.
  * [10] M. Fleder and S. Pillai, “3D Object Tracking Using the Kinect,” p. 8.
  * [11] F. Leeb, A. Byravan, and D. Fox, “Motion-Nets: 6D Tracking of Unknown Objects in Unseen Environments using RGB,” _arXiv:1910.13942 [cs]_ , Oct. 2019.
  * [12] D. Kragic, M. Björkman, H. I. Christensen, and J.-O. Eklundh, “Vision for robotic object manipulation in domestic settings,” _Robotics and Autonomous Systems_ , vol. 52, no. 1, pp. 85–100, July 2005.
  * [13] J. Mahler, J. Liang, S. Niyaz, M. Laskey, R. Doan, X. Liu, J. A. Ojea, and K. Goldberg, “Dex-Net 2.0: Deep Learning to Plan Robust Grasps with Synthetic Point Clouds and Analytic Grasp Metrics,” _arXiv:1703.09312 [cs]_ , Mar. 2017\.
  * [14] D. Morrison, P. Corke, and J. Leitner, “Closing the loop for robotic grasping: A real-time, generative grasp synthesis approach,” in _Proc. of Robotics: Science and Systems (RSS)_ , 2018.
  * [15] S. Kumra, S. Joshi, and F. Sahin, “Antipodal robotic grasping using generative residual convolutional neural network,” in _2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2020.
  * [16] A. Doucet, N. de Freitas, and N. Gordon, “Sequential Monte Carlo Methods in Practice,” p. 12.
  * [17] Vermaak, Doucet, and Perez, “Maintaining multimodality through mixture tracking,” in _Proceedings Ninth IEEE International Conference on Computer Vision_. Nice, France: IEEE, 2003, pp. 1110–1116 vol.2.
  * [18] K. Okuma, A. Taleghani, N. de Freitas, J. J. Little, and D. G. Lowe, “A Boosted Particle Filter: Multitarget Detection and Tracking,” in _Computer Vision - ECCV 2004_ , T. Kanade, J. Kittler, J. M. Kleinberg, F. Mattern, J. C. Mitchell, O. Nierstrasz, C. Pandu Rangan, B. Steffen, M. Sudan, D. Terzopoulos, D. Tygar, M. Y. Vardi, G. Weikum, T. Pajdla, and J. Matas, Eds. Berlin, Heidelberg: Springer Berlin Heidelberg, 2004, vol. 3021, pp. 28–39.
  * [19] R. B. Rusu, “Tracking 3D objects with Point Cloud Library,” Jan. 2012\.
  * [20] W. Gao and R. Tedrake, “FilterReg: Robust and Efficient Probabilistic Point-Set Registration using Gaussian Filter and Twist Parameterization,” _arXiv:1811.10136 [cs]_ , July 2019.
  * [21] Y. Xiao, X. Qiu, P.-A. Langlois, M. Aubry, and R. Marlet, “Pose from Shape: Deep Pose Estimation for Arbitrary 3D Objects,” _arXiv:1906.05105 [cs]_ , Aug. 2019.
  * [22] J. M. Wong, V. Kee, T. Le, S. Wagner, G.-L. Mariottini, A. Schneider, L. Hamilton, R. Chipalkatty, M. Hebert, D. M. Johnson, _et al._ , “Segicp: Integrated deep semantic segmentation and pose estimation,” in _Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2017.
  * [23] B. A. Erol, A. Majumdar, J. Lwowski, P. Benavidez, P. Rad, and M. Jamshidi, “Improved Deep Neural Network Object Tracking System for Applications in Home Robotics,” in _Computational Intelligence for Pattern Recognition_ , W. Pedrycz and S.-M. Chen, Eds. Cham: Springer International Publishing, 2018, vol. 777, pp. 369–395.
  * [24] G. Pavlakos, X. Zhou, A. Chan, K. G. Derpanis, and K. Daniilidis, “6-DoF Object Pose from Semantic Keypoints,” _arXiv:1703.04670 [cs]_ , Mar. 2017.
  * [25] S. Song and J. Xiao, “Deep Sliding Shapes for Amodal 3D Object Detection in RGB-D Images,” in _2016 IEEE Conference on Computer Vision and Pattern Recognition (CVPR)_. Las Vegas, NV, USA: IEEE, June 2016, pp. 808–816.
  * [26] J. Hou, A. Dai, and M. Nießner, “3D-SIS: 3D Semantic Instance Segmentation of RGB-D Scans,” _arXiv:1812.07003 [cs]_ , Apr. 2019.
  * [27] C. R. Qi, H. Su, K. Mo, and L. J. Guibas, “PointNet: Deep Learning on Point Sets for 3D Classification and Segmentation,” _arXiv:1612.00593 [cs]_ , Apr. 2017.
  * [28] C. R. Qi, L. Yi, H. Su, and L. J. Guibas, “PointNet++: Deep Hierarchical Feature Learning on Point Sets in a Metric Space,” _arXiv:1706.02413 [cs]_ , June 2017.
  * [29] C. R. Qi, O. Litany, K. He, and L. J. Guibas, “Deep Hough Voting for 3D Object Detection in Point Clouds,” _arXiv:1904.09664 [cs]_ , Aug. 2019.
  * [30] B. Leibe, A. Leonardis, and B. Schiele, “Combined Object Categorization and Segmentation with an Implicit Shape Model,” p. 16.
  * [31] R. Luo, R. Mullen, and D. Wessell, “An adaptive robotic tracking system using optical flow,” in _Proceedings. 1988 IEEE International Conference on Robotics and Automation_. Philadelphia, PA, USA: IEEE Comput. Soc. Press, 1988, pp. 568–573.
  * [32] A. Pieropan, N. Bergstrom, M. Ishikawa, and H. Kjellstrom, “Robust 3D tracking of unknown objects,” in _2015 IEEE International Conference on Robotics and Automation (ICRA)_. Seattle, WA, USA: IEEE, May 2015, pp. 2410–2417.
  * [33] J. Mahler, M. Matl, V. Satish, M. Danielczuk, B. DeRose, S. McKinley, and K. Goldberg, “Learning ambidextrous robot grasping policies,” _Science Robotics_ , vol. 4, no. 26, p. eaau4984, 2019.
  * [34] D. Driess, O. Oguz, J.-S. Ha, and M. Toussaint, “Deep visual heuristics: Learning feasibility of mixed-integer programs for manipulation planning,” in _Proc. of the IEEE International Conference on Robotics and Automation (ICRA)_ , 2020.
  * [35] A. Mousavian, C. Eppner, and D. Fox, “6-dof graspnet: Variational grasp generation for object manipulation,” in _International Conference on Computer Vision (ICCV)_ , 2019.
  * [36] D. Kappler, F. Meier, J. Issac, J. Mainprice, C. G. Cifuentes, M. Wüthrich, V. Berenz, S. Schaal, N. Ratliff, and J. Bohg, “Real-time perception meets reactive motion generation,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 3, pp. 1864–1871, 2018.
  * [37] M. Otte and E. Frazzoli, “Rrtx: Asymptotically optimal single-query sampling-based motion planning with quick replanning,” _The International Journal of Robotics Research_ , vol. 35, no. 7, pp. 797–822, 2016\.
  * [38] Q. Wang, L. Zhang, L. Bertinetto, W. Hu, and P. H. S. Torr, “Fast Online Object Tracking and Segmentation: A Unifying Approach,” _arXiv:1812.05050 [cs]_ , Dec. 2018.
  * [39] A. Sauer, E. Aljalbout, and S. Haddadin, “Tracking Holistic Object Representations,” _arXiv:1907.12920 [cs, stat]_ , Aug. 2019.
  * [40] A. Segal, D. Haehnel, and S. Thrun, “Generalized-icp.” in _Robotics: Science and Systems_ , vol. 2, no. 4. Seattle, WA, 2009, p. 435.
  * [41] D. Driess, O. Oguz, and M. Toussaint, “Hierarchical task and motion planning using logic-geometric programming (hlgp),” RSS Workshop on Robust Task and Motion Planning, 2019.
  * [42] D. Driess, J.-S. Ha, and M. Toussaint, “Deep visual reasoning: Learning to predict action sequences for task and motion planning from an initial scene image,” in _Robotics: Science and Systems (R:SS)_ , 2020.
  * [43] “Tracking 3D objects with Point Cloud Library - Point Cloud Library (PCL),” http://www.pointclouds.org/news/2012/01/17/tracking-3d-objects-with-point-cloud-library/.