Trajectory optimization on manifolds with applications to quadrotor systems

Abstract

Manifolds are used in almost all robotics applications even if they are not modeled explicitly. We propose a differential geometric approach for optimizing trajectories on a Riemannian manifold with obstacles. The optimization problem depends on a metric and collision function specific to a manifold. We then propose our safe corridor on manifolds (SCM) method of computationally optimizing trajectories for robotics applications via a constrained optimization problem. Our method does not need equality constraints, which eliminates the need to project back to a feasible manifold during optimization. We then demonstrate how this algorithm works on an example problem on $SO (3)$ and a perception-aware planning example for visual–inertially guided robots navigating in three dimensions. Formulating field of view constraints naturally results in modeling with the manifold $R^{3} \times S^{2}$ , which cannot be modeled as a Lie group. We also demonstrate the example of planning trajectories on $SE (3)$ for a formation of quadrotors within an obstacle filled environment.

Keywords

Differential geometry optimization planning quadrotors

1. Introduction

Differential geometry for trajectory optimization in robotics is often used with the special orthogonal groups $SO (n)$ and special Euclidean groups $SE (n)$ . Previous work (Watterson et al., 2016) used this theory to generate curves for a free-flying space robot. More general works, such as that in Belta and Kumar (2002), generated trajectories for rigid bodies based on the metric proposed by Zefran et al. (1996), which ensured the associated affine connection obeys the differential structure of rigid-body dynamics. Bullo and Lynch (2001) showed how to compute a basis of trajectories for robots using differential geometry by decoupling dimensions, but requires zero velocity in between each segment of a multi-dimensional motion.

Polynomials and rational splines are frequently used for optimization on Euclidean spaces $R^{n}$ as part of the algorithms in Mellinger and Kumar (2011), Zucker et al. (2013), Richter et al. (2013), Liu et al. (2017), Flores (2007), and Oleynikova et al. (2016). There are more specialized methods for modeling Bézier curves on manifolds using geometric constructions from Park and Ravani (1995) applied to rigid-body motion. More recently, Popiel and Noakes (2007) proved properties of this construction and showed how to construct splines with this method. These are recursive definitions for Bézier curves, but the derivatives do not have the same convenient knot properties as the Euclidean case such as the end velocity being a linear function of the control points.

Trajectory optimization with obstacle avoidance is a well-studied problem on $R^{3}$ . Using convex polyhedra to model the free space has been performed by Deits and Tedrake (2015) and Liu et al. (2017) with mixed integer programming and quadratic programming, respectively. Schulman et al. (2014) used a collision cost function with sequential convex programming. Others (Gao and Shen, 2016; Zhu et al., 2015) used spheres to model free space with convex programming to generate dynamically feasible trajectories. Some methods (Oleynikova et al., 2016; Richter et al., 2013) used collision data directly from an occupancy grid during the optimization process, but are not guaranteed to converge to a feasible solution in polynomial time.

CHOMP (Zucker et al., 2013) formulates a trajectory optimization problem on Hilbert spaces rather than Riemannian manifolds and uses variational calculus to compute gradients. Collision-free constraints are enforced by adding a term to their cost functional similar to barrier functions (Bemporad and Morari, 2000). A philosophical difference between that work and ours is that we focus on manifolds that are directly parameterizable, but require multiple parameterizations ( $SE (n)$ , $S^{n}$ , $R P^{n}$ ) as opposed to parameterizations which have one chart in $R^{n}$ , but have complicated constraints (e.g., serial chain manipulators with task space collision avoidance). The construction of our algorithm ensures that the optimization does not have to enforce non-linear equality constraints unlike the approach of Zucker et al. (2013). This is important because optimization iterations remain implicitly on the manifold and do not have to be projected back. This avoids issues with the projection function needing to be defined and will save computation if this method is applied to a larger number of dimensions than in examples we look at. (Jaillet and Porta, 2013) formulates RRT^★ on a manifold using random sampling, but their construction of manifold charts requires iterative projecting using Newton’s method.

Perception aware planning (Costante et al., 2016) for vision-based navigation has been explored with a variety of approaches. Chen et al. (2011) summarizes a large variety of works from before 2010. Some relevant works from before and after then include Zhang et al. (2009), who used an extension of a probabilistic road map (PRM) that incorporates an information-theoretic model to improve observation of landmarks during path planning, and Preiss et al. (2017) who used an observation model to generate collision-free trajectories to maximize the convergence of a monocular visual–inertial estimator. Maintaining a level of features in view through a re-planning framework was done by (Sadat et al., 2014).

The main contributions of this work are:

a manifold trajectory optimization algorithm (for example see Figure 1), that optimizes without projecting to the manifold from a higher-dimensional embedding, is parameterization invariant, and is formulated without equality constraints;

a method for free space constraints via convex sub-approximation on coordinate charts;

an application of this method for navigation with constrained-field-of-view visual estimation;

an application of this method for trajectory generation on $SO (3)$ with obstacle avoidance;

an application of this method for trajectory generation on $SE (3)$ with experimental results for quadrotor swarms in fixed formations.

Fig. 1.

Example two-dimensional manifold embedded in $R^{3}$ with a trajectory $γ$ in red.

This article is organized with a background section then four main sections. We describe the manifold optimization and safe corridor algorithm, then we describe how to use our method on the example of $R^{3} \times S^{2}$ for the application of a quadrotor with a gimbal. We discuss an example of how to plan trajectories on $SO (3)$ using this method. We present results with experimental hardware using this trajectory generation algorithm. We finally present the application of this method to a swarm of quadrotors.

This article is an extended version of Watterson et al. (2018). We have extended the description of our optimization formulation and variable marginalization method. In addition, we have applied this work to $SE (3)$ with an example of planning for a swarm of quadrotors and experimentally demonstrated the flexibility of this approach.

2. Mathematical Background

We briefly review a few important definitions and properties of Riemannian manifolds that are critical to this article. For a more detailed explanation of these concepts, please refer to Gallier and Quaintance (2017).

Manifold: A manifold $M$ is an n-dimensional subset of Euclidean space $R^{m}$ with $n \leq m$ that is locally homeomorphic to $R^{n}$ .

Chart: We define a chart as a function $φ : R^{n} \to M$ that is a local n-dimensional parameterization of the manifold. As the manifold is homeomorphic to $R^{n}$ , $φ$ is locally invertible. If $φ$ and $φ^{- 1}$ are both $C^{\infty}$ , we say the manifold is a smooth manifold. For the scope of this work, we assume all manifolds to which we apply our formulation are smooth. Common examples of smooth manifolds are $R^{n}$ , $SO (n)$ , $SE (n)$ , $S^{n}$ , and $R P^{n}$ . As a consequence of the homeomorphic property, we can find a chart $φ_{p} : R^{n} \to M$ centered at p such that a ball of radius $ϵ$ around the origin is homeomorphic to an open subset of $M$ containing p for every $ϵ \in (0, δ)$ for some $δ > 0$ . A chart with this property is drawn in Figure 2. This property is important for trajectory planning because for a small enough curve $γ : [0, 1] \to M$ , we can always find a chart $φ_{γ}$ and function $η : [0, 1] \to R^{n}$ such that $φ_{γ} ° η = γ$ . It is important to note that, for a curve $γ$ with arbitrary length, it is not always possible to find a parameterization that only spans one chart.

Fig. 2.

A chart centered at p, which is homeomorphic over a domain of a ball of radius $δ$ .

Vector on a manifold: A vector on a manifold at a point p is a derivative quantity that is tangent to the manifold at p. As our manifold is n-dimensional, the differential structure lives in a space homeomorphic to $R^{n}$ . For our purposes, it is fine to just use $V = \overset{\cdot}{η}$ as a vector, and not worry about coordinate free ways to define the vector.

Metric: A metric is a defined inner product 〈.,.〉 between vectors on a manifold. For example, the magnitude of the velocity of a curve in coordinates with $V = \overset{\cdot}{η}$ is

〈 V, V 〉 = \sum_{i j} g_{i j} V_{i} V_{j}

(1)

with $g_{ij}$ being a function of the point on the manifold. For a single chart $φ$ that is a function of the variables $x_{1} \dots x_{n}$ , we can default to using the metric:

g_{ij} = \sum_{l} \frac{\partial φ_{l}}{\partial x_{i}} \frac{\partial φ_{l}}{\partial x_{j}}

(2)

If we want to have our metric make physical sense for a dynamical system, we can choose an appropriate $g_{ij}$ that are compatible with the chosen manifold (Bullo and Lynch, 2001; Zefran et al., 1996).

Riemannian manifold: A manifold, along with a metric, is called a Riemannian manifold. We assume that all manifolds are Riemannian and thus our formulation will require having a metric.

Covariant derivative: To calculate higher derivatives of vectors, we use the covariant derivative ∇, which defines the derivative of vectors along each other. The acceleration of a curve on the manifold is defined as $\nabla_{V} V$ . We can also chain the ∇ operator to define jerk and higher derivatives:

\underset{n \nabla}{\underset{︸}{\nabla_{V} \nabla_{V} \nabla_{V} \dots \nabla_{V} V}} = \nabla_{V}^{n} V

(3)

In coordinates $ξ$ , these are evaluated as

\nabla_{V} V = {\overset{\cdot\cdot}{ξ}}_{k} + \sum_{ij} Γ_{ij}^{k} {\overset{\cdot}{ξ}}_{i} {\overset{\cdot}{ξ}}_{j}

(4)

\nabla_{V} \nabla_{V} V = {\overset{\cdot\cdot\cdot}{ξ}}_{k} + \sum_{ij} Γ_{ij}^{k} {\overset{\cdot}{ξ}}_{i} ({\overset{\cdot\cdot}{ξ}}_{j} + \sum_{lm} Γ_{lm}^{j} {\overset{\cdot}{ξ}}_{l} {\overset{\cdot}{ξ}}_{m})

(5)

Christoffel symbols: The $Γ_{ij}^{k}$ in the above equation are called the Christoffel symbols. We can compute them from the metric with $g^{ij}$ being the ith, jth element of the matrix $G^{- 1}$ if the matrix G elements are $g_{kl}$ :

Γ_{ij}^{k} = \frac{1}{2} \sum_{l} g^{kl} (\frac{\partial g_{jl}}{\partial x_{i}} + \frac{\partial g_{il}}{\partial x_{j}} - \frac{\partial g_{ij}}{\partial x_{l}})

(6)

Coordinate invariant: A mathematical object in relation to a manifold is coordinate invariant if its value is the same regardless of the chart or basis that is used to describe it. For some of the following definitions it is not obvious why the values are independent of the coordinate chart. See the more detailed reference for proofs of this critical property (Gallier and Quaintance, 2017). For example, the components in $\nabla_{V} V$ in Equation (4) depend on the coordinate chart $φ$ as we implicitly choose a basis for the vector V. However, the value $〈 \nabla_{V} V, \nabla_{V} V 〉$ is independent of the coordinate chart. We note that constraints of the form $\nabla_{V} V = 0$ are also coordinate free because there is a linear transform L between any two sets of basis vectors and $L (0) = 0$ .

Coordinate free: Coordinate-free expressions are ways of denoting quantities that are coordinate invariant. For example, $\nabla_{V} V$ is coordinate free, but its evaluation on the right-hand side of Equation (4) is not because those quantities depend on a specific set of coordinates. In addition, there are expressions in dynamics that are coordinate free because they do not depend on choosing a basis to represent coordinate frame.

Parallel transport: On $R^{n}$ , vectors can be translated around while their magnitude and direction stays the same. As a manifold is a subset of $R^{n}$ , a vector tangent to the manifold at one point will not necessarily stay tangent if it is simply translated around. Thus, we have the concept of parallel transport, which describes how vectors are equivalent between different tangent locations on a manifold. For a path $γ$ and vector field X, the parallel transport equation is

\nabla_{\overset{\cdot}{γ}} X = 0

(7)

Solving this system of differential equations gives a parallel vector at each point along the path. This transport of the vector is, in general, not independent of the path between two points on the manifold.

Geodesic: The shortest path between two points on a manifold can be found by solving the system of differential equations $\nabla_{V} V = 0$ with the boundary condition of the two points. This equation describes a curve that is parallel to itself.

3. Regional Inflation by Line Search

The Regional Inflation by Line Search (RILS) algorithm (Liu et al., 2017) computes collision-free polygons around collision-free line segments (Figure 3). The line segment is used to define the major axis of an ellipsoid that is inflated until it is tangent to enough obstacles that it cannot grow while staying collision free. At all points of tangency of the ellipsoid, we can find half-planes that are tangent to the ellipsoid. If all obstacles outside those half-planes are removed, we can continue inflating the ellipse while it is collision free. This process repeats until we have a set of half-planes that define a collision-free polygon in the environment.

Fig. 3.

RILS computes convex collision-free polygons by inflating an ellipsoid around a series line segments and using it to find half-planes that define the polygons. If obstacles outside a half-plane of tangency are removed, the ellipsoid can continue to grow.

4. Trajectory generation on manifolds

4.1. Algorithm design and problem scope

We formulate the free space constraints on a chart and not intrinsically for several reasons: (1) a convex region on a chart may describe a larger region than a convex set on a manifold and (2) is easier to transform into a numerical optimization problem. For example, a region of $S^{2}$ larger than a hemisphere is never convex using an intrinsic definition of convexity, but can be convex in stereographic coordinates.

With a differential-geometry-based formulation, we would like several properties: (1) to be independent of coordinates, (2) to be calculable, and (3) to be physically meaningful. Although (3) might seem difficult to do without a specific manifold, our formulation actually allows for it. We use a class of functionals from Zefran et al. (1996) that depend on a metric for the manifold. As in Belta and Kumar (2002), these can be used to describe physical dynamics, for example when the manifold is $SE (3)$ , the metric can be chosen so that the covariant derivative of a curve along itself corresponds to translational and rotational accelerations.

Along with the background in the previous section, our formulation also assumes we have a function $C : M \to {True, False}$ , which determines whether a point p is in $C^{free}$ . This collision checking does not appear in the work of Bullo and Lynch (2001) or Belta and Kumar (2002). We can use this function, along with a coordinate chart, to check the collision of a value in $R^{n}$ . Our formulation is

\begin{matrix} min_{ξ} \int_{0}^{T} 〈 \nabla_{V}^{n} V, \nabla_{V}^{n} V 〉 dt \\ s . t . ξ (0) = ξ_{N} \\ ξ (T) = ξ_{F} \\ ξ \in C^{free} \\ \overset{\cdot}{ξ} = V \\ \nabla_{V}^{k} V (0) = N_{k} k = 1 \dots p \\ \nabla_{V}^{k} V (T) = F_{k} k = 1 \dots p \\ 〈 \nabla_{V}^{k} V, \nabla_{V}^{k} V 〉 \leq L_{k} k = 1 \dots p \end{matrix}

(8)

where $L_{k}$ is a set of scalar Limits on the velocity, acceleration, etc., up to the pth derivative, $N_{k}$ and $F_{k}$ are vector boundary conditions on the iNitial and Final derivative constraints of the trajectory and $ξ_{N}$ and $ξ_{F}$ are the iNitial and Final points on the manifold. We note that these conditions are coordinate invariant in the sense that, if their value is given in one chart, they can be transformed into a different chart to give the same meaning on the manifold even if their value is different. In the case that they are homogeneous ( $N_{k} = 0$ and $F_{k} = 0$ ) those constraints are the same in any coordinate chart as stated in the previous section. The $ξ \in C^{free}$ constraint will be detailed in the next section.

The cost functional is coordinate free and a extension of the cost functionals used in Watterson et al. (2016) and Belta and Kumar (2002) generalized to arbitrary derivatives. Alternatively it can be seen as a generalization to arbitrary manifolds of the cost functional used in Richter et al. (2013), Liu et al. (2017), Mellinger and Kumar (2011), and Deits and Tedrake (2015).

4.2. Algorithm overview

We propose a new safe corridor on manifolds (SCM) algorithm that can be interpreted as a generalization of the safe flight corridor in Liu et al. (2017). The basic idea is as follows: (1) generate a collision-free “optimal” discrete kinematic path from a graph search on the manifold; then (2) form a locally convex corridor around this path in a local parametrization of the manifold around this path, and (3) optimize a trajectory using the set of local parameters around which the convex corridor is described. This basic picture is shown in Figure 4 with a manifold and three regions of the corridor of the corridor with their associated coordinate charts.

