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 due to local minima. 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 path 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. Videos of the experiments are available at https://youtu.be/NEVVHEhQnOQ.

翻译：本文提出一种基于优化的移动机械臂任务与运动规划（TAMP）求解方法。逻辑几何规划（LGP）在处理包含抽象约束与几何约束的混合TAMP问题时展现出优越的优化能力。然而，LGP难以有效扩展至高维系统（如移动机械臂），且因局部极小值问题可能导致避障失败。本研究通过引入基于采样的可达性图扩展LGP框架，使其能够求解高自由度移动机械臂的最优TAMP问题。所提出的可达性图可融合环境信息（障碍物）为规划器提供充分的几何约束。该可达性感知启发式方法能够有效剪除连续域中不可行的动作序列，通过确保最终全路径轨迹优化的可行性来减少重规划次数。本框架在计算最优无碰撞解方面表现出高效率，并在成功率、规划时间、路径长度以及步骤数等指标上均优于当前最先进方法。我们通过实物丰田HSR机器人验证了该框架，并报告了在难度递增的系列移动操作任务上的对比结果。实验视频可通过https://youtu.be/NEVVHEhQnOQ获取。