Incrementally Stochastic and Accelerated Gradient Information mixed Optimization for Manipulator Motion Planning

Gaussian process (GP) describes a multi-variant Gaussian distribution of trajectory $\bm{\theta}\backsim\mathcal{GP}(\bm{\mu},\bm{\mathcal{K}})$. $\bm{\mu}_{\mathbf{T}}=[\mu_{i}^{\top}]^{\top}\big{|}_{i\in\mathbf{T}}$ and $\bm{\mathcal{K}}_{\mathbf{T}}=[\mathcal{K}_{i,j}]\big{|}_{i,j\in\mathbf{T}}$ determine its expectation value and covariance matrix of trajectory $\bm{\theta}_{\mathbf{T}}=[\theta_{i}^{\top}]^{\top}\big{|}_{i\in\mathbf{T}}$, respectively, where a set $\mathbf{T}=\{t_{1},\dots,t_{2}\}$ denotes a period of time from $t_{1}$ to $t_{2}$. Since the above distribution originates from Gauss–Markov model (GMM)¹¹1Section 4 of [2] introduces GP-prior, GMM generated by a linear time-varying stochastic differential equation (LTV-SDE), and GP-interpolation utilized by Section 5 for up-sampling. , informed by the start and goal under zero acceleration assumption, we define the smooth functional

Since the collision-free trajectory means a safe motion or successful planning, we first utilize $\bm{x}(\mathcal{B}_{i},t)$ to map from a state $\theta_{t}$ at time $t$ to the position state of a collision-check ball (CCB-$\mathcal{B}_{i}$) on the manipulator. Then we calculate the collision cost $c(\bm{x})$, which increases when the distance between $\mathcal{B}_{i}$ and its closest obstacle decreases, and define the obstacle functional

where $\bm{\mathcal{B}}=\{\mathcal{B}_{1},\dots,\mathcal{B}_{|\bm{\mathcal{B}}|}\}$ consists of $|\bm{\mathcal{B}}|$ CCBs.

(i) Stuck case and local minima: The functional (2) consists of discrete CCBs for calculating the cost and gradient. So there may exist the sub-gradients with opposite directions in a narrow workspace. In this case, the gradient of (2) will approach zero when a collision cost is still high. It is a so-called body-obstacle stuck case that results in the local minima. According to Section IV-C1 and the objective functional (6), all local minima are unsafe trajectories because the non-convexity originates from the obstacle part (2) caused by the stuck case rather than the convex GP part (1).

(ii) Continuous-time safety: Though the above form combines the collision cost $c(\bm{x})$ with the change rate $\dot{\bm{x}}$ of CCB state for the continuous-time collision avoidance between two adjacent waypoints, it still cannot ensure safety when the obstacle allocation is sparse. This combination at one single time $t$ cannot precisely represent the obstacle functional within a continuous-time interval measured by an infinite number of timestamps, including $t$. So the up-sampling [2] method is adopted for continuous-time safety. According to GMM, we get $\bm{\Phi}(t+1,t)$ transforming $\theta_{t}$ to $\theta_{t+1}$ and

where $\mathbf{Q}_{t,\tau}$ transforms the GP-kernel from $t$ to $(t+\tfrac{\tau}{n_{t}^{\textit{ip}}+1})$ with $\tau\in\{1,\dots,n_{t}^{\textit{ip}}\}$, and $n_{t}^{\textit{ip}}$ is the number of intervals between two adjacent waypoints $\{\theta_{t},\theta_{t+1}\}$. Then we utilize $\bm{\Lambda}_{t}=[\bm{\Lambda}_{t,1}^{\top}\ldots\bm{\Lambda}_{t,n_{t}^{\textit{ip}}}^{\top}]^{\top}$, $\bm{\Psi}_{t}=[\bm{\Psi}_{t,1}^{\top}\ldots\bm{\Psi}_{t,n_{t}^{\textit{ip}}}^{\top}]^{\top}$ to form the upsampling matrix of trajectory $\bm{\theta}_{\mathbf{T}}$ in the period $\mathbf{T}$:

