This paper presents an optimization-based solution to task and motion planning (TAMP) on mobile manipulators. Logic-geometric programming (LGP) has shown promising capabilities for optimally dealing with hybrid TAMP problems that involve abstract and geometric constraints. However, LGP does not scale well to high-dimensional systems (e.g. mobile manipulators) and can suffer from obstacle avoidance issues. In this work, we extend LGP with a sampling-based reachability graph to enable solving optimal TAMP on high-DoF mobile manipulators. The proposed reachability graph can incorporate environmental information (obstacles) to provide the planner with sufficient geometric constraints. This reachability-aware heuristic efficiently prunes infeasible sequences of actions in the continuous domain, hence, it reduces replanning by securing feasibility at the final full trajectory optimization. Our framework proves to be time-efficient in computing optimal and collision-free solutions, while outperforming the current state of the art on metrics of success rate, planning time, path length and number of steps. We validate our framework on the physical Toyota HSR robot and report comparisons on a series of mobile manipulation tasks of increasing difficulty.

翻译：本文提出了一种基于优化的移动机械臂任务与运动规划（TAMP）解决方案。逻辑几何编程（LGP）在处理包含抽象与几何约束的混合TAMP问题时展现出优异能力，但在高维系统（如移动机械臂）中扩展性不足，且易受避障问题影响。本研究通过引入基于采样的可达性图扩展LGP，使其能够解决高自由度移动机械臂上的最优TAMP问题。所提出的可达性图可整合环境信息（障碍物），为规划器提供充分的几何约束。该可达感知启发式方法有效剪除了连续域中不可行的动作序列，通过确保最终全轨迹优化的可行性减少了重规划次数。实验证明，本框架在计算最优无碰撞解方面具有时间高效性，并在成功率、规划时间、路径长度及步骤数等指标上优于现有最优方法。我们通过物理丰田HSR机器人平台验证了该框架，并报告了一系列难度递增的移动操作任务上的对比结果。