Fig. 4.

Constraining equality of spline knot points. With convex polyhedral constraint regions on each chart. Yellow points are equated between adjacent charts. Green points represent $ξ_{N}$ and $ξ_{F}$ .

Instead of optimizing over a full constraint manifold, we optimize a continuous trajectory over a piecewise convex region of the parameter space representation of a manifold. For a convex cost functional, this approximates a non-convex optimization problem with a convex one and thus can be solved quickly and efficiently with global convergence during the optimization phase. We note that the optimization step will return the optimum among all feasible trajectories that both (1) can be expressed using our trajectory parameterization and (2) satisfy the corridor constraints of the path returned by graph search.

In comparison with existing work, this method is substantially different. Unlike rapidly exploring random trees (RRT)-based methods, ours is designed to be resolution complete, which will find a feasible solution in finite time. With this difference, a set of problems can be designed to make our method converge arbitrarily faster than as shown in Watterson and Kumar (2015). As explained in the introduction, we are solving a different problem than that solved by Zucker et al. (2013), which required an embedding of a manifold to be used directly and does not account for use of multiple charts. Other work on perception-aware planing is not presented as general to smooth manifolds like our work is.

4.3. Path finding on manifolds

Collision-free path finding is a much easier problem than finding collision-free dynamically feasible trajectories (position + higher-order constraints) especially because finding a dynamically feasible trajectory in a n-dimensional configuration space for a dth-order system requires search on $n \cdot d$ dimensions of state space. As graph search algorithms are $O (v \log v + e)$ , runtime for v vertices and e edges, the runtime of the search will be at least $O (nd 2^{nd})$ assuming each dimension produces a constant number of nodes. For even a small number of dimensions (e.g., five), the bottleneck of the trajectory generation algorithm is in the graph search. Thus, for tractability, it is better to reduce the dimensionality of search as much as possible. In state-of-the-art RRT search-based methods (Karaman et al., 2011), (LaValle, 2006) over very high dimensions (Shkolnik and Tedrake, 2009), the search is biased towards a much lower-dimensional manifold to achieve reasonable runtime performance. The worst-case runtime of those algorithms was not considered in their work. Owing to their probabilistic nature, they cannot guarantee convergence in finite time.

Instead, we can construct a graph on a manifold with a series of points on the manifold connected with edges that are geodesics. With such a graph, we assume the edges are non-overlapping, thus we can precompute this graph on an arbitrary manifold with a grid or uniform sampled points by connecting all pairs of points with geodesic edges and then whenever two geodesics intersect, we remove the shorter of the two. As long as the points are uniformly spaced on the manifold, searching over this graph will cover the entire manifold up to a small discretization error. This is the same notion as searching over a discrete grid on $R^{n}$ instead of the full continuous space. This graph construction can also be done in a more structured way, as we will do with $S^{2}$ , because there are easy constructions for geodesic grids. There are also versions of structured grid constructions on $SO (3)$ used in Yershova et al. (2010). Performing graph search on this manifold grid is done using the geodesic length as the edge length and the $A^{★}$ heuristic. No modification is needed for $A^{★}$ because graph search only needs nodes and edges and does not care how it is embedded in a manifold. In addition, the graph construction and search can be done in parallel to avoid building up large mostly unexplored graphs in higher dimensions.

4.4. SCM

For any path $H = (h_{1}, \dots, h_{m})$ of points on the manifold that are sufficiently close to each other, we can find a corresponding sequence of charts with one chart $φ^{i}$ centered at each $h_{i}$ . The RILS algorithm (Liu et al., 2017) computes a convex polyhedron around a line segment in three dimensions. We can extend this by finding a collision-free polyhedron on each domain of each chart and adding them to a list of polyhedra $P$ . This is done by creating a voxel grid on the parameter space, then checking collision of all the voxels through the composition of $C ° φ_{i}$ . With an $R^{n}$ occupancy grid, we perform an inflation around the line from the nominal path $l (0, φ_{i}^{- 1} φ_{i + 1} (0))$ using n-dimensional RILS. The n-dimensional version is the same as the three-dimensional version, except the ellipsoids are expanded about extra minor axes during the inflation step. Finally, the set of charts is pruned if the geodesic between the center of adjacent charts is collision free and less than a distance of $δ$ . In general, increasing $δ$ will make the corridor narrower with fewer polyhedra in tight clutter, which trades off between speed and trajectory quality. For too small $δ$ , the corridor will have too many polyhedra and the time horizons will be too short for a good optimization result.

4.5. Optimization on manifolds

To numerically solve the optimization problem in Equation (8), we need to convert an infinite-dimensional functional optimization to a finite-dimensional one. In the literature, this is done by either using splines (Deits and Tedrake, 2015; Flores, 2007; Liu et al., 2017; Mellinger and Kumar, 2011; Richter et al., 2013; Watterson et al., 2016) or using a discrete time optimization (Bemporad and Morari, 2000). It is well known that splines can approximate arbitrary continuous functions within a given $ϵ > 0$ (Nocedal and Wright, 2006). Converting inequality constraints on a trajectory into inequality constraints on spline parameters is more involved than that same conversion in the discrete time case, but requires far fewer optimization variables.

Computing the integral in Equation (8) cannot be done in closed form for a general manifold. We can compute an approximation of the integral using Gaussian quadrature to approximate it as a sum of the functional evaluated at some $t_{i}$ with weights $w_{i}$ , which are found in Press (1992). For some Riemannian manifolds with enough $w_{i}$ , this will actually be exact, such as in the case of $R^{n}$ :

\int_{0}^{T} f (t) dt \approx \sum_{i} w_{i} \cdot f (t_{i})

(9)

We choose to represent the trajectory as a polynomial spline in the parameter space of the charts $φ^{i}$ we found in Algorithm 1. Each chart has an associated polynomial $p_{i} (s) : (0, 1) \to R^{n}$ , which corresponds to the ith segment of the trajectory with normalized time. This is represented as a sum of basis polynomials $b_{j} (s)$ and coefficients $α_{ij} \in R^{n}$ . To evaluate the trajectory, there is also an associated time interval for each polynomial $Δ_{i} \in R^{+}$ :

\begin{matrix} p_{i} (s) = \sum_{j} α_{ij} b_{j} (s) \\ ξ_{i} (t) = p_{i} (\frac{t - t_{i}}{Δ_{i}}) \\ (t_{i + 1} = t_{i} + Δ_{i}, t_{0} = 0) \end{matrix}

(10)

Algorithm 1. Calculating the safe corridor on a manifold. Will return a set of charts

Φ

and a set of polyhedra

P

(Φ, P) \leftarrow {}

H \leftarrow Manifold

A^{☆}

(p_{N}, p_{F})

Path

3: for

h_{i} \in H

4: Find

φ^{i}

, such that

φ^{i} (0) = h_{i}

Φ \leftarrow {Φ, φ^{i}}

P \leftarrow {P, RILS ((φ^{i})^{- 1} ° Geodesic (φ^{i} (0), φ^{i + 1} (0)))}

7: end for

8: for

(φ^{i}, P_{i}) \in (Φ, P)

9: if

φ^{i} (Line (0, (φ^{i})^{- 1} ° φ^{i + 1} (0))

is collision free and

d (φ^{i} (0)), φ^{i + 1} (0)) < δ

then

10: Prune

(φ^{i}, P_{i})

11: end if

12: end for

13: return

(Φ, P)

The reason we keep the polynomials evaluated in the interval $[0, 1]$ as opposed to $[0, Δ_{i}]$ is for numerical stability. For example, Richter et al. (2013) found numerical issues in Mellinger and Kumar (2011) with polynomials of degree greater than nine, which we partly attribute to this difference in modeling.

With each polynomial being on a different chart, we need to explicitly constrain the knot points to be continuous up to a certain derivative. The endpoint constrained basis is a good choice because the derivatives at the knot points are exactly the optimization variables. Using the map $(φ^{i})^{- 1} ° φ^{i + 1}$ , we can express equality in terms of the coefficients $α_{ij}$ . The picture of these constraints are shown in Figure 4. If we use a higher degree polynomial than twice the degree of continuity, we can ensure that the constraints on the ith knot point share no variables with the $i + 1$ th knot point. Owing to this, we can replace variables in expressions for the knot points using the map $(φ^{i})^{- 1} ° φ^{i + 1}$ . Doing so will eliminate all equality constraints on the optimization. In Richter et al. (2013), they do this for the quadratic program case and provide a more detailed explanation of the endpoint-constrained basis.

To confine continuous trajectories inside each polyhedron, we use the property that the convex hull of the control points of a Bézier curve is a conservative bound on the convex hull of the curve. As any two polynomial bases are interchangeable via a linear change of coordinates, this bound can be enforced with linear inequality constraints (Preiss et al., 2017; Watterson et al., 2016).

Thus, the full optimization is the non-linear program:

\begin{matrix} min_{α, Δ} & f (α, Δ) \\ s . t . & g (α, Δ) \leq 0 \end{matrix}

(11)

In general, this problem is non-convex in ( $α, Delta$ ). Here $α$ is a subset of the basis coefficients describing the trajectory and $Δ$ is a set of time durations corresponding to the duration of each segment of the spline. The number of free $α$ parameters equals the number of coefficients in the spline minus the number of equality constraints in (8) and number of knot points for each dimension in Figure 4. After optimizing, we can solve for the remaining $α$ parameters using the non-linear chart transition functions.

4.6. Optimization method

We can solve the optimization problem in Equation (11), with a constrained search method. We implemented the primal–dual Newton’s method with the centering heuristic as detailed in Bemporad and Morari (2000). Changing the variable initialization did not seem to affect convergence, so all values were set to 1. The full optimization in Liu et al. (2017) is usually not convex. In practice, it is straightforward to find approximately optimal $Δ$ values using a heuristic proposed in Liu et al. (2017). With fixed $Δ$ , optimizing only over the $α$ values is convex for suitable manifolds.

We can model polynomials of both $α$ and $Δ$ as multi-dimensional polynomials with integer powers. As shorthand, we concatenate $x = [α, Δ]$ :

f (x) = ζ \sum_{i \in I} [β_{i} \underset{j \in J_{i}}{Π} x_{ij}^{q_{ij}}]

(12)

where I and $J_{i}$ are index sets, $ζ, β_{i} \in R$ are coefficients, and $q_{ij} \in Z$ are the integer powers.

The cost functional and inequality constraints are, in general, non-linear functions of these polynomials. In our vision-based example, the cost will be a rational function of these polynomials.

We assume we can calculate the gradients and Hessians of functions of polynomials using the chain rule.

For optimization problems, we make the fundamental assumption that the bottleneck of the optimization process is inverting a matrix of hundreds to thousands of variables to compute a search direction. Based on the structure of our formulation, this a fairly sparse matrix because each segment’s variables are independent from each other except for the dependency introduced by the continuity constraints. For example, variables relating the first segment and third segment will have no cross terms in the matrix formed in the Bemporad and Morari (2000) method. Thus, the number of non-zero elements in this matrix is $O (m \cdot n^{2})$ as opposed to $O (m^{2} \cdot n^{2})$ if it were dense, for optimizing an nth-degree polynomial with m segments.

When we reduce the number of variables using a manifold based approach, this sparsity property remains the same. In addition, with there being fewer variables we eliminate the need to perform the projection step, which, in general, needs to be done iteratively (Janson and Pavone, 2013).

4.7. Nested gradients and Hessians

We can further improve the computational efficiency of our trajectory generation implementation through usage of the formulas of (Faà di Bruno, 1855), which are the multivariate versions of the chain rule:

\frac{\partial}{\partial x_{i}} f (y (x)) = \sum_{k} \frac{\partial f}{\partial y_{k}} \frac{\partial y_{k}}{\partial x_{i}}

(13)

\frac{\partial^{2}}{\partial x_{i} \partial x_{j}} f (y (x)) = \sum_{k} \frac{\partial f}{\partial y_{k}} \frac{\partial^{2} y_{k}}{\partial x_{i} \partial x_{j}} + \sum_{k, l} \frac{\partial^{2} f_{k}}{\partial y_{k} \partial y_{l}} \frac{\partial y_{k}}{\partial x_{i}} \frac{\partial y_{l}}{\partial x_{j}}

(14)

This reduces the number of terms needing to be calculated when evaluating the gradients and Hessians needed by the optimization algorithm. We leverage dynamic programming to evaluate all terms (Kleinberg and Tardos, 2006). This is illustrated in Figure 5 where we have drawn function composition as arrows.

Fig. 5.

Marginalization scheme used to eliminate non-linear equality constraints in our optimization. Using a graph representation, we can calculate all gradients and Hessians of Equation (11) recursively using Equations (13) and (14).

From an initial set of variables drawn in orange corresponding to three segments, we find the subset that are defined by the initial and final conditions, as well as the subset that can be defined by chart transition functions, and remove those variables from the optimization. The remaining set of optimization variables is shown in blue. We can then construct an array of new variables in green, which correspond to the original set in orange, but are described as functions of the set of remaining blue variables. Finally, we can use these nested variables in our cost and inequality constraints directly.

In this section, we have proposed a trajectory optimization method for Euclidean manifolds that is computed without equality constraints, that ensures the optimization never needs to project back to the manifold. Using a collision map on the manifold, we show how we can add collision constraints with convex sub-approximation on a series of coordinate charts found by our algorithm.

5. Field-of-view-constrained trajectory optimization details

With the algorithm developed in the previous section, we now look at an example planning problem that arises when trying to control a quadrotor with a visual–inertial navigation system (VINS) in an environment lacking texture in some areas.¹ We would like to keep areas conducive to estimation inside the robot’s field of view, while quickly navigating through a cluttered environment. We apply our arbitrary manifold optimization to $R^{3} \times S^{2}$ . When planning for vision-based navigation, we look at a system in which we can control the view direction of the camera on $S^{2}$ . This is analogous to controlling the pan–tilt of a hardware gimbal. In our experimental setup, we emulate this in software by cropping the appropriate region of a wide-angle view based on the orientation of the robot. We assume, for sufficiently slow trajectories, the robot orientation is close enough to hover so the desired cropped image is within the wide-angle lens. In practice, we can rescale the time of trajectory to fit this even with the aggressive motions. We choose this, and not $SO (3)$ , because it has lower dimension and modeling the viewing region of a camera as a cone has some convenient geometric properties, which we can exploit with stereographic coordinates.

5.1. Vision model

The standard projection equation of a point $[X_{i}, Y_{i}, Z_{i}]^{T}$ in normalized image coordinates $(p_{x}, p_{y})$ for a camera whose orientation is R and position is $[x, y, z]^{T}$ measured in an inertial frame is

\frac{1}{λ} [\begin{matrix} p_{x} \\ p_{y} \\ 1 \end{matrix}] = R ([\begin{matrix} X_{i} \\ Y_{i} \\ Z_{i} \end{matrix}] - [\begin{matrix} x \\ y \\ z \end{matrix}])

(15)

We use this equation to model whether a known feature in world coordinates is in the field of view of the camera. We also assume that a feature is only useful for the navigation system if it is closer than a max depth in the camera frame.

The optical flow is a function of a point in pixel coordinates:

\overset{\cdot}{p} = [\begin{matrix} {\overset{\cdot}{p}}_{x} \\ {\overset{\cdot}{p}}_{y} \end{matrix}] = [\begin{matrix} \frac{p_{x} \cdot \overset{\cdot}{z} - \overset{\cdot}{x}}{λ} \\ \frac{p_{y} \cdot \overset{\cdot}{z} - \overset{\cdot}{y}}{λ} \end{matrix}] + [\begin{matrix} p_{x} p_{y} & - {(1 + p_{x})}^{2} & p_{y} \\ {(1 + p_{y})}^{2} & - p_{x} p_{y} & - p_{x} \end{matrix}] ω

(16)

The front-end of visual odometry has decreased accuracy and increased computational load with increasing optical flow. With Equation (16) and assuming known ranges on the depth of features and pixel coordinates, we can conservatively upper bound the optical flow with a linear function of the camera’s linear and angular velocities. Thus, we choose $\nabla_{V} V$ as the derivative we minimize for the $S^{2}$ part of the cost functional.

5.2. Field-of-view formulation

For vision-based navigation, we would like a cost function that produces a trajectory that is good for the estimator. Paramount to a VINS estimator is maintaining track of all optical features. This tracking can be lost two ways: (1) by lack of features, and (2) by an excessive rate of optical flow. We ensure (1) by keeping a minimum number of visible features in the direction of camera z with a region constraint on $S^{2}$ . The tracking of features is roughly symmetric with respect to rotations about the camera z axis, so we consider a feature to be in the field of view if it is inside a conical region that is a circular region on $S^{2}$ . To limit (2), we minimize $\nabla_{V} V$ or V, which will minimize the angular and linear acceleration or velocity, respectively, as seen in Equation (16).

5.3. Stereographic coordinates For $S^{n}$

In this section, we look to find a chart function generator $Φ$ , that can find a chart on the sphere from a rotation matrix R. Let $P \in S^{n} \subset R^{n + 1}$ be a point on the sphere. For an arbitrary matrix $R \in SO (n + 1)$ , we define a set of projective coordinates with $R e_{1}$ being the singular point on $S^{n}$ . Let $p \in R^{n}$ be the stereographic coordinates. If $e_{i}$ is a vector that is all $0$ , except the ith element, which is 1, we can define a chart $φ$ with the functions:

ξ = p_{1} R e_{2} + p_{2} R e_{3} + \cdot + p_{n} R e_{n + 1} - R e_{1}

(17)

P = \frac{2}{∥ p ∥^{2} + 1} ξ + R e_{1}

(18)

These equations can be derived from geometric interpretation of stereographic coordinates in Figure 6 or in Milnor (1978). The inverse $φ$ can be shown to be

P' = \frac{P}{1 - P \cdot R e_{1}}

(19)

p = (P' \cdot R e_{2}, P' \cdot R e_{3}, \dots, P' \cdot R e_{n + 1})

(20)

Fig. 6.

Geometric interpretation of a set of stereographic coordinates for a chart centered at the point $- R \cdot e_{1}$ . The second to $n + 1$ th columns of R form an orthonormal basis of the hyperplane tangent to the sphere at the chart center. The stereographic point P is the intersection of the sphere along the dashed line connecting $R \cdot e_{1}$ and a point on this hyperplane.

The maps $(φ^{2})^{- 1} ° φ^{1} : R^{n} \to R^{n}$ can be computed with

\hat{ξ} = \frac{2}{∥ p ∥^{2} + 1} (p_{1} e_{2} + p_{2} e_{3} + \dots + - e_{1}) + e_{1}

(21)

((φ^{2})^{- 1} ° φ^{1})_{k} (p) = \frac{e_{k + 1}^{T} R_{2}^{T} R_{1} \hat{ξ}}{1 - e_{1}^{T} R_{2}^{T} R_{1} \hat{ξ}}

(22)

5.3.1. Christoffel symbols on $S^{n}$

To be able to calculate the Christoffel symbols, we use the standard $R^{n + 1}$ metric tensor that corresponds to a $L_{2}$ norm. This tensor is shown in Equation (2), and results in

g_{ij} = {\begin{matrix} \frac{4}{{(1 + ∥ x ∥^{2})}^{2}} & i = j \\ 0 & otherwise \end{matrix}

(23)

With x being the point in coordinates. The metric is independent of the $SO (n)$ rotation in the stereographic coordinates, which can be shown by seeing that Equation (18) is linear in R and then cancels out during the multiplication in Equation (2).

We can use Equation (6) to calculate symbols for the metric induced by $R^{n + 1}$ on the sphere. With stereographic coordinates, these are

Γ_{ij}^{k} = \frac{2}{(1 + ∥ x ∥^{2})} \cdot {\begin{matrix} x_{k} & i \neq k and j \neq k and i = j \\ - x_{j} & i = k \\ - x_{i} & j = k \\ 0 & otherwise \end{matrix}

(24)

We note that these symbols are independent of the value of R. This can be proved by considering the rotational symmetry of the sphere or algebraically. We can see that these symbols are torsion free $(Γ_{ij}^{k} = Γ_{ji}^{k})$ .

5.4. Search on $S^{2}$

Generating a uniform regular grid on a sphere in Figure 7, can only be done when the nodes are one of the five platonic solids (Yershova et al., 2010). To approximate a grid on the sphere with more nodes, we use the well-known geodesic polyhedron construction. We start with an icosahedron and sub-divide each triangular face into $n^{2}$ triangles and then project the vertices onto the unit sphere. If we then take the dual of this shape by interchanging vertices with edges, we end up with a graph that almost uniformly covers the sphere and has the property that each node has three neighbors everywhere on the sphere.

Fig. 7.

Example graph of nodes (red circles) and associated edges (geodesic segments) on $S^{2}$ based off the icosahedron. Note how each of the 20 nodes corresponding to faces of the icosahedron have exactly 3 edges extending from them.

With a graph on the sphere, we can use $A^{★}$ with the geodesic length representing the arc length between adjacent nodes. To check whether or not a given node has enough features, we need to calculate how many features are within view from that point. To do this, we partition the features between a minimum and maximum distance from the point in $R^{3}$ and then use the property of stereographic coordinates in the next section.

5.5. Circles to circles

In this section, we leverage a property of stereographic coordinates to improve the computational efficiency of our collision checks. It is known that a circle on $S^{2}$ is a circle in stereographic coordinates (Figure 8), with some center but a different radius. Thus, when we model our visual constraint with a cone, we can transform all of the point into stereographic coordinates and then check to see how many of them lie within a given circle. Since this calculation needs to happen many times for the corridor creation, calculating this in stereographic coordinates is more efficient than counting features in $R^{3}$ . To be able to compute this, we need to know the relation between the location and radius of a circle on the sphere to the location and radius of a circle in the coordinate chart.

Fig. 8.

A circle on a sphere $S^{2}$ will be a circle in the parameter space $R^{2}$ when using stereographic coordinates. We use this to efficiently determine whether or not a viewpoint has enough features or not during the corridor construction.

We first need to find the transform between center of the circle in the parameter space and center of the circle on the sphere, expressed in the parameter space. These two quantities are actually different except at the identity element.

Exploiting the symmetry when expressing points on a sphere, we look at the point p expressed in polar coordinates $p = (r \cos (θ), r \sin (θ))$ , the radius of the circle in coordinates is $ρ$ with the field of view being $ϕ$ :

ρ = \frac{(r^{2} + 1) \sin \frac{ϕ}{2}}{\cos \frac{ϕ}{2} + 1 + r^{2} (\cos \frac{ϕ}{2} - 1)}

(25)

We can also specify the center of this circle in the parameter space in polar coordinates as $p' = (r' \cos (θ), r' \sin (θ))$

r' = \frac{2 r}{\cos \frac{ϕ}{2} + 1 + r^{2} (\cos \frac{ϕ}{2} - 1)}

(26)

5.6. Experimental results

We have implemented the version of our algorithm on $R^{3} \times S^{2}$ in C++, with ROS handling all of the interprocess communication and Python running some of the high-level control commands. Dynamic feasibility of the trajectory is ensured using the time-scaling method in Richter et al. (2013). We control the position of the quadrotor with the translation. The desired yaw of the robot is the projection of the $S^{2}$ part of the trajectory onto the plane perpendicular to gravity. We convert these into real-time control commands by Mellinger and Kumar (2011). The depth sensor was used to build up a collision occupancy grid at the same time as the vision cameras build up a map of visual features. During the map creation phase, stereo visual inertial odometery was used to provide more accurate ground-truth locations of features in the environment. The region of interest planned with the $S^{2}$ part of the state is converted into a cropped region of a wide-angle-lens camera image. This is to simulate a hardware-controlled gimbal in software, similar to the system found in commercial platforms.

The trajectory generation done was performed off-line on an Intel i7 3.4 GHz processor with a single-threaded implementation. The computation time is bottlenecked by the graph search and corridor generation, but in total was less than 30 s for multiple sets of start and end locations. Searching over a five-dimensional space with $A^{★}$ takes a while because of the large number of nodes in the graph. Computing the collision on the spherical part of the state for many features is slow because it requires checking all the features in the vision map at each node expansion. We believe there are ways to improve the computational performance of this step, but they are outside the scope of this work because we are not targeting on-line applications.

We used a monocular version of the multi-state constraint Kalman filter (MSCKF) (Mourikis and Roumeliotis, 2007) as a benchmark for the visual–1inertial odometry. We set up an environment with feature-poor obstacles as shown in Extension 1. We compare this trajectory generation method with that of Liu et al. (2017), which does not take into account that the obstacles have no features on them. This is in contrast to our method, which we set to require at least 10 features in the field of view at all times. The performance over several environments is shown in Figure 9, where the distribution of our errors is to the left of that planning without the vision based constraints.

Fig. 9.

Estimator performance summarized from 45 trials.

When planning in the environment in Extension 1 and Figure 11, our algorithm found a corridor consisting of five polyhedrons that are each five dimensional. These are hard to visualize, but we have split them into their projections on the $R^{3}$ and $S^{2}$ part of the state separately in Figure 10.

Fig. 10.

Corridor sections for $R^{3} \times S^{2}$ trajectory consisting of five segments. The top row is the $R^{3}$ representation of the polyhedra plotted with a three-dimensional perspective. The bottom two rows are the $S^{2}$ parts of the trajectory. The middle row is the polyhedron in parameter space and the bottom row is that polyhedron projected onto a sphere and drawn with a three-dimensional perspective.

Fig. 11.

Top down view of the environment used in the $R^{3} \times S^{2}$ experiment and accompanying video (Extension 1).

6. Trajectory generation on $SO (3)$

Now, we show how the same trajectory generation algorithm on manifolds can be applied to generate trajectories on $SO (3)$ . We first need a graph of nodes connected covering $SO (3)$ in which to search for charts. With the work of Yershova et al. (2010), we can use a uniform grid on this manifold using the Hopf fibration. This allows us to use the existing graph we described on $S^{2}$ and take its Cartesian product with uniform samples on $S^{1}$ . As long as the following equation holds for $n_{S^{1}}$ samples on $S^{1}$ and $n_{S^{2}}$ samples on $S^{2}$ , the Cartesian product samples are close to uniform on $SO (3)$ :

\frac{π}{n_{S^{1}}} = \sqrt{\frac{π}{n_{S^{2}}}}

(27)

To generate charts after a path is found on $SO (3)$ , we use the exponential map because this parameterization can be easily extended to other Lie groups (Gallier and Quaintance, 2017). From a point on the path $R_{c}$ , we can locally parameterize $SO (3)$ with the exponential coordinates vector $ξ \in R^{3}$ and the $\hat{.}$ operator:

R \in SO (3) = R_{c} \exp ([\begin{matrix} 0 & - ξ_{3} & ξ_{2} \\ ξ_{3} & 0 & - ξ_{1} \\ - ξ_{2} & ξ_{1} & 0 \end{matrix}]) = R_{c} \exp (\hat{ξ})

(28)

The chart transition function can be found by equating the coordinates from the two adjacent charts and rearranging:

ξ_{1} = \log (R_{1}^{- 1} R_{2} \exp ({\hat{ξ}}_{2}))^{\lor}

(29)

where ∨ is the inverse of the $\hat{.}$ operator (Chirikjian, 2011). The derivative of this guarantees continuity of angular velocity during the chart transition. As it has physical meaning, we can specify that the cost functional is the norm of the angular velocity. This functional has been formulated in the form of Equation (8) by Zefran et al. (1996).

In Figure 12, we show the results of this algorithm on a simple example of reorienting an object. Plotting obstacles in orientation space would clutter the view, but we can see the effect of how the trajectory needs to change to get around a blockage placed in the middle of what would otherwise be the optimal path. The constraint this puts on R is where the body $e_{1}$ axis must not enter a cone:

(R [\begin{matrix} 1 \\ 0 \\ 0 \end{matrix}]) \cdot [\begin{matrix} \frac{1}{\sqrt{2}} \\ \frac{1}{\sqrt{2}} \\ 0 \end{matrix}] \geq \arccos 0.2

(30)

Fig. 12.

Trajectories generated on $SO (3)$ using this method. Orientations are visualized as a box with a red–blue–green coordinate axis and time is visualized by translating the box to the right as it rotates. The start is from identity and the trajectory goes to a rotation that is rotated about the blue axis by 90°. In the obstacle-free case, the optimal trajectory rotates about this axis, which will produce the same result as Park and Ravani (1995) or Belta and Kumar (2002). In the bottom example, we place an obstacle in the middle of the obstacle free path, so the optimized trajectory rotates out of plane around it.

This problem could arise in numerous applications such as reorienting a camera outside while avoiding pointing it directly at the Sun, or avoiding a singular gimbal orientation.

In the obstacle-free version, our method performs slower than the methods of Park and Ravani (1995) or Belta and Kumar (2002), because they provide a closed-form solution, but results in the same trajectory in the case of no obstacles and zero boundary velocities and accelerations. These methods, however, cannot be used in the case with obstacles.

7. Trajectory generation for quadrotor formations

We demonstrate that the manifold-based trajectory generation method can be used to generate trajectories with obstacles on $SE (3)$ unlike our method in Watterson et al. (2016), which only considers obstacles on $R^{3}$ and assumes no collision on the $SO (3)$ part of the state. A swarm of quadrotors flying in a fixed formation while avoiding obstacles is a tangible example that is testable with our testbed capabilities. With obstacles in the space, the orientation of the swarm results in collision of individual robots in the task space. Thus, our collision avoidance of all robots in the task space results in a collision on the formation configuration on $SE (3)$ .

The testbed platform we use is a swarm of CrazyFlie robots. Their small size is ideal for running experiments safely and their popularity provides lots of support for their control. With recent hardware advances, they can now fly fully autonomously without a Vicon system using small on-board sensors, but we use the Vicon system for easier system integration.

7.1. Search on $SE (3)$ with obstacles

The first step of the manifold trajectory generation algorithm in the non-Lie groups section is to find a feasible collision-free kinematic path through the manifold before optimizing a trajectory. Search on $SE (3)$ can be done through search on a joint six-dimensional state on $SO (3) \times R^{3}$ , which requires a graph on both sub-manifolds. On $R^{3}$ , we can simply use an occupancy grid structure, which uses a uniformly sampled grid with each cell being connected to six neighbors. For the graph on $SO (3)$ , we use the method from Yershova et al. (2010) that uses the $S^{2} \times S^{1}$ product Hopf fibration parameterization of $SO (3)$ . As we have an approximately uniform grid on $S^{2}$ from the manifold trajectory generation work, we can add a uniformly sampled $S^{1}$ grid to produce a graph on $SO (3)$ . Each node has five neighbors in this graph: three from the $S^{2}$ grid and two from the $S^{1}$ grid. Yershova et al. (2010) showed how to choose the resolution on both of these grids such that the resultant voxels on $SO (3)$ are sampled with approximately equal area. Searching in six dimensions will take longer than the other examples in this article, which search over a maximum of five dimensions. However, the alternative to search over the dynamics of the robots, which requires jerk to be continuous, takes at least $4 * 6 = 24$ dimensions. This sized state is not currently tractable with state-of-the-art methods unless clever system-dependent projections to lower-dimensional search spaces are used (Shkolnik and Tedrake, 2009).

We can formulate the collision avoidance constraints as a conjugation of the collision avoidance constraints of each robot individually with the obstacles. Like before, we inflate each obstacle in $R^{3}$ based on the radius of each of the robots and treat each robot as a point for collision. If the location of each robot in the formation is $r_{i}$ and the pose of the formation is represented as $(R, d)$ as shown in Figure 13, the collision function of a point $c : R^{3} \to {0, 1}$ can be transformed to $C_{o} : SE (3) \to {0, 1}$ with the following:

C_{0} (R, d) = \lor_{i} c (R \cdot r_{i} + d)

(31)

Fig. 13.

Formation coordinate system.

We also need to consider the downwash constraints of the robots as well because flying on top of each other will cause the control to be unstable (Figure 14). The simplest way to calculate a model to use is to assume a cylindrical shape for the downwash. Two robots will not interfere with each other if their vertical distance is more than $l_{D}$ or their horizontal distance is greater than the robots radii $r_{d}$ .

Fig. 14.

Robots will not be in collision if they are far enough away from each other vertically and horizontally.

The collision function for downwash only depends on the orientation of the formation and not the position (d) :

\begin{matrix} \underset{i, j}{\lor} (‖ [\begin{matrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 0 \end{matrix}] R \cdot (r_{i} - r_{j}) ‖_{2} < l \lor ‖ [\begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 1 \end{matrix}] \\ R \cdot (r_{i} - r_{j}) ‖_{2} < l_{D}) \end{matrix}

(32)

with the total collision function C being

C = C_{o} \lor C_{D}

(33)

With this collision function and graph, we can use the rest of the manifold trajectory generation technique without modification to create a local map for each chart, inflate a polyhedron, and optimize a trajectory within. We use the metric proposed by Zefran et al. (1999) to calculate our cost functional. The last step is to scale the total time of the trajectory to the individual dynamic limits of the individual robots. This can be done by calculating a maximum velocity, acceleration, and jerk of each robot in the formation and then scaling the total time to make it dynamically feasible as done in Liu et al. (2017).

7.2. Experimental validation

We set up three representative example environments for our swarm to navigate that demonstrate the capabilities of $SE (3)$ trajectory generation. For each example, we plan for six quadrotors in a fixed formation with one or more physical obstacles in the environment. We show the robots executing these trajectories with a Vicon motion capture system and off-board position control.

The first environment we test in is shown in Figure 15: the robots are in a plus formation, where each robot is 0.5 m away from the center of the formation in each axis direction. In this formation, the top and bottom quadrotors are outside the downwash region vertically when the formation is at the identity rotation. The environment is composed of a series of poles, which we model as rectangular prisms for collision avoidance. In the obstacle map, the cylinders are extended higher than in the constructed environment, so a solution where the robots fly over the poles is not found. In the visualization, the formation center is plotted as a magenta curve, with the orientation plotted as red, blue, and green lines corresponding to the x, y, and z, formation frame axes, respectively. The individual robot’s locations are plotted as magenta spheres. The obstacles are all plotted as crimson boxes in the visualization.

Fig. 15.

Trajectory generated in first environment with pole obstacles. Red–blue–green axis is the formation state on $SE (3)$ over time. Individual robots are plotted as magenta spheres.

In the first environment, the planned trajectory has the robots going around different sides of the same obstacle as shown in Figures 15 and 16. This is an example of our method producing a trajectory that cannot be planned by planning for only the center of the formation and using a conservative convex collision model.

Fig. 16.

Robots flying trajectory in the first environment.

The second environment, shown in Figures 17 and 18, uses the same formation as in the first environment, but now with rectangular obstacles that the formation needs to avoid. Here, the generated trajectory rotates a little bit to allow the robots on one side of the formation to be a bit closer to the obstacle than would be possible by planning a trajectory that just optimizes the translation of the formation center.

Fig. 17.

Trajectory generated for formation in second environment with wall obstacles.

Fig. 18.

Robots flying trajectory in the second environment.

In the final environment, we plan for a line formation of robots that are distributed evenly 0.5 m apart from each other. Here we demonstrate planning from different initial and final orientations of the formation. As our method generates a trajectory within a local region, the trajectory is in a class of solutions where the bottom right quadrotor must clear the obstacle before the formation has enough space to rotate to the final orientation. As a result, there is a change in the speed of the rotation after the bottom right quadrotor clears the obstacle. We show two different views of this trajectory in Figures 19 and 20 and a freeze-frame from the video in Extension 1 in Figure 21.

Fig. 19.

Trajectory generated for line formation in the third environment with wall obstacles.

Fig. 20.

Alternate view of the third environment trajectory.

Fig. 21.

Robots flying trajectory in the third environment.

In the first and last environment, we ran six trials of the trajectory to analyze the control performance of the robots. In Figures 22 and 23, we plot the $x, y, z$ position error of each robot versus time over the collection of trials. There is some noise in the position of the robots, but they are still able to execute the trajectories without colliding with the obstacles in the environment. One source of error is that the robots are quite lightweight and subject to disturbance from the HVAC system in the room and currents from doors opening. Most significantly, the robots on the outside of the formation in the second environment (fourth and sixth row) have more drag disturbance than the robots in the center of the formation and are closer to the ground during parts of the trajectory. For these tests, we did not consider ground effect, but those constraints could be easily added into this framework.

Fig. 22.

Plot of desired and actual position of each robots during trials of the first environment trajectory. Each row is an individual robot and each column in the $x, y, z$ location of the robot in the world frame.

Fig. 23.

Plot of desired and actual position of each robots during trials of the third environment trajectory.

8. Limitations of approach

There are situations where our approach is not ideal. For systems where the state, cost, and constraints all live on $R^{n}$ , this method is simplified, but the added complexity of the differential geometric formulation would likely hinder implementation. Using differential geometry to formulate an optimization problem can be a potential pitfall as it adds complexity if applied to the wrong problem. In addition, while all parameterizations of a manifold can work with this approach, with obstacle avoidance in the formulation, the global optimality is dependent on the parameterization. Therefore, a different choice of parameterization can lead to poorer results because of a too tight restriction on the domain of the parameterization creating a tighter corridor.

Searching then optimizing restricts our solution to one homotopy class. This can produce sub-optimal results in environments where there are narrow paths that place restrictions on dynamics. For example, a robot will choose to take a narrow shortcut over a safer wider path unless the search heuristic takes path clearance as part of its cost metric. In general, this method is beneficial where there is sufficient room around the kinematically feasible shortest path for a dynamically feasible trajectory to be generated. In situations where the kinematic planning problem is hard, the trajectory generation will have similar performance to following a kinematic path at slow speed.

When computing trajectories, the main advantage of our algorithm is that we transform an unbounded dynamic search to a lower-dimensional kinematic path search, then optimize a trajectory in a local region around that path. The runtime of our entire algorithm is currently bottlenecked by the kinematic search for the representative environments we tested on. This trades off global optimality for an algorithm that runs in polynomial time. For our work, we used a distance-based heuristic to guide the finding of local regions, a possible research extension is to look at how other choices of heuristic affect performance. For example, incorporating other information into the search or optimizing over multiple paths could produce better results in certain environments. When doing tasks such as navigating an office building, forcing the corridor to be in the middle of doorways would be more robust to disturbances than always staying some collision radius away.

When extending to other manifolds and arbitrary environments, it is key to be aware of pitfalls in performance when choosing a manifold parameterization. For common smooth manifolds such as $S^{n}$ or $SE (n)$ used in robotics, choice of parameterizations with close together singularities such as Euler angles, orthographic project, or spherical coordinates, will only add at worst a couple of segments to a path. However, in general, it is always possible to construct a pathological parameterization for any given manifold that is singular over an arbitrarily small domain. For example, composing any parametrization with the function $\frac{1}{ϵ^{2} - ∥ ξ ∥^{2}}$ will make its domain a ball of radius $ϵ$ . In addition, it is always possible to construct an environment that by searching first for a feasible path instead of a global optimum will make this method perform poorly. For example, without penalizing for width in the search heuristic, our method will choose a shorter narrow passageway over a wider one. Once dynamics are incorporated, a longer path may have a lower optimal cost if the shorter path requires stopping to go around a tight corner.

9. Conclusion

We have proposed a trajectory optimization method for Riemannian manifolds that is computed without equality constraints, and ensures the optimization never needs to project back to the manifold. Using a collision map on the manifold, we show how we can add collision constraints with convex sub-approximation on a series of coordinate charts found by our algorithm. We have then applied this method to $SO (3)$ , and an example problem in robotics with planning a robots position on $R^{3}$ and camera view on $S^{2}$ simultaneously. We show that this method improves estimator performance of a monocular visual–inertial system by generating smooth trajectories in position and view direction. We also have further demonstrated the versatility of this algorithm by also applying to an example of planning on $SE (3)$ for quadrotor swarms on a hardware testbed. By keeping the formation fixed, this trajectory generation method scales linearly with the number of robots as opposed to combinatorially if each robot is planned individually. We show several example environments where our robots smoothly avoid obstacles while maintaining formation spacing.

Footnotes

Appendix. Index to multimedia extensions

Archives of IJRR multimedia extensions published prior to 2014 can be found at http://www.ijrr.org, after 2014 all videos are available on the IJRR YouTube channel at http://www.youtube.com/user/ijrrmultimedia

SAGE-Journals-Accessible-Video-Player 10.1177/0278364919891775.M1 sj-vid-1-ijr-10.1177_0278364919891775 Extension 1

a brief overview of our algorithm, visualization of trajectories on SO(3),experiments for the $R^{3} \times S^{2}$ testbed and the SE(3) swarm.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: We thank the support of DARPA grants HR001151626/HR0011516850, ARO grant W911NF-08-2-0004, NSF grant IIS-1426840, and ONR grant N00014-07-1-0829. This work was also supported by a NASA Space Technology Research Fellowship.

ORCID iD

Michael Watterson

Notes

References

Belta

Kumar

(2002) Euclidean metrics for motion generation on SE(3). Proceedings of the Institution of Mechanical Engineers, Part C: Journal of Mechanical Engineering Science 216(1).

Bemporad

Morari

(2000) Predictive control of constrained hybrid systems. In: Allgöwer

Zheng

(eds.), Nonlinear Model Predictive Control. Basel: Birkhäuser.

Bullo

Lynch

(2001) Kinematic controllability and decoupled trajectory planning for underactuated mechanical systems. IEEE Transactions on Robotics 17(4): 402–412.

Chen

Kwok

(2011) Active vision in robotic systems: A survey of recent developments. In: ISRR 2011.

Chirikjian

(2011) Stochastic Models, Information Theory, and Lie Groups, Volume 2: Analytic Methods and Modern Applications. Springer Science & Business Media.

Costante

Forster

Delmerico

Valigi

Scaramuzza

(2016) Perception-aware path planning. arXiv Preprinthttps://arxiv.org/abs/1605.04151.

Deits

Tedrake

(2015) Efficient mixed-integer planning for UAVs in cluttered environments. In: 2015 IEEE International Conference on Robotics and Automation (ICRA), pp. 42–49.

Faà di Bruno

(1855) Sullo sviluppo delle funzione. Annali di Matematica 6: 479–480.

Flores

(2007) Real-time trajectory generation for constrained nonlinear dynamical systems using non-uniform rational B-spline basis functions. PhD Thesis, California Institute of Technology.

10.

Gallier

Quaintance

(2017) Notes on Differential Geometry and Lie Groups. New York: Springer.

11.

Gao

Shen

(2016) Online quadrotor trajectory generation and autonomous navigation on point clouds. In: SSRR. IEEE.

12.

Jaillet

Porta

(2013) Asymptotically-optimal path planning on manifolds. In: Robotics: Science and Systems VIII, p. 145.

13.

Janson

Pavone

(2013) Fast Marching trees: A fast marching sampling-based method for optimal motion planning in many dimensions. In: International Symposium on Robotics Research (ISRR), Singapore.

14.

Karaman

Walter

Perez

Frazzoli

Teller

(2011) Anytime motion planning using the RRT*. In: ICRA. IEEE.

15.

Kleinberg

Tardos

(2006) Algorithm Design. Pearson.

16.

LaValle

(2006) Planning Algorithms. Cambridge: Cambridge University Press.

17.

Liu

Watterson

Mohta

, et al. (2017) Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments. IEEE Robotics and Automation Letters 2(3): 1688–1695.

18.

Mellinger

Kumar

(2011) Minimum snap trajectory generation and control for quadrotors. In: ICRA.

19.

Milnor

(1978) Analytic proofs of the “Hairy Ball Theorem” and the Brouwer fixed point theorem. The American Mathematical Monthly 85(7): 521–524.

20.

Mourikis

Roumeliotis

(2007) A multi-state constraint Kalman filter for vision-aided inertial navigation. In: ICRA 2007.

21.

Nocedal

Wright

(2006) Numerical Optimization (Springer Series in Operations Research). New York: Springer.

22.

Oleynikova

Burri

Taylor

Nieto

Siegwart

Galceran

(2016) Continuous-time trajectory optimization for online UAV replanning. In: 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, pp. 5332–5339.

23.

Park

Ravani

(1995) Bezier curves on Riemannian manifolds and Lie groups with kinematics applications. Journal of Mechanical Design 117(1): 36–40.

24.

Popiel

Noakes

(2007) Bézier curves and C2 interpolation in Riemannian manifolds. Journal of Approximation Theory 148(2): 111–127.

25.

Preiss

Hausman

Sukhatme

Weiss

(2017) Trajectory optimization for self-calibration and navigation. In: Robotics: Science and Systems (RSS).

26.

Press

(ed.) (1992) Numerical Recipes in C: The Art of Scientific Computing. Cambridge: Cambridge University Press.

27.

Richter

Bry

Roy

(2013) Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments. In: The International Symposium on Robotics Research (ISRR).

28.

Sadat

Chutskoff

Jungic

Wawerla

Vaughan

(2014) Feature-rich path planning for robust navigation of MAVs with mono-SLAM. In: ICRA. IEEE.

29.

Schulman

Duan

, et al. (2014) Motion planning with sequential convex optimization and convex collision checking. The International Journal of Robotics Research 33(9): 1251–1270.

30.

Shkolnik

Tedrake

(2009) Path planning in 1000+ dimensions using a task-space Voronoi bias. In: 2009 IEEE International Conference on Robotics and Automation. IEEE, pp. 2061–2067.

31.

Watterson

Kumar

(2015) Safe receding horizon control for aggressive MAV flight with limited range sensing. In: 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, pp. 3235–3240.

32.

Watterson

Liu

Sun

Smith

Kumar

(2018) Trajectory optimization on manifolds with applications to SO(3) and R3xS2. In: Robotics Science and Systems (RSS).

33.

Watterson

Smith

Kumar

(2016) Smooth trajectory generation on SE(3) for a free flying space robot. In: 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, pp. 5459–5466.

34.

Yershova

Jain

Lavalle

Mitchell

(2010) Generating uniform incremental grids on SO(3) using the Hopf fibration. The International Journal of Robotics Research 29(7): 801–812.

35.

Zefran

Croke

Kumar

(1996) Choice of Riemannian metrics for rigid body kinematics. In: ASME Design Technical Conference.

36.

Zefran

Croke

Kumar

(1999) Metrics and connections for rigid body kinematics. The International Journal of Robotics Research 18(2): 242.

37.

Zhang

Ferrari

Qian

(2009) An information roadmap method for robotic sensor path planning. Journal of Intelligent and Robotic Systems 56(1–2): 69–98.

38.

Zhu

Schmerling

Pavone

(2015) A convex optimization approach to smooth trajectories for motion planning with car-like robots. In: CDC. IEEE.

39.

Zucker

Ratliff

Dragan

, et al. (2013) CHOMP: Covariant Hamiltonian Optimization for Motion Planning. The International Journal of Robotics Research 32(9–10): 1164–1193.

Trajectory optimization on manifolds with applications to quadrotor systems

Abstract

Keywords

1. Introduction

2. Mathematical Background

3. Regional Inflation by Line Search

4. Trajectory generation on manifolds

4.1. Algorithm design and problem scope

4.2. Algorithm overview

4.3. Path finding on manifolds

4.4. SCM

4.5. Optimization on manifolds

4.6. Optimization method

4.7. Nested gradients and Hessians

5. Field-of-view-constrained trajectory optimization details

5.1. Vision model

5.2. Field-of-view formulation

5.3. Stereographic coordinates For S n

5.3.1. Christoffel symbols on S n

5.4. Search on S 2

5.5. Circles to circles

5.6. Experimental results

6. Trajectory generation on SO ( 3 )

7. Trajectory generation for quadrotor formations

7.1. Search on SE ( 3 ) with obstacles

7.2. Experimental validation

8. Limitations of approach

9. Conclusion

Footnotes

Appendix. Index to multimedia extensions

Funding

ORCID iD

Notes

References

5.3. Stereographic coordinates For $S^{n}$

5.3.1. Christoffel symbols on $S^{n}$

5.4. Search on $S^{2}$

6. Trajectory generation on $SO (3)$

7.1. Search on $SE (3)$ with obstacles