Open Access
This article is

- freely available
- re-usable

*Robotics*
**2019**,
*8*(4),
89;
https://doi.org/10.3390/robotics8040089

Article

Estimation and Closed-Loop Control of COG/ZMP in Biped Devices Blending CoP Measures and Kinematic Information

^{1}

Department of Control and Computer Engineering, Politecnico di Torino, Corso Duca degli Abruzzi 24, 10129 Torino, Italy

^{2}

Department of Management and Production Engineering, Politecnico di Torino, Corso Duca degli Abruzzi 24, 10129 Torino, Italy

*

Correspondence: [email protected]; Tel.: +39-011-0907261

^{†}

These authors contributed equally to this work.

Received: 17 July 2019 / Accepted: 16 October 2019 / Published: 22 October 2019

## Abstract

**:**

The zero moment point ($ZMP$) and the linearized inverted pendulum model linking the $ZMP$ to the center of gravity ($COG$) have an important role in the control of the postural equilibrium (balance) of biped robots and lower-limb exoskeletons. A solution for balance real time control, closing the loop from the joint actual values of the $COG$ and $ZMP$, has been proposed by Choi. However, this approach cannot be practically implemented: While the $ZMP$ actual value is available from the center of pressure ($CoP$) measured under the feet soles, the $COG$ is not measurable, but it can only be indirectly assessed from the joint-angle measures, the knowledge of the kinematics, and the usually poorly known weight distribution of the links of the chain. Finally, the possible presence of unknown external disturbance forces and the nonlinear, complex nature of the kinematics perturb the simple relationship between the $ZMP$ and $COG$ in the linearized model. The aim of this paper is to offer, starting from Choi’s model, a practical implementation of closed-loop balance control fusing $CoP$ and joint-angle measures, eliminating possible inconsistencies. In order to achieve this result, we introduce a model of the linearized inverted pendulum for an extended estimation, not only of $COG$ and $ZMP$, but also of external disturbances. This model is then used, instead of Choi’s equations, for estimation and balance control, using ${\mathcal{H}}_{\infty}$ theory. As the $COG$ information is recovered from the joint-angle measures, the identification of a statistically equivalent serial chain ($SESC$) linking the $COG$ to the joint angles is also discussed.

Keywords:

biped robotics; exoskeletons; postural equilibrium; zero moment point; inverted pendulum; robust control## 1. Introduction

The zero moment point ($ZMP$) and linearized inverted pendulum have continued to play a fundamental role in the control of postural equilibrium of biped robots and lower-limb exoskeletons since their introduction by Vukobratovic [1]. Vukobratovic showed that the center of pressure ($CoP$) of reaction forces under the feet soles on a flat horizontal surface coincides with a point he called the zero moment point ($ZMP$) and that postural equilibrium can be guaranteed if the $CoP$ (alias $ZMP$) is maintained inside the convex hull of the surface encompassing the supporting foot (or feet in double stance). Moreover, a very simple relationship, based on the linearized inverted pendulum, links the $ZMP$ and the center of gravity ($COG$) projection on the ground of the mechanical chain. The goal is to control the $COG$ acting on the joint angles as both are algebraically linked by the kinematics of the chain, the target objective being the $ZMP$.

If motion maintaining postural equilibrium is desired (e.g., during a step in gait), to control the $ZMP$ requires a certain degree of anticipation. Hence, in the so-called preview control framework [2], transition of $COG$, $COG$ velocity, and $ZMP$ were precomputed in advance and applied in open loop as reference to the biped control. However, this approach has not been able to model a closed-loop system and does not face the problems of disturbance rejection and stability.

A solution to track the preview trajectories in closed loop was successively proposed by Choi [3]. The preview $COG\u2013ZMP$ trajectories during rectilinear gait were reviewed, and a closed-loop strategy was devised and proven, using Lyapunov techniques, to guarantee closed-loop stability and a bounded error tracking of the $COG$ and $ZMP$ preview references. The loop was closed from the actual values of $COG$ and $ZMP$, generating a feedback signal to control the $COG$ velocity. Then, a speed control for the joints from the $COG$ velocity was designed using a $COG$ Jacobian with specified embedded motion. The output measures were not detailed, but presumably, $COG$ was assessed by measuring the joint angles from the weight distribution on the kinematic chain, and $ZMP$ was measured by pressure sensors under the feet of the biped. The strategy, according to theory, guarantees closed-loop stability. However, when the authors tested it in simulations and practical examples, it showed a lack of robustness to disturbances or poor damping of the closed-loop dynamics. Lyapunov theory guarantees stability but does not say how much the resulting closed-loop poles will be damped.

If real-time measures (let us call them $CO{G}_{m}$ and $ZM{P}_{m}$) of $COG$ and $ZMP$ are independently available, it is reasonable to assume that before closing the loop, a filtering is performed, fusing both data. However, if these are generated, as stated before, the latter by direct measures and the former indirectly from joint measures and a priori information, they are not always consistent with the relationship stated by the linearized inverted pendulum. The main reasons are: uncertainties in the model parameters (especially in the weight distribution when dealing with an exoskeleton interacting with a patient), external forces acting on the biped (crutches or a chair in a sit-to-stand exercise), centrifugal forces in the frontal plane when motion is not rectilinear.

