The evolution of robotics continually pushes towards machines that can seamlessly operate in environments designed for humans. In this pursuit, the humanoid robot stands as a pinnacle, designed with an anthropomorphic form to navigate and manipulate a world built to our scale and specifications. Among the various locomotion strategies for humanoid robot platforms, wheeled bases offer distinct advantages in terms of energy efficiency, speed on flat terrain, and mechanical simplicity compared to their legged counterparts. However, integrating a sophisticated, multi-degree-of-freedom upper body with a mobile base creates a system of significant complexity—a wheeled mobile manipulator with a human-like structure. The core challenge for such a wheeled humanoid robot lies in whole-body motion control: coordinating the motion of the mobile base and all limb joints in unison to execute tasks while satisfying a plethora of physical and safety constraints.
Traditional control methods often decompose the problem, treating the mobile base and the manipulator arm as separate subsystems controlled in sequence or with limited coordination. While simpler, this approach lacks the synergy and efficiency required for dynamic, cluttered environments. The robot may need to contort its entire body to reach an object, maintain balance while carrying a load, or navigate through a narrow passage. This necessitates a unified control framework that considers the state of every actuator simultaneously. Model Predictive Control (MPC), particularly its nonlinear variants, has emerged as a powerful paradigm for this purpose. MPC optimizes a sequence of future control inputs based on a model of the system, inherently accounting for constraints and enabling proactive, coordinated movement. This article presents a comprehensive framework for the whole-body motion control of a wheeled humanoid robot using a Sequential Linear Quadratic MPC (SLQ-MPC) approach, incorporating kinematic modeling, task-space objectives, and critical safety constraints through a soft-constraint mechanism.
Kinematic Modeling of a Wheeled Humanoid Robot
The first step in controlling a complex humanoid robot is to develop a manageable yet representative model. For whole-body motion planning at moderate speeds, a kinematic model often suffices, focusing on the geometry of motion rather than the dynamics. The robot is conceptually simplified into a differential-drive mobile base coupled with a multi-joint serial manipulator comprising the legs, torso, and one arm. The head and the other arm can be considered fixed for a specific task involving unilateral manipulation. The combined system has a high number of degrees of freedom (DoF). A typical distribution for a comprehensive wheeled humanoid robot is summarized below:
| Robot Component | Degrees of Freedom |
|---|---|
| Mobile Base (Differential Drive) | 2 |
| Legs (Hip, Knee, Ankle) | 3 |
| Torso (Waist) | 1 |
| Arm (Shoulder to Wrist) | 6 |
| Total for Control Model | 12 |
The generalized coordinate vector $\mathbf{q}$ describing the robot’s configuration is defined as:
$$
\mathbf{q} = [\boldsymbol{\zeta}^\top, \mathbf{q}_s^\top]^\top \in \mathbb{R}^{3+n}
$$
where $\boldsymbol{\zeta} = [x_{base}, y_{base}, \phi_{base}]^\top$ represents the pose of the mobile base in the world frame $\{w\}$, and $\mathbf{q}_s \in \mathbb{R}^{n}$ is the vector of joint angles for the serial chain (legs, torso, arm). The end-effector pose (e.g., hand) is denoted as $\mathbf{x}_{ee} = [x, y, z, \alpha, \beta, \gamma]^\top \in \mathbb{R}^6$, containing its position and orientation (e.g., in Euler angles). The forward kinematics function maps the configuration to the end-effector pose:
$$
\mathbf{x}_{ee} = f(\mathbf{q})
$$
The differential kinematics, relating joint velocities to end-effector velocity, is given by the geometric Jacobian $\mathbf{J}_{ee}$:
$$
\dot{\mathbf{x}}_{ee} = \mathbf{J}_{ee}(\mathbf{q}) \dot{\mathbf{q}} = [\mathbf{J}_s(\mathbf{q}_s), \mathbf{J}_{base}(\boldsymbol{\zeta})] \dot{\mathbf{q}}
$$
Here, $\mathbf{J}_s$ is the manipulator Jacobian for the serial chain when the base is stationary, and $\mathbf{J}_{base}$ is the Jacobian relating base motion to end-effector motion when the joints are locked. This unified Jacobian is central to whole-body control, as it allows the MPC to trade off motion between the base and the arm optimally.
Controller Design: Sequential Linear Quadratic MPC
The core of the proposed whole-body motion control framework is a Sequential Linear Quadratic Model Predictive Control (SLQ-MPC) algorithm. SLQ-MPC is an iterative, optimization-based method well-suited for nonlinear systems like a humanoid robot. It operates by repeatedly solving a finite-horizon optimal control problem approximated as a linear-quadratic regulator (LQR) problem around the current state trajectory. The system’s state and input are defined for the MPC formulation. The state vector is simply the configuration: $\mathbf{x} = \mathbf{q}$. The control input vector is $\mathbf{u} = [\mathbf{u}_{base}^\top, \mathbf{u}_s^\top]^\top$, where $\mathbf{u}_{base} = [v_{base}, \omega_{base}]^\top$ are the linear and angular velocities of the differential-drive base, and $\mathbf{u}_s = \dot{\mathbf{q}}_s$ are the joint velocities of the serial chain. The system model for the MPC is the kinematic differential equation:
$$
\dot{\mathbf{x}} = f(\mathbf{x}, \mathbf{u}) = \begin{bmatrix} \mathbf{R}(\phi_{base}) \mathbf{u}_{base} \\ \mathbf{u}_s \end{bmatrix}
$$
where $\mathbf{R}(\phi_{base})$ is the rotation matrix converting base velocities from the robot frame to the world frame.
The MPC solves the following problem over a time horizon $t_h = t_f – t_0$:
$$
\begin{aligned}
\min_{\mathbf{u}(\cdot)} \quad & \varphi(\mathbf{x}(t_f)) + \int_{t_0}^{t_f} L(\mathbf{x}(t), \mathbf{u}(t), t) \, dt \\
\text{s.t.} \quad & \dot{\mathbf{x}} = f(\mathbf{x}, \mathbf{u}, t), \quad \mathbf{x}(t_0) = \mathbf{x}_0 \\
& h(\mathbf{x}, \mathbf{u}, t) \geq 0
\end{aligned}
$$
Here, $\varphi$ is the terminal cost, $L$ is the running cost, and $h(\mathbf{x}, \mathbf{u}, t) \geq 0$ represents all inequality constraints (joint limits, input limits, collision avoidance, stability).
Cost Function for Task-Space Control
The cost function drives the humanoid robot to achieve its primary task, typically defined in the operational space of the end-effector. The running cost $L$ is formulated as a quadratic function promoting accurate tracking and minimal control effort:
$$
L = \frac{1}{2} \left( \|\mathbf{u}\|^2_{\mathbf{R}_u} + \|\mathbf{e}_{p}^{IE}\|^2_{\mathbf{Q}_p} + \|\mathbf{e}_{o}^{IE}\|^2_{\mathbf{Q}_o} \right) + \sum B
$$
where:
- $\|\mathbf{u}\|^2_{\mathbf{R}_u} = \mathbf{u}^\top \mathbf{R}_u \mathbf{u}$ penalizes control effort with a positive definite weight matrix $\mathbf{R}_u = \text{diag}(\mathbf{R}_{base}, \mathbf{R}_s)$.
- $\|\mathbf{e}_{p}^{IE}\|^2_{\mathbf{Q}_p} = (\mathbf{p}_{ee} – \mathbf{p}_{target})^\top \mathbf{Q}_p (\mathbf{p}_{ee} – \mathbf{p}_{target})$ is the position error cost, with $\mathbf{Q}_p \succeq 0$.
- $\|\mathbf{e}_{o}^{IE}\|^2_{\mathbf{Q}_o}$ is a similarly defined quadratic cost for orientation error, using a suitable representation like the quaternion logarithm.
- $\sum B$ represents the sum of Relaxed Barrier Functions (RBF) used to softly enforce constraints, detailed in the next section.
The terminal cost $\varphi(\mathbf{x}(t_f))$ has a similar structure, emphasizing the importance of reaching the target state by the end of the horizon.
Handling Constraints with Relaxed Barrier Functions
A critical advantage of MPC for humanoid robot control is its ability to handle constraints. Hard constraints can make the optimization problem non-convex and difficult to solve in real-time. Instead, we employ Relaxed Barrier Functions (RBF) as a soft-constraint mechanism. An RBF transforms an inequality constraint $h(\mathbf{z}) \geq 0$ into a smooth, twice-differentiable penalty term $B(h)$ added to the cost function. The RBF is defined as:
$$
B(z) =
\begin{cases}
-\mu \ln(z), & z > \delta \\
\mu \, \beta(z; \delta), & z \leq \delta
\end{cases}
$$
with the quadratic continuation function:
$$
\beta(z; \delta) = \frac{1}{2}\left[\left(\frac{z – 2\delta}{\delta}\right)^2 – 1\right] – \ln(\delta)
$$
Parameters $\mu > 0$ and $\delta > 0$ adjust the stiffness and the transition point of the barrier. This function blows up gracefully as $z \to 0^+$ (preventing constraint violation) but has a bounded second derivative for $z \leq \delta$, ensuring numerical stability during the LQ approximation step of SLQ-MPC.
We apply this mechanism to several vital constraints for the wheeled humanoid robot:
1. Joint Position and Velocity Limits: Each joint $i$ has mechanical limits: $q_{i,min} \leq q_i \leq q_{i,max}$ and $\dot{q}_{i,min} \leq \dot{q}_i \leq \dot{q}_{i,max}$. Similarly, the base has velocity limits: $v_{min} \leq v \leq v_{max}$, $\omega_{min} \leq \omega \leq \omega_{max}$. For each limit, a constraint $h(\mathbf{x},\mathbf{u}) = \text{limit} – \text{value} \geq 0$ (or $\text{value} – \text{limit} \geq 0$) is formulated and converted to an RBF penalty $B(h)$.
2. Self-Collision Avoidance: To prevent the robot from hitting itself, critical link pairs are monitored. Using geometric primitives (spheres, capsules) to approximate the robot’s links, the distance $d_j(\mathbf{x})$ between pair $j$ is computed (e.g., using the GJK algorithm). A constraint $h_j(\mathbf{x}) = d_j(\mathbf{x}) – \epsilon_{safe} \geq 0$ is created, where $\epsilon_{safe}$ is a safety margin. This non-linear, state-dependent constraint is seamlessly integrated via its RBF.
3. Tip-Over Prevention (Stability Constraint): Unlike low-slung mobile manipulators, a wheeled humanoid robot has a higher center of mass (CoM), making tip-over a real concern, especially when the arm is extended or carrying a load. Stability is enforced by ensuring the projected CoM remains within the support polygon of the mobile base. For a differential-drive base, the support polygon is essentially a rectangle defined by the wheel axles. A simplified, conservative approach is to enforce that the CoM’s $(x, y)$ coordinates in the base frame lie within a circle of radius $r_{sc}$ inscribed in this rectangle:
$$
\sigma_{CoG}(\mathbf{x}) = r_{sc}^2 – (x_{CoG}^2 + y_{CoG}^2) \geq 0
$$
The CoM position $\mathbf{r}_{CoG} = [x_{CoG}, y_{CoG}]^\top$ is a function of the robot’s configuration $\mathbf{x}$ and its mass distribution. This highly non-linear constraint $h_{stab}(\mathbf{x}) = \sigma_{CoG}(\mathbf{x})$ is again handled via an RBF. This is a crucial differentiator for safe whole-body control of a humanoid robot, as it allows the MPC to automatically plan motions that lean the body or adjust the arm to counteract the momentum of a moving payload.
Simulation, Results, and Performance Analysis
The proposed SLQ-MPC framework was implemented and tested in a dynamic simulation environment (e.g., ROS2 with Pinocchio for kinematics and OCS2 for the MPC solver). The control loop ran at a fixed frequency, with the MPC solving a new optimal trajectory at each step over a receding horizon.
In a characteristic task, the wheeled humanoid robot started from a nominal standing posture and was commanded to move its end-effector to a target position $\mathbf{p}_{target} = [2.0, 0.0, 0.8]^\top$ meters with a specific orientation. The initial end-effector position was $\mathbf{p}_0 \approx [0.21, -0.28, 0.68]^\top$ m. The key parameters for the MPC controller and constraints were set as follows:
| Parameter Group | Symbol | Value |
|---|---|---|
| Cost Weights | $\mathbf{Q}_p$, $\mathbf{Q}_{pf}$ | $10 \cdot \mathbf{I}_3$ |
| $\mathbf{Q}_o$, $\mathbf{Q}_{of}$ | $5 \cdot \mathbf{I}_3$ | |
| $\mathbf{R}_u$ | $\text{diag}(0.05\mathbf{I}_2, 0.01\mathbf{I}_{10})$ | |
| Constraint Parameters | Velocity Limits $(v, \omega, \dot{q}_i)$ | $\pm 0.5 \, \text{m/s, rad/s, rad/s}$ |
| Collision Margin $\epsilon_{safe}$ | $0.1 \, \text{m}$ | |
| Stability Circle Radius $r_{sc}$ | $0.15 \, \text{m}$ | |
| RBF Parameters $(\mu, \delta)$ | $(0.01, 0.001)$ |
The results demonstrated the efficacy of the whole-body control approach. The end-effector smoothly and accurately converged to the target pose. The motion was a synergistic combination of base movement and articulated arm motion, as planned by the MPC. The joint angles and velocities remained within their prescribed limits throughout the motion, with only minor, transient violations due to the soft-constraint nature, which were quickly corrected.
The plots of the robot’s Center of Mass trajectory in the base frame were particularly insightful. When the stability constraint (RBF for $\sigma_{CoG}$) was active, the CoM trajectory remained tightly bounded within the prescribed stability circle. In a comparative test without this constraint, the CoM moved significantly outside this region, indicating a motion that would likely cause the humanoid robot to tip over, especially if executing the motion with a payload. This validates the necessity and effectiveness of integrating the tip-over prevention constraint directly into the MPC’s optimization.
The total cost function value decreased monotonically over the MPC iterations, indicating stable convergence of the SLQ algorithm. A statistical validation was performed by randomizing 50 target end-effector positions within a defined workspace volume. The controller successfully reached the target in 96% of cases, with an average motion time of 14.2 seconds and an average end-effector path length of 0.76 meters. Failure cases were primarily due to targets placed outside the kinematically feasible workspace of the robot given its joint limits, a fundamental limitation not of the controller but of the physical structure.
Conclusion and Future Perspectives
This article has presented a unified framework for whole-body motion control of a wheeled humanoid robot based on advanced Model Predictive Control. By formulating the control problem using a kinematic model and a quadratic cost function focused on end-effector task achievement, the SLQ-MPC controller efficiently generates coordinated motions for the mobile base and all upper-body joints. The innovative use of Relaxed Barrier Functions provides a robust and numerically stable method to incorporate essential real-world constraints—joint limits, velocity bounds, self-collision avoidance, and crucially, tip-over prevention—as soft penalties within the optimization. This allows the humanoid robot to not just perform a task, but to do so safely and stably, automatically trading off between different movement strategies to satisfy all constraints simultaneously.
The simulation results confirm that the approach is viable and effective, producing smooth, stable, and constraint-compliant motions. The next steps involve transferring this control framework to a physical wheeled humanoid robot platform, which will introduce additional challenges such as model inaccuracies, dynamic effects at higher speeds, sensor noise, and real-time computational limits. Extending the model from pure kinematics to include full-body dynamics would allow for even more dynamic and agile motions, such as recovering from pushes or executing fast manipulation tasks. Furthermore, integrating perception to autonomously generate target poses and constraints in unstructured environments will be key to achieving true autonomy for the wheeled humanoid robot in real-world applications, from logistics and inspection to assisted living and beyond.