*Article* **Redundancy Exploitation of an 8-DoF Robotic Assistant for Doppler Sonography**

**Elie Gautreau 1, Juan Sandoval 1,\*, Aurélien Thomas 1, Jean-Michel Guilhem 2, Giuseppe Carbone 3,\*, Saïd Zeghloul <sup>1</sup> and Med Amine Laribi <sup>1</sup>**


**Abstract:** The design of a teleoperated 8-DoF redundant robot for Doppler sonography is detailed in this paper. The proposed robot is composed of a 7-DoF robotic arm mounted on a 1-DoF linear axis. This solution has been conceived to allow Doppler ultrasound examination of the entire patient's body. This paper details the design of the platform and proposes two alternative control modes to deal with its redundancy at the torque level. The first control mode considers the robot as a full 8-DoF kinematics chain, synchronizing the action of the eight joints and improving the global robot manipulability. The second control mode decouples the 7-DoF arm and the linear axis controllers and proposes a switching strategy to activate the linear axis motion when the robot arm approaches the workspace limits. Moreover, a new adaptive Joint-Limit Avoidance (JLA) strategy is proposed with the aim of exploiting the redundancy of the 7-DoF anthropomorphic arm. Unlike classical JLA approaches, a weighting matrix is actively adapted to prioritize those joints that are approaching the mechanical limits. Simulations and experimental results are presented to verify the effectiveness of the proposed control modes.

**Keywords:** medical robot; redundancy resolution; human-robot interaction; torque-control; Doppler sonography

#### **1. Introduction**

Nowadays, the use of medical robot assistants arises as a suitable solution to improve the working conditions of practitioners, cooperating with them to accomplish the medical tasks. Generally, the quality of the executed task is improved through this collaboration, where the medical capabilities of the expert are magnified by the robot, resulting in improved precision in tasks and lower time of execution while guaranteeing the well-being of the practitioner [1]. Thereby, several medical robot assistants are currently used in the operating room, such as the da Vinci Surgical System [2] from Intuitive Surgical, which has been the market leader for years. Similar examples can be found in other surgical specialties such as neurological and spine surgery [3], joint replacement surgery [4] or laparoscopic surgery [5].

Non-invasive applications can significantly benefit from medical robotic assistants. This is the case in Doppler sonography application, where a number of studies have recently been conducted to propose efficient robotic assistants aiding the specialists to improve their working conditions [6–8]. Indeed, the practitioner must adopt uncomfortable postures during the execution of standard ultrasound examinations. This often makes sonographers suffer musculoskeletal disorders early in their careers. To address this issue, several teleoperated robotic solutions have been proposed in recent years, mostly using commercial robotic arms as a probe-holder [9–11]. We have also recently proposed a teleoperated robotic

**Citation:** Gautreau, E.; Sandoval, J.; Thomas, A.; Guilhem, J.-M.; Carbone, G.; Zeghloul, S.; Laribi, M.A. Redundancy Exploitation of an 8-DoF Robotic Assistant for Doppler Sonography. *Actuators* **2022**, *11*, 33. https://doi.org/10.3390/act 11020033

Academic Editors: Marco Carricato and Edoardo Idà

Received: 29 December 2021 Accepted: 20 January 2022 Published: 24 January 2022

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

assistant using a 7-DoF anthropomorphic arm as probe-holder [7,8]. The practitioner controls the robot by handling a haptic interface into a comfortable workspace. The main drawback of these proposed systems is the limited robot workspace, excluding the possibility of realizing an exam in the whole patient's body. Mobile solutions, such as the one proposed in [12], overcome this problem but need the aid of a human assistant to hold the mobile robot over the patient. A new version of the system proposed in [8] has recently been introduced in [13], including a motorized linear axis at the base of the robot to enlarge its workspace and allow a complete exam through the entire patient's body without needing manual readjustments of the platform's position. Knowing that the new platform at the patient site has 8-DoF, the degree of redundancy can be exploited in several ways, for instance, to consider a kinematic constraint [14], to avoid collisions [15] or mechanical joint limits [16], or to optimize a performance criterion such as the manipulability index [17]. This paper proposes two control modes to deal with the redundancy resolution of the 8-DoF robot. Knowing that safe human-robot interaction must be guaranteed, these control modes are proposed at the torque level, in order to implement a compliant behavior of the robot.

The first control mode considers the robot as a full 8-DoF kinematics chain. The advantage of this approach is that the eight joints are simultaneously activated, avoiding the presence of certain singularities typically linked to the limits of the workspace when only moving the 7-DoF arm. Therefore, the manipulability of the platform is naturally improved. The second control mode considers the two systems, the 7-DoF arm and the linear axis, separately. The robotic arm is activated in priority whereas the linear axis is only activated once the arm reaches the desired workspace limits. Furthermore, we present a new adaptive Joint-Limit Avoidance (JLA) strategy with the aim of exploiting the redundancy of the 7-DoF anthropomorphic arm. Classical JLA approaches define the diagonal weighting matrix as constant, which lets a continuous generation of null space torques to avoid joint limits, even for the joints that are far from their limits [16]. An improved approach is proposed here, where the weighting matrix is actively adapted to prioritize those joints approaching the mechanical limits. This feature allows an enhanced distribution of the redundant space of the robot. Moreover, when none of the joints is in the vicinity of the limits, zero null space torque is generated, allowing a free motion of the robot's elbow in case the expert wants to manually reconfigure the robot.

The main contributions of the paper are summarized as follows: (a) detailed presentation of an 8-DoF teleoperated platform for Doppler sonography, (b) validation of a fully redundant control mode at the torque level for the 8-DoF robotic system and (c) introduction of a new adaptive JLA strategy for redundant robots controlled by torque by means of an optimal variation of the weighting matrix.

The paper is organized as follows: Section 2 details the medical requirements and the description of the proposed robotic assistant platform. Sections 3 and 4 present the first and the second control modes, respectively. The last section concludes the presented work and opens the perspectives of future works.

#### **2. Robotic-Assistant Platform**

This section presents the proposed teleoperated robotic platform for Doppler Sonography. The platform has been designed based on the medical requirements collected through a study of the medical gesture.

#### *2.1. Medical Requirements*

Doppler ultrasound is a medical imaging method used by sonographers (i.e., angiologists) to study the cardiovascular system of patients using an ultrasound probe. According to the Society of Diagnostic Medical Sonography, 90% of clinical sonographers have experienced symptoms of work-related musculoskeletal disorders (WRMDs), mainly due to an accumulation of repeated gestures in awkward postures and to the frequent application of downward pressure with the probe [18].

In order to confirm this thesis, a study of the angiologist's gestures has been carried out in [7], and we resume in this section the main obtained results. A Motion Capture (MoCap) system, i.e., Qualisys, was used to record several doppler ultrasound examinations made by a practitioner on real patients and under real conditions. Markers used for MoCap were placed both on the practitioner and on the probe to record the right arm, pelvis and head movements of the practitioner during the performed examinations. Moreover, the probe have been instrumented with a FSR sensor to measure the force applied by the specialist (Figure 1). Each patient has been examined in regions all along the body: carotid, legs, and abdomen. The study highlighted an outing from the expert's joint comfort zone, which can cause WRMSDs with repetitive movements. The maximum measured values of the orientation angles considered correspond to the worst postures. The latter are compared to the comfort zone reference angles defined by ISO 11226, ISO 11228-3 and NF EN 1005-4 norms. An example of head rotation and wrist angles for the carotid exam is given in Figure 2. These values, joint angles for neck twist and wrist joint, prove that the angiologist very often works outside the comfort zone described by the standards.

**Figure 1.** Study of the angiologist's gestures using MoCap: (**a**) Measured angles on the angiologist's body. (**b**) Reflective markers fixed to the angiologist's body (**c**) Reflective markers and FSR sensor fixed to the probe.

**Figure 2.** Neck and wrist joint range angles measured on angiologist during carotid exams.

In addition, the efforts measured with the instrumented probe revealed that the intensity of the effort depends on the type of examination and the morphology of the patient and can also lead to the development of MSDs. For instance, the maximum force applied was measured during the examination of an abdomen. A greater effort was required by the angiologist to determine the position of the abdominal aorta, which is particularly recurrent for the category of fat patients.

The interest of this study is twofold. It highlighted (1) the common gestures performed out of the joint comfort zones and (2) the significant efforts to be applied during an examination. The combination of the latter can lead to musculoskeletal disorders during numerous repetitive examinations. In order to overcome these two observations, a robotic teleoperation platform composed of a haptic interface (expert site) and a cobot (patient site) was developed. Thus, the cobot performs the efforts and postures controlled by the doctor through a haptic interface. Since the cobot workspace is not large enough to cover the whole patient's body, it has been mounted on a motorized linear axis, producing an 8-DoF robotic platform.

#### *2.2. Experimental Setup*

