∎

11institutetext:  ✉ Jihua Zhu 22institutetext<EMAIL_ADDRESS>44institutetext: 1 School of Software Engineering, Xi’an Jiaotong University,
Shaanxi, China
2 Department of Computer Science, University of North Carolina at Charlotte,
USA
3 School of Information Science and Technology, Northwest University, Shaanxi,
China

# Weighted Motion Averaging for the Registration of Multi-View Range Scans

Rui Guo1 Jihua Zhu1∗ Yaochen Li1 Dapeng Chen1 Zhongyu Li2 Yongqin Zhang3

(Received: date / Accepted: date)

###### Abstract

Multi-view registration is a fundamental but challenging task in 3D
reconstruction and robot vision. Although the original motion averaging
algorithm has been introduced as an effective means to solve the multi-view
registration problem, it does not consider the reliability and accuracy of
each relative motion. Accordingly, this paper proposes a novel motion
averaging algorithm for multi-view registration. Firstly, it utilizes the
pair-wise registration algorithm to estimate the relative motion and
overlapping percentage of each scan pair with a certain degree of overlap.
With the overlapping percentage available, it views the overlapping percentage
as the corresponding weight of each scan pair and proposes the weighted motion
averaging algorithm, which can pay more attention to reliable and accurate
relative motions. By treating each relative motion distinctively, more
accurate registration can be achieved by applying the weighted motion
averaging to multi-view range scans. Experimental results demonstrate the
superiority of our proposed approach compared with the state-of-the-art
methods in terms of accuracy, robustness and efficiency.

###### Keywords:

Multi-view registration Iterative closest point algorithm Overlapping
percentage Motion averaging

##  1 Introduction

In the past decades, registration problem has been attracted immense
attentions in many domains, such as 3D reconstruction Lu16 ; Rich15 ; Brad08 ,
shape recognition Held16  and robot mapping Ma16 ; Shir15 . According to the
number of range scans to be registered, the problem of registration can be
divided into two categories: pair-wise registration and multi-view
registration.

For the pair-wise registration, the most popular approach is the iterative
closest point (ICP) algorithm Besl02 , which has activated a bunch of
registration algorithms to be proposed and improved. Given initial parameters,
it iteratively establishes the correspondence between two scan range and
calculate the rigid transformation by minimizing the least square error.
Although this algorithm is efficient, it can not be deal with outliers.
Accordingly, a straightforward solution is to reject point pairs with
distances larger than the specified threshold Rusi01 . Besides, Godin Godi94
proposed the concept of weighted scan pairs, which can assign lower weights to
point pairs with large point-to-point distances. These approaches are simple
but ineffective due to the varied resolution of range scans. Therefore,
Chetverikov et al. Chet05  proposed the trimmed ICP (TrICP) algorithm, which
introduced an overlapping percentage into the original ICP algorithm so as to
automatically reject outliers. Moreover, some probabilistic approaches Zhu16 ;
Jian11 ; Myro06 ; Tsin04  were also proposed for the registration of partially
overlapping range scans. As most of these approaches are based on the idea of
ICP algorithm, they all suffers from the local convergence problem. To obtain
the desired global minimum, genetic algorithm (GA) Lomo06 ; Zhu14 , as well as
the particle filter Lu16  has been adopted to search the optimal solution for
registration.

The above-mentioned approaches can only deal with pair-wise registration
problem. To solve the multi-view registration problem, Chen and Medioni Chen92
proposed a primary approach. It repeatedly registers two range scans and
integrates them into one range scan until all the range scans are integrated
into the whole model. However, the error accumulation is an unavoidable
problem in this approach. To address this issue, some other approaches has
been proposed and most of them were based on the pair-wise registration
algorithms. Given the pair-wise registration results, the multi-view
registration can be converted into a quadratic programming problem of Lie
algebra parameters Shi09  and solved by distributing the accumulation error to
proper positions in the graph, where each node and each edge in the graph
represents a range scan and a pairwise registration, respectively. Recently,
Fantoni et al. Fant12  and Guo et al. Guo14  proposed two novel approaches for
registration of multiple range scans by extracting and describing the features
from range scans. Although these approaches are efficient, there may not have
enough features to be extracted from range scans, which can lead to
registration failure. Besides, Zhu et al. ZhuJ14  proposed a coarse-to-fine
approach for multi-view registration. In this approach, each range scan is
sequentially registered to a coarse model reconstructed by other registered
range scans. By applying the TrICP algorithm, it can obtain good registration
result for each range scan, which can then be immediately utilized to refine
the coarse model for registration of other range scans. Since the TrICP
algorithm is applied to registration, its accuracy is satisfactory, yet it may
be trapped into local minimum due to the poor initial parameters.

Additionally, the multi-view registration can also be viewed as the problem of
low-rank and sparse matrix decomposition Arri16 . By applying the pair-wise
registration approach, it can obtain the relative motions for some scan pairs
with high overlapping percentage and then concatenate them into a large
matrix. As some scan pair only contain low overlapping percentage, there are
missing data in the assembled matrix. To achieve multi-view registration, this
large matrix should be recovered. This is a low-rank and sparse matrix
decomposition problem, which can be solved by many algorithms, such as R-GoDec
Arri14 , Grasta He12  and L1-Alm Zhen12 . However, this approach may not
achieve multi-view registration due to the high ratio of missing data.
Recently, Govindu and Pooja Govi14  proposed the motion averaging algorithm,
which can be applied to a set of relative motions and recover the global
motions for each range scan. With the motion averaging algorithm, the
prerequisite is how to obtain accurate and reliable relative motions.
Subsequently, Li and Zhu Li14  proposed a method to estimate the overlapping
percentage between each range scan pair, so as to apply the pair-wise
registration approach to scan pairs with a certain degree of overlapping
percentage. However, the original motion averaging algorithm treat each
relative motion equally, which may lead to inaccurate results for multi-view
registration.

Accordingly, this paper extends the approach presented in Govi14  and proposes
the weighted motion averaging algorithm for multi-view registration of range
scans. The main differences between the previous work Govi14  and this one are
described as follows: 1) it presents a method to estimate the overlapping
percentage between scan pair involved in multi-view registration; 2) for the
scan pair with a certain degree of overlapping percentage, the TrICP algorithm
is unitized to calculate the relative motion with the corresponding weight
indicated its accuracy and reliability; 3) weighted motion algorithm is
proposed to pay more attention to the relative motion with large weight. All
these three techniques can improve the performance of multi-view registration.

The remainder of this paper is organized as follows: Section 2 briefly
introduces the principle of Motion averaging. After that, our proposed
approach is demonstrated in Section 3. Then in Section 4, experimental results
are displayed to compare with some related approaches. Finally, some
conclusions are drawn in Section 5.

##  2 Motion averaging

In the motion averaging algorithm, the rigid transformation
$({\bf{R}},\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{t}})$
can be assembled into the matrix of Lie group and has the form as the
following:

${\bf{M}}=\left[{\begin{array}[]{*{20}{c}}{\bf{R}}&{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{t}}}\\\
{\bf{O}}&1\end{array}}\right],$ (1)

where ${\bf{M}}\in SE(3)$, ${\bf{R}}\in SO(3)$ and $O=[0,0,0]$.

In mutli-view registration, the above matrix has two forms: relative motion
${{\bf{M}}_{ij}}$ and global motion ${{\bf{M}}_{N}}$, which indicate the
motion from $j$th range scan to $i$th range scan and $N$th range scan to the
reference range scan, respectively. The goal of multi-view registration is to
acquire a set of global motions:

${\kern 1.0pt}{\kern 1.0pt}{{\bf{M}}_{global}}=\left\\{{I,{\kern 1.0pt}{\kern
1.0pt}{{\bf{M}}_{2}},{\kern 1.0pt}{\kern 1.0pt}...{\kern 1.0pt}{\kern
1.0pt},{\kern 1.0pt}{\kern 1.0pt}{{\bf{M}}_{N}}}\right\\},$ (2)

from a set of relative motions:

${{\bf{M}}_{ij}}={({{\bf{M}}_{i}})^{-1}}{{\bf{M}}_{j}}.$ (3)

Given accurate relative motions and global motions, the following equation can
be established:

$I={\bf{M}}_{i}{{\bf{M}}_{ij}}{({\bf{M}}_{j})^{-1}},$ (4)

where $I$ represent the identity matrix. However, the relative motions are
obtained by the pair-wise registration, which inevitably involves error.
Therefore, the following increment can be defined as follows:

$\Delta{{\bf{M}}_{ij}}={\bf{M}}_{i}{{\bf{M}}_{ij}}{({\bf{M}}_{j})^{-1}}.$ (5)

As in Govi04 , we can assign a 6*(6n) matrix ${{\bf{D}}_{ij}}$ to each
relative motion:
${{\bf{D}}_{ij}}=\left[{\begin{array}[]{*{20}{c}}\ldots&{-{I_{ij}}}&\ldots&{{I_{ij}}}&\ldots\end{array}}\right]$,
where the 6*6 identity matrices ${-I_{ij}}$ and ${I_{ij}}$ are located in the
position of $i$th block and $j$th block, respectively. Stacking all these
matrix into one single matrix, it can directly obtain the formulation
${\bf{D}}={[{{\bf{D}}_{ij1}},{{\bf{D}}_{ij2}},\ldots{{\bf{D}}_{ijr}}]^{T}}$,
where $r$ indicates the number of participating scan pairs. According to
Govi04 , the original motion averaging algorithm can be summarized as
Algorithm 1:

Algorithm 1 Motion Averaging Algorithm
---
Input:
$\left\\{{{{\bf{M}}_{ij1}},{{\bf{M}}_{ij2}},\cdots,{{\bf{M}}_{ijr}}}\right\\}$
Output: ${\kern 1.0pt}{\kern 1.0pt}{{\bf{M}}_{global}}=\left\\{{I,{\kern
1.0pt}{\kern 1.0pt}{{\bf{M}}_{2}},{\kern 1.0pt}{\kern 1.0pt}...{\kern
1.0pt}{\kern 1.0pt},{\kern 1.0pt}{\kern 1.0pt}{{\bf{M}}_{N}}}\right\\}$
Set ${{\bf{M}}_{global}}$ to an initial guess
Do
$\Delta{{\bf{M}}_{ij}}={\bf{M}}_{i}{{\bf{M}}_{ij}}{({\bf{M}}_{j})^{-1}}$
$\Delta{\bf{m}}_{ij}=log(\Delta{\bf{M}}_{ij})$
$\Delta{\bf{v}}_{ij}=vec(\Delta{\bf{m}}_{ij})$
$\Delta\Im={{\bf{D}}^{\dagger}}\Delta{{\bf{V}}_{ij}}$
$\forall k\in[2,N],{\kern 1.0pt}{\kern 1.0pt}{\kern 1.0pt}{\kern 1.0pt}{\kern
1.0pt}{\kern 1.0pt}{{\bf{M}}_{k}}={e^{\Delta{{\bf{m}}_{k}}}}{{\bf{M}}_{k}}$
Repeat till $\left\|{\Delta\Im}\right\|<\varepsilon$

