Second-Order Convergent Collision-Constrained Optimization-Based Planner

The issue of robot safety stands as a paramount concern, imposing stringent constraints on motion planning algorithms. Among these constraints, collision avoidance requirements demand that the robot maintains a minimum distance from static or moving obstacles at all times. This paper addresses the challenge of generating optimal robot poses and trajectories while adhering to collision constraints and optimizing a user-defined cost function. Such a problem finds practical applications in (multi-)UAV trajectory planning [1], inverse kinematics [2], object settling [3], and dynamic simulations [4]. Ideally, an effective planning algorithm should possess several key attributes to cater to the diverse needs of these applications: (Scalability) handling a significant number of robot links and obstacles; (Efficacy) rapid convergence and low iterative and overall computational costs; (Robustness) guarantee to satisfy all collision constraints; (Generality) applying to arbitrary articulated robots with complex geometries. However, despite decades of dedicated research in pursuit of such algorithms, it remains a challenge to find a single solution that fully satisfies all these diverse requirements.

Existing trajectory generation algorithms are engineered to suit a set of specific applications. Take, for instance, (online) UAV trajectory generation, where computational efficiency is paramount. Existing algorithm [5] employs penalty methods to handle constraints, which can compromise robustness. In contrast, other algorithms such as [1, 6] offer guaranteed robustness and global optimality. Unfortunately, their applicability is limited to point robots and convex-decomposable freespace. When dealing with general articulated robots, the formulation of collision constraints becomes notably challenging. Many optimization-based algorithms, as seen in the works [7, 8], once again resort to less robust penalty-based techniques. Furthermore, these algorithms often exhibit limited efficacy, having only first-order convergence. Recent advancements [9, 10] have introduced interior point methods to address collision constraints with guaranteed robustness. However, [9] [9] necessitates the decomposition of robot geometry into a gazillion of triangles, which scales poorly with the complexity of the robot geometry. On the other hand, [10] [10] relies on the strict convexity of robot links, demanding intricate geometric modifications and suffering from first-order convergence limitations.

Main Results: We present two innovative optimization solvers designed for collision-constrained robot pose and trajectory planning. Our approach highlights a structural analysis of the barrier function governing collision constraints between pairs of convex hulls. We demonstrate that the Hessian matrix of this barrier function possesses a unique structure that enables efficient factorization using a Schur-complement solver, leading to our ECB method. Furthermore, by employing a generalized barrier function, we eliminate the need for a separating plane between convex hull pairs by representing it as an implicit function of robot poses, leading to our ICB method. Both of these advancements empower us to apply second-order convergent Newton’s methods with an iterative cost linear in the number of convex hull pairs.

Our method exhibits several advantageous properties that have not been simultaneously achieved by prior algorithms. First, we decompose the robot’s geometry into convex hulls rather than points [3] or triangles [9]. As a result, our method can scale to large, complex robot and environmental geometries using a moderate number of geometric primitives, leading to enhanced scalability. Second, in contrast to previous first-order convergent methods [12, 10], our approach enjoys faster second-order convergence. Finally, our method adopts the interior point algorithm that inherits the robustness established by [9, 10], i.e., our method ensures the collision-free properties of the optimize robot poses/trajectories. We have conducted evaluations of our method across six challenging robot planning scenarios. Our results unequivocally demonstrate that our approach achieves significantly faster convergence compared to the first-order baseline method.

We review related work on collision-constrained planning, collision constraint formulations, numerical optimization techniques, and semi-infinite collision constraints.

Collision-Constrained Planning: The algorithms summarized in Table I predominantly fall into the category of local optimization methods. These approaches leverage (high-order) derivatives of the objective/constraint functions to efficiently identify locally optimal plans. In contrast, optimal sampling-based methods [14, 15] pursue globally optimal plans by exploring the entire configuration space. However, a notable drawback of these methods is their limited scalability concerning the dimensionality of the configuration space [16]. An additional category of techniques includes the control barrier function [17], designed for dynamic systems to adhere to specific constraints. However, these algorithms primarily focus on constraint satisfaction and do not directly address the planning of robot motions that fulfill users’ requirements. We also acknowledge recent advancements [18, 19] that involve precomputing feasible sub-domains within the configuration space. Subsequently, mixed-integer second-order numerical solvers are employed to derive globally optimal motion plans restricted to these sub-domains. This approach represents a promising alternative to our method, which obviates the need for sub-domain precomputation or restriction.

