# Control of mechanical systems

Post-publication activity

Curator: Suguru Arimoto

Mechanical systems as a controlled objective are mostly characteristic of multi-DOF (Degrees-Of-Freedom), existence of strong nonlinearities due to rotational joints, subjection to physical constraints, and redundancy in DOF. The desirable control goals may be related to not only physical variables of position and velocity of the system but also force or torque that is exerted on environment.

In nature, motion of mechanical systems is governed by the Lagrange equation that follows from the variational principle in mechanics, as described early on by Landau and Lifschitz (1960).

Robot motion control is typical of control of mechanical systems with multi degrees-of-freedom. Motion of a robotic arm with $$n$$ joints shown in Figure 1 is governed by the Lagrange equation in terms of the vector of joint angle $$q = (q_1, \cdots, q_n)^{\rm T}\ :$$ $\tag{1} H (q) \ddot{q} + \left\{ \frac{1}{2} \dot{H} (q) + S (q, \dot{q}) \right\} \dot{q} + g (q) = u$

where $$\dot{q}$$ denotes the vector of joint angular velocities defined as $$\dot{q} = {\rm d}q/ {\rm d}t\ ,$$ the derivative of $$q$$ in time parameter $$t\ ,$$ $$\ddot{q} = {\rm d} \dot{q}/ {\rm d} t\ ,$$ $$H (q) = (h_{ij} (q))$$ the $$n \times n$$ inertia matrix, $$g (q)$$ the gravity torque vector defined as a gradient of the gravity potential $$P (q)\ ,$$ that is, $$g (q) = \partial P (q)/ \partial q\ ,$$ $$u$$ the external joint torque that can be regarded as control input, and $$S (q, \dot{q}) = (s_{ij} (q))$$ is given by (e.g. Arimoto (1996)) $\tag{2} s_{ij} (q) = \frac{1}{2} \left\{ \frac{\partial}{\partial q_j} \left( \sum_{k=1}^n \dot{q}_k h_{ik} (q) \right) - \frac{\partial}{\partial q_i} \left( \sum_{k=1}^n \dot{q}_k h_{jk} (q) \right) \right\}$

From this form, $$S (q, \dot{q})$$ is homogeneous in $$\dot{q}$$ and skew-symmetric, that is, $$S^{\rm T} (q, \dot{q}) = - S (q, \dot{q})\ ,$$ and hence $$\dot{q}^{\rm T} S (q, \dot{q}) \dot{q} = 0\ .$$ The inner product of eq. (1) and $$\dot{q}$$ leads to $$\dot{q}^{\rm T} u = {\rm d} E (q, \dot{q})/ {\rm d} t\ ,$$ where $$E (q, \dot{q}) = K (q, \dot{q}) + P (q), \ K (q, \dot{q}) = (1/2) \dot{q}^{\rm T} H (q) \dot{q}\ .$$ Here, $$K$$ stands for the kinetic energy and $$E$$ the total energy of the system. Then, the Lagrange equation of motion described by eq.(1) follows from the variational principle applied to the Lagrangian$L (q, \dot{q}) = K (q, \dot{q}) - P (q)\ .$ Since the number of independent control inputs $$n=\dim(u)$$ is equal to the number of DOF, such mechanical systems are said fully actuated.

## Passivity-based Control

Energy is one of the fundamental concepts in control of mechanical systems with multi-degrees-of-freedom. The action of a controller can be understood in energy terms as a dynamical system called "actuator" that supplies energies to the controlled system, upon interconnection, to modify desirably the behavior of the closed-loop (interconnected) system. This idea has its origin in Takegaki and Arimoto (1981) and is later called the "energy-shaping" approach, which is now known as a basic controller design technic common in control of mechanical systems. Its systematic interpretation is called "passivity-based control".

Given a target posture $$q_d$$ for a robot manipulator, consider the two control inputs: $\tag{3} {\rm a)}\quad u = g (q) - C \dot{q} - A (q - q_d), \qquad {\rm b)} \quad u = g (q_d) - C \dot{q} - A (q - q_d)$

