An Adaptive Control Approach to Robotic Assembly with Uncertainties in Vision and Dynamics

The objective of this paper is to propose an adaptive impedance control framework to cope with uncertainties in vision and dynamics in robotic assembly tasks. The framework is composed of an adaptive controller, a vision system, and an interaction planner, which are all supervised by a finite state machine. In this framework, the target assembly object’s pose is detected through the vision module, which is then used for the planning of the robot trajectories. The adaptive impedance control module copes with the uncertainties of the vision and the interaction planner modules in alignment of the assembly parts (a peg and a hole in this work). Unlike the classical impedance controllers, the online adaptation rule regulates the level of robot compliance in constrained directions, acting on and responding to the external forces. This enables the implementation of a flexible and adaptive Remote Center of Compliance (RCC) system, using active control. We first evaluate the performance of the proposed adaptive controller in comparison to classical impedance control. Next, the overall performance of the integrated system is evaluated in a peg-in-hole setup, with different clearances and orientation mismatches.


I. INTRODUCTION
Robotic applications play a prominent role in industry, where repetitive tasks can be performed faster and cheaper in comparison to manual layouts. Early applications of robots in industry situated them in closed and isolated workcells to avoid physical collisions with human workers and the consequent injures. Nowadays, however, robots can enter in direct contacts with humans, due to their adaptive hardware and software characteristics that ensure compliance with several safety standards such as ISO 10218-1. The use of elastic elements such as springs [1] in the mechanical design of the robots, or the implementation of advanced control strategies (e.g., impedance control) to render variable output dynamics [2] are common examples to achieve adaptive and safe operations for robots in unstructured and hybrid (human populated) environments.
Peg-in-hole like assembly actions is well-known scenarios where the application of robots with the aforementioned characteristics has been studied since 1980s [3], [4]. In such tasks, the uncertainty sources arising from the planning and perception [5] contribute to increasing the complexity of the task execution from a robotics point of view, which is mainly due to the misalignment of the peg with the hole. Passive approaches based on the remote center of compliance * Contributed equally to this work.
The authors are with the HRI 2 Lab, Istituto Italiano di Tecnologia, Genoa, Italy.
emir.mobedi@iit.it, nicola.villa@iit.it  (RCC) mechanism are among the most effective yet low-cost techniques to this problem, which allow a robot to bear the lower amount of torques at about the orientation of the endeffector [6]. However, it is known that such passive solutions lack flexibility and scalability.
With the aim to improve the flexibility of robotic assembly, adaptive control techniques [7], such as hybrid position/force control algorithms have been proposed [8]- [10]. The general idea among the existing control techniques is to implement a position control along the lateral direction while pushing the robot towards the direction of the insertion. However, setting a desired force at the end-effector limits the application area of the robots, since force control is usually suitable for tasks in which all the constraints are predefined. In fact, sudden contact losses in direct force control approaches can result in unexpected robot behaviours, which may cause damage to human coworkers or the environment.
To overcome these limitations, the assembly task can be achieved using impedance control [5], by adjusting the control gains in each axis with respect to the geometrical constraints or friction (due to different clearance levels) [11]. Nevertheless, the choice of impedance gains should be specific to task conditions, which is usually chosen by robot programmers. This can severely reduce the flexibility of the robot to various interaction scenarios (e.g., see concept illustration in Fig. (1)).
As an alternative technique to the model-based approaches [7] described above, model-free algorithms based on machine learning have been proposed. These techniques can be di-vided in two categories, i.e., learn from demonstration (LfD) and reinforcement learning (RL). The former one is implemented by recording human data during assembly tasks, which is followed by the programming of the robot [12]. Instead, the latter is based on processing the data acquired from the environment in several attempts of executing a task [13], [14]. The main drawback of such techniques is the need for large experimental data (LfD) or trials (RL) to ensure the execution of an assembly task subject to varying geometric constraints.
In response to the above mentioned limitations, we propose an adaptive impedance control strategy to regulate the level of robot adaptation to the environmental constraints and external forces in assembly tasks. Unlike the method of estimating the robot parameters via the gradient [15] or least square method [16], the proposed method modifies the control gain intuitively depending on the external forces, which is occurred in the physical interaction.
In the framework we present in this paper, the controller implements a stiff profile along the insertion direction, which is detected by an external vision system, and an adaptive impedance controller in all other axes. This consideration is to facilitate the insertion of the peg in the hole, since the stiff profile will help overcome the friction in the direction of insertion, while the 'compliant-as-needed' profile in other directions maximise the alignment of the peg with the hole during insertion. The later will eliminate the generation of unnecessarily high interaction forces in axes other than the insertion.
The efficiency of the proposed controller is evaluated experimentally in a peg-in-the-hole task, using a 7 degrees of freedom (DoF) Franka robot. We compare the proposed controller with two other control implementations, in which robot Cartesian stiffness is set to a stiff and a compliant value. The rest of the paper is structured as follows. In section II and II-D, the working principle of the controller, the details of the vision system, and the implemented Finite State Machine are explained. In section III, the experimental setup is introduced. In section IV, the experimental results are discussed. Finally, conclusion and the future studies are discussed in section V.

