Configuration-based Compliance Control of Kinematically Redundant Robot Arm Part I – Theoretical Framework

When the robot endpoint compliance is dominantly influenced by the flexibility of its joints, the robot taskspace generalized stiffness matrix can be mapped onto jointspace using appropriate congruence transformation. Thus produced, the jointspace stiffness matrix is generally nondiagonal. Off-diagonal elements can be generated by redundant actuation only (polyarticular actuators). Although this kind of actuation is widely present in biological systems, its practical implementation in engineering systems is very difficult. To overcome this problem, use of kinematic redundancy is proposed. This two-part paper presents an approach to the control of robot endpoint compliance, i.e., elasto-mechanical interaction between a robot and its environment using kinematic redundancy instead of actuation redundancy. In Part I this approach is developed by proposing the Configuration-based Stiffness Control (CSC) method for kinetostatically consistent control of robot compliant behaviour, based on the gradient projection of the cost function which minimizes the norm of off-diagonal elements of the jointspace matrix.


INTRODUCTION
Compliance is essential for any robot task which includes constrained motion of the robot endpoint, or other movable parts of its mechanism.Compliance in motion control enables effective use of robots in tasks where small errors in model acquisition and estimation are present, since these errors can be compensated through compliant behaviour.Compliance is important for explorative activities in environments which are not well-structured or change over time.This also includes the environments populated by humans, in which physical proximity and physical interaction with humans are expected or planned, as in collaborative tasks with human workers on manufacturing lines, or in most medical applications where complex physical interaction between humans and robot(s) is inherent to robot tasks (minimally invasive surgery, robot assisted ultrasonography, rehabilitation, or even massage / relaxing muscle fatigue).
Regardless of mechanical and kinematical composition, any robot is an elastomechanical structure which deforms while interacting with the environment.Contrary to other elastomechanical mechanisms, flexibility of robot mechanism and its compliant behaviour can be actively controlled by the robot control system.
Compliance control, or more generally, impedance / admittance control is a classical research topic, studied for decades by many researchers, [1], [2], [3].Due to mechanical, sensing and control complexity, compliance control is still an open and very challenging research topic.Recent progress in the development of soft and humanoid robots (including robots with multiple and cooperating arms), especially their extensive implementation in complex tasks with intensive human-robot physical interaction as well as in constrained motion tasks where precise morphing of the robot taskspace stiffness is essential for successful task execution -increase the importance of compliance research, [4], but also backlash in joint actuation system [5].New understanding of underlying phenomena and consequently, new extensions, or at least refinements of the already existing theory are necessary, [6].
The robot compliant behaviour, which is considered in this paper is limited to the kinematically redundant robot mechanisms with rigid links and compliant, or more precisely, soft joints, driven by variable stiffness actuators.Kinematic redundancy is very important for compliance control.Although it is a complex mathematical and computational problem, kinematic redundancy is a source of freedom in task execution [7], because it provides the robot mechanism with an increased level of dexterity.
The inverse kinematics problem for a kinematically redundant robot mechanism allows an infinite number of solutions and therefore, additional criteria can be used to select one of them, [8].Probably the earliest report on investigation of the kinematic redundancy and the corresponding Jacobian matrix nullspace for additional/secondary tasks execution is given in [9], and later on formulated in [10] as the 'extended Jacobian technique'.
The inherent nullspace potentials of redundant Jacobian matrix is most frequently studied for singularity avoidance, optimization of joint range availability, workspace obstacle avoidance, or minimization of joint torques.
The increased level of dexterity is also used to optimize the taskspace stiffness of the robot mechanism, because kinematic redundancy also implies the redundancy in the force domain (force-displacement duality) [7], [11].A comprehensive study of compliance in redundant robot mechanisms and its active control based on complementary nullspace projectors is given in [6].The importance of conservative properties of the congruence transformation which maps taskspace stiffness to jointspace stiffness is pointed out in [12].The concept of Conservative Congruence Transformation (CCT) allows implementation of a joint level stiffness matrix without the off-diagonal elements.Theoretical analysis and an optimization solution for the problem of inducing a desired taskspace stiffness based on the CCT concept and kinematically uncoupled joints are given in [13].
In this paper, the Configuration-based Stiffness Control (CSC) method is formulated as a kinetostatically consistent framework for robot compliant behaviour control.Kinetostatical consistency is provided by gradient projection of an appropriate cost faction onto the Jacobian null space.The cost function is derived from the robot jointspace domain as a norm of off-diagonal elements of the jointspace matrix.Such cost function is physically correct, due to the homogeneity of the jointspace stiffness (which generally is not true for the taskspace stiffness domain).Since the optimization problem is set as a minimization of the norm of off-diagonal elements of the jointspace stiffness matrix, the optimal solution will be consistent with the monoarticular actuation system which drives the robot arm mechanism.In general, CSC method can be considered an extension of the Configuration Control concept which is proposed in [14], but cannot be related to the compliance control.
The remainder of this paper is organized as follows: a concept of generalized stiffness and jointspace stiffness is explained in section 2; a generalized virtual displacement of the robot endpoint and complementary nullspace projector are formulated in section 3; the proposed cost function for canonization of the jointspace stiffness matrix is presented in section 4; and finally, simulation examples are given in section 5.The paper ends with the conclusion given in section 6.

GENERALIZED KINETOSTATIC MODEL
Considering the kinetostatic domain, serial linkage of the robot arm mechanism and its actuation system can be reduced to a generalized nonlinear spring as shown in Fig. 1.Nonlinear behaviour of this elastomechanical structure can be locally linearized, which leads to the following relation between the external excitation force vector F∈R m (robot-environment interaction force) and the corresponding response, i.e., the displacement vector δx∈R m of the robot tool center point (TCP): Linear transformation operator between two vector spaces (1) is the generalized stiffness matrix K x ∈R m×m , which is expressed in the robot taskspace Cartesian coordinates.In case of a robot arm with flexible joints and rigid links, the generalized stiffness matrix is defined by the following relation: ( , ) where: h(F,x)∈R m is the nonlinear displacement function which generally includes passive displacement components of the robot actuation system and active (controllable) displacement components generated by the robot control system, where x∈R m is the vector of the robot TCP current position.Generalized stiffness matrix ( 2) is physically nonhomogeneous.Therefore, it can be interpreted as a block matrix composed of four physically homogeneous blocks (submatrices): K Ttranslational stiffness block, K R -rotational stiffness block, and K TR and K RT -coupling blocks which represent interrelation (crosstalk) between the translational and rotational displacements of the robot TCP, [13].Diagonal blocks K T and K R are full submatrices, which implies that coupling phenomenon is also present within the physically homogeneous subdomains.A more detailed analysis in this sense can be found in [15], [16].Mapping of the generalized stiffness matrix (2) into the robot joint space can be analytically achieved by applying the energy conservation principle, [7]: where δW τ is the virtual work associated with the joint torques, δW F is the work associated with the external force, δW g is the work associated with the gravitational forces, and δW μ is the virtual work associated with the frictional forces.If gravitational and frictional terms are neglected, the (3) immediately leads to: where τ∈R n is the joint torques vector and δq∈R n is the corresponding joint displacement vector.Substituting: into ( 4) leads to a very important relation which defines the mapping of external force from the robot taskspace to the corresponding joint torques vector, and vice versa: Under the assumptions used in (4), the relation ( 6) can be understood as a matrix equation which defines the static equilibrium conditions between the external force applied to the robot TCP and the corresponding torques which generated by the joint actuators of the robot mechanism.The J(q)∈R m×n denotes Jacobian matrix.
By definition, jointspace stiffness matrix can be directly derived from ( 6): ( ) Then, by replacing (1) and ( 5) into (7a), the jointspace stiffness matrix becomes a function of robot joint coordinates and taskspace stiffness matrix: As stated in [13], the second term in (7b) can be neglected since the joint displacement vector δq is very small when the robot arm TCP is close to its equilibrium configuration (δq ≈ 0).This leads to the simplified relation which is well known from the early stages of research in the field of active stiffness control, [3]: However, the adopted approximation generates significant integration errors when some external force F is present at the robot TCP, such as the loaded equilibrium or near equilibrium configuration of the robot mechanism, [17], [18].In this case the congruence transformation (8) becomes physically inconsistent in terms of its conservative properties, even though the symmetry of the joint-space stiffness matrix is preserved (congruence always preserves symmetry).Therefore, the mapping, which is consistent with the fundamental properties of a conservative system, must include the second term defined by the relations (7a) and (7b).This leads to a more general framework of Conservative Congruence Transformation which is proposed in [12].
The joint-space stiffness matrix resulting from ( 7) and ( 8) is generally nondiagonal.The existence of offdiagonal nonzero elements (cross-stiffness components of the robot actuation system) is of important consequence to robot compliance control.Today, robots are almost exclusively driven by monoarticular actuators.Regardless of their mechanical design and the control concept applied, off-diagonal nonzero elements are impossible to physically generate in robots driven by monoarticular actuators.
Consequently, a nonredundant robot which is capable of generating an arbitrary generalized stiffness matrix, needs to be driven with redundant, polyarticular actuators.For example, a 2 d.o.f.SCARA robot requires an additional third actuator in parallel with the already existing monoarticular actuators (shown in Fig. 2).This actuator simultaneously affects both joints (biarticular actuation).The difference between the jointspace stiffness matrix of the robot which is driven only by the monoarticular actuators: and the jointspace stiffness matrix of the same robot, but with additional biarticular actuator: can be easily recognized.Physical meaning of the matrix elements in (9a) and (9b) is illustrated in Fig. 2. Apparently, the influence of introduced biarticular actuator is very strong, since it is present in all elements of the joint-space stiffness matrix (9b) and, most importantly, the off-diagonal elements are no longer equal to zero.It is clear that actuation redundancy can provide a robot control system with the ability to generate an arbitrary, generalized stiffness matrix K x .Biarticular actuator fundamentally changes the elastomechanical behaviour of the robot mechanism linkage.However, the price to pay is the increased technical complexity of the redundant actuation.This is why robots which employ the redundant actuation concept are confined to laboratories.
It is worth noting that the actuation redundancy of this type is very frequent in biomechanical systems, including human arms [1], [20].For example, biarticular muscles of human upper arm are shown in Fig. 3. Biceps and triceps, two dominant muscles of the upper arm, are biarticular and simultaneously actuate shoulder and elbow joints.More specifically, the origin of the triceps muscle is located at the scapula, while its insertion point is located at the ulna, the bone which belongs to the forearm skeletal system.Thus, the triceps muscle affects motion of the shoulder and elbow joints simultaneously.
In order to solve this problem, an analytical framework which is based on kinematic redundancy instead of actuation redundancy has been developed.Kinematic redundancy is technically much simpler to realize then actuation redundancy.

CONFIGURATION BASED STIFFNESS CONTROL
When dimension of the robot taskspace m is less than the dimension n of its jointspace, i.e., when m < n, the robot mechanism is considered to be kinematically redundant.As a consequence, mapping between the taskspace vectors and corresponding jointspace vectors becomes underconstrained in terms of robot TCP position tracking.Therefore, the primary task which is defined by the relation ( 5) has infinite number of solutions.Due to extra degrees of freedom, a kinematically redundant robot mechanism has a greater capacity of configuration space than is necessary to accomplish the primary task.Under these circumstances one can use this potential as an opportunity to assign additional tasks to the robot controller and thus exploit the kinematic redundancy in a useful way.These additional tasks must be represented as a function of robot joint coordinates, [21].Within the context of robot arm compliance control, the secondary task should be related to the control of robot elastomechanical behaviour in the task space.More specifically, the secondary task should be configuration-based stiffness control (CSC) of the robot TCP stiffness in a way which is consistent with the conservative congruence transform given by the relation (7), or its less rigorous equivalent (8).Basically, CSC method is an extension of the configuration control concept originally proposed in [14].In our approach the concept of configuration control is anchored in the nullspace projectors of the robot Jacobian matrix.Proposition #1: Within the nullspace N(J(q)) of the kinematically redundant robot with r = (n -m) redundant degrees of freedom (r ≥ 1), exists at least one configuration subspace Q ⊆ N(J(q)) with a nonempty set of nonsingular configurations (robot postures) q*∈Q ∧ Q≠∅ which simultaneously satisfies following conditions: 1.The vector q* is consistent with the desired position x d of the robot TCP point -the primary task constraint, 2. The vector q* is consistent with the desired generalized stiffness K xd of the robot TCP -the secondary task constraint, and 3.The vector q* is consistent with the canonical form of the corresponding actuation stiffness matrix K qd such that K qd is the congruence transformation ( 7) or ( 8) of the desired stiffness matrix of the robot TCP.The Proposition #1 is based on the hypothesis that the increased mobility/dexterity of the kinematically redundant robot mechanism which is actuated by a set of variable stiffness monoarticular actuators is sufficient to effectively induce generalized stiffness matrix of the robot TCP such that the Frobenius norm of the error matrix ΔK x satisfies the following condition: ( , , ) where K xd stands for user defined and task-related desired stiffness matrix of the robot TCP, while ε denotes an arbitrarily small positive scalar, appropriate for the task in hand.Note that the Frobenius norm is an entrywise norm.This implies that internal structure of the error matrix ΔK x is destroyed by inherent summation operation in (10), and consequently, the corresponding information content is lost.Further research is needed to define the more comprehensive condition which preserves the information content of the error matrix ΔK x in the sense of broadly covering the structure of K x as defined by ( 2).The nullspace of Jacobian matrix denoted by N(J(q)), is defined as: Nullspace ( 11) is in fact a set of nontrivial solutions of the homogeneous system of linear equations associated to the Jacobian matrix: Contrary to the nonredundant robot mechanism, the inverse kinematics of a kinematically redundant robot has a more general solution, which is composed of two components: the nonhomogeneous member, i.e., particular solution δq P of the nonhomogeneous linear system (5), and the homogeneous member, i.e., the solution δq N which is related to the nullspace of Jacobian matrix, (11) and analytically defined by (12).This leads to the definition of the generalized virtual displacement which can be analytically expressed by the following relation: where: J + (q) is the generalized inverse of the Jacobian matrix J(q) which satisfies Moore-Penrose least norm condition, δq 0 is an arbitrary vector from the robot configuration space, and P c N(J) is the complementary projector, i.e., an operator which projects the vector δq 0 to the nullspace of Moore-Penrose generalized inversion J + (q) of the Jacobian matrix.Homogeneous member in (13) is the formal vehicle which can be used for generation of internal motions in the robot mechanism, i.e., the selfmotions which do not alter the robot TCP position.Since the jointspace stiffness matrix is dependant of the robot joint coordinates, ( 7) and ( 8), internal motions in the Jacobian nullspace will affect the jointspace stiffness matrix.In that sense, nullspace motion can be used to satisfy secondary task objective, [9], [10], that is control of generalized stiffness of the robot TCP.Since the nullspace motions have no influence on the robot TCP motion, equation ( 13) allows the primary task to be accomplished independently.
Relation ( 13) can be interpreted as an analytical formulation of a Task Fusion Inference Machine which provides formally consequent integration (superposition) of the primary task of the robot control system: position and motion control of the robot arm, and any other task of secondary priority.The task fusion is achieved by a nonconflicting inclusion of the robot configuration displacement vector q* which is consistent with the secondary control task, in the sense of Proposition #1, into the primary control task.The nonconflicting property of the task fusion ( 13) possesses an intrinsic hierarchical structure which guarantees that the primary task is always of highest priority and will not be compromised by the secondary tasks under any circumstances.
In accordance to [10], [19], the complementary projector used in ( 13) is defined by the following relations: while the Moore-Penrose generalized inverse satisfying the least norm condition is given by the following relation, [19]: Relations ( 13) to ( 16) are essential for the Configuration-based Stiffness Control method (CSC) which is proposed in this paper.

THE COST FUNCTION
According to the relation (13), an arbitrary joint displacement vector δq 0 should be determined in a way to satisfy a suitable cost function u(q), which is related to the objectives specified within the Proposition #1.The cost function u(q) is an appropriate measure of the loss of performances, or the discrepancy between the desired robot TCP stiffness K xd and induced robot TCP stiffness K x which is governed by its current configuration q and the stiffness of its joints k q_ii (the diagonal elements of the jointspace stiffness matrix).Although the stiffness optimization problem has been extensively studied by numerous researchers, the problem of explicit synthesis of the cost function for the kinematically redundant robot control based on gradient projection method, has remained almost untackled.In other words, rather than dealing with the control, efforts have been predominantly focused on stiffness modelling, identification and characterization.Probably the most relevant research results are presented in [13] where three cost functions are proposed for the general framework for robot TCP stiffness control, defined as an optimization problem which is based on minimization of the following functional: norm[K xd , K x (K q , q)].Evidently, the proposed cost functions are related to the robot taskspace and the corresponding generalized stiffness matrix induced by the robot configuration and stiffness of its soft controlled joints: In our approach we have focused on the robot jointspace and synthesis of the cost function which is related to the robot jointspace stiffness matrix, following the basic relations for its analytical definition, given in (7) and (8).In fact, this approach is opposite to the approach presented in [13], because the robot TCP stiffness appears to be the consequence, rather than the immediate object of the optimization problem.This leads to the following definition of the nullspace vector optimization criterion: . (18) In that sense, the following proposition is stated: Proposition #2: Euclidian norm of the off-diagonal elements of the jointspace stiffness matrix, which is analytically defined by the following relation: ) is a sufficiently representative measure of the discrepancy between the desired robot TCP stiffness and the robot TCP stiffness induced by the monoarticulated linkage of the robot mechanism, and can be effectively used as a cost function for the robot TCP point stiffness optimization, consistent with the Configuration-based Stiffness Control framework (CSC), formally based on relation (13), and the set of constraints expressed by the Proposition #1.
The cost function (19) generates a scalar potential field over the robot hyperdimensional configuration space.Since this potential field is nonlinear, continuous, and therefore differentiable function, the gradient optimization method, [9], [10] can be effectively applied to find the optimal nullspace joint vector q* which locally minimizes influence of the cross-joint members of the robot jointspace stiffness matrix.This leads to the following relation: 0 ( ) ( ), 0 q uq uq q Gradient optimization method is known to be algorithmically stable and always converges to the nearest local minimum.Being essentially discrete, this algorithm is very suitable for real time implementation on digital control systems using the following integration scheme: where δq(t k ) is defined by the relation (13).
In any integration step of (21), virtual displacement vector δq 0 calculated by ( 20) is proportional to the negative gradient of the potential field (19).Therefore, it is not consistent with the nullspace kinematics of the robot arm mechanism.The scalar operator, α > 0, is used to scale the magnitude of the δq 0 .This operator is a very general, intuitive solution, which must be somehow related to additional constraints that can enable both kinematically and dynamically consistent nullspace displacements.In that context, following relation is adopted: where β is the global multiplier, which is time independent and defined by the supervisor, or by the higher levels of the robot control system.The main role of the β multiplier is to control the convergence speed of the secondary task, and thus synchronize it with the primary task execution.There is no straightforward strategy on how to do that.One of the options is to choose β as high as possible, thus providing very fast convergence of the secondary task to its desired or best possible state.The other option is to choose a value of the β multiplier such that both tasks converge to the terminal states at the same time (ideal synchronization).The denominator of the fraction member in ( 22) is the magnitude of the displacement vector δq N i.e., the cost function gradient projected onto the nullspace of the Jacobian pseudoinverse (16) using the complementary projector (14), and its role is to normalize the nullspace displacement vector δq N .The numerator is the magnitude of the δq P , which is the minimum norm displacement vector generated by the Moore-Penrose pseudoinverse.Note that the vector δq P is calculated even when the primary task is completed, i.e., for the corresponding nullspace motions which are governed by the homogeneous solution δq N of the (13).
As a consequence, the α operator ( 22) is intrinsically adaptive and consistent with the kinematics of the robot mechanism.Although these properties look very convenient, they are actually not quite correct.The main problem here is the sensitivity of the Moore-Penrose pseudoinverse to the singularities.Near the singular configurations, the norm of the δq P becomes very large, [10], [21].Therefore, the operator (22) should incorporate handling of singularities, for instance in the way proposed in [20], or by introducing additional secondary task which independently optimizes nullspace motions in the sense of singularity avoidance.Dynamical consistency of nullspace component in ( 13) is very important for stability issues of the robot control system and therefore must be carefully considered.In that context, the approach proposed in this paper, based on the robot nullspace compliant behaviour control, should be extended from the kinetostatic domain to the complementary dynamical nullspace domain of the Jacobian matrix.Taking this into account, one of the possible solutions is proposed in [7].

CONCLUSION
In this tow part paper, the Configuration-based Stiffness Control (CSC) method is formulated as a kinetostatically consistent framework for robot compliant behaviour control, based on gradient projection of the cost faction which minimizes the norm of off-diagonal elements of the jointspace matrix.We have found that in robotic arms which are driven by variable stiffness monoarticular actuators, kinematic redundancy and nullspace of the corresponding Jacobian matrix are potentially applicable for effective control of robot TCP compliant behaviour by canonization of the jointspace stiffness matrix.The CSC method proposed in this paper is limited to the stiffness control task only.Singularity avoidance and joint range limits must be taken into account as an additional secondary task which has to be simultaneously optimized with stiffness control task (three secondary tasks altogether) by selecting the appropriate task priority levels.

Figure 1 .
Figure 1.Reduction of a serial link robot mechanism to a generalized spring

Figure 3 .
Figure 3. Biarticular antagonistic muscles in human upper arm that redundantly drive shoulder and elbow joints.The triceps and biceps antagonistic muscles affect motion of the shoulder and elbow joints simultaneously.