where $$C$$ and $$A$$ denote an $$n \times n$$ constant gain matrix with positive definiteness. This signal form is called a "PD (Position and Derivative) feedback" with gravity compensation. It presumes not only real-time measurements of joint angles $$q_i$$ and angular velocities $$\dot{q}_i$$ but also real-time computation of the gravity function $$g (q)$$ in the case of a). Gravity compensation in the case of b) is regarded as a target torque regulation that enough withstands the external joint torques to be exerted from the gravitational force at its target position. Substitution of the control signal b) of eq.(3) into eq.(1) yields $\tag{4} H (q) \ddot{q} + \left\{ \frac{1}{2} \dot{H} (q) + S (q, \dot{q}) + C \right\} \dot{q} + g (q) - g (q_d) + A (q - q_d) = 0$

that is called the closed-loop dynamics. The position control problem for the system with a specified target posture $$q = q_d$$ is now interpreted in terms of the mathematical method of Lyapunov's stability, that is, to prove the theorem that any solution trajectory (that is called "orbit") to eq.(4) starting from a neighborhood of the equilibrium state $$(q_d, \dot{q} = 0)$$ remains in its vicinity and converges asymptotically to it as $$t \to \infty\ .$$ This stability proof can be established by finding Lyapunov's relation by taking the inner product of eq.(4) and $$\dot{q}\ ,$$ which results in $\tag{5} \frac{\rm d}{{\rm d}t} \left\{ K (q, \dot{q}) + \bar{P} (\Delta q) \right\} = - \dot{q}^{\rm T} C \dot{q}$

where $$\Delta q = q - q_d$$ and $$\bar{P} (\Delta q) = P (q) - P (q_d) + \Delta q^{\rm T} g (q_d) + (1/2) \Delta q^{\rm T} A \Delta q\ .$$ Now the new total energy of the closed-loop system is expressed as $$\bar{E} = K + \bar{P}$$ and it is possible to check that, by choosing the gain matrix $$A$$ adequately, $$\bar{P}$$ becomes positive definite in $$\Delta q\ .$$

Then, the total energy becomes positive definite with respect to the state vector $$(\Delta q, \dot{q})$$ and has a minimum at $$(0, 0)$$ as the desired state. By virtue of Lyapunov's relation of eq.(5) and recalling LaSalle's invariance theorem, the orbits converge to the state that minimizes the total energy, and hence the equilibrium is asymptotically stable. However, note that this stability theorem does not provide any practical information about the speed of convergence to the desired state.

The choice of gain $$A$$ of the artificial potential and gain $$C$$ of damping injection is far from obvious from the practical point of view.

The energy-shaping approach can be applied for the task-space position control problem. Given a target endpoint position $$\mathbf{x} = \mathbf{x}_d$$ where $$\mathbf{x} = (x, y, z)$$ in $$\mathbf{E}^3\ ,$$ it is possible to design a control signal as $\tag{6} u = g (q) - C \dot{q} - k J^{\rm T} (q) (\mathbf{x} - \mathbf{x}_d)$

where $$J (q)$$ signifies the Jacobian matrix $$\mathbf{x}$$ in $$q\ ,$$ that is, $$J (q) = \partial \mathbf{x}^{\rm T}/ \partial q$$ and $$J^{\rm T} (q)$$ the transpose of $$J (q)\ .$$ If the dimension of the task space (dim $$\mathbf{x}$$) is equal to the number of arm joints, then along an orbit to the closed-loop dynamics (when eq.(6) is substituted into eq.(1)) Lyapunov's relation follows as $${\rm d} \bar{E}/ {\rm d} t = - \dot{q}^{\rm T} C \dot{q}\ ,$$ where $$\bar{E} = K + (k/2) \| \Delta \mathbf{x} \|^2\ .$$ This total energy can be regarded as a positive definite Lyapunov function, as long as $$J (q)$$ is nondegenerate. Therefore, asymptotic stability must be treated in a careful manner. In particular, the number of DOF of the robot exceeds the dimension of the task space, $$J (q)$$ becomes non-square and inverses of $$q = \mathbf{x}^{-1} (\mathbf{x}_d)$$ exist numerously as it is so-called "ill-posedness of inverse kinematics".

