# Efficient Belief Road Map for Planning Under Uncertainty

Zhenyang Chen, Hongzhe Yu and Yongxin Chen Z. Chen, H. Yu and Y. Chen are with
the Georgia Institute of Technology, Atlanta, GA, USA. {zchen927, hyu419,
<EMAIL_ADDRESS>
###### Abstract

Robotic systems, particularly in demanding environments like narrow corridors
or disaster zones, often grapple with imperfect state estimation. Addressing
this challenge requires a trajectory plan that not only navigates these
restrictive spaces but also manages the inherent uncertainty of the system. We
present a novel approach for graph-based belief space planning via the use of
an efficient covariance control algorithm. By adaptively steering state
statistics via output state feedback, we efficiently craft a belief roadmap
characterized by nodes with controlled uncertainty and edges representing
collision-free mean trajectories. The roadmap’s structured design then paves
the way for precise path searches that balance control costs and uncertainty
considerations. Our numerical experiments affirm the efficacy and advantage of
our method in different motion planning tasks. Our open-source implementation
can be found at https://github.com/hzyu17/VIMP/tree/BRM.

##  I Introduction

In the challenging realm of robotic motion planning, uncertainty presents a
critical hurdle for effective operation in dynamic and complex real-world
environments. Historically, motion planning under uncertainty evolved from
deterministic motion planning foundations [1], adopting one of two primary
trajectories: the optimization-based approach and the sampling-based strategy.

The trajectory optimization paradigm, extensively studied in works like [2]
and [3], transforms planning challenges into optimal control problems. This
transformation necessitates the resolution of the Hamilton–Jacobi–Bellman
equation through dynamic programming techniques. However, while this method
promises precision, it faces significant scalability issues [4] [5], often at
the cost of local solutions or even infeasibility.

Figure 1: A belief space graph depicting sampled state beliefs and the
connected edges against obstacles. When prioritizing entropy cost over control
energy, the planner opts for Path 2 (shown in purple), which allows for
increased uncertainty, enhancing robot safety. Conversely, if control energy
minimization is paramount, the planner selects the more direct Path 1 (shown
in orange), demonstrating reduced uncertainty tolerance.

On the other end of the spectrum, sampling-based planning establishes motion
planning as a search problem. By utilizing algorithms such as the
probabilistic road maps (PRM) [6] and rapidly exploring random trees (RRT)
[7][8], this approach leverages graph structures filled with random feasible
states to pinpoint optimal paths. What sets this approach apart is its promise
of probabilistically complete solutions, assuring an increasing likelihood of
finding a feasible solution with more samples.

While deterministic motion planning offers a robust framework, introducing
uncertainties complicates the scenario significantly. This led to the
development of belief space planning [9] [10] [11] [12]. Essentially an
expansion of traditional planning to incorporate uncertainties, this approach
has seen a growing emphasis on belief road maps (BRM) [13] [14] [15] [16].
Unlike the conventional nodes of deterministic states in PRM, BRM employs
state distributions, bringing forth unique challenges, especially regarding
computational efficiency.

At the intersection of these challenges lies covariance steering, a discipline
geared towards guiding distributions. Notably, in a series of studies [17]
[18], Chen et al. illustrated that linear system distribution steering can be
achieved through closed-form solutions. More recent works [19] [20] expanded
on this by integrating safety constraints and addressing nonlinear control-
affine dynamics.

Building upon these advances, our research introduces a nuanced covariance
steering approach for graph-based motion planning. We tackle the BRM’s
existing challenges by proficiently crafting probabilistic graph edges.
Incorporating state estimation [21] further aligns our methodology with the
broader belief space planning framework, drawing parallels to chance-
constrained strategies like CC-RRT* [22]. Empirical evidence, as we will
present, accentuates the advantages of our approach over existing methods,
showcasing both its effectiveness and efficiency in managing uncertainties.

##  II Background

This section introduces belief space planning and covariance steering as key
components of our proposed method.

###  II-A Belief space planning

Belief space planning addresses the challenge of making decisions with
uncertain robot states where the belief state $b$ is a composite
representation of the robot’s state and its associated uncertainty. With new
input $u$ and observation $z$, the state transition function $\tau$ updates
the belief state $b^{{}^{\prime}}=\tau(b,u,z)$. Instead of always choosing the
shortest path, belief space planners leverage the belief information and
search for a more conservative motion plan when the state estimation is
uncertain, as shown in figure 1. A significant concern when planning in belief
spaces is the computational challenge due to the high dimensionality of belief
states. This problem can be addressed by sampling-based algorithms like PRM.

The belief-space variant of the PRM is called the Belief Roadmap (BRM) [13].
The primary idea of BRM is to sample both configurations and their
distributions in the belief state space, test them for feasibility, and then
attempt to connect nearby configurations to form a roadmap. The BRM can be
mathematically represented as a graph $G=(V,E)$ where $V=\\{b_{i}\\}$ is the
set of nodes representing feasible belief states and $E$ is the set of edges
indicating belief paths between adjacent nodes. To construct BRM, for each
pair of belief nodes $(b_{i},b_{j})$, a local planner attempts to find a
feasible path considering both the spatial constraints and the belief
evolution. The belief evolution accounts for uncertainty propagation,
influenced by robot dynamics and environmental factors. Once the belief
roadmap is constructed with the cost associated with traversal and belief
uncertainty, an optimal path can be found by graph search algorithms like
$A^{*}$ and Dijkstra.

###  II-B Covariance steering for control-affine systems

The covariance steering problem for nonlinear systems remains a challenge.
Recent progress established in [20] demonstrates an efficient algorithm
tailored for control-affine systems. We present the main results in this
section. The nonlinear system under consideration is

$dX_{t}=f(t,X_{t})dt+B(t)(u_{t}dt+\sqrt{\epsilon}dW_{t})$ (1)

where $X_{t}\in{\mathbb{R}}^{n}$ is the state vector,
$u_{t}\in{\mathbb{R}}^{p}$ is the input vector and $f(t,X_{t})$ is the drift
function. The input matrix $B(t)\in{\mathbb{R}}^{n\times p}$ is assumed to be
full rank. $W_{t}\in{\mathbb{R}}^{p}$ represents a standard Wiener process
[23], and $\epsilon>0$ parameterizes the intensity of the disturbance. The
covariance steering problem minimizes the control energy while seeking a state
feedback policy to steer state statistics of the system from an initial value
to a terminal one.

$\displaystyle\min_{u}$
$\displaystyle{\mathbb{E}}\left\\{\int_{0}^{T}[\frac{1}{2}\|u_{t}\|^{2}+V(X_{t})]dt\right\\}$
(2c) $\displaystyle dX_{t}=f(t,X_{t})dt+B(t)(u_{t}dt+\sqrt{\epsilon}dW_{t})$
$\displaystyle X_{0}\sim\rho_{0},\quad X_{T}\sim\rho_{T},$

where $\rho_{0}$ ($\rho_{T}$) is a probability distribution with mean $m_{0}$
($m_{T}$) and covariance $\Sigma_{0}$ ($\Sigma_{T}$).

By leveraging the Girsanov theorem, problem (2) can be transferred into a
composite optimization problem which can be solved by the proximal gradient
algorithm. From the results established in [20], each proximal gradient
iteration with step size $\eta$ amounts to solving the following linear
covariance steering problem

$\displaystyle\min_{u}$
$\displaystyle\\!\\!\\!\\!\\!\\!\\!\\!{\mathbb{E}}\left\\{\int_{0}^{T}[\frac{1}{2}\|u_{t}\|^{2}+\frac{1}{2}X_{t}^{T}Q_{k}(t)X_{t}+X_{t}^{T}r_{k}(t)]dt\right\\}$
(3c)
$\displaystyle\\!\\!\\!\\!dX_{t}=\frac{1}{1+\eta}[A_{k}(t)+\eta\hat{A}_{k}(t)]X_{t}dt+\frac{1}{1+\eta}[a_{k}(t)$
$\displaystyle\\!\\!\\!\\!\\!\\!\hskip
22.76228pt+\eta\hat{a}_{k}(t)]dt+B(t)(u_{t}dt+\sqrt{\epsilon}dW_{t})$
$\displaystyle\\!\\!\\!\\!X_{0}\sim\rho_{0},\quad X_{T}\sim\rho_{T},$

where $A_{k}(t),a_{k}(t)$ are the results from last iteration. Also,
$\bar{x}_{k}(t)$ is the mean trajectory at $k$, $\hat{A}_{k}(t)$,
$\hat{a}_{k}(t)$ are linearization matrices along $\bar{x}_{k}(t)$, and
$Q_{k}(t),r_{k}(t)$ are the weighting matrices [20].

This result bridges the gap between the non-linear covariance steering problem
and the linear covariance steering problem. The linear covariance steering
problem in 3 enjoys a closed-form feedback solution in the form [17] [24]

$u_{t}^{\star}=-B(t)^{T}\Pi(t)(X_{t}-x_{t}^{\star})+v_{t}^{\star}.$

where $\Pi(t)$ satisfies a coupled Riccati equations. This closed-form
solution for the proximal gradient update allows us to solve the covariance
steering problem for the control-affine systems with a sublinear rate [20].

##  III Problem formulation

In this work, we consider the motion planning problem under uncertainty.
Uncertainty of the robot results from three sources: robot motion, robot state
estimation, and environment. In our work, we assume the environment is
deterministic and only considers the uncertainty of the robot itself. Robots
are nonlinear control-affine systems whose dynamics and sensor models are

$\displaystyle dX_{t}$
$\displaystyle=f(t,X_{t})dt+B(t)(u_{t}dt+\sqrt{\epsilon}dW_{t})$ (4a)
$\displaystyle z(t)$ $\displaystyle=h(X_{t})+v(t),\quad
v(t)\sim{\mathcal{N}}(0,R(t))$ (4b)

Here, the notations follow the above Section 1 and $z(t)$ is the observation
output with function $h$ and Gaussian noise $v(t)$. The dynamics and sensor
model can be viewed as the belief transition function of the robot. For the
uncertainty that stems from the robot motion and dynamic model, we denote
$\Sigma$ as the covariance of the actual robot states $x$, which follows 4.
$\Sigma$ describes the influence of noise $W_{t}$ on the ideal robot states
which follow the uncorrupted dynamic model.

For the uncertainty that stems from the state estimation, we denote $P(t)$ as
the state error-covariance of the estimation error $\tilde{x}$. It is worth
noting that the covariance of the terminal state is required to be larger than
the state error-covariance $\Sigma_{T}>P(T)$ when using state output as
feedback [21]. Denote estimated robot states as $\hat{x}=x-\tilde{x}$ and its
covariance as $\hat{\Sigma}=\Sigma-P$. We hope to steer the state covariance
$\Sigma$ by controlling the estimated state covariance $\hat{\Sigma}$. We can
state our control problem as for the given waypoints $x_{0},x_{T}$ and their
estimated state covariance $\hat{\Sigma}_{0},\hat{\Sigma}_{T}$, finding a
control sequence $u_{t}$ such that 1) control the mean of the robot states
from $x_{0}$ to $x_{T}$, 2) control the covariance of the robot states from
$\Sigma_{0}$ to terminal covariance $\Sigma_{T}$ via output feedback
$\hat{x}$, 3) generate a collision-free mean trajectory and 4) minimize the
objective function of expected control energy and a state cost

$\min_{u}{\mathbb{E}}\left\\{\int_{0}^{T}[\frac{1}{2}\|u_{t}\|^{2}+V(X_{t})]dt\right\\}$
(5)

##  IV belief space collision-avoiding covariance steering

We hope to build a BRM and solve problem (5) by edge construction and graph
search. Constructing edges in belief space is challenging in terms of
computation [13] since it involves steering state statistics under safety
constraints using partially observable state information. We leverage the
proximal gradient algorithm for problem (2) with a collision-avoiding state
cost [19]

$V(X)=\lVert{\rm hinge}(S(X))\rVert^{2}$ (6)

to achieve the node connection in a BRM. In (6), ${\rm hinge}(\cdot)$
represents the hinge loss function, and $S(\cdot)$ is a differentiable signed
distance function to the obstacles. We showed in [19] that the proposed
proximal gradient algorithm in [20] is effective and efficient in producing
collision-free belief space trajectories.

###  IV-A Collision avoiding covariance steering

(a) Forward propagation.

(b) Collision-avoiding covariance steering.

Figure 2: Procedure of edge construction in PGCS-BRM.

By employing the hinge loss function ${\rm hinge}(\cdot)$, we can succinctly
define our cost function as (6) to penalize risky behaviors and circumvent
obstacle collisions. For each iteration of the proximal gradient covariance
steering algorithm associated with (3), rather than detailing the intricate
mathematics of deriving the weighting matrices $Q_{k}(t)$ and $r_{k}(t)$, it
suffices to say that they are formulated based on the gradient and Hessian of
the cost function. They integrate the effects of system dynamics, control
inputs, and uncertainties.

Upon solving (3) within the paradigm of linear covariance steering, the
optimal control policy is formulated as:

$u_{t}^{\star}=K_{k}(t)X_{t}+d_{k}(t)$

This control policy, when injected into the closed-loop process, offers the
subsequent update dynamics

$\displaystyle dX_{t}$
$\displaystyle=\frac{1}{1+\eta}[A_{k}(t)+\eta\hat{A}_{k}(t)]X_{t}dt+$
$\displaystyle\frac{1}{1+\eta}[a_{k}(t)+\eta\hat{a}_{k}(t)]dt+B(t)(u_{t}^{\star}dt+\sqrt{\epsilon}dW_{t}),$