where $log(.)$ and $exp(.)$ denotes matrix operations, ${\bf{m}}_{ij}$ is the
corresponding Lie algebra of ${\bf{M}}_{ij}$, $\Delta{\bf{m}}_{ij}$ indicates
the corresponding Lie algebra element of $\Delta{\bf{M}}_{ij}$ and
${{\bf{D}}^{\dagger}}$ is the pseudo-inverse of ${\bf{D}}$. As
$\Delta{\bf{m}}_{ij}$ is a skew-symmetric matrix, it can be uniquely expressed
by a 6-element vector given by $\Delta{\bf{v}}_{ij}$, which is obtained by the
operation $vec(.)$. Besides, all $\Delta{\bf{v}}_{ij}$ vectors are stacked
into one matrix, which has the form:
$\Delta{{\bf{V}}_{ij}}={[{\Delta{\bf{v}}_{ij1}},{\Delta{\bf{v}}_{ij2}},\ldots,{\Delta{\bf{v}}_{ijr}}]^{T}}$.
Similarly, the $\Delta\Im$ contains all the variables ${\bf{v}}_{i}$ stacked
together into a single vector.

In the multi-view registration, the pair-wise registration approach should be
utilized to obtain relative motions, which are taken as the input of the
motion averaging algorithm. As there is no perfect registration approach, the
pair-wise registration result contains error, which can varied from small to
large due to the outliers and noise of scan pair. Therefore, the accuracy and
reliability of each relative motion are totally different. However, the
original motion averaging algorithm treats each relative motion equally, which
can lead to inaccurate multi-view registration result. To obtain more accurate
result, more effective motion averaging algorithm should be designed.

##  3 The proposed multi-view registration approach

In this section, an effective approach is proposed for multi-view registration
of initially posed range scans and its overview can be shown in Fig. 1. As
shown in Fig. 1, the proposed approach consists of the following three major
steps. 1) Estimate the overlapping percentage for each scan pair; 2) Compute
the relative motion and its corresponding weight for each pair of range scans
with high overlapping percentage; 3) Calculate the multi-view registration by
the weighted motion averaging algorithm.

Subsequently, these three steps will be presented by more details.

Figure 1: The framework of proposed approach for multi-view registration of
range scans

###  3.1  Estimation of the overlapping percentage

To obtain reliable and accurate relative motions, the pair-wise registration
algorithm can be only applied to these scan pairs which contain a certain
degree of overlapping percentages. Here, we employ our previous method Govi14
to estimate the overlapping percentage ${\xi}_{ij}$ for each scan pair. It
does not directly estimate the overlapping percentage for each individual scan
pair. Instead, it firstly determines the distance threshold by considering all
the scans and then utilize this threshold to estimate overlapping percentages.
Since it uses all scans to estimate the overlapping percentage for each scan
pair, the estimation result is reliable, even though the actual overlapping
percentage of one scan pair may be low.

As shown in Fig. 1, the proposed approach should be implemented in iterative
manner to achieve good multi-view registration. Given good registration
parameters, the overlapping percentage can be estimated accurately by our
previous method, which is helpful to multi-view registration. Intuitively, it
seems that the overlapping percentage estimation can also be included in the
iterative loop, so as to compute more accurate the overlapping percentage.
However, the percentage estimation is time-consuming and the estimation
results are only adopted to judge whether one scan pair contains high or low
overlapping percentage. Therefore, the accuracy should yield to efficiency and
estimation is only implemented once.

###  3.2  Pair-wise registration

To guarantee good pair-wise registration, the pair-wise registration can only
by applied to these scan pairs with overlapping percentage
${\xi}_{ij}>{\xi_{thr}}$. As shown in Fig. 1, the pair-wise registration
approach should provide the motion and its corresponding weight for each pair
of range scans with high overlapping percentage. Because it is easy to obtain
relative motion by any pair-wise registration, so its corresponding weight
will be designed as follows.

####  3.2.1 Design of the weight

Given a set of relative motions, the original motion averaging algorithm views
and treats each relative motion equally. However, some relative motions are
accurate and other maybe not very accurate. Therefore, the weight of each
motion should be introduced so as to deal with them differently.

Figure 2: The rotation error with respect to the overlapping percentage of
range scans to be registered

Intuitively, more attention should be paid to these relative motions, which
are more accurate and reliable than others. Therefore, it should find a
function, which can indicates the accuracy and reliability of the relative
motions. As the relative motions are estimated by the pair-wise registration
algorithm, we utilized the TrICP algorithm to estimate relative motion between
scan pairs with different overlapping percentages and recorded the
corresponding registration error with respect to overlapping percentage.

As shown in Fig. 2, the pair-wise registration error is dramatically decreased
with the increase of overlapping percentage. This is because a scan pair with
low overlapping percentage contains more outliers, which can reduce the
reliability and accuracy of pair-wise registration results. Subsequently, it
is straight-forward to design the weight of relative motions as follows:

${w_{ij}}=\xi_{ij}^{2},$ (6)

where ${\xi_{ij}}=\frac{{|{P_{i}}\cap{P_{j}}|}}{{|{P_{j}}|}}$, ${P_{i}}$ and
${P_{j}}$ denotes two range scans, ${P_{i}}\cap{P_{j}}$ denotes the
overlapping regions of the scan pair. That means the overlapping percentage
can be utilized to indicate the accuracy and reliability of the corresponding
relative motion.

####  3.2.2 Computation of the relative motion

Obviously, it is expected that the pair-wise registration can estimate the
relative motion and the corresponding overlapping percentage for a given pair
of range scans simultaneously. Therefore, the TrICP algorithm can be adopted.

Suppose there are two partially overlapping range scans, a data shape
$P\buildrel\Delta\over{=}\\{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{p}}_{i}}\\}_{i=1}^{{N_{p}}}$
and a model shape
$Q\buildrel\Delta\over{=}\\{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{q}}_{j}}\\}_{j=1}^{{N_{q}}}$.
Denote $\xi$, ${\bf{R}}\in{\mathbb{R}^{3\times 3}}$,
$\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{t}}\in\mathbb{R}{{}^{3}}$
as the overlapping percentage, 3D rotation matrix and translation vector,
respectively. The goal of partially overlapping registration is to find the
optimal transformation
$({\bf{R}},\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{t}})$
with which $P$ is registered to be in the best alignment with $Q$. This
problem can be formulated as follows:

$\begin{array}[]{l}\mathop{\min}\limits_{{\bf{R}},\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{t}},\xi}\left({\frac{1}{{\left|{{P_{\xi}}}\right|{{(\xi)}^{1+\lambda}}}}\sum\limits_{{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{p}}}_{i}}\in{P_{\xi}}}{\left\|{{\bf{R}}{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{p}}}_{i}}+\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{t}}-{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{q}}}_{c(i)}}}\right\|_{2}^{2}}}\right)\\\
s.t.\quad{{\bf{R}}^{T}}{\bf{R}}={{\bf{I}}_{3}},\det({\bf{R}})=1\\\
\quad\quad\xi\in[{\xi_{thr}},1],{P_{\xi}}\subseteq
P,\left|{{P_{\xi}}}\right|=\xi\left|P\right|\end{array},$ (7)

where
${{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{q}}}_{c(i)}}}$
denotes the correspondence of the
${{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{p}}}_{i}}}$
in model shape, $\lambda$ is a preset parameter, $\left|\cdot\right|$ denotes
the cardinality of a scan and ${{P_{\xi}}}$ represents the overlapping part of
data shape to model shape.

The TrICP algorithm achieve pair-wise registration by iterations. Given the
initial transformation
$({{\bf{R}}_{0}},{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{t}}_{0}})$,
three steps are included in each iteration:

(1) Based on
$({{\bf{R}}_{k-1}},{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{t}}_{k-1}})$,
assign the correspondence between two scans:

${c_{k}}(i)=\mathop{\arg\min}\limits_{j\in\\{1,2,..,{N_{q}}\\}}{\left\|{{{\bf{R}}_{k-1}}{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{p}}}_{i}}+{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{t}}}_{k-1}}-{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{q}}}_{j}}}\right\|_{2}}.$
(8)

(2) Update the overlapping percentage ${\xi_{k}}$ and subset
${P_{{\xi_{k}}}}$:

$({\xi_{k}},{P_{{\xi_{k}}}})=\mathop{\arg\min}\limits_{{\xi_{thr}}<\xi\leq
1}({{\sum\limits_{{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{p}}}_{i}}\in{P_{\xi}}}{\left\|{{{\bf{R}}_{k-1}}{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{p}}}_{i}+{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{t}}}_{k-1}}-{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{q}}}_{c_{k}(i)}}}}\right\|_{2}^{2}}}\mathord{\left/{\vphantom{{\sum\limits_{{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{q}}}_{j}}\in{Q_{\xi}}}{\left\|{{s_{k-1}}{{\bf{R}}_{k-1}}{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{p}}}_{i}}+{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{t}}}_{k-1}}-{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{q}}}_{{c_{k}}(j)}}}\right\|_{2}^{2}}}{\left|{{Q_{\xi}}}\right|{\xi^{1+\lambda}}}}}\right.\kern-1.2pt}{\left|{{P_{\xi}}}\right|{\xi^{1+\lambda}}}}).$
(9)

(3) Calculate the $k$th transformation:

$({{\bf{R}}_{k}},{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{t}}_{k}})=\mathop{\arg\min}\limits_{\xi,{\bf{R}},\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{t}}}\sum\limits_{{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{p}}}_{i}}\in{P_{{\xi_{k}}}}}{\left\|{{\bf{R}}{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{p}}}_{i}}+\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{t}}-{{\stackrel{{\scriptstyle\raisebox{-1.0pt}[0.0pt][0.0pt]{\small$\rightarrow$}}}{{q}}}_{{c_{k}}(i)}}}\right\|_{2}^{2}}.$
(10)

For the scan pair with a certain degree of overlapping percentage, its optimal
rigid transformation can be computed by iterating these three steps until some
convergence criteria are satisfied. Then the rigid transformation can be
assembled into a relative motion.

###  3.3 Weighted motion averaging algorithm

Since the pair-wise registration can provide the relative motion and its
corresponding weight, more effective motion averaging algorithm can be
proposed as follows.

####  3.3.1 Principle of the weighted motion averaging

Before proposing the weighted motion averaging algorithm, we firstly
demonstrate the averaging procedure that holds for all motions with
corresponding weight. Given a set of motions
$\\{{\bf{M}}_{1},{\bf{M}}_{2},\ldots,{\bf{M}}_{N}\\}$, the corresponding
weight ${w_{i}}$ is assigned to each of them. To find their average motion
$\overline{\bf{M}}\in SE(3)$, we can minimise the variational measure of
$\sum\nolimits_{i=1}^{N}{w_{i}}{{d^{2}}({{\bf{M}}_{i}},\overline{\bf{M}}})$,
where $d(\dots,\dots)$ is the Riemannian metric of Lie group. Based on Govi14
, the procedure for estimation the average motion $\overline{\bf{M}}$ can be
given as Algorithm 2:

Algorithm 2: Averaging weighted motions
---
Input: $\left\\{{{{\bf{M}}_{1}},{{\bf{M}}_{2}},...{\kern 1.0pt}{\kern
1.0pt},{\kern 1.0pt}{\kern 1.0pt}{{\bf{M}}_{N}}}\right\\}$ and
$\left\\{{{w_{1}},{w_{2}},...{\kern 1.0pt}{\kern 1.0pt},{\kern 1.0pt}{\kern
1.0pt}{w_{N}}}\right\\}$
Output: $\overline{\bf{M}}$
Set $\overline{\bf{M}}={{\bf{M}}_{1}}$
Do
$\Delta{{\bf{M}}_{i}}={\overline{\bf{M}}^{-1}}{{\bf{M}}_{i}}$
$\Delta{{\bf{m}}_{i}}={w_{i}}logm(\Delta{{\bf{M}}_{i}})$
$\Delta\overline{\bf{m}}={{\sum\nolimits_{i=1}^{N}{\Delta{{\rm{m}}_{i}}}}\mathord{\left/{\vphantom{{\sum\nolimits_{i=1}^{N}{\Delta{{\rm{m}}_{i}}}}{\sum\nolimits_{i=1}^{N}{{w_{i}}}}}}\right.\kern-1.2pt}{\sum\nolimits_{i=1}^{N}{{w_{i}}}}}$
$\Delta\overline{\bf{M}}=expm(\Delta\overline{\bf{m}})$
$\overline{\bf{M}}=\overline{\bf{M}}\Delta\overline{\bf{M}}$
Untill $||\Delta\overline{\bf{m}}||\leq\varepsilon$

Algorithm 2 can be extended to deal with the registration of multi-view range
scans.

####  3.3.2 The weighted motion averaging for multi-view registration

Previously, the motion average TrICP algorithmLi14  has been proposed to solve
the multi-view registration problem. Given a set of relative motions, it
evenly distributes redundant information among all range scans involved in
multi-view registration. To handle each relative motion differently, each
relative motion can be assigned to be with a weight, which allow us pay more
attention to accurate and reliable relative motions. Accordingly, the original
motion averaging algorithm should be extended to the weighted motion averaging
algorithm. As shown in 3, the goal of weighted motion averaging is to recover
the global motions from a set of weighted relative motions.

Figure 3: The symbols involved in weighted motion averaging algorithm, where
one circle represents a range scan, ${{\bf{M}}_{ij}}$ is the relative motion
from the $j$th scan to the $i$th scan and ${{\bf{M}}_{i}}$ denotes global
motions required to be estimated, $w_{ij}$ indicates the weight of relative
motion ${{\bf{M}}_{ij}}$

For each scan pair with a certain degree of overlapping percentage, the
relative motion and its corresponding weight has been estimated by the TrICP
algorithm. Then Eq.(3) can be extended as:

${w_{ij}}{{\bf{M}}_{ij}}={w_{ij}}{({{\bf{M}}_{i}})^{-1}}{{\bf{M}}_{j}},$ (11)

where the weight ${w_{ij}}$ denotes the contribution of relative motion
${\bf{M}}_{ij}$ in the averaging process. Accordingly, the Lie algebra of
Eq.(11) can be derived as:

${w_{{\rm{ij}}}}{{\bf{m}}_{ij}}\approx{w_{{\rm{ij}}}}({{\bf{m}}_{j}}-{{\bf{m}}_{i}}).$
(12)

Accordingly, the matrix ${{\bf{D}}_{ij}}$ can be assigned for each relative
motion as follows:

${{\bf{D}}_{ij}}=\left[{\begin{array}[]{*{20}{c}}\ldots&{-w_{ij}{I_{ij}}}&\ldots&w_{ij}{{I_{ij}}}&\ldots\end{array}}\right],$
(13)

where the 6*6 identity matrices $w_{ij}{-I_{ij}}$ and $w_{ij}{I_{ij}}$ are put
into the position of $i$th block and $j$th block, respectively. All these
matrix can be stacked into one matrix, which has the form:

${\bf{D}}={[{{\bf{D}}_{ij1}},{{\bf{D}}_{ij2}},\ldots{{\bf{D}}_{ijr}}]^{T}},$
(14)

where $r$ denotes the number of participating range scan pairs. As stated in
the section 2, the operation ${\bf{v}}=mat2vec({\bf{m}})$ is defined to
extract parameters from ${\bf{m}}$ to form a column wise vector
$v={\left[{\begin{array}[]{*{20}{c}}{{\Omega_{21}}}&{{\Omega_{31}}}&{{\Omega_{32}}}&{{u_{1}}}&{{u_{2}}}&{{u_{3}}}\end{array}}\right]^{T}}$.
Similarly, we can stack all these weighted vectors into one single vector and
get the formulation:

${{\bf{V}}}={[{w_{ij1}}{{\bf{v}}_{ij1}},{{w_{ij2}}{\bf{v}}_{ij2}},\ldots,{{w_{ijr}}{\bf{v}}_{ijr}}]^{T}}.$
(15)

Based on these derivations, the following equation can be acquired:

${{\bf{V}}}={{\bf{D}}}\Im,$ (16)

where $\Im={[{{\bf{v}}_{1}},{{\bf{v}}_{2}}\cdots{{\bf{v}}_{n}}]^{T}}$ and
${{\bf{v}}_{i}}$ is a 6*1 vector composed of elements in ${{\bf{m}}_{i}}$.
Obviously, it is easy to obtain the result as follows:

$\Im={{\bf{D}}^{\dagger}}{{\bf{V}}},$ (17)

where ${{\bf{D}}^{\dagger}}$ is the pseudo-inverse of ${\bf{D}}$.

The above description results in the algorithm that can estimate the global
motions ${{\bf{M}}_{global}}$ from a set of weighted relative motions
$\left\\{{{{\bf{w}}_{ijr}},{{\bf{M}}_{ijr}}}\right\\}_{r=1}^{R}$. Algorithm 3
summarizes the process of motion averaging for weighted relative motions:

Algorithm 3: The weighted motion averaging for multi-view registration
---
Input: Initial global motions
${{\bf{M}}_{global}^{0}}\buildrel\Delta\over{=}\left\\{{{{\bf{I}},{{\bf{M}}_{2}^{0}}},\cdots,{{\bf{M}}_{n}^{0}}}\right\\}$
and a set of weighted relative motions
$\left\\{{{{\bf{w}}_{ijr}},{{\bf{M}}_{ijr}}}\right\\}_{r=1}^{R}$
Output: Global motions
${{\bf{M}}_{global}}\buildrel\Delta\over{=}\left\\{{{{\bf{I}},{{\bf{M}}_{2}}},\cdots,{{\bf{M}}_{N}}}\right\\}$
Do
$\Delta{{\bf{M}}_{ijm}}={\bf{M}}_{i}^{0}{{\bf{M}}_{ijm}}{({\bf{M}}_{j}^{0})^{-1}}$
$\Delta{{\bf{m}}_{ijm}}=logm(\Delta{{\bf{M}}_{ijm}})$
$\Delta{{\bf{v}}_{ijm}}=mat2vec(\Delta{{\bf{m}}_{ijm}})$
${\Delta{\bf{V}}}={[{w_{ij1}}{\Delta{\bf{v}}_{ij1}},{w_{ij2}}{\Delta{\bf{v}}_{ij2}},\ldots,{w_{ijr}}{\Delta{\bf{v}}_{ijR}}]^{T}}$
${\bf{D}}={[{{\bf{D}}_{ij1}},{{\bf{D}}_{ij2}},\ldots,{{\bf{D}}_{ijR}}]^{T}}$
$\Delta\Im={{\bf{D}}^{\dagger}}\Delta{{\bf{V}}}$
$\Delta{{\bf{m}}_{k}}=vec2mat(\Delta\Im)$
$\forall k\in[2,N],{\kern 1.0pt}{\kern 1.0pt}{\kern 1.0pt}{\kern 1.0pt}{\kern
1.0pt}{\kern 1.0pt}{{\bf{M}}_{k}}={e^{\Delta{{\bf{m}}_{k}}}}{{\bf{M}}_{k}}$
Untill $\left\|{\Delta\Im}\right\|<\varepsilon$

where $mat2vec(.)$ is defined to extract parameters from ${\bf{m}}$ to form a
column wise vector ${\bf{v}}$ and $vec2mat(.)$ is defined to recover
${{\bf{m}}}$ using ${{\bf{v}}}$.

###  3.4 Algorithm implementation

Based on the above description, we can present the approach for registration
of multi-view range scans. It can achieve multi-view registration by
iterations. Given a set of initial global motions
$\\{I,{\bf{M}}_{2}^{0},\ldots,{\bf{M}}_{N}^{0}\\}$, four steps are included in
each iteration:

(1) Compute the overlapping percentages for each range scan pair according to
the method presented in Li14 ;

(2) Get initial parameters by Eq.(3), then apply the TrICP algorithm to
calculate the relative motion ${{\bf{M}}_{ijr}}$ and its corresponding weight
${{\bf{w}}_{ijr}}$ for each scan pair with overlapping percentage
${\xi_{ijr}}\geq{\xi_{thr}}$;

(3) According to algorithm 3, utilize
$\left\\{{{{\bf{w}}_{ijr}},{{\bf{M}}_{ijr}}}\right\\}_{r=1}^{R}$ to refine
global motions
${{\bf{M}}_{global}}\buildrel\Delta\over{=}\left\\{{{{\bf{I}},{{\bf{M}}_{2}}},\cdots,{{\bf{M}}_{N}}}\right\\}$;

(4) Repeat steps (1)$\sim$(3) until
$\sum\nolimits_{i=1}^{n}{||\Delta{{\bf{M}}_{i}}||<\delta}$, where $\delta$ is
a preset threshold.

By introducing the weighted motion averaging, the proposed approach can
effectively achieve good registration of multi-view range scans.

##  4 Experiments

To illustrate its good performance, the proposed approaches (WMAA) was tested
on Stanford repository Levo  and the UWA 3D Modeling Dataset, where seven
range datasets were selected. They are the Bunny, Dragon, Happy Buddha,
Chicken, Parasaurolophus, Chef and Trex, which include 8, 10, 15, 15, 16, 22
and 21 range scans, respectively. As each raw data set includes huge number of
points, the testing data sets are sampled from the raw data sets with the
sampling frequency set to be 8. During experiments, some parameters were set
as follows: $\lambda=2$, ${\xi_{\min}}=0.3$, ${\xi_{thr}}=0.4$. All the
competed approaches adopted the nearest-neighbor search method based on $k-d$
tree to establish correspondences. Experiments were implemented in MATLAB and
conducted on a desktop with 3.6GHz processor of eight-cores and 8G RAM.

During experiments, the WMAA algorithm was compared with four approaches: the
low-rank and sparse matrix decomposition (LRS-L1alm) Arri16 , the motion
average TrICP (MATrICP) Li14  and the coarse-to-fine TrICP approach (CFTrICP)
ZhuJ14 . Both LRS-L1a1m and MATrICP share the similar framework with the WMAA
algorithm. They utilize the pair-wise registration to estimate the relative
motions and adopt the corresponding algorithm to recover the multi-view
registration from the set of relative motions. Besides, CFTrICP can obtain
accurate results for multi-view registration and it is also based on the pair-
wise registration. Therefore, we compared the WMAA algorithm with these
related approaches.

###  4.1 Efficiency and accuracy

Since all these four approaches require the initial global motions, it only
needs to compare the runtime in multi-view registration step. For comparison
of different approaches, the objective function presented in Li14  is adopted
as the error criterion for accuracy evaluation of multi-view registration
results. During the experiment, the same noise is added to the initial
registration parameters obtained by Eq.(3). Accordingly, the four approaches
can be applied to registeration of multi-view range scans. For comparison,
Table 1 records the runtime and objective function value of the final
registration result for all these competed approaches, where the bold number
denotes the best performance among these competed approaches.

Table 1: Performance comparison among different approaches for different
shapes

