Optimization Methods for Prioritized Control, Sampling Design, and Motion Planning in Robotics

Modern robots are expected to operate autonomously in unstructured environments, where they must satisfy multiple, often conflicting objectives—such as safety, task performance, and energy efficiency—in real time. Traditional approaches based on weighted cost functions require careful parameter tuning and provide no formal guarantee that critical objectives will not be compromised. This thesis develops optimization-based methods for robotic planning and control along two complementary axes: a scope axis, from single-robot systems to decentralized multi-robot fleets, and a computational depth axis, from hierarchical priority enforcement to sampling time optimization and sampling-based planning. The central methodological contribution is the development and application of Hierarchical Quadratic Programming (HQP), which enforces strict lexicographic task priorities without weight tuning. At the single-robot level, this thesis demonstrates the versatility of HQP across diverse physical domains. Standard locomotion planners assume rigid ground contact, failing on compliant terrain such as sand or vegetation; to address this, the novel Soft Bilinear Inverted Pendulum (SBIP) model is proposed and integrated into both the motion planner and the whole-body controller via HQP, enabling legged robots to traverse deformable ground. Separately, actuator overheating—a key limitation during prolonged operation—is tackled through an epigraph reformulation that encodes motor temperature dynamics as hard constraints within HQP, allowing robots to exploit kinematic redundancy to dissipate heat without sacrificing task performance. A natural question is whether these strict priority guarantees extend to multi-robot systems. This thesis shows that they do—first in a centralized setting, then in a fully decentralized one. For heterogeneous multi-robot coordination, an MPC framework embedding HQP is proposed that hierarchically resolves safety, mission, and efficiency objectives for robot fleets without parameter tuning. To overcome the scalability limitations of centralized coordination, a decentralized HQP framework is then developed, in which each robot solves a local quadratic program using only neighbor-to-neighbor communication while preserving strict lexicographic priorities. Together, these contributions trace a clear progression from single-robot control to decentralized fleet coordination within a unified priority-based framework. All preceding contributions assume a fixed, uniform temporal discretization—a ubiquitous yet rarely questioned design choice in predictive control. This thesis also addresses this assumption by treating sampling intervals as optimization variables. A framework for jointly optimizing states, inputs, and sampling times in continuous-time constrained linear quadratic regulation is proposed, together with analytical gradient expressions that make the resulting nonlinear program computationally tractable. This idea is further extended through differentiable optimization layers, bridging classical optimal control with the differentiable programming paradigm.