Without invoking knowledge of the gravity term $$g (q)$$ or $$g (q_d)$$ in eq.(3), a PID control signal can be devised by adding a term of integral of position error $$\Delta q$$ in $$t$$ to the PD feedback (e.g. Arimoto (1996)). Another PID scheme using a saturated position error is proposed, which leads to global asymptotic stability (Arimoto (1995)).

## Position/Force Control under Constraints

In many practical problems of control of multi-DOF systems, some part of the body is geometrically constrained with environment. For example, consider Figure 2 where the endpoint $$\mathbf{x}$$ of the 3 DOF manipulator is in contact with a fixed surface. This kind of constraints that can be expressed in finite terms, are called holonomic constraints in contrast to non-holonomic ones that are encountered in mobile robots for examples. The term non-holonomic is due to the Germain physicist Heinrich Rudolf Hertz.

Dynamics of such a system with constraint can be modeled by the Lagrange equation $\tag{7} H (q) \ddot{q} + \left\{ \frac{1}{2} \dot{H} + S \right\} \dot{q} + g (q) = \lambda J^{\rm T} (q) \frac{\partial \varphi}{\partial \mathbf{x}}$

where $$J (q) = \partial \mathbf{x}/ \partial q$$ and $$\lambda$$ denotes a Lagrange multiplier. Actually, this equation follows from applying the variational principle to the Lagrangian $$L = K (q, \dot{q}) - P (q) + \lambda \varphi (\mathbf{x} (q))\ .$$ Physically, $$\mathbf{f} = - \lambda \partial \varphi (\mathbf{x})/ \partial \mathbf{x}$$ stands for the constraint force with which the endpoint presses the constraint surface in the direction normal to it. Then, a control problem of positioning of the endpoint toward $$\mathbf{x} = \mathbf{x}_d$$ together with targetting the magnitude of pressing force $$\lambda = \lambda_d$$ becomes sensible, which was first tackled by McClamroch and Wang (1990) and is now called "position/force hybrid control". It is shown that the control signal $\tag{8} u = g (q) - C \dot{q} - J^{\rm T} (q) \left\{ k (\mathbf{x} - \mathbf{x}_d) + \lambda_d \frac{\partial \varphi}{\partial \mathbf{x}} \right\}$

renders the closed-loop system asymptotically stable in the sense that $$\mathbf{x} (t) \to \mathbf{x}_d$$ and $$\lambda (t) \to \lambda_d$$ as $$t \to \infty$$ (e.g. Wang and McClamroch (1993)).

## Control of Redundant Systems

It has been understood so far that the PD feedback with damping shaping is in effect for a multi-DOF system when the dimension of task space is equal to the total DOF of the system. For a given target endpoint position $$\mathbf{x}_d\ ,$$ to obtain a posture $$q$$ whose endpoint coincides with $$\mathbf{x}_d\ ,$$ $$\mathbf{x} (q) = \mathbf{x}_d\ ,$$ is called the inverse kinematics. Even in a case of planar reaching movement for a planar arm with excess joints, the inverse kinematics becomes unsolvable, because there arises an infinite number of possible postures $$q$$ that may satisfy $$\mathbf{x} (q) = \mathbf{x}_d$$ (see Figure 3). In this meaning, the inverse kinematics is said to be illposed. It implies also an infinite number of different endpoint trajectories and different joint trajectories starting from a given initial posture and reaching at the specified endpoint position (see Figure 3). This problem is called Bernstein's DOF problem (Bernstein (1996)) in developmental psychology and neuro-physiology. In order to resolve this a variety of optimization methods to uniquely determine a joint or endpoint trajectory have been proposed, such as a quadratic form of jerk, acceleration, torque, or torque change and an energy or fatigue criterion in the area of not only robotics (e.g. Hollerbach (1987)) but also neuro-physiology (Hogan (1985)) and developmental psychology (Thelen and Smith (1996)). In such an illustrative example of reaching as shown in Figure 3, a control signal with the form $\tag{9} u = - C \dot{q} - J^{\rm T} (q) \left\{ \zeta \sqrt{k} \dot{\mathbf{x}} + k \Delta \mathbf{x} \right\}$