Collision-Constraint Formulations: The local optimization techniques outlined in Table I can be distinguished by the granularity at which they formulate collision constraints. Several methods [5, 3] opt to replace collision constraints with point constraints, due to their neat closed-form derivatives. However, this approach necessitates a considerable number of point constraints to ensure the safety of a robot with complex geometries. Recent advancements, as exemplified in [9], extend point constraints to collision constraints between pairs of triangles, benefiting from closed-form second-order derivatives. Nevertheless, even this approach requires a substantial number of triangles to represent intricate robot geometries, resulting in a high volume of collision constraints. As proposed in [13, 11], the number of geometric primitives can be significantly reduced if we can formulate collision constraints between general convex hulls. While [12] [12] demonstrated that the distance function between convex hulls possesses first-order derivatives, a comprehensive exploration of second-order differentiability and the efficient evaluation of convex constraints remains an area that requires further investigation.

Numerical Optimization Techniques: Methods from Table I harness the power of numerical optimization solvers to search for optimal plans. The majority of existing approaches [5, 1, 6] cast the problem in the form of a conventional (mixed-integer) Nonlinear Program (NLP) and subsequently employ off-the-shelf optimization software packages [20, 21]. These algorithms typically exhibit second-order convergence when an accurate Hessian matrix can be evaluated. However, they regress to first-order convergence when solely gradient information is accessible, as in [12]. Furthermore, customized optimization solvers have been devised to enhance robustness or efficiency. For instance, [9] [9] seamlessly integrated continuous collision checks into the line-search procedure to circumvent the challenge of tunneling. Meanwhile, methods like [3, 10] strategically sample collision constraints along the trajectory to approximate the solution of a Semi-Infinite Program (SIP). [11] [11] introduced a first-order ADMM approach that interleaves the optimization of robot trajectories and the separating planes between convex hulls to facilitate parallel computation.

Semi-Infinite Collision Constraints: As clarified in Section I, the necessity to satisfy collision constraints at infinitely many time instances inherently gives rise to SIP problem instances, a concept recently articulated by [3, 10]. While this paper primarily addresses a finite set of collision constraints, we emphasize that our methodology can be adapted to tackle SIP problems. Notably, existing SIP solvers propose to transform SIP into a standard NLP by either sampling [3] or discretizing [10] the constraints. In this context, we can readily substitute the underlying solver in [3, 10] with our approach to achieve second-order convergence when dealing with convex constraints. However, it is noteworthy that, while this enables second-order convergence in the NLP subproblem, the convergence speed of the original SIP problem remains an open question and warrants further exploration, which we leave as a subject for future work.

We address the challenge of collision-constrained optimization, focusing on the task of optimizing the pose/trajectory of a robot within the context of a twice-differentiable objective function $\mathcal{O}(\theta)$, where $\theta$ represents the robot’s configuration parameter. The robot is subject to a set of collision constraints taking the following general form:

Here, $C_{i}$ and $C_{j}$ represent pairs of potentially contacting objects drawn from the set of contact pairs denoted as $\mathcal{C}$. $C_{\bullet}$ can correspond to a link within an articulated robot or a static obstacle. For the sake of generality, we assume that $\mathcal{O}(\theta)$ is a function of the robot’s configuration. Additionally, we presume that $C_{\bullet}$ possesses a (not necessarily strictly) convex shape, which is a widely adopted geometric representation for robots [12, 22, 18]. Using convex shapes can effectively reduce the number of constraints as compared with point-pair or triangle-pair constraints. Non-convex shapes can be further decomposed into convex primitives. Consequently, the primary optimization model addressed in this paper can be summarized as:

Equation 1 embraces a generalized formulation that encompasses both the optimization of a static robot pose and a dynamic robot trajectory. In the case of optimizing a static robot pose, $\theta$ corresponds to a single configuration, and $\mathcal{C}$ is a finite set that enumerates all potential convex object pairs susceptible to collision. In contrast, when optimizing a robot trajectory, $\theta$ represents the parameterization of the robot’s trajectory, such as the control points of a spline curve [1, 23]. In this scenario, $\mathcal{C}$ expands to an infinite set that encompasses all possible convex object pairs at infinitely many time instances across the trajectory. This extension leads to the formulation of SIP problem instances. In the latter case, $\mathcal{C}$ can be further approached by sampling at finite time instances [3] or discretizing [10], thus unified with the former case.

The effective resolution of Equation 1 poses practical challenges, particularly when dealing with convex collision constraints known for their non-differentiability [12, 3]. As illustrated in Figure 1, when complex environmental geometries are considered, the number of collision pairs can rapidly escalate, leading to sizable problem dimensions. In this study, we introduce two efficient optimization techniques, both featuring local second-order convergence and an iterative cost linear in the number of collision pairs. Our approach draws inspiration from recent research [13, 11], which introduced separating planes between pairs of convex objects, represented by the normal direction $n_{ij}$ and offset $d_{ij}$. However, prior works [13, 11] employed an Alternating Optimization (AO) approach that interleaved the updates of $n_{ij},d_{ij}$ and $\theta$. Unfortunately, this approach is limited to at most first-order convergence speed [24]. Our primary contribution is the development of two quasi-Newton algorithms that jointly optimize $n_{ij},d_{ij}$, and $\theta$ without significantly increasing the computational overhead. To achieve this, we augment our objective function with a barrier energy designed to prevent the intersection of $C_{i}$ and $C_{j}$. In our ECB method, we derive the Hessian matrix of the augmented objective function concerning both $n_{ij},d_{ij}$, and $\theta$. Subsequently, we efficiently invert this large Hessian matrix using the Schur-complement lemma [25]. In our ICB method, we eliminate $n_{ij}$ and $d_{ij}$ by expressing them as implicit functions of $\theta$ and derive its Hessian using the implicit function theorem [26].