for an upsampled trajectory $\bm{\theta}^{\textit{up}}_{\mathbf{T}}=\mathbf{M}(\bm{\theta}_{\mathbf{T}}-\bm{\mu}_{\mathbf{T}})+\bm{\mu}^{\textit{up}}_{\mathbf{T}}$. Section IV-C1 will detail the generation of a stochastic gradient in the time scale by the random selection of $\{n^{\textit{ip}}_{t}|_{t\in\mathbf{T}}\}$.

This paper adopts the objective functional of [1, 2] to generate an optimal trajectory $\bm{\theta}_{\mathbf{T}}$ in high smoothness without collision:

where $\varrho$ trades off between the GP prior (smoothness) and obstacle (collision-free) functional. Here we recommend [1, 2] for the detailed derivation of its gradient.

Since (6) reveals that the whole objective functional uses the penalty factor $\varrho$ to gather the GP prior and obstacle information which accumulates the sub-functions calculated by collision-check balls (CCBs) in discrete time, we generate an SG

in the Functional, Time, and Space scales:

(i) Functional: We select the penalty factor $\frac{1}{\hat{\varrho}}\backsim\mathcal{U}(0,\frac{1}{\varrho})$.

(ii) Time: We select a set $\{n_{t}^{\textit{ip}}\backsim\mathcal{U}(0,N^{\textit{ip}})\big{|}_{t\in\mathbf{T}}\}$ whose element is the number of intervals of two support waypoints $[\theta_{t}^{\top},\theta_{t+1}^{\top}]^{\top}$ to gain a sub-gradient for collision-check:

where (5) generates the upsampling matrix $\mathbf{M}$ with the set $\{n_{t}^{\textit{ip}}\big{|}_{t\in\mathbf{T}}\}$ to map the randomly upsampled gradient

into the gradient $\bar{\nabla}\mathcal{F}_{\textit{obs}}^{sg}$ of $\bm{\theta}_{\mathbf{T}}$ with $\mathbf{T}=\{t_{1},\dots,t_{2}\}$.

(iii) Space: The intuition of the Space rule comes from the collision avoidance of a human arm on a bookshelf in Figure 2(a). When a participant stretches for an object, he makes sequential reflexes, stimulated by the exterior forces (orange arrow) affecting the danger parts (orange/red balls), in response to the body-obstacle collision. It indicates that the collision risk rises from shoulder to hand because the number of orange/red balls rises from shoulder to hand. So we first reconstruct each rigid body of a tandem manipulator by the geometrically connected CCBs. Next, we build each sub-problem (i.e., the collision avoidance of a single CCB) from shoulder to end-effector, considering the risk variation. Then ${\nabla}\mathcal{F}^{\bm{\mathcal{B}}}_{\textit{obs}}$ is calculated by accumulating the CCB-gradients from $\mathcal{B}_{1}$ nearby shoulder up to $\mathcal{B}_{k}$ nearby end-effector:

where $\bm{q}_{i}$ contains the joints actuating CCB-$\mathcal{B}_{i}$, and $\phi_{i}$ is the included angle between $\mathcal{B}_{i}$’s gradient ${\nabla}_{\bm{q}_{i}}\mathcal{F}_{obs}$ and the accumulated gradient ${\nabla}\mathcal{F}_{obs}^{i-1}$ (from $\mathcal{B}_{1}$ to $\mathcal{B}_{i-1}$). The random tolerance angle $\phi_{\text{tol}}$ generates SG in the Space scale by the random rejection of $\nabla_{\bm{q}_{i}}\mathcal{F}_{obs}$ with $\phi_{i}>\phi_{\text{tol}}$, meaning that the space stochasticity will vanish when $\phi_{\text{tol}}\rightarrow 180\degree$.