From this, we deduce the iterative update rules:

$\displaystyle A_{k+1}(t)$ $\displaystyle=$
$\displaystyle\frac{1}{1+\eta}[A_{k}(t)+\eta\hat{A}_{k}(t)]+B(t)K_{k}(t)$ (7a)
$\displaystyle a_{k+1}(t)$ $\displaystyle=$
$\displaystyle\frac{1}{1+\eta}[a_{k}(t)+\eta\hat{a}_{k}(t)]+B(t)d_{k}(t).$
(7b)

To synchronize the evolution of $\bar{x}_{k}(t)$ and $\Sigma_{k}(t)$ at each
iteration $k$, one can employ the aforementioned update rule (7), ensuring an
efficient iterative process.

In the following discourse, we showcase the state connection algorithm (as
presented in Algorithm 1). Given the constructs $A_{k}(t)$ and $a_{k}(t)$ at
the $k^{th}$ iteration, the algorithm commences by propagating the mean
trajectory $\bar{x}_{k}(t)$ and subsequently estimating the state covariance
along the path, as represented in Figure 2(a). Leveraging the updated nominal
trajectory, the algorithm exploits the control-observation separation
principle to compute both the Kalman gain and the state error-covariance
$P_{k}(t)$.

###  IV-B Steering state statistics using partially-observed output

To initialize the state prediction for each sampled state, we set
$\hat{x}_{k}(t_{0})={\mathbb{E}}[x_{k}(t_{0})]$ and $P_{k}(t_{0})$ is sampled
from a proper space. At each iteration, the continuous-time EKF propagates
state error covariance $P_{k}(t)$ based on the linearized system dynamics
model $A_{k}(t),a_{k}(t)$ and updates the near-optimal Kalman gain. These
steps are coupled in continuous time and governed by the following Riccati
equations

$\begin{split}\dot{P}(t)&=F(t)P(t)+P(t)F(t)^{T}+B(t)QB^{T}(t)\\\
&-P(t)H(t)^{T}R(t)^{-1}H(t)P(t)\end{split}$ (8)

where noise covariance $Q=\epsilon\mathbb{I}_{n}$ and $F(t)$ and $H(t)$
represent the Jacobian matrices of the system dynamics function and
measurement function, respectively, as

$\displaystyle F(t)=\frac{\partial f}{\partial
x}\bigg{|}_{\hat{x}(t),u(t)}\quad H(t)=\frac{\partial h}{\partial
x}\bigg{|}_{\hat{x}(t),u(t)}$

The target uncertainty of the robot state is known from the sampling stage.
With the uncertainty from sensing calculated, we are able to compute the
terminal error covariance of the Kalman filter state

$\hat{\Sigma}_{k}(t)=\Sigma(t)-P_{k}(t)$

and use it as output state feedback to control the covariance of the path in
the next iteration.

$\hat{A}_{k}(t),\hat{a}_{k}(t)$ represent the Gaussian Markov process
approximation of the trajectory at the current iteration, which can be
calculated by linearizing the system with respect to the nominal trajectory.
$\hat{A}_{k}(t),\hat{a}_{k}(t)$ are used in the construction of cost matrices
$Q_{k}(t)$ and $r_{k}(t)$. Solving the linear covariance steering problem in
3, the optimal control policy $K_{k}(t),d_{k}(t)$ is calculated and
$A_{k+1}(t),a_{k+1}(t)$ are updated following (7), as shown in Figure 2(b).

Algorithm 1 PGCS State Connection Algorithm

1: Start state and covariance $m_{0},\Sigma_{0},P_{0}$

2: Target state and covariance $m_{T},\Sigma_{T}$

3: Initialize $A_{0},a_{0}$

4: for iteration $k=0,1,2,\ldots$ do

5: $\bar{x}_{k}(t),\;\Sigma_{k}(t)\leftarrow{\rm
UpdateTrajectory}(A_{k},a_{k})$;

6: $P_{k}(t)\leftarrow{\rm KalmanGain}(\bar{x}_{k}(t),P_{k}(0))$(8);

7: Update $\hat{\Sigma}_{k}=\Sigma(t)-P_{k}(t)$

8: $\hat{A}_{k}(t),\hat{a}_{k}(t)\leftarrow{\rm
Linearization}(\bar{x}_{k}(t))$;

9: $K_{k}(t),d_{k}(t)\leftarrow{\rm
PGCS}(m_{0},\hat{\Sigma}_{0},m_{T},\hat{\Sigma}_{T})\eqref{eq:iterlinear}$;

10: Update $A_{k}(t)$ using (7a);

11: Update $a_{k}(t)$ using (7b);

12: end for

###  IV-C Entropy regularized edge cost

For every trajectory between states, the cost is calculated using the sum of
control energy, collision cost, and entropy cost. Entropy cost is defined as

$E(\Sigma_{k}^{i}(t))=-\int_{0}^{T}\log(|\Sigma_{k}^{i}(t)|)dt.$

A smaller entropy cost indicates the trajectory allows higher tolerance in the
robot uncertainty and requires less sensing and control effort to control the
uncertainty. Leveraging the duality between stochastic control and variational
inference, the objectives for the linearized system in each step of our edge
construction problem formulation (2) is equivalent to an entropy-regularized
motion planning [25] [19]

${\max}\;{\mathbb{E}}_{q}[-\log J]+H(q),$

where $J$ denotes a composite cost involving a prior process-induced cost and
the collision cost

$J={\rm J_{prior}}(X)+V(X),$

and $q$ is the joint Gaussian distribution induced by the stochastic process
(4) after linearization. In other words, optimizing the problem (2) is
equivalently optimizing an entropy-regularized motion planning objective for
the path distribution. We found that a trajectory distribution with a smaller
entropy cost is safer than one with a higher cost in a probability sense. In
the same spirit, we define the total cost for the $\textit{i}^{\rm th}$
trajectory $z_{k}^{i}$ is the weighted sum of the control energy along the
mean trajectory and the entropy cost