This study was motivated by the intention to improve postural equilibrium in lower-limb exoskeletons for rehabilitation. The same approach is at the basis of all applications needing balance control of biped robots, such as biped walking in rectilinear [4,5] and curved trajectories [6], in haptic lower-limb exoskeletons [7], and in performing sit-to-stand exercises [8], described by the authors in other papers.

#### 1.1. Paper Contributions

The main contribution of the paper is the development of a feedback control more robust than the one offered by Choi. In order to achieve this result, a detailed understanding of the closed-loop dynamics generated by controlling an inverted pendulum is presented, with particular attention devoted to the control design techniques and the engineering problems in closing the loop. Then, in order to make the filtering effective and close the loop from $CO{G}_{m}$ and $ZM{P}_{m}$, ensuring compatibility, the proposed approach operates at two levels: a nonlinear algebraic function and a a linear dynamic model.

The nonlinear function is a simplified mapping from joint angles to $COG$, called statistically equivalent serial chain (SESC) [9], to be identified in a priori experiments. As this identification is based on the same force sensors under the feet used for measuring the $CoP$, it also resolves any calibration mismatch.

The linear model is an extended system based on the inverted pendulum from input u, the reference $COG$ velocity, and output $CO{G}_{m}$ and $ZM{P}_{m}$, used to estimate, along with the $COG$, $ZMP$, and external disturbances affecting the $CoP$, the model states. Then, using the estimated states, the loop from $CO{G}_{m}$ and $ZM{P}_{m}$ can be closed applying robust control theory. The reasons for estimating the unknown external force disturbances are twofold: (1) to take into account real external disturbances, especially in exoskeletons but also in biped robots (e.g., centrifugal forces in turning while walking); (2) robustness in the $COG\u2013ZMP$ joint estimation, accommodating modeling errors, parameter uncertainties, and the simplifications introduced by the linearized inverted pendulum.

Still recently, the linearized inverted pendulum has continued to be at the basis of the models for balance control ([10,11] and references therein). However, to the authors’ knowledge, there are no works introducing, for robustness, an extended system to estimate disturbances, or the need for a SESC identification.

The proposed control is a non-conventional tracking problem, as two separate model states are tracked. Two different control design techniques are proposed and tested to control the extended system: (1) computing a robust estimator and solving the output feedback problem from the estimated states using a numerical approach based on the Levenberg–Marquardt algorithm [12,13]; (2) solving the standard robust regulator, adapted to deal with the preview signal tracking.

In order to test the approaches, three different experiments were performed. First, both observer and state feedbacks were implemented and compared with Choi’s original feedback through simulation of the 2D linearized inverted pendulum. In a standing position, a preview shift of the $COG$ from the heels to the tips of the feet was imposed, while in the meantime, an external force disturbance was applied. Then, a real lower-size mechanical mock-up was considered, composed of foot, leg, thigh, and trunk, with three degrees of freedom (DOF) in the sagittal plane to represent the real exoskeleton for implementing the sit-to-stand exercise. The SESC model of this simplified kinematics was identified with a priori experiments and used in the proposed feedback control through the $COG$ Jacobian of the chain. Finally, a non-linear simulation of the full-scale exoskeleton with patient was run on the same exercise executed by the linearized inverted pendulum, emulating the first phase of a stand-to-sit exercise. A complete sit-to-stand exercise with the presence of a chair and switching dynamics exploiting the same control technique can be found in [8].

The paper is organized as follows. Section 2 reviews Choi’s results. Section 3 introduces the main contribution of the paper: a $COG\u2013ZMP$ model of the linearized inverted pendulum and an extended system, embedding in the model external disturbances, for applying robust estimation and robust control. This model is also used in the Appendix to show the limitations of Choi’s feedback. Section 3.1 and Section 3.2 present the robust estimator–estimate state feedback and the standard robust regulator. Section 3.3 and Section 4 contain simulated and real control experiments. In particular, Section 4.1 approaches the identification of the SESC model, and Section 4.2 presents the simulation of a stand-to-sit exercise. Section 5 concludes the paper. The appendix discusses the limitations of Choi’s original method.

## 2. Choi’s Approach

As in [3], the 3D linearized inverted pendulum is split into two separate, independent 2D models for the sagittal and the frontal planes. However, in this paper only the sagittal plane will be considered, with axes x (horizontal) and z (vertical). The equation linking $COG$ and $ZMP$, adopting Choi’s notation, is
where p is the coordinate of the $ZMP$, c is the projection of the $COG$ on the ground, ${c}_{z}$ is the height of the $COG$, and g is the acceleration of gravity. ${w}_{n}$ is the only parameter of the model of the simplified biped walking system.

$$p=c-(1/{w}_{n}^{2})\ddot{c},$$

$${w}_{n}\triangleq \sqrt{g/{c}_{z}},$$

Let ${p}_{d}$, ${c}_{d}$, and ${\dot{c}}_{d}$ indicate the desired preview trajectories of the $ZMP$, of the $COG$, and of its derivative during a postural exercise, and assume that the pendulum joint servo is controlled in speed by an input signal u according to
where $\u03f5$ accounts for the speed-tracking error and process disturbances and u is given by the following feedback law:

$$\dot{c}=u+\u03f5,$$

$${e}_{c}={c}_{d}-c,{e}_{p}={p}_{d}-p,u={\dot{c}}_{d}+{k}_{c}{e}_{c}-{k}_{p}{e}_{p}.$$