Figure 3 presents the assembled prototype of the proposed teleoperated robotic platform. The platform at the patient site, i.e., Franka Emika and linear axis, is teleoperated by the practitioner through a 6-DoF haptic interface.

**Figure 3.** Assembled prototype of the 8-DoF robotic platform for medical applications.

The platform's workspace has been increased by 0.855 m on the *Y-axis* thanks to range of motion of the linear axis. This latter is powered by a Kollmorgen brushless motor via a belt drive. The motor driver is controlled through a Telnet protocol. Moreover, the high-level control of the overall platform is implemented in the Robot Operating System (ROS) middleware.

#### 2.2.1. Expert Site: 6-DoF Haptic Interface

Haptic devices have been widely employed in many areas such as surgery and craniotomy application [19], rehabilitation and manufacturing. In a general framework, haptic devices are based on mechanical interfaces that link tactile information between a human and the device. Various haptic interface architectures have been developed, with either serial or parallel architectures. While serial devices present several drawbacks as inertia, rigidity and positioning issues, parallel interfaces overcome these drawbacks but present limited workspaces and singular configurations issues. Hybrid haptic interface architectures take advantage of the previous ones. Some devices have been developed commercially, such as the Sigma 7 [20] or the 6-DoF interface proposed by Tsumaki et al. [21].

We have developed a 6-DoF hybrid haptic interface, as shown in Figure 4. It is composed of a 3-DoF Novint Falcon interface combined with an inertial measurement unit (IMU) attached with a spherical wrist. Novint Falcon allows translational motions in 3D space, similar to the one proposed by Tsai [22], into a maximum volume of 10.16 cm along each direction, and a maximum force feedback of around 9 N. A LPMS-B2 inertial measurement unit, located inside the artificial probe measures the rotational motions (3-DoF). The artificial probe mounted on the haptic device simulates a real probe for Doppler sonography examinations. Additionally, the system at the expert site includes a foot pedal to prolong the desired motion sent to the robot along the current direction of motion of the haptic interface.

**Figure 4.** 6-DoF hybrid haptic interface.

2.2.2. Patient Site: 8-DoF Redundant Robot

As mentioned above, we previously proposed the use of a 7-DoF Franka Emika cobot at the patient site for the robotic assistant platform [7]. Nevertheless, the workspace of the 7-DoF cobot is insufficient to perform an examination of the entire patient's body. Therefore, we propose to enlarge the workspace of the robot by mounting it on a beltdriven linear axis (Parker HMRB15) placed parallel to the longitudinal axis of the patient's table (Figure 3). The linear axis is motorized by a 24 V brushless motor coupled with a gear reducer (12.5 mm/lap). More technical details about the linear axis are presented in Table 1.

**Table 1.** Technical characteristics of the linear axis.


When employing the platform, the robotic system is placed next to the patient's table so that the linear axis motion allows the entire table to be covered. It is therefore planned that the patient's waist is aligned with the middle position of the linear axis, facilitating the identification of his positioning by the practitioner.

#### 2.2.3. Communication Framework

Robot Operating System is the middleware used in the platform to guarantee real-time data exchange between the different devices. These applications have been developed in Python and/or C++ and launched in ROS Kinetic version under Ubuntu 16.04 with real-time patched kernel, so that a soft real-time operating system is configured.

The diagram in Figure 5 shows the information transmitted between the different systems. The packages Rosfalcon, Joy and Lmps\_IMU are, respectively, those of the Novint Falcon, the foot pedal and the inertial measurement unit composing the hybrid haptic interface. Concerning the operator site, the package Franka\_ros allows to read/write commands to the cobot's controller and the package Axis communicates with the controller of the linear axis. Finally, the motion of the axis is determined by the Pos\_criteria package, according to the control mode implemented (see Sections 3 and 4).

**Figure 5.** Data exchange of the platform in ROS environment.

Communication between the different systems is achieved through Bluetooth (inertial unit), USB (haptic interface), Ethernet (Franka Emika) and Telnet (linear axis) protocols. In the case of the linear axis, telnet protocol has been chosen for its ease of use, as it uses a Telnet library, i.e., Telnetlib in python.

As the linear axis has been implemented as an upgrade, the package Axis has been created and linked to the existing devices. This package enables the control of the linear axis in either position, speed or torque (compliance). The latter gives us the possibility to achieve compliance control. The ROS node to control the linear axis in torque is depicted in Figure 6.

**Figure 6.** Block diagram of the Axis torque node.

The layout of the three nodes corresponding to the position, speed and torque control remains the same. The speed limit stands low as this application aims to deal with patients, thus security is an important issue. Moreover, in the case of high speed, where robot joints can be damaged, the robot will detect this and freeze, preventing the robot from being used.

Two alternative control modes for the platform at the patient site are proposed in the following sections. The first control mode, presented in Section 3, considers the kinematics of the robot as a fully 8-DoF system. Therefore, the motions of the 8 axes are synchronized to fulfill the desired task and the two degrees of redundancy guarantee the avoidance of singularities of the robot arm. This control mode has for now been validated in simulation. The second control mode, presented in Section 4 and validated experimentally, decouples the motion of the linear axis and the robot arm. The linear axis is only activated if the robot arm approaches its workspace limits. Furthermore, the remaining degree of redundancy of the robot arm is employed to avoid the joint limits through a new adaptive JLA strategy.

#### **3. Fully Redundant Control Mode**

#### *3.1. Robot Modelling*

As mentioned above, the first control mode considers the robot model of an 8-DoF system. Here below we detail the kinematic and dynamic model of the proposed platform. First, the Denavit–Hartenberg parameters' table [23] of the platform is presented in Figure 7, where joint *i* = 0 corresponds to the linear axis and joints *i* = 1, ... , 7 are related to the cobot arm.

**Figure 7.** D-H parameters of the 8-DoF robotic assistant platform.

Considering that a Doppler ultrasound exam requires that the probe fully moves in the space, a task-space dimension of *m* = 6 is defined. Then, let define the non-squared Jacobian matrix **<sup>J</sup>**(**q**) ∈ *m*×*<sup>n</sup>* with *<sup>n</sup>* = 8 denoting the number of robot joints. Therefore, the relation between the joint-space velocity **. <sup>q</sup>** ∈ *<sup>n</sup>* and the task-space velocity **. <sup>x</sup>** ∈ *<sup>m</sup>* yields,

**.**

$$
\dot{\mathbf{x}} = \mathbf{J}(\mathbf{q}) \cdot \dot{\mathbf{q}} \tag{1}
$$

Since the robotic platform is torque-controlled, let us now define its dynamic equation of motion in joint-space as,

$$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\pi}\_c + \boldsymbol{\pi}\_{\text{ext}} + \boldsymbol{\pi}\_f \tag{2}$$

This model depends on the inertial matrix **<sup>M</sup>**(**q**) ∈ *n*×*n*, the centrifugal and Coriolis matrix **C q**, **. q** ∈ *n*×*<sup>n</sup>* and the vector of gravitational torques **<sup>g</sup>**(**q**) ∈ *n*. Moreover, vectors <sup>τ</sup>*<sup>c</sup>* ∈ *n*, <sup>τ</sup>*<sup>f</sup>* ∈ *<sup>n</sup>* and <sup>τ</sup>*ext* ∈ *<sup>n</sup>* represents the output, friction and external torques, respectively.

This section may be divided by subheadings. In the following, we explain the details of the control law defining τ*c*, as well as results of the robot's behavior performed in a customized simulator.

#### *3.2. Control Law*

A compliant behavior of the robotic platform is suitable to reduce the effects of undesired collisions between the robot and the patient. The following control law allows to reproduce the effects of a mechanical damper-spring system at the cartesian space during the execution of a desired trajectory **<sup>x</sup>***<sup>d</sup>* ∈ *m*,

$$\mathbf{F}\_{task} = \mathbf{K}\_p(\mathbf{x}\_d - \mathbf{x}) - \mathbf{K}\_d \dot{\mathbf{x}} \tag{3}$$

where the stiffness and damping effects can be adjusted with the diagonal constant matrices **<sup>K</sup>***<sup>p</sup>* ∈ *m*×*<sup>m</sup>* and **<sup>K</sup>***<sup>d</sup>* ∈ *m*×*m*, respectively [24].

A joint-torque control law implementing the compliant behavior of Equation (3) can be defined as follows,

$$\mathbf{\dot{\tau}\_c} = \mathbf{J^T} \cdot \mathbf{F\_{task}} + \mathbf{\tau\_N} + \mathbf{\tau\_{comp}} \tag{4}$$

This control law also includes the torque vector <sup>τ</sup>*comp* ∈ *n*, compensating the gravitational and dynamic effects, and the torque vector <sup>τ</sup><sup>ℵ</sup> ∈ *n*, exploiting the redundancy of the robot. As mentioned above, redundancy can be used in several ways according to the specific needs of the application. For instance, a suitable way to exploit the redundancy is to stabilize the internal motion, yielding,

