Hierarchical Task Model Predictive Control for Sequential Mobile Manipulation Tasks

TL;DR

This work proposes a novel Hierarchical-Task Model Predictive Control framework that is able to complete sequential tasks with improved performance and reactivity by effectively leveraging the robot's redundancy.

Abstract

Mobile manipulators are envisioned to serve more complex roles in people's everyday lives. With recent breakthroughs in large language models, task planners have become better at translating human verbal instructions into a sequence of tasks. However, there is still a need for a decision-making algorithm that can seamlessly interface with the high-level task planner to carry out the sequence of tasks efficiently. In this work, building on the idea of nonlinear lexicographic optimization, we propose a novel Hierarchical-Task Model Predictive Control framework that is able to complete sequential tasks with improved performance and reactivity by effectively leveraging the robot's redundancy. Compared to the state-of-the-art task-prioritized inverse kinematic control method, our approach has improved hierarchical trajectory tracking performance by 42% on average when facing task changes, robot singularity and reference variations. Compared to a typical single-task architecture, our proposed hierarchical task control architecture enables the robot to traverse a shorter path in task space and achieves an execution time 2.3 times faster when executing a sequence of delivery tasks. We demonstrated the results with real-world experiments on a 9 degrees of freedom mobile manipulator.

Figures (9)

• Figure 1: Our mobile manipulator executes a sequence of delivery tasks while leveraging redundancy for efficiency. Our robot needs to deliver to/pick up from three people following the order, Red, Blue, Yellow. The high-level goal is decomposed into a sequence of alternating base trajectories (strips) and end effector waypoints (dots). At time $t_0$, the robot leverages its redundancy to the current EE task $\mathcal{T}_4$ (Blue dot) to perform the subsequent base task $\mathcal{T}_5$ (Yellow strip) so that it can reach its next EE waypoint $\mathcal{T}_6$ faster. In this test, our proposed HTMPC motion control architecture is 2.3 times faster than the typical single-task method. A complete video can be found at http://tiny.cc/htmpc.
• Figure 2: Proposed HTMPC planning and control architecture for sequential mobile manipulation tasks. Autonomy modules are represented as blocks. Arrows indicate the direction of information flow. Feedback loops from the robot are presented along with their desired close-loop frequency for reactive behaviours. External disturbances or changes coming from the robot or environment are specified as incoming arrows to each autonomy module affected.
• Figure 3: Illustration of 25 random square wave tests plotted in end effector frame using polar coordinates. The hierarchical tasks are, $\mathcal{T}_0$ minimizing constraints violation, $\mathcal{T}_1$ holding its end effector in home position, and $\mathcal{T}_2$ base tracking a square wave trajectory. The square wave trajectories have a duration of $16s$ that peaks during $t=0\sim8s$ (Part 1) and bottoms during $t=8\sim16s$ (Part 2). Valley target is the same for all cases which is the robot's home position. Peak targets are randomly selected outside the robot's workspace. An example base trajectory executed by HTMPC is provided with arrows indicating the direction of motion. Its corresponding peak target is marked as a yellow star. Note that \ref{['fig:radial_heatmap']} is plotted in the same coordinate to show the correlation between the base tracking performance and the base targets' position.
• Figure 4: Experimental results of the three competing methods for the example shown in \ref{['fig:squarewave']}. Since the peak target is not reachable, all three methods converge to a non-zero base error in Part 1 when the robot arm becomes fully extended and singular. For Part 2, only the proposed methods HTMPC and HTMPC_WPT are converged to its valley target. HTIDKC did not converge because the parameters were optimized for Part 1.
• Figure 5: HTMPC normalized base tracking error with different lexicographic constraints. The red cross indicates that the base tracking has diverged and is removed from the plot. A desired steady state error bound of 5% is shaded in red. Formulation \ref{['eq:ST-MPC-lexcst-box']} outperforms \ref{['eq:ST-MPC-lexcst-baseline']} with shorter convergence time for both tolerance levels. Increasing the constraint tolerance improves base tracking performance at the cost of the EE task (\ref{['tbl:lex']}).

Theorems & Definitions (2)

• Definition 3.1
• Definition 3.2