Then, Choi’s results prove, with Lyapunov theory ([3], Theorem 1), that if ${k}_{c}\phantom{\rule{3.33333pt}{0ex}}>\phantom{\rule{3.33333pt}{0ex}}{w}_{n}$ and $0\phantom{\rule{3.33333pt}{0ex}}<\phantom{\rule{3.33333pt}{0ex}}{k}_{p}\phantom{\rule{3.33333pt}{0ex}}<\phantom{\rule{3.33333pt}{0ex}}{w}_{n}$, the closed-loop system is bounded disturbance ($\u03f5$) - bounded errors (${e}_{c},{e}_{p}$) stable.

Anyway, in spite of stability, a feedback implemented using Choi’s equations has a poor damping of the closed-loop dynamics in practical operating conditions. A proof and discussion about this topic can be found in Appendix A.

## 3. An Extended System for COG–ZMP Robust Estimation and Control

Choi does not introduce any input–output dynamic model to prove his results, but only a Lyapunov function directly based on Equations (1)–(4). Here, vice versa, the essence of the feedback control problem involving measures of $COG$ and $ZMP$ with the reference velocity as input is captured by the simple model of the block diagram of Figure 1: a third-order model with states $c,\dot{c},\ddot{c}$, where the jerk of the $COG$ (in the following with an excess of notation, $COG$ indicates its projection on the ground) is controlled by a reference velocity signal u in an internal partial speed loop with velocity gain ${k}_{v}$ and output $COG$ and $ZMP$. The third-order model is needed to guarantee a realistic strictly proper system for the design of the state estimator and feedback as position and acceleration are both present in the output, and in the meantime, representing the internal speed loop with the servo dynamics. If the gain of the local speed loop is taken relatively high (${k}_{v}>100$) the $COG$ speed will closely track the reference u, as desired. This model does not take into account external forces acting on the system or internal disturbances, as in the case of a lower-limb exoskeleton with the presence of a patient. External forces are introduced when crutches are used or when the patient is sitting on a chair in a sit-to-stand exercise, or simply to accommodate discrepancies between $COG$ and $ZMP$ measures. Internal disturbances are generated by the involuntary motion of the patient in the small freedom offered by the exoskeleton, independent of the joint motion, and obviously, by modeling errors.

Then the model can be completed, generating the extended system of Figure 2, as defined in robust control theory (for the definition of the extended system and its role in robust control and ${\mathcal{H}}_{\infty}$ theory, see [14,15]).

F represents low-frequency external forces influencing the $CoP$. In the model $ZM{P}_{actual}$ (i.e., $CoP$), the value measured and $ZM{P}_{ideal}$, the one linked to the $COG$ by the linearized inverted pendulum relationship, are defined separately, where $\delta $ is the difference between the two, the effect of disturbance F to be estimated. $COG$ and $CoP$ are measured as before, taking into account measurement noise represented by two high pass filters ${W}_{nois{e}_{COG}},{W}_{nois{e}_{ZMP}}$. Output objectives are set on the $COG$ and on the $ZMP$ for sensitivity requirements with respect to process noise $\u03f5$ (in a different context, here $\u03f5$ has the same interpretation and scope as in Equation (3)) and the effect of the unknown external force F. The weighting functions ${W}_{COG}$ and ${W}_{ZMP}$ are chosen to guarantee steady-state gain (i.e., tracking error with respect to disturbances $\u03f5$ and F) and the frequency band of the loop transfer function in the designed feedback. In order to have a balanced design, the control activity ${z}_{u}$ (with a weighting function ${W}_{u}$) is added as an objective against measurement noises ${n}_{COG}$ and ${n}_{ZMP}$, to set the control activity. This extended system is used to design robust estimators of $COG$, $C\dot{O}G$, $ZM{P}_{ideal}$, and $\delta $, as well as robust controls.

Let $\widehat{c},\hat{\dot{c}},\widehat{p},\widehat{\delta}$ be the estimates of the $COG$ projection on the ground, its derivative, the $ZM{P}_{ideal}$, and $\delta $. Then the control strategy of Equation (4) is modified as follows:
with the control scheme represented in Figure 3. This means that c (and in the steady state, p) must track a perturbed reference in order to guarantee that the $CoP$, and not the $ZM{P}_{ideal}$ linked to the $COG$, follows the desired preview signal, despite $ZM{P}_{ideal}$ and $COG$ converging to the same value in the steady state, independently of the presence of disturbances. A feedback from $\hat{\dot{c}}$ is also introduced as it has a critical influence on the closed loop damping.

$${e}_{c}={c}_{d}-\widehat{c}-\widehat{\delta},$$

$${e}_{cv}={\dot{c}}_{d}-\hat{\dot{c}},$$

$${e}_{p}={p}_{d}-\widehat{p}-\widehat{\delta},$$

$$u={\dot{c}}_{d}+{k}_{c}{e}_{c}-{k}_{p}{e}_{p}+{k}_{cv}{e}_{cv},$$

In the next subsections, two different approaches, based on robust control theory, to compute the state observer and the state feedback coefficients, are introduced and tested. The estimates and the coefficients in Equation (5) result explicitly from standard ${\mathcal{H}}_{\infty}$ techniques by operating a state-space transformation in the extended system of Figure 2, choosing as states $c,\dot{c},p,\delta $, augmented (for the whole extended system) by the unobservable or uncontrollable states introduced by the dynamics of the weighting functions.