II. THE ROBOTIC ASSEMBLY FRAMEWORK
This section describes the individual components of the proposed framework, which consists of an adaptive controller, an external vision system, an interaction planner module, and a finite state machine (FSM).

A. Adaptive controller module
To characterize the adaptive controller, first a brief overview of the impedance control theory is provided. Defined the generalised coordinates as q ∈ R n , the dynamic equation of motion for n-DoFs robot can be written as: where M (q) is the n × n inertia matrix, C(q,q) is the Coriolis/centrifugal matrix, g(q) is the vector of gravity joints torques, and τ is the vector of actuated joint torques. The robot's dynamic equation includes the reaction torques τ ext = J (q) T F ext , which are exploited by the k linearly independent constraint forces F ext ∈ R k and the Jacobian matrix J (q) ∈ R k×n . For instance, F ext is a external wrench at the robot's end-effector in Cartesian space (i.e. k = 6). Hence, the dynamic behavior in Cartesian space can be written as: wherex = x − x d ∈ R 6 represents the Cartesian error calculated with respect to the desired Cartesian coordinates x d . In addition, Λ d , D d and K d are the desired Cartesian (∈ R 6×6 ) inertia, damping and stiffness matrices respectively, which are positive definite and symmetric. By avoiding the inertia shaping and assuming the desired velocity to be zero, (2) can be simplified to: Inserting (3) into (1) results in the target impedance behaviour [17]: However, the control gains K d and D d of such a classical impedance controller require some appropriate values, which rely on the specific geometrical task constraints, hence, it will be severely limited to cope with uncertainties of a task (e.g. the mismatches between the estimated and actual observation). To resolve this issue, the adaptive controller employed in the framework enables a flexible behaviour according to the external wrench. This can be achieved by implementing the regulation term x a (x adjusted ) in (3) as follows where,ẋ K f 1 and K f 2 (∈ R 6×6 ) are symmetric and positive definite matrices and defined as the inverse damping and stiffness matrices, respectively. Considering that the two matrices are normally set equal, in the following it is referred to these two as a single matrix K f , called compliance matrix according to [18]. The overall block-diagram of the proposed adaptive controller is illustrated in Fig.2. If there is no external interaction, the adaptive controller (the upper part in Fig.2) does Fig. 3. Illustration of RCC system's behaviour. x and δ are the initial linear and rotational mismatch between peg and hole, respectively. The red circles are the Contact Points (CPs) between peg and hole, Fa is the applied force, while Wext is the external wrench. In peg insertion phase, the resulting forces at CPs act: (1) pushing the RCC laterally with a single CP; (2) Rotating the RCC with two CPs. not interfere with the controller input since F ext = 0, and behaves as the classical impedance controller with K d and D d positive definite control gains. On the contrary, if the external wrenches are applied,ẋ a and x a , which are obtained by the direction and the amplitude of the external forces, regulate the target impedance behaviour through modification of the position/velocity error. As a consequence of this condition, the robot enables compliant motion in a certain direction where K f is applied.
A high (low) value inside K f leads to a high (low) correction to position/velocity error (x/ẋ) along different Cartesian coordinates; therefore, the robot responds with lower (higher) forces to the environment, showing a more compliant (stiffer) behavior. About K d and D d values in (5), these can be chosen high enough to ensure high tracking capability of manipulator in free motion. It is therefore clear how the controller behaves as a virtual and active RCC system (Fig.3), which actively modulates the response of a highly stiff robot in case of interaction with the workpiece.
In a peg-in-hole assembly as shown in Fig.3, for example, the external forces are applied to the peg around orientation axes, and the adaptive controller moves the peg along with the opposite orientation of the geometric constraint due to the adjusted trajectory.
Lastly, a stability analysis of the proposed adaptive controller is presented. The asymptotic stability of the classical impedance controller in (3) is considered via a Lyapunov function, as in [17], [19], [20]. Now the following Lyapunov function is proposed: Considering the adaptive controller, the open version of (5) can be obtained by substituting (6) into (5): By isolating F ext in (8): (3) is replaced with K * , which consists in a division between positive definite matrices. Therefore, recalling (7), this concludes that the controller is asymptotically stable.