$$\mathbf{r}\_{\mathbb{R}} = \mathbb{N}(\mathbf{q}) \left[ \mathbf{K}\_{\mathcal{P}\_{null}} (\mathbf{q}\_{\text{init}} - \mathbf{q}) - \mathbf{K}\_{d\_{null}} \dot{\mathbf{q}} \right] \tag{5}$$

Joint torques produced by Equation (5) attempt to keep the joint positions as best as possible at the initial joint configuration **qinit** ∈ *n*. The weight of this law for each joint can be tuned with the values of diagonal constant matrices *<sup>K</sup>pnull* ∈ *n*×*<sup>n</sup>* and *<sup>K</sup>dnull* ∈ *n*×*n*. In order to guarantee that this control law is only performed in the null-space of the robot, avoiding undesired perturbations in the cartesian-space, the torque vector is premultiplied by a null-space projector <sup>ℵ</sup>(**q**) <sup>=</sup> **<sup>I</sup>***n*×*<sup>n</sup>* <sup>−</sup> **<sup>J</sup>TJ**<sup>+</sup> , defined in terms of the Moore–Penrose pseudoinverse of **J**(**q**), i.e., **J**<sup>+</sup> = **JT <sup>J</sup>**·**JT**−<sup>1</sup> .

#### *3.3. Performance Analysis*

To achieve a compliance control mode of the robotic platform, a dynamic simulator was developed in Matlab-Simulink, using the Simscape toolbox. For purpose of realistic simulations, the CAD of the real robotic platform was added to the simulator. The dynamic model was calculated by Simulink based on the geometric and inertial parameters of the assembled multi-body system. This new tool allowed us to verify the performance of the robot when controlling it by torque, based in [25]. Figure 8 shows the flowchart for the simulator operating principle.

The control law is an association of three laws:

• Recursive Newton–Euler algorithm: This algorithm has been implemented to calculate the gravitational, centrifugal and Coriolis compensation torques.


**Figure 8.** Control schema implemented in the dynamic simulator.

A linear trajectory with a total length of 0.8 m along the *Y-axis* has been imposed to validate the correct functioning of the robotic platform when controlled as an 8-DoF redundant robot. The desired linear trajectory has been chosen in such a way that it exceeds the Franka cobot workspace.

In the initial configuration, the robot end-effector cartesian position is set to 0 m along the *Y-axis*. To perform the desired trajectory, a simultaneous motion is conducted by the cobot and the linear axis, as shown in Figures 9 and 10. Indeed, the values of *Kpnull* have a high influence on the portion of motion performed by the axis and by the cobot along the *Y-axis*. In this case, we put a larger value to the first diagonal value, related to the action of the linear axis, so that its motion is launched easily. However, in the case that the motion of the linear axis is not a priority, this constant value could be reduced to prioritize the motion of the cobot. A comparison between the desired and the performed trajectories are also presented in Figure 11. It can be seen that the desired trajectory is entirely executed by the platform. Errors between the real and desired motions along the three cartesian axis remain low: *xerror* = 1.6·10−5, *yerror* = 1.39·10−<sup>4</sup> and *zerror* = 2.9·10−5, considering that a natural error is induced by the compliance law implemented in Equation (3).

A suitable performance criterion to be analyzed in this new platform is the manipulability index [13], i.e., **M** = **J***T***J**. It is well-known that the manipulability index decreases when the manipulator approaches a singularity configuration, i.e., *det*(**J**) = 0. Naturally, if the linear trajectory is only performed by the cobot arm, the manipulability index would

decrease to zero since the workspace limits would be reached. In contrast, the use of the linear axis and the simultaneous activation of the cobot and the axis allows it to preserve higher manipulability values, such as shown in Figure 12. The manipulability of the robotic platform (8-DoF) is always increasing because of the linear axis's motion. It is also calculated the manipulability of the Franka cobot arm separately, proving that it does not fall, since it is not reaching the limit of its working space.

**Figure 9.** Sequential evolution of the robot configuration during the execution of a linear trajectory of 0.8 m along the *Y*-*axis*. (**a**) Initial, (**b**) intermediate and (**c**) final robot configuration.

**Figure 10.** Joint position trajectories of the 8-DoF robot assistant during the execution of a linear trajectory along the *Y*-*axis*.

**Figure 11.** Cartesian position trajectory of the 8-DoF robot assistant during the execution of a linear trajectory along the *Y*-*axis*.

**Figure 12.** Manipulability Index of the 8-DoF robotic platform and the one of the Franka cobot (7-DoF) during the execution of a linear trajectory along the *Y*-*axis*.

#### **4. Decoupled Redundant Control Mode**

In this section we propose a decoupled control mode for the proposed 8-DoF redundant robotic platform. This means that both the 7-DoF cobot and the linear axis are controlled independently as two separate systems. The details of this control mode are depicted here below.

#### *4.1. Manipulator/Axis Switching Strategy*

The control strategy adopted to associate the linear axis and the 7-DoF robot is achieved by decoupling the two devices. The motion distribution between the cobot and the linear axis is achieved through a central ROS node called pos\_criteria (Figure 6). A desired cartesian position limit *Xlim* is defined to establish a switching condition determining whether the robot or the linear axis moves along the direction of motion of the linear axis, i.e., *Y*-*axis*. Upper and lower limits are imposed to (−0.45, 0.45) m. These limits are largely lower than the maximum robot workspace (0.855 m). Two scenarios depending on the position condition are shown in Figure 13.


**Figure 13.** Movement distribution between the Franka Emika cobot and the linear axis in the *pos\_criteria* node.

Evidently, the cobot always executes the movements along the remaining directions since the linear axis only can be actuated along the *Y*-*axis*.

We note that ∓0.820 m corresponds to the length of the linear axis, where 0 m is the linear axis origin (middle point). The desired position transmitted by the haptic device is q*daxe*. Figure 14 depicts the proposed switching strategy.

#### *4.2. Adaptive Joint-Limit Avoidance Strategy*

A suitable way to exploit the redundancy of the *n* = 7-DoF cobot in the application of Doppler sonography is to enlarge its cartesian workspace, mainly rotational, allowing the ultrasound probe to execute large rotational movements. To achieve this, a suitable strategy consists of moving away the joint positions from the mechanical limits **q***mini* , **q***maxi* . Thereby, the null-space control approach can be defined as follows,

$$\mathbf{r}\_{\mathbb{K}} = \mathbb{K}(\mathbf{q})\mathbf{r}\_{/LA} = \mathbb{K}(\mathbf{q}) \left[ \mathbf{K}\_{/LA} \left( \frac{\partial \mathbf{w}\_{/LA}(\mathbf{q})}{\partial \mathbf{q}} \right)^{T} - \mathbf{D}\_{/LA} \dot{\mathbf{q}} \right] \tag{6}$$

The torque vector <sup>τ</sup>*JLA* ∈ *<sup>n</sup>* maximizes the objective function **<sup>w</sup>***JLA*(**q**) ∈ and is projected to the null-space of *J*(*q*) through ℵ(**q**) to guarantee a compatibility with the cartesian task performed by the cobot. The second term of τ*JLA* allows to stabilize the internal motion of the robot. Let us define the objective function as,

$$\mathbf{w}\_{fLA}(\mathbf{q}) = -\frac{1}{2\pi} \sum\_{i=1}^{n} \left( \frac{\mathbf{q}\_i - \mathbf{q}\_{c\_i}}{\mathbf{q}\_{max\_i} - \mathbf{q}\_{min\_i}} \right)^2 \tag{7}$$

**Figure 14.** Switching robot/axis motion strategy. During a linear trajectory, the robot is initially activated to execute the desired motion. Once *Xlim* is reached (transition area), the axis is activated instead of the robot to execute the motion along the *Y-axis* (the robot keeps moving along the remaining axes).

The diagonal weighting matrix *<sup>K</sup>JLA* ∈ *n*×*<sup>n</sup>* enhances to ponderate each joint according to a prediction of those joints having higher risks to reach the mechanical limits during the execution of the cartesian task. Classical JLA strategies set *KJLA* as constant, which is not an optimal way to exploit the redundant motion of the robot. First, if *KJLA* is full-rank, it causes a permanent generation of null-space torques for all the joints that are not exactly at the middle point of the joint range **q***ci* , even if these joints are far of the mechanical limits. Moreover, the simultaneous avoidance of mechanical limits for different combination of joints can be incompatible with respect to the cartesian task and, therefore, insolvable by the robot.