| LRS-L1alm | CFTrICP | MATrICP | WMAA
---|---|---|---|---
| Obj | T(min) | Obj | T(min) | Obj | T(min) | Obj | T(min)
Bunny | 0.6469 | 1.2000 | 0.7119 | 0.6145 | 0.6383 | 0.8753 | 0.6297 | 0.8272
Dragon | 0.4410 | 1.3484 | 0.4063 | 0.7201 | 0.4420 | 0.7751 | 0.4140 | 0.5942
Happy | 0.1371 | 4.9258 | 0.1389 | 5.4873 | 0.1349 | 2.5753 | 0.1331 | 2.5031
Chicken | 4459.5128 | 28.8087 | 0.4264 | 2.9762 | 0.3831 | 1.9029 | 0.3734 | 1.7771
Trex | 1.028 | 12.2773 | 0.2371 | 2.3224 | 0.2464 | 2.2839 | 0.2343 | 2.2800
Paras | 3.4256 | 160.1035 | 0.3418 | 2.8764 | 0.3364 | 2.5810 | 0.3174 | 2.2441
Chef | 32.438 | 59.2495 | 0.2331 | 25.1739 | 0.1892 | 12.1122 | 0.1850 | 11.9201

As shown in Table 1, the WMAA algorithm can obtain almost the best performance
in accuracy and efficiency among these approaches. This is because the WMAA
algorithm can handle each relative motion differently. As each relative motion
is assigned with a weight, it allows us pay more attention to accurate
relative motions. Therefore, the WMAA algorithm can obtain the most accurate
multi-view registration results. What’s more, accurate multi-view registration
can provide the pair-wise registration with good initial parameters to obtain
accurate and reliable relative motions, which can further accelerate the
multi-view registration. Subsequently, the WMAA algorithm is more efficient
than other competed approaches. Sometimes, the CFTrICP algorithm many obtain
the best performance among these competed approaches. This is because the
CFTrICP algroithm achieves multi-view registration by minimizing the objective
function presented in ZhuJ14 , it is reasonable to obtain accurate results.
Besides, both LRS-L1a1m and MATrICP handle each relative motion equally when
they utilize these relative motions to estimate the global motions. As each
relative motion has different accuracy and reliability, the equal treatment
can not ensure accurate registration results, which may increase the number of
iteration to achieve muti-view registration. Therefore, the WMAA algorithm has
good performance for multi-view registration on efficiency and accuracy.

(a)

(b)

(c)

(d)

(e)

(f)

Figure 4: Cross-section of multi-view registration results for four competed
approaches. From the first to fourth rows are Stanford Bunny, Dragon, Happy
Buddha, Chicken and Parasaurolophus. (a) The 3D model obtained by the WMAA
algorithm; (b) Cross-section of initial model; (c)Cross-section of LRS-L1alm;
(d) Cross-section of CFTrICP; (e) Cross-Section of MATrICP; (f) Cross-Section
of WMAA

(a)

(b)

(c)

(d)

(e)

(f)

Figure 5: Cross-section of partially amplified the dragon and Parasaurolophus
model results for four competed approaches. (a) Cross-section of the initial
model; (b) Amplified cross-section of the initial model; (c)Amplified cross-
section of LRS-L1alm; (d) Amplified cross-section of CFTrICP; (e) Amplified
cross-Section of MATrICP; (f) Amplified cross-Section of WMAA

To evaluate the registration accuracy in a more intuitive way, Fig. 4
illustrates the 3D model of Bunny, Dragon, Buddha, Chicken, Parasaurolophus,
and also provides the cross-section of the corresponding four competitive
methods. As shown in Fig. 4, the WMAA algorithm can obtain the most efficient
and accurate registration results among these competed approaches. Given good
initial global motions, all competed approaches can obtain satisfactory
results. However, without good initial global motions, they may be failed to
achieve good muti-view registration. This is because all these competed
approaches are heavily dependent on initial global motions.

Figure 6: The objective function value of the registration results for the
competed approaches in each MC trial, the sub-graphs (e) and (f) are the
amplified results of (c) and (d), respectively

###  4.2 Robustness

To illustrate the robustness of the WMAA algorithm, all competed approaches
were tested on Standford Bunny with varied initial parameters, which can be
obtained by adding the uniform noises to the initial global motions
$\\{{\bf{R}}_{2}^{0},{\bf{R}}_{3}^{0},\ldots,{\bf{R}}_{N}^{0}\\}$. In order to
eliminate randomness, 50 MC trials were carried out with respect to three
noise levels for all competed approaches. For comparison, Table 2 presents the
mean value, standard deviation of objective function and the mean runtime for
these competed approaches, where the bold number denotes the best performance
among these competed approaches. To view the registration results in a more
intuitive way, Fig. 6 depicts the the objective function value of the
registration results for all competed approaches in each MC trial.

Table 2: Performance comparison of three approaches under varied noise levels

| [-0.02,0.02]rad | [-0.04,0.04]rad | [-0.06,0.06]rad
---|---|---|---
| Obj | T(min) | Obj | T(min) | Obj | T(min)
| Mean | Std | Mean | Mean | Std | Mean | Mean | Std | Mean
LRS-L1alm | 0.6500 | 0.0047 | 1.5772 | 0.6513 | 0.0040 | 1.7477 | 1.0424 | 1.1452 | 3.4957
CFTrICP | 0.6333 | 0.0161 | 1.0182 | 0.7201 | 0.0395 | 1.2277 | 0.7942 | 0.3358 | 1.3293
MATrICP | 0.7163 | 0.0026 | 1.2829 | 0.6361 | 0.0046 | 1.0653 | 0.6426 | 0.0085 | 1.0268
WMAA | 0.6298 | 0.0002 | 0.8802 | 0.6299 | 0.0009 | 0.9674 | 0.6317 | 0.0032 | 0.9538

As displayed in Table 2 and Fig. 6 , the WMAA algorithm can get the most
accurate and robust registration results under different noise levels.
Although both the WMAA and MATrICP algorithm utilize the motion averaging
algorithm to achieve muti-view registration, they treat each relative motion
differently. In multi-view registration, each relative motions has different
importance due to the overlapping percentages of the corresponding scan pair.
Thanks for the introduction of the weight, which can allow the WMAA algorithm
pay more attention to the reliable relative motions so as to obtain the
accurate and robust registration results. While, the MATrICP algorithm treat
each relative motion equally, so it is difficult to obtain good registration
results. To achieve multi-view registration, CFTrICP should adjust all the
registration parameters simultaneously, which can make it easy to trap into
local minimum. Hence, the robustness of CFTrICP is poor especial under the
high noise levels. Besides, the LRS-L1a1m algorithm adopts the low-rank and
sparse decomposition method to achieve multi-view registration. It only allows
some of relative motions are absent and may be failed due to the high ratio of
missing relative motions. In one words, the WMAA algorithm has the superior
performance for the registration of multi-view range scans.

##  5 Conclusion

This paper presents a novel approach for registration of multi-view range
scans. The main contribution is proposing the weighted motion averaging
algorithm, which can take into account the reliability and accuracy of each
motion obtained from the pair-wise registration. Since the reliability and
accuracy of the pair-wise registration results can be raised with the increase
of the overlapping percentage of scan pair, it then adopts the TrICP algorithm
to estimate the overlapping percentage and obtain the relative motion of each
scan pair with a certain degree of overlapping percentage. By viewing the
estimated overlapping percentages as weights, it can apply the weighted motion
averaging algorithm to achieve good registration of multi-view range scans.
Experimental results tested on several public datasets demonstrate that the
WMAA algorithm has superior performances in accuracy, robustness and
efficiency over the state-of-art approaches.

As this approach requires initial parameters for multi-view registration, our
future work will be committed to estimating the initial registration
parameters.

##  6 Acknowledgments

This work is supported by the National Natural Science Foundation of China
under Grant nos. 61573273, 61573280 and 61503300.

## References

  * (1) Lu H, Li Y, Serikawa S, et al (2016) 3D underwater scene reconstruction through descattering and colour correction. International Journal of Computational Science and Engineering 12(4): 352-359
  * (2) Yu N, Kihara K, Tadoh R, et al (2016) Super Resolving of the Depth Map for 3D Reconstruction of Underwater Terrain Using Kinect. In: Proceedings of IEEE International Conference on Parallel and Distributed Systems (ICPADS) 2016:1237-1240
  * (3) Geng N, Ma F, Yang H, et al (2016) Neighboring constraint-based pairwise point cloud registration algorithm. Multimedia Tools and Applications 75(24): 16763-16780
  * (4) Held D, Thrun S, and Savarese S (2016) Robust single-view instance recognition. In: Proceedings of IEEE International Conference on Robotics and Automation 2016:2152-2159
  * (5) Zhu J, Du SY, Ma L, Yuan ZJ, Zhang Q (2013) Merging Grid maps via point set registration, International Journal of Robotics and Automation, 28(2):180-191
  * (6) Shiratori T, Berclaz J, Harville M et al (2015) Efficient large-scale point cloud registration using loop closures. In: Proceedings of International Conference on 3d Vision(3DV) 2015:232-240
  * (7) Besl PJ,Mckay ND (2002) A method for registration of 3d shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence 14(2):239-256
  * (8) Chetverikov D, Stepanov D, Krsek P (2005) Robust euclidean alignment of 3d point sets: the trimmed iterative closest point algorithm. Image and Vision Computing 23(3):299-309
  * (9) Rusinkiewicz S, Levoy M (2001) Efficient variants of the icp algorithm. In: Proceedings of 3DIM 2001:145-152
  * (10) Godin G, Rioux M, Baribeau R (1994) Three-dimensional registration using range and intensity information. Proceedings of SPIE Volume 2350:279-290
  * (11) Zhu J, Wang D, Bai XX et al (2016) Registration of Point Clouds Based on the Ratio of Bidirectional Distances. In: Proceedings of 2016 Fourth International Conference on 3D Vision (3DV) 2016:102-107
  * (12) Jian B, Vemuri BC (2011) Robust point set registration using gaussian mixture models. IEEE Transactions on Pattern Analysis and Machine Intelligence, 33(8):1633-1645
  * (13) Myronenko A, Song X (2006) Point set registration: Coherent point drift. Advances in Neural Information Processing Systems 19(12):1009-1016
  * (14) Tsin Y, Kanade T (2004) A correlation-based approach to robust point set registration. In: Proceedings of European Conf. on Computer Vision(ECCV) 3023:558-569
  * (15) Lomonosov E, Chetverikov D, Ekart A (2006) Preregistration of arbitrarily oriented 3d surfaces using a genetic algorithm. Pattern Recognition Letters 27(11):1201-1208
  * (16) Zhu J, Meng D, Li Z et al (2014) Robust registration of partially overlapping point sets via genetic algorithm with growth operator. IET Image Processing 8(10):582-590
  * (17) Lu H, Li Y, Xu X et al (2016) Underwater image enhancement method using weighted guided trigonometric filtering and artificial light correction. Journal of Visual Communication and Image Representation 38:504-516
  * (18) Chen Y , Medioni G (1992) Object modeling by registration of multiple range images. IEEE International Conference on Robotics and Automation 10(3):145-155
  * (19) Shi SW, Chuang YT, Yu TY (2009) An efficient and accurate method for the relaxation of multiview registration error. IEEE Transactions on Image Processing A Publication of the IEEE Signal Processing Society 17(6):968-981
  * (20) Fantoni S, Castellani U, Fusiello A (2012) Accurate and automatic alignment of range surfaces. In: Proceedings of Conference on 3D Imaging 2012:73-80
  * (21) Guo YL, Sohel F, Bennamoun M et al (2014) An accurate and robust range image registration algorithm for 3D object modeling. IEEE Transactions on Multimedia 16(5):1377-1390
  * (22) Zhu J, Li Z, Du S, Ma L et al (2014) Surface reconstruction via efficient and accurate registration of multiview range scans. Optical Engineering 53(10):2468-2468
  * (23) Arrigoni F, Rossi B, Fusiello A (2016) Global Registration of 3D Point Sets via LRS Decomposition. Computer Vision ECCV 2016:489-504
  * (24) Arrigoni F, Magri L, Rossi B et al (2014) Robust absolute rotation estimation via low-rank and sparse matrix decomposition. In: Proceedings of the International Conference on 3D Vision (3DV) 2014:491-498
  * (25) He J, Balzano L, Szlam A (2012) Incremental gradient on the Grassmannian for online foreground and background separation in subsampled video. In: Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition 2012:1568-1575
  * (26) Zheng Y, Liu G, Sugimoto S (2012) Practical low-rank matrix approximation under robust L1-norm. In: Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition 2012:1410-1417
  * (27) Govindu VM, Pooja A (2014) On averaging multiview relations for 3d scan registration. IEEE Transactions on Image Processing 23(3):1289-1302
  * (28) Govindu VM (2004) Lie-algebraic averaging for globally consistent motion estimation. In:Proceedings of the IEEE Computer Society Conference on Computer Vision and Pattern Recognition(CVPR) 2004:684-691
  * (29) Li ZY, Zhu J, Lan K (2014) Improved Techniques for Multi-view Registration with Motion Averaging. In:Proceedings of the International Conference on 3D Vision(3DV) 2014:713-719
  * (30) Levoy M. The stanford 3d scanning repository, http://graphics.stanford.edu/data/3dscanrep/


