Control and Simulation of Motion of Constrained Multibody Systems Based on Projection Matrix Formulation

Many robotic systems are formulated as multibody systems with closed-loop topologies, such as manipulators with end-effector constraints [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11], cooperative manipulators [12, 13, 14, 15, 16], robotic hands for grasping objects [17, 18, 19, 20], parallel manipulators [21, 22, 23, 24], impact in robotics [25], humanoid robot and walking robots [26, 27, 28], and VR/Haptic applications [29, 30]. Simulation and control of such systems call for corresponding direct dynamics and inverse-dynamics models, respectively. Mathematically, constrained mechanical systems are modeled by a set of $n$ differential equations coupled with a set of $m$ algebraic equations, i.e. Differential Algebraic Equations (DAE). Although computing the dynamics model is of interest for both simulation and control, the research done in these two areas are rather divided. Surveys of the existing techniques for solving DAE may be found in [31, 32, 33, 34, 35, 29], while model-based control of constrained manipulators can be found in [4, 2, 36, 5, 37, 38, 39, 40, 41, 42, 43].

The classical method to deal with DAE is to express the constraint condition at the acceleration level. This allows replacement of the original DAE system with an ODE system by augmenting the inertia matrix with the second derivative of the constraint equation. However, this method performs poorly in the vicinity of singularities [44, 45, 46], because the augmented inertia matrix is invertible only with a full rank Jacobian matrix.

Other methods are based on coordinate partitioning [47, 48, 49] by using the fact that the $n$ coordinates are not independent because of the $m$ constraint equations. The motion of the system can be described by the independent coordinates which can be separated using an annihilator operator. Although this method may significantly reduce the number of equations, finding the annihilator operator is a complex task [29]. Moreover; the sets of independent and dependent coordinates should be determined first. But a fixed set of independent coordinates occasionally leads to ill-conditioned matrices [31, 50] when the system changes its topology or the number of degrees of freedom. The concept of coordinate separation is used in [9]for controlling manipulator robots with constrained end-effectors. The augmented Lagrangian formulation proposed in [51, 52, 53] can handle redundant constraints and singular situations. However, this formulation solves the equations of motion through an iterative process. Nakamura et al. [54] developed a general algorithm that provides a way to partition the coordinates into independent and dependent ones even around the singular configuration, which is suitable for simulation of mechanical systems with structure-varying kinematic chains. This is a special case of the projection method proposed herein that allows generic constraints which cannot be handled by coordinate partitioning.

There are also efficient algorithms for solving direct dynamics of constrained systems that are suitable for parallel processing. Feathersone’s work in [55, 56] presents a recursive algorithm, which is called Divide-and-Conquer (DAC), for calculating the forward dynamics of general rigid-body system on a parallel computer. The central formula for these DAC algorithm takes the equations of motions of two independent subassembly (rigid-body) and also a description of how they are to be connected, and the output is the equation of motion of the assembly, i.e., those of two articulated-body. Since the equation of acceleration of the assembly is written in terms of two independent equation of motions, the formulation is suitable for parallel processing and one can apply the formula recursively to construct the articulated-body equations of motions of an entire rigid-body assembly from those of its constituent parts. The author claims that the DAC algorithm is computationally effective if a large number of processors, more than $100$, is available.

Another group of researchers [52, 57, 58, 46, 59, 60] focused on other techniques to deal with the problem of accurately maintaining the constraint condition. Blajer [58, 59] proposed an elegant geometric interpretation of constrained mechanical systems. Then the analysis was extended and modified in [61] for control application. The augmented Lagrangian formulation proposed in [51, 52, 53] can handle redundant constraints and singular situations. However, this formulation solves the equations of motion through an iterative process.

In the realm of control of constrained multibody system, the vast majority of the literature is devoted to control of manipulators with constrained end-effectors. The hybrid position/force control concept was originally introduced in [4], and then the manipulator dynamic model was explicitly included in the control law in [2]. The constrained task formulation with inverse-dynamics controller is developed in [5, 7] by assuming that the Cartesian constraints are linearly independent. Hybrid motion/force control proposed in [62, 63, 64] achieves a complete decoupling between channels of acceleration and force. In these approaches all joints are assumed to have an actuator and no redundancy was considered in the kinematic constraint.

In this work, we propose a new formulation for the direct and the inverse-dynamics of constrained mechanical systems based on the notion of a projection operator [1, 6]. First, constraint reaction forces are eliminated by projecting the initial dynamic equations into the tangent space with respect to the constraint manifold. Subsequently, the direct dynamics, or the equations of motion, is derived in a compact form that relates explicitly the generalized force to the acceleration by introducing a constraint inertia matrix, which turns out to be always invertible. The constraint reactions can then be retrieved from the dynamics projection in the normal space [1]. Unlike in the other formulations, the projection matrix is a square matrix of order equal to the number of dependent coordinates. Since the formulation of the projector operators is based on pseudo-inverting the constraint Jacobian (the process not conditioned upon the maximal rank of the Jacobian), the present approach is valid also for mechanical systems with redundant constraints and/or singular configurations, which is unattainable with many other classical methods. A projected inverse-dynamics control scheme is developed based on the dynamics formulation. The motion control proves to be stable while minimizing the weighted Euclidean norm of actuation force. The notion of the projected inverse-dynamics is further developed for control of constrained mechanical systems which have passive joints, i.e., joints with no actuator. This result is particulary important for control of parallel manipulators. Finally, a hybrid force/motion control scheme based on the proposed formulation is presented. Also some useful insights are gained from the dynamics formulation. For instance, the condition on the inertia matrix for achieving a complete decoupling between force and motion equations is rigorously derived.

This paper is organized as follows: We begin with the notion of linear operator equations in Section 2 by reviewing some basic definitions and elementary concepts which will be used in the rest of the paper. Using the projection operator, we derive models of inverse and direct dynamics in sections 4 and 5 which are used as a basis for developing strategies for simulation and control of constrained mechanical systems in sections 6 and 7. Section 7.2 presents change of coordinate if there is inhomogeneity in the spaces of the force and velocity. In section 7.4, the inverse-dynamics control scheme is extended for constrained systems which have some joints with no actuator (passive joints).

For any linear operator transformation $A:\mathbb{R}^{n}\rightarrow\mathbb{R}^{m}$, range space and null space are defined as ${\cal R}(A)=\{y\in\mathbb{R}^{m}:\exists x\in\mathbb{R}^{n}\ni y=Ax\}$ and ${\cal N}(A)=\{x\in\mathbb{R}^{n}:Ax=0\}$, respectively. The linear transformation maps vector space ${\cal X}$ into vector space ${\cal Y}$. Assume that the Euclidean inner-product is defined in ${\cal X}$, that is elements of vectors, such as $x_{1}$ and $x_{2}$, of ${\cal X}$ have homogeneous units. Then, by definition the vectors are orthogonal iff their inner-product is zero, i.e.

where the superscript ^T denotes transpose. It follows that the orthogonal complement of any set ${\cal S}$, denoted by ${\cal S}^{\perp}$, is the set of vectors each of which is orthogonal to every vector in ${\cal S}$.

See Appendix A for a proof.

As will be seen in the following sections, it is desirable to be able to project any vector in $\mathbb{R}^{n}$ to the null space of $A$ by a projector operator. Let $P\in\mathbb{R}^{n\times n}$ be the orthogonal projection onto the null space, i.e., ${\cal R}(P)={\cal N}(A)$. Note that every orthogonal projection operator has these properties: $P^{2}=P$ and $P^{T}=P$ [66].

The projection operator can be calculated by the singular value decomposition (SVD) method [66, 67, 68]. Assuming

then there exist unitary matrices $U=[U_{1}\;\;U_{2}]$ and $V=[V_{1}\;\;V_{2}]$ (i.e. $U^{T}U=I$ and $V^{T}V=I$) so that

where $\Sigma=\mbox{diag}(\sigma_{1},\cdots,\sigma_{r})$, and $\sigma_{1}\geq\cdots\geq\sigma_{r}\geq 0$ are the singular values. The proof of this statement is straightforward and can be found, for example, in [66, 67]. Since ${\cal N}(A)=\mbox{span}(V_{2})$ [66, 67], the projection operator can be calculated by

The kinematics of a constrained mechanical system can be represented by a set of $m$ nonlinear equations ${\Phi}(q)=[\phi_{1}(q),\cdots,\phi_{m}(q)]^{T}=0$, where $q\in\mathbb{R}^{n}$ is the vector of generalized coordinate, and $m\leq n$. Without loss of generality, we consider time-invariant (scleronomic) constraint conditions, but the methodology can be readily extended to a time varying case (rheonomic). By differentiating the constraint equation with respect to time, we have

where $A={\partial\Phi}/{\partial q}$ is the Jacobian of the constraint equation with respect to the generalized coordinate. For brevity of notation, in the following, we assume that the elements of the force and velocity vectors have homogeneous units. This assumption will be relaxed in Section 7.2 by changing of the coordinates similar to (8).

Equation (11) is expressed in form of the linear operation equation. This matrix equation specifies that any admissible velocity must belong to the null space of the Jacobian matrix, that is, $\dot{q}\in{\cal N}(A)$. Thus, the constraint equation (11) can be expressed by the notion of the projection operator, i.e.,

Time differentiation of the above equation yields

where $C:=\frac{d}{dt}P$ which, in turn, can be obtained from (4) by

It is apparent from equation (13) and (12) that, unlike the case of velocity, the null space orthogonal component of the acceleration is not always zero – a physical interpretation of (13) is given in Section 5.4. Equation (13) expresses the component of acceleration produced exclusively by the constraint and not by dynamics. As will be seen in Section 5, this equation can compliment the dynamics equation in order to provide sufficient independent equations for solving the acceleration.

Consider a constrained mechanical system with Lagrangian ${\cal L}={\cal T}-{\cal V}$, where ${\cal T}=\frac{1}{2}\dot{q}^{T}M\dot{q}$ and ${\cal V}(q)$ are the kinetic and the potential energy functions, and $M\in\mathbb{R}^{n\times n}$ is the inertia matrix. The fundamental equation of differential variational principles of a mechanical system containing constraint can be written as [32]

where $f_{tot}=\varphi-(f-{\cal F})$, $\varphi=\frac{d}{dt}\left(\frac{\partial{\cal L}}{\partial\dot{q}}\right)-\frac{\partial{\cal L}}{\partial q}$, ${f}\in\mathbb{R}^{n}$ is the vector of generalized input force, and ${\cal F}\in\mathbb{R}^{n}$ is the generalized constraint force, which is related to the Lagrange multipliers ${\lambda}\in\mathbb{R}^{m}$ by

Then, the equations describing the system dynamics can be obtained as

where vector $h(q,\dot{q})\in\mathbb{R}^{n}$ contains the Coriolis, centrifugal, gravitational terms. In solving the DAE equations (19)-(20), it is typically assumed that: (i) the inertia matrix is positive definite and hence invertible (ii) the constraint equations are independent, i.e. the Jacobian matrix is not rank-deficient [31, 32, 33, 34, 35, 29]. In this work, we solve the equations without relying on the second assumption.

From (18) and by virtue of Theorem 1, one can immediately conclude that ${\cal F}\in{\cal N}(A)^{\perp}$. In other words, the projection operator $P$ is an annihilator for the constraint force, i.e. $P{\cal F}=0$. Therefore, the constraint force can be readily eliminated from equation (19) if the equation is projected on $P$, i.e.

Equation (21) is called projected inverse-dynamics of a constrained multibody system that is expressed in the so-called descriptive form. This is because matrix $PM$ is singular and hence the acceleration cannot be computed from the equation through matrix inversion.

As mentioned earlier, the acceleration cannot be determined uniquely from equation (21), because there are fewer independent equations than unknowns. Nevertheless; equations (13) and (21) are in orthogonal spaces and thus cannot cancel out each other. Therefore, a unique solution can be obtained by solving these two equations together. To this end, we simply multiply equation (13) by $M$ and then add both sides of the equation to those of (21). After factorization, the resultant equation can be written concisely in the following form;

where $C_{c}:=MC$, and $M_{c}\in\mathbb{R}^{n\times n}$ is called constraint inertia matrix which is related to the unconstrained inertia matrix $M$—assuming a symmetric inertia matrix—by

and

Equation (22) constitutes the so-called direct dynamics of a constrained multibody system from which the acceleration can be solved. It is worth mentioning that if $M$ commutes with $P$, then $\tilde{M}=0$ and hence $M_{c}=M$. To compute the acceleration from (22) requires that the constraint inertia matrix be invertible.

Proof: It is evident from (24) that $\tilde{M}$ is a skew-symmetric matrix, i.e. $\tilde{M}^{T}=-\tilde{M}$. Consequently, adding $\tilde{M}$ to the inertia matrix in equation (23) preserves the positive definiteness property of the inertia matrix. This is because, for any vector $z\in\mathbb{R}^{n}$ we can say

Therefore, one can conclude that $z^{T}{M}z=z^{T}M_{c}z$, or

$\Box$ .

Theorem 2 is pivotal in showing the usefulness of the dynamics equation (22); it signifies that the constraint inertia matrix is always invertible regardless of the constraint condition. Therefore, the acceleration can be always obtained from (22).

It can be envisaged from Remark 2 that it is useful to decompose the generalized input force into two orthogonal components

where $f_{\parallel}\in{\cal N}$ and $f_{\perp}\in{\cal N}^{\perp}$ are called acting input force (potent) and passive input force (impotent), respectively. The decomposition of the generalized input force can be carried out by the projection operator according to (5). Now, the equation of motion can be written as

where the nonlinear vector is decomposed in the same way as the generalized force, i.e., $h=h_{\parallel}+h_{\perp}$. Equation (25) is the so-called equation of motion of a constrained mechanical system in a compact form. It is worth mentioning that only one matrix inversion operation is required in (25) which is one less than in the standard Lagrangian method (see Appendix B).

To simulate the dynamics of a constrained multibody system, one can make use of the acceleration model in (25). Having computed the generalized acceleration from the equation, one may proceed a simulation by integrating the acceleration to obtain the generalized coordinates. However, the integration inevitably leads to drift that eventually results in a large constraint error. Baumgarte’s stabilization term [35] is introduced to ensure exponential convergence of the constraint error to zero. However, this creates a very fast dynamics which tends to slow down the simulation. In this section, we use the pseudo-inverse for correcting the generalized coordinate in order to maintain the constraint condition precisely. It should be noted that using the pseudo-inverse here does not impose any extra computation burden, because the pseudo-inverse has to be obtained to compute the acceleration anyway.

Having obtained the velocity through integrating the acceleration, one may obtain the generalized coordinate by integration

where $\Delta T$ depicts integration time step. However, the constraint condition may be violated slightly because of integration drift. Let $q_{0}$ denote the coordinate after a few integration steps and $\Phi(q_{0})\neq 0$. Now, we seek a small compensation in the generalized coordinate $\delta q*=q^{*}-q_{0}$ such that the constraint condition is satisfied: that is, a set of nonlinear equations $\Phi(q^{*})=0$ must be solved in terms of $q^{*}$. The Newton-Raphson method solves a set of nonlinear equations iteratively based on linearized equations.

The constraint equation can be written by the first order approximation as

Neglecting the ${\cal O}(\delta q^{2})$ term, one can obtain the solution of the linear system using any generalized inverse of the Jacobian. The pseudo-inverse yields the minimum-norm solution, i.e., $\min_{A\delta{q}=-\Phi(q_{0})}\|{\delta q\|}$. Therefore, the following loop

may be worked out iteratively until the error in the constraint falls into an acceptable tolerance, e.g. $\|{\Phi}\|\leq\epsilon$.

The condition for local convergence of multi-dimensional Newton-Raphson (NR) iteration can found, e.g., in [73, 74, 31]. Although it is known that NR iteration will not always converge to a solution, the convergence is guaranteed if the initial approximation is close enough to a solution [73, 74].

One can expect that the drift, and hence the inial constraint error, can be reduced by decreasing the integration time step. It should be pointed out that the iteration loop (39) corrects the error in constraint coordinate caused by the integration process. Since the drifting error within a single integration time step $\Delta T$ is quite small, the initial estimate given by (38) cannot be far from the exact solution. Therefore, as shown by experiments, a fast convergence is achieved even though the iteration loop (39) is called once every few time steps.

Finally, the simulation of a constrained mechanical system based on the projection method can be proceeded in the following steps:

1. i.

   compute the acceleration from equation (25).

2. ii.

   obtain the states $\{q,\dot{q}\}$ as a result of numerical integration of the acceleration.

3. iii.

   in the case constraint error exceeds the tolerance, carry out iteration (39), upon convergence or counting the maximum number of iterations go to step(1).

In this section we discuss the position and/or force control of constrained multibody systems based on the proposed dynamics formulation. The I/O realization of a constrained mechanical system is depicted in Fig.1, which will be used subsequently as a basis for development of control algorithm. In fact, the topology of a control system can be inferred from the figure by considering the decomposed components of the generalized input force, $f_{\parallel}$ and $f_{\perp}$, as the corresponding control inputs for position and force feedback loops.

Due to the decoupled nature of the acceleration channel, an independent position feedback loop can be applied. The input channel $f_{\perp}$ is directly transmitted to the constraint force, $f_{\parallel}$, and the velocity enters as a disturbance and hence must be compensated for in a feedforward loop. Note that in the case of decoupled mechanical systems, where the cross-coupling channel vanishes, $f_{\perp}$ exclusively determines the constraint force.

In this section, we describe the results obtained from applying the proposed inverse and direct dynamics formulations for simulation and control of a slider-crank mechanism, Fig.3A. Assume that the dimension of the crank and the connecting rod are the same. Then, singular configurations occur at

see Fig.3B.