For fixed-base manipulators, we formulate the base placement challenge as an optimization problem.
Consider an ordered sequence of desired end-effector poses in \(\textit{SE}(3)\):
\[\boldsymbol{x}_{1:t} =
[\boldsymbol{x}_{1},\boldsymbol{x}_{2},\cdots,\boldsymbol{x}_{t}]\in\mathbb{R}^{t\times 6}.\]
We seek to compute:
- The optimal base placement \(\boldsymbol{q}^b=[x^{b},y^{b},\theta^{b}]^{\mathsf{T}}\in
\textit{SE}(2)\).
- The corresponding feasible joint configurations
\(\boldsymbol{q}^{m}_{1:t}=[\boldsymbol{q}_{1},\boldsymbol{q}_{2},\cdots,\boldsymbol{q}_{t}]\in\mathbb{R}^{t\times
n}\), where \(n\) is DoF of the manipulator.
This comprehensive formulation addresses the base placement problem and also enables simultaneous
path planning. Primarily, these configurations must reach each target pose
\(\boldsymbol{x}_i\), which leads to the following equality constraints:
\[\psi(\boldsymbol{q}^b, \boldsymbol{q}^m_{i}) = \boldsymbol{x}_i,\]
where \(\psi(\cdot)\) represents the forward kinematics function. Additionally, the solution must satisfy
these inequality constraints:
- Base placement limits:
\[{\boldsymbol{q}^b}^{m} \preccurlyeq \boldsymbol{q}^b \preccurlyeq
{\boldsymbol{q}^b}^{M},\]
where \({\boldsymbol{q}^b}^{m}\) and \({\boldsymbol{q}^b}^{M}\) define the
lower and upper bounds.
- Joint limits:
\[{\boldsymbol{q}^m}^{m} \preccurlyeq \boldsymbol{q}^m_i \preccurlyeq
{\boldsymbol{q}^m}^{M},\]
where \({\boldsymbol{q}^m}^m\) and
\({\boldsymbol{q}^m}^M\) represent the lower and upper bounds.
- Collision-free configurations:
\[\text{sd}(\boldsymbol{q}^b,\boldsymbol{q}^{m}_{i})\geq 0,\]
where \(\text{sd}(\cdot)\) denotes the signed distance function.
Finally, the objective function could be very flexible and can be tailored to specific task requirements.
It can be designed either as a feasibility test or to minimize the total path length:
\[f(\boldsymbol{q}^b,\boldsymbol{q}^{m}_{1:t}) = \sum_{i=1}^{t-1}
\|\boldsymbol{q}^{m}_{i+1}-\boldsymbol{q}^{m}_{i}\|_1.\]
B* addresses the inherently non-convex base placement problem through a two-layer hierarchical
optimization framework:
- Outer layer with progressive constraint tightening: The terminal optimization
problem is temporarily reformulated into a more tractable form through systematic constraint relaxation,
particularly the base mobility constraint.
Specifically, we treat the fixed base as mobile,
transforming the timestep-invariant base configuration \(\boldsymbol{q}^b\) into a time-varying
sequence
\(\boldsymbol{q}^b_{1:t}\), effectively introducing three additional DoFs per timestep.
To ensure convergence to a fixed base configuration, we introduce a base movement penalty with
an
iteration-dependent coefficient \(\mu(j)\) that increases with outer loop iteration \(j\) The modified
objective function \(f'(\boldsymbol{q}^b_{1:t},\boldsymbol{q}^{m}_{1:t};\mu(j))\) becomes:
\[f'(\boldsymbol{q}^b_{1:t},\boldsymbol{q}^{m}_{1:t};\mu(j)) =
f(\boldsymbol{q}^b_{1:t},\boldsymbol{q}^{m}_{1:t}) +
\mu(j)\sum_{i=1}^{t}\|\boldsymbol{q}^{b}_{i}-\bar{\boldsymbol{q}}^{b}\|_1,\]
where \(\bar{\boldsymbol{q}}^{b}\) is the arithmetic mean of base poses across all steps.
Thereby, all constraints involving the fixed base \(\boldsymbol{q}^b\) must be reformulated for the
relaxed problem. Each instance of \(\boldsymbol{q}^b\) in the original constraints is replaced with
its
time-indexed counterpart \(\boldsymbol{q}^b_i\) at the corresponding timestep \(i\). These constraints
are subsequently transformed into penalty terms to accommodate potentially infeasible initial
conditions, ensuring the algorithm can start from arbitrary initial states while maintaining numerical
stability.
- Inner layer with sequential linearization: B* addresses the non-convexities of each outer-layer
subproblem by sequential local linearization within trust regions.
Given a non-convex function \(\phi(x)\), we construct its convex approximation \(\phi_c(x)\) through
first-order Taylor expansion around the current point \(x_0\):
\[\phi_c(x) = \phi(x_0) + \dot{\phi}(x_0)(x-x_0),\]
where \(\dot{\phi}(x_0)\) represents the first-order derivative at \(x_0\). The trust region size
adapts based on approximation quality—expanding when the approximation performs well and contracting
when it poorly represents the original function.
The two-layer hierarchical optimization framework effectively transforms the original problem into a
tractable sequential linear programming (SLP).
For a more detailed analysis and complete methodology, please refer to our paper.
