Humanoid robots have become a crucial research direction in robotics owing to their adaptability to complex environments and wide application scenarios. Among various actuation paradigms, hydraulically driven humanoid robots exhibit superior load capacity and dynamic performance, making them promising for industrial and rescue missions. Stable walking is the foundation for performing more complex tasks. In this thesis, I develop a complete walking control system for a hydraulically actuated humanoid robot by combining optimal control methods with a hierarchical control architecture. The proposed system is validated both in simulation and on the physical humanoid robot platform HIT-HU. The main contributions of my work are organized as follows.
1. Overview of the Control Architecture
I design a multi-layer control framework that consists of motion planning, whole-body control, hydraulic joint actuation, and state estimation. The upper layer generates reference gait, center-of-mass (CoM) trajectories, and swing-foot trajectories. The middle layer, i.e., whole-body control, translates these desired trajectories into joint position or torque commands while coordinating all degrees of freedom (DoFs). The low-level layer performs hydraulic cylinder servo control to track the commanded joint trajectories. Simultaneously, a floating-base state estimator provides accurate feedback of the robot’s pose and velocity. I integrate these modules under the ROS2 Control architecture, which ensures modularity, real-time performance, and portability between simulation and hardware.
2. Motion Planning for Bipedal Walking
2.1 Reference Trajectory Generation Based on Linear Inverted Pendulum Model
The linear inverted pendulum model (LIPM) is widely used in humanoid robot locomotion because of its linearity and simplicity. I assume that the entire mass of the humanoid robot is concentrated at the CoM and that the supporting foot acts as a revolute joint without slipping. When the CoM height remains constant, the dynamics of the CoM with respect to the zero moment point (ZMP) can be written as
$$ \ddot{x} = \frac{g}{z_0}(x – p_x), \qquad \ddot{y} = \frac{g}{z_0}(y – p_y), \label{eq:lip} $$
where $(x,y)$ is the CoM position, $(p_x,p_y)$ is the ZMP position, $z_0$ is the constant CoM height, and $g$ is the gravitational acceleration. To extend the LIPM to three-dimensional motion and to simplify footstep planning, I adopt the divergent component of motion (DCM), also known as the capture point, defined as
$$ \boldsymbol{\xi} = \mathbf{x} + b \dot{\mathbf{x}}, \qquad b = \sqrt{\frac{z_0}{g}}, \label{eq:dcm_def} $$
where $\mathbf{x} \in \mathbb{R}^3$ is the CoM position and $b$ is a time constant. Differentiating \eqref{eq:dcm_def} and substituting the LIPM dynamics yields the first-order DCM dynamics
$$ \dot{\boldsymbol{\xi}} = \frac{1}{b} (\boldsymbol{\xi} – \mathbf{r}_{vrp}), \label{eq:dcm_dyn} $$
where $\mathbf{r}_{vrp} = \mathbf{p} + z_0 \mathbf{e}_z$ is the virtual repulsive point, located one CoM height above the foot position. The solution of \eqref{eq:dcm_dyn} during a single support phase is
$$ \boldsymbol{\xi}(t) = \mathbf{r}_{vrp} + e^{t/b} \left(\boldsymbol{\xi}_0 – \mathbf{r}_{vrp}\right). \label{eq:dcm_sol} $$
Given the initial DCM state and the desired foot placement, I can compute the entire DCM trajectory, and then recover the CoM trajectory using
$$ \mathbf{x}(t) = \boldsymbol{\xi}(t) – b \dot{\boldsymbol{\xi}}(t). \label{eq:com_from_dcm} $$
To generate a reference gait, I plan three future steps offline. The initial DCM of the first support phase is obtained by backward propagation from the final stopping DCM. Let $T_{step}$ be the step duration. If the robot is supposed to stop after four steps, then
$$ \begin{aligned} \boldsymbol{\xi}_{0,3} &= \mathbf{r}_{vrp,3} + e^{T_{step}/b}\left(\boldsymbol{\xi}_{0,4} – \mathbf{r}_{vrp,3}\right), \\ \boldsymbol{\xi}_{0,2} &= \mathbf{r}_{vrp,2} + e^{T_{step}/b}\left(\boldsymbol{\xi}_{0,3} – \mathbf{r}_{vrp,2}\right), \\ \boldsymbol{\xi}_{0,1} &= \mathbf{r}_{vrp,1} + e^{T_{step}/b}\left(\boldsymbol{\xi}_{0,2} – \mathbf{r}_{vrp,1}\right). \end{aligned} \label{eq:backward} $$
I take the first segment from this backward-computed path as the reference DCM trajectory for the current step. In double support phases, I interpolate the DCM boundary values with a fifth-order polynomial to ensure continuity of the DCM and its derivative. The polynomial coefficients are determined by the initial and final DCM states, as shown in the following matrix form:
$$ \boldsymbol{\xi}_{DS}(t) = \begin{bmatrix} t^5 & t^4 & t^3 & t^2 & t & 1 \end{bmatrix} \mathbf{C}, \label{eq:ds_poly} $$
where $\mathbf{C}$ is a $6 \times 3$ matrix computed from the boundary conditions. This approach smoothly transfers the ZMP from one foot to the other and reduces impact forces at touchdown.
2.2 Real-Time Footstep and Timing Optimization
Fixed step length and step time cannot properly handle external disturbances. Therefore, I formulate a quadratic program (QP) to adjust the next foot placement and step duration based on the current DCM error. Defining the difference between the DCM and the next foot position as
$$ \mathbf{e} = \boldsymbol{\xi} – \mathbf{p}_{next}, \label{eq:error_dcm} $$
the DCM dynamics imply that if $\mathbf{e}$ is kept close to a nominal value, the robot follows the desired walking pattern. I solve the following optimization problem:
$$ \begin{aligned} \min_{\mathbf{p}, \tau} \quad & \left\| \mathbf{p} – \mathbf{p}_{nom} \right\|_{\mathbf{Q}_P}^2 + \left\| \tau – \tau_{nom} \right\|_{\mathbf{Q}_T}^2 + \left\| \mathbf{e} – \mathbf{e}_{nom} \right\|_{\mathbf{Q}_e}^2 \\ \text{s.t.} \quad & \boldsymbol{\xi}_0 + \tau (\boldsymbol{\xi}_0 – \mathbf{p}) = \mathbf{e}, \\ & \mathbf{A}(\mathbf{p}) \leq \mathbf{0}, \\ & \tau_{min} \leq \tau \leq \tau_{max}, \end{aligned} \label{eq:footstep_qp} $$
where $\tau = e^{T_{step}/b}$, $\mathbf{p}_{nom}$ is the nominal foot placement obtained from the reference gait, $\tau_{nom}$ is the nominal time constant, and $\mathbf{e}_{nom}$ is the nominal DCM-foot offset. The geometric constraint $\mathbf{A}(\mathbf{p}) \leq \mathbf{0}$ restricts the foot position to a feasible region that avoids self-collision and respects joint limits. The weights are chosen as $\mathbf{Q}_P = 1$, $\mathbf{Q}_T = 5$, and $\mathbf{Q}_e = 1000$, so that the controller prioritizes stabilizing the DCM while still allowing foot placement and timing to vary. This QP is solved rapidly at each control cycle and provides a real-time adaptive footstep plan.
2.3 Swing Foot Trajectory Planning
For flat-ground walking, I use polynomial interpolation for the swing foot trajectory because the footstep position and timing may change at every control cycle. The horizontal motion ($x$ and $y$) follows a fifth-order polynomial that satisfies the initial position, velocity, and acceleration, as well as the final foot position and zero touchdown velocity. For the vertical motion, I use a sixth-order polynomial and impose an additional constraint on the apex height. To avoid infeasibility when the step time changes, I formulate a small optimization:
$$ \begin{aligned} \min_{\mathbf{a}} \quad & \left( f_z(T_{opt}/2) – h_{des} \right)^2 \\ \text{s.t.} \quad & f_z(t_{k-1}) = z_{k-1}, \quad \dot{f}_z(t_{k-1}) = \dot{z}_{k-1}, \quad \ddot{f}_z(t_{k-1}) = \ddot{z}_{k-1}, \\ & f_z(T_{opt}) = p_z, \quad \dot{f}_z(T_{opt}) = 0, \quad \ddot{f}_z(T_{opt}) = 0, \\ & 0 \leq f_z(t) \leq h_{max}, \end{aligned} \label{eq:swing_z_opt} $$
where $\mathbf{a}$ is the vector of polynomial coefficients. If the swing foot has not touched down when $t \geq T_{opt}$, I command the foot to vertically descend to the planned landing position to ensure timely ground contact.
For stepping over obstacles or climbing stairs, I employ a Bézier curve with five control points. The curve is given by
$$ \mathbf{B}(\sigma) = (1-\sigma)^4 \mathbf{P}_0 + 4(1-\sigma)^3 \sigma \mathbf{P}_1 + 6(1-\sigma)^2 \sigma^2 \mathbf{P}_2 + 4(1-\sigma) \sigma^3 \mathbf{P}_3 + \sigma^4 \mathbf{P}_4, \label{eq:bezier} $$
where $\sigma = t/T_{step} \in [0,1]$. The control points are chosen to guarantee a vertical lift at liftoff, a desired clearance height, and a vertical descent at touchdown. The Bézier curve is adopted when the terrain geometry is known in advance, such as stairs or ramps, and the footstep is not modified in real time.
2.4 Model Predictive Control for CoM Trajectory Generation
To enhance robustness against model errors and external disturbances, I design a model predictive controller (MPC) based on the single rigid body dynamics (SRBD) model. The SRBD model treats the humanoid robot as a single rigid body with mass $m$ and rotational inertia. The external forces and moments are applied at the contact points (feet). The dynamics are
$$ \begin{aligned} m(\ddot{\mathbf{p}}_c – \mathbf{g}) &= \mathbf{F}_l + \mathbf{F}_r, \\ \mathbf{H} &= \mathbf{p}_l \times \mathbf{F}_l + \mathbf{p}_r \times \mathbf{F}_r + \mathbf{M}_l + \mathbf{M}_r, \end{aligned} \label{eq:srbd} $$
where $\mathbf{p}_c$ is the CoM position, $\mathbf{F}_l, \mathbf{F}_r$ are the ground reaction forces at the left and right feet, $\mathbf{M}_l, \mathbf{M}_r$ are the moments, $\mathbf{p}_l, \mathbf{p}_r$ are the foot positions, and $\mathbf{H}$ is the angular momentum derivative. I discretize this model with a sampling time $\Delta t$ and define the state vector as
$$ \mathbf{X} = \begin{bmatrix} \mathbf{p}_c^T & \boldsymbol{\Theta}^T & \dot{\mathbf{p}}_c^T & \boldsymbol{\omega}^T & \mathbf{g}^T \end{bmatrix}^T, \label{eq:state_srbd} $$
where $\boldsymbol{\Theta}$ is the Euler-angle parametrization of the base orientation and $\boldsymbol{\omega}$ is the angular velocity in the body frame. The input vector is $\mathbf{u} = [\mathbf{F}_l^T, \mathbf{F}_r^T, \mathbf{M}_l^T, \mathbf{M}_r^T]^T$. The discretized linear state-space equation is
$$ \mathbf{X}_{t+1} = \mathbf{A}_t \mathbf{X}_t + \mathbf{B}_t \mathbf{u}_t, \label{eq:ss_srbd} $$
where $\mathbf{A}_t$ and $\mathbf{B}_t$ depend on the current foot positions, CoM position, and orientation. At each time step, I set the initial state from the estimator and use the planned foot positions as future contact locations. The MPC optimization is
$$ \begin{aligned} \min_{\mathbf{u}_0, \dots, \mathbf{u}_{n-1}} \quad & \sum_{i=0}^{n-1} \left( \left\| \mathbf{X}_{i+1} – \mathbf{X}_{i+1,des} \right\|_{\mathbf{Q}_X}^2 + \left\| \mathbf{u}_i \right\|_{\mathbf{Q}_u}^2 \right) \\ \text{s.t.} \quad & \mathbf{X}_{i+1} = \mathbf{A}_i \mathbf{X}_i + \mathbf{B}_i \mathbf{u}_i, \\ & \mathbf{g}(\mathbf{u}_i) \leq \mathbf{0}, \end{aligned} \label{eq:mpc_srbd} $$
where $\mathbf{X}_{i+1,des}$ is the reference state derived from the DCM-based trajectory, and $\mathbf{g}(\mathbf{u}_i)$ contains the friction cone constraints and the zero-force constraints for the swing foot. I convert the friction cone into a linearized four-sided pyramid to maintain convexity. For each foot, the contact wrench is constrained by
$$ \begin{aligned} |F_x| & \leq \mu F_z, \\ |F_y| & \leq \mu F_z, \\ F_z & \geq 0, \\ |M_x| & \leq \frac{Y}{2} F_z, \\ |M_y| & \leq \frac{X}{2} F_z, \\ M_z & \geq \mu \left( X F_x + Y F_y \right) – \frac{X^2 + Y^2}{4} F_z, \\ M_z & \leq \mu \left( X F_x + Y F_y \right) + \frac{X^2 + Y^2}{4} F_z, \end{aligned} \label{eq:friction_linear} $$
where $X$ and $Y$ are the foot dimensions and $\mu$ is the friction coefficient. This linear constraint is equivalent to enforcing the static friction at the four corner points. The weights are chosen as listed in the table below.
| Weight matrix | $Q_{cpx}$ | $Q_{cpy}$ | $Q_{cpz}$ | $Q_{\Theta}$ | $Q_{\omega}$ | $Q_{F}$ | $Q_{M}$ |
|---|---|---|---|---|---|---|---|
| Value | 1000 | 400 | 1000 | 600 | 20 | 1 | 10 |
The MPC runs at 200 Hz and computes the optimal control input for the current step. The first component of the optimal input is used to compute the desired CoM acceleration, which is then integrated to obtain the desired CoM trajectory. The entire optimization is formulated as a QP and solved using the OSQP solver, leading to a computation time below 3 ms.
2.5 Validation of Motion Planning
I validate the proposed motion planning algorithms using the ZMP stability criterion. For a given CoM trajectory, the ZMP is computed as
$$ \mathbf{p}_{zmp} = \mathbf{p}_{com} – \frac{\ddot{\mathbf{p}}_{com,z}}{\ddot{\mathbf{p}}_{com,z} + g} \left(\mathbf{p}_{com,xy} – \mathbf{g}_{xy}\right), \label{eq:zmp_compute} $$
where $\mathbf{p}_{com,z}$ is the vertical CoM coordinate. I generate a reference trajectory with step length $0.3$ m, step period $0.5$ s, and double support time $0.1$ s. The resulting ZMP lies strictly inside the support polygon, confirming that the planned trajectory satisfies the dynamic balance condition.
To test the footstep adaptation, I apply a velocity disturbance of $0.2$ m/s in the forward direction at $t=1$ s. The optimization immediately increases the forward step length by $0.15$ m and reduces the step period from $0.5$ s to $0.46$ s. This response repositions the support foot ahead of the CoM, allowing the robot to recover from the disturbance within two steps. The ZMP trajectory remains within the support polygon during the entire process.
For the MPC-based CoM planning, I compare the reference trajectory with the optimized trajectory. The optimized trajectory deviates by at most $0.07$ m in the $x$-direction and $0.02$ m in the $y$-direction, while exhibiting smaller accelerations, which reduces the required ground friction. Under the same velocity disturbance, the MPC replans the CoM trajectory from the actual state, smoothly guiding it back to the reference path. This prevents the robot from tracking an infeasible trajectory after a large disturbance.
3. Optimization-Based Whole-Body Control
3.1 Kinematic and Dynamic Modeling
The humanoid robot is a floating-base system. I define the generalized coordinates as
$$ \mathbf{q} = \begin{bmatrix} \mathbf{q}_{bP} \\ \mathbf{q}_{bR} \\ \mathbf{q}_j \end{bmatrix}, \label{eq:gen_coord} $$
where $\mathbf{q}_{bP}$ is the base position in the world frame, $\mathbf{q}_{bR}$ is the base orientation represented by a quaternion, and $\mathbf{q}_j$ is the vector of joint angles. The generalized velocity is $\boldsymbol{\nu} = [\boldsymbol{\nu}_b^T, \dot{\mathbf{q}}_j^T]^T$, and the generalized acceleration is $\dot{\boldsymbol{\nu}} = [\dot{\boldsymbol{\nu}}_b^T, \ddot{\mathbf{q}}_j^T]^T$. I build multiple kinematic chains originating from the pelvis to each limb. For each link, the Jacobian matrix $\mathbf{J}_i$ maps the generalized velocity to the link velocity. The CoM Jacobian is the mass-weighted sum of the link Jacobians:
$$ \mathbf{J}_{com} = \frac{1}{m} \sum_{i=1}^{n} m_i \mathbf{J}_i, \qquad \dot{\mathbf{J}}_{com} = \frac{1}{m} \sum_{i=1}^{n} m_i \dot{\mathbf{J}}_i. \label{eq:com_jacobian} $$
Using the Euler-Lagrange formulation, the floating-base dynamics can be written as
$$ \mathbf{M}(\mathbf{q}) \dot{\boldsymbol{\nu}} + \mathbf{b}(\mathbf{q}, \boldsymbol{\nu}) + \mathbf{g}(\mathbf{q}) = \mathbf{B} \boldsymbol{\tau} + \mathbf{J}_c^T \mathbf{F}_c, \label{eq:dynamics_floating} $$
where $\mathbf{M}(\mathbf{q})$ is the mass matrix, $\mathbf{b}$ contains Coriolis and centrifugal terms, $\mathbf{g}$ is the gravitational term, $\mathbf{B}$ is the actuation selection matrix, $\boldsymbol{\tau}$ is the joint torque vector, $\mathbf{J}_c$ is the contact Jacobian, and $\mathbf{F}_c$ contains the contact forces and moments at the feet.
3.2 Whole-Body Torque Control
I formulate a whole-body torque controller (WBC) as a QP that optimizes over the generalized acceleration $\dot{\boldsymbol{\nu}}$, the joint torque $\boldsymbol{\tau}$, and the contact wrench $\mathbf{F}_c$. The objective function is a weighted sum of several task cost functions. Each task is expressed as a linear function of the optimization variables. The tasks are listed in the following table.
| Task | Cost function expression |
|---|---|
| CoM tracking | $\left\| \mathbf{J}_{com}\dot{\boldsymbol{\nu}} + \dot{\mathbf{J}}_{com}\boldsymbol{\nu} – \ddot{\mathbf{x}}_{com,des} \right\|_{\mathbf{Q}_{com}}^2$ |
| Body orientation | $\left\| \mathbf{J}_{body}\dot{\boldsymbol{\nu}} + \dot{\mathbf{J}}_{body}\boldsymbol{\nu} – \ddot{\mathbf{x}}_{body,des} \right\|_{\mathbf{Q}_{body}}^2$ |
| Arm joint tracking | $\left\| \mathbf{S}_{arm}\dot{\boldsymbol{\nu}} – \ddot{\mathbf{q}}_{arm,des} \right\|_{\mathbf{Q}_{arm}}^2$ |
| Swing foot tracking | $\left\| \mathbf{J}_{swf}\dot{\boldsymbol{\nu}} + \dot{\mathbf{J}}_{swf}\boldsymbol{\nu} – \ddot{\mathbf{x}}_{swf,des} \right\|_{\mathbf{Q}_{swf}}^2$ |
| Angular momentum | $\left\| \mathbf{A}_g \dot{\boldsymbol{\nu}} + \dot{\mathbf{A}}_g \boldsymbol{\nu} – \dot{\mathbf{L}}_{com,des} \right\|_{\mathbf{Q}_L}^2$ |
| Smoothing | $\left\| \boldsymbol{\tau} – \boldsymbol{\tau}_{last} \right\|_{\mathbf{Q}_{d\tau}}^2$ |
| Energy minimization | $\left\| \dot{\boldsymbol{\nu}} \right\|_{\mathbf{Q}_a}^2 + \left\| \boldsymbol{\tau} \right\|_{\mathbf{Q}_{\tau}}^2$ |
The desired accelerations are computed using PD control laws. For example, the desired CoM acceleration is
$$ \ddot{\mathbf{x}}_{com,des} = k_{p,com} (\mathbf{x}_{com,des} – \mathbf{x}_{com}) + k_{d,com} (\dot{\mathbf{x}}_{com,des} – \dot{\mathbf{x}}_{com}) + \ddot{\mathbf{x}}_{com,des,ff}. \label{eq:com_pd} $$
Similarly, the desired swing foot acceleration, body orientation acceleration, and arm joint acceleration are computed with appropriate PD gains. The constraints include the dynamics equality, friction inequalities \eqref{eq:friction_linear}, support foot velocity damping, and joint torque limits. The support foot constraint is applied as an equality on the contact point acceleration:
$$ \mathbf{J}_{cfoot} \dot{\boldsymbol{\nu}} + \dot{\mathbf{J}}_{cfoot} \boldsymbol{\nu} = -k_{d,cfoot} \mathbf{J}_{cfoot} \boldsymbol{\nu}. \label{eq:st_foot_constraint} $$
The complete optimization is
$$ \begin{aligned} \min_{\dot{\boldsymbol{\nu}}, \boldsymbol{\tau}, \mathbf{F}_c} \quad & \sum_{i \in \mathcal{T}} \left\| \mathbf{A}_i \dot{\boldsymbol{\nu}} + \mathbf{b}_i \right\|_{\mathbf{Q}_i}^2 \\ \text{s.t.} \quad & \mathbf{M} \dot{\boldsymbol{\nu}} + \mathbf{b} + \mathbf{g} = \mathbf{B} \boldsymbol{\tau} + \mathbf{J}_c^T \mathbf{F}_c, \\ & \mathbf{G} \mathbf{F}_c \leq \mathbf{0}, \\ & \mathbf{J}_{cfoot} \dot{\boldsymbol{\nu}} + \dot{\mathbf{J}}_{cfoot} \boldsymbol{\nu} = -k_{d,cfoot} \mathbf{J}_{cfoot} \boldsymbol{\nu}, \\ & -\boldsymbol{\tau}_{max} \leq \boldsymbol{\tau} \leq \boldsymbol{\tau}_{max}. \end{aligned} \label{eq:wbc_torque} $$
The weights used in the torque WBC are summarized below.
| Weight | $\mathbf{Q}_{com}$ | $\mathbf{Q}_{body}$ | $\mathbf{Q}_{arm}$ | $\mathbf{Q}_{swf}$ | $\mathbf{Q}_L$ | $\mathbf{Q}_{d\tau}$ | $\mathbf{Q}_a$ | $\mathbf{Q}_{\tau}$ |
|---|---|---|---|---|---|---|---|---|
| Value | 2000 | 1000 | 100 | 2000 | 200 | 1 | 1 | 1 |
3.3 Whole-Body Position Control
Although torque control provides better compliance, hydraulic joints in my robot are more conveniently controlled in position mode. Therefore I also design a whole-body position controller that solves for the generalized velocity $\boldsymbol{\nu}$ instead of torque. The desired velocities are computed using PD feedback on the position errors. For instance, the desired CoM velocity is
$$ \dot{\mathbf{x}}_{com,des} = k_{com} (\mathbf{x}_{com,des} – \mathbf{x}_{com}) + \dot{\mathbf{x}}_{com,des,ff}. \label{eq:com_pd_vel} $$
The cost functions are analogous to the torque controller, replacing accelerations with velocities. The constraints include the support foot velocity constraint and joint position limit constraints:
$$ \mathbf{J}_{cfoot} \boldsymbol{\nu} = -k_{cfoot} (\mathbf{x}_{cfoot} – \mathbf{x}_{cfoot,des}), \label{eq:st_foot_vel} $$
$$ \mathbf{q}_{min} \leq \mathbf{q} + \mathbf{S}_j \boldsymbol{\nu} \Delta t \leq \mathbf{q}_{max}. \label{eq:joint_limit} $$
The position WBC is formulated as
$$ \begin{aligned} \min_{\boldsymbol{\nu}} \quad & \sum_{i \in \mathcal{T}} \left\| \mathbf{A}_{v,i} \boldsymbol{\nu} + \mathbf{b}_{v,i} \right\|_{\mathbf{Q}_i}^2 + \left\| \boldsymbol{\nu} – \boldsymbol{\nu}_{last} \right\|_{\mathbf{Q}_{dv}}^2 + \left\| \boldsymbol{\nu} \right\|_{\mathbf{Q}_v}^2 \\ \text{s.t.} \quad & \mathbf{J}_{cfoot} \boldsymbol{\nu} = -k_{cfoot} (\mathbf{x}_{cfoot} – \mathbf{x}_{cfoot,des}), \\ & \mathbf{q}_{min} \leq \mathbf{q} + \mathbf{S}_j \boldsymbol{\nu} \Delta t \leq \mathbf{q}_{max}. \end{aligned} \label{eq:wbc_position} $$
The weights are listed in the following table.
| Weight | $\mathbf{Q}_{com}$ | $\mathbf{Q}_{body}$ | $\mathbf{Q}_{arm}$ | $\mathbf{Q}_{swf}$ | $\mathbf{Q}_L$ | $\mathbf{Q}_{dv}$ | $\mathbf{Q}_v$ |
|---|---|---|---|---|---|---|---|
| Value | 2000 | 1000 | 100 | 2000 | 200 | 1 | 1 |
3.4 Validation of Whole-Body Control
I evaluate both WBC approaches in the RaiSim simulation environment. The first test is a squatting motion, where the CoM tracks a sinusoidal trajectory in the vertical direction. The torque-based WBC achieves a maximum CoM tracking error of $0.036$ m, while the position-based WBC achieves a maximum error of $0.004$ m. The position controller is more accurate because it directly optimizes the velocity, whereas the torque controller trades some accuracy for compliance and dynamic balance.
The second test is a single-leg stance task, where the robot balances on the left foot while the right leg performs a vertical lift. The torque-based WBC yields a swing foot tracking error below $0.004$ m, while the position-based WBC yields an error below $0.001$ m except at touchdown, where the impact causes a transient error. The torque controller shows smoother contact forces and better impact absorption, making it more suitable for dynamic locomotion. The position controller may be preferred for tasks requiring precise foothold placement at low speed.
4. Hydraulic Joint Actuation and State Feedback
4.1 Hydraulic Joint Position Control
The humanoid robot used in my experiments employs hydraulic cylinders connected to each joint through a four-bar linkage or a rack-and-pinion mechanism. To simplify the conversion between joint angle and cylinder length, I use polynomial fitting. For the knee joint, the relationship is approximately
$$ \theta = 25710 l^3 – 425.7 l^2 + 36.44 l – 2.556, \label{eq:angle_to_len} $$
$$ l = 0.002379 \theta^3 + 0.01479 \theta^2 – 0.005654 \theta + 0.07022, \label{eq:len_to_angle} $$
where $\theta$ is the joint angle in radians and $l$ is the cylinder extension in meters. Differentiating these polynomials yields the relation between joint angular velocity and cylinder velocity. The joint position command is therefore transformed into a cylinder length command.
The hydraulic cylinder is controlled by a servo valve. I implement a feedforward-plus-PID scheme. The feedforward current is computed from the orifice flow equation. For a critical-center servo valve, the flow rate is
$$ Q_1 = K_v I \sqrt{\Delta p_1}, \qquad Q_2 = K_v I \sqrt{\Delta p_2}, \label{eq:flow_valve} $$
where $K_v$ is the flow gain, $I$ is the valve current, and $\Delta p_1, \Delta p_2$ are the pressure drops across the valve lands. Given the desired cylinder velocity $\dot{l}_{des}$, the required flow in each chamber is $Q_1 = A_1 \dot{l}_{des}$ and $Q_2 = A_2 \dot{l}_{des}$, where $A_1$ and $A_2$ are the piston areas. Solving for $I$ gives the feedforward term. The final valve command is
$$ I_{cmd} = I_{ff} + K_p (l_{des} – l) + K_i \int (l_{des} – l) dt + K_d \frac{d}{dt}(l_{des} – l). \label{eq:pid_cylinder} $$
This scheme provides fast tracking and good disturbance rejection.
4.2 Floating-Base State Estimation with Invariant Extended Kalman Filter
Since the floating base states cannot be measured directly, I design an invariant extended Kalman filter (InEKF) to fuse IMU measurements and kinematic information from the supporting legs. Let the IMU state be represented by the Lie group element
$$ \mathbf{X} = \begin{bmatrix} \mathbf{R} & \mathbf{v} & \mathbf{p} & \mathbf{d} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & 1 \end{bmatrix}, \label{eq:lie_state} $$
where $\mathbf{R}$ is the orientation, $\mathbf{v}$ is the velocity, $\mathbf{p}$ is the position, and $\mathbf{d}$ is the position of the contact foot. The IMU measurements are modeled as
$$ \boldsymbol{\omega}_m = \boldsymbol{\omega} + \mathbf{b}_g + \mathbf{n}_g, \qquad \mathbf{a}_m = \mathbf{a} + \mathbf{b}_a + \mathbf{n}_a, \label{eq:imu_model} $$
where $\mathbf{b}_g$ and $\mathbf{b}_a$ are slowly varying biases. The state prediction uses the IMU measurements and the bias estimates. The error state is defined using the right-invariant error:
$$ \boldsymbol{\eta} = \exp\left( \begin{bmatrix} \boldsymbol{\xi}_R & \boldsymbol{\xi}_v & \boldsymbol{\xi}_p & \boldsymbol{\xi}_d \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & 0 \end{bmatrix} \right), \qquad \boldsymbol{\zeta} = \begin{bmatrix} \tilde{\mathbf{b}}_g \\ \tilde{\mathbf{b}}_a \end{bmatrix}. \label{eq:error_lie} $$
The linearized error dynamics are
$$ \frac{d}{dt} \begin{bmatrix} \boldsymbol{\xi} \\ \boldsymbol{\zeta} \end{bmatrix} = \mathbf{A}_t \begin{bmatrix} \boldsymbol{\xi} \\ \boldsymbol{\zeta} \end{bmatrix} + \mathbf{B}_t \mathbf{n}, \label{eq:error_dynamics} $$
where the matrices are evaluated at the current estimates. Notably, the matrix $\mathbf{A}_t$ does not depend on the state estimate itself except through the small bias error, which improves consistency compared to a standard EKF.
The update step uses the forward kinematics from the supporting foot to the IMU. The measurement equation is
$$ \mathbf{h}(\hat{\mathbf{X}}_t) – \mathbf{Y}_t = \mathbf{H}_t \boldsymbol{\xi}_t + \mathbf{V}_t, \label{eq:update_eq} $$
where $\mathbf{H}_t = [\mathbf{0}, -\mathbf{I}, \mathbf{I}, \mathbf{0}]$ and $\mathbf{V}_t$ is the measurement noise. The Kalman gain is computed as
$$ \mathbf{K}_t = \mathbf{P}_t \mathbf{H}_t^T \left( \mathbf{H}_t \mathbf{P}_t \mathbf{H}_t^T + \mathbf{V}_t \right)^{-1}. \label{eq:kf_gain} $$
When a new foot contact is detected, the contact point state is reset and the covariance is updated accordingly. The InEKF provides accurate estimates of velocity and attitude (roll, pitch), while position and yaw angle are only locally observable. In practice, I reset the global coordinate frame at the beginning of each step to prevent unbounded error accumulation.
I validate the estimator in simulation by injecting Gaussian noise into the IMU and joint measurements. During a straight-line walk of $6$ m, the velocity estimation errors remain below $0.03$ m/s, and the roll and pitch errors are negligible. The yaw error stays below $0.02$ rad over one second. In a convergence test, the estimator recovers from an initial velocity error of $1.0$ m/s within $0.5$ s. These results confirm that the InEKF provides sufficiently accurate state feedback for stable walking.
5. Simulation and Experimental Results
5.1 Simulation Setup
I deploy the complete control framework in the RaiSim physics engine. The robot model corresponds to the 21-DoF hydraulically actuated humanoid robot HIT-HU. The control algorithms are implemented in C++ using Pinocchio for kinematics/dynamics and OSQP for QP solving. The control loop runs at 200 Hz for the WBC and 1000 Hz for the joint-level controller.
5.2 Fast Walking in Simulation
Using the torque-based WBC, the humanoid robot achieves a forward speed of up to $1.0$ m/s with a step length of $0.3$ m and a step period of $0.3$ s. The CoM tracking error is below $0.01$ m in the $x$-direction and below $0.005$ m in the $y$-direction. The swing foot tracking error is below $0.03$ m in the $x$-direction and $0.018$ m in the $z$-direction. The robot exhibits natural arm swing, which helps to reduce angular momentum about the vertical axis.
With the position-based WBC, the maximum walking speed is reduced to about $0.2$ m/s (step length $0.2$ m, period $1.0$ s). The position controller achieves higher accuracy, with CoM tracking errors below $0.002$ m in the horizontal directions, but the motion is less smooth. The torque controller is therefore preferable for fast dynamic walking.
5.3 Locomotion over Various Terrains
I first test the robot on a known terrain composed of stairs and a ramp. The staircase has steps of height $0.1$ m and depth $0.3$ m; the ramp has a $15^\circ$ slope. The robot successfully climbs the stairs and descends the ramp with a step period of $1.0$ s and step length $0.3$ m. The CoM tracking error remains below $0.003$ m in the $x$ and $z$ directions, and the swing foot tracking error is below $0.004$ m in the $x$-direction.
Next, I test an unknown terrain with a $10^\circ$ slope. The robot assumes the ground is flat but adapts to the actual height change through real-time replanning. The MPC adjusts the CoM trajectory so that the robot gradually ascends the slope. The robot does not lose balance despite the unexpected height difference.
I also construct a rubble field by randomly embedding cubes of sizes $0.2$ m, $0.15$ m, and $0.06$ m into the ground. The reference step period is $0.4$ s and the reference step length is $0.15$ m. The robot successfully walks across the rubble while exhibiting slightly increased lateral sway. The CoM trajectory remains close to the reference, demonstrating the robustness of the MPC and the WBC.
5.4 Impact Disturbance Rejection
I apply a lateral impulse of $300 \, \mathrm{N \cdot s}$ (i.e., $300$ N for $0.2$ s) to the pelvis during walking. The step period is $0.5$ s and the step length is $0.2$ m. After the impact, the robot immediately steps in the direction of the disturbance with a shorter step time, as predicted by the footstep optimization. The lateral deviation of the CoM is limited and the robot regains balance within two steps. Without the MPC replanning, the maximum tolerable impulse is only half as large, confirming the benefit of the proposed real-time optimization.
5.5 Physical Robot Experiments
The final experiments are conducted on the physical hydraulic humanoid robot HIT-HU. Due to hardware limitations of the hydraulic pump and servo valves, the joint torque control mode is not yet sufficiently mature for high-dynamic walking. I therefore use the position-based WBC for the hardware experiments.
In the first experiment, the robot walks on flat ground with a step length of $0.08$ m and a step period of $1.5$ s. The robot successfully performs alternating steps with smooth motion. The joint angles of the left leg show good tracking performance, with some sagging under load during single support, which is compensated by the controller.
In the second experiment, I place a $24$ mm high foam board on the ground as an obstacle. The robot walks over the board with a step length of $0.1$ m and a step period of $2.0$ s. When the swing foot contacts the board earlier than expected, the controller detects the touchdown and switches to double support promptly. The robot then raises its body to adapt to the increased ground height. This experiment demonstrates that the proposed control framework can handle small terrain irregularities and tracking errors on real hardware.
6. Conclusion
In this thesis, I develop an optimal control framework for stable bipedal locomotion of a hydraulically actuated humanoid robot. The main contributions are as follows.
First, I design a real-time motion planning system that combines DCM-based reference trajectory generation, QP-based footstep and timing optimization, and MPC-based CoM trajectory replanning. The ZMP stability criterion verifies the validity of the planned trajectories. Disturbance experiments show that the proposed planning algorithms significantly improve the robot’s ability to recover from external pushes.
Second, I design two whole-body control algorithms: a torque-based WBC using multi-rigid-body dynamics and a position-based WBC using kinematics. Both are formulated as quadratic programs with hierarchical task weighting. The torque controller provides better dynamic performance and compliance, while the position controller offers higher tracking accuracy. Simulation tests on squatting and single-leg stance validate both approaches.
Third, I implement hydraulic cylinder position control using polynomial mapping and feedforward-plus-PID servo control. I also develop an invariant extended Kalman filter for floating-base state estimation, which fuses IMU and joint encoder data. The estimator provides accurate velocity and attitude estimates with fast convergence.
Finally, I integrate all modules into a complete control framework and deploy it in the RaiSim simulator and on the physical HIT-HU robot. The simulator experiments demonstrate fast walking up to $1.0$ m/s, traversal of stairs, ramps, and rubble fields, and robustness against large lateral impulses. The hardware experiments confirm that the framework enables dynamic walking on flat ground and adaptation to small ground-height variations.
Future work will focus on implementing torque control on the hydraulic hardware to achieve higher agility, adding active compliance control at foot touchdown to reduce impact forces, and incorporating vision sensors for autonomous path planning and obstacle avoidance.