B. Vision system module
This section describes the vision module, responsible for the object's pose detection in the robot workspace. Thanks to this module, the robot is able to understand the task's environment that includes the target points. In this work, we target a peg in a hole task. Moreover, the vision system is in charge of identifying the insertion axis, that is crucial for assembly tasks. The required reference frames are defined as: base frame (fixed) Σ B , tool frame Σ H , end-effector frame Σ EE and camera frame Σ C . The location of these references will be shown in the following section.
To detect position and orientation of an assembly part (here, a hole), we implemented a template matching technique [21]. This approach ensures good approximation of object pose in the work space, through the fitting on scene point cloud of an object's model. The algorithm operates on point clouds, which contain position information of points in the space, without neither the need of further conversions from pixels to meters space nor issues related to work space illumination, as in the case of image based algorithms. Through this approach, it is possible to select a point on the template cloud which can be used for the detection of the hole fixed frame. In case of peg in a hole, the bottom of the hole model point cloud was selected. In the following, we refer to this point as P H .
Once the model is fitted on the environment point cloud, a rigid transformation is applied from the new reference frame (rotated according to the fitted template) to P H . The output of the algorithm is a transformation matrix U C H ∈ R 4×4 from the camera frame (Σ C ) to a frame fixed to the cylindrical hole (Σ H ), translated according to P H and rotated according to the hole inclination. The z axis of the new frame is set along the main axis of insertion, i.e. the cylinder axis. The transformation matrix U B H from hole to robot base frame (Σ B ) is obtained through a multiplication between the object pose in camera frame U C H and the camera pose in robot base frame U B C by U B H = U B C U C H .

C. Interaction planner module
Interaction planning module consists in the generation of the robot trajectories to reach, roughly align the peg with the hole and insert the peg, as well as the planning of the robot Cartesian stiffness to ensure an RCC-like behaviour at robot end-effector. The first is obtained by means of a trajectory planner implementing a 5 th order polynomial trajectory that enables smooth trajectory planning between the current and the target pose of the robot end-effector. The target is expressed through a transformation matrix T B target from base frame (Σ B ) to target frame. The second is achieved via the modification of the original compliance matrix K f , characterized relatively to the hole fixed frame. In case the estimated hole frame is not aligned with the robot base frame (e.g. the hole is inclined), K f is transformed into the base reference according to the hole orientation. Hence, the transformed K f can be employed in the adaptive controller withx and F ext in (5). For sake of clarity, the compliance matrix with respect to the hole fixed frame is called K H f , while K B f corresponds to the compliance matrix in robot base frame. To perform this transformation, the rotational part of matrix U B H (explained in sec.II-B) is considered, i.e. R B H ∈ R 3×3 . The matrix ∈ R 6×6 is built as: K B f is computed by K H f T which represents the Singular Value Decomposition of the symmetric stiffness matrix K B f . In such a way, the interaction planner can assign high stiffness value on the major axis (insertion axis) and the adaptive behavior in (5) along other DoFs.