In order to mitigate the limits of the classical approach, let us propose a new way to ponderate the weight of each joint when applying the JLA strategy of Equation (6). The goal is to adapt the weighting value of a joint *i* according to its proximity to the mechanical limits. Let's denote **<sup>q</sup>***limmini* , **<sup>q</sup>***limmaxi* as the joint position thresholds indicating that the joint is approaching the mechanical limits. Between these two values the weighting value *KJLAi* is set to zero since it is estimated that the joint is far enough from the mechanical limits. Once one of the thresholds is reached, *KJLAi* linearly increases its value from the threshold and until the mechanical limit. Figure 15a compares the evolution of *KJLAi* for both, the classical JLA strategy and the proposed adaptive JLA strategy. Moreover, the consequences for the generation of the null-space torque term *<sup>K</sup>JLA∂***w***JLA*(**q**) *∂***q** *T* are shown. When the proposed strategy is applied to the 7 joints, the proposed strategy generates zero null-space torques if all the joints move between the threshold limits (Figure 15b), which is an interesting advantage since it allows a manual reconfiguration of the elbow robot if needed. Moreover, the proposed strategy avoids the calculation of unneeded null-space torques and limits the case of incompatible torques acting in the nulls-space.

**Figure 15.** Evolution of (**a**) the weighting value and (**b**) the null-space torque for classical and adaptive JLA strategies along the position range of joint *i*.

#### *4.3. Study Case*

In order to validate the usefulness of the proposed adaptive joint-limit avoidance strategy, a situation typically encountered in a Doppler Ultrasound exam is investigated. Since the self-rotation axis of the ultrasound probe is coincident with the last robot joint, i.e., 7th joint, self-rotation movement only requests motions in the last robot joint. This assumption is correct if no null-space torques are applied to the control input. Therefore, the range of the self-rotation is limited to the one of the 7th joints, i.e., (−166◦, 166◦) for the Franka Emika robot. This is frequently not sufficient to carry out common exams. The degree of redundancy of the anthropomorphic robot can then be used to enlarge the rotational workspace of the robot. Figure 16 shows the setup of the study case, where the robot holds the probe over the patient's body and a self-rotation motion is executed according to the orders of the expert site. For the sake of repeatability, the same desired self-rotation trajectory is applied for all the performed tests presented below.

First, a desired self-rotation motion is performed by the robot without a joint-limit avoidance strategy. Figure 17 compares the desired (*φd*) and real (*φr*) orientation trajectory, respectively. It is worth mentioning that the time delay evidenced between the two curves is due to the compliant behavior imposed by the control approach. It can be shown that the robot is unable to execute the desired trajectory since it reaches the mechanical limit of the 7th joint, as confirmed by the joint position trajectories (Figure 18). These results also confirm that only the 7th joint is requested to move in the absence of a null-space torque.

**Figure 16.** Exemplary image of the study case (no human subjects participated to the experiments). A self-rotation motion is imposed to the haptic probe during a Doppler ultrasound exam (see the accompanying Video S1).

**Figure 17.** Comparison between the desired probe self-rotation *φ<sup>d</sup>* and the executed self-rotation *φ<sup>r</sup>* in the absence of a JLA strategy.

**Figure 18.** Joint position trajectories in the absence of a JLA strategy.

The same case is investigated when applying the joint-limit avoidance strategy of Equation (6). Initially, the classical JLA approach with constant values at the diagonal weighting matrix is evaluated. The performed experiences are resumed in Table 2 and the results are presented in Figure 19. Since we seek to avoid that the 7th joint reaches the mechanical limits, the first tests have been performed setting a non-zero constant value only to the 7th joint. Experiments have been performed with three different values (*KJLA*<sup>7</sup> <sup>=</sup> 6, 8 and 10 <sup>√</sup>*Nmrad*) and results in Figure <sup>19</sup> confirm that these values effectively enlarge the rotational robot's workspace moving away from the 7th joint from the mechanical limits. To do this, the robot's elbow motion, related to the degree of redundancy of the cobot, is reoriented accordingly. However, none of these tests allows completely following the desired trajectory since the elbow motion causes the 2nd joint to reach the mechanical limits. Therefore, three supplementary tests have been performed setting non-zero values to joints two and seven (see cases 4–6 in Table 2). Although the obtained results show that the rotational workspace can be enlarged depending on the combination of the constant values. This classical strategy is still limited because of the linear evolution of the null-space torques along with the joints' range, such as were explained in the previous section.


**Table 2.** Different weighting matrix choices for classical joint-limit avoidance strategy.

Finally, we tested the proposed adaptive JLA strategy presented in Section 4.2. The control parameters of the strategy were tuned according to Table 3, where a margin of security of 30◦ has been set up between the joint limit thresholds and the mechanical limits provided by the constructor.

**Figure 19.** Comparison between the desired probe self-rotation *φ<sup>d</sup>* and the executed self-rotation *φ<sup>r</sup>* for the different constant weighting matrix choices presented in Table 1.

**Table 3.** Joint parameters applied for the adaptive JLA strategy. The mechanical limits **q***min* and **q***max* are provided by the constructor.


Figures 20–22 show the obtained results for the execution of the desired self-rotation trajectory. It can be seen in Figure 20 that the robot completely executes the desired trajectory due to the adaptive JLA strategy. It is worth mentioning that the offset observed at the end of the trajectory is caused by the tuning of low stiffness values in **K***p*. Figures 21 and 22 also show that only joints two and seven reach the joint limit thresholds and, therefore, the other weighting joints rest at zero, avoiding the generation of useless null-space torques. Furthermore, unlike the classical JLA strategy, in this case, if none of the joints reaches the corresponding limit thresholds, no null-space torques are generated and the elbow robot can freely be moved by hand.

However, in order to endorse the correct functioning of the entire platform, a test phase in real conditions on multiple exams must be undertaken. This step will validate the conducted work and will initiate the improvement of the haptic interface to meet the angiologist's requirements. This first work opens the door to study multiple control strategies exploiting the degrees of redundancy of the platform.

**Figure 20.** Comparison between the desired probe self-rotation *φ<sup>d</sup>* and the executed self-rotation *φ<sup>r</sup>* when applying the adaptive JLA strategy.

**Figure 21.** Joint position trajectories when applying the adaptive JLA strategy.

**Figure 22.** Evolution of the diagonal weighting values related to each robot joint for the adaptive JLA strategy.

#### **5. Conclusions and Future Work**

This paper presents an innovative redundant robotic assistant that combines a 7-DoF collaborative Franka robot and a linear axis to achieve a 7 + 1 DoF platform. This robot assistant is conceived to assist sonographers in Doppler Sonography exams and, unlike existing systems, is suitable for full body scanning without requiring manual repositioning of the robot base. The details of the kinematic design have been presented here, including the operator site (6-DoF hybrid haptic interface) and the patient site (7-DoF cobot + 1-DoF linear axis). Moreover, two alternative torque-based control laws have been proposed to deal with the degrees of redundancy of the platform, whereas a compliant behavior has been implemented for the execution of the main medical task, which is a valuable safety feature in case of undesired contacts with the patient. The first mode, called "fully redundant control mode", comprises the 8-DoF in the kinematic model so that the motion of the 8 axes is synchronized. Moreover, the prioritization of the linear axis motion is tuned by a null-space torque added to the torques performing the main task. The second control mode, "decoupled control mode", considers the two systems, the cobot and the linear axis, separately, and a switching strategy is proposed to launch the motion of the linear axis when the cobot is approaching the workspace limits. In addition, an adaptive JLA strategy has been proposed in this paper to exploit the redundancy of the cobot. In fact, unlike the classical strategies tuning a constant weighting matrix for the JLA strategy and generating unsuitable remaining torques, the new strategy proposes a variable weighting matrix, whose values are adapted according to the proximity of each robot joint to its mechanical limit. Therefore, the priority is judiciously given by order to the joints closest to their limits. In the opposite case, if none of the joints is near the mechanical limit, no additional null-space torques are generated. The application of this strategy naturally enlarges the workspace of the robot, as it has been shown for a study case presented for the examination of the inner side of a knee.

Future work will be dedicated to the medical validation of the experimental platform. It is planned to validate the usefulness of the proposed platform during Doppler sonography exams compared to the classical gesture, mostly in terms of time of execution and ergonomic conditions. Moreover, technical adjustments such as the velocity of motion of the linear axis and the choice of the optimal type of control will be studied. Finally, this work opens the door to the study of the proposed adaptive JLA strategy for several applications and robotic systems.

**Supplementary Materials:** The following supporting information can be downloaded at: https://www. mdpi.com/article/10.3390/act11020033/s1; Video S1: Caption of Figure 16 refers the accompanying video.

**Author Contributions:** E.G. and J.S. have designed the experiments and co-wrote the paper; A.T. has participated to the development of the platform; J.-M.G. is the Doppler sonographer who has validated the platform; the research work has been supervised by M.A.L., G.C. and S.Z. All authors have read and agreed to the published version of the manuscript.

**Funding:** This research was funded by the University of Poitiers and PPRIME Institute.

**Institutional Review Board Statement:** Not applicable.

**Informed Consent Statement:** Not applicable.

**Conflicts of Interest:** The authors declare no conflict of interest.

#### **References**


**Luca Bruzzone \*, Pietro Fanghella and Davide Basso**

DIME Department, University of Genoa, 16145 Genoa, Italy; pietro.fanghella@unige.it (P.F.); dvd.genova@gmail.com (D.B.)

**\*** Correspondence: luca.bruzzone@unige.it; Tel.: +39-010-3352967

**Abstract:** This paper presents an extension of impedance control of robots based on fractional calculus. In classical impedance control, the end-effector reactions are proportional to the end-effector position errors through the stiffness matrix K, while damping is proportional to the first-order timederivative of the end-effector coordinate errors through the damping matrix D. In the proposed approach, a half-derivative damping is added, proportional to the half-order time-derivative of the end-effector coordinate errors through the half-derivative damping matrix HD. The discrete-time digital implementation of the half-order derivative alters the steady-state behavior, in which only the stiffness term should be present. Consequently, a compensation method is proposed, and its effectiveness is validated by multibody simulation on a 3-PUU parallel robot. The proposed approach can be considered the extension to MIMO robotic systems of the PDD1/2 control scheme for SISO mechatronic systems, with potential benefits in the transient response performance.

**Keywords:** impedance control; fractional calculus; half-order derivative; parallel kinematics machine

#### **1. Introduction**

In a wide range of robotic applications, for example, assembly of electronic boards or handling of objects to be placed on horizontal pallets, the full mobility (6-DOF) of the end-effector is not necessary, since the tasks can be proficiently performed by means of a 3-DOF translational motion or by a 4-DOF motion with three translations and one rotation around a vertical axis (Schoenflies motion [1]). The rotational degree of freedom of the Schoenflies motion is often obtained by adding a 1-DOF wrist to a translational mechanism.

Considering serial robots, translational and Schoenflies motions are realized in most cases by Cartesian robots or by SCARA robots [2–5]. Considering parallel kinematics machines (PKMs), translational motion can be obtained by parallel Cartesian robots [6–8], characterized by three legs that are planar serial mechanisms moved by three orthogonal linear actuators perpendicularly to their planes, or by other closed-loop schemes which are not purely translational in general, but become purely translational in case of specific orientations of the joint axes [1,9–11]. If necessary, translational PKMs can be upgraded to Schoenflies motions by a 1-DOF wrist, but there are also some designs of PKMs which perform native Schoenflies motions [12,13].

No matter if it is serial or parallel, a robot can be controlled in position if the task can be correctly performed regulating only the end-effector trajectory, or by hybrid position/force control when the proper execution of the task requires accurate force regulation in some phases and in some directions [14]. An intermediate approach, which is widely adopted since it does not require force sensors, is represented by impedance control [15,16].

The basic concept of impedance control is that the robot end-effector, subject to external forces, follows a trajectory with a predetermined spatial compliance. The relationship between the force/moment exerted by the end-effector on the environment and the end-effector position/orientation error is defined by means of the stiffness and damping

**Citation:** Bruzzone, L.; Fanghella, P.; Basso, D. Application of the Half-Order Derivative to Impedance Control of the 3-PUU Parallel Robot. *Actuators* **2022**, *11*, 45. https:// doi.org/10.3390/act11020045

Academic Editors: Marco Carricato and Edoardo Idà

Received: 20 December 2021 Accepted: 30 January 2022 Published: 1 February 2022

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

matrices. This approach allows to obtain a compliant behavior in the directions which must be force-controlled, and a stiff behavior in the directions which must be position-controlled.

There are many possible ways to the define the orientation of a rigid body in space [17], and this results in different possible approaches for the definition of the rotational stiffness/damping in impedance control [18–21]. On the contrary, for translational robots, the approach to impedance control is much less diversified and simpler, since a point position in space is always represented using an orthogonal reference frame. Additionally, in case of a robot with Schoenflies motion, impedance control is simplified, since the rotation is mono-dimensional, and the rotational behavior is decoupled from the translational behavior. In this work, we will consider only translational impedance control, considering that it can be easily extended to robots with Schoenflies motion.

For translational robots, the impedance behavior of the end-effector is usually defined by means of the stiffness and damping matrices, which respectively represent the zero-order and the first-order terms of a three-dimensional PD control in the external coordinates, expressed in the principal reference frame. These matrices are non-diagonal in the world frame when the principal stiffness and damping directions are not parallel to the axes of the fixed reference frame [11].

Some authors have proposed nonlinear impedance algorithms, in which nonlinear stiffness and damping are imposed to the end-effector, in particular for a better execution of cooperative human–robot tasks, or to maintain position/orientation within a specified region even in case of excessive forces/moments [22–24].

An alternative method to define the impedance behavior is based on fractional calculus, which introduces derivatives and integrals of non-integer order [25]. Accordingly, the damping term can be defined proportional not to the first-order derivative of the endeffector error, but to a derivative with generic non-integer order *μ*. In the scientific literature, there are some examples of fractional-order impedance controls of robots [26,27]. This kind of impedance control generalizes to a three-dimensional system the fractional-order PD*<sup>μ</sup>* control scheme for SISO systems [28]. Fractional-order impedance can be used, for example, to perform contact force tracking control more accurately than traditional impedance control [29].

In the proposed work, the stiffness/damping behavior imposed by the impedance control is linear, but a half-order term, based on the fractional derivative of order 1/2 of the position error, is added to the zero-order and first-order terms of classical impedance control. This impedance control generalizes to a three-dimensional system the fractionalorder PDD1/2 control scheme developed, so far, for single axes [30].

As a matter of fact, it is evident that traditional linear impedance control of a *n*-DOF mechanical system (KD) is the *n*-dimensional version of the PD control of a 1-DOF mechanical system, with the *n*-dimensional stiffness term K, proportional to the position error, corresponding to the proportional term, and the *n*-dimensional damping term D, proportional to the first-order derivative of the position error, corresponding to the derivative term. The proposed KDHD impedance control is obtained from the KD scheme with the addition of an *n*-dimensional half-order damping term HD, proportional to the derivative of order 1/2 (half-derivative) of the position error; therefore, the aim of this work is to investigate if the addition of the half-derivative term in the implementation of the impedance algorithm can bring the same benefits that have been shown theoretically and experimentally for the PDD1/2 control of a 1-DOF inertial system with respect to the classical PD scheme [31].

As a case study, the KDHD impedance control is applied to a 3-PUU parallel robot and compared to the classical KD impedance control.

In the remainder of the paper, Section 2 discusses the theoretical definition and the digital implementation of the half-derivative of a function of time, Section 3 proposes the KDHD impedance control, highlighting the differences with respect to the classical KD impedance control, and Section 4 presents the 3-PUU architecture and its kinematics. Section 5 compares the KDHD and the KD impedance controls applied to the 3-PUU robot by multibody simulation, and Section 6 outlines conclusions and future developments.

#### **2. Half-Order Derivative: Definition and Digital Implementation Issues**

Fractional calculus generalizes the concept of derivative and integral to non-integer order [25]. According to the Grünwald–Letnikov definition, suitable for robust discretetime implementation [32], the fractional differential operator for a continuous function of time, *x(t)*, is defined as:

$$\frac{d^a}{dt^a}\mathbf{x}(t) = \lim\_{h \to 0} \frac{1}{h^a} \sum\_{k=0}^{\lfloor \frac{t-a}{h} \rfloor} (-1)^k \frac{\Gamma(a+1)}{\Gamma(k+1)\Gamma(a-k+1)} \mathbf{x}(t-kh),\tag{1}$$

where *<sup>α</sup>* <sup>∈</sup> <sup>R</sup><sup>+</sup> is the differentiation order, *<sup>a</sup>* and *<sup>t</sup>* are the fixed and variable limits, <sup>Γ</sup> is the Gamma function, *h* is the time increment, and [*y*] is the integer part of *y*. For realtime digital implementation with sampling time *Ts*, Equation (1) can be rewritten in the following form [31]:

$$\mathbf{x}(t)^{(a)} \cong \mathbf{x}(kT\_s)^{(a)} \cong \frac{1}{T\_s^a} \left( \sum\_{j=0}^k w\_j^a \mathbf{x}((k-j)T\_s) \right), k = [(t-a)/T\_s]. \tag{2}$$

where:

$$w\_0^{\kappa} = 1,\\
w\_j^{\kappa} = \left(1 - \frac{\kappa + 1}{j}\right) w\_{j-1}^{\kappa}, j = 1, \ 2, \ \dots \tag{3}$$

The calculation of Equation (2) requires considering all the *k+1* sampled values of the time history of *x*; therefore, the incessant increase of the number of addends is an issue for real-time implementation, and consequently, only a fixed number of *n* previous steps is used in Equation (2), realizing a *nth* order digital filter, with fixed memory length *L = nTs*. This is acceptable according to the so-called short-memory principle [33], which states that considering only the recent past of the function in the interval [*t–L, t*] does not yield relevant approximations in the evaluation of the fractional derivative.

Nevertheless, the truncation of the summation of Equation (2) to *n+1* addends is an issue if the fractional derivative is applied to impedance control. As a matter of fact:

$$\lim\_{k \to \infty} \sum\_{j=0}^{k} w\_j^{\alpha} = 0,\tag{4}$$

but with finite *n,* this summation is non-null:

$$\mathcal{W}\_{\mathfrak{a}}(n) = \sum\_{j=0}^{n} w\_j^{\mathfrak{a}} > 0. \tag{5}$$

Moreover, *Wα* tends to zero quite slowly, as shown in Figure 1, as an example, for *α* = 1/2 (half-derivative).

**Figure 1.** Summation of the approximated fractional-derivative filter terms (*Wα*) as a function of the filter order, for *α* = 1/2 (half-derivative).

As a consequence, the numerically evaluated fractional derivative of a constant *c* is non-null, but equal to *cWα*(*n*)/*T<sup>α</sup> <sup>s</sup>* , tending to zero as *n* tends to infinite. This influences the behavior of impedance control in the steady state, giving rise to a constant position error. In this condition, only the stiffness term should be non-null, but if fractional-order damping is numerically evaluated with a finite-order digital filter, the half-derivative damping term is also present, and should be properly compensated.

#### **3. KDHD Impedance Control**

The classical formulation of impedance control with gravity compensation of a nonredundant parallel robot is expressed by the following control law [11]:

$$\mathbf{r} = \left(\mathbf{J}\_p^T\right)^{-1} \left[\mathbf{K}\_{KD}(\mathbf{x}\_d - \mathbf{x}(\mathbf{q})) + \mathbf{D}\_{KD}(\mathbf{x}\_d - \mathbf{x}(\mathbf{q}))^{(1)}\right] + \mathbf{r}\_\circ(\mathbf{q}),\tag{6}$$

where **J**p is the Jacobian matrix for a parallel robot, which defines the relationship between the time-derivative of the external coordinates **x** and the time-derivative of the internal coordinates **<sup>q</sup>**: **. .**

$$
\dot{\mathbf{q}} = \mathbf{J}\_p \dot{\mathbf{x}} \tag{7}
$$

In Equation (6), **K***KD* and **D***KD* are the stiffness and damping matrices, **x***<sup>d</sup>* is the reference trajectory expressed in external coordinates, and τ*<sup>g</sup>* is the gravity compensation vector.

Let us note that the matrices **K***KD* and **D***KD*, on the basis of the robot mobility, define the translational impedance, the rotational impedance, or both [21]. In case of robots with Schoenflies motion, their size is 4 × 4, but the rotational behavior is evidently decoupled from the translational behavior; therefore, the stiffness and damping matrices are blockdiagonal, with a 3 × 3 submatrix representing the translational behavior and a fourth diagonal element representing the rotational behavior. In the following, for the sake of simplicity, we will consider translational robots with 3 × 3 stiffness and damping matrices, bearing in mind that the proposed approach can be easily extended to robots with Schoenflies motion.

The stiffness and damping matrices are in general non-diagonal in the fixed reference frame, W. On the basis of the task requirements, it is possible to choose a principal reference frame, P, in which it is convenient to define decoupled stiffness and damping; therefore, the stiffness and damping matrices are defined diagonal in P and are transformed into the frame W by means of the rotation matrix between W and P [21]:

$$\mathbf{K}\_{KD} = \left(\mathbf{R}\_W^P\right)^T \mathbf{K}\_{KD}^P \mathbf{R}\_{W'}^P \tag{8}$$

$$\mathbf{D}\_{KD} = \left(\mathbf{R}\_W^P\right)^T \mathbf{D}\_{KD}^P \mathbf{R}\_W^P. \tag{9}$$

In the proposed KDHD impedance control, the damping term is not proportional only to the first-order derivative of the position error in external coordinates, but also to its half-derivative (derivative of order 1/2), in order to implement the extension from PD to PDD1/2 in the three-dimensional space:

$$\mathbf{r} = \left(\mathbf{J}\_p^T\right)^{-1} \left[\mathbf{K}\_{\mathrm{KDHD}}(\mathbf{x}\_d - \mathbf{x}(\mathbf{q})) + \mathbf{D}\_{\mathrm{KDHD}}(\mathbf{x}\_d - \mathbf{x}(\mathbf{q}))^{(1)} + \mathbf{H}\mathbf{D}\_{\mathrm{KDHD}}(\mathbf{x}\_d - \mathbf{x}(\mathbf{q}))^{(1/2)}\right] + \mathbf{\tau}\_\mathcal{S}(\mathbf{q}) \tag{10}$$

Obviously, the half-derivative damping matrix **HD***KDHD* can also be defined in the principal reference frame, P, and then transformed to the frame W:

$$\mathbf{HD}\_{KDHD} = \left(\mathbf{R}\_{W}^{P}\right)^{T} \mathbf{HD}\_{KDHD}^{P} \mathbf{R}\_{W}^{P}.\tag{11}$$

Let us note that in the definition of the KDHD impedance control law, a non-redundant parallel robot has been considered. In case of non-redundant serial robots, the approach is similar, but it is more convenient to adopt the Jacobian matrix, which transfers from internal coordinates' derivatives to external coordinates' derivatives [2]:

**.**

$$
\dot{\mathbf{x}} = \mathbf{J}\_s \dot{\mathbf{q}}.\tag{12}
$$

Consequently, for application to non-redundant serial robots, the impedance control laws (6) and (10) can be rewritten in the following form:

$$\mathbf{r} = \mathbf{J}\_s^T \left[ \mathbf{K}\_{KD} (\mathbf{x}\_d - \mathbf{x}(\mathbf{q})) + \mathbf{D}\_{KD} (\mathbf{x}\_d - \mathbf{x}(\mathbf{q}))^{(1)} \right] + \mathbf{r}\_\mathcal{S}(\mathbf{q}),\tag{13}$$

$$\mathbf{r} = \mathbf{J}\_s^T \left[ \mathbf{K}\_{\mathrm{KHDD}} (\mathbf{x}\_d - \mathbf{x}(\mathbf{q})) + \mathbf{D}\_{\mathrm{KHDD}} (\mathbf{x}\_d - \mathbf{x}(\mathbf{q}))^{(1)} + \mathbf{H} \mathbf{D}\_{\mathrm{KHDD}} (\mathbf{x}\_d - \mathbf{x}(\mathbf{q}))^{(1/2)} \right] + \mathbf{\tau}\_\mathcal{S}(\mathbf{q}) \tag{14}$$

#### **4. Kinematic and Dynamic Model of the 3-PUU Parallel Robot**

The proposed KDHD impedance control has been tested by multibody simulation on the 3-PUU parallel robot, whose scheme is shown in Figure 2.

The reference frame O(x,y,z) of the base platform is located at the center of the equilateral triangle B*1*B*2*B*3*, whose vertices lie on a circumference with center O and radius *R*, while the reference frame O'(x',y',z') of the moving platform is located at the center of the equilateral triangle A*1*A2A*3*, whose vertices lie on a circumference with center O' and radius *r*. The unit vector **e**, perpendicular to the plane of the triangle B*1*B*2*B*3*, defines the direction of the three prismatic joints, and the vector of the internal coordinates **q** = [*q1, q2, q3*] <sup>T</sup> is composed by the distances of the centers of the three sliders from the points B*1*, B*2*, and B*3*; consequently, the three vectors, *q1***e**, *q2***e**, and *q3***e**, represent the displacements of the sliders' centers with respect to B*1*, B*2*, and B*3*. For constructive reasons, the centers of the universal joints mounted on the sliders do not lie on the prismatic joints' axes, but are shifted by the three vectors **d***i*, *i* = 1 ... 3, with equal module *d* and the direction of the vector (O–B*i*). Six universal joints are located at the points C*1*, C*2*, C*3*, A*1*, A*2*, and A*3*. In order to obtain purely translational motion of the moving platform, in each PUU kinematic chain, the first revolute axis of the upper U joint must be parallel to the first revolute axis of the lower U joint, and the second revolute axis of the upper U joint must be parallel to the second revolute axis of the lower U joint [34].

#### **5. Comparison of the KD and KDHD Impedance Control Laws by Multibody Simulation**

#### *5.1. Modeling and Simulation Overview*

The model of the 3-PUU parallel robot has been implemented in the multibody simulation environment Simscape MultibodyTM by MathWorks (Figure 3). The robot geometrical and inertial parameters considered in the simulations are collected in Table 1.

**Figure 3.** Multibody model of the 3-PUU parallel robot.



The proposed KDHD impedance control law has been compared to KD impedance control in the following case studies.

(A) End-effector reference trajectory, **x**d, characterized by displacements along the three directions with trapezoidal speed laws, no external force on the end-effector, and the end-effector behavior is isotropic: the stiffness and damping matrices are diagonal with three equal elements. This case represents an approach/depart motion without contact with the environment.

(B) Constant end-effector reference position and external force applied to the endeffector. The stiffness and damping matrices are diagonal in the world frame (the fixed reference frame, W, coincides with the principal stiffness/damping reference frame, P), but the end-effector behavior is not isotropic, since the three diagonal elements are not

equal. This case study evidences how with the KDHD scheme, the approximation of the half-derivative calculation by means of a digital filter with a fixed memory length alters the stiffness imposed by impedance control, as discussed in Section 2; consequently, the stiffness matrix must be properly compensated. A compensation method (KDHDc) is proposed, and its effectiveness is validated by simulation.

(C) Constant end-effector reference position and external force applied to the endeffector, as in case B, but with non-diagonal stiffness and damping matrices, since the fixed reference frame, W, does not coincide with the principal stiffness/damping reference frame, P. Additionally, in this case, the effectiveness of the proposed stiffness-compensated KDHDc impedance control is validated by simulation.

In all the case studies, gravity force is neglected, since its effect is exactly compensated by the gravity compensation term, which is equal for all the considered impedance controls schemes. This allows to better highlight the differences between the KD, KDHD, and KDHDc schemes, eliminating the equal contribution to the actuation forces due to gravity.

Impedance control is based on the measurement of the internal coordinates of the robot without measurement of the force exerted by the end-effector on the environment, as in the case of hybrid position/force control. The internal coordinates of the robot, both for rotational and for linear actuators, are measured by digital encoders, which are not affected by noise; therefore, we have decided not to take into account noise on the measured position of the three sliders.

On the other hand, some noise is certainly present on the actuation forces, due to the electrical effects of the current loop of the motor drivers; nevertheless, the sensitivity of the closed-loop transfer function to disturbances on the direct path, which comprises the motor driver, is much lower than the sensitivity to disturbances on the feedback path. Consequently, noise has been neglected in the first stage of the research.

#### *5.2. Case Study A*

In this case study, the stiffness and damping matrices are diagonal and isotropic both for the KD and KDHD impedance controls. For the KD scheme:

$$\mathbf{K}\_{KD} = k\_{KD}\mathbf{I}\_{\prime} \tag{15}$$

$$\mathbf{D}\_{\rm KD} = d\_{\rm KD} \mathbf{I}\_{\prime} \tag{16}$$

where **I** is the identity matrix. The diagonal value, *dKD*, of the damping matrix can be selected starting from the nondimensional damping coefficient, *ζKD*, according to the following expression [35]:

$$d\_{KD} = 2\zeta\_{KD}\sqrt{k\_{KD}m\_{mp\prime}}\tag{17}$$

in which the mass, *mmp*, of the moving platform (including the three lower universal joints) is considered.

For the KDHD scheme, the stiffness matrix and the first-order and half-order damping matrices are:

$$\mathbf{K}\_{\rm KDHD} = k\_{\rm KDHD} \mathbf{I}\_{\rm \prime} \tag{18}$$

$$\mathbf{D}\_{\rm KDHD} = d\_{\rm KDHD} \mathbf{I}\_{\rm \prime} \tag{19}$$

$$\mathbf{H} \mathbf{D}\_{\text{KDHD}} = h d\_{\text{KDHD}} \mathbf{I}.\tag{20}$$

The diagonal values *dKDHD* and *hdKDHD* of the damping matrices can be selected starting from the nondimensional coefficients *ζKDHD* and *ψKDHD*, according to the following expressions:

$$d\_{\rm KDHD} = 2\zeta\_{\rm KDHD}\sqrt{k\_{\rm KDHD}m\_{mp\_{\prime}}} \tag{21}$$

$$
\hbar d\_{\rm KDHD} = \psi\_{\rm KDHD}^{3/4} m\_{mp}^{1/4}.\tag{22}
$$

The coefficients *ζKDHD* and *ψKDHD* non-dimensionally represent the derivative and half-derivative damping terms [35]. In [36], three couples of PD and PDD1/2 tunings are compared in the control of a linear axis (Table 2).



These three couples of PD and PDD1/2 tunings have been selected as starting points of the research using the nondimensional approach discussed in [36] to derive the coefficients *ζ* and *ψ* of a PDD1/2 controller from the coefficient *ζ* of a reference PD controller, with application to a nondimensional second-order purely inertial linear system, with transfer function *G(s) = 1/s2*. This method can be summarized as follows:


Table 2 collects the PDD1/2 *ζ*–*ψ* combinations associated to three reference PD controllers, with *ζ* = 0.8, 1, and 1.2. The basic idea of this approach is to obtain a PDD1/2 tuning with a similar control effort as the corresponding PD tuning, but with improved readiness. Since this approach is nondimensional, the association between the coefficient *ζ* of the PD and the *ζ*–*ψ* combination of the PDD1/2 controller is not influenced by the system mass/inertia nor by the proportional gain; moreover, even if this approach is based on the step input, the benefits in terms of control readiness of the PDD1/2 controller are also demonstrated with different reference inputs [36].

The stiffness and damping matrices for the KD and KDHD impedance controls have been obtained starting from the nondimensional values of Table 2, imposing *kKD* <sup>=</sup> *kKDHD* = 1·10<sup>3</sup> N/m and using Equations (15) to (22); then, the two control laws have been compared in the presence of a trapezoidal reference trajectory, **x***d*, characterized by four phases:


Some simulation results are shown in Figures 4 and 5, with reference to the KD-KDHD comparison number I (*ζKD* = 0.8, Table 2), ramp time *tramp* = 0.5 s, and stop time = 3 s.

**Figure 4.** Case study A, *kKD* <sup>=</sup> *kKDHD* = 1·103 N/m, KD-KDHD comparison number I, ramp time *tramp* = 0.5 s, stop time *tstop* = 3 s, external coordinates.

**Figure 5.** Case study A, *kKD* <sup>=</sup> *kKDHD* = 1·103 N/m, KD-KDHD comparison number I, ramp time *tramp* = 0.5 s, stop time *tstop* = 3 s, actuation forces.

Table 3 summarizes the results of the case study A with the three considered KD-KDHD tuning couples of Table 2, and with two different ramp times (0.5 and 1 s), while the stop time has been kept constant at 3 s, sufficient to reach the settling time to within 2% after phases 1 and 3. The half-derivative is calculated using Equation (2) with sampling time *Ts* = 0.005 s and filter order *n* = 10, and these values are suitable for a real-time digital implementation on a commercial controller. The total control effort related to the three motors, reported in Table 3, is defined as:

$$E\_{\rm tot} = \sum\_{i=1}^{3} \int\_{0}^{\tau\_{\rm sim}} \tau\_{i}^{2} dt\_{\prime} \tag{23}$$

where *Tsim* is the simulation time.

Comparing the performances of the two impedance controllers in the six cases of Table 3, it is possible to observe the following outcomes:




**Table 3.** KD/KDHD impedance control comparison, case study A.

In [31], a detailed discussion of the PDD1/2 tuning of a second-order, purely inertial system is outlined, and these tuning criteria can be applied to the KDHD impedance control tuning, bearing in mind that the nonlinearity of a MIMO system as an impedance-controlled PKM introduces some alterations in the three-dimensional extension of the PDD1/2 concept.

Let us note that the considered trapezoidal reference position law is characterized by velocity steps and impulsive accelerations in 0, *tramp*, (*tramp* + *tstop*), and (2*tramp* + *tstop*); consequently, the position error cannot be completely eliminated, not even adopting a model-based control scheme, since infinite actuation forces would be required. Accordingly, some overshoot is unavoidable in the transitory states after the discontinuities in *tramp* and (2*tramp* + *tstop*). A smoother reference law for position, for example, with cycloidal or trapezoidal speed, would decrease the position error; nevertheless, here, a trapezoidal position law was adopted to highlight the differences between the KD and the KDHD impedance controls in trajectory tracking. The position error can be reduced by using higher stiffness, with increasing force peaks; for example, Figures 6 and 7 represent the external coordinates and the actuation forces for the same KD-KDHD comparison of Figures 4 and 5 (comparison number I, ramp time *tramp* = 0.5 s, stop time *tstop* = 3 s), but with *kKD* <sup>=</sup> *kKDHD* = 1·104 N/m. The maximum error of the external coordinates is lower, but the actuation forces are higher.

#### *5.3. Case Study B*

In this case study, the stiffness and damping matrices are diagonal in the world frame, but the desired end-effector behavior is not isotropic; therefore, the three diagonal elements of each matrix are not equal as in case study A. We impose higher compliance on the *<sup>z</sup>*-axis through the following values: *kKD,x* <sup>=</sup> *kKDHD,x* <sup>=</sup> *kKD,y* <sup>=</sup> *kKDHD,y* = 2·10<sup>4</sup> N/m, *kKD,z* <sup>=</sup> *kKDHD,z* = 1·10<sup>3</sup> N/m. The diagonal values of the damping matrices are obtained as in case A, starting from the nondimensional parameters of Table 2 and using Equations (17), (21), and (22) separately for each axis. A force *F* = [100,100,100] <sup>T</sup> N is applied to the end-effector at *t* = 0 s. The half-derivative is calculated adopting the same discrete-time implementation as in case A (*Ts* = 0.005 s, *n* = 10).

Figures 8 and 9 show the simulation results with reference to the KD-KDHD comparison number II (*ζKD* = 1, Table 2), in terms of external coordinates and actuation forces. Observing Figure 8, it is possible to note that the steady-state displacements of the external

coordinates using the impedance control KD (blue) and KDHD (violet) are different. This is due to the fact, already discussed in Section 2, that the approximation of the half-derivative calculation by means of a digital filter with a fixed memory length alters the stiffness imposed by impedance control. As a matter of fact, considering that the half-derivative of a constant *c* is non-null, but equal to *cW1/2(n)/(Ts) 1/2*, as discussed at the end of Section 2, the following stiffness-compensated KDHD impedance control (KDHDc) is proposed:

$$\begin{aligned} \mathbf{r} &= \left(\mathbf{J}^T\right)^{-1} \Big[ \left(\mathbf{K}\_{KDHD} - \frac{W\_{1/2}(n)}{T\_s^{1/2}} \mathbf{H} \mathbf{D}\_{KDHD}\right) \big(\mathbf{x}\_d - \mathbf{x}(\mathbf{q})\big) + \\\ &+ \mathbf{D}\_{KDHD} \big(\mathbf{x}\_d - \mathbf{x}(\mathbf{q})\big)^{(1)} + \mathbf{H} \mathbf{D}\_{KDHD} \big(\mathbf{x}\_d - \mathbf{x}(\mathbf{q})\big)^{(1/2)} \Big] + \mathbf{r}\_\mathcal{S}(\mathbf{q}) \end{aligned} \tag{24}$$

The effectiveness of this stiffness compensation is validated by the fact that applying the KDHDc control law, the external coordinates (Figure 8, yellow) tend to the same values obtained by applying the KD control law, which is not affected by the stiffness alteration due to the numerical evaluation of the half-derivative; as expected, these steady-state values correspond to the force/stiffness ratios *Fx/kKD,x*, *Fy/kKD,y*, and *Fz/kKD,z* for the three directions. Let us note that, due to the definition of *Wα*, this compensation is correct only in the steady state with a constant position reference, which are the conditions for which it has been introduced to obtain a correspondence between steady-state force and displacement.

**Figure 6.** Case study A, *kKD* <sup>=</sup> *kKDHD* = 1·104 N/m, KD-KDHD comparison number I, ramp time *tramp* = 0.5 s, stop time *tstop* = 3 s, external coordinates.

**Figure 7.** Case study A, *kKD* <sup>=</sup> *kKDHD* = 1·104 N/m, KD-KDHD comparison number I, ramp time *tramp* = 0.5 s, stop time *tstop* = 3 s, actuation forces.

**Figure 8.** Case study B, KD-KDHD comparison number II, external coordinates.

**Figure 9.** Case study B, KD-KDHD comparison number II, actuation forces.

#### *5.4. Case Study C*

In this case study, the stiffness compensation of the impedance control law KDHDc (Equation (24)) is applied when the fixed reference frame, W, does not coincide with the principal stiffness/damping reference frame, P. This occurs when impedance control must impose compliance in a specific direction, not coincident with one axis of the reference frame W, and higher stiffness in the remaining ones. In this case, a rotation of 45◦ around the *z*-axis is considered; therefore, the rotation matrix between W and P is:

$$\mathbf{R}\_{W}^{P} = \begin{bmatrix} \sqrt{2}/2 & \sqrt{2}/2 & 0\\ -\sqrt{2}/2 & \sqrt{2}/2 & 0\\ 0 & 0 & 1 \end{bmatrix} \tag{25}$$

The stiffness matrix expressed in the principal reference frame, P, is characterized by the following diagonal values: *kKDp,x* <sup>=</sup> *kKDHDp,x* = 1 × <sup>10</sup><sup>3</sup> N/m, and *kKDp,y* <sup>=</sup> *kKDHDp,y* <sup>=</sup> *kKDp,z* <sup>=</sup> *kKDHDp,z* = 2 × 104 N/m, with higher compliance along the *<sup>x</sup>*-axis of the reference P. A force of *F* = [0,100,100] <sup>T</sup> N in the frame W is applied to the end-effector at *t* = 0 s. The half-derivative is calculated adopting the same discrete-time implementation as in cases A and B (*Ts* = 0.005 s, *n* = 10). Figure 10 shows the simulation results with reference to the comparison number II (*ζKD* = 1, Table 2), in terms of external coordinates for the KD and KDHDc control laws.

**Figure 10.** Case study C, KD-KDHD comparison number II, external coordinates.

The effectiveness of the KDHDc stiffness compensation is also validated in this case by the fact that the external coordinates tend to the same steady-state values of the KD control law. The steady-state displacement along *xW* is slightly higher than the steady-state displacement *yW*, while the steady-state displacement along *zW* is much lower. This is coherent with the fact that the principal direction with lower stiffness is *xP*, which is rotated by 45◦ around the *z*-axis; therefore, considering only the *x-y* plane (Figure 11), a force along *xW* causes a larger displacement along *xP* with a positive direction, and a smaller displacement along *yP* with a negative direction. This results, in the frame W, in a slightly higher displacement along *xW* with respect to *yW*. In this case study, the ratio between the stiffness along *xP* and the ones along *yP* and *zP* is 1/20; imposing a lower ratio, the displacement would more precisely follow the desired compliance direction.

**Figure 11.** Case study C, *F*: external force; *x*ss: steady-state displacement (*x-y* plane).

#### **6. Conclusions**

In this paper, an extension of classical impedance control (KD), with compliance defined by the stiffness (**K**) and damping (**D**) matrices, has been proposed and tested by multibody simulation on a 3-PUU parallel robot. This extension is based on fractional calculus, and in particular on the half-order derivative (derivative of order 1/2), adding a half-derivative damping defined by the matrix **HD**.

This work was inspired by the research about the PDD1/2 control scheme for SISO systems, which is a PD with the addition of the half-derivative term, and extended it to a particular class of MIMO systems, impedance-controlled robots. In this work, a PKM was considered, but the approach can also be applied to serial robots by using Equations (12) to (14).

This work is only a starting point, and the effects of the introduction of the halfderivative damping must be further investigated. However, it is possible to outline the following conclusions:

• Even if applied to a nonlinear MIMO system, the introduction of the half-derivative term allows to tune the system behavior differently from a classical KD impedance control. The gains' set used in the case study A attained a reduction of maximum actuation force and total control effort, but the system can be tuned differently in order to improve other performance indexes, exploiting the previous works on PDD1/2 control.

• The addition of the half-derivative damping necessarily introduced, with the discretetime implementation by a finite-order digital filter, an alteration to the stiffness of the impedance control in steady state, as shown by Equation (5) and Figure 1. This hinders the capability of impedance control to regulate the contact force between the endeffector and the environment, which is the main scope of impedance control. To solve this issue, a stiffness-compensated KDHD impedance control algorithm (KDHDc) has been proposed (Equation (24)), and its effectiveness has been verified by simulation.

The saturation of the currents, and consequently of the actuation forces on the sliders, has not been taken into account to avoid the influence of an additional parameter, introducing a highly nonlinear effect and making the KD-KDHD comparison more complex. In any case, the simulations showed that the maximum actuation forces, with the considered control gains, were lower with the KDHD impedance control, so saturation cannot undermine the benefits of the proposed scheme.

In the future research, a systematic analysis of the influence of the control parameters of the KDHDc impedance control algorithm will be carried out in order to highlight the benefits of the proposed approach, also considering serial chains and robots with rotational mobility.

**Author Contributions:** L.B. conceived the control algorithm and designed the simulation campaign; D.B. developed the multibody model and performed simulations; P.F. supervised the scientific methodology; L.B. and P.F. prepared the manuscript. All authors have read and agreed to the published version of the manuscript.

**Funding:** This research received no external funding.

**Institutional Review Board Statement:** Not applicable.

**Informed Consent Statement:** Not applicable.

**Data Availability Statement:** Data is contained within the article. The simulation data presented in this study are available in figures and tables.

**Conflicts of Interest:** The authors declare no conflict of interest.

#### **References**