$c_{ij}=\frac{1}{2}\int_{0}^{T}||u^{*}(t)||^{2}+\lVert{\rm
hinge}(S(X(t))\rVert^{2}dt+\alpha E(\Sigma_{k}^{i}(t)).$ (9)

By setting $\alpha$ differently, the planner can return different optimal
paths with lower control effort or lower risks.

###  IV-D Uncertainty-aware State Sampler

We utilize BRM to divide the original problem into several easier state
connection subproblems. To leverage the PGCS state connection Algorithm 1, it
is important to provide a meaningful covariance to represent the uncertainty
for each sample state. Define the distance $d_{obs}$ between an obstacle
region ${\mathcal{X}}_{obs}$ and sampled state $x_{s}$ as the minimum distance
from $x$ to any point $p_{obs}\in{\mathcal{X}}_{obs}$, and the corresponding
point in ${\mathcal{X}}_{obs}$ is the closet point $p_{obs}^{c}$ to $x$. For
$n$ dimensional spatial state space, we hope to find $n$ such points
$p_{obs}^{c}$ and form a covariance ellipsoid with the center point $x_{s}$.
The covariance for spatial states can be calculated from the parameter for
this ellipsoid and a given confidence level $P_{conf}$, such that the actual
state $x$ distribution satisfies

$P((x-x_{s})<d_{obs})>P_{conf}.$ (10)

We assume a constant velocity and covariance at each sampled state. The
direction of the velocity can align with the direction of the current node and
adjacent node.

###  IV-E Main algorithm

The implementation of the PGCS-BRM algorithm is summarized in Algorithm 2. To
calculate the hinge loss of obstacles, a signed distance field is used which,
together with the start, and target states, are initialized by the user. Then
the uncertainty-aware sampler samples a certain number of states in the state
space and their covariance matrices determined by the environment. The main
loop starts in line 11, where each feasible sampled state is looped through
and whose nearest neighbors are found. The number of neighbors found is
determined by a preset neighbor distance and the total number of sampled
states. Next, we connect the current state with all its feasible neighbors
using the state connection Algorithm 1. For each state pair, the nonlinear
covariance steering connection algorithm is run twice to generate two
trajectories from two different directions. To ensure the connection algorithm
returns a feasible solution, we need the estimated robot state error-
covariance $\hat{\Sigma}>0$.

Algorithm 2 PGCS-BRM

1: Initialize map $M$;

2: Initialize graph $G\leftarrow\emptyset$;

3: Start state and covariance $S\leftarrow{x_{init},\Sigma_{init}}$;

4: Target state and covariance $T\leftarrow{x_{target},\Sigma_{target}}$;

5: for $i=1,2,\ldots$ do

6: $b_{i}=(x_{i},\Sigma_{i})\leftarrow{\rm UncertaintyAwareSampler(M)}$;

7: if $\rm Feasible(b_{i})$ then

8: $G\leftarrow b_{i}$;

9: end if

10: end for

11: for $i=1,2,\ldots$ do

12: $\\{b_{j}\\}_{i}\leftarrow\rm NearestNeighbors(b_{i},M,G)$;

13: for $j=1,2,\ldots$ do

14: $\bar{x}_{ij}(t),\;\Sigma_{ij}(t)\leftarrow\rm
StateConnection(b_{i},b_{j})$ 1;

15: if $\rm CollisionFree(\bar{x}_{ij})$ then

16: $c_{ij}\leftarrow\rm Cost(\bar{x}_{ij}(t),\;\Sigma_{ij}(t))$;

17: $G\leftarrow(\bar{x}_{ij}(t),\;\Sigma_{ij}(t),c_{ij})$

18: end if

19: end for

20: end for

21: $\bar{x}_{ST}(t),\;\Sigma_{ST}(t)\leftarrow\rm SearchPath(S,T,G)$

22: return Belief Roadmap G, Path ($\bar{x}_{ST}(t),\;\Sigma_{ST}(t))$

(a) 10 nodes and 24 edges

(b) 20 nodes and 62 edges

(c) 30 nodes and 92 edges

Figure 3: Belief Roadmap planning for a 2D environment. Red dashed ellipsoids represents the estimated state covariances $P(t)$ propagated using (8), and light blue ellipsoids are the state covariances $\Sigma(t)$. Notice that $P_{T}$ is expected to be less than $\Sigma_{T}$ at every end of an edge.  Nodes | 25 | 50 | 75 | 100
---|---|---|---|---
Edges | 106 | 104 | 302 | 310 | 476 | 588 | 940 | 974
Time/s | 45.41 | 23.77 | 75.74 | 91.82 | 111.25 | 282.64 | 341.83 | 227.62
TABLE I: Time consumption in different graph scales for a robot in 3D
environment. We conduct two different experiments for the same number of nodes
to show that when the number of nodes are relatively small, the variance in
graph construction time is large.

##  V Experiment

We conducted several numerical experiments to validate the proposed method.
All experiments are conducted on a machine with CPU of i7-12700KF and 16GiB
memory.

###  V-A Effect on Changing $\alpha$

(a) $\alpha=0.2$ Penalize control energy cost more than entropy cost. PGCS-BRM
returns a shorter path but more risky path.

(b) $\alpha=0.6$ Penalize entropy cost more than control energy cost. PGCS-BRM
returns a longer but less risky path.

(c) Control energy cost and entropy cost with different $\alpha$. With an
increasing $\alpha$, the entropy wins over the control energy costs.

Figure 4: Different paths are chosen by different weights on entropy.

To demonstrate how $\alpha$ impacts the returned path and the ability of PGCS-
BRM to handle non-linear systems, we conduct experiments on 2-D planning for a
risky area. We consider the same nonlinear dynamical system used in [26]

$\displaystyle dx_{1}$ $\displaystyle=x_{2}dt,$ (11a) $\displaystyle dx_{2}$
$\displaystyle=(u-c_{d}\lVert x_{2}\rVert x_{2})dt+\sqrt{\epsilon}dW_{t}.$
(11b)

1,000 states are sampled and more than 10,000 trajectories are generated for
state connection. In the search phase, $A^{*}$, a best-first search algorithm,
is deployed to find a path to the given goal state with minimum total cost. In
Figure 4(a) and 4(b), we show that by changing the weighting factor $\alpha$,
PGCS-BRM is able to build belief graphs and find a path with less control cost
or less entropy cost.

###  V-B Evaluation of Running Time

We compare the proposed method with the CS-BRM method in [16] using a linear
double integrator dynamics

$dX_{t}=(\begin{bmatrix}0&I\\\ 0&0\end{bmatrix}X_{t}+\begin{bmatrix}0\\\
I\end{bmatrix}U)dt+\sqrt{\epsilon}dW_{t}.$ (12)

We use a map of 5 rectangular obstacles to compare these two methods. In each
experiment with a different number of nodes, the same sampling setup is
deployed and we used the same start and goal states for the graph building.
For PGCS-BRM, each edge building is set to execute 50 iterations of the
proximal gradient with step size $\eta=0.001$ and discretized into 50
timesteps. We recorded the times for constructing the belief space graph after
node sampling and repeated each experiment three times. Both algorithms are
able to build a belief roadmap, however, due to the high computation cost in
performing Monte-Carlo collision checking and solving optimization problems,
CS-BRM requires higher computation time. On the other hand, PGCS-BRM is able
to penalize collision in the cost function and directly solve the nonlinear
covariance steering with a sublinear rate. PGCS-BRM is around 100 times faster
and only requires 3.38s to build a roadmap with 30 nodes, compared to CS-BRM
which needs 782s on average.

Figure 5: Running time comparison of graph construction between PGCS-BRM and
CS-BRM [16]. The proposed method is more than 100 times more efficient in
graph building.

###  V-C 3D Experiment

Figure 6: Belief space graph and an optimal path in 3D space. The graph
consists of 100 sampled states. The red funnel shows the optimal path found by
the PGCS-BRM.

We conduct experiments in an obstacle-clustered 3-D environment for a 3-D
point robot model (12) to demonstrate the generalizability of the proposed
methods in higher-dimension space. The visualization result is shown in Figure
6. We record the average time for building a PGCS-BRM with 25, 50, 75, and 100
nodes and different numbers of edges. On average, PGCS takes 0.23-0.48s to
build an edge in 3D space under 100 max iterations. It is worth noticing that
the actual run times vary because different states are sampled in each
experiment.

##  VI Conclusion and future work

This work presents an efficient belief space roadmap (PGCS-BRM) for planning
under uncertainty. The proposed method models the belief as state
distributions and leverages nonlinear covariance steering with safety
constraints for edge construction. We also include an entropy cost in the edge
costs to account for robustness under uncertainty. Experiments show that the
proposed method effectively constructs BRMs in different dimensions and
outperforms state-of-the-art sampling-based belief space planning methods.
Though PGCS-BRM shows promising results in building a belief roadmap with
controlled covariance, the generated trajectory is still rough and not smooth.
This is mainly the result of the lack of reasonable velocity sampling. Unlike
spatial states, there are no explicit constraints on velocity in the sampling
stage, and poorly selected velocity might result in a non-smooth trajectory.
Developing a better velocity sampling algorithm and smoothing algorithm can
greatly enhance the performance of the current algorithms. Another future
direction worth exploring is to deploy such an algorithm in time-varying
environments. The ability to control the uncertainty in planning and quickly
replan the route is essential in such scenarios.

## References

  * [1] S. M. LaValle, _Planning algorithms_. Cambridge university press, 2006.
  * [2] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “Chomp: Gradient optimization techniques for efficient motion planning,” in _2009 IEEE International Conference on Robotics and Automation_. IEEE, 2009, pp. 489–494.
  * [3] J. Schulman, Y. Duan, J. Ho, A. Lee, I. Awwal, H. Bradlow, J. Pan, S. Patil, K. Goldberg, and P. Abbeel, “Motion planning with sequential convex optimization and convex collision checking,” _The International Journal of Robotics Research_ , vol. 33, no. 9, pp. 1251–1270, 2014.
  * [4] F. L. Lewis, D. Vrabie, and V. L. Syrmos, _Optimal control_. John Wiley & Sons, 2012.
  * [5] H. Yu, J. Moyalan, U. Vaidya, and Y. Chen, “Data-driven optimal control of nonlinear dynamics under safety constraints,” _IEEE Control Systems Letters_ , vol. 6, pp. 2240–2245, 2022.
  * [6] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” _IEEE transactions on Robotics and Automation_ , vol. 12, no. 4, pp. 566–580, 1996.
  * [7] S. LaValle, “Rapidly-exploring random trees: A new tool for path planning,” _Research Report 9811_ , 1998.
  * [8] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” _The International Journal of Robotics Research_ , vol. 30, no. 7, pp. 846–894, 2011.
  * [9] R. Platt Jr, R. Tedrake, L. Kaelbling, and T. Lozano-Perez, “Belief space planning assuming maximum likelihood observations,” 2010.
  * [10] M. P. Vitus and C. J. Tomlin, “Closed-loop belief space planning for linear, gaussian systems,” in _2011 IEEE International Conference on Robotics and Automation_. IEEE, 2011, pp. 2152–2159.
  * [11] O. Macindoe, L. P. Kaelbling, and T. Lozano-Pérez, “Pomcop: Belief space planning for sidekicks in cooperative games,” in _Proceedings of the AAAI Conference on Artificial Intelligence and Interactive Digital Entertainment_ , vol. 8, no. 1, 2012, pp. 38–43.
  * [12] A. R. Pedram, R. Funada, and T. Tanaka, “Gaussian belief space path planning for minimum sensing navigation,” _IEEE Transactions on Robotics_ , 2022.
  * [13] S. Prentice and N. Roy, “The belief roadmap: Efficient planning in belief space by factoring the covariance,” _The International Journal of Robotics Research_ , vol. 28, no. 11-12, pp. 1448–1465, 2009.
  * [14] T. Shan and B. Englot, “Belief roadmap search: Advances in optimal and efficient planning under uncertainty,” in _2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2017, pp. 5318–5325.
  * [15] C. Wang, J. Cheng, J. Wang, X. Li, and M. Q.-H. Meng, “Efficient object search with belief road map using mobile robot,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 4, pp. 3081–3088, 2018.
  * [16] D. Zheng, J. Ridderhof, P. Tsiotras, and A.-a. Agha-mohammadi, “Belief space planning: A covariance steering approach.”
  * [17] Y. Chen., T. Georgiou, and M. Pavon, “Optimal steering of a linear stochastic system to a final probability distribution, Part I,” _IEEE Trans. on Automatic Control_ , vol. 61, no. 5, pp. 1158–1169, 2016.
  * [18] Y. Chen, T. T. Georgiou, and M. Pavon, “Optimal steering of a linear stochastic system to a final probability distribution, Part II,” _IEEE Trans. on Automatic Control_ , vol. 61, no. 5, pp. 1170–1180, 2016.
  * [19] H. Yu and Y. Chen, “Stochastic motion planning as gaussian variational inference: Theory and algorithms,” _ArXiv_ , vol. abs/2308.14985, 2023.
  * [20] H. Yu, Z. Chen, and Y. Chen, “Covariance steering for nonlinear control-affine systems,” 2023.
  * [21] Y. Chen, T. T. Georgiou, and M. Pavon, “Steering state statistics with output feedback,” _2015 54th IEEE Conference on Decision and Control (CDC)_ , pp. 6502–6507, 2015.
  * [22] B. Luders, S. Karaman, and J. P. How, “Robust sampling-based motion planning with asymptotic optimality guarantees,” 2013.
  * [23] S. N. Cohen and R. J. Elliott, _Stochastic calculus and applications_. Springer, 2015, vol. 2.
  * [24] Y. Chen, T. T. Georgiou, and M. Pavon, “Optimal steering of a linear stochastic system to a final probability distribution—part iii,” _IEEE Transactions on Automatic Control_ , vol. 63, no. 9, pp. 3112–3118, 2018.
  * [25] H. Yu and Y. Chen, “A gaussian variational inference approach to motion planning,” _IEEE Robotics and Automation Letters_ , vol. 8, no. 5, pp. 2518–2525, 2023.
  * [26] J. Ridderhof, K. Okamoto, and P. Tsiotras, “Nonlinear uncertainty control with iterative covariance steering,” in _2019 IEEE 58th Conference on Decision and Control (CDC)_. IEEE, 2019, pp. 3484–3490.


# Macroscopic polarization from nonlinear gradient couplings

Massimiliano Stengel  Institut de Ciència de Materials de Barcelona (ICMAB-
CSIC), Campus UAB, 08193 Bellaterra, Spain  ICREA - Institució Catalana de
Recerca i Estudis Avançats, 08010 Barcelona, Spain<EMAIL_ADDRESS>
###### Abstract

We show that a lattice mode of arbitrary symmetry induces a well-defined
macroscopic polarization at first order in the momentum and second order in
the amplitude. We identify a symmetric flexoelectric-like contribution, which
is sensitive to both the electrical and mechanical boundary conditions, and an
antisymmetric Dzialoshinskii-Moriya-like term, which is unaffected by either.
We develop the first-principles methodology to compute the relevant coupling
tensors in an arbitrary crystal, which we illustrate with the example of the
antiferrodistortive order parameter in SrTiO3.

###### pacs:

71.15.-m, 77.65.-j, 63.20.dk

The interaction between structural, polar and magnetic degrees of freedom in
multiferroics has long been identified as a promising source of advanced
material functionalities. The recent focus on inhomogeneous structures such as
skyrmions Fert  _et al._ (2017), domain walls Meier (2015) and vortices Behera
_et al._ (2022); Naumov  _et al._ (2004) has renewed the interest in the so-
called _Lifshitz invariants_ (LIs), i.e., coupling terms that depend on the
first gradient of one order parameter component. LIs play a key role in the
stabilization of spatially modulated phases Eliseev  _et al._ (2013);
Shapovalov and Stengel (2022); Pöttker and Salje (2016) and often determine
their emerging physical properties. A paradigmatic example is the macroscopic
Dzyaloshinskii-Moryia Dzyaloshinsky (1958); Moriya (1960) (DM) interaction,

$E_{\rm DM}=\zeta{\bf
P}\cdot\left[\bm{\phi}(\bm{\nabla}\cdot\bm{\phi})-(\bm{\phi}\cdot\bm{\nabla})\bm{\phi}\right].$
(1)

where $P$ is the macroscopic polarization, and $\bm{\phi}$ may correspond to
the magnetic Mostovoy (2006) or antiferromagnetic Sparavigna  _et al._ (1994)
degrees of freedom. [Realizations of Eq. (1) in other contexts, e.g., in
liquid crystals Sparavigna (2009) also exist.] The importance of Eq. (1) lies
in its topological character, Xiao  _et al._ (2009) and the rich phenomenology
it can lead to, ranging from the switchable $P$ in ferroelectric multiferroics
Mostovoy (2006) to the stabilization of incommensurate spin orders in broken-
symmetry environments.

Another category of LIs involves flexoelectric-like terms, whereby $P$ is
coupled to the gradient of a symmetric second-rank tensor field, ${\bf s}$,

$E_{\rm flexo}=\frac{K_{\alpha\beta\gamma\lambda}}{2}\left[\frac{\partial
P_{\alpha}}{\partial r_{\beta}}s_{\gamma\lambda}-P_{\alpha}\frac{\partial
s_{\gamma\lambda}}{\partial r_{\beta}}\right].$ (2)

In the original form of Eq. (2), ${\bf s}$ corresponds to the elastic strain,
Zubko  _et al._ (2013); Stengel and Vanderbilt (2016) and the coupling is
harmonic in the order parameter amplitudes. More recently, Eq. (2) was
generalized Eliseev  _et al._ (2013) to a much broader class of _nonlinear_
couplings, where $s_{\gamma\lambda}=\phi_{\gamma}\phi_{\lambda}$ is the dyadic
product of two (pseudo)vectors [e.g., the ferroelectric polarization
Korzhenevskii (1981) or the antiferrodistortive (AFD) tilts Eliseev  _et al._
(2013) in perovskite-structure oxides]. Regardless of symmetry or the physical
nature of ${\bf s}$, the coupling tensor $K_{\alpha\beta\gamma\lambda}$ is a
universal property of all crystals, hence its fundamental and practical
interest.

Research efforts are currently directed at exploring practical realizations of
these ideas in a variety of materials and order parameter types. Pyatakov  _et
al._ (2015) It would be highly desirable, for instance, to find nonmagnetic
analogues of Eq. (1), in contexts where the strength of the coupling constant
$\zeta$ is not limited by weak relativistic effects. Zhao  _et al._ (2021) The
so-called ferroelectric DM interaction, Zhao  _et al._ (2021); Chen  _et al._
(2022) which involves the polarization itself as the primary order parameter,
appears as an especially promising candidate. An antiferrodistortive (AFD)
realization of Eq. (1) was also hinted at in Ref. Schiaffino and Stengel
(2017), although the relationship between the “rotopolar” coupling described
therein and Eq. (1) is not immediately obvious. Meanwhile, additional
_indirect_ contributions to $P$ have also been pointed out, either involving
the strain (“flexo-roto” Morozovska  _et al._ (2012) effect in the case of
tilts) or other nonpolar degrees of freedom (e.g., the antiferroelectric
$R$-mode of Refs. Schiaffino and Stengel, 2017 and Casals  _et al._ , 2018).
The coexistence of several effects, whose mutual relationship is sometimes
paradoxical, Eliseev  _et al._ (2013) complicates the understanding of flexo-
and DM-type couplings, calling for a fundamental treatment.

The main priority for microscopic theory lies in clarifying the physical
mechanisms that generate a polarization in inhomogeneous ferroic structures,
either directly via Eq. (2) and (1), or via the aforementioned indirect
routes. In particular, it is of central importance to know whether these
effects are well-defined bulk properties of the crystal, or whether they are
plagued by ambiguities (e.g., due to boundary issues) as in the case of
flexoelectricity. Stengel (2013); Royo and Stengel (2022a) At the same time,
it would be desirable to establish an efficient and accurate methodological
framework to predict the value of the relevant coupling coefficients in real
materials, e.g., via first-principles techniques. Selected components of the
rotopolar tensor in SrTiO3 have been calculated already; Schiaffino and
Stengel (2017) however, conceptual and technical difficulties with the
treatment of spatial dispersion effects at nonlinear order have so far
thwarted the development of a full-fledged theory.

Here we provide a unified first-principles theory of both flexo- and DM-type
couplings by expressing them as, respectively, the symmetric and antisymmetric
parts of the same fourth-rank tensor. Based on this result, we argue that an
arbitrary inhomogeneous field $\bm{\phi}$ always couples to polar degrees of
freedom via both mechanisms, with the special case where ${\bf P}$ and
$\bm{\phi}$ are _the same mode_ as an interesting exception. We further show
that the DM-like coefficient $\zeta$ is a well-defined physical property of
the crystal, while the flexo-type tensor, $K_{\alpha\beta\gamma\lambda}$, is
not. The reason lies in the macroscopic elastic and electrostatic
interactions, which contribute to the latter but not to the former. Similarly
to the flexoelectric case, these long-ranged (“nonanalytic”, in the language
of perturbation theory) terms lead to ambiguities in the definition of the
reference electrostatic potential and the center of mass of the cell, Stengel
(2013); Royo and Stengel (2022a) which must be adequately treated to guarantee
the internal consistency of the theory. Diéguez and Stengel (2022) From a
practical point of view we recast the nonlinear interaction between modulated
order parameters as well-defined third derivatives of the total energy. The
long-wavelength expansion Royo and Stengel (2019, 2022a) of the latter, which
we treat in the framework of density-functional perturbation theory Baroni
_et al._ (2001); Gonze and Lee (1997) (DFPT), readily yields the coupling
constants of Eq. (2) and Eq. (1) at first order in the momentum. Calculations
are performed with minimal effort via the recently developed Royo and Stengel
(2019, 2022a) long-wave module of Abinit, Gonze  _et al._ (2009); Romero  _et
al._ (2020) in combination with a post-processing tool that we have
implemented and tested as part of this work. As a numerical demonstration, we
focus on the leading terms involving the AFD order parameter in SrTiO3.

Following Ref. Schiaffino and Stengel, 2017, we base our derivations on
_unsymmetrized_ inhomogeneous couplings of the type

$E_{\rm
uns}=-W_{\alpha\beta\gamma\lambda}p_{\alpha}\frac{\partial\phi_{\gamma}}{\partial
r_{\beta}}\phi_{\lambda},$ (3)

where $E_{\rm uns}$ is an energy (per primitive 5-atom cell), $\bm{\phi}({\bf
r})$ is the main order parameter, and the field ${\bf p}({\bf r})$ corresponds
to some polar lattice mode of the crystal. Eq. (3) is the most general
trilinear coupling between $\bm{\phi}({\bf r})$ and ${\bf p}({\bf r})$
occurring at first order in the gradient expansion; any other expression can
be written as a linear combination thereof. To verify this point explicitly in
the case of Eq. (2) and (1), it suffices to separate the symmetric and
antisymmetric contributions with respect to the last two indices,
$W_{\alpha\beta\gamma\lambda}=W_{\alpha\beta(\gamma\lambda)}+W_{\alpha\beta[\gamma\lambda]}$.
Within the assumed cubic symmetry, elementary calculus leads then to

$W_{\alpha\beta\gamma\lambda}=\underbrace{2K_{\alpha\beta\gamma\lambda}}_{W_{\alpha\beta(\gamma\lambda)}}+\underbrace{\zeta(\delta_{\alpha\gamma}\delta_{\beta\lambda}-\delta_{\alpha\lambda}\delta_{\beta\gamma})}_{W_{\alpha\beta[\gamma\lambda]}},$
(4)

which establishes the formal link between Eq. (3) and Eqs. (2–1). In a cubic
crystal, $\mathbf{K}$ has three independent entries ($K_{11}=K_{1111}$,
$K_{12}=K_{1122}$ and $K_{44}=K_{1212}$), similarly to the flexoelectric
tensor. These, in combination with the DM-type scalar $\zeta$, account for the
four components of the tensor ${\bf W}$; the latter coincides with the
“rotopolar” coupling of Ref. Schiaffino and Stengel, 2017 in the AFD case.

The special case where $\bm{\phi}={\bf p}$, of relevance to the recently
proposed “electric DM interaction” Zhao  _et al._ (2021); Chen  _et al._
(2022), deserves a separate discussion. Eq. (3) reduces then to Eq. (2) via a
permutation of indices and integration by parts. This means that the DM-type
coupling of Eq. (1) is redundant in this case: the flexo-type expression of
Eq. (2) describes the trilinear self-interaction of a polar vector field in
full generality. Assuming cubic symmetry of the undistorted crystal, Eq. (2)
adopts the following compact form,

$E_{\rm p}=Kp^{2}\bm{\nabla}\cdot{\bf p}.$ (5)

where $K=K_{12}-K_{44}$ is a single material coefficient, and $p^{2}={\bf
p}\cdot{\bf p}$. The remaining independent components of the
$\mathbf{K}$-tensor ($K_{11}$ and $K_{12}+K_{44}$) are irrelevant at the bulk
level as they do not contribute to the forces nor to the energy. Crucially,
Eq. (5) depends directly on the longitudinal components of ${\bf p}$, which
are typically suppressed by depolarizing effects; for this reason, henceforth
we shall restrict to our attention to cases where the primary order parameter
$\bm{\phi}$ is nonpolar.

To work our way towards a first-principles expression, we need to specify the
microscopic nature of the field variables entering Eqs. (3). Following Ref.
Diéguez and Stengel, 2022, we use a perturbative approach in terms of
monochromatic lattice distortions of the type

$u^{l}_{\kappa\alpha}=u^{\bf q}_{\kappa\alpha}e^{i{\bf q}\cdot{\bf
R}^{(0)}_{l\kappa}}.$ (6)

Here $\kappa$ and $l$ are sublattice and cell indices, respectively;
$u^{l}_{\kappa\alpha}$ indicates the atomic displacement along the Cartesian
direction $\alpha$; ${\bf R}^{(0)}_{l\kappa}$ stands for the unperturbed
atomic locations in the high-symmetry reference structure; ${\bf q}$ is the
momentum. The microscopic representation of the continuum fields is then
defined as

$u^{{\bf q}}_{\kappa\alpha}=\langle\kappa\alpha|p_{\beta}\rangle
p_{\beta}^{\bf q}+\langle\kappa\alpha|\phi_{\beta}\rangle\phi_{\beta}^{\bf
q},$ (7)

where the symbol $\langle\kappa\alpha|v\rangle$ corresponds sm  to the
eigendisplacements of a given phonon mode $|v\rangle$, and ${\bf v}^{\bf q}$
refers to the Fourier representation of the field ${\bf v}({\bf r})$. (Bra and
kets refer to real vectors in the $3N$-dimensional space of the atomic
displacements, where $N$ is the number of basis atoms in the cell. sm )

Based on the above, we can express Eq. (3) in reciprocal space as a three-
phonon vertex,

$E_{\rm
uns}=-iq_{\beta}W_{\alpha\beta\gamma\lambda}p_{\alpha}^{{\bf-q}{\bf-q}^{\prime}}\phi_{\gamma}^{\bf
q}\phi_{\lambda}^{\bf q^{\prime}}.$ (8)

In the ${\bf q,q^{\prime}}\rightarrow 0$ limit, we can then write the tensor
${\bf W}$ in terms of the third derivatives of the total energy $E$,

$\frac{\partial^{3}E}{\partial p_{\alpha}^{\bf-q}\partial\phi_{\gamma}^{\bf
q}\partial\phi_{\lambda}^{0}}=\langle p_{\alpha}|\frac{\partial\Phi^{\bf
q}}{\partial\phi^{0}_{\lambda}}|\phi_{\gamma}\rangle.$ (9)

or equivalently as the first derivative of the force-constant matrix,
$\Phi^{\bf q}$, with respect to the homogeneous perturbation
$\phi^{0}_{\lambda}$. By recalling Stengel (2013); Royo and Stengel (2022a)
the long-wave expansion of $\Phi^{\bf q}$, $\Phi^{\bf
q}\simeq\Phi^{(0)}-iq_{\beta}\Phi^{(1,\beta)}$, we arrive then at a closed
expression for the ${\bf W}$-tensor components as projection on the polar mode
$\langle p_{\alpha}|$ of the _force-response tensor_
$|w_{\beta\gamma\lambda}\rangle$,

$W_{\alpha\beta\gamma\lambda}=\langle
p_{\alpha}|w_{\beta\gamma\lambda}\rangle,\quad|w_{\beta\gamma\lambda}\rangle=\frac{\partial\Phi^{(1,\beta)}}{\partial\phi^{0}_{\lambda}}|\phi_{\gamma}\rangle.$
(10)

Thanks to cubic symmetry, Eq. (10) allows one to capture all the independent
components of ${\bf W}$ at once as part of a single linear-response
calculation; the flexo-like and DM-like contributions are then extracted via
Eq. (4). (Whenever appropriate, the mode index will be indicated with a
superscript, either in the form $W^{(i)}_{\alpha\beta\gamma\lambda}$ or
$W^{[i]}_{\alpha\beta\gamma\lambda}$ for the normal-mode or symmetry-adapted
Hong and Vanderbilt (2013) sublattice representation sm  of the tensors,
respectively.)

Our next goal is to understand whether ${\bf W}$ (or its decomposition into
$\mathbf{K}$ and $\zeta$) is a well-defined physical property of the crystal.
A first concern lies in the definition of the force-response tensor,
$|w_{\beta\gamma\delta}\rangle$, via a long-wave expansion of $\Phi^{\bf q}$.
To perform the latter operation, short-circuit electrical boundary conditions
need to be imposed, Stengel (2013) which implies setting to zero the
macroscopic electrostatic potential, $V^{\rm mac}$, in the calculations.
$V^{\rm mac}$ is, however, ill-defined in a periodic crystal, Resta  _et al._
(1990) which leads to a _reference potential ambiguity_ in the definition of
the flexo-type coefficients. Stengel (2013, 2016); Royo and Stengel (2022b) As
this issue only affects the longitudinal components of the polarization
response, which are expected to be small, we won’t delve into it further here;
in any case, the DM-type constant $\zeta$ is manifestly unaffected by
electrostatics, due to the transverse nature of Eq. (1).

A second issue concerns the translational freedom of the polar mode
eigendisplacement vector, which is only defined modulo a rigid shift of the
cell. Diéguez and Stengel (2022) Based on the criteria of Ref. Diéguez and
Stengel, 2022, a necessary condition for a material property to be “well
defined” is its invariance with respect to the following transformation,

$|p_{\alpha}^{\prime}\rangle=|p_{\alpha}\rangle+\lambda,$ (11)

where $\lambda$ an arbitrary constant. To understand the impact of Eq. (11) on
${\bf W}$, recall that the acoustic eigendisplacement vector reduces to a
translation Stengel (2016); Diéguez and Stengel (2022) regardless of the
microscopics, $\langle\kappa\alpha|u_{\beta}\rangle=\delta_{\alpha\beta}$.
This implies that

${W}^{\prime}_{\alpha\beta\gamma\delta}={W}_{\alpha\beta\gamma\delta}+\lambda\langle
u_{\alpha}|w_{\beta\gamma\delta}\rangle,$ (12)

where $\langle u_{\alpha}|w_{\beta\gamma\delta}\rangle$ is a net elastic force
on the cell as a whole that arises in a locally inhomogeneous order parameter
$\bm{\phi}$. That such a force does not vanish is a direct consequence of the
_strain coupling_

$E_{\rm
sc}=-R_{\alpha\beta\gamma\delta}\varepsilon_{\alpha\beta}\phi_{\gamma}\phi_{\delta},$
(13)

which is always allowed by symmetry. Since the force is the divergence of the
stress, a trivial integration by parts leads to the following _sum rule_ ,

$-\frac{1}{2}\sum_{\kappa}\langle\kappa\alpha|w_{\beta\gamma\lambda}\rangle=R_{\alpha\beta\gamma\lambda},$
(14)

relating the sublattice sum of the force-response tensor
$|w_{\beta\gamma\lambda}\rangle$ to the _strain coupling_ tensor, ${\bf R}$.
After observing that $R_{\alpha\beta\gamma\lambda}$ is symmetric both in
$\alpha\beta$ and $\gamma\lambda$, we arrive at the following transformation
law for the coupling coefficients,

$K_{\alpha\beta\gamma\delta}^{\prime}=K_{\alpha\beta\gamma\delta}-\lambda
R_{\alpha\beta\gamma\delta},\qquad\zeta^{\prime}=\zeta.$ (15)

Eq. (15) is one of the central results of this work, showing that the DM-like
coupling constant, unlike $\mathbf{K}$, is indeed invariant with respect to
Eq. (11), and hence a well-defined bulk property, as anticipated earlier.

Notwithstanding the aforementioned ambiguity of $\mathbf{K}$, the information
contained in it is crucial to obtaining a well-defined value of the local
polarization at leading order in $\phi$ and ${\bf q}$. To see this, we assume
in the following that the fields are modulated along a single direction
$\hat{s}$ and constant along the normal planes. (This is appropriate, for
example, to modeling a domain wall oriented along ${\hat{s}}$.) Within these
mechanical boundary conditions, we obtain (see Section S8 sm ) the relaxed
electrical polarization as (summation over repeated indices is implied)

$P_{\alpha}=\frac{1}{\Omega}Z^{[i]}{\Phi}_{ij}^{+}\left(\tilde{K}^{[j]}_{\alpha\gamma\lambda}(\hat{s})\mathcal{S}_{\gamma\lambda,s}+\zeta^{[j]}\mathcal{A}_{\alpha}\right),$
(16)

where ${\Phi}^{+}$ is the pseudoinverse Born and Huang (1954); Hong and
Vanderbilt (2013); Royo and Stengel (2022b) of the zone-center force-constants
matrix;
$\mathcal{S}_{\gamma\lambda,s}=\partial(\phi_{\gamma}\phi_{\lambda})/\partial
s$ and $\mathcal{A}_{\alpha}=\phi_{s}\partial\phi_{\alpha}/\partial
s-\phi_{\alpha}\partial\phi_{s}/\partial s$ are the relevant symmetric and
antisymmetric components of the nonlinear $\phi$-gradient tensor; $Z^{[i]}$
are the mode dynamical charges; and the renormalized flexo-like coefficients
are

$\tilde{K}^{[j]}_{\alpha\gamma\lambda}(\hat{s})=K^{[j]}_{\alpha\hat{s}\gamma\lambda}+C^{[j]}_{\alpha\hat{s}\beta\hat{s}}[\mathcal{C}(\hat{s})]^{-1}_{\beta\sigma}R_{\sigma\hat{s}\gamma\lambda}.$
(17)

Here $C^{[j]}_{\alpha\hat{s}\beta\hat{s}}$ and
$\mathcal{C}_{\beta\sigma}(\hat{s})=\mathcal{C}_{\beta\hat{s}\sigma\hat{s}}$
are the projections along $\hat{s}$ of the flexoelectric coupling sm  and
elastic tensors, respectively. The second term in Eq. (17) originates from the
relaxation of the acoustic modes, which produce a strain gradient (and hence
atomic forces via flexoelectricity) at first order in ${q}$.

By combining the sum rule Eq. (14) with its flexoelectric counterpart, Stengel
(2013); Royo and Stengel (2022b)
$\sum_{j}C^{[j]}_{\alpha\beta\gamma\lambda}=\mathcal{C}_{\alpha\beta\gamma\lambda}$,
it is straightforward to verify that the sublattice sum of the renormalized
force-response coefficients, $|\tilde{K}_{\gamma\lambda}(\hat{s})\rangle$,
identically vanishes. This guarantees Born and Huang (1954) that the total
polarization $P_{\alpha}$ is well defined, proving our point. Conversely, the
individual contributions to $P_{\alpha}$ associated with the two terms in Eq.
(17) depend on how the pseudoinverse is constructed, Diéguez and Stengel
(2022) and are therefore ill defined as stand-alone properties. Such intimate
relationship between the direct flexo-like contribution to the atomic forces
[first term in Eq. (17)] and the _nonanalytic elastic contribution_ of the
second term provide a nice illustration of the covariance principle of Ref.
Diéguez and Stengel (2022), which we generalize here to the nonlinear regime.
These conclusions have direct implications for the continuum modeling of
inhomogeneous ferroelectric Yudin  _et al._ (2012); Gu  _et al._ (2014); Chen
_et al._ (2022) and ferroelastic Eliseev  _et al._ (2013); Morozovska  _et
al._ (2012); Schiaffino and Stengel (2017); Schranz  _et al._ (2020); Tröster
_et al._ (2022) structures, where the aforementioned two mechanisms play a
central role.

As a representative demonstration of the above arguments, we consider the case
where the field $\bm{\phi}$ corresponds to the out-of-phase
antiferrodistortive (AFD) tilts in perovskite-structure oxides, with SrTiO3
Schiaffino and Stengel (2017) as testcase. Calculations of the rotopolar
Schiaffino and Stengel (2017) force-response tensor
$|w_{\beta\gamma\lambda}\rangle$, (its symmetric part,
$|K_{\beta\gamma\lambda}\rangle=(|w_{\beta\gamma\lambda}\rangle+|w_{\beta\lambda\gamma}\rangle)/4$,
corresponds to the “flexo-AFD” effect described in Ref. Eliseev  _et al._
(2013)) are carried out within the framework of the local-density
approximation (LDA) to density-funcional theory as implemented in the Abinit
Gonze  _et al._ (2016); Romero  _et al._ (2020); Hamann (2013); van Setten
_et al._ (2018) package. We use a nonprimitive cell of 10 atoms in order to
accommodate a small uniform tilt $\phi^{0}_{\alpha}$ in the structure, which
allows us to treat the third derivatives of Eq. (10) via finite differences in
$\phi^{0}_{\alpha}$. (The parametrization of the AFD mode amplitudes, in
length units, follows the established convention. Cao and Barsch (1990);
Schiaffino and Stengel (2017); relaxation of the antiferroelectric $R$-mode of
Ti Schiaffino and Stengel (2017); Casals  _et al._ (2018) is fully accounted
for in the calculated $W^{[i]}_{\alpha\beta\gamma\lambda}$ coefficients.)
Numerical results, details of the method and additional supporting data are
reported in Ref. sm ; of particular note, we provide sm  a stringent numerical
proof of the sum rule, Eq. (14), which we base on an independent calculation
of the ${\bf R}$ (rotostriction Eliseev  _et al._ (2013); Morozovska  _et al._
(2012); Schiaffino and Stengel (2017)) tensor.

Figure 1:  Comparison between the atomic forces extracted from a direct
calculation of an AFD cycloid (symbols) and the predictions of the macroscopic
model (solid curves). Forces on Sr (black circles), Ti (red squares), and the
two $T_{1u}$ oxygen modes $\xi_{3}$ (blue triangles) and $\xi_{4}$ (green
diamonds) are shown. Dashed lines show the uniform DM-like forces. (a): no
elastic relaxation; (b): mechanical equilibrium. Lower insets show the
sublattice sum of the forces: first-principles (triangles) and model (solid
curve). Upper insets schematically illustrate the relevant portion of the
(fixed- or relaxed-strain) cycloidal pattern.

To illustrate the physical meaning of the calculated rotopolar coefficients,
and as a further numerical validation thereof, we next consider a frozen-in
cycloidal Schiaffino and Stengel (2017) tilt pattern in the form
$\phi_{s}=\phi\cos({\bf q\cdot r})$, $\phi_{r}=\phi\sin({\bf q\cdot r})$,
$\phi_{z}=0$, where both the AFD pseudovector $\bm{\phi}$ and the propagation
direction ${\bf q}=q\hat{s}=q[\cos(\theta),\sin(\theta),0]$ lie in the
pseudocubic $xy$ plane. [Here $\hat{r}=\hat{z}\times\hat{s}$ is the in-plane
direction that is orthogonal to ${\bf q}$.] Our long-wavelength approach
predicts, for the symmetry-adapted sublattice mode $[i]$, a _geometric_ force
(both DM- and flexo-type couplings are linear in $P$, which implies an
improper Stengel  _et al._ (2012); Bousquet  _et al._ (2008); Fennie and Rabe
(2005); Benedek and Fennie (2011) mechanism for local inversion-symmetry
breaking) at a given point ${\bf r}$ in the crystal, whose transverse
component reads as

$f_{r}^{[i]}({\bf r})=\phi^{2}q\left[\zeta^{[i]}+2K^{[i]}(\hat{q})\cos(2{\bf
q\cdot r})\right].$ (18)

($K^{[i]}(\hat{q})=K^{[i]}_{rsrs}$ stand for the 1212 components of the flexo-
type tensor in the rotated ${\hat{s}},{\hat{r}},\hat{z}$ system.) In Fig. 1(a)
we compare the prediction of Eq. (18) with the forces that we obtain via a
direct first-principles calculation of the AFD cycloid. (We use
$\theta=\pi/4$, corresponding to $\hat{s}\parallel[110]$ in the pseudocubic
system, and $q=2\pi/(12\sqrt{2}a_{0})$, which we accommodate in a 120-atom
supercell; the tilt amplitude is set to $|\bm{\phi}|=0.02a_{0}$, i.e., to a
tilt angle of $2.3^{\circ}$.) The agreement is excellent, with a discrepancy
of the order of few percents at most. Note the qualitative difference between
the uniform DM-like contribution to $f_{r}^{[i]}$ (dashed lines), and the
spatially modulated flexo-like term, which averages to zero in any periodic
tilt pattern.

The uniform DM-like forces sum up to zero, consistent with the translational
symmetry of the crystal and with our formal results. Conversely, the flexo-
like forces display the expected drift, shown in the inset of Fig. 1(a), that
originates from the strain coupling via Eq. (14). To verify that the net drift
disappears at mechanical equilibrium, we determine the elastic displacement
amplitude via $u(s)=-\phi^{2}R_{rsrs}/(2q\mathcal{C}_{rsrs})\cos(2{qs})$.
After incorporating $u(s)$ into the simulation cell, we recalculate the forces
from first principles, and compare them in Fig. 1(b) with the predictions of
Eq. (17). [The latter implies an additional contribution to Eq. (18) of the
type $\Delta f^{[i]}_{r}=-4q^{2}C^{[i]}_{rsrs}u(s)$.] Again, the agreement is
excellent, and the elastic forces (inset) now vanish as expected.

In addition to validating our claims numerically, this test clarifies the
relation between the “flexoantiferrodistortive” Eliseev  _et al._ (2013) and
“flexo-roto” Morozovska  _et al._ (2012) effects (corresponding to the second
term in Eq. (18) and to $\Delta f^{[i]}_{r}$, respectively) described in the
recent literature, and the necessity to account for both in order to obtain
quantitatively accurate physical answers. Fig. 1 also demonstrates (once more
Diéguez and Stengel (2022)) the ability of our _second-principles_ Ghosez and
Junquera (2022) macroscopic theory to predict the atomic forces (and hence the
relaxed positions) in arbitrary inhomogeneous ferroic structures, with an
accuracy that is comparable to that of direct _ab initio_ calculations. This
provides an ideal framework for multiscale simulations, as the coefficients of
the continuum Hamiltonian can be systematically validated against the
microscopic reference in an exact limit.

The usefulness of the present theory is especially manifest in materials like
SrTiO3, where the lowest-frequency (“soft”) mode at the zone center carries
99% of the polar response to a static perturbation. This observation leads to
a compact representation of the physical effects described insofar in terms of
few materials-specific parameters; their calculated values are reported in
Table S9 sm . The information therein allows one to calculate the local
polarization in an arbitrary inhomogeneous tilt structure via Eq. (16). For
example, in the cycloid model of Fig. 1, which is representative of the
typical inhomogeneities of the order parameter in the ferroelastic phase of
SrTiO3, we obtain a macroscopic polarization of $P=-0.9$ $\mu$C/cm2. The
effect increases linearly with the static dielectric constant, $\epsilon$,
which should lead to values of $P$ that are at least an order of magnitude
larger at low temperature. ($\epsilon\simeq 900$ in our LDA calculations.)

In summary, the trilinear Lifshitz invariants of Eq. (1) and (2) emerge here
as a rich and barely tapped playground of potentially useful crystal
properties, thus opening many opportunities for future research. We shall
explore these promising directions in the context of forthcoming publications.

###### Acknowledgements.

We acknowledge support from Ministerio de Ciencia Y Innovación (MICINN-Spain)
through Grant No. PID2019-108573GB-C22; from Severo Ochoa FUNFUTURE center of
excellence (CEX2019-000917-S); from Generalitat de Catalunya (Grant No. 2021
SGR 01519); and from the European Research Council (ERC) under the European
Union’s Horizon 2020 research and innovation program (Grant Agreement No.
724529).

## References

  * Fert  _et al._ (2017) Albert Fert, Nicolas Reyren, and Vincent Cros, “Magnetic skyrmions: advances in physics and potential applications,” Nature Reviews Materials 2, 17031 (2017).
  * Meier (2015) Dennis Meier, “Functional domain walls in multiferroics,” Journal of Physics: Condensed Matter 27, 463003 (2015).
  * Behera  _et al._ (2022) Piush Behera, Molly A. May, Fernando Gomez-Ortiz, Sandhya Susarla, Sujit Das, Christopher T. Nelson, Lucas Caretta, Shang-Lin Hsu, Margaret R. McCarter, Benjamin H. Savitzky, Edward S. Barnard, Archana Raja, Zijian Hong, Pablo Garcia-Fernandez, Stephen W. Lovesey, Gerrit van der Laan, Peter Ercius, Colin Ophus, Lane W. Martin, Javier Junquera, Markus B. Raschke, and Ramamoorthy Ramesh, “Electric field control of chirality,” Science Advances 8, eabj8030 (2022), https://www.science.org/doi/pdf/10.1126/sciadv.abj8030 .
  * Naumov  _et al._ (2004) Ivan I. Naumov, L. Bellaiche, and Huaxiang Fu, “Unusual phase transitions in ferroelectric nanodisks and nanorods,” Nature 432, 737–740 (2004).
  * Eliseev  _et al._ (2013) Eugene A. Eliseev, Sergei V. Kalinin, Yijia Gu, Maya D. Glinchuk, Victoria Khist, Albina Borisevich, Venkatraman Gopalan, Long-Qing Chen, and Anna N. Morozovska, “Universal emergence of spatially modulated structures induced by flexoantiferrodistortive coupling in multiferroics,” Phys. Rev. B 88, 224105 (2013).
  * Shapovalov and Stengel (2022) K. Shapovalov and M. Stengel, “Tilt-driven antiferroelectricity in PbZrO3,” arXiv:2112.12167 (2022).
  * Pöttker and Salje (2016) Henning Pöttker and Ekhard K H Salje, “Flexoelectricity, incommensurate phases and the lifshitz point,” Journal of Physics: Condensed Matter 28, 075902 (2016).
  * Dzyaloshinsky (1958) I. Dzyaloshinsky, “A thermodynamic theory of weak ferromagnetism of antiferromagnetics,” Journal of Physics and Chemistry of Solids 4, 241–255 (1958).
  * Moriya (1960) Tôru Moriya, “Anisotropic superexchange interaction and weak ferromagnetism,” Phys. Rev. 120, 91–98 (1960).
  * Mostovoy (2006) Maxim Mostovoy, “Ferroelectricity in spiral magnets,” Phys. Rev. Lett. 96, 067601 (2006).
  * Sparavigna  _et al._ (1994) A. Sparavigna, A. Strigazzi, and A. Zvezdin, “Electric-field effects on the spin-density wave in magnetic ferroelectrics,” Phys. Rev. B 50, 2953–2957 (1994).
  * Sparavigna (2009) Amelia Sparavigna, “Role of lifshitz invariants in liquid crystals,” Materials 2, 674–698 (2009).
  * Xiao  _et al._ (2009) Di Xiao, Junren Shi, Dennis P. Clougherty, and Qian Niu, “Polarization and adiabatic pumping in inhomogeneous crystals,” Phys. Rev. Lett. 102, 087602 (2009).
  * Zubko  _et al._ (2013) P. Zubko, G. Catalan, and A. K. Tagantsev, “Flexoelectric effect in solids,” Annu. Rev. Mater. Res. 43, 387–421 (2013).
  * Stengel and Vanderbilt (2016) Massimiliano Stengel and David Vanderbilt, “First-principles theory of flexoelectricity,” in  _Flexoelectricity in Solids From Theory to Applications_ , edited by Alexander K. Tagantsev and Petr V. Yudin (World Scientific Publishing Co., Singapore, 2016) Chap. 2, pp. 31–110.
  * Korzhenevskii (1981) A. L. Korzhenevskii, “On the theory of phase transitions into inhomogeneous states,” Sov. Phys. JETP 54, 568 (1981).
  * Pyatakov  _et al._ (2015) A.P. Pyatakov, A.S. Sergeev, F.A. Mikailzade, and A.K. Zvezdin, “Spin flexoelectricity and chiral spin structures in magnetic films,” Journal of Magnetism and Magnetic Materials 383, 255–258 (2015), selected papers from the sixth Moscow International Symposium on Magnetism (MISM-2014).
  * Zhao  _et al._ (2021) Hong Jian Zhao, Peng Chen, Sergey Prosandeev, Sergey Artyukhin, and Laurent Bellaiche, “Dzyaloshinskii–moriya-like interaction in ferroelectrics and antiferroelectrics,” Nature Materials 20, 341–345 (2021).
  * Chen  _et al._ (2022) Peng Chen, Hong Jian Zhao, Sergey Prosandeev, Sergey Artyukhin, and Laurent Bellaiche, “Microscopic origin of the electric dzyaloshinskii-moriya interaction,” Phys. Rev. B 106, 224101 (2022).
  * Schiaffino and Stengel (2017) Andrea Schiaffino and Massimiliano Stengel, “Macroscopic polarization from antiferrodistortive cycloids in ferroelastic ${\mathrm{srtio}}_{3}$,” Phys. Rev. Lett. 119, 137601 (2017).
  * Morozovska  _et al._ (2012) Anna N. Morozovska, Eugene A. Eliseev, Maya D. Glinchuk, Long-Qing Chen, and Venkatraman Gopalan, “Interfacial polarization and pyroelectricity in antiferrodistortive structures induced by a flexoelectric effect and rotostriction,” Phys. Rev. B 85, 094107 (2012).
  * Casals  _et al._ (2018) Blai Casals, Andrea Schiaffino, Arianna Casiraghi, Sampo J. Hämäläinen, Diego López González, Sebastiaan van Dijken, Massimiliano Stengel, and Gervasi Herranz, “Low-temperature dielectric anisotropy driven by an antiferroelectric mode in ${\mathrm{srtio}}_{3}$,” Phys. Rev. Lett. 120, 217601 (2018).
  * Stengel (2013) M. Stengel, “Flexoelectricity from density-functional perturbation theory,” Phys. Rev. B 88, 174106 (2013).
  * Royo and Stengel (2022a) Miquel Royo and Massimiliano Stengel, “Lattice-mediated bulk flexoelectricity from first principles,” Phys. Rev. B 105, 064101 (2022a).
  * Diéguez and Stengel (2022) Oswaldo Diéguez and Massimiliano Stengel, “Translational covariance of flexoelectricity at ferroelectric domain walls,” Phys. Rev. X 12, 031002 (2022).
  * Royo and Stengel (2019) Miquel Royo and Massimiliano Stengel, “First-principles theory of spatial dispersion: Dynamical quadrupoles and flexoelectricity,” Phys. Rev. X 9, 021050 (2019).
  * Baroni  _et al._ (2001) S. Baroni, S. de Gironcoli, and A. Dal Corso, “Phonons and related crystal properties from density-functional perturbation theory,” Rev. Mod. Phys. 73, 515 (2001).
  * Gonze and Lee (1997) X. Gonze and C. Lee, “Dynamical matrices, Born effective charges, dielectric permittivity tensors, and interatomic force constants from density-functional perturbation theory,” Phys. Rev. B 55, 10355 (1997).
  * Gonze  _et al._ (2009) X. Gonze, B. Amadon, P.-M. Anglade, J.-M. Beuken, F. Bottin, P. Boulanger, F. Bruneval, D. Caliste, R. Caracas, M. Côté, T. Deutsch, L. Genovese, Ph. Ghosez, M. Giantomassi, S. Goedecker, D.R. Hamann, P. Hermet, F. Jollet, G. Jomard, S. Leroux, M. Mancini, S. Mazevet, M.J.T. Oliveira, G. Onida, Y. Pouillon, T. Rangel, G.-M. Rignanese, D. Sangalli, R. Shaltaf, M. Torrent, M.J. Verstraete, G. Zerah, and J.W. Zwanziger, “ABINIT: First-principles approach to material and nanosystem properties,” Computer Phys. Commun. 180, 2582–2615 (2009).
  * Romero  _et al._ (2020) Aldo H. Romero, Douglas C. Allan, Bernard Amadon, Gabriel Antonius, Thomas Applencourt, Lucas Baguet, Jordan Bieder, François Bottin, Johann Bouchet, Eric Bousquet, Fabien Bruneval, Guillaume Brunin, Damien Caliste, Michel Côté, Jules Denier, Cyrus Dreyer, Philippe Ghosez, Matteo Giantomassi, Yannick Gillet, Olivier Gingras, Donald R. Hamann, Geoffroy Hautier, François Jollet, Gérald Jomard, Alexandre Martin, Henrique P. C. Miranda, Francesco Naccarato, Guido Petretto, Nicholas A. Pike, Valentin Planes, Sergei Prokhorenko, Tonatiuh Rangel, Fabio Ricci, Gian-Marco Rignanese, Miquel Royo, Massimiliano Stengel, Marc Torrent, Michiel J. van Setten, Benoit Van Troeye, Matthieu J. Verstraete, Julia Wiktor, Josef W. Zwanziger, and Xavier Gonze, “ABINIT: Overview and focus on selected capabilities,” The Journal of Chemical Physics 152, 124102 (2020).
  * (31) “See supplemental material at [hyperlink],” .
  * Hong and Vanderbilt (2013) J. Hong and D. Vanderbilt, “First-principles theory and calculation of flexoelectricity,” Phys. Rev. B 88, 174107 (2013).
  * Resta  _et al._ (1990) R. Resta, L. Colombo, and S. Baroni, “Absolute deformation potentials in semiconductors,” Phys. Rev. B 41, 12358–12361 (1990).
  * Stengel (2016) Massimiliano Stengel, “Unified ab initio formulation of flexoelectricity and strain-gradient elasticity,” Phys. Rev. B 93, 245107 (2016).
  * Royo and Stengel (2022b) Miquel Royo and Massimiliano Stengel, “Lattice-mediated bulk flexoelectricity from first principles,” Phys. Rev. B 105, 064101 (2022b).
  * Born and Huang (1954) Max Born and Kun Huang,  _Dynamical Theory of Crystal Lattices_ (Oxford University Press, Oxford, 1954).
  * Yudin  _et al._ (2012) P. V. Yudin, A. K. Tagantsev, E. A. Eliseev, A. N. Morozovska, and N. Setter, “Bichiral structure of ferroelectric domain walls driven by flexoelectricity,” Phys. Rev. B 86, 134102 (2012).
  * Gu  _et al._ (2014) Yijia Gu, Menglei Li, Anna N. Morozovska, Yi Wang, Eugene A. Eliseev, Venkatraman Gopalan, and Long-Qing Chen, “Flexoelectricity and ferroelectric domain wall structures: Phase-field modeling and dft calculations,” Phys. Rev. B 89, 174111 (2014).
  * Schranz  _et al._ (2020) W. Schranz, C. Schuster, A. Tröster, and I. Rychetsky, “Polarization of domain boundaries in $\mathrm{Sr}\mathrm{Ti}{\mathrm{o}}_{3}$ studied by layer group and order-parameter symmetry,” Phys. Rev. B 102, 184101 (2020).
  * Tröster  _et al._ (2022) A. Tröster, C. Verdi, C. Dellago, I. Rychetsky, G. Kresse, and W. Schranz, “Hard antiphase domain boundaries in strontium titanate unravelled using machine-learned force fields,” Phys. Rev. Mater. 6, 094408 (2022).
  * Gonze  _et al._ (2016) X. Gonze, F. Jollet, F. Abreu Araujo, D. Adams, B. Amadon, T. Applencourt, C. Audouze, J.-M. Beuken, J. Bieder, A. Bokhanchuk, E. Bousquet, F. Bruneval, D. Caliste, M. Côté, F. Dahm, F. Da Pieve, M. Delaveau, M. Di Gennaro, B. Dorado, C. Espejo, G. Geneste, L. Genovese, A. Gerossier, M. Giantomassi, Y. Gillet, D.R. Hamann, L. He, G. Jomard, J. Laflamme Janssen, S. Le Roux, A. Levitt, A. Lherbier, F. Liu, I. Lukačević, A. Martin, C. Martins, M.J.T. Oliveira, S. Poncé, Y. Pouillon, T. Rangel, G.-M. Rignanese, A.H. Romero, B. Rousseau, O. Rubel, A.A. Shukri, M. Stankovski, M. Torrent, M.J. Van Setten, B. Van Troeye, M.J. Verstraete, D. Waroquiers, J. Wiktor, B. Xu, A. Zhou, and J.W. Zwanziger, “Recent developments in the ABINIT software package,” Computer Physics Communications 205, 106–131 (2016).
  * Hamann (2013) D. R. Hamann, “Optimized norm-conserving Vanderbilt pseudopotentials,” Phys. Rev. B 88, 085117 (2013).
  * van Setten  _et al._ (2018) M.J. van Setten, M. Giantomassi, E. Bousquet, M.J. Verstraete, D.R. Hamann, X. Gonze, and G.-M. Rignanese, “The PseudoDojo: Training and grading a 85 element optimized norm-conserving pseudopotential table,” Comp. Phys. Comm. 226, 39–54 (2018).
  * Cao and Barsch (1990) Wenwu Cao and Gerhard R. Barsch, “Landau-ginzburg model of interphase boundaries in improper ferroelastic perovskites of ${\mathit{d}}_{4\mathit{h}}^{18}$ symmetry,” Phys. Rev. B 41, 4334–4348 (1990).
  * Stengel  _et al._ (2012) Massimiliano Stengel, Craig J. Fennie, and Philippe Ghosez, “Electrical properties of improper ferroelectrics from first principles,” Phys. Rev. B 86, 094112 (2012).
  * Bousquet  _et al._ (2008) Eric Bousquet, Matthew Dawber, Nicolas Stucki, C. Lichtensteiger, Patrick Hermet, Stefano Gariglio, Jean-Marc Triscone, and Philippe Ghosez, Nature (London) 452, 732–736 (2008).
  * Fennie and Rabe (2005) Craig J. Fennie and Karin M. Rabe, “Ferroelectric transition in $\mathrm{Y}\mathrm{Mn}{\mathrm{o}}_{3}$ from first principles,” Phys. Rev. B 72, 100103(R) (2005).
  * Benedek and Fennie (2011) Nicole A. Benedek and Craig J. Fennie, “Hybrid improper ferroelectricity: A mechanism for controllable polarization-magnetization coupling,” Phys. Rev. Lett. 106, 107204 (2011).
  * Ghosez and Junquera (2022) Philippe Ghosez and Javier Junquera, “Modeling of ferroelectric oxide perovskites: From first to second principles,” Annual Review of Condensed Matter Physics 13, 325–364 (2022), https://doi.org/10.1146/annurev-conmatphys-040220-045528 .


# Autonomous Underwater Vehicle-Manipulator Systems Path Planning with
RRTAUVMS Algorithm

Xiaoxu Cao1,2,3, Linyi Gu1, JunChen Mu1, Qian Zhang1, Qi Song1
Chunxiao Liu2, Cong Qiu2 1 Zhejiang University, State Key Lab of Fluid Power &
Mechatronic Systems, Hangzhou, CN<EMAIL_ADDRESS>Shenzhen Sensetime
Technology CO.,LTD 3 Shenzhen Institutes of Advanced Technology, Chinese
Academy of Sciences

###### Abstract

Autonomous Underwater Vehicle-Manipulator systems (AUVMS) is a new tool for
ocean exploration, the AUVMS path planning problem is addressed in this paper.
AUVMS is a high dimension system with a large difference in inertia
distribution, also it works in a complex environment with obstacles. By
integrating the rapidly-exploring random tree(RRT) algorithm with the AUVMS
kinematics model, the proposed RRTAUVMS algorithm could randomly sample in the
configuration space(C-Space), and also grow the tree directly towards the
workspace goal in the task space. The RRTAUVMS can also deal with the
redundant mapping of workspace planning goal and configuration space goal.
Compared with the traditional RRT algorithm, the efficiency of the AUVMS path
planning can be significantly improved.

##  1 INTRODUCTION

AUVMS is the new tool for ocean exploration, some prototypes have been
proposed to evaluate the work performance of underwater intervention tasks[1,
2, 3, 4]. Although the traditional working class remotely operated
vehicles(ROV)-Manipulator Systems have already been deployed in many
underwater tasks with proven techniques, the ROVMS(ROV-Manipulator Systems)
requires many expensive auxiliary systems, such as the Tether Management
System and Dynamic Position system to reduce the risk of umbilical cable
broken[5]. Experienced onboard operators are required when conducting the
underwater tasks, however, the fatal accident like ROV intertwining with
underwater facilities still can’t be totally avoided. Without umbilical cable,
the AUVuMS pose obvious advantages in long distance inspection task, and
manipulation work in the multi-obstacles environment, however, the complex
working environment also makes the coordinate motion planning of AUVMS a core
research topic.

Motion planning technology is one of the key technologies to realize the
autonomous operation. For autonomous robots, the motion planning is divided
into two parts, path planning, mainly to search the collision-free paths in
the global scope; trajectory planning, solving the problems of motion
allocation and freedom redundancy processing of highly redundant systems.

The pseudo-inverse method is one of the widely used solutions for redundant
systems, the optimal vehicle/joint velocities in configuration space can be
obtained, this also applies to the weighted pseudo-inverse method, which will
lead to the minimization of weighted vehicle/joint velocities norm[6, 7].

The task priority redundancy resolution techniques are proposed by Gianluca
Antonelli[8], this method can guarantee the end effector(eef) trajectory
tracking while at the same time, achieves a second task such as the optimal
energy consumption or robot manipulability. In paper[9] the task priority
method is used to solve the output saturation problem. After that, further
researches are proposed in [10, 11] to merge the fuzzy algorithm with task
priority approach. The fuzzy technique is used to coordinate the multiple
secondary tasks, the multi-task motion planning can be achieved with this
method.

Tremendous research effort has been devoted to the robot motion planning. The
randomized sampling based algorithms have been widely used for robots high-
dimensional motion planning. In 1998, the Rapidly-exploring Random Tree (RRT)
algorithm was proposed[12, 13]. The RRT algorithm can efficiently search the
path in the high-dimensional C-space, also the resulting path can guarantee no
violation with obstacles. The RRT algorithm and its variations have been used
in the underwater robots planning scenarios. The motion planning of AUV in
underwater environment has been studied based on RRT algorithm[14, 15], the
optimal rapidly-exploring random tree (RRT*) is extended using concepts of
anytime algorithms and lazy collision evaluation. The motion planning of
underwater vehicle manipulator systems for autonomous underwater inspection
operations is investigated[16]. The collision avoidance, approximation of the
given task curve and critical optimization criteria are considered in the
planning algorithm. The planner can deal with the unknown obstacles inside the
workspace while executing the task. The RRT-connect algorithm is implemented
in GIRONA500 UVMS motion planning[17], RRT-connect works by growing two
separated trees of paths rooted at the start and goal poses. The trees explore
the space around them by progressing toward one another using a simple greedy
heuristic until they join together, it’s an efficient path searching method,
and it is suitable for high-dimensional C-space.

The RRT algorithm is also used in redundant robot system planning. For
redundant system, solving the inverse kinematics(IK) is challenging, the
numerical approximation method provides a way for IK solution, however, this
method has high computational complexity, and sometimes can’t converge to a
valid solution. So in[18], the heuristic workspace metric which measures the
manipulator’s distance to goal is used to guide the search in C-Space, the IK
problem is avoided here. In [19], the Jacobian transpose is used to guide the
RRT tree growing direction, the modified RRT algorithm is implemented in the 7
degree-of-freedom robotic arm. The IK solution is not required using the
proposed algorithm, the Jacobian inverse solution is also avoided here.
However, the Jacobian transpose based solution can not take the joint inertia
difference into account, also the joint limits constrained is not considered.
So the algorithm in paper[19] doesn’t apply to the AUVMS system, which is
composed of two subsystems with a large inertia difference, and the
manipulator joint limits constrain is considered.

In this paper, the AUVMS kinematics constrains are integrated with the RRT
algorithm to solve the AUVMS path planning problem. The core idea can be
summarized as: Using the pseudo-inverse Jacobian matrix to guide the RRT tree
exploration in the workspace, while randomly sampling in C-space. So in this
paper, we will investigate how to achieve the whole body motion planning for a
high dexterous system with large inertia difference. This paper is organized
as follows. In the next section, a brief description of AUVMS kinematics model
are given.

##  2 PROBLEM FORMULATION

###  2.1 AUVMS Kinematics Model

The system configuration of AUVMS is shown in Fig1, the manipulator with 4
revolute joints is mounted on the left side of AUV. The body-fixed frame is
built on the AUV center of gravity, the eef frame is built on the end tip of
AUV, the earth-fixed reference frame aligns with the body-fixed frame. The eef
position & pose can be expressed as:

Figure 1: AUVMS system configuration and frame definition

First, the relationship of body-fixed velocity of AUV and the velocity in
earth-fixed frame of AUV can be expressed as:

$\dot{\eta}={\left[\begin{array}[]{cc}J_{v1}(\eta_{2})&0\\\
0&J_{v2}(\eta_{2})\end{array}\right]}\nu$ (1)

Where $J_{v1},J_{v2}$ are the Jacobian matrixs of AUV,

$J_{v1}={\left[\begin{array}[]{ccc}c{\psi}c{\theta}&-s{\psi}c{\phi}+c{\psi}s{\theta}s{\phi}&s{\psi}s{\phi}+c{\psi}s{\theta}c{\phi}\\\
s{\psi}c{\theta}&c{\psi}c{\phi}+s{\psi}s{\theta}s{\phi}&-c{\psi}s{\phi}+s{\psi}s{\theta}c{\phi}\\\
-s{\theta}&s{\phi}c{\theta}&c{\phi}c{\theta}\end{array}\right]}$ (2)

$J_{v2}=\frac{1}{c{\theta}}{\left[\begin{array}[]{ccc}1&s{\phi}s{\theta}&c{\phi}s{\theta}\\\
0&c{\phi}c{\theta}&-c{\theta}s{\phi}\\\ 0&s{\phi}&c{\phi}\end{array}\right]}$
(3)

Where $s\bullet=\sin{\bullet},c\bullet=\cos{\bullet}$.

For manipulator system, the kinematics can be derived using the D-H method.

The position and pose of eef in the body-fixed frame can be defined as:

$\eta_{ve}=\begin{bmatrix}\eta^{T}_{ve1}&\eta^{T}_{ve2}\end{bmatrix}^{T}$ (4)

Where $\eta_{ve1}=[x_{ve},y_{ve},z_{ve}]^{T}$ is the position of eef in body-
fixed frame, $\eta_{ve2}=[\phi_{ve},\theta_{ve},\psi_{ve}]^{T}$ is the pose
vector of eef in body-fixed frame.

Manipulator Jacobian matrix describes the relationship of joint space velocity
and eef velocity in body-fixed frame.

$\dot{\eta}_{ve}=J_{m}\dot{q}=\begin{bmatrix}J_{mp}(Q)\\\
J_{mo}(Q)\end{bmatrix}\dot{Q}$ (5)

Where, $Q=[q_{1},q_{2},q_{3},q_{4}]^{T}$ is the joint position vector,
$\dot{Q}$ is the joint velocity; $\dot{\eta}_{ve}\in\mathbb{R}^{6}$is the
manipulator eef velocity in body-fixed frame, $J_{m}\in\mathbb{R}^{6\times 4}$
is the manipulator Jacobian matrix.

$S(\omega)$ donates the skew-symmetric matrix,
$\omega=[\omega_{x},\omega_{y},\omega_{z}]^{T}$, then $S(\omega)$ is

$S(\omega)=\begin{bmatrix}0&-\omega_{z}&\omega_{y}\\\
\omega_{z}&0&-\omega_{x}\\\ -\omega_{y}&\omega_{x}&0\end{bmatrix}$ (6)

Jacobian matrix describe the relationship of configuration space velocity
($\zeta=[\nu_{1},\nu_{2},\dot{Q}]^{T}\in\mathbb{R}^{8}$)and the eef speed in
working
space($\dot{\eta}_{ie}=[\eta^{T}_{ie1}\eta^{T}_{ie2}]^{T}\in\mathbb{R}^{6}$),
$\eta_{ie1}=[x_{ie},y_{ie},z_{ie}]^{T}$ is the position vector in reference
frame, $\eta_{ie2}=[\phi_{ie},\theta_{ie},\psi_{ie}]^{T}$ is the pose vector.

$\dot{\eta}_{ie}=J_{auvms}\zeta$ (7)

The Jacobian matrix of the whole system can be expressed as:

$J_{auvms}=\begin{bmatrix}J_{v1}(\eta_{2})&-S(J_{v1}(\eta_{2})\eta_{ve1})J_{v2}(\eta_{2})&J_{v1}(\eta_{2})J_{mp}\\\
0&J_{v2}(\eta_{2})&J_{v1}(\eta_{2})J_{mo}\end{bmatrix}$ (8)

###  2.2 Weighted Pseudo-Inverse Solution

As the system is redundant, solution of Eq.7 can not be directly obtained by
matrix inverse. So the pseudo-inverse is derived by the following steps.

Define the quadratic cost function:

$E=\frac{1}{2}\zeta^{T}W\zeta$ (9)

Where $W$ is the weight for each joint in C-space. Then optimal solution which
can minimize the cost function Eq.9 can be derived as:

$\zeta=W^{-1}J^{T}_{auvms}(J_{auvms}W^{-1}J^{T}_{auvms})^{-1}\dot{\eta}_{ie}$
(10)

The weighted pseudo-inverse can be expressed as:

$J^{\dagger}_{auvms}=W^{-1}J^{T}_{auvms}(J_{auvms}W^{-1}J^{T}_{auvms})^{-1}$
(11)

Consider the joint position constrain problem, in the motion planning, to
avoid the invalid planning results, for example the joint position out of
limit, the motion planning considering the joint constrain are proposed here.

Design the cost function as:

$H(q)=\sum_{i=1}^{4}\frac{(q_{i,max}-q_{i,min})}{C_{i}(q_{i.max}-q_{i})(q_{i}-q_{i,min})}$
(12)

For joint $i$, $C_{i}>0$, $q_{i}$ is the joint position, and
$q_{i}\in(q_{i,max},q_{i,min})$, the partial derivative of $H(q)$ is:

$\frac{\partial H(q)}{\partial
q_{i}}=\frac{1}{C_{i}}\frac{(q_{i,max}-q_{i,min})(2q_{i}-q_{i,max}-q_{i,min})}{(q_{i.max}-q_{i})^{2}(q_{i}-q_{i,min})^{2}}$
(13)

so the weight for joint $i$ is

$w_{i}=1+\left\|\frac{\partial H(q)}{\partial q_{i}}\right\|$ (14)

so the weight matrix for manipulator is:

$W_{m}=\mathrm{diag}[w_{1},w_{2},w_{3},w_{4}]$ (15)

Then the whole system weight matrix can be defined as:

$W=\mathrm{diag}[w_{x},w_{y},w_{z},w_{r},w_{1},w_{2},w_{3},w_{4}]$ (16)

So far, the AUVMS kinematic model and the weight matrix has been obtained,
they will play a important role in the new path planning algorithm.

##  3 RRTAUVMS ALGORITHM

The traditional RRT algorithm will sample in the C-space uniformly, also the
start state and goal state are in C-space are needed. As we explained before,
the system is constrained by the kinematic model, so sampling in the workspace
and then map the extension into C-space, that will be more efficient. Also,
the goal state are normally given in workspace, such as a position in the
workspace, the goal state should be mapped into C-space at first for the
traditional RRT, however for redundant system, the mapping will lead to a
variety of optimization problems. Here the RRTAUVMS algorithm is proposed the
for the motion planning of AUVMS.

Algorithm 1 RRTAUVMS Algorithm

1:Map, Start, goal, Step length in CSpace,Step length in WSpace

2:

3:function RRTAUVMS(start, goal, K, CStep, WStep)

4: Tree.init($x_{init}$)

5: while DistancetoGoal$()>$Threshold do

6: for i=1 to K do

7: p=rand

8: if $p<p_{g}$ then

9: Tree=ExtendRandomly(start, goal, CStep,Tree)

10: else

11: Tree =ExtendToGoal(start, goal, WStep,Tree)

12: end if

13: end for

14: end while

15:end function

16:function ExtendToGoal(start, goal, WStep,Tree)

17: $x_{nearest}$=ClosestNodetoGoal(Tree)

18: Jauvms_pesudo=JacobianCal($x_{nearest}$)

19: $\Delta_{x}$=WorkspaceStep($x_{nearest}$, workspacegoal, WStep)

20: $\Delta_{q}$= Jauvms_pesudo $\Delta_{x}$

21: $x_{new}$=$x_{nearest}$+$\Delta_{q}$

22: if ObstacleFree($x_{new}$) then

23: Tree.add_vertex($x_{new}$)

24: end if

25: Return Tree

26:end function

Algorithm 2 Path Smooth Algorithm

1:Map, Raw path

2:

3:function SmoothPath(()PathList,map)

4: n=length(PathList)

5: StartIndex=1, EndIndex=n, SmoothList=PathList(1);

6: while StartIndex $<$ n do

7: while StartIndex $<$ EndIndex do

8: if isEdgeBelongsFreeSpace(PathList(StartIndex),PathList(EndIndex), map)
then

9: SmoothPaht.add(PathList(EndList))

10: StartIndex=EndIndex

11: else

12: EndIndex=EndIndex-1

13: end if

14: end while

15: end while

16:end function

The main algorithm is shown in Algorithm.1. In the pseudo code, the RRTAUVMS
is the main function, the start state, goal state, maximum iteration steps K,
C-space step: CStep and work space step WStep are the variables of the
function. At the beginning of the algorithm, a Tree is created with only one
node, the node corresponding to the start state. Then the algorithm enter the
while loop, for every iteration step, the algorithm will generate a random
number $p\in[0,1]$, if $p$ is larger than the possibility threshold $p_{g}$,
then the tree will extend to the workspace goal with function ExtendToGoal, or
the tree will extend randomly with function ExtendRandomly. The function
ExtendRandomly is just the same as the traditional RRT, it will sample in the
feasible C-space, while the ExtendToGoal, will sample in the work space. As
shown in the pseudo code, the first step of function ExtendToGoal is find the
nearest node to the goal, which is expressed as $x_{nearest}$. Then the
Jacobian matrix of the system state $x_{nearest}$ and pseudo-inverse Jacobian
matrix are obtained. The work space extension vector $\Delta x$ can be
calculate from WorkspaceStep function, the vector $\Delta x$ starts from
$x_{nearest}$ and ends in workspace goal. Because the step extension can be
treat as an instantaneous movement direction, the Eq.10 can be applied to
calculate the step extension $\Delta q$ in the configuration space, then the
new state $x_{new}$ is obtained. If the new state is collision-free, then it’s
added to the tree. Here the selection of Jacobian inverse is important, when
compute the new state $x_{new}$, the joints angle may be out of the limits,
however, if the weighted Jacobian pseudo-inverse Eq.11 is used, the new state
will not violate the joint limits constrain. The RRTAUVMS function working
mechanism is shown in Fig.2.

Figure 2: RRTAUVMS algorithm flow chart

After the path searching procedure, a raw path with a lot of nodes can be
obtained. The raw path is not suitable for robot path following as a lot of
sharp peak are observed in the path. The Algorithm.2 is used to get a smooth
path with little nodes. It works as following, in the raw path list, connect
the start node(StartIndex) and then end node (EndIndex), then check collision.
If it’s not collision free, check the edge between (EndIndex-1) node and start
node(StartIndex), if it’s collision free, the edge is kept in the smooth list,
and make the StartIndex equals the EndIndex, the algorithm will iterate until
the StartIndex equals to the length of the raw path.

##  4 SIMULATION RESULTS

Considering the real working conditions, the simulation is designed as
following. We will command the end-effector of manipulator to move toward the
goal position, get around the obstacles on the way.

The goal position is set to be $[x_{g},y_{g},z_{g}]$=[4, 4, 4]m; and the goal
ball region is 0.3m; the CSpace step CStep is [0.1, 0.1 0.1 0.08, 0.05, 0.05,
0.05, 0.05]; the working space step WStep is: [0.2 0.2 0.2 0.05 0.05 0.05];
The start state in CSpace is
$[x_{v},y_{v},z_{v},\psi,q_{1},q_{2},q_{3},q_{4}]=[0,0,0,0,0,0,0,\pi]$, the
corresponding position and pose in working space is
$[x,y,z,\varphi,\theta,\psi]=[-0.1176,0.1898,-0.4120,0,-1.5708,0]$; the
obstacles position and radius is:$P_{obs1}=[2,2,3]^{T},r_{obs1}=0.3$;
$P_{obs2}=[1,1,1]^{T},r_{obs2}=0.2$; $P_{obs3}=[1,3,2]^{T},r_{obs3}=0.3$;

To evaluate the performance of of RRTAUVMS algorithm, contrast experiments are
conducted to compare the efficiency of traditional RRT algorithm and RRTAUVMS
algorithm. The single obstacle and multi obstacles environments are evaluated.
In the simulation experiments, the program will capture the time when the
searching is completed. The simulation platform is MATLAB/Simulink 2015a
running on PC with i5-3320M 2.6GHz CPU.

Figure 3: Non-obstacle RRTAUVMS searching result Figure 4: Single obstacle RRTAUVMS searching result Figure 5: Multi-obstacles RRT searching result Figure 6: Multi-obstacles RRTAUVMS searching result Table 1: Single-Obstacle Path Searching Results Seed | RRT searching time(s) | RRTAUVMS searching time(s)
---|---|---
10 | 30.85 | 0.3
20 | 18.55 | 0.549
30 | 13.77 | 0.3
40 | 2.99 | 0.47
50 | 1.39 | 0.26
Table 2: Multi-Obstacles Path Searching Results Seed | RRT searching time(s) | RRTAUVMS searching time(s)
---|---|---
10 | Not Found | 0.58
20 | 11.3 | 1.91
30 | 26.56 | 1.64
40 | 2.01 | 2.26
50 | Not Found | 1.07

The simulation results are shown in Fig.3 $\sim$ Fig.6. In these diagrams, the
red circles represent the arm tip position for every valid sampling node; The
green lines are the final searching results, while the black dash lines are
the final smooth paths; Start and goal positions of end effector are represent
by the blue pentagram and yellow pentagram. The Fig.3 is the RRTAUVMS
algorithm searching result after 3000 times iterations, no obstacle is added
in the workspace. Fig.4 is the searching result with one obstacle. These
simulations verify the validity of the RRTAUVMS, the algorithm can
successfully find a solution whether there is obstacle or not. Fig.5 and Fig.6
are the comparison studies of the traditional RRT and new proposed RRTAUVMS
algorithm. Three obstacles are added to the workspace to test the algorithm
performance in complex environment. The charts Fig.5 and Fig.6 show all the
collision free sampling nodes when the algorithm first find the satisfactory
path. As shown in the charts, the traditional RRT algorithm will run much more
iterations than the RRTAUVMS algorithm, so more red circles can be observed in
Fig.5. To obtain the quantitative performance of two algorithms, a great deal
of simulation experiments are conducted, the comparison results are given in
Table.1 and Table.2, the seed values are the MATLAB random number generator
input values. The tables record the time consumed by each algorithm with
different seed. The efficiency advantage of the proposed algorithm is shown in
the table. Whether for single obstacle or multi-obstacles, the planning time
of proposed algorithm for single obstacle environment is less than 1 second,
the time consumed by the traditional RRT is much more than 1 second for most
of the cases. For multi-obstacles environment, the planning time is less than
2 seconds for RRTAUVMS, however, the RRT algorithm can’t even find a path in
some cases.

Figure 7: Trajectory RRTAUVMS Figure 8: Time histories of end effector
position in workspace

Fig.7 is the AUVMS trajectories in CSpace, the trajectories are obtained by
cubic spline interpolation method with the smoothed RRTAUVMS path, Fig.8 is
the time histories of end effector position in workspace. Noting that in
Fig.7, the joint position $q_{1},q_{2},q_{3}$ are all in the joint limits, the
$q_{4}$ is a continuous revolution joint, and the $q_{4}$ position will not
affect the end effector position, so it’s not bounded to a certain range. From
these to diagrams, we can conclude that the proposed algorithm and the path
smooth algorithm can give a piratical path for AUVMS system planning.

##  5 CONCLUSIONS

In this paper, a new path planning algorithm based on RRT and system
kinematics is proposed. The AUVMS is a high redundant system, the proposed
algorithm could take advantage of the kinematics model to reduce the sampling
dimensions, and the joint position constrain is considered by introducing the
weighted pseudo-inverse solution. The simulation experiments are conducted
after the algorithm is derived. The simulation results show that the RRTAUVMS
algorithm could find a collision free path in complex environment, also it’s
more efficient than traditional RRT algorithm with a less than 2 seconds
searching time.

## References

  * [1] J. Yuh, S. K. Choi, C. Ikehara, G. H. Kim, G. McMurty, M. Ghasemi-Nejhad, N. Sarkar, and K. Sugihara, “Design of a semi-autonomous underwater vehicle for intervention missions (SAUVIM),” in _Proceedings of 1998 International Symposium on Underwater Technology_ , Apr. 1998, pp. 63–68.
  * [2] P. J. Sanz, P. Ridao, G. Oliver, C. Melchiorri, G. Casalino, C. Silvestre, Y. Petillot, and A. Turetta, “TRIDENT: A Framework for Autonomous Underwater Intervention Missions with Dexterous Manipulation Capabilities,” _IFAC Proceedings Volumes_ , vol. 43, no. 16, pp. 187–192, Jan. 2010.
  * [3] E. Simetti, G. Casalino, S. Torelli, A. Sperindé, and A. Turetta, “Floating Underwater Manipulation: Developed Control Methodology and Experimental Validation within the TRIDENT Project,” _Journal of Field Robotics_ , vol. 31, no. 3, pp. 364–385, May 2014.
  * [4] F. Maurelli, M. Carreras, J. Salvi, D. Lane, K. Kyriakopoulos, G. Karras, M. Fox, D. Long, P. Kormushev, and D. Caldwell, “The PANDORA project: A success story in AUV autonomy,” in _OCEANS 2016 - Shanghai_. IEEE, apr 2016, pp. 1–8.
  * [5] P. Ridao, M. Carreras, D. Ribas, P. J. Sanz, and G. Oliver, “Intervention AUVs: The Next Challenge,” _IFAC Proceedings Volumes_ , vol. 47, no. 3, pp. 12 146–12 159, Jan. 2014. [Online]. Available: http://www.sciencedirect.com/science/article/pii/S1474667016435494
  * [6] D. E. Whitney, “Resolved motion rate control of manipulators and human prostheses,” _IEEE Transactions on Man-Machine Systems_ , vol. 10, no. 2, pp. 47–53, June 1969.
  * [7] C. A. Klein and C. H. Huang, “Review of pseudoinverse control for use with kinematically redundant manipulators,” _IEEE Transactions on Systems, Man, and Cybernetics_ , vol. SMC-13, no. 2, pp. 245–250, Mar. 1983.
  * [8] G. Antonelli and S. Chiaverini, “Task-priority redundancy resolution for underwater vehicle-manipulator systems,” in _Proceedings. 1998 IEEE International Conference on Robotics and Automation (Cat. No.98CH36146)_ , vol. 1, May 1998, pp. 768–773 vol.1.
  * [9] G. Antonelli, G. Indiveri, and S. Chiaverini, “Prioritized closed-loop inverse kinematic algorithms for redundant robotic systems with velocity saturations,” in _2009 IEEE/RSJ International Conference on Intelligent Robots and Systems_ , Oct. 2009, pp. 5892–5897.
  * [10] G. Antonelli and S. Chiaverini, “Fuzzy redundancy resolution and motion coordination for underwater vehicle-manipulator systems,” _IEEE Transactions on Fuzzy Systems_ , vol. 11, no. 1, pp. 109–120, Feb. 2003.
  * [11] G. Antonelli and S. Chiaverini, “A fuzzy approach to redundancy resolution for underwater vehicle-manipulator systems,” _Control Engineering Practice_ , vol. 11, no. 4, pp. 445–452, Apr. 2003.
  * [12] L. E. Kavraki, P. Svestka, J. C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” _IEEE Transactions on Robotics and Automation_ , vol. 12, no. 4, pp. 566–580, Aug. 1996.
  * [13] S. M. Lavalle, “Rapidly-exploring random trees: A new tool for path planning,” _Algorithmic & Computational Robotics New Directions_, pp. 293–308, 1998.
  * [14] J. D. Hernández, E. Vidal, G. Vallicrosa, E. Galceran, and M. Carreras, “Online path planning for autonomous underwater vehicles in unknown environments,” in _2015 IEEE International Conference on Robotics and Automation (ICRA)_ , May 2015, pp. 1152–1157.
  * [15] M. Carreras, J. D. Hernández, E. Vidal, N. Palomeras, and P. Ridao, “Online motion planning for underwater inspection,” in _2016 IEEE/OES Autonomous Underwater Vehicles (AUV)_ , Nov. 2016, pp. 336–341.
  * [16] P. Sotiropoulos, V. Kolonias, N. Aspragathos, and E. Housos, “Rapid motion planning algorithm for optimal UVMS interventions in semi-structured environments using GPUs,” _Robotics and Autonomous Systems_ , vol. 74, no. Part A, pp. 15–29, Dec. 2015.
  * [17] D. Youakim, P. Ridao, N. Palomeras, F. Spadafora, D. Ribas, and M. Muzzupappa, “MoveIt!: Autonomous Underwater Free-Floating Manipulation,” _IEEE Robotics Automation Magazine_ , vol. 24, no. 3, pp. 41–51, Sept. 2017\.
  * [18] D. Bertram, J. Kuffner, R. Dillmann, and T. Asfour, “An integrated approach to inverse kinematics and path planning for redundant manipulators,” in _Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006._ , May 2006, pp. 1874–1879.
  * [19] M. V. Weghe, D. Ferguson, and S. S. Srinivasa, “Randomized path planning for redundant manipulators without inverse kinematics,” in _2007 7th IEEE-RAS International Conference on Humanoid Robots_ , Nov. 2007, pp. 477–482.