D. Finite State Machine
To make the above modules communicate, a finite state machine (FSM) is implemented. The FSM consists in 3 states: detection (S1), hole reaching (S2), assembly (S3).
The unified control framework is illustrated in Fig.4. The control flow shows that S1 triggers the vision module, asking for hole detection. This module returns the transformation matrix U B H . If the hole is found, FSM passes to S2, which triggers the trajectory planner to compute a path to the hole (T B hole top ); the module passes the path to the controller as a sequence of equilibrium points, together with K B f , which is computed only once according to hole pose. If the reaching is successful, FSM triggers S3 which sends a request to trajectory planner, i.e. going down to T B hole bottom for inserting the peg. If the task is accomplished, the FSM stops.

III. EXPERIMENTAL SETUP
The experimental setup and reference frames are illustrated in Fig.5. The experimental evaluation involved a pegin-hole task. The first set of experiments was named Experiment A, and designed to compare the performance of the adaptive controller with the classical impedance controller. Following that, the performance of the complete framework (Experiment B) was evaluated by a realistic assembly in the inclined hole setup, where was demonstrated the integration of the modules. The hardware setup included the Franka Emika's Panda robot equipped with an ASUS Xtion Pro Live RGB-D camera. A hole with 34.6 mm, and three pegs with 32.6 mm, 33.6 mm, 34.4 mm diameters (difference clearances) were 3D printed for the experiments. Noteworthy, F ext using in (4) were obtained from the Franka Emika's external torque estimation algorithm [22], and represented in respect of Σ B .
In Experiment A, the repeatability performance is evaluated. In the first type, named as the stiff controller, the Cartesian diagonal stiffness matrix had fixed-high values as (K x = K y = K z = 1000 N/m, K roll = K pitch = K yaw = 30 Nm/rad). Similarly, the compliant controller implemented fixed-low diagonal values (K x = K y = 500, K z = 1000 N/m, K roll = K pitch = K yaw = 20 Nm/rad). Finally, the adaptive controller was implemented as described in Section II-A. In this controller, the adaptive gains of K f were selected as (K f x = K f y = 0.001 m/N, K f z = 0, K f roll = K f pitch = K f yaw = 0.14 rad/Nm) for the first and the second peg. However, for the third peg, the gains were modified as (K f x = K f y = 0.001 m/N, K f z = 0, K f roll = K f pitch = K f yaw = 0.15 rad/Nm) since the third peg has the lowest clearance among the others. During the experiments, K f 1 and K f 2 were assigned as the same value. K f z = 0 intends to implement a fixed stiff behaviour in insertion direction (z). As mentioned in the controller section before, the selection of K f depends on the geometrical constraints in the robot environment. If the higher forces are expected in a direction, the K f should be assigned with higher values along that direction to decrease the forces. In our application, since the goal is to be adaptive around the roll, pitch and yaw to allow the peg insert the hole, the selection of K f roll , K f pitch and K f yaw were kept higher with respect to x, y and z directions.
x, y positions and pitch angles of the robot (in Σ B ) were kept the same for overall trials. Either α or β orientations was varied to check whether RCC can compensate those   mismatches. In the following steps, the procedure is written for one pose (α varied, β constant). 1) Go to the initial pose (z = 0.13 m : α 1 : β 1 ).
3) If the peg half-inserted or stucked, record the wrench and write "−" for the next poses (see Table(I)). 4) If the insertion is done (z = 0.07 m), send zero torque to the robot and record the wrench. 5) Repeat the abovementioned steps three times, and calculate the mean value and the standard deviation of wrench (z). 6) Go to the next test pose (z = 0.13 m : α 2 : β 1 ). After that, the proposed experiment for the whole framework validation is performed with the hardware of Fig.1 but with an hole inclination of β = 0.78 rad. The controller parameters were kept the same with the adaptive one for first peg. In Experiment B, the robot is first set to a random pose; then, it is required to reach the hole, detected by vision module (i.e. a relative position of 10 cm along z axis of Σ H ), and then to insert the peg (i.e. reaching a relative position of 1 cm along the same direction). To ensure the insertion, an additional rotation around hole axes is added to the target, i.e. α = −0.25 rad and β = 0.25 rad.