(iv) Stuck case: Though, unlike the robot-arm, dragged out by the SG (Figure 2(b)), the stuck case cannot occur during the human-arm avoidance (Figure 2(a)), we can still imagine how confused a human-arm reflex is when a pair of opposite forces stimulate it. So we select a constant $\phi_{\text{tol}}$ to detect the stuck case confusing the collision-free planning. Figure 3 visualizes how an arm is stuck in an obstacle when $\exists\phi_{i}>\phi_{\text{tol}}$ and how a sub-trajectory containing a series of arms is stuck. We do not adopt the pair-wise check because the risk variates among different parts, and it is more efficient for the stuck check by a constant $\phi_{\text{tol}}$ to synchronize with the SG generation by a random $\phi_{\text{tol}}$.

Adam [23] adopts the piecewise production of gradients and uses EMA from the initial step up to the current step and the bias correction method for the adaptive momentum estimation. Our work uses the squared $\ell_{2}$-norm of SG to estimate $\hat{\bm{\mathfrak{M}}}_{1}=\|\bar{\nabla}\mathcal{F}^{sg}_{1}\|^{2}$ and adopt EMA and bias correction for 2nd-momenta $\{\bm{\mathfrak{M}}_{k}\}$ estimation:

As shown in Algorithm 3, we adopt the AGD rules [19] rather than the EMA with bias correction for first-momentum adaption. $\{\alpha_{k},\bm{\mathfrak{B}}_{k},\lambda_{k}\}$’s update rules are also reset as

In this way, the descent process, approximately bounded by a $\delta$-sized trust region, accelerates via the $\alpha$-linear interpolation between the conservative step and the accelerated step driven by $\{\lambda_{k}\}$ and $\{\bm{\mathfrak{B}}_{k}\}$ correspondingly. Then the process varies from the lag state guided by $\bm{\theta}$ to the shifting one guided by $\bm{\theta}^{ag}$ with $\{\alpha_{k}\}$ value. Since the SGD is roughly Lipschitz continuous, we get

where $\mathcal{F}_{k}$ and $\mathcal{F}_{k}^{sg}$ simplify $\mathcal{F}(\bm{\theta}_{k})$ and $\mathcal{F}(\bm{\theta}_{k}^{sg})$, and

according to lines 3, 3, 3 of Algorithm 3. Combining the Cauchy-Schwarz inequation, we get

Then through the accumulation from $k=1$ to $N$, we get

where $\beta_{k}=|\bm{\mathfrak{B}}_{k}|_{\infty^{-}}$ and $\Gamma_{l}=\left(1-\alpha_{l}\right)\Gamma_{l-1}$ with $\Gamma_{0}=1$.

In sight of the stochasticity of $\mathcal{F}(\bm{\theta}^{sg})$ and its gradient $\bar{\nabla}\mathcal{F}^{sg}$ and presuming $\|\bar{\nabla}\mathcal{F}_{k}\|c_{l}\cdot\delta\geq\mathcal{L}_{\mathcal{F}}^{sg}$, we get

where $c_{l}$ is the fraction of $\mathcal{L}^{sg}_{\mathcal{F}}$ and trust region box $\delta\cdot\|\bar{\nabla}\mathcal{F}_{k}\|$, and $\mathbf{G}_{\infty}\leq\mathds{E}(\|[\bar{\nabla}^{\top}\mathcal{F}_{1}^{sg},\dots,\bar{\nabla}^{\top}\mathcal{F}_{N}^{sg}]\|_{\infty})$.

Figure 2(b) illustrates how a robot reflexes to the stuck case like the human in Figure 2(a), following the update rules in SgdIter. To improve the efficiency and reliability, we nest the above SGD (i.e., SgdIter) in reSgdIter. Once the SGD is restarted, we select the trajectory with the lowest cost from the sample set $\bm{\Theta}_{K}$ to reinitialize the next SGD, whose maximum number $N_{\textit{sg}}$ is selected randomly in $[N_{lo}^{\textit{sg}},N_{up}^{\textit{sg}}]$.

