Continuum Dexterous Manipulators (CDMs) have changed the paradigm of Minimally Invasive Surgical (MIS) procedures due to their added dexterity and compliancy compared to the common rigid surgical instruments . These robots have been studied in a variety of surgical procedures interacting with soft tissues e.g. Natural OrificeTrans-luminal Endoscopic Surgery ,, endoscopic submucosal dissection , and petrous apex lesions in the skull base . Our group is currently focusing on developing a surgical system for MIS treatment of hard tissues and particularly treatment of osteolytic lesions behind the hip acetabular implant using a custom designed CDM , and Fiber Bragg Grating (FBG) optical sensors , .
As shown in Fig. 1, in our proposed approach for MIS removing and treatment of osteolytic lesions behind the confined area of the implant, the CDM is first inserted through the screw holes of the well-fixed acetabular implant (with 8 mm diameter), and then controlled to the desired locations behind the implant. To this end, we utilize a hybrid redundant surgical system including a six Degrees-of-Freedom (DoF) robotic manipulator, as the positioning robot, and a one DoF CDM, as the dexterous guiding channel for different instruments (e.g. curettes, burs, and shaving tools). Of note, the used CDM is made of nitinol tubes (with outer diameter of 6 mm and inner diameter of 4 mm) with notches along its length, which constrains its bending to a single plane via two actuating tendons . Successful motion control of this redundant surgical system in real time considering the safety concerns demands a fast enough and reliable concurrent control framework.
Redundancy resolution of manipulators has been widely studied in the literature. For instance, Chirikjain and Burdick  and Sen et al.  used variational calculus for motion control of hyper-redundant manipulators and parallel robots, respectively. Further, other researchers implemented pseudoinverse control approach for dexterity optimization  and torque minimization . Kapoor et al.  has investigated redundancy resolution of a hybrid surgical system with a CDM using an optimization framework proposed by Funda et al. . In  and , we also incorporated a similar approach to control our proposed surgical system. In another study, Bajo et al.  used the pseudoinverse-based method to control their surgical subsystem consisting of planar parallel mechanisms and continuum snake-like arms. These methods mostly use optimization-based approaches to solve a constraint-free or constrained redundancy resolution problem. However, to the best of our knowledge, this problem has not yet been studied from a convex optimization perspective. Therefore, in this paper, due to the convex nature of our problem formulation (i.e. constrained -regularized quadratic minimization), we are particularly interested in convex optimization approaches, which are fast enough and easy to implement for online control of our hybrid redundant system. To this end, we choose an Alternating Direction Method of Multipliers (ADMM) algorithm , which has been widely used in the literature due to its less complicated computations that can be solved in a parallel and distributed processing.
The ADMM algorithm is related to the augmented Lagrangian method (ALM) and was developed by Glowinski and Marrocco and Gabay and Mercier in mid-1970s . It combines the superior convergence properties of the method of multipliers and the decomposability of dual ascent . Motivated by the various applications of the ADMM algorithm particularly in solving real-time MPC control problems , in this paper, we investigate the potential implementation of this algorithm for redundancy resolution of robotic manipulators. To this end, we study the convergence conditions of this algorithm in the case of motion control of redundant hybrid robots subject to linear equality and inequality constraints. Further, as a case study, we evaluate performance of this algorithm in motion control of our surgical system via simulation.
Ii constrained motion control of redundant manipulators using the DLM formulation
The basic equation defining forward kinematics of robotic manipulators, which relates the joint space velocities to the end-effector velocity , is written as follows:
where is the Jacobian matrix and is a function of manipulator joint angles. If we can calculate the Jacobian matrix in each time instance, then using (1) the changes in the end-effector position
for an infinitesimal time period can be estimated based on joint angles changes:
Solving this equation is dependent on the matrix and generally does not provide unique solutions. For redundant manipulators, infinite solutions exist. Several methods, such as the Jacobian transpose method, pseudo inverse method, and damped least square method (DLM) have been proposed in literature for solving (2). The Jacobian transpose and pseudo inverse methods encounter problems in the configurations where becomes either rank-deficient or ill-conditioned. However, DLM does not encounter the singularity problems and can provide a numerically stable procedure for calculating . In this approach, instead of solving for satisfying (2), the following convex least-square optimization problem is solved:
where and is a positive non-zero damping constant. This optimization problem tries to find a set of that minimizes the error between the desired task-space displacement and the generated task-space movement by the robot (first term), considering the feasible minimum joint-space displacement (second term).
In the case of an unconstrained environment, (3) has closed form solution for redundancy resolution problems. However, most of the real-world problems impose constraints on the movement of the robot. Therefore, we modify (3) to formulate the constrained redundancy resolution problem under linear inequality constraints as the following:
together with vectordefine linear inequality constraints. Of note, here matrix is dependent on the Jacobian of the robotic manipulator in each time instant.
The defined constrained DLM problem in (4) is indeed a constrained -regularized quadratic minimization problem. While this problem can be solved using various approaches of convex and non-convex optimization techniques, Ghadimi et al.  have shown optimally tuned ADMM method can significantly outperform existing alternatives in the literature.
Iii the ADMM algorithm 
The ADMM algorithm solves problems in the following form:
where and are convex functions, , 〖, , , and . To solve this problem, we need to first form the Augmented Lagrangian (AL):
where is a vector of Lagrange multipliers and is a constant called the AL penalty parameter. In the ADMM algorithm, a new iteration , is generated given the current iteration by first minimizing the AL with respect to , then with respect to , and finally updating the multiplier :
In this paper, we will use the scaled form of the ADMM algorithm by combining the linear and quadratic terms in the AL, and scaling the dual variable (Algorithm 1).
Convergence of the ADMM algorithm is usually defined by the primal and dual residuals:
Using these residuals, a termination criterion is defined as:
where and are positive feasibility tolerances for the primal and dual feasibility conditions. These tolerances can be defined as follows:
where and are absolute and relative tolerances, and and are coefficients demonstrating norms in and , respectively.
Iv ADMM algorithm for the constrained DLM problem of redundant robots
To apply the ADMM algorithm to the defined constrained DLM problem (4), we first convert it to the standard ADMM form (5). To this end, we use a slack vector and put an infinite penalty on its negative components. Considering this, we can rewrite (4) as follows:
where , , , and . together with vector define linear inequality constraints. is the indicator function of the positive orthant defined as for and otherwise. The associated augmented Lagrangian of (11) in the scaled form would be:
which leads to the following scaled ADMM iterations:
where is the identity matrix.
Iv-a Proof of Convergence
Inspired by the approach proposed in , in this section, we show that the convergence of the constrained DLM problem (11) under some conditions is independent of the choice of , which makes this algorithm a powerful method in solving constraint redundancy resolution problems of robots. To prove the convergence of (11), a vector of indicator variables is defined to check whether the inequality constraint in (11) is active or not. In this definition, means the inequality constraint is active (i.e. in IV) or the slack variable equals zero at the current iteration. Considering this, we introduce the following auxiliary variables:
From the definition of and , we have and Now, we can rewrite - and -updates of (IV) as:
where updates are derived from the definition of and . In (IV-A), we use the definition of and substitute in to obtain:
Let’s define matrix as
So, using (17) we can obtain
We use (19) to prove the convergence of the ADMM algorithm and find the optimal penalty parameter that results in the fastest convergence rate.
proof: We start from (19) and take the norm of both sides
Since is a diagonal matrix with elements and is a positive vector, we can write:
Therefore, we can rewrite (20) as
By the matrix inversion lemma, we have
In (23), calculation of depends on the invertibility of matrix and . For all , adding to the positive semidefinite matrix makes
a positive definite matrix. Therefore, it is invertible and all its eigenvalues are real and positive. Sinceis a positive definite matrix, is a positive semi definite matrix for any choice of matrix . Further, with similar analogy, is a positive definite matrix and invertible for all .
We calculate eigenvalues of (i.e. ) as a function of eigenvalues of (i.e. ) :
In this equation is a positive semi-definite matrix which, based on (24), implies . Therefore, if and
Case 1: is invertible or is a full row-rank matrix then , which proves the linear convergence of (22) to zero.
Considering the definition of in (18), we multiply both sides by to obtain:
Given in Case 2, a vector 〖 exists in the null space of , which implies . This means either vector is in the null space of or 〖. Considering assumptions in Case 2, is a full column rank matrix implying vector is not in the null space of matrix . In other words, 〖 are the stationary points of the algorithm (IV). Therefore, algorithm (IV) converges linearly to zero and the case of zero eigenvalues for matrix can be neglected.
Iv-B Optimal Convergence Parameters
In the previous section, we proved that for all , the ADMM iterations in (IV) linearly converge to zero. To obtain the fastest convergence rate, we use the following optimal AL penalty parameter introduced by Ghadimi et al.  that results in the fastest convergence rate assuming all the conditions of Theorem 1 hold and the constraint matrix , in the redundancy resolution problem (11) and the corresponding ADMM iteration (IV), is either full row-rank or invertible:
where and are the minimum and maximum eigenvalues of , respectively. Furthermore, when rows of are linearly dependent, can still reduce the convergence time if is replaced by the smallest nonzero eigenvalue of .
V case study: a redundant surgical system for treatment of osteoyisis
We are developing an MIS robotic system to remove and treat osteolytic lesions behind a well-fixed implant (Fig. 1). This surgical workstation consists of a six DoF robotic arm (UR5, Universal Robotics- Denmark) integrated with a one DoF cable-driven continuum dexterous manipulator (CDM), which are concurrently controlled to position the CDM tip behind the acetabular implant ,. Position of the CDM tip behind the acetabular implant is controlled using concurrent control of the coupled CDM-robotic manipulator. In this paper, we use this redundant system to evaluate performance of the proposed ADMM algorithm. Furthermore, we define appropriate linear constraints to satisfy both operational and safety objectives, which are necessary during robot-assisted treatment of osteolysis. To this end, considering the DLM formulation introduced in (11), kinematics of the redundant robot as well as the appropriate linear constraints will completely be defined in the following sections.
V-a Kinematics of the integrated system
V-A1 Kinematics of the UR5
The forward kinematics of a manipulator is obtained by a mapping , which describes the end-effector configuration as a function of the robot joint variables and can be written using the product of exponentials formula:
where is the axis of rotation, is a point on the axis and is the twist corresponding to the joint axis in the reference configuration . Fig. 2 demonstrates the twist axes for all six joints of UR5.
The velocity of the base of the CDM relative to the base of UR5 in the Cartesian space is related to the UR5 joint velocities via its Jacobian matrix as follows:
where the manipulator Jacobian has the following form:
where is the adjoint matrix, which depends on the UR5 configuration .
V-A2 Kinematics of the CDM 
The 35 mm CDM is made of two nested nitinol tubes with notches on its surface and has outer diameter of 6 mm and inner diameter of 4 mm. Considering these notches, two actuating cables on the sides of the CDM provide a planar bend for the continuum manipulator. The inner space of the CDM is used as the tool channel for passing different types of instruments used for the debriding and treatment process. Two DC motors (RE16, Maxon Motor Inc.) with spindle drives (GP16, Maxon Motor, Inc.) actuate the CDM via the actuation cables (Fig. 1). In , we have derived experimental kinematics of the CDM in the free bending motion. A series of experimental tests have been performed to investigate the relation between the actuating cable length and tip position of the CDM . Using this experimental function, the velocity of the CDM tip relative to its base can be determined by the following partial differentiation:
V-A3 Combined Kinematics
V-B1 The Remote Center of Motion (RCM) constraint
For robotic manipulators without a mechanical RCM (e.g., the UR5), a virtual RCM can be applied . This constraint ensures movements of the CDM base are confined in a virtual cylinder around the screw hole axis with a radius of (Fig.3). Further, it prevents any collision between the CDM base and the screw hole edges while the integrated system pivots around the center of the hole with the radius . We can define this virtual constraint as:
where vectors are vectors normal to each side of the polygon which approximates the cylinder and defines the degree of approximation of a circle by a polygon. is the incremental Cartesian movement of the closest point on the CDM base axis to the RCM point and is the Jacobian matrix of the closest point in the UR5 base frame.
V-B2 Joints Limit Constraints
To ensure a safe range for the incremental joint movements of the UR5, we defined:
where is a identity matrix and and refer to the lower and upper incremental joint movements of the UR5, respectively. Furthermore, considering (28) as the experimental model of the CDM and a single-cable bend, the following constraints are applied:
This means that the total change in the cable length of the CDM (
), at each moment should not exceed 9 mm and clearly it should not be less than 0 mm. We can stack these joints limit constraints and write:
where , .
V-B3 Combining Constraints as Matrix A and Vector b
V-C Control Algorithm
Considering the combined Jacobian in (V-A3) and the stacked constraints in (36), Algorithm (2) demonstrates the concurrent control method of the integrated UR5-CDM system. The goal of this algorithm is to use the solution of the proposed constrained DLM formulation in (11) and follow a pre-defined trajectory.
Vi Results and Discussion
The performance of the proposed ADMM algorithm in solving the constrained concurrent control of the described surgical system is evaluated with simulation. To this end, we used a representative osteolytic lesion boundary reconstructed based on data provided by collaborating surgeons . This complex 3D-path includes 36 waypoints and can be confined in a cubic space. For this study, we assumed that the CDM already passed through one of the screw holes of the acetabular implant and the CDM was completely inside the cavity behind the implant. In addition, considering (V-A1), we assumed that no external force was acting on the CDM body during the procedure.
All simulations started with identical initial poses of the UR5 and CDM. The goal of these simulations was to track the path while satisfying all the constraints described in Section V-B on joint limits. We approximated the RCM circle by a polygon with sides as described in (14). Hence, dimension of the matrix in (31) is . We used (IV) to solve the problem with the ADMM algorithm and (10) as the termination criteria by setting 〖 and 〖 with and . All simulations were performed in MATLAB (The MathWorks Inc, Natick, MA), on a Core i5, 2.5 GHz processor and 4 GB of RAM computer running Windows 7.
Vi-a Trajectory Tracking with Constant Parameters
Fig. 4 demonstrates the resulted configurations of the integrated system along the considered path when applying the ADMM algorithm. During this simulation the following parameters were used for evaluations: , the maximum allowable tip error of the CDM defined as the maximum Euclidean distance between the desired position and the achieved CDM tip position, and the maximum RCM constraint error , which defines the pivoting freedom of the CDM in the screw holes of the implant. Further, we used (26) to calculate optimal AL penalty parameter in each iteration. As shown, for each point, the integrated system could satisfy the constraints as well as the RCME and MTE criteria. The ADMM algorithm accomplished the task in 5.62 seconds ( second/point). It should be noted that in , we solved the similar problem using an active-set algorithm with and achieved error with a runtime of more than 300 seconds. The use of the ADMM, therefore, significantly improved the runtime and the rate of convergence.
Vi-B Effect of Parameter on the Convergence Rate of the ADMM Algorithm
As we proved in Theorem 1, convergence of the ADMM algorithm under the mentioned assumptions is independent of parameter . However, the convergence rate of the algorithm is directly affected by this parameter. To investigate this result, we ran the algorithm 50 times with different values of while keeping the other parameters constant as: . We considered one target point and evaluated the convergence rate of the algorithm- defined as the maximum number of iterations- to reach this point from an initial configuration with a defined MTE. Furthermore, we used the optimal penalty parameter defined in (26) and evaluated the convergence rate of the algorithm. Fig. 5 presents the results of these experiments and shows the dependency of the convergence rate on the choice of the penalty parameter. As we proved in Theorem 1, for all the 50 considered , the algorithm converges; however, values larger than 0.6 and less than 0.3 significantly increase the convergence rate. Furthermore, using (26), we calculated the optimal penalty parameter and compared it with the ideal value of obtained from the simulations (Fig. 5). As we discussed in Section IV-B, for a tall matrix A, does not return the optimal AL penalty parameter, however, it still provides a sufficiently close estimation of the optimal parameter and results in a fast convergence rate (37 iterations) compared to the ideal case (28 iterations). Fig. 6 and Fig. 7 compare the primal and dual residuals and objective function values for four different cases of , respectively. As we expected, for the case of , number of iterations is less than other considered penalty parameters.
Vi-C Sensitivity to the MTE and RCME
The MTE defines the accuracy of the tracking as the maximum allowable Euclidean distance between the CDM tip and the desired point. On the other hand, the RCME is a measure assigning maximum allowable violation from the RCM constraint. Clearly, we are interested in the smallest values for these two parameters. As such, we considered various values for the RCME and the MTE and carried out simulations with a constant . Table I summarizes the results of these simulations. In all simulations, the ADMM successfully tracked the path and reached all 36 waypoints without violating considered MTEs. The maximum overall runtime using ADMM algorithm for tracking the considered trajectory consisting of 36 waypoints was 21.52 second ( second/point) and the minimum runtime was 4.27 second ( 0.12 second/point). The average overall runtime for all 10 ADMM simulations in MATLAB, was 13.3 second and the runtime for each waypoint was about 0.37 second. Furthermore, the ADMM demonstrated acceptable sensitivity and robustness to the reduction of the tracking error (up to MTE= 0.1 mm) as well as the maximum violation of the RCM constraint (up to RCME= 0.5 mm).
|MTE (mm)||RCME (mm)||run time (s)|
Vii Concluding Remarks
In this paper, we formulated the constrained redundancy resolution problem of manipulators in the context of a constrained -regularized quadratic minimization problem. We solved this problem using a generic technique of convex optimization called ADMM algorithm. Further, we proved global convergence of the algorithm at linear rate and introduced expressions and assumptions for the involved parameters that results in a fast convergence rate. We validated the analytical results using simulation on a novel redundant surgical workstation. In conclusion, we demonstrated the global convergence and robustness of the optimally tuned ADMM algorithm independent of the choice of parameters involved in the constrained DLM formulation. Future efforts will focus on validating the proposed workstation and method on other applications such as treatment of articular cartilage injury ,  and osteonecrosis of femoral head . Further, we plan to compare the performance of the proposed optimally tuned ADMM with other redundancy resolution methods in the literature.
-  N. Simaan, K. Xu, W. Wei, A. Kapoor, P. Kazanzides, R. Taylor, and P. Flint, “Design and integration of a telerobotic system for minimally invasive surgery of the throat,” The International journal of robotics research, vol. 28, no. 9, pp. 1134–1153, 2009.
-  A. Kapoor, N. Simaan, and R. H. Taylor, “Suturing in confined spaces: Constrained motion control of a hybrid 8-dof robot,” in Advanced Robotics, 2005. ICAR’05. Proceedings., 12th International Conference on. IEEE, 2005, pp. 452–459.
-  N. Patel, C. A. Seneci, J. Shang, K. Leibrandt, G.-Z. Yang, A. Darzi, and J. Teare, “Evaluation of a novel flexible snake robot for endoluminal surgery,” Surgical endoscopy, vol. 29, no. 11, pp. 3349–3355, 2015.
-  S. Coemert, F. Alambeigi, A. Deguet, J. Carey, M. Armand, T. Lueth, and R. Taylor, “Integration of a snake-like dexterous manipulator for head and neck surgery with the da vinci research kit,” in Proc. Hamlyn Symp. Med. Robot., 2016, pp. 58–59.
-  F. Alambeigi, S. Sefati, R. J. Murphy, I. Iordachita, and M. Armand, “Design and characterization of a debriding tool in robot-assisted treatment of osteolysis,” in Robotics and Automation (ICRA), 2016 IEEE International Conference on. IEEE, 2016, pp. 5664–5669.
-  F. Alambeigi, Y. Wang, R. J. Murphy, I. Iordachita, and M. Armand, “Toward robot-assisted hard osteolytic lesion treatment using a continuum manipulator,” in Engineering in Medicine and Biology Society (EMBC), 2016 IEEE 38th Annual International Conference of the. IEEE, 2016, pp. 5103–5106.
-  S. Sefati, F. Alambeigi, I. Iordachita, M. Armand, and R. J. Murphy, “Fbg-based large deflection shape sensing of a continuum manipulator: Manufacturing optimization,” in SENSORS, 2016 IEEE. IEEE, 2016, pp. 1–3.
-  S. Sefati, M. Pozin, F. Alambeigi, I. Iordachita, R. H. Taylor, and M. Armand, “A highly sensitive fiber bragg grating shape sensor for continuum manipulators with large deflections,” in 2017 IEEE SENSORS, Oct 2017, pp. 1–3.
-  F. Alambeigi, R. J. Murphy, E. Basafa, R. H. Taylor, and M. Armand, “Control of the coupled motion of a 6 dof robotic arm and a continuum manipulator for the treatment of pelvis osteolysis,” in Engineering in Medicine and Biology Society (EMBC), 2014 36th Annual International Conference of the IEEE. IEEE, 2014, pp. 6521–6525.
-  G. S. Chirikjian and J. W. Burdick, “The kinematics of hyper-redundant robot locomotion,” IEEE transactions on robotics and automation, vol. 11, no. 6, pp. 781–793, 1995.
-  S. Sen, B. Dasgupta, and A. K. Mallik, “Variational approach for singularity-free path-planning of parallel manipulators,” Mechanism and Machine Theory, vol. 38, no. 11, pp. 1165–1183, 2003.
-  Y. Nakamura and H. Hanafusa, “Optimal redundancy control of robot manipulators,” The International Journal of Robotics Research, vol. 6, no. 1, pp. 32–42, 1987.
-  J. Hollerbach and K. Suh, “Redundancy resolution of manipulators through torque optimization,” IEEE Journal on Robotics and Automation, vol. 3, no. 4, pp. 308–316, 1987.
-  J. Funda, R. H. Taylor, K. Gruben, and D. LaRose, “Optimal motion control for teleoperated surgical robots,” in Optical Tools for Manufacturing and Advanced Automation. International Society for Optics and Photonics, 1993, pp. 211–222.
-  P. Wilkening, F. Alambeigi, R. J. Murphy, R. H. Taylor, and M. Armand, “Development and experimental evaluation of concurrent control of a robotic arm and continuum manipulator for osteolytic lesion treatment,” IEEE Robotics and Automation Letters, vol. 2, no. 3, pp. 1625–1631, 2017.
-  A. Bajo, R. E. Goldman, L. Wang, D. Fowler, and N. Simaan, “Integration and preliminary evaluation of an insertable robotic effectors platform for single port access surgery,” in Robotics and Automation (ICRA), 2012 IEEE International Conference on. IEEE, 2012, pp. 3381–3387.
S. Boyd, N. Parikh, E. Chu, B. Peleato, and J. Eckstein, “Distributed
optimization and statistical learning via the alternating direction method of
Foundations and Trends® in Machine Learning, vol. 3, no. 1, pp. 1–122, 2011.
-  D. P. Bertsekas and A. Scientific, Convex optimization algorithms. Athena Scientific Belmont, 2015.
-  M. Khadem, C. Rossa, R. S. Sloboda, N. Usmani, and M. Tavakoli, “Ultrasound-guided model predictive control of needle steering in biological tissue,” Journal of Medical Robotics Research, vol. 1, no. 01, p. 1640007, 2016.
-  E. Ghadimi, A. Teixeira, I. Shames, and M. Johansson, “Optimal parameter selection for the alternating direction method of multipliers (admm): quadratic problems,” IEEE Transactions on Automatic Control, vol. 60, no. 3, pp. 644–658, 2015.
-  R. A. Horn and C. R. Johnson, Matrix analysis. Cambridge university press, 2012.
-  R. M. Murray, Z. Li, S. S. Sastry, and S. S. Sastry, A mathematical introduction to robotic manipulation. CRC press, 1994.
-  R. Behrou, H. Foroughi, and F. Haghpanah, “Numerical study of temperature effects on the poro-viscoelastic behavior of articular cartilage,” Journal of the mechanical behavior of biomedical materials, vol. 78, pp. 214–223, 2018.
-  ——, “A novel study of temperature effects on the viscoelastic behavior of articular cartilage,” arXiv preprint arXiv:1710.05467, 2017.
-  F. Alambeigi, Y. Wang, S. Sefati, C. Gao, R. J. Murphy, I. Iordachita, R. H. Taylor, H. Khanuja, and M. Armand, “A curved-drilling approach in core decompression of the femoral head osteonecrosis using a continuum manipulator,” IEEE Robotics and Automation Letters, vol. 2, no. 3, pp. 1480–1487, 2017.