A. Controller Validation
In Fig.6, six plots are reported for one of the repeatability tests poses to analyze the generalized interaction forces, and position and orientation error. Also, x a is illustrated for roll (α adjusted ) and pitch (β adjusted ) axis.
At phase 1 (A.1), the orientation errors of the compliant controller are greater than the stiff and adaptive ones because of its gains. In the meantime, the adjusted orientation is zero as there is no interaction. At phase 2 (A.2), the values of α adjusted and β adjusted increase in the negative direction to reduce the position and velocity error (see (5)). This adaptation achieves forces and torques that are significantly lower for the adaptive controller in constraint axes (and not the insertion). To clarify, for the wrench along x and y directions, the data for compliant and adaptive controllers are relatively closer to each other than the force data in (z) since K f is selected as lower (K f x = K f y = 0.001). At the next phase (A.3), the adaptive controller fully compensated for the hole constraints, which could be easily seen from the orientation error graph (β A ). Finally, the insertion was done, and the desired wrench was set to zero.
In Table I, the results of 51 trials (17 poses for three pegs, pointed as P-1, P-2, P-3) are reported. The success rate is computed according to the trial number. When α is 0.34 rad for P-3, the adaptive controller can achieve the task. However, the opposite pose (α = −0.34 rad) cannot be accomplished. The reason arises from the force estimation error of Franka Emika (the error varies from axis to axis), leading to acquire different wrench according to (6).
When α or β test poses are greater than |0.24| rad, both compliant and stiff controller are not capable of achieving the task, which concludes that the proposed controller bears the highest success rate among others.

B. Framework Validation
In this section, the validation results for the whole framework are presented. Figure 7 Here, the experiment is explained in details. Once the hole is detected (B.1), the robot starts moving toward a target (B.2), i.e. the hole entrance. Vision output is shown in enlargement 1 of Fig.7(a), where the red point cloud corresponds to the hole template matched on the scene. As in 1 st and 2 nd row of Fig.7(b), during B.2 the control behaves as an impedance controller with high stiffness parameters, tracking perfectly the desired trajectory with a maximum position and orientation error of 1 cm and 0.05 rad, respectively. Indeed, the wrenches are all zero during this phase and consequently x a andẋ a in (5).
On the contrary, during B.3, the wrenches start to increase. The enlargement 2 of Fig.7(a) shows how this is caused by a linear and rotational misalignment, which occurs between peg and hole due to some error in detection. In this situation, the robot keeps on pushing the peg down, due to the predefined parameter inside K f , leading the perceived wrenches to increase. The presence of external forces causes an increment in robot compliance along DoFs with non-zero K f parameters. The combined action of the z-axis force and the increasing compliance leads the robot to correct the mismatch and accomplish the task inserting the peg (B.4). The increment in position and orientation errors during B.3 is caused by the higher compliance of the robot in this phase which allows a higher deviation from the target trajectory according to environment constraints. The manipulator is therefore guided by the environment (here the hole borders) in correcting the pose for accomplishing the task.
During B.4, the wrenches and the error drop down. However, the latter remains constant to a non-zero value for x, z and orientation coordinates. The final errors in orientation are caused by the mismatch correction. The final errors along x and z are due to both mismatch correction and unreachability of the target. Indeed, the peg is completely inserted and stops before it reaches the target required for the insertion, i.e the hole bottom. Nevertheless, the task is accomplished and the controller is stopped.
V. CONCLUSION In this work, we proposed a novel adaptive framework for accomplishing robotic assembly tasks. The framework is composed of an adaptive controller, a vision system, and a trajectory planner, each communicating through a Finite State Machine. The controller was validated through experiments on a typical assembly action, i.e. peg-in-hole, as a representative example of tasks with dynamic uncertainties. First, the adaptive controller was compared with two classical impedance controller to demonstrate the strength of the proposed strategy. Next, the integrated framework was tested: the vision system was able to detect an inclined hole on the work table and the robot was able to reach the hole with very low pose error during the tracking phase. The adaptive controller enabled the robot to correct some linear and rotational misalignment between peg and hole, to accomplish the insertion. Results demonstrated the high potential of the proposed approach in increasing the flexibility of robotic assembly operations.