We assume the velocity is constant and gain the GMM and GP-prior (1) to reduce the dimension. Our benchmark uses $N=12$ support states with $N^{\textit{ip}}=8$ intervals (116 states in total) for iSAGO and GPMP2, 116 states for STOMP and CHOMP, 12 and 58 states for TrajOpt⁴⁴4Since TrajOpt proposes a swept-out area between support waypoints for continuous-time safety, we choose TrajOpt-12 for safety validation and TrajOpt-58 to ensure the dimensional consistency of the benchmark. , and MaxConnectionDistance $=\frac{\|\theta_{g}-\theta_{0}\|}{12}$ for RRT-Connect. All implementations except RRT-Connect apply linear interpolation rather than manual selection to initialize trajectory for a fair comparison.

We adopt the polynomial piecewise form of [1] for the collision cost $c(\bm{x})$ of (2).

The motion cost function in GPMP [2] is adopted to drive the robot under the preplanned optimal collision-free trajectory in the real world.

This paper benchmarks iSAGO against the numerical planners (CHOMP [1], TrajOpt [3], GPMP2 [2]) and the sampling planners (STOMP [8], RRT-Connect [7]) on a 7-DoF robot (LBR-iiwa) and a 6-DoF robot (AUBO-i5). Since the benchmark executes in MATLAB, we use BatchTrajOptimize3DArm of GPMP2-toolbox to implement GPMP2, plannarBiRRT with 20s maximum time for RRT-Connect, fmincon⁵⁵5We use optimoptions(’fmincon’,’Algorithm’,’trust-region-reflective’,’SpecifyObjectiveGradient’, true) to apply fmincon for the trust-region method like TrajOpt and calculates ObjectiveGradient analytically and Aeq (a matrix whose rows are the constraint gradients) by numerical differentiation. for TrajOpt, hmcSampler⁶⁶6Our benchmark defines an hmcSampler object whose logarithm probabilistic density function logpdf is defined by (6), uses hmcSampler.drawSamples for HMC adopted by CHOMP. for CHOMP, and mvnrnd⁷⁷7STOMP samples the noise trajectories by mvnrnd and updates the trajectory via projected weighted averaging. for STOMP. Since all of them are highly tuned in their own studies, our benchmark uses their default settings.

To illustrate the competence of iSAGO for planning tasks, we conduct 25 experiments on LBR-iiwa at a bookshelf and 12 experiments on AUBO-i5 at a storage shelf. We categorize all tasks into 3 classes (A, B, and C) whose planning difficulty rises with the stuck cases in the initial trajectory increase. Figure 4 visually validates our classification because the number of red in-collision CCBs increases from Task A-1 to Task C-3. Considering the difficulty of different classes, we first generate 2 tasks of class A, 3 tasks of class B, and 4 tasks of class C for LBR-iiwa (Figures 4(a)-4(c)). Then we generate 6 tasks of class C for AUBO-i5 (Figures 4(d)). Each task consists of 2 to 4 problems⁸⁸8Since LBR-iiwa has 7 DoFs, meaning infinite solutions for the same goal constraint, our benchmark uses LM [14] with random initial points to generate different goal states. So one planning task of LBR-iiwa with the same goal constraint has several problems with different goal states. Meanwhile, each task of AUBO-i5 has two problems with the same initial state and goal constraint because the same goal constraint has only one solution for the 6-DoF, and we switch yaw-angle in $\{0\degree,180\degree\}$ for each. with the same initial state and goal constraint. Moreover, we compare iSAGO with SAGO to show the efficiency of the incremental method and compare SAGO with STOMA or AGD to show the efficiency or reliability of the mixed optimization.

Since STOMA (Algorithm 3, Figure 2(b)) proceeds SGD roughly contained in a $\delta$-sized trust-region with $\gamma$-damped EMA of historical momenta, this section first tunes these two parameters, then the others.

According to abundant experiences, we tune the key parameters $\delta\in[0.04,0.80]$, $\gamma\in[0.50,0.99]$ and conduct 10 different class C tasks for each pair of parameters. Table I indicates that the SGD rate increases with $\delta$-expansion. In contrast, the success rate decreases with the excessive $\delta$ (i.e., too high or too low). Moreover, the SGD rate increases with $\gamma$-shrink while the success rate decreases with $\gamma$-shrink. All in all, iSAGO performs more stable with a smaller step-size $\delta$, hampering local minima’s overcoming. Meanwhile, it combines less historical momenta to perform faster, reducing the reliability. So we set $\delta=0.40$ and $\gamma=0.90$.

Besides $\delta$ and $\gamma$, we set the initial penalty $\varrho=1.25\text{e-2}$, penalty factor $\kappa_{\varrho}=0.4$, and filter factor $c_{\eta}=2.0$ in Algorithm 1. In Algorithm 2, we set the tolerances $\mathcal{F}\text{tol}=8\text{e-4}$, $\theta\text{tol}=\text{1e-3}$, $\textit{obs}\text{tol}=\text{1e-4}$ and scaling factor $\kappa_{\mathcal{L}}=6.67$. In Algorithm 3, we set the number of samples $K=12$, tolerance $\textit{SG}\text{tol}=6.4\text{e-3}$, and the number of SgdIter $N_{\textit{sg}}\backsim\mathcal{U}(35,55)$. Moreover, we select $\phi_{\text{tol}}=95\degree$ to check stuck case while $\phi_{\text{tol}}\backsim\mathcal{U}(60\degree,180\degree)$ to generate SG.

Figure 5(a) shows how iSAGO drags a series of in-stuck arms of Figure 4(c) out of the bookshelf by STOMA to grasp a cup located in the middle layer of the bookshelf safely. Meanwhile, Figure 5(b) shows how AUBO-i5 avoids the collision of Figure 4(d) to transfer the green piece between different cells of the storage shelf safely.

Table II shows iSAGO gains the highest success rate compared to the others. The random SgdIter number $N_{\textit{sg}}$ and AGD help iSAGO gain the fourth solving rate after TrajOpt-12, TrajOpt-58, and GPMP2-12. Though TrajOpt-58 compensates for the continuous safety information leakage of TrajOpt-12 in iiwa_AB, it still cannot escape from the local minima contained by the trust region and gains the second lowest success rate just above TrajOpt-12 in iiwa_C/aubo_C. In contrast, iSAGO successfully descends into an optimum with adaptive stochastic momenta and an appropriate initial trust-region. Thanks to HMC, randomly gaining the Hamiltonian momenta, CHOMP approaches the optimum with the third-highest success rate, whose failures are informed by the deterministic rather than the stochastic gradients. RRT-Connect with a limited time has the highest and the second-highest success rate in iiwa_AB and iiwa_C/aubo_C, respectively. However, a higher rate needs a smaller connection distance which restricts the RRT growth and computation efficiency. Though STOMP is free of gradient calculation, the significant time it takes to resample does a minor effect on feasible searching, mainly limited by the Gauss kernel. As for GPMP2-12 and TrajOpt-58, the LM and trust-region methods help approach the stationary point rapidly. In contrast, the point of iiwa_C/aubo_C has significantly lower feasibility than that in iiwa_A because the initial trajectory of iiwa_C/aubo_C gets stuck deeper.

The comparison between STOMA and SAGO in Table II shows how AGD performs a 45% accelerated descent towards an optimum. The comparison between iSAGO and SAGO indicates a 55% higher efficiency of incremental planning. The 90% lower success rate of AGD than SAGO shows the limitation of the numerical method. It validates that STOMA can modify the manifold with less local minima by randomly selecting sub-functional.

The comparison between iiwa_C and aubo_C in Tables II shows that iSAGO performs better when the feasible solution is nearby the motion constraints. That is because the motion constraint somewhat stabilizes the $\delta$-sized SGD of STOMA. So the large $\delta$-sized steps bring a severe disturbance and weaken STOMA’s performance by 20% when obstacles wrap the feasible solution. Meanwhile, the comparison between iiwa_AB and iiwa_C shows that the stuck case reduces the success rate of the numerical method by 90% and validates iSAGO’s higher reliability in a narrow workspace under the same non-manual initial condition.