We conducted our experiments using C++ on a single desktop machine equipped with an 8-core AMD EPYC 7K62 CPU. All available cores were utilized to facilitate parallel computations related to separating planes, such as the computation of per-plane Schur-complements in the ECB method and the evaluation of implicit functions and their derivatives in the ICB method. We also incorporated the widely used acceleration technique of broadphase collision checks, similar to the one employed in prior works [9, 11]. This technique excludes distant pairs of convex hulls from consideration, introducing barrier terms only when the convex hulls are in close proximity. It is important to note that once inserted, these barrier terms are not removed, even if the pair of convex hulls later move apart. Throughout our analysis, we require our penalty function to be positive and strictly convex. One function satisfying our needs is the layered penalty function [28], which is used in our implementation. Finally, we use hyper-parameters $\epsilon_{1}=0.001$ and $\epsilon_{2}=0.001$ throughout our experiements.

Additionally, we integrated our solver with an implementation of the SIP solver from [10]. This allows us to solve SIP problem instances, ensuring collision-free trajectories at every continuous time interval, by leveraging their constraint discretization method. For comparison, both the ECB and ICB methods are compared with the baseline Alternating Optimization (AO) technique [11], which optimizes $\theta$ and $p_{ij}$ in separate subproblems, where we also optimize $p_{ij}$ subproblems in parallel. This is state-of-the-art optimization technique, allowing the optimization of arbitrary articulated robot poses under convex constraints, with the only difference being the convergence speed.

We assess the performance of our method using a series of six challenging benchmarks, as outlined in Figure 2. The first two benchmarks involve object settling problems, with bulky objects (a) and thin objects (b) dropped into a box. These problems require us to compute the static equilibrium poses of the objects. Since we optimize a single pose, these scenarios only involve a finite set of collision constraints. The remaining four scenarios focus on trajectory optimization problems, where we solve SIP problem instances by integrating both our methods and AO [11] into the SIP solver framework [10]. In Figure 2 (c), four robot arms reach various end-effector positions on a table, while avoiding obstacles. In Figure 2 (d), we optimize the trajectories of six UAVs with a giraffe-shaped obstacle in the middle. Figure 2 (e) extends this challenge with longer trajectories for the nine UAVs and multiple obstacles along their paths. Finally, Figure 2 (f) presents three fetch robots reaching 3 positions in a basket. In these scenarios, the trajectories for each robot’s configuration space are parameterized using high-order spline curves, with the number of degrees of freedom and separating planes summarized in Table II, which shows that our method can scale to complex high-DOF robots and environmental setups. We measure the number of iterations and computational costs required to reach different gradient norm values, and the results are summarized in Table III and Table IV, respectively.

Our results clearly demonstrate the superior convergence speed of our method compared to AO, especially when aiming for highly optimal motion plans with a small threshold value ($\epsilon$). The key factor behind this superior performance is the significantly lower number of iterations required by our method, while maintaining a comparable per-iteration cost. Furthermore, it is worth noting that ICB exhibits faster convergence compared to ECB, despite both methods being locally second-order. This can be attributed to ICB’s approach of computing the optimal separating planes during each iteration. Although this leads to a slightly higher iterative cost, the overall reduction in the number of iterations more than compensates for this additional computational overhead. In summary, our method consistently achieves a remarkable reduction in computational time, with a speedup ranging from $5.12$ to $206.34$ for pose optimization (Figure 2a-b) and $2.63$ to $151.23$ for trajectory optimization (Figure 2c-f).

We present innovative second-order optimization-based planners for both robot poses and trajectories, building upon the convex constraint formulation introduced by [13, 11]. A key distinguishing feature of our approach is the efficient application of Newton’s method to simultaneously optimize the separating planes and robot poses. Our work has paved the way for several promising avenues in future research. Although both of our methods outperform the AO baseline, they each come with their own advantages and disadvantages. On the one hand, ICB is more efficient than ECB, requiring even fewer iterations to converge. However, it demands a more complex implementation with an intricate inner loop of optimization and a careful treatment of second-order derivatives. The reliable evaluation of these derivatives under finite-precision floating-point formats requires further investigation. Further, due to the inherent complexity of the underlying problem, our method may still be time-consuming. \AtNextBibliography

We use Einstein’s notation with scripts $\alpha,\beta,\gamma$: