Implementation of impedance trajectory control on a 6-DoF manipulator

. Aiming at enhancing the performance of a robotic system, i.e. of a robot manipulator, it is extremely important to ensure the safety of its functioning and the convenience of operating it. Special attention is required if there is a possibility of a collision, especially with costly equipment or with a person. Among the different means and approaches to prevent or react to such situations, implementation of force-torque control is considered in this paper. A trajectory task is analysed and an impedance control law is employed to achieve the desired robot behaviour. The effectiveness of the approach has been verified by using a 6-DoF elbow manipulator to perform two types of operations: a peg-in-hole task and path execution with obstacles on the way.


Introduction
There are a number of criteria to evaluate quality of robotic systems and their suitability to appropriate tasks. Among the whole variety of these criteria one can assess mechanical performance, functionality, reliability/safety and ergonomics. Mechanical performance comprises, for example, load capacity (and load to weight ratio), top and nominal speed, accuracy, and workspace dimensions. Functional criteria describe the range of tasks that a system is capable of due to its structure and overall design. However, even a fast and accurate manipulator with high load capacity and notable functionality does not value much if it cannot provide safe and reliable functioning, by which the ability to maintain its own operability is assumed along with preventing doing harm to workers that can be present in the workspace and the environment itself.
Easy operation and control is another important criterion, with different kinds of control under consideration, namely, manual control, copy control and automatic control. If it is too difficult for an operator to maintain control, it generally leads to the operator's fatigue and increases a chance of mistake. There are also situations when the operator physically cannot provide required motion, e.g. the motion of tool centre point (TCP) along a curvy surface keeping constant and limited contact with this surface. It is an extremely difficult task for the operator to keep the vector of TCP velocity in conformity with the surface curviness, not to mention that the available workspace view may not allow operator to estimate the surface shape. The other example is aligning the peg with the hole in a typical task illustrated on Figure 1. In this case it is possible to see the axes misalignment only in one projection, because of the static view of a single external camera. Safe functioning and convenient control can be achieved through different approaches in robotics, most popular of which include machine vision [1,2] and forcetorque control [3][4][5]. An approach here is a general paradigm, first of all, a type of employed hardware, such as video cameras, 3D-scanners or force-torque sensors. A variety of specific methods implementing control laws and their modifications, and based on them concrete algorithms generally correspond to any particular approach. Applicability of methods depends primarily on present hardware and also on the description complexity of particular task in terms of selected method.
This article focuses on force-torque control that implies the presence of joint torque sensors and/or end-effector multi-axis force-torque sensor that should be situated between the manipulator output flange and the tool itself. Primary sensitive elements of such sensors do not interact with the environment directly and their performance is not affected by the manipulator configuration unlike the performance of cameras and scanners, which is an issue with machine vision employment. Considering force-torque sensing in space robotic systems, the absence of interaction with the environment gives an advantage because of the lack of necessity for extra mechanical protection of additional weight.
There is a small chance that an external mobile vision system is available, whereas equipment of the manipulator with its own vision is limited by the overall mechanical design and the pursuit for workspace maximization. The location of the vision system, as a rule, cannot be changed during the manipulator lifetime. A survey manipulator, which is an additional manipulator with a camera as a tool, may be present; however, the area of possible camera location is limited within the workspace of this manipulator. Thus, machine vision is incapable of complete fulfilment of the specified criteria and hence force-torque control can be of interest in order to increase safety and operation convenience.

Control law
A topic of force-torque (FT) control represents a wide area within the scope of robot control. All developed FT methods can be divided into direct FT and indirect FT depending on a controlled variable that closes the outer control loop. The value of this variable (task value) should be specified either numerically or by the means of an input device, e.g. a joystick. Consequently, the direct FT control assumes active interaction (mechanical contact) with the environment, and a vector of TCP forces and torques (wrench) should be set to be performed by the manipulator. If a task is related to time-determined trajectory execution, then the direct FT is applicable only in special cases.
The indirect FT essentially is either position or velocity control constructed in such way that applied wrench is controlled or at least limited. Trajectory execution implies position control at each moment of time, thus we need an indirect FT control based on the position control, and trajectory impedance control is designed which perfectly suits. Consider a general form of impedance control law for a manipulator joint, i.e.
where τ req -control signal in the form of joint torque, τ obj -objective joint torque, comprising generally gravity, centrifugal and Coriolis terms; S -virtual stiffness factor, D -virtual damping factor, q req and req q  -required position and speed, q cur and cur q  -actual position and speed. The last term may be absent, and then the law is often mentioned as stiffness control. It is widespread to assume the TCP pose for the position variable. However, in the case when: • Manipulator is non-redundant; • Trajectory is smooth; • Manipulator is rigid (rigid links and rigid joints); every TCP pose in the workspace relates to a specific manipulator configuration, which is a vector of joint positions, or generalized coordinates. Typically, the last condition is not satisfied, but this fact effects either in complication of the model and computations or in deviation from the trajectory, the latter being often rather small and, thus, acceptable. Therefore, specifying trajectory for TCP pose results in obtaining joint-level trajectories.
In our implementation, on the user side, trajectory contains an arbitrary number of points, which are set by key point memorization, or by manual table input, or by teaching (record and repeat). The first two cases provide key points to be further linked with given geometric primitives (e.g. lines). Either way, the control system constructs a dense geometric trajectory, converts it into a joint space trajectory and then translates the latter trajectory into a time-dependent form [6].
Provided that τ obj is computed with absolute accuracy and there is no mechanical interaction with external objects, the above law results in fine trajectory reproducing (q cur = q req ). If for some reason an object appears on the path and a collision happens, the reaction force from the contact leads to the unbalanced torque which in turn leads to joint position deflection until a new equilibrium is found, i.e. a point, where in the case of stiffness control, so τ un is a classical elastic torque, only of synthetic, or virtual, nature due to virtual stiffness S. Accordingly, the object itself (the joint) behaves as a spring, and changing the value of S, the controller actually changes the stiffness of this spring. Consequently, such a system allows deviation (deflection) from the desired path due to external forces action in a linear, smooth and safe way. The general motion task formulation, though not necessary or unique, is to find a collision-free trajectory from the initial to goal position and perform it. However, obstacles still can exist on the manipulator path. It is possible that an obstacle gets on the path after the planning completion, e.g. a worker walks into the manipulator workspace. Another reason could be an error made in the determination of the manipulator position relative to external objects. Therefore, both the situations have a potential to employ impedance control. Superimposing an obstacle shape onto the given trajectory, position deviation can be computed for each point. If τ req is no greater than the critical torque value for the considered joint, the transition to the goal position is possible, even though the trajectory itself cannot be reproduced. Otherwise, the motion is dangerous, and should be stopped when the critical torque is reached, until the next command comes from the operator. In the case of movable or elastic obstacles the analysis becomes more difficult, but as torque is being checked for limit exceeding, task execution is deemed to be safe. However, as this time the external objects change their location or/and shape, the operator should be sure the motion will not cause actual damage.
We implement the law (1) on the joint controller level, providing all the task values (required position, required speed, and objective torque) and factors (stiffness, damping) from the next-upper level, i.e. from the manipulator controller. For relatively slow manipulators functioning in earth conditions objective torque consists only of gravity torque. The overall control structure (data flow) is shown in Figure 2. To make the system work, the manipulator controller finds all the joint trajectories and loads them into the joint controllers before the motion begins, and provides joints with gravity torque values in the time of execution.

Test bed description
A series of experiments has been performed to tune the system and confirm the applicability of the proposed approach. Two tasks have been simulated: a) peg-in-hole task; b) execution of the path along the polygonal line intersecting a rigid object in the environment.
A general view of the experimental test bed is given in Figure 3 while the detailed description is provided in [7]. The controlled object is a 6 DoF manipulator devised for the verification of control system and design of a manipulation system for servicing spacecraft [8,9] as a ground prototype. A photo and a simple kinematic model of the prototype are given in Figure 4. The manipulator comprises six torque sensors (one for each joint) and a three-axis force-torque output sensor. The sensors description is given in [10]. A software package has been written in Matlab Programmatic Workflow framework for the testing and control purposes, described again in [7]. Tasks (a) and (b) have been performed on the panels 1 and 2, respectively, of the test bed from Figure 3.

Peg-in-hole task
The task assumes motion in partly determined or erroneously determined environment. The initial position is shown in Figure 1. A peg is a probe rod that is mounted rigidly on the manipulator flange as a tool. A hole is a metal cylindrical glass with the inner diameter slightly larger than that of the tool. Task specification states: "perform a step of X mm in the tool axis direction with the speed of Y mm/s", X being a sufficient step size to touch the glass bottom. If the configuration is coaxial, task execution results in simple free motion until the bottom is reached. However, it is not so easy in the case when axes misalignment exists in the initial position. To secure the possibility of performing the task, the rod should be located so that initial entrance is possible, which is facilitated by the presence of a chamfer.
The task has been executed under trajectory impedance control. The limits were set on the TCP force (200 N) and torque (20 N•m) as well as on the joint critical torques to ensure the rod safety. There was no limit breaking during the motion until the bottom was touched, which was the purposeful end-of-motion condition. The task is illustrated by Figures 5 and 6.     Figure 5 shows how the resulting force and torque on the TCP change depending on the tool depth. Despite the overall gradual force increase, a local drop can be seen at the TCP depth of 0.06 -0.12 m. This part corresponds to passing over the local force maximum, after which the tool axis becomes close enough to the glass axis to allow continuing the motion into the glass. The orientation deviation is still non-zero due to the peg and hole diameter ratio. In the right part of the plot the force steeply increases and stabilizes, as the glass bottom is reached and the motion is halted when the force limit is triggered.
The relationship of the resulting torque and orientation, i.e. angle deviation, is shown in Figure 6. Phi here is the angle that the axes of peg and hole form, while Delta Phi is the difference between the initial value of Phi and transitional values, which is shown for the convenience of comparing angle deviation with torque (both start from zero). The relationship is close to be directly proportional according to the plot, thus it can be seen that the desired spring-like behaviour of the law (1) is actually achieved.
The important fact is that the angle deviation causes the TCP torque growth, consequently, increasing the initial deviation leads to higher maximal torques. The experiment has been performed for the maximal initial deviation, when the corresponding maximal torque values are still within the limits.

Trajectory task
This time the task objective is to test manipulator motion in the case of obstacles lying on its path. Task specification states: "perform the trajectory presenting a polygonal line connecting five given points that intersects solid metal", which is illustrated by Figure 7. The vertices of the task line ABCDE are set with the method of the key point memorization. The motion has been performed in the direction from A to E under the trajectory impedance control. Again the limits were set of the same size to secure safe motion, but this time they were not reached. The diagram of the motion is given in Figure 8. The task path ABCDE is shown with straight (red) lines in Figure 8, while the actual path is shown with the (blue) curve. The contact with the metal profile has been not through TCP, but through some other point, though close to it. The exact position of the contact point is not identifiable with the available sensor set. Still, since the contact point is close to the TCP, the processing of FT information is done as though the contact actually is at the TCP, which is not a crucial assumption in the scope of the work. With short (green) lines the resulting force is shown at each observable point of the trajectory. It follows from the plot in Figure 8 that the motion has substantial deviation from the task, but at the same time significant decrease (up to 5 times) of contact forces is seen in Figure 9 for the motion under impedance control ('compliant'). Thus, applying the impedance control law makes it possible to provide not only higher mechanical system compliance, but also higher functional compliance by allowing motions that are undesirable or even unacceptable in stiff position control. Intentionally permissible trajectory deviation substitutes abrupt contact force leaps when hitting obstacles with smooth force growth, lowering negative effects of the contact and increasing safety of manipulator performance.
As a main deduction, a basic criterion of impedance control applicability can be derived: this type of control is highly recommended for applications where safety prevails over trajectory execution accuracy. However, even for the tasks with accuracy in priority, impedance control can be of use, e.g. during a free space operation, impedance control has the minimal effect on the performed trajectory, but in the case of a worker located on the manipulator path, impedance control will mitigate the impact (due to smooth force changing) and possibly reduce the damage to the worker's health. The importance of accuracy in such situation concedes to person's health and the motion should be stopped.
There is one special observation on the last experiment. Provided the mutual position of the manipulator and the metal panel are known to the operator, who should transfer the TCP from A to E, he can deliberately make the movement through a sequence of lines instead of setting the complex curve. Despite of guaranteed contact with the metal, the goal will have been safely achieved while the technique itself is much easier for the operator and meets the criterion of easy operation and control.

Conclusion
In this paper, a method to enhance robot safety and operation convenience was presented, that implements trajectory impedance control. The conducted experiments prove the proposed benefits of this method, which has shown significant decrease of reaction force and confirmed improved ergonomic qualities. Since there are other methods and approaches (like machine vision), the issue of choosing a method becomes a design matter, both for a whole system design and for the planning of a specific mission. For example, a peg-n-hole task seems quite suitable for hybrid force-velocity control and the appropriate experiments are to be carried out soon. In any case, the choice of the method depends on the available hardware and on the particular task conditions. Furthermore, a task of trajectory impedance control on the operation level (directly for the tool) in a Cartesian coordinate system and a topic of force-torque control of redundant manipulators are to be considered.
Research are carried out (conducted) with the financial support of the state represented by the Ministry of Education and Science of the Russian Federation. Unique project Identifier: RFMEFI57517X0143.