Versatile Telescopic-Wheeled-Legged Locomotion of Tachyon 3 via Full-Centroidal Nonlinear Model Predictive Control

NMPC has been widely applied to legged robots, particularly for torque-controlled quadruped robot. Di Carlo et al. (2018) models the dynamics of a quadruped robot as single rigid body dynamics (SRBD) while the kinematics are inaccruately modeled. Farshidian et al. (2017) and Grandia et al. (2023) combine the SRBD with whole-body kinematics and further improve the accuracy in terms of the kinematics. Bjelonic et al. (2021, 2022) apply the NMPC formulation of Farshidian et al. (2017) to wheeled quadruped robot. Sleiman et al. (2021) improves the approach using whole-body kinematics and centroidal dynamics (Orin et al. (2013)) instead of the SRBD to further improve accuracy. These methods, called as reduced-order model-based NMPC, only computes optimal contact forces and kinematic quantities. Therefore, for torque-controlled robots, a whole-body dynamics-based controller is typically employed to compute the joint torque commands from the NMPC solution. By contrast, Mastalli et al. (2022) and Katayama and Ohtsuka (2022) incorporate the whole-body dynamics into NMPC. One can then improve the dynamics accuracy and also avoid using the additional whole-body dynamics-based controller at the expense of larger computational burden.

In the predictive controls of position-controlled robots such as humanoid robots, the SRBD, centroidal, and whole-body dynamics models have have garnered less attension that highly simplified models such as the linear inverted pendulum model (Kajita et al. (2003)). A pioneer study by Koenemann et al. (2015) reports the applications of the whole-body dynamics-based NMPC for a position-controlled humanoid robot. However, to avoid discontinuities in the joint position commands, it does not use any state feedback, that is, it is almost the same as an application of offline trajectory optimization. Therefore, the resultant trajectory can be far from optimal or infeasible once the state prediction is different from the actual state.

In this paper, we have developed NMPC for telescopic-wheeled-legged Tachyon 3 that has a novel and unique hardware structure. Our contributions are summarized as follows:

• •

  Application of the full-centroidal NMPC (Sleiman et al. (2021)) to the novel hardware Tachyon 3 with novel physical and environmental constraints.

• •

  State feedback using an internal state integrator to apply NMPC to robots, the actuators of which employ high-gain position-controllers.

• •

  Control pipeline and implementation details to integrate NMPC and other modules such as the state integrator and CBF-QP on an on-board PC that has limited computation resources.

• •

  Validation through a simulation study against our previous controller (Takasugi et al. (2023)) and real-world hardware experiment on perceptive locomotion

This paper is organized as follows. Section 2 proposes the NMPC formulation of Tachyon 3. Section 3 introduces implementation of NMPC from the viewpoint of a numerical optimization solver to the overall system architecture implemented on the on-board PCs of Tachyon 3. Section 4 shows the effectiveness of the proposed NMPC framework through simulation and hardware experiments on the perceptive locomotion over structured terrains. Finally, Section 5 concludes this paper with future works.

In robotic applications, NMPC is often referred to as an online motion planning method that solves an optimal control problem (OCP) each time. We herein formulate an OCP for Tachyon 3 under a fixed contact sequence, i.e., a sequence of contact flags (on/off) of all feet and the switching times. We define a mode for each combination of contact flags of six feet. An OCP under such a setting can be defined as a general OCP of switched systems under a given mode sequence $[m_{1},...,m_{M}]$ and switching times $[t_{0},...,t_{M-1}]$, where $t_{i}$ denotes the switching time from mode $m_{i}$ to $m_{i+1}$. Then, for the given horizon $[t_{0},t_{M}]$ and initial state $\bar{x}$, the OCP is formulated as

Here, subscript $m$ indicates that a function is given for the mode $m$ and $V_{m}(\cdot)$, $l_{m}(\cdot)$, $f_{m}(\cdot)$, $e_{m}(\cdot)$ and $g_{m}(\cdot)$ denote the terminal cost, stage cost, state equation, equality constraint, and inequality constraint, respectively.

In NMPC, at each time $t$, we measure or estimate the current state $x(t)$ and solves the above OCP with $t_{0}=t$, $\bar{x}=x(t)$, and $t_{M}=t+T$, where $T>0$ is the horizon length. The latest optimal trajectory is then utilized to determine the control action of the system.

We model Tachyon 3 using the full-kinematics and centroidal dynamics model as is the case with Sleiman et al. (2021), which is termed a full-centroidal model. The full-kinematics can capture the precise constraint while the centroidal dynamics can express the accurate dynamics under certain assumptions (Orin et al. (2013)). The precise constraint evaluation is particularly crucial for Tachyon 3, which has severe joint range limits, limited foot-wise DOF, and non-holonomic wheel contact constraints. The full-centroidal model has a good balance between accuracy and computational cost; for example, it is much computationally cheaper than the exact whole-body dynamics formulation.

The state vector of the full-centroidal model is defined as

where $q_{\rm b}\in SE(3)$ denotes the base pose (position and rotation), $q_{\rm J}\in\mathbb{R}^{n_{\rm J}}$ denotes the joint positions ($n_{\rm J}=12$ in the case of Tachyon 3), $q$ denotes the configuration, and $h_{\rm com}\in\mathbb{R}^{6}$ denotes the centroidal momentum. We denote the time variation of the configuration $q$ at its tangent space as

where $v_{\rm b}\in\mathbb{R}^{6}$ denotes the spatial (linear and angular) velocity of the base expressed in the base local coordinate frame. The control input of the model is defined as

where $f_{i}\in\mathbb{R}^{3}$ denotes the contact force acting on the $i$-th foot and $n_{c}$ is the number of feet ($n_{c}=6$ in the case of Tachyon 3).

To derive the state equation, we introduce the centroidal momentum matrix $A(q)\in\mathbb{R}^{6\times(6+n_{\rm J})}$, which maps the generalized velocity onto the centroidal momentum (Orin et al. (2013)) as

where $A_{\rm b}(q)\in\mathbb{R}^{6\times 6}$ and $A_{\rm b}(q)\in\mathbb{R}^{6\times n_{\rm J}}$. The state equation is then given by

where $m$ is the total mass of the robot, ${\rm FK}_{i}(q)\in\mathbb{R}^{3}$ is a forward kinematics, i.e., position of the $i$-th foot, and ${\rm COM}(q)\in\mathbb{R}^{3}$ is the COM position.

The OCP of Tachyon 3 presented so far has many terms that depend on contact flags. It is not easy to implement such problems efficiently using general-purpose nonlinear programming (NLP) solvers such as Ipopt (Wächter and Biegler (2006)), particularly to extract the problem-specific structure and warm-start strategy. On the other hand, OCS2 (Farshidian et al. (2017–2023)) is a promising open-source framework for such NMPC implementation and it has been utilized in NMPC for legged robots (Farshidian et al. (2017); Sleiman et al. (2021); Bjelonic et al. (2021, 2022); Grandia et al. (2023)). However, it is not suitable for embedded implementation as it deeply depends on Robot Operation Sysmte (ROS) and involves a large amount of runtime temporary memories.

To mitigate such issues, we have developed a numerical optimal control solver, which employs a similar numerical method and interface similar to that of OCS2 while avoiding the aforementioned technical issues. In our solver, the OCP is transcribed into a finite-dimensional NLP using the direct multiple shooting method (Bock and Plitt (1984)). It employs the structure-exploiting primal-dual interior point method (Nocedal and Wright (1999)) and Gauss-Newton Hessian approximation (Rawlings et al. (2017)) to solve the NLP. Equality constraints are handled using the projection method (Nocedal and Wright (1999); Farshidian et al. (2017)). The open-source structure-exploiting numerical solver HPIPM (Frison and Diehl (2020)) is used to compute the search-direction efficiently. Further, our solver can directly handle Lie-groups within the direct multiple shooting method in contrast to OCS2 that consistently depends on Euler-angles. To implement algorithms regarding robot kinematics and dynamics, we use the C++ open-source library Pinocchio (Carpentier et al. (2019)). In contrast to OCS2 which consistently uses automatic differentiation for kinematics and dynamics, we utilize the analytical derivatives provided by Pinocchio (Carpentier and Mansard (2018)) to reduce the redundant computations (e.g., frame Jacobians $J(q)$ and body local velocity (14)). Pinocchio also enables us to handle Lie-groups directly in the numerical optimization. Thanks to the careful implementation and the efficient use of Pinocchio, our solver can improve the computation speed slightly over that of OCS2.

In the following experiments on NMPC, the OCP solver performed Newton-type iteration ones each sampling time as was the case with Diehl et al. (2005). We fixed the barrier parameter of the primal-dual interior point method to enable fast computation. We also used a soft constraint (Feller and Ebenbauer (2016)) for state-only inequality constraints with the fixed barrier and relaxation parameters to avoid numerical difficulties. We set the horizon length to 1.5 s. We set the discretization time step for direct multiple shooting to 0.015 s (i.e., there were around 100 discretization grids over the horizon).

Fig. 3 shows the overall control pipeline. Tachyon 3 has two on-board PCs. One is a perception PC and the other is a control PC. The perception PC is used for environment perception based on exteroceptive sensors such as LiDAR. In particular, it extracts surfaces used in (13) in a way similar to Grandia et al. (2023) as well as obstacles. The control PC (CPU: Intel(R) Core(TM) i7-8850 H CPU @ 2.606 GHz) is used for motion planning and control based on the proprioceptive sensors and the environment information sent from the perception pipeline. The main thread (real-time thread) of the control PC runs at 1 kHz and includes the contact planner, CBF-QP, internal state integrator, and state estimator. For the state estimator, we used a variant of the extended Kalman filter (Hartley et al. (2020)). NMPC is implemented on another thread of the control PC. The NMPC thread asynchronously receives state and contact plans from the real-time thread and sends the optimal trajectory to the real-time thread. NMPC runs as fast as possible and we observed that it runs around 25 Hz.

The optimal state trajectory as well as the optimal control input trajectory are used to compute the desired acceleration. However, numerical optimization causes a non-negligible time-lag and therefore the predicted optimal state trajectory can have some error from the actual state. To compensate for the error from the actual state, after solving the NMPC optimization problem, we refined the optimal state trajectory based on the latest received state. This is achieved by integrating the state over the horizon using the state equation (6) with the latest received state and the optimal control input as shown in Fig. 4.

From the refined optimal trajectory, the positions, velocities, and accelerations of the base pose and feet are extracted for a given queried time. The acceleration commands are then computed by the sum of feedforward and feedback terms. For example, the acceleration command of the $i$-th foot is given by

where $p_{i},v_{i},a_{i}\in\mathbb{R}^{3}$ are the position, velocity, and acceleration of the $i$-th foot extracted from the optimal trajectory, $\bar{p}_{i},\bar{v}_{i}\in\mathbb{R}^{3}$ are the current velocity and position of the $i$-th foot retrieved from the internal state integrator, and $K_{p},K_{d}\geq 0$ are the position and velocity feedback gains. The acceleration commands are further filtered by the CBF-QP and the safe-filtered accelerations are then sent to the state integrator to update its internal state. The CBF-QP is of practical importance for a rigorous safety because the update rate of NMPC is limited.

The internal state integrator in Fig. 3 also plays an essential role for Tachyon 3, which employs high-gain joint position-controllers. As reported in Koenemann et al. (2015), NMPC for position-controlled robots with naive state feedback causes oscillation in the joint position commands due to the prediction error of NMPC, which is inevitable because the numerical optimization of NMPC has a non-negligible time lag. For this issue, in the same spirit as Kuindersma et al. (2016), we employ an internal state integrator module that updates the internal state of the robot by a combination of the output acceleration of the CBF-QP and the estimate of the actual state. With the state integrator, we can avoid oscillation in the resultant joint position commands while consistently feedback the actual state. This is in contrast to previous NMPC for a position controlled robot (Koenemann et al. (2015)) which does not use any actual state feedback.

We examined NMPC with and without the internal state integrator in real hardware. Fig. 5 shows the time histories of target and actual joint positions of the experiments. We can see the oscillation in target joint commands of NMPC using the naive state feedback without using the state integrator while NMPC using the state integrator resolved the issue. We believe that this framework enables the application of NMPC based on the full-kinematics not only to Tachyon 3 but also to other position-control-based robots.

To demonstrate the effectiveness of the proposed NMPC framework, we conducted simulation and hardware experiments. We assumed realistic settings, that is, Tachyon 3 did not have any prior information regarding the environment and only relied on its on-board sensors. The proprioceptive sensor observations of Tachyon 3 were the joint positions from joint encoders, local base linear acceleration and angular velocity from the IMU. Also, the foot contact sensors and actuator current were used to detect foot contacts for the state estimation. The exteroceptive sensor observations were point coulds from a LiDAR. Throughout the experiments, we considered trot³³3We define trot as a walking pattern alternating [LF, RF, MR] feet contact and [MF, LR, RR] feet contact. gait. Note that our NMPC is not limited to a trot: it can realize a variety of gait patterns as long as the numerical optimization converges.

We compared the proposed framework combining the NMPC and CBF-QP and the previous controller using only CBF-QP (Takasugi et al. (2023)) through simulations in which the robot traverses a 20 cm step by trotting. Both methods could successfully traverse the step with a trotting gait while producing in different performances. Fig. 6 shows the time histories of base local $v_{z}$ (base local $z$ linear velocity), base local $w_{y}$ (base local angular velocity around $y$ axis), and MF and MR feet heights. Velocities $v_{z}$ and $w_{y}$ are preferably small, but inevitably change along with the terrain. Table 1 summarizes the averages of $|v_{z}|$ and average $|w_{y}|$. We can see that NMPC reduced the unnecessary body velocities $v_{z}$ and $w_{y}$ compared with the previous method. The table also shows that the proposed method achieved a faster traverse time than the previous controller.

In hardware experiment, Tachyon 3 went back and forth over two steps (ascending and descending 20 cm and 16.5 cm steps) via the proposed NMPC. Fig. 7 shows the time histories of base linear velocity, joint positions, and feet heights. From Fig. 7, we observe that the joint positions were close to the joint limit boundaries that are showed as dotted gray lines, which shows that Tachyon 3 required the severe motion generation close to constraint boundaries. Fig. 8 shows snapshots of the experiment. We can see that the feet have been moving close to collisions with the environment, which also shows that the motion is close to constraint boundaries. We also observed oscillation of the steps themselves which worked as a disturbance. Nevertheless, the proposed NMPC could successfully traverse these steps with the online optimization subject to the physical and environmental constraints retrieved from on-the-fly perception. Fig. 9 shows the CPU time of each NMPC update and we can see that the proposed NMPC run up to 25 Hz on the on-board PC.