#### 3.1. Separate Estimator and Feedback

In this first approach, the extended system with the new state representation is used to design a state estimator.

Then, a static output feedback problem for constrained pole placement is solved on the cascade of the extended system and observer to derive the gains ${k}_{c}$, ${k}_{cv}$, and ${k}_{p}$ in (5).

It is known that static output feedback has no analytical solution. Hence, a numerical technique based on the Levenberg–Marquardt algorithms was implemented. No algorithm details are presented here. Just note that by minimizing the sum of the squares of a certain number of penalty functions, the closed-loop poles are brought into a stability region with desired damping, constraining ${k}_{c}$ to be greater than a lower bound, and the control activity (measured by the ${\mathcal{H}}_{\infty}$ norm of the closed-loop operator from measuring noises to control objectives) to be smaller than an upper bound.

#### 3.2. ${\mathcal{H}}_{\infty}$ Robust Control

In the second approach, estimation and feedback are jointly computed solving a ${\mathcal{H}}_{\infty}$ robust control regulator. Even if the classical separation of ${\mathcal{H}}_{2}$ does not apply in ${\mathcal{H}}_{\infty}$ controls [14,15], a weakly coupled state observer and estimated state feedback can still be recognized. Then, maintaining the feedback coefficients of the estimated states of interest ( $\widehat{c},\hat{\dot{c}},\widehat{p},\widehat{\delta}$) and setting to zero the remaining ones relative to unobservable or uncontrollable modes (it can be verified that this has very little influence on the closed-loop poles), the strategy of Equation (5) can be implemented. It is interesting to note the similarity of the performances obtained between the two approaches, as shown in the next section.

#### 3.3. Simulation Results

The model of an exoskeleton with a patient used in [5,7] was considered, with parameter ${w}_{n}=3.34$ and choosing a speed gain for the velocity loop of ${k}_{v}=250$. In this experiment, the linear model of the block diagram of Figure 2 was simulated. In an erect posture, with a preview reference computed as suggested by [2], a step transition on the sagittal plane of p of $0.2$ m moving the c from the heels to the tips of the feet at time $t=1.25$ s was imposed. Then at $t=3.5$ s, an external horizontal force disturbance acting in the sagittal plane on the $COG$, tries to move the $CoP$ outside the feet support by an additional $0.1$ m, causing—if not compensated—a loss of balance.

The experiments are compared using the identical tracking gain ${k}_{c}$ of the $COG$ loop and the best (for damping) value of ${k}_{p}$ with Choi’s control and feedback from a robust estimator obtained from the extended system of Figure 2. The gain parameters adopted in the case of Choi’s feedback were ${k}_{c}=60$, ${k}_{p}=5$, with a resulting damping ratio of the dominant poles of $0.05$; and in the case of observer feedback, ${k}_{c}=60$, ${k}_{p}=3$, ${k}_{cv}=4$, with a resulting damping ratio of the dominant poles of $0.7$.

The figures represent reference (dashed) and actual $COG$ (blue), $ZM{P}_{ideal}$ (red), and $CoP$ (cyan), $COG$ speed estimate (green), and the estimate of the disturbance effect (violet).

Figure 4 shows the results adopting the Choi control. The low damping of the closed-loop poles is clearly visible. Note that, ignoring the disturbances, the $CoP$ does not follow the reference path and exits from the tip of the feet.

Vice versa, when disturbances are also estimated, after a short interval of time depending on the filtering bandwidth of the estimator, the $ZMP$ returns to the desired value. Figure 5 and Figure 6 show the feedback from the extended estimator with compensation of disturbances, obtained with the approaches of Section 3.1 and Section 3.2, respectively.

The transition of the force disturbance was chosen to be unrealistically steep to evidence that, because of the estimator bandwidth, the compensation of the disturbance can’t be perfect, depending on the values assigned to the weighting functions in the extended system.

The delay in the estimation of $\delta $, as a consequence of the disturbance, is shown in Figure 7, where $\delta $ is in blue and its estimate is in green.

## 4. Control of a 3 DOF Biped

The approach was tested on the real three-DOF small-scale mock-up of an exoskeleton (Figure 8) and on the simulation of the full-scale exoskeleton. The chains, in both cases, are composed of two joint feet, legs, thighs, and one trunk. The 3 × 3 Jacobian matrix relating $COG$ to joints embedding knee motion and trunk attitude is the following:
where ${\theta}_{1},{\theta}_{2},{\theta}_{3}$ are the angles of the ankle, knee, and hip, ${J}_{cog}$ is the Jacobian of c, and ${\theta}_{trunk}$ is the attitude of the trunk. Joints are controlled by velocity servos, with their references being obtained through the inverse of the Jacobian matrix (6) driven by speed feedback signals. The $COG$ speed feedback is similar to the one used for the linearized inverted pendulum (5), where the measures of the $ZM{P}_{m}$ were obtained from the $CoP$ and that of the $CO{M}_{m}$ from the joint-angle measures ${\theta}_{1m},{\theta}_{2m},{\theta}_{3m}$. The remaining two feedbacks, from the knee angle and trunk attitude measures ${\theta}_{{2}_{m}},{\theta}_{trun{k}_{m}}$, are simply proportional feedbacks, the last measure being obtained from an inertial sensor:
where ${\theta}_{{2}_{ref}}$ and ${\theta}_{trun{k}_{ref}}$ are the references chosen according to the desired postural exercise.