works well if the stiffness $$k$$ and the damping matrix $$C$$ are carefully chosen, where $$\zeta$$ can be set around $$\zeta \approx 1/ \sqrt{2}$$ as a non-dimensional constant. Since in this case the gravity term $$g (q)$$ is missing, along a joint trajectory of the closed-loop equation when eq.(9) is substituted into eq.(1) it follows that $${\rm d} E (\Delta \mathbf{x}, \dot{q})/ {\rm d} t = - \dot{q}^{\rm T} C \dot{q} - \zeta \sqrt{k} \| \dot{\mathbf{x}} \|^2\ ,$$ where $$E (\Delta \mathbf{x}, \dot{q}) = (1/2) \{ \dot{q}^{\rm T} H (q) \dot{q} + k \| \Delta \mathbf{x} \|^2 \}\ .$$ Note that $$E (\Delta \mathbf{x}, \dot{q})$$ is not positive definite with respect to joint angle vector $$q$$ and therefore it can not be regarded as a Lyapunov function. Instead, it is possible to take into account a cross term in addition to the total energy in such a way that $\tag{10} W (\alpha, \Delta q, \dot{q}) = E (\Delta \mathbf{x}, \dot{q}) + \alpha k \Delta \mathbf{x}^{\rm T} J (q) C^{-1} H (q) \dot{q}$

with a small positive parameter $$\alpha\ .$$ Then, it can be shown (Arimoto et al. (2005)) that, under suitable choices of $$k$$ and $$C\ ,$$ there exist positive constants $$\alpha$$ and $$\gamma$$ such that $$W$$ and $$E$$ satisfy 1) $$(1 - \alpha) E \le W \le (1 + \alpha) E$$ and 2) $${\rm d} W/ {\rm d} t \le - \gamma W\ .$$ Therefore, the total energy converges exponentially to zero with exponent $$\gamma > 0\ ,$$ that means $$\mathbf{x} (t) \to \mathbf{x}_d\ ,$$ $$\dot{q} (t) \to 0\ ,$$ and the posture itself $$q (t)$$ tends to some still state $$q_{\infty}$$ satisfying $$\mathbf{x} (q_{\infty}) = \mathbf{x}_d\ .$$ Nevertheless, it is important to evaluate the exponent parameter $$\gamma$$ that represents the speed of convergence. Due to the redundancy in DOF, undesired "self-motion" may occur in the transient behavior of joint trajectories (Seraji (1989)). In other words, it should be noted that the distance $${\rm d} (q^1, q^2) = \| q^1 - q^2 \|$$ in joint space $$\mathbf{R}^n$$ differs from the Euclidean distance $${\rm d} (\mathbf{x}, \mathbf{x}_d)$$ $$(= \| \Delta \mathbf{x} \|)$$ with physical unit [m] in task space $$\mathbf{E}^2$$ or $$\mathbf{E}^3\ .$$

## Renewal of Stability Concept by Riemannian Distance

