Generating and Optimizing Topologically Distinct Guesses for Mobile Manipulator Path Planning

Rufus Cheuk Yin Wong,Mayank Sewlia,Adrian Wiltz,Dimos V. Dimarogonas

2024-10-28

Abstract:Optimal path planning often suffers from getting stuck in a local optimum. This is often the case for mobile manipulators due to nonconvexities induced by obstacles and robot kinematics. This paper attempts to circumvent this issue by proposing a pipeline to obtain multiple distinct local optima. By evaluating and selecting the optimum among multiple distinct local optima, it is likely to obtain a closer approximation of the global optimum. We demonstrate this capability in optimal path planning of nonholonomic mobile manipulators in the presence of obstacles and subject to end effector path constraints. The nonholomicity, obstacles, and end effector path constraints often cause direct optimal path planning approaches to get stuck in local optima. We demonstrate that our pipeline is able to circumvent this issue and produce a final local optimum that is close to the global optimum.

Robotics

What problem does this paper attempt to address?

The problem that this paper attempts to solve is: in the presence of obstacles and end - effector path constraints, the optimal path planning of non - holonomic mobile manipulators is prone to getting trapped in local optimal solutions. Specifically, due to the non - convexity caused by obstacles and robot kinematics, when directly using gradient - based optimization methods to solve non - linear programming (NLP) problems, usually only local optimal solutions rather than global optimal solutions can be found.

To solve this problem, the paper proposes a new planning process. By generating multiple topologically different initial guess paths and optimizing them, the possibility of finding a solution closer to the global optimal solution is increased. The specific steps include:

1. **Generate Configuration Graph (CG)**: Discretize the continuous, collision - free robot configuration space into a low - dimensional graph structure.
2. **Apply Modified Neighborhood - Augmented Graph Search (Modified NAGS)**: Find multiple topologically different paths from the CG as initial guesses.
3. **Trajectory Optimization**: Using these topologically different paths as initial conditions, use an optimization algorithm to solve the local optimal solution corresponding to each path.
4. **Evaluate Local Optima**: Compare all the obtained local optimal solutions and select the one with the least cost as the final result.

This method aims to overcome the limitations of the traditional single local optimal solution and provide a more robust and effective path - planning scheme, especially performing well in task execution in complex environments.

### Formula Representation

- **Robot Base Motion Equation**:
\[
\dot{x}_b=\begin{bmatrix}
\cos\theta\\
\sin\theta
\end{bmatrix}v, \quad \dot{\theta}=\omega
\]
where \(v\) is the linear velocity and \(\omega\) is the angular velocity.

- **Elbow and End - Effector Position Constraints**:
\[
\|x_w - x_b^{\perp}\|^2 = l_1
\]
\[
\|x_w - x_e\|^2 = l_2
\]
\[
\exists a, b\in\mathbb{R}:x_w - x_b^{\perp}=a(x_e - x_b^{\perp})+\begin{bmatrix}0\\0\\b\end{bmatrix}
\]

- **Optimization Objective Function**:
\[
\min\int_0^1(v(t)^2+\omega(t)^2 + c(t)^2)dt
\]

Through these steps and formulas, the paper shows how to effectively generate and optimize multiple topologically different paths, thereby increasing the probability of finding the global optimal solution.