$$\left[\begin{array}{c}\dot{c}\\ \dot{{\theta}_{2}}\\ {\dot{\theta}}_{trunk}\end{array}\right]=\left[\begin{array}{cc}\phantom{\rule{1.em}{0ex}}& {J}_{cog}\\ 0& 1& 0\\ 1& 1& 1\end{array}\right]\xb7\left[\begin{array}{c}{\dot{\theta}}_{1}\\ {\dot{\theta}}_{2}\\ {\dot{\theta}}_{3}\end{array}\right],$$

$$\left[\begin{array}{c}{u}_{{\theta}_{2}}\\ {u}_{{\theta}_{trunk}}\end{array}\right]=\left[\begin{array}{c}{k}_{knee}\xb7({\theta}_{{2}_{ref}}-{\theta}_{{2}_{m}})\\ {k}_{trunk}\xb7({\theta}_{trun{k}_{ref}}-{\theta}_{trun{k}_{m}})\end{array}\right],$$

#### 4.1. Joint Angles–$CO{G}_{m}$ Map Fitting and Control

In order to apply the results of the previous section to a biped device, it is necessary to have a reliable evaluation of the $COG$ from the joint angles, consistent with the $CoP$. Its value depends on the position of the center of masses and weights of each link of the chain. Those data are scarcely known in advance but can be identified with a series of a priori experiments. This approach is called statically equivalent serial chain (SESC) modeling [9] (see also [16,17] for applications to rehabilitation).

Espiau et al., in [9], showed from experiments measuring the projection of the $COG$ on a force table that the physical parameters of a kinematic chain cannot be identified uniquely. What can be identified is only a set of expressions of them, representing classes of equivalent (with respect to the $COG$) chains. These expressions appear in linear form in the SESC model. Considering a three-joint kinematic model composed of feet, leg, thigh, and HAT (head, arms, trunk) for motions in the sagittal plane, the parameters of the SESC model can be identified using least squares with two equations and two sets of experiments: collecting and recording a series of joint-angle positions with the corresponding measures of the $CoP$ in the steady state and a set of samples of joint angles and $CoP$ trajectories in motion spanning the operating area at random. A slight modification of the model presented in [16] is proposed here, where the first equation, expressing explicitly $CO{G}_{x}$, refers to steady-state experiments, while the second equation, expressing $CO{G}_{z}$ indirectly (Equations (1) and (2) can be rewritten as $CO{G}_{x}-ZM{P}_{x}=C\ddot{O}{G}_{x}/9.81\xb7CO{G}_{z}$ ), refers to dynamical ones.

The equations are:
with
where ${l}_{1},{l}_{2}$ are the length of legs and thighs, ${m}_{0},{m}_{1},{m}_{2},{m}_{3}$ are the masses of feet, legs, thighs, and trunk (HAT), (${m}_{tot}={m}_{1}+{m}_{2}+{m}_{3}+{m}_{4}$), ${x}_{0}$ is the center of mass of the feet, ${x}_{1},{z}_{1}$ are the coordinates of the ankle, and ${l}_{10},{l}_{20},{l}_{30}$ are the distances from the center of mass to the distal joints for the leg, thigh, and proximal joint for HAT. Coefficient b accounts for the difference (the feet do not move during the dynamical experiments) in sensing the $COG$ statically and dynamically. From this model, the six parameters of (9) are identified recursively with a non-linear least squares technique such as Levenberg–Marquardt, where $C\ddot{O}G$ is obtained approximately from numerical differentiation of $COG$. The actual small-scale leg was first identified, with results (statical and dynamical) contained in Figure 9 and Figure 10.

$$\left[\begin{array}{c}CO{G}_{x}\\ CO{G}_{x}-ZM{P}_{x}\end{array}\right]=$$

$$\left[\begin{array}{ccc}1& \phantom{\rule{1.em}{0ex}}& 0\\ 0& \phantom{\rule{1.em}{0ex}}& C\ddot{O}{G}_{x}/9.81\end{array}\right]\xb7$$

$$\left[\begin{array}{ccccccccc}1& \phantom{\rule{1.em}{0ex}}& 0& \phantom{\rule{1.em}{0ex}}& sin\left({\theta}_{1}\right)& \phantom{\rule{1.em}{0ex}}& sin({\theta}_{1}+{\theta}_{2})& \phantom{\rule{1.em}{0ex}}& sin({\theta}_{1}+{\theta}_{2}+{\theta}_{3})\\ 0& \phantom{\rule{1.em}{0ex}}& 1& \phantom{\rule{1.em}{0ex}}& cos\left({\theta}_{1}\right)\xb7b& \phantom{\rule{1.em}{0ex}}& cos({\theta}_{1}+{\theta}_{2})\xb7b& \phantom{\rule{1.em}{0ex}}& cos({\theta}_{1}+{\theta}_{2}+{\theta}_{3})\xb7b\end{array}\right]\xb7$$

$$\left[\begin{array}{c}{r}_{1x}\\ {r}_{1z}\\ {r}_{2}\\ {r}_{3}\\ {r}_{4}\end{array}\right],$$