# Informed Circular Fields for Global Reactive Obstacle Avoidance of Robotic
Manipulators

Marvin Becker  Philipp Caspers  Tom Hattendorf  Torsten Lilge  Sami Haddadin
Matthias A. Müller  Institut of Automatic Control, Leibniz University
Hannover, Germany (e-mail<EMAIL_ADDRESS> Munich
School of Robotics and Machine Intelligence and Chair of Robotics and System
Intelligence, Technical University Munich, Germany (e-mail:
sami.haddadin@tum.de).

###### Abstract

In this paper a global reactive motion planning framework for robotic
manipulators in complex dynamic environments is presented. In particular, the
circular field predictions (CFP) planner from Becker et al. (2021) is extended
to ensure obstacle avoidance of the whole structure of a robotic manipulator.
Towards this end, a motion planning framework is developed that leverages
global information about promising avoidance directions from arbitrary
configuration space motion planners, resulting in improved global trajectories
while reactively avoiding dynamic obstacles and decreasing the required
computational power. The resulting motion planning framework is tested in
multiple simulations with complex and dynamic obstacles and demonstrates great
potential compared to existing motion planning approaches.

###### keywords:

Autonomous robotic systems, Robots manipulators, Guidance navigation and
control, Motion Planning, Real-Time Collision Avoidance

††thanks: This work was supported in part by the Region Hannover in the
project _roboterfabrik_.

##  1 Introduction

_Motivation:_ Classical industrial robotic applications require fences or
other peripheral safety installations to ensure seamless production processes
and the safety of human coworkers. However, these measures result in
increasing costs and larger space requirements while decreasing flexibility.
As a consequence, robots, especially collaborative systems, are increasingly
exposed to dynamic and unpredictable environments requiring collision-free
motion planning. However, collision avoidance of robotic manipulators in
dynamic environments remains a major challenge (Liu et al., 2022).
_Related Work:_ Due to the amount of research on motion planning, we focus on
sampling-based and reactive approaches, using the classification of motion
planners into _sense-plan-act_ , _locally reactive control_ and _reactive
planning_ (Kappler et al., 2018). Traditional approaches typically use the
_sense-plan-act_ paradigm with a rather strict separation of perception,
motion planning and control. Most sampling-based approaches belong to this
category. Among those, the rapidly-exploring random trees (RRT) algorithm
(LaValle, 1998) and the probabilistic roadmap method (PRM) approach (Kavraki
et al., 1996) are the most widely used concepts. The RRT algorithm
incrementally constructs a search tree with randomly selected nodes in the
robot’s configuration space to find a feasible path from the initial to the
goal pose. Due to the ease of implementation, many successful applications,
and the simple extension to higher dimensional problems, the RRT planner has
been improved and extended to different variants, e.g., the RRT-connect
improves the runtime (Kuffner and LaValle, 2000), whereas the asymptotically
optimal RRT * decreases the resulting path length (Karaman and Frazzoli,
2011). The PRM algorithm consists of a learning and a query phase. During the
learning phase, the robot’s configuration space is randomly sampled and
configurations that collide with obstacles are rejected. The resulting roadmap
is then used for the query phase, which calculates the shortest path from a
start to a goal pose. Extensions of the PRM planner focus on improved path
quality (PRM * (Karaman and Frazzoli, 2011)) or shorter runtimes (LazyPRM *
(Hauser, 2015)). Although the improvements of sampling based planners
significantly decrease computation costs and show good results even in dynamic
environments, Kappler et al. (2018) has shown the advantages of reactive
approaches over the classical sense-plan-act methodology, particularly in
highly dynamic or uncertain environments.
While sampling-based approaches generally perform global planning, locally
reactive control methods only consider obstacles in direct proximity and
calculate the immediate next control command, allowing instantaneous reactions
to environment changes. A widespread example is the artifical potential field
(APF) approach (Khatib, 1986), where the robot is controlled by artificial
repulsive and attractive forces for a collision-free motion to the goal pose.
Other approaches use repulsive forces or velocities for collision avoidance of
the whole structure of the robot, which were either applied on predefined
control points along the robot structure (Chen and Song, 2018) or use the
closest distance between robot and obstacle (Maciejewski and Klein, 1985).
Another alternative is the circular field (CF) approach (Singh et al., 1996)
generating artificial forces similar to forces on charged particles in
electromagnetic fields. The approach gained recent interest due to its ability
to smoothly guide the robot around obstacles without local minima (Haddadin et
al., 2011; Ataka et al., 2018; Laha et al., 2021a, b). However, immediate
reactions and local sensor information prevent locally reactive approaches
from finding optimal global paths.
Kappler et al. (2018) introduce the term _reactive planner_ for hybrid motion
planners that are able to quickly react to local changes while simultaneously
improving the global path in case of larger changes. Hybrid approaches,
generally using a combination of PRM or RRT and APF, have been mainly applied
in mobile robotics, e.g., (Ravankar et al., 2020), but also for manipulators
(Li et al., 2021). Other hybrid approaches are the popular elastic strip
framework (Brock and Khatib, 2002), or our recent circular field predictions
(CFP) planner (Becker et al., 2021), which extends the CF approach with a
predictive multi-agent framework for global path exploration. The CFP planner
showed promising results in terms of computation load and path quality and was
successfully applied on a 7-degree of freedom (DoF) manipulator. For this
planner, collision avoidance and goal convergence under defined conditions
were rigorously proven in Becker et al. (2022). Nevertheless, the approach was
so far only used to achieve obstacle avoidance of the robot’s endeffector
(EE).
_Contribution:_ The contributions of this paper include:

  * •

Extension of the CFP planner for full body obstacle avoidance of robotic
manipulators by introducing additional control points along the robot
structure and defining suitable control forces.

  * •

Integration of global environment information from arbitrary global planners
in the multi-agent framework from Becker et al. (2021) resulting in the
informed circular fields (ICF) planner.

  * •

Two algorithms for leveraging global information about promising avoidance
directions with the reactive ICF planner.

  * •

Extensive comparison of the ICF planner against widely used global and local
motion planning approaches in a total of more than 200 simulations.

##  2 Full Body Avoidance using Circular Field Predictions

In this section, we introduce the natural extension of the CFP planner from
Becker et al. (2021) for full body avoidance of a robotic manipulator. We use
a steering force for controlling the EE

$\boldsymbol{f}_{\mathrm{s_{ee}}}=\boldsymbol{f}_{\mathrm{cf}}+k_{\mathrm{vlc}}\boldsymbol{f}_{\mathrm{vlc}},$
(1)

that consists of the CF force $\boldsymbol{f}_{\mathrm{cf}}$ (Section 2.2) for
avoiding obstacles and an attractive potential force
$\boldsymbol{f}_{\mathrm{vlc}}$ (Section 2.1) for goal convergence. The
scaling factor $k_{\mathrm{vlc}}\geq 0$ of $\boldsymbol{f}_{\mathrm{vlc}}$ is
explained in detail in Section 2.1. While $\boldsymbol{f}_{\mathrm{cf}}$ only
contains translational forces, $\boldsymbol{f}_{\mathrm{vlc}}$ additionally
includes rotational components to reach the desired goal pose.

###  2.1 Attractive Goal Force

As in Becker et al. (2021) we use the velocity limiting controller (VLC) from
Khatib (1986) instead of a simple attractive potential. This ensures that the
robot moves at a constant maximum velocity during the nominal motion.
Additionally, we define the scaling factor $k_{\mathrm{vlc}}$ as

$k_{\mathrm{vlc}}=\begin{cases}0&\\!\text{if
}\dot{\boldsymbol{x}}\\!\cdot\\!\\!\boldsymbol{f}_{\mathrm{vlc}}\\!\leq
0\land|\\!|\dot{\boldsymbol{x}}|\\!|\\!\leq\\!v_{\min}\land|\\!|\boldsymbol{x}_{\mathrm{d}}-\boldsymbol{x}|\\!|\\!>\\!\xi\\\
w&\\!\text{otherwise}\end{cases}$ (2)

with the scaling factor $w=w_{1}w_{2}w_{3}$ and $w_{1},w_{2},w_{3}\geq 0$ from
Ataka et al. (2018), which are used to prioritize obstacle avoidance over goal
convergence. The CF force does not change the magnitude of the robot velocity,
which is therefore only modified by the VLC. Using Eq. 2 and therefore
deactivating $\boldsymbol{f}_{\mathrm{vlc}}$ when it works against the current
motion direction while the velocity is below or equal to a defined $v_{\min}$,
we ensure that the robot will only decrease its velocity below this minimum,
when the robot is in the vicinity $\xi>0$ of the goal pose.

###  2.2 Endeffector Obstacle Avoidance

We use a similar CF force definition as Becker et al. (2021) for the obstacle
avoidance of the EE and assume that obstacle data is received in point cloud
format, which is commonly used by robotic sensors like depth cameras or laser
scanners. The CF force for a single point on a point cloud obstacle is defined
as

$\boldsymbol{f}_{\mathrm{cf}}:=\frac{k_{\mathrm{cf}}}{|\\!|\boldsymbol{d}|\\!|}\,\text{\@text@baccent{$\dot{\dv}$}}\times\boldsymbol{B}$
(3)

with the notation
$\text{\@text@baccent{$\dot{\dv}$}}=\frac{\dot{\boldsymbol{d}}}{|\\!|\dot{\boldsymbol{d}}|\\!|}$
and the constant gain $k_{\mathrm{cf}}\geq 0$, the distance
$\boldsymbol{d}=\boldsymbol{x}_{\mathrm{o}}-\boldsymbol{x}$ and the relative
translational velocity $\dot{\boldsymbol{d}}$ between the obstacle at position
$\boldsymbol{x}_{\mathrm{o}}$ and the robot. The artificial magnetic field
$\boldsymbol{B}$ is defined as
$\boldsymbol{B}:=\boldsymbol{c}\times\text{\@text@baccent{$\dot{\dv}$}}$ with
the artificial electric current
$\boldsymbol{c}:=\boldsymbol{n}\times\boldsymbol{b}$, where $\boldsymbol{n}$
is the normalized surface normal of the point cloud point111Note that many
approaches for the surface normal approximation of point clouds exist in the
literature. We use the provided functionality of the Point Cloud Library (Rusu
and Cousins, 2011). and $\boldsymbol{b}$ is the artificial magnetic field
vector, which is used to define the avoidance direction around an obstacle.
The artificial magnetic field vector is therefore of crucial importance for
the avoidance maneuver and different possibilities for a meaningful choice are
introduced in Sections 2.5 and 3.2. The total avoidance force is the sum of
the forces of all points $m_{j}$ on all obstacles $n_{\mathrm{o}}$

$\boldsymbol{f}_{\mathrm{cf}}=\frac{1}{\sum^{n_{\mathrm{o}}}_{j=0}m_{j}}\sum^{n_{\mathrm{o}}}_{j=0}\sum^{m_{j}}_{k=0}\boldsymbol{f}_{\mathrm{cf}_{j,k}}.$
(4)

In order to save computational resources, only obstacle points $k$ on surfaces
facing the robot, i.e. $\boldsymbol{n}_{k}\cdot\boldsymbol{d}_{k}<0$, and in a
range $|\\!|\boldsymbol{d}_{k}|\\!|\leq d_{\mathrm{r}}$ around the robot are
used. However, in contrast to a majority of approaches in the literature, we
clearly exploit more information about the obstacles than only the obstacle
point with the minimum distance. This makes our algorithm less sensitive to
sensor noise and yields improved avoidance behavior in the presence of
multiple obstacles without oscillations.

###  2.3 Robot Body Obstacle Avoidance

To enable obstacle avoidance for the entire robot body, we define
$n_{\mathrm{cp}}$ additional control points along its structure, as
exemplarily shown for a 7-DoF robot arm in Fig. 1. The control points should
be placed on prominent points of the robot so that the whole structure can be
moved away from obstacles.

Figure 1: Visualization of possible robot control points.

Note that for the example in Fig. 1, we have refrained from adding more
control points on the lower links as their obstacle avoidance capabilities are
limited. Those additional control points are also subject to the CF avoidance
forces to guide them around obstacles. However, the motion of these control
points may be constrained and the guiding force in Eq. 4 might not be
sufficient to keep a safe distance to the obstacle. Towards this end, we add
an additional repulsive CF-like force, where the artificial current is defined
as the negative distance vector

$\boldsymbol{f}_{\mathrm{r}}:=\text{\@text@baccent{$\dot{\dv}$}}\times\left(-\text{\@text@baccent{$\dv$}}\times\text{\@text@baccent{$\dot{\dv}$}}\right)f(|\\!|\boldsymbol{d}|\\!|),$
(5)

where we use the logistic amplitude function
$f(|\\!|\boldsymbol{d}|\\!|)=\frac{1}{2}(1+\tanh(\alpha-\beta\cdot|\\!|\boldsymbol{d}|\\!|))$
from Luo et al. (2014) with the tunable parameters $\alpha,\beta\geq 0$. Note
that Eq. 5 is a simplification of the force from Ataka et al. (2022), which
pushes the control point away from the obstacle point while maintaining the
useful properties of the original CF force, i.e., it is perpendicular to the
robot velocity and therefore does not induce local minima. Subsequently, the
total avoidance force on a control point is ${\rm
Fund.~{}Cosmic~{}Phys.}=\boldsymbol{f}_{\mathrm{cf}}+\boldsymbol{f}_{\mathrm{r}}$.

###  2.4 Robot Control Signal

In this section we describe how we generate feasible desired joint velocities
for a robotic manipulator from the task space avoidance forces. Towards this
end, we consider the EE motion to be the primary task and first calculate a
desired joint velocity from the current EE velocity
$\dot{\boldsymbol{q}}_{\mathrm{d}}=\boldsymbol{J}_{\mathrm{ee}}^{\\#}(\boldsymbol{q})\dot{\boldsymbol{x}}_{\mathrm{ee}}$
using the Moore-Penrose pseudo inverse $\boldsymbol{J}_{\mathrm{ee}}^{\\#}$.
The artificial steering force is interpreted as a force on a unit mass, i.e.,
$\ddot{\boldsymbol{x}}_{\mathrm{ee}}=\boldsymbol{f}_{\mathrm{s_{ee}}}$. Then,
the operational space formulation from Khatib (1987) is used to calculate a
meaningful robot input

$\boldsymbol{\tau}_{\mathrm{ee}}=\boldsymbol{J}_{\mathrm{ee}}^{T}(\boldsymbol{q})\left(\boldsymbol{M}_{\mathrm{c}}(\boldsymbol{q})\boldsymbol{f}_{\mathrm{s_{ee}}}+\boldsymbol{c}_{\mathrm{c}}(\boldsymbol{q},\dot{\boldsymbol{q}})+\boldsymbol{g}(\boldsymbol{q})\right),$
(6)

with the Cartesian mass matrix $\boldsymbol{M}_{\mathrm{c}}(\boldsymbol{q})$,
the Coriolis and centrifugal terms
$\boldsymbol{c}_{\mathrm{c}}(\boldsymbol{q},\dot{\boldsymbol{q}})$, and the
gravitational forces $\boldsymbol{g}(\boldsymbol{q})$. Note that we do not
consider real forces but want to transform the artificial task space forces
into desired joint velocities as inputs of an appropriate joint velocity
controller that compensates gravity and dynamics. Therefore, we ignore
gravitational and Coriolis terms and set the mass matrix to the identity
matrix $\boldsymbol{I}$, which implies
$\ddot{\boldsymbol{q}}_{\mathrm{ee}}=\boldsymbol{\tau}_{\mathrm{ee}}$ and
leads to
$\boldsymbol{M}_{\mathrm{c}}(\boldsymbol{q})=\left(\boldsymbol{J}_{\mathrm{ee}}(\boldsymbol{q})\boldsymbol{I}\boldsymbol{J}_{\mathrm{ee}}^{T}(\boldsymbol{q})\right)^{-1}$.
Inserting $\boldsymbol{M}_{\mathrm{c}}(\boldsymbol{q})$ into Eq. 6 and
neglecting the gravitation and dynamic components yields
$\ddot{\boldsymbol{q}}_{\mathrm{ee}}=\boldsymbol{J}_{\mathrm{ee}}^{\\#}(\boldsymbol{q})\boldsymbol{f}_{\mathrm{s_{ee}}}$.
The forces on the control points do not need to follow any given dynamics and
are superposed and converted to joint accelerations conventionally:

$\ddot{\boldsymbol{q}}_{\mathrm{cp}}=\sum_{i=0}^{n_{\mathrm{cp}}-1}\boldsymbol{J}_{\mathrm{cp},i}^{T}(\boldsymbol{q})\,\boldsymbol{f}_{\mathrm{cp},i}.$
(7)

Finally, we calculate the joint velocity command for the next sampling step of
the controller

$\dot{\boldsymbol{q}}_{\mathrm{cmd}}=\dot{\boldsymbol{q}}_{\mathrm{d}}+\left(\ddot{\boldsymbol{q}}_{\mathrm{ee}}+\ddot{\boldsymbol{q}}_{\mathrm{cp}}+\ddot{\boldsymbol{q}}_{\mathrm{jla}}\right)\Delta
T,$ (8)

with the sampling time $\Delta T$. Note that we also added a simple spring-
like acceleration $\ddot{\boldsymbol{q}}_{\mathrm{jla}}$ for avoiding joint
limits.

###  2.5 Predictive Multi-Agent Framework

In order to transform the original locally reactive CF approach into a
reactive planner, we extend the predictive multi-agent framework from Becker
et al. (2021) to robotic manipulators. The framework is used to evaluate the
influence of selected control parameters on the robot in a current perception
of the environment under simplified dynamics. A predictive agent represents
the robot in this environment snapshot with one specific set of parameters
$\mathbb{P}$. In this paper we use the multi-agent framework primarily for the
magnetic field vector
$\boldsymbol{b}\in\mathbb{B}\subset\mathbb{R}^{n_{\mathrm{cp}}\times
n_{\mathrm{o}}}$ of the $n_{\mathrm{cp}}$ control points (including the EE).
After an initial agent with the current robot configuration is created, the
joint velocity commands are calculated as described in Eq. 8. Then, we
simulate the motion of the robot, assuming simplified dynamics and that the
controller follows the joint commands perfectly, i.e.,

$\displaystyle\boldsymbol{q}(t+1)$
$\displaystyle=\boldsymbol{q}(t)+\dot{\boldsymbol{q}}_{\mathrm{cmd}}(t)\Delta
T$ $\displaystyle\dot{\boldsymbol{q}}(t+1)$
$\displaystyle=\dot{\boldsymbol{q}}_{\mathrm{cmd}}(t)$
$\displaystyle\dot{\boldsymbol{x}}_{\mathrm{ee}}(t+1)$
$\displaystyle=\boldsymbol{J}_{\mathrm{ee}}(\boldsymbol{q}(t+1))\dot{\boldsymbol{q}}(t+1).$

This procedure is repeated until the simulated agent reaches the goal pose
$\boldsymbol{x}=\boldsymbol{x}_{\mathrm{d}}$. Whenever the EE or a control
point of the predictive agent comes close to an obstacle, i.e.,
$|\\!|\boldsymbol{d}|\\!|\leq d_{\mathrm{r}}$, $n_{\mathrm{a}}$ new agents
with different parameters are created for exploring different avoidance
directions around an obstacle. All agents are evaluated after a defined number
of sampling steps by a cost function, which can be adapted to the desired
robot behavior and required task. The parameters of the best agent are then
used to control the robot. In order to ensure reactive behavior of the
planner, all agent calculations are computed in parallel to the control
command of the real robot. Details on the multi-agent framework and extensive
simulation results can be found in Becker et al. (2021). In contrast to our
previous work, the additional control points on the robot lead to a
significant increase of agents, especially in the case of many obstacles.
Furthermore, many combinations of magnetic field vectors for the different
control points will lead to infeasible trajectories or even to collisions.
Consequently, a considerable amount of computing power is wasted. To
compensate for these disadvantages, we introduce the concept of ICF in the
next section.

##  3 Informed Circular Fields

The idea of ICF is to leverage the strengths of global and local motion
planning strategies by extracting useful information from a global pre-
planner, which are then transferred and evaluated by the predictive agents to
improve the reactive motion generation. The generation of a motion command in
our ICF framework can be split into four distinct phases. First, a global
configuration space motion planner is used to create a (coarse) joint
trajectory. Then, the global information is extracted in form of the magnetic
field vectors for all control points and obstacles as a basis for CF-based
reactive motion planning. Note that when we refer to the control points here
and in the following, the EE is also included if not stated otherwise.
Subsequently, multiple predictive agents are created with the extracted
magnetic field vectors to process and evaluate the global motion plan. The
parameters of the best agent are then transferred to the real robot and used
to generate the reactive joint velocity commands. The different phases are
executed in parallel with different sampling rates.

###  3.1 Global Trajectory Generation

The main purpose of the global pre-planner is to infer estimates for feasible
global trajectories from the current robot pose to a goal pose in
configuration space, i.e., finding non-conflicting avoidance directions around
the obstacles for all control points is more important than generating short
and smooth paths. Thus, we use a rather coarse discretization of the global
planner for shorter planning cycles $T_{\mathrm{global}}$. The global planner
is continuously running and restarted after $T_{\mathrm{global}}$ seconds with
updated obstacle and robot information. When a successful trajectory is found
during the planning time, the trajectory is passed to the global information
extraction module. We use the MoveIt! motion planning framework for global
trajectory generation because it features a variety of sampling-based planners
and supports point clouds as a format for obstacle representation (Coleman et
al., 2014). However, other configuration space planners can also be used and
exchanged easily. Note that the global motion planning is done in a static
snapshot of the environment while the actual reactive creation of the control
signal (Eq. 8) is always done with the most current environment information.

###  3.2 Global Information Extraction

Whenever the global pre-planner finds a joint trajectory, the global
information extraction module transforms this joint trajectory into Cartesian
trajectories for each control point. Then, the avoidance direction of each
control point around each obstacle is extracted from those Cartesian
trajectories and the corresponding magnetic field vectors are calculated. For
each control point $i$ and for each obstacle $j$ described by the points
$\boldsymbol{x}_{\mathrm{o}_{j,k}}\in\mathbb{O}_{j}\subset\mathbb{R}^{3\times
m_{j}}$, the following calculations are performed.

_Algorithm 1_

  1. 1.

Find the closest point
$\boldsymbol{p}_{\mathrm{c}}=\boldsymbol{x}_{\mathrm{cp_{i}}}(\tau)$ of
trajectory $\boldsymbol{x}_{\mathrm{cp_{i}}}$ to obstacle $j$, where
$\displaystyle\tau=\underset{t\geq
0}{\arg\min}(\underset{\boldsymbol{x}_{\mathrm{o}_{j,k}}\in\mathbb{O}_{j}}{\min}|\\!|\boldsymbol{x}_{\mathrm{cp_{i}}}(t)-\boldsymbol{x}_{\mathrm{o}_{j,k}}|\\!|)$
.

  2. 2.

Calculate the (approximate) direction of movement
$\boldsymbol{v}_{\mathrm{c}}=\boldsymbol{x}_{\mathrm{cp_{i}}}(\tau+1)-\boldsymbol{x}_{\mathrm{cp_{i}}}(\tau-1)$
at the closest point $\boldsymbol{p}_{\mathrm{c}}$.

  3. 3.

Calculate
$\boldsymbol{d}_{\mathrm{c}}=\boldsymbol{x}_{\mathrm{o}_{j,\mathrm{c}}}-\boldsymbol{p}_{\mathrm{c}}$
pointing from $\boldsymbol{p}_{\mathrm{c}}$ to the closest obstacle point
$\boldsymbol{x}_{\mathrm{o}_{j,\mathrm{c}}}=\underset{\boldsymbol{x}_{\mathrm{o}_{j,k}}}{\arg\min}(|\\!|\boldsymbol{x}_{\mathrm{o}_{j,k}}-\boldsymbol{p}_{\mathrm{c}}|\\!|)$.

  4. 4.

Calculate the magnetic field vector
$\boldsymbol{b}_{i,j}=\frac{\boldsymbol{d}_{\mathrm{c}}\times\boldsymbol{v}_{\mathrm{c}}}{|\\!|\boldsymbol{d}_{\mathrm{c}}\times\boldsymbol{v}_{\mathrm{c}}|\\!|}$.

_Algorithm 2_

  1. 1.

Find the first point
$\boldsymbol{p}_{\mathrm{in}}=\boldsymbol{x}_{\mathrm{cp_{i}}}(\tau_{\min})$
of the trajectory $\boldsymbol{x}_{\mathrm{cp_{i}}}$ in a ball of radius $r$
around obstacle $j$, where $\tau_{\min}=\underset{t\geq
0,\boldsymbol{x}_{\mathrm{o}_{j,k}}\in\mathbb{O}_{j}}{\min}\left(t\mid|\\!|\boldsymbol{x}_{\mathrm{cp_{i}}}(t)-\boldsymbol{x}_{\mathrm{o}_{j,k}}|\\!|\leq
r\right)$.

  2. 2.

Find the last point
$\boldsymbol{p}_{\mathrm{out}}=\boldsymbol{x}_{\mathrm{cp_{i}}}(\tau_{\max})$
of the trajectory $\boldsymbol{x}_{\mathrm{cp_{i}}}$ in a ball of radius $r$
around obstacle $j$, where $\tau_{\max}=\underset{t\geq
0}{\max}\underset{\boldsymbol{x}_{\mathrm{o}_{j,k}}\in\mathbb{O}_{j}}{\min}\left(t\mid|\\!|\boldsymbol{x}_{\mathrm{cp_{i}}}(t)-\boldsymbol{x}_{\mathrm{o}_{j,k}}|\\!|\leq
r\right)$.

  3. 3.

Find the point
$\boldsymbol{p}_{\mathrm{m}}=\boldsymbol{x}_{\mathrm{cp_{i}}}(\tau)$ with
$\tau$ being the largest integer less than or equal to the midpoint in
$[\tau_{\min},\tau_{\max}]$, i.e., $\tau=\lfloor
0.5(\tau_{\max}+\tau_{\min})\rfloor$.

  4. 4.

Calculate the vectors
$\boldsymbol{v}_{\mathrm{m,in}}=\boldsymbol{p}_{\mathrm{in}}-\boldsymbol{p}_{\mathrm{m}}$
pointing from $\boldsymbol{p}_{\mathrm{m}}$ to $\boldsymbol{p}_{\mathrm{in}}$
and
$\boldsymbol{v}_{\mathrm{m,out}}=\boldsymbol{p}_{\mathrm{out}}-\boldsymbol{p}_{\mathrm{m}}$
from $\boldsymbol{p}_{\mathrm{m}}$ to $\boldsymbol{p}_{\mathrm{out}}$.

  5. 5.

Calculate the magnetic field vector
$\boldsymbol{b}_{i,j}=\frac{\boldsymbol{v}_{\mathrm{m,in}}\times\boldsymbol{v}_{\mathrm{m,out}}}{|\\!|\boldsymbol{v}_{\mathrm{m,in}}\times\boldsymbol{v}_{\mathrm{m,out}}|\\!|}$.

Visualizations of both algorithms are shown in Fig. 2 in 2D example setups.
The quality of the resulting avoidance motion from both methods highly depends
on the environment and the global trajectory. While the first algorithm
captures the avoidance motion only at the closest obstacle point, the second
algorithm is used to recreate the resulting avoidance direction in a larger
area around the obstacle. Therefore, we use both methods to calculate magnetic
field vectors, which are adopted by the predictive agents as described in the
next section. Nevertheless, we simplify a potentially complex avoidance motion
to a single vector per obstacle and control point. Thus, the resulting motion
of the ICF planner is expected to deviate from the global trajectory. However,
we would like to emphasize again that we only use the global planner to
estimate possible, non-contradictory avoidance directions, which are
subsequently simulated and evaluated before being used to enable reactive
control of the real robot.

Figure 2: Visualization of global information extraction.

###  3.3 Adapting the Multi-Agent Framework

In order to adequately account for the new information from the global pre-
planner, the process for creating and deleting predictive agents (compare
Section 2.5) is redesigned. Instead of creating new agents when an obstacle is
encountered, new agents are created when new magnetic field vectors are
received. In particular, one agent with the current best parameter set
$\mathbb{P}_{\mathrm{best}}$ and the current best magnetic field vectors
$\mathbb{B}_{\mathrm{best}}$ and two agents with the current best parameter
set $\mathbb{P}_{\mathrm{best}}$ and the new magnetic field vector sets
$\mathbb{B}_{\mathrm{A1}}$, $\mathbb{B}_{\mathrm{A2}}$ from Alg. 1 and Alg. 2
are created. Afterwards, more agents can be created to simulate the robot
motion with different parameters and the three magnetic field vector sets. In
this paper, new agents are created which use additional forces in the
nullspace to push the robot into configurations with higher manipulability
(details are omitted due to space limitations) but also simple adaptions of,
e.g., the scaling factors could be simulated. Note that an agent with the best
parameter set but older environment data could still exist when new agents are
created. However, the planning time of the global planner is typically much
higher than the simulation of the agents and all agents finish before new
magnetic field vectors are received. Moreover, the trajectory of the real
robot is continuously compared to the predicted trajectories of the agents.
Whenever the deviation of a trajectory point reaches a defined limit, the
respective agent is deleted. Additionally, agent predictions are stopped and
evaluated after a defined number of prediction steps $n_{\mathrm{ps}}$ and
appended to the end of the waiting queue, preventing agents that take long
suboptimal trajectories from blocking the available computation slots.

###  3.4 Safety-Improving Fallback

During motion planning, it may happen that none of the predicted trajectories
match the current state of the environment, either because of unpredictable
movements of the obstacles or because the robot cannot follow the predicted
trajectories with sufficient accuracy. In this case, collision avoidance has
absolute priority and the robot switches to a safety-improving fallback
behavior. Here, the CF forces on the control points on the robot structure
(not the EE) are replaced by repulsive forces with the scaling from Luo et al.
(2014) in the form
$\boldsymbol{f}_{\mathrm{cp}_{\mathrm{rep}}}=-0.5\left(1+\tanh(\alpha-\beta\cdot|\\!|\boldsymbol{d}|\\!|)\right)\text{\@text@baccent{$\dv$}}$.
The safety-improving fallback is deactivated as soon as an agent finds a
feasible trajectory to the goal pose.

##  4 Evaluation

In this section, we evaluate the performance of the ICF planner in five static
and dynamic environments against other reactive and global motion planning
approaches. The evaluations are performed in a kinematic simulation without
disturbances and with complete knowledge of the current positions and
velocities of the obstacles. We use a C++ implementation on a PC with an AMD
Ryzen 9 5950X CPU with 16 cores, $3.4\text{\,}\mathrm{G}\mathrm{H}\mathrm{z}$.
Our simulations include complex environments with multiple dynamic obstacles
which are much easier to follow in videos than images. Thus, in addition to
presenting the scenarios in Fig. 3, we uploaded videos of all simulations
here: https://doi.org/10.25835/erbbvx5h.

Figure 3: Simulation environments used for comparisons.

For the comparison, we included two different empirically chosen pre-planners
within our ICF framework, the PRM and the RRT * planner. Among the reactive
control approaches, we use the original APF approach from Khatib (1986), the
CF approach from Ataka et al. (2018) and our extension of the CFP from Becker
et al. (2021) introduced in Section 2. Note that the safety-improving fallback
and our attractive force is also implemented for the locally reactive
controllers to allow a fair comparison of the force command generation. The
global planners include the same sampling-based planners in their original
version as used for our pre-planner, the PRM and RRT * as well as the RRT
Connect and the optimizing STOMP planner from Kalakrishnan et al. (2011) due
to their high prevalence and availability in the MoveIt! framework. The
sampling-based planners and the STOMP planner only consider static obstacles
and are therefore only tested in the two static environments, in which a
maximum planning time of $0.5\text{\,}\mathrm{s}$ was allowed. If no solution
was found in this time, the attempt was considered a failure. All non-
deterministic planners were executed until ten successful runs were recorded
for each planner in each environment. Note that iteration time refers to the
planning time for global planners (depicted with a * in Table 1) while it is
used to specify the calculation time for a control command for the reactive
planners. Also note that the path duration depends highly on the allowed
maximum velocities. While the EE velocity of the force-based methods can be
specified via a Cartesian maximum velocity, the random-based planners used
here operate exclusively in joint space. Specifying an EE velocity by means of
constraints in the joint space is not straightforward, therefore, a
measurement of the duration for the affected planners was omitted.
Overall, the results in Table 1 show that the ICF planner is able to
outperform the other tested motion planners. Although its path length in
static environments is (slightly) larger than the result from the optimizing
STOMP planner, it still remains in a comparable range while additionally being
able to reactively avoid dynamic obstacles. The advantage of the ICF planner
is then particularly noticeable in the second static environment. Even though
the environment does not look complicated at first, a relatively complex joint
movement of the robot is required in order not to collide with the obstacles,
which leads to problems of the optimizing STOMP and RRT *, reflected in the
significantly lower success rates. The CFP planner also showed problems in
this environment because of the extensive number of predictive agents that
were created, also noticeable in the significantly increased iteration times.
Therefore, no suitable combination of avoidance directions could be found and
opposing forces on the control points led to a collision. The complex
environment requires simultaneous avoidance motions of the EE and the body and
was meant to test the limits of the motion planners; our ICF planner was the
only planner that was able to successfully reach the goal without a collision.
However, as can be seen in Table 1, it was not able to succeed in all runs.
The ICF planner failed when the pre-planner was repeatedly not able to find a
path to the goal in the given planning time of $0.2\text{\,}\mathrm{s}$. Then,
the ICF planner used the safety-improving fallback method and the obstacles
pushed the robot into configurations which were close to the joint limits,
where no avoidance was possible and a collision became inevitable. Similarly,
in one execution in the environment _Dynamic 1_ , the ICF-RRT* planner ended
in a configuration close to the joint limits and was not able to reach the
goal afterwards.

Table 1: Motion planner comparison.

Env. | Method | Length [m] | Duration [s] | Success | Iter. Time [ms]
---|---|---|---|---|---
Static 1 | PRM | 1.81 | - | 100 % | 190.0*
RRTConnect | 1.51 | - | 100 % | 74.6*
RRT* | 1.71 | - | 100 % | 500.0*
STOMP | 1.34 | - | 100 % | 120.1*
Khatib | 1.65 | 17.81 | True | 0.020
Ataka | 1.86 | 19.56 | True | 0.018
CFP | 1.49 | 13.00 | True | 0.019
ICF-PRM | 1.41 | 11.88 | 100 % | 0.018
ICF-RRT* | 1.49 | 11.78 | 100 % | 0.019
Static 2 | PRM | 2.02 | - | 100 % | 237.3*
RRTConnect | 2.01 | - | 100 % | 257.8*
RRT* | 2.00 | - | 62 % | 500.0*
STOMP | 1.72 | - | 43 % | 163.3*
Khatib | - | - | False | 0.039
Ataka | - | 60.00 | False | 0.045
CFP | - | - | False | 0.209
ICF-PRM | 1.88 | 30.45 | 100 % | 0.057
ICF-RRT* | 2.15 | 26.51 | 100 % | 0.053
Dyn. 1 | Khatib | 1.50 | 11.46 | True | 0.029
Ataka | 1.53 | 10.87 | True | 0.030
CFP | - | - | False | 0.053
ICF-PRM | 1.31 | 12.55 | 100 % | 0.035
ICF-RRT* | 1.48 | 13.13 | 91 % | 0.033
Dyn. 2 | Khatib | 1.68 | 33.81 | True | 0.020
Ataka | - | 60.00 | False | 0.021
CFP | 1.47 | 12.75 | True | 0.029
ICF-PRM | 1.51 | 11.96 | 100 % | 0.016
ICF-RRT* | 1.54 | 12.55 | 100 % | 0.014
Complex | Khatib | - | - | False | 0.082
Ataka | - | - | False | 0.080
CFP | - | - | False | 0.126
ICF-PRM | 2.12 | 27.42 | 71 % | 0.048
ICF-RRT* | 2.20 | 28.46 | 62 % | 0.058

##  5 Conclusion

In this paper, the CFP motion planner from Becker et al. (2021) was applied to
robotic manipulators and extended to the ICF algorithm by extracting
information about feasible avoidance directions from a global pre-planner. The
ICF planner was tested in several simulations, where it performed well even in
complex environments with multiple dynamic obstacles and demonstrated its
advantage over other widespread global and locally reactive motion planners.
Future research includes improving the global information extraction module,
which is currently not able to extract sufficient information when more
involved avoidance motions of complex obstacles are necessary. Additionally,
we are working on an implementation on a real robot.

## References

  * Ataka et al. (2018) Ataka, A., Lam, H.K., and Althoefer, K. (2018).  Reactive magnetic-field-inspired navigation for non-holonomic mobile robots in unknown environments.  In _Proc. IEEE Int. Conf. Robot. Autom._ , 6983–6988.
  * Ataka et al. (2022) Ataka, A., Lam, H.K., and Althoefer, K. (2022).  Magnetic-field-inspired navigation for robots in complex and unknown environments.  _Frontiers in Robotics and AI_ , 9.
  * Becker et al. (2022) Becker, M., Köhler, J., Haddadin, S., and Müller, M.A. (2022).  Motion Planning using Reactive Circular Fields: A 2D Analysis of Collision Avoidance and Goal Convergence.  _arXiv preprint arXiv:2210.16106_.  10.48550/ARXIV.2210.16106.
  * Becker et al. (2021) Becker, M., Lilge, T., Müller, M.A., and Haddadin, S. (2021).  Circular fields and predictive multi-agents for online global trajectory planning.  _IEEE Robot. Autom. Lett._ , 6(2), 2618–2625.
  * Brock and Khatib (2002) Brock, O. and Khatib, O. (2002).  Elastic Strips: A Framework for Motion Generation in Human Environments.  _Int. J. Robot. Res._ , 21(12), 1031–1052.
  * Chen and Song (2018) Chen, J.H. and Song, K.T. (2018).  Collision-free motion planning for human-robot collaborative safety under cartesian constraint.  In _Proc. IEEE Int. Conf. Robot. Autom._ , 4348–4354.
  * Coleman et al. (2014) Coleman, D., Sucan, I., Chitta, S., and Correll, N. (2014).  Reducing the Barrier to Entry of Complex Robotic Software: a MoveIt! Case Study.  _J. Soft. Eng. Robot._
  * Haddadin et al. (2011) Haddadin, S., Belder, R., and Albu-Schäffer, A. (2011).  Dynamic motion planning for robots in partially unknown environments.  _IFAC Proc. Vol._ , 44(1), 6842–6850.
  * Hauser (2015) Hauser, K. (2015).  Lazy collision checking in asymptotically-optimal motion planning.  In _Proc. IEEE Int. Conf. Robot. Autom._
  * Kalakrishnan et al. (2011) Kalakrishnan, M., Chitta, S., Theodorou, E., Pastor, P., and Schaal, S. (2011).  STOMP: Stochastic trajectory optimization for motion planning.  In _Proc. IEEE Int. Conf. Robot. Autom._ , 4569–4574.
  * Kappler et al. (2018) Kappler, D., Meier, F., Issac, J., Mainprice, J., Cifuentes, C.G., Wüthrich, M., Berenz, V., Schaal, S., Ratliff, N., and Bohg, J. (2018).  Real-Time Perception Meets Reactive Motion Generation.  _IEEE Robot. Autom. Lett._ , 3(3), 1864–1871.
  * Karaman and Frazzoli (2011) Karaman, S. and Frazzoli, E. (2011).  Sampling-based algorithms for optimal motion planning.  _Int. J. Robot. Res._ , 30(7), 846–894.
  * Kavraki et al. (1996) Kavraki, L.E., Svestka, P., Latombe, J.C., and Overmars, M.H. (1996).  Probabilistic roadmaps for path planning in high-dimensional configuration spaces.  _IEEE Trans. Robot. Autom._ , 12(4), 566–580.
  * Khatib (1987) Khatib, O. (1987).  A unified approach for motion and force control of robot manipulators: The operational space formulation.  _IEEE J. Robot. Autom._ , 3(1), 43–53.
  * Khatib (1986) Khatib, O. (1986).  Real-time obstacle avoidance for manipulators and mobile robots.  _Int. J. Robot. Res._ , 5(1), 90–98.
  * Kuffner and LaValle (2000) Kuffner, J.J. and LaValle, S.M. (2000).  RRT-connect: An efficient approach to single-query path planning.  In _Proc. IEEE Int. Conf. Robot. Autom._ , volume 2, 995–1001.
  * Laha et al. (2021a) Laha, R., Figueredo, L.F., Vrabel, J., Swikir, A., and Haddadin, S. (2021a).  Reactive cooperative manipulation based on set primitives and circular fields.  In _Proc. IEEE Int. Conf. Robot. Autom._ , 6577–6584.
  * Laha et al. (2021b) Laha, R., Vorndamme, J., Figueredo, L.F., Qu, Z., Swikir, A., Jähne, C., and Haddadin, S. (2021b).  Coordinated motion generation and object placement: A reactive planning and landing approach.  In _Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst._ , 9401–9407.
  * LaValle (1998) LaValle, S.M. (1998).  Rapidly-exploring random trees: A new tool for path planning: Technical Report.
  * Li et al. (2021) Li, S., Han, K., Li, X., Zhang, S., Xiong, Y., and Xie, Z. (2021).  Hybrid Trajectory Replanning-Based Dynamic Obstacle Avoidance for Physical Human-Robot Interaction.  _J. Intell. & Robot. Syst._, 103(3), 41.
  * Liu et al. (2022) Liu, H., Qu, D., Xu, F., Du, Z., Jia, K., Song, J., and Liu, M. (2022).  Real-time and efficient collision avoidance planning approach for safe human-robot interaction.  _J. Intell. & Robot. Syst._, 105(4).
  * Luo et al. (2014) Luo, R.C., Ko, M.C., Chung, Y.T., and Chatila, R. (2014).  Repulsive reaction vector generator for whole-arm collision avoidance of 7-DoF redundant robot manipulator.  In _Proc. IEEE/ASME Int. Conf. on Adv. Intell. Mechat._ , 1036–1041.
  * Maciejewski and Klein (1985) Maciejewski, A.A. and Klein, C.A. (1985).  Obstacle Avoidance for Kinematically Redundant Manipulators in Dynamically Varying Environments.  _Int. J. Robot. Res._ , 4(3), 109–117.
  * Ravankar et al. (2020) Ravankar, A.A., Ravankar, A., Emaru, T., and Kobayashi, Y. (2020).  HPPRM: Hybrid Potential Based Probabilistic Roadmap Algorithm for Improved Dynamic Path Planning of Mobile Robots.  _IEEE Access_ , 8, 221743–221766.
  * Rusu and Cousins (2011) Rusu, R.B. and Cousins, S. (2011).  3D is here: Point Cloud Library (PCL).  In _Proc. IEEE Int. Conf. Robot. Autom._
  * Singh et al. (1996) Singh, L., Stephanou, H., and Wen, J. (1996).  Real-time robot motion control with circulatory fields.  In _Proc. IEEE Int. Conf. Robot. Autom._ , volume 3, 2737–2742.