A set of all postures of a multi-joints mechanism can be regarded as a topological manifold denoted by $$M\ .$$ For example, a posture of the two-DOF manipulator shown in Figure 1 can be represented by joint angles $$( \theta_1, \theta_2)\ .$$ Therefore, the set of all postures can be regarded as a direct product of unit circle $$S^1\ ,$$ that is, $$T^2 = S^2 \times S^1$$ that is called a torus. Motion of the manipulator can be expressed as a curve $$q (t)$$ called an orbit on the torus. Then, given two points $$p$$ and $$p'$$ on $$T^2\ ,$$ there are numerous smooth curves that map $$I = [a, b] \to M$$ and connect the points. Define the length of a curve $$q (t)$$ as $\tag{11} L (q) = \int_a^b \sqrt{\sum g_{ij} (q) \dot{q}^i (t) \dot{q}^j (t)} {\rm d} t$

where $$g_{ij} (q)$$ is set identically to $$h_{ij} (q)$$ of the entry of inertia matrix $$H (q)\ .$$ Then, the curve $$q (t)$$ that minimizes $$L (q)$$ connecting the points $$q (a) = p$$ and $$q (b) = p'$$ among all possible curves is called "geodesic", and it is known in Riemannian geometry that such a minimizing curve should satisfy the Euler equation $\tag{12} \ddot{q}^k + \Gamma_{ij}^k \dot{q}^i \dot{q}^j = g^{kj} u^j, \qquad k = 1, \cdots, n$

with free input, i.e., $$u = 0\ ,$$ where $$\Gamma_{ij}^k$$ denotes Christoffel's symbol and $$g^{kj}$$ denotes the inverse of $$G (q)$$ $$(= H (q))\ .$$ Importantly, multiplication of eq.(12) from the left by $$G (q)$$ reproduces exactly eq.(1) when the gravity term is excluded. Moreover, geodesics are invariant under any choice of local coordinate chart (that is, configuration space). For example, the set of all endpoints $$\mathbf{x}$$ of the two-DOF robot in Figure 1 also constitutes physically a torus in $$\mathbf{E}^3$$ as shown in Figure 4, that expresses another choice of local coordinates rather than a mathematical torus $$T^2$$ $$( = S^1 \times S^1)\ .$$ Nevertheless, the Riemannian distance $$d (p, p')$$ as well as geodesics is invariant and hence the set of all postures can be regarded as a metric space $$\{ M, g_{ij} \}$$ called the Riemannian manifold. Stability concept of position control or position/force hybrid control for redundant mechanical systems can be renovated with the aid of Riemannian distance.

## Model-based Adaptive Control of Mechanical Systems

When the goal of control is given as a desired joint trajectory $$q_d (t)$$ together with its velocity and acceleration, real-time estimation of nonlinear terms in the Lagrange equation of motion becomes indispensable. To be fortunate, important physical parameters such as link masses and inertia moments appear linearly in the left hand side of eq.(1). This leads to the idea that the Lagrange equation can be expressed as $$Y (\ddot{q}, \dot{q}, \dot{q}, q) \Theta = u$$ where $$\Theta$$ signifies an $$m$$-dim. vector of such physical parameters and $$Y$$ denotes an $$n \times m$$-matrix computable based upon the knowledge of $$\ddot{q}\ ,$$ $$\dot{q}\ ,$$ and $$q\ .$$ Since the acceleration $$\ddot{q}$$ can not be assumed accessible for controller design, the control signal is suggested in the following form: $\tag{13} u = - A_1 \Delta q - B_1 \Delta \dot{q} + Y (q_d, \dot{q}_d, \dot{q}_d, \ddot{q}_d) \hat{\Theta}$

where $$\hat{\Theta}$$ is continuously updated by $\tag{14} \hat{\Theta} (t) = \hat{\Theta} (0) - \int_0^t \Gamma^{-1} Y^{\rm T} \left( q_d (\tau), \dot{q}_d (\tau), \dot{q}_d (\tau), \ddot{q}_d (\tau) \right) y (\tau) {\rm d} \tau$

where $$\Delta q = q - q_d\ ,$$ $$\Delta \dot{q} = \dot{q} - \dot{q}_d\ ,$$ and $$y = \Delta \dot{q} + \gamma \Delta q\ .$$ This method of adaptive scheme was originally proposed by Slotine and Li (1987) and now called the model-based adaptive control.