$$\begin{array}{cc}\hfill \phantom{\rule{1.em}{0ex}}& {r}_{1x}=({m}_{0}\xb7{x}_{0}+({m}_{1}+{m}_{2}+{m}_{3})\xb7{x}_{1})/{m}_{tot},\hfill \\ \hfill \phantom{\rule{1.em}{0ex}}& {r}_{1z}=({m}_{1}+{m}_{2}+{m}_{3})\xb7{z}_{1}/({m}_{1}+{m}_{2}+{m}_{3}),\hfill \\ \hfill \phantom{\rule{1.em}{0ex}}& {r}_{2}=({m}_{1}{\dot{l}}_{10}+({m}_{2}+{m}_{3})\xb7{l}_{1})/{m}_{tot},\hfill \\ \hfill \phantom{\rule{1.em}{0ex}}& {r}_{3}=({m}_{2}\xb7{l}_{20}+{m}_{3}\xb7{l}_{2})/{m}_{tot},\hfill \\ \hfill \phantom{\rule{1.em}{0ex}}& {r}_{4}={m}_{3}\xb7{l}_{30}/{m}_{tot},\hfill \\ \hfill \phantom{\rule{1.em}{0ex}}& b={m}_{tot}/({m}_{1}+{m}_{2}+{m}_{3}),\hfill \end{array}$$

Then a control exercise was carried out, maintaining the $CoP$ position fixed and the posture erect while performing an up-and-down motion (such as sit-to-stand) of the body. The results ($Co{P}_{x}$ and $CO{G}_{x}$), based on the identified model and the proposed control scheme, when a disturbing force is applied in the sagittal plane are shown in Figure 11. The action of the feedback on the $COG$ to compensate the disturbance is clear.

#### 4.2. Simulation of a Stand-to-Sit Exercise

In order to validate the results on the linearized inverted pendulum of Section 3.3, the proposed control (with the same estimator and feedback parameters) was applied to the non-linear simulation of a multi-chain with 3 DOF, having as average the same $CO{G}_{z}$. It represents a biped in the sagittal plane emulating the first phase of a stand-to-sit exercise to test the balance control of a future full-scale exoskeleton with a patient. While the pelvis is lowered from a standing posture to reach the chair and the trunk attitude assumes a natural bending forward, the $CO{G}_{x}$ is shifted from heels to tips and a disturbance force is applied, as in the previous experiment of Figure 5 and Figure 6. The animation of the exercise can be seen in Figure 12. The resulting response of the $COG\u2013ZMP$ in Figure 13 is very similar to that of the linearized inverted pendulum. Particularly, in the final phase of the exercise, the reaction of the exoskeleton to preserve equilibrium against the push forward of the disturbing force is particularly visible.

Vice versa, Choi’s original feedback with identical $COG$ gain, applied to the nonlinear simulator, has not been able to guarantee stability.

## 5. Conclusions

In this paper, Choi’s feedback for postural control of a biped robot, based on a linearized inverted pendulum model, has been revised. In practical situations, this feedback generates very undamped closed-loop dynamics. Then, the design problem was reformulated using state estimation and state feedback control. In fact, closing the loop with a state observer of the $COG$ and exploiting velocity along with position and acceleration estimates guarantees a greater damping of closed-loop poles, with identical steady-state gain. However, to be effective in fusing $CoP$ and $COM$ measures, this observer needs to be extended to also estimate external disturbance forces, and the kinematic model of the $COM$ needs to be tuned to the actual mass distribution. The former problem was tackled by a robust estimator based on an extended system embedding into the model unknown force disturbances, the latter by identifying a priori the SESC model of the mapping between joint angles and the $COM$. This a priori identification can also be repeated to maintain the mapping up to date in cases of changes in the weight distribution of the biped.

Two approaches to design the feedback were pursued: one is numerical, computing the state feedback for a given observer with a Levenberg–Marquardt algorithm. The second exploits integrally estimator and feedback obtained from a robust control regulator and adapts it to the tracking of the preview signals. The results show similar performances, with good disturbance compensation. It must be emphasized that the adoption of an extended system with its weighting functions offers a formal technique to set the observer and feedback characteristics, guaranteeing the desired loop gain and bandwidth.

Robustness was shown by applying the control designed for a linearized inverted pendulum to two non-linear systems: a three-DOF kinematic chain of an actual mechanical small-scale leg and the simulation of an exoskeleton constraining a patient to perform a joint-legged stand-to-sit exercise in the sagittal plane. The proposed control correctly integrates the $COG$ information, which is poorly reconstructed from joint measures and kinematics of the chain, with actual $CoP$ measures, accommodating uncertainties in the model and unknown external force disturbances. Moreover, an identification procedure of the SESC model was proposed and tested.

COG–ZMP and linearized inverted pendulum models continue to be at the basis of balance control of bipeds. However, extended systems and SESC models have not yet been proposed in order to offer robustness to the approach.

The proposed $COG\u2013ZMP$ control was successfully used by the authors for the balance of turning during walking of biped robots and for a more detailed and complete sit-to-stand exercise described in the companion paper [8]. In particular, future developments will consider haptic exoskeletons, where the action of the patient on some joints, through electromiographical signals, controls the motion of part of the degrees of freedom, while the automatic control discussed here guarantees balance acting on the ankles or on the hips.

Computing robust estimation and robust control, as well as the block diagrams present in the paper, were made with the design environment G++ developed by the authors described in [18] and that can be downloaded from [19]. However, the used technique is fairly standard in the robust control field and can be found in classical textbooks such as [14].

## Author Contributions

Methodology, G.M. and M.G.; writing—original draft, G.M.; writing—review & editing, M.G.

## Funding

This research has been partially supported by MIUR, the Italian Ministry of Instruction, University and Research through project ESOPO, and the Piedmont Region through project ESROB.

## Conflicts of Interest

The authors declare no conflict of interest.

## Appendix A. Choi’s Feedback Limitations

This appendix is devoted to showing that, in spite of stability, Choi’s original feedback has a poor damping of the closed-loop dynamics in practical operating conditions. Applying the control strategy of Equation (4) to the model of Figure 1, we obtain a closed-loop system with three design gains ${k}_{v},{k}_{c},{k}_{p}$ and one coefficient ${w}_{n}$, as depicted in Figure A1. In classical linear control theory, it is customary to introduce disturbances in the input and output of the system and to study the closed-loop performance, analyzing the open-loop transfer function (t.f.), and the related closed-loop sensitivity functions linking the output to the input and output noises. The block diagram presents two partial feedback loops, on $COG$ and on $ZMP$, that can be analyzed separately by opening (indicated with an X in the block diagram) the two feedbacks one at a time and considering the other part of the system.

The functions related to the $ZMP$ loop are not particularly interesting: the feedback of the $ZMP$, although important for stability reasons, does not play any role in disturbance rejection and tracking error because its steady-state loop gain is always lower than 1. In fact, it is given by
where ${k}_{p}<{k}_{c}$ always holds.

$${G}_{{a}_{zmp}}\left(s\right)=\frac{{k}_{v}{k}_{p}(1-{s}^{2}/{w}_{n}^{2})}{{s}^{3}+{k}_{v}s+{k}_{v}{k}_{c}},$$

Vice versa, the following t.f.s of the $COG$ loop are noteworthy: the open-loop transfer function ${G}_{a}\left(s\right)$, the output tracking error sensitivity $S\left(s\right)$, and the output sensitivity to input disturbances ${G}_{e{q}_{\u03f5\u2013COG}}\left(s\right)$.

$${G}_{a}\left(s\right)=\frac{{k}_{v}{k}_{c}}{{s}^{3}+{k}_{v}{k}_{p}/{w}_{n}^{2}{s}^{2}+{k}_{v}s-{k}_{v}{k}_{p}}$$

$$S\left(s\right)=\frac{{s}^{3}+{k}_{v}{k}_{p}/{w}_{n}^{2}{s}^{2}+{k}_{v}s-{k}_{v}{k}_{p}}{{s}^{3}+{k}_{v}{k}_{p}/{w}_{n}^{2}{s}^{2}+{k}_{v}s+{k}_{v}({k}_{c}-{k}_{p})}$$

$${G}_{e{q}_{\u03f5\u2013COG}}\left(s\right)=\frac{{k}_{v}}{{s}^{3}+{k}_{v}{k}_{p}/{w}_{n}^{2}{s}^{2}+{k}_{v}s+{k}_{v}({k}_{c}-{k}_{p})}$$

In order to guarantee stability (negative real part of the roots of the third-order, closed-loop, characteristic polynomial appearing as denominator in Equations (A3) and (A4)), the following condition on the parameter ${k}_{p}$ must be satisfied:

$$\frac{{w}_{n}^{2}}{{w}_{n}^{2}+{k}_{v}}{k}_{c}<{k}_{p}<{k}_{c}\phantom{\rule{3.33333pt}{0ex}},$$

Note that condition (A5) is slightly different from Choi’s result.

In order to have more insights about the closed-loop poles of Equations (A3) and (A4), consider the root locus, function of ${k}_{v}$, of the following open-loop transfer function:

$${G}_{{a}_{{k}_{v}}}\left(s\right)=\frac{{k}_{v}({k}_{p}/{w}_{n}^{2}{s}^{2}+s+{k}_{c}-{k}_{p})}{{s}^{3}}.$$

Note that the numerator of $1+{G}_{{a}_{{k}_{v}}}\left(s\right)$ is exactly the characteric polynomial of (A3) and (A4). When ${k}_{v}\to \infty $, one real closed-loop pole $\to -\infty $, but the dominant closed-loop poles are complex conjugate and approach asymptotically the zeroes of the t.f. (A6), i.e., the root of the polynomial
having the damping ratio

$${s}^{2}+{w}_{n}^{2}/{k}_{p}s+{w}_{n}^{2}({k}_{c}-{k}_{p})/{k}_{p},$$

$$\zeta =\frac{{w}_{n}}{2}\sqrt{\frac{1}{{k}_{p}({k}_{c}-{k}_{p})}}.$$

Moreover, the root locus shows that for any value of ${k}_{v}<\infty $, the damping ratio of the pair of dominant complex conjugate poles is always lower than that of these zeroes. From previous results, the following observations can be made:

- with high values of the ${k}_{v}$ gain, the dominant closed-loop poles depend on the pair ${k}_{c},{k}_{p}$ only, as they are highly insensitive to ${k}_{v}$;
- the steady-state $COG$ loop gain (A2) is proportional to the rate ${k}_{c}/{k}_{p}$ (independent from ${k}_{v}$);
- the steady-state gain of both sensitivities related to $COG$ are inversely proportional to ${k}_{c}-{k}_{p}$;
- however, if the gain ${k}_{c}$, or more precisely, the difference ${k}_{c}-{k}_{p}$, increases, then the damping ratio of the dominant closed-loop poles decreases.

In conclusion, if a sufficiently high loop gain in the $COG\u2013ZMP$ control system is imposed, with the feedback proposed by Choi, even if the closed loop remains stable, its behavior becomes highly undamped. However, a high loop gain, and hence a high value of ${k}_{c}$ is needed when a robust control has to be used in exoskeletons to improve postural equilibrium for ill or elderly people, in order to cope with uncertainties.

## References

- Vukobratovic, M.; Borovac, B.; Surla, D.; Stokic, D. Biped Locomotion: Dynamics, Stability, Control, and Application; Springer: Berlin/Heidelberg, Germany, 1990. [Google Scholar]
- Kajita, S.; Kanehiro, F.; Kaneko, K.; Fujiwara, K.; Harada, K.; Yokoi, K.; Hirukawa, H. Biped Walking Pat-tern Generation by using Preview Control of Zero-Moment Point. In Proceedings of the 2003 IEEE International Conference on Robotics and Automation, Taipei, Taiwan, 4–19 September 2003. [Google Scholar]
- Choi, Y.; Kim, D.; Oh, Y.; You, B. Posture/Walking Control for Humanoid Robot Based on Kinematic Resolution of CoM Jacobian With Embedded Motion. IEEE Trans. Robot.
**2007**, 23, 1285–1293. [Google Scholar] [CrossRef] - Menga, G.; Ghirardi, M. Modelling, Simulation and Control of the Walking of Biped Robotic Devices—Part I: Modelling and Simulation Using Autolev. Inventions
**2016**, 1, 6. [Google Scholar] [CrossRef] - Menga, G.; Ghirardi, M. Modeling, Simulation and Control of the Walking of Biped Robotic Devices, Part II: Rectilinear Walking. Inventions
**2016**, 1, 7. [Google Scholar] [CrossRef] - Menga, G.; Ghirardi, M. Modeling, Simulation and Control of the Walking of Biped Robotic Devices—Part III: Turning while Walking. Inventions
**2016**, 1, 6. [Google Scholar] [CrossRef] - Menga, G.; Ghirardi, M. Lower Limb Exoskeleton for Rehabilitation with Improved Postural Equilibrium. Robotics
**2018**, 7, 28. [Google Scholar] [CrossRef] - Menga, G.; Ghirardi, M. Control of the sit-to-stand transfer of a biped robotic device for postural rehabilitation. Robotics
**2019**, 8, 91. [Google Scholar] - Espiau, B.; Boulic, R. On the Computation and control of the mass center of articulated chains. In Research Report INRIA, n.3479; INRIA: Rocquencourt, France, 1998; Available online: https://hal.inria.fr/inria-00073210/document (accessed on 10 June 2019).
- Kajita, S.; Morisawa, M.; Miura, K.; Nakaoka, S.; Harada, K.; Kaneko, K.; Kanehiro, F.; Yokoi, K. Biped Walking Stabilization Based on Linear Inverted Pendulum Tracking. In Proceedings of the 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems, Taipei, Taiwan, 18–22 October 2010. [Google Scholar]
- Kasaei, M.; Lau, N.; Pereira, A. Comparison Study of Well-Known Inverted Pendulum Models for Balance Recovery in Humanoid Robot. arXiv
**2019**, arXiv:1906.01936. [Google Scholar] - Levenberg, K. A Method for the Solution of Certain Non-linear Problems in Least Squares. Q. Appl. Math.
**1944**, 2, 164–168. [Google Scholar] [CrossRef] - Marquardt, D. An Algorithm for the Least-Squares Estimation of Nonlinear Parameters. SIAM J. Appl. Math.
**1963**, 11, 431–441. [Google Scholar] [CrossRef] - Colaneri, P.; Geromel, C.; Locatelli, A. Control Theory and Design: An RH2 and RHinf Viewpoint; Academic Pressl: San Diego, CA, USA, 1997. [Google Scholar]
- Zhou, K.; Doyle, J.; Glover, K. Robust and Optimal Control; Prentice Hall: Upper Saddle River, NJ, USA, 1996. [Google Scholar]
- Cotton, S.; Vanoncini, M.; Fraisse, P.; Ramdani, N.; Demircan, E.; Murray, A.; Keller, T. Estimation of the centre of mass from motion capture and force plate recordings: A study on the elderly. Appl. Bionics Biomech.
**2011**, 8, 67–84. [Google Scholar] [CrossRef] - Cotton, S.; Murray, A.; Fraisse, P. Estimation of the Center of Mass: From Humanoid Robots to Human Beings. IEEE/ASME Trans. Mechatron.
**2009**, 14, 707–712. [Google Scholar] [CrossRef] - Menga, G. Automatic Control. DAUIN—Politecnico di Torino. Available online: https://www.dropbox.com/s/jhkai3pabok0t8r/Book.pdf (accessed on 21 October 2019).
- Menga, G. Automatic Control Software. DAUIN—Politecnico di Torino. Available online: https://www.dropbox.com/s/xs5mwy4hvzqigcn/controldesign3.zip (accessed on 21 October 2019).

**Figure 9.**Results of estimating the statically equivalent serial chain (SESC) model in static experiments.

**Figure 11.**Maintaining the $CoP$ position during a stand-to-sit like exercise in the presence of a disturbance force.

**Figure 13.**COG–ZMP during stand-to-sit and shift of $CoP$ in a nonlinear 3 DOF kinematics-robust control.

© 2019 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).