# The Design of Compact Robotic-Assisted Needle Position System with MPC-Based Remote Control

Authors: Jing Guo; Yi Liu; Jin Wang; Chao Zeng; Jie Huang; Chao Liu
Journal: Complexity
 (2020)

---

## Abstract

This article introduces the design and control performance of a lightweight, flexible, 4-degree-of-freedom (DOF) parallel robot for percutaneous biopsy guided by computed tomography (CT). At present, the CT guidance method allows surgeons to quickly locate the lesion area; however, it is necessary to manually adjust the position of the puncture needle for insertion. In this paper, a three-dimensional assisted method is used to infer the control input required to reach the target point through the kinematic model of the robot. A Kalman filter is designed to estimate model parameters and obtain a more accurate model. To further improve the control performance of the robot system, a model-based control method—the model predictive control (MPC) controller—is used to increase the accuracy of the needle position in the developed robot system. In this way, medical efficiency is improved while reducing the burden on the surgeon.


---

## Body

## 1. Introduction

The surgeries have been advanced in the past decades thanks to novel surgical techniques [1], such as laparoscopy surgeries, in which a surgeon can operate a surgery through tiny holes on a human body for removing pathological tissue. However, the hand-hold tools still make the operation hard to be performed due to limited ergonomics and accuracy. Meanwhile, the surgeons may feel fatigued when operating for long time during the surgery. In order to improve the quality of surgical operation, the robot assistant technique has been introduced into the operation room which has eased the burden of the surgeon during the surgery [2–4]. Nowadays, the robotic systems have been developed and used for various surgeries such as biopsy, brachytherapy, and tumor ablation [5–7].Before performing a minimally invasive surgical (MIS) task, the coordinates of the robot are usually registered to the medical image coordinate system, so that the robot is controlled by analysis of the lesion’s position for further operations. According to the characteristics of MIS, the pose of the robot end-effector before needle insertion during surgery becomes very important. The needle positioning system of the surgical assistant robot can be independent of the insertion action [8–10]. The surgeon can guide the robot’s needle to the insertion position of the lesion on the basis of the medical image through image guidance [11–16] and then manually insert the needle into the lesion area. Medical imaging, acting as a real-time feedback tool for the needle pose, is crucial in the process of needle positioning and control. In current needle-guided systems, CT [11–13, 17, 18] is often used. Today’s commercially available minimally invasive robotic systems (MIRS) maintain and expand the flexibility of the surgeon’s hand [19–22]. Navarro-Alarcon et al. [12] developed a 3-DOF needle driver for biopsy. The positioning and insertion of the needle are achieved through three interfaces, the first two of which are aligned to the target, and the third is inserted. Koseki et al. [23] established a cooperative manipulation structure which uses the optical linear encoder to measure the needle position. However, manual remote operation control requires multiple scans using CT to obtain accurate targets. The robot-assisted positioning approach can locally record the workspace and medical images of the robot system to reduce the risk of cancer induced by repeated CT scans [24]. This approach poses challenges to the compatibility and size of the robot system.In the development of a surgical robot, another major problem is the network system in the remote operating system, which is caused by long distance or wireless link [23–27]. Sampling and delay are involved in this problem, which is a challenge for remote controlling of the robot. The solution to the delay problem in remote is discussed in [28–31]. However, the local optimizer on the slave side of the robot will also cause a delay [32, 33]. This part of the delay will also affect the remote operating system following the feedback signal. Dong et al. [34] modified standard functions to improve the response speed. Norizuki and Uchimura [35] embedded the MPC controller into the remote operating system to reduce the impact of the optimizer’s delay in the system.In order to improve the quality of the surgery, the accuracy of the needle pose becomes crucial. The needle pose is affected by the robot control system and medical images. However, factors such as friction in the design of the robot structure and noise in the feedback of medical images are inevitable, which makes it difficult to accurately control the needle positioning [36, 37]. Thus, the closed-loop control method becomes a good solution [38, 39]. In this paper, firstly, as shown in Figure 1, a novel light-weighted puncture robotic system is provided. A surgeon controls the medical robot through the master in the remote system. The medical robot can provide feedback of the end-effector pose and force to the surgeon. The size of the designed robot is 16cm high, 37.6cm long, and 37.6cm wide. In addition, the robot is controlled by four motors, which leads to the coupling of robot control. Due to the characteristics of the robot structure, specific constraints are proposed to decouple the kinematics. Finally, we develop a related needle positioning system on this robotic platform, which uses an MPC strategy to achieve the robotic local closed-loop control of the needle tip position. This approach not only solves the problem of the time delay (TD) of the needle positioning system in local, but also reduces the cost and time of personnel training required for the operation of the robotic needle positioning system.Figure 1
Surgical assistant robot remote operating system.The structure of the paper is organized as follows: in Section2, the overall robotic system design is introduced, including the derivation of constrained kinematic decoupling approach. Section 3 introduces model parameter estimation. Section 4 describes MPC control method. In Section 5, the experiments and the analysis of the experimental results are introduced. We conclude this article in Section 6.In our previous work [40], we have designed a remote system which contains a master and a slave robot (see Figure 1). A surgeon can control the slave robot through the master side, and during a task, the desired position of the end-effector can be marked and recorded. The end-effector position of the slave robot is updated via medical imaging or other sensors, and such position feedback is sent to master. There are many ways to solve the remote communication scheme between the master and slave robots [28–31]. However, when the slave robot uses the optimized control method to improve the control effect, a delay will occur in the slave robot, which will affect the use of the remote operating system. Thus, the local controller design of the slave robot needs to be considered.Because the robotic control system is a multi-input multioutput (MIMO) system, the controller selected for the slave robot should be able to adapt to the MIMO system. The proposed slave robot control system can be seen in Figure2(a). The surgeon uses CT to mark the target lesion area or trajectory in the robot workspace. The surgical robot obtains the control trajectory which serves as the reference input for the MPC controller through kinematic model. The MPC controller optimizes the current control input to the surgical robot according to the reference trajectory and the reference state of the surgical robot. Furthermore, in order to improve the control performance, the state-space model of the robot required by the MPC controller should be accurate. In this paper, according to the structure of the surgical robot, the kinematics model is divided into a linear part and a nonlinear part. The linear part uses the model parameter estimator to identify the model parameters, and the nonlinear part is linearized using the Taylor formula. As shown in Figure 2(b), the model parameter estimator of the linear part is composed of an order judger and a parameter estimator. The order judger is used to judge the order of the linear system. The Kalman filter is selected as the parameter estimator to estimate the parameters of the linear system.Figure 2
(a) The overall control framework of the surgical robot positioning system. (b) A schematic of the model parameter estimator of the linear part of the kinematics model.


## 2. Kinematic Modeling

In this section, the modeling process of surgical robot kinematics will be described. The robotic kinematic is analyzed for the establishment of model parameter estimator and MPC controller. A suitable kinematics model is constructed by analyzing the mechanical structure of the parallel puncture needle robot [40]. Figure 3 shows the three-dimensional mechanical structure and sectional structure of the surgical robot. The blocks, i.e., block-a and block-b, can be moved by controlling the angle of four motors. When the positions of the two blocks change, the position and insertion direction of the puncture needle will change accordingly. Among this structure, the vertical distance d between the two blocks and the length h of the puncture needle below the block-b are both constant.Figure 3
The three-dimensional mechanical structure and sectional structure of the surgical robot. It uses two parallel blocks to control the insertion direction and position of the same puncture needle.In order to obtain a suitable kinematics model, the coordinate relationship as shown in Figure4 is established. The fixed world coordinate system is at the initial center position of block-b. Twa is the coordinate system of block-a, which is used to describe the moving distance of block-a. Similarly, the value of the coordinate system Twb is the moving distance of block-b. Coordinate system Twe indicates the position of the tip of the puncture needle.Figure 4
The surgical robot’s coordinate relationship of the kinematic model. Define the world coordinate system and the coordinate system of each joint to describe the kinematics model.


### 2.1. Forward Kinematic

The purpose of forward kinematics is to obtain the position of the tip of the puncture needlexe, ye, and ze, and the forward kinematics of the surgical robot can be obtained using geometric theorem. As shown in Figure 4, firstly, the rotation of the motor will drive the block. The moving distance of the two blocks xa, xb, ya, and yb will change the angle of the puncture needle θ.(1)xa=ρβa1,(2)θ=arctanya−yb2+xa−xb2d,(3)α=arctanxa−xbya−yb,where ρ is the pitch of the screw connected to the block; β is the rotation angle of the motor, and the relationship between the rotation angle of other motors and its block coordinates is similar; α is the angle generated by two blocks on the X-Y plane; next, for coordinate system Twb, the change in the position of the tip of the puncture needle Δx, Δy, and Δz can be described by θ, α, and the constant length of the needle h:(4)Δx=h⋅sinθ⋅sinα,(5)Δy=h⋅sinθ⋅cosα,(6)Δz=h⋅cosθ.Finally, the position of the puncture needle’s tipxe, ye, and ze relative to the fixed world coordinate system can be obtained as follows:(7)xe=Δx+xb,(8)ye=Δy+yb,(9)ze=Δz.


### 2.2. Inverse Kinematic

The surgical robot is a parallel structure; hence, there is a coupling relationship between the control inputs. To address this problem, constraints are first defined to decouple the kinematic model. From the point of view of the positioning range of the puncture needle, it is better to have a larger range that the puncture needle can reach in a limited space. Thus, the first constraint is that the two blocks should move in opposite directions. On the other hand, from the view of the lesion area marked by surgeon, it can be seen that the position of the tip of the puncture needle in theX-Y plane is of more significance. In order to obtain a feasible solution for the motor control input, the second constraint is defined. That is, when block-a moves to the limit position and still cannot reach the lesion area, block-b is then moved. The inverse kinematics model is constructed using geometric theorem based on the constraints:(10)xb,yb=0,0s.t.−η≤xt,yt≤η,and(11)xa,ya=−2sgnxt,−2sgnyts.t.xt,yt≤−η,where η is the largest distance that the tip of the puncture needle can reach when only block-a is in motion; xt and yt are the lesion’s target point coordinates marked by the surgeon. However, no complete feasible solution can be obtained yet. Then, the solution of the coordinates of block-a within the limit η is discussed:(12)xaya=−d⋅sgnxttanarcsinθsinarctanxtyt−d⋅sgnyttanarcsinθcosarctanxtyt,where θ is the angle between the block and puncture needle:(13)θ=arccos−zth.So far, a feasible solution within the limitη from equations (10) and (12) is obtained:(14)xayaxbyb=−d⋅sgnxttanarcsinθsinarctanxtyt−d⋅sgnyttanarcsinθcosarctanxtyt00s.t.−η≤xt,xt≤η.Finally, the feasible solution in the other case will be analyzed to obtain the complete feasible solution. When the lesion area exceeds the limitη, the coordinate values of block-a are fixed. Therefore, according to the angle θ between the block and the puncture needle, the relationship between xb and xb can be acquired:(15)xa−xb2+ya−yb2=d⋅tanθ,(16)xa−xbya−yb=xt−xbyt−yb.The solution of coordinate value ofblock-b can be acquired by equations (15) and (16):(17)xbyb=−EB−EB2−4⋅EA⋅EC2⋅EAE−2⋅xa−xtya−yt,where E, EA, EB, and EC are known parameters as follows:(18)E=h2−zt2+xa2−yt2+ya22,EA=xa−xt2ya−yt2+1,EB=2dya−ytxa−xt−2ya−yt2xt−2Exa−xtya−yt2,EC=ya−yt2xt2+yt2−h2+zt2−2dEya−ytya−yt2.Then, the feasible solution that exceeds the limitη is obtained by equations (11) and (17):(19)xayaxbyb=−2sgnxt−2sgnyt−EB−EB2−4⋅EA⋅EC2⋅EAE−2⋅xa−xtya−yts.t.xt,yt≤−η.Now, the complete solution of the kinematic model of the surgical robot has been obtained by equations (14) and (19). The corresponding motor control angle can be obtained through the lesion area marked by the surgeon.


## 2.1. Forward Kinematic

The purpose of forward kinematics is to obtain the position of the tip of the puncture needlexe, ye, and ze, and the forward kinematics of the surgical robot can be obtained using geometric theorem. As shown in Figure 4, firstly, the rotation of the motor will drive the block. The moving distance of the two blocks xa, xb, ya, and yb will change the angle of the puncture needle θ.(1)xa=ρβa1,(2)θ=arctanya−yb2+xa−xb2d,(3)α=arctanxa−xbya−yb,where ρ is the pitch of the screw connected to the block; β is the rotation angle of the motor, and the relationship between the rotation angle of other motors and its block coordinates is similar; α is the angle generated by two blocks on the X-Y plane; next, for coordinate system Twb, the change in the position of the tip of the puncture needle Δx, Δy, and Δz can be described by θ, α, and the constant length of the needle h:(4)Δx=h⋅sinθ⋅sinα,(5)Δy=h⋅sinθ⋅cosα,(6)Δz=h⋅cosθ.Finally, the position of the puncture needle’s tipxe, ye, and ze relative to the fixed world coordinate system can be obtained as follows:(7)xe=Δx+xb,(8)ye=Δy+yb,(9)ze=Δz.


## 2.2. Inverse Kinematic

The surgical robot is a parallel structure; hence, there is a coupling relationship between the control inputs. To address this problem, constraints are first defined to decouple the kinematic model. From the point of view of the positioning range of the puncture needle, it is better to have a larger range that the puncture needle can reach in a limited space. Thus, the first constraint is that the two blocks should move in opposite directions. On the other hand, from the view of the lesion area marked by surgeon, it can be seen that the position of the tip of the puncture needle in theX-Y plane is of more significance. In order to obtain a feasible solution for the motor control input, the second constraint is defined. That is, when block-a moves to the limit position and still cannot reach the lesion area, block-b is then moved. The inverse kinematics model is constructed using geometric theorem based on the constraints:(10)xb,yb=0,0s.t.−η≤xt,yt≤η,and(11)xa,ya=−2sgnxt,−2sgnyts.t.xt,yt≤−η,where η is the largest distance that the tip of the puncture needle can reach when only block-a is in motion; xt and yt are the lesion’s target point coordinates marked by the surgeon. However, no complete feasible solution can be obtained yet. Then, the solution of the coordinates of block-a within the limit η is discussed:(12)xaya=−d⋅sgnxttanarcsinθsinarctanxtyt−d⋅sgnyttanarcsinθcosarctanxtyt,where θ is the angle between the block and puncture needle:(13)θ=arccos−zth.So far, a feasible solution within the limitη from equations (10) and (12) is obtained:(14)xayaxbyb=−d⋅sgnxttanarcsinθsinarctanxtyt−d⋅sgnyttanarcsinθcosarctanxtyt00s.t.−η≤xt,xt≤η.Finally, the feasible solution in the other case will be analyzed to obtain the complete feasible solution. When the lesion area exceeds the limitη, the coordinate values of block-a are fixed. Therefore, according to the angle θ between the block and the puncture needle, the relationship between xb and xb can be acquired:(15)xa−xb2+ya−yb2=d⋅tanθ,(16)xa−xbya−yb=xt−xbyt−yb.The solution of coordinate value ofblock-b can be acquired by equations (15) and (16):(17)xbyb=−EB−EB2−4⋅EA⋅EC2⋅EAE−2⋅xa−xtya−yt,where E, EA, EB, and EC are known parameters as follows:(18)E=h2−zt2+xa2−yt2+ya22,EA=xa−xt2ya−yt2+1,EB=2dya−ytxa−xt−2ya−yt2xt−2Exa−xtya−yt2,EC=ya−yt2xt2+yt2−h2+zt2−2dEya−ytya−yt2.Then, the feasible solution that exceeds the limitη is obtained by equations (11) and (17):(19)xayaxbyb=−2sgnxt−2sgnyt−EB−EB2−4⋅EA⋅EC2⋅EAE−2⋅xa−xtya−yts.t.xt,yt≤−η.Now, the complete solution of the kinematic model of the surgical robot has been obtained by equations (14) and (19). The corresponding motor control angle can be obtained through the lesion area marked by the surgeon.


## 3. Model Parameter Estimation

After the desired control input is obtained, it cannot, in fact, directly act on the robotic controller. In order to make the state-space model of the robot closer to the actual model, the transmission process of the surgical robot will be divided into two parts for modeling. The first part is the linear system model of the block movement, and the second part is the nonlinear system model of the puncture needle movement. The determinant ratio is used to estimate the order of the linear system model, and a Kalman filter is established to estimate the linear system model parameters. Taylor formula is used to linearize the nonlinear system model into the controller.


### 3.1. Linear Part

Firstly, the linear system model is a MIMO system; however, each input corresponds to only one output. Thus, the MIMO system is equivalent to four single-input single-output (SISO) systems. The order and parameters of the SISO system are considered:(20)yk+a1yk−1+⋯+anyk−n=b0uk+ek,where ek is the Gaussian white noise with mean zero, uk is the input signal, and yk is the output signal. The input and output signals at each moment are known, but parameters are unknown. Then, input signal and output signal are used to construct determinants for comparison to obtain the ratio DR∗n^:(21)DR∗n^=detH∗n^detH∗n^+1,where n^ is the estimated order and H∗ is a matrix composed of input and output and has the following relationship:(22)Hn^∗=yn^yn^−1⋯y1un^un^−1⋯u1yn^+1yn^⋯y2un^+1un^⋯u2⋮⋮⋱⋮⋮⋮⋱⋮yn^+L−1yn^+L−2⋯yLun^+L−1un^+L−2⋯uL,H∗n^=1LHn^∗Hn^∗,where L is the sampling length of the signal. n^ gradually increases from one. If only DR∗n^ has a significant growth compared to DR∗n^−1, n^ is the estimated order.Next, a Kalman filter is designed to dynamically identify the system’s unknown parametersaii=1,2,…,n and b0. The Kalman filter is usually used to optimally estimate the state value of the state-space equation, which is composed of the unknown parameters:(23)Xk+1=ΦXk+ΓWk,Yk=HkXk+Vk,where both Vk and Wk are noise matrix composed of Gaussian white noise with zero mean. Γ is the process noise drive matrix. Xk is the state matrix at the k-th sampling moment. Φ is the state transition matrix at the k-th sampling moment. Yk is the output matrix at the k-th sampling moment. And we assume that the variances of the two noises are Q and R. Then, the update equation of Kalman filter can be derived based on Euclidean theorem:(24)X^k+1k=ΦX^kk,X^k+1k+1=X^k+1k+Kk+1Yk+1−HX^k+1k,Kk+1=Kk+1kHTHPk+1kHT+R−1,Pk+1k=ΦPkkΦT+ΓQΓT,Pk+1k+1=In−Kk+1HPk+1k,where X^kk is the optimal estimate at the k-th sampling moment, Pkk is the error covariance of the current sampling time, and Kk+1k is the gain of the Kalman filter, which is used to update the values of X^kk and Pkk. In this paper, the estimated state matrix with unknown parameters is constructed:(25)x1k=a1k,x2k=a2k,⋮xnk=ank,xn+1k=b0k.Then the system’s state equation is obtained as follows:(26)Xk+1=Xk+Wk,where Xk is composed of xiki=1,2,…,n+1. We use the observed input and output data to construct the matrix Hk and state equation:(27)Hk=−yk−1,−yk−2,…,−yk−n,uk,Xk+1=Xk+Wk,yk=HkXk+ek.Finally, state equation (28) can be updated by equation (25) to obtain the actual system parameters. The SISO systems of four motors are combined to obtain the MIMO system of the linear part:(28)A=010⋯0001⋯0⋮⋮⋮⋱⋮000⋯1−an−an−1−an−2⋯−a1,(29)BT=00⋯0b0,(30)C=00⋯01,(31)Xk+1=Axa0000Aya0000Axb0000AybXk+Bxa0000Bya0000Bxb0000Bybuk,(32)Yk=Cxa0000Cya0000Cxb0000CybXk,where the linear part’s MIMO system is expressed by rewriting the state equations (32) and (33); Xk consists of samples of the output y before the k-th time step; Axa, Axb, Aya, and Ayb are composed of each motor model parameter matrix A; similarly, other parameters of the MIMO system can also be expressed by equations (30) and (31).


### 3.2. Nonlinear Part

The nonlinear part of the system mainly represents the relationship between the rotation of the puncture needle and the coordinates of the block. The relationship between them through equations (2)–(9) can be obtained as follows:(33)xe,ye,ze=fxa,ya,xb,yb,where f is their nonlinear mapping. Then, a first-order expansion of Taylor’s formula is performed for f to obtain the following linear relationship:(34)xe,ye,zeT=Jfxa−xarya−yarxb−xbryb−ybr+σ,(35)Jf=∂fxt∂xa∂fxt∂ya∂fxt∂xb∂fxt∂yb∂fyt∂xa∂fyt∂ya∂fyt∂xb∂fyt∂yb∂fzt∂xa∂fzt∂ya∂fzt∂xb∂fzt∂yb,where Yrk=xar,yar,xbr,ybrT is the target reference values obtained by the inverse kinematics model, σ is an infinitesimal quantity, and Jf is the linearized Jacobian matrix. Finally, the complete input and output linear system model of the surgical robot can be obtained through equations (33) and (35) as follows:(36)Xk+1=AsXk+Bsuk,(37)xe,ye,zeT=JfCsXk−Yrk,where(38)As=Axa0000Aya0000Axb0000Ayb,(39)Bs=Bxa0000Bya0000Bxb0000Byb,(40)Cs=Cxa0000Cya0000Cxb0000Cyb.


## 3.1. Linear Part

Firstly, the linear system model is a MIMO system; however, each input corresponds to only one output. Thus, the MIMO system is equivalent to four single-input single-output (SISO) systems. The order and parameters of the SISO system are considered:(20)yk+a1yk−1+⋯+anyk−n=b0uk+ek,where ek is the Gaussian white noise with mean zero, uk is the input signal, and yk is the output signal. The input and output signals at each moment are known, but parameters are unknown. Then, input signal and output signal are used to construct determinants for comparison to obtain the ratio DR∗n^:(21)DR∗n^=detH∗n^detH∗n^+1,where n^ is the estimated order and H∗ is a matrix composed of input and output and has the following relationship:(22)Hn^∗=yn^yn^−1⋯y1un^un^−1⋯u1yn^+1yn^⋯y2un^+1un^⋯u2⋮⋮⋱⋮⋮⋮⋱⋮yn^+L−1yn^+L−2⋯yLun^+L−1un^+L−2⋯uL,H∗n^=1LHn^∗Hn^∗,where L is the sampling length of the signal. n^ gradually increases from one. If only DR∗n^ has a significant growth compared to DR∗n^−1, n^ is the estimated order.Next, a Kalman filter is designed to dynamically identify the system’s unknown parametersaii=1,2,…,n and b0. The Kalman filter is usually used to optimally estimate the state value of the state-space equation, which is composed of the unknown parameters:(23)Xk+1=ΦXk+ΓWk,Yk=HkXk+Vk,where both Vk and Wk are noise matrix composed of Gaussian white noise with zero mean. Γ is the process noise drive matrix. Xk is the state matrix at the k-th sampling moment. Φ is the state transition matrix at the k-th sampling moment. Yk is the output matrix at the k-th sampling moment. And we assume that the variances of the two noises are Q and R. Then, the update equation of Kalman filter can be derived based on Euclidean theorem:(24)X^k+1k=ΦX^kk,X^k+1k+1=X^k+1k+Kk+1Yk+1−HX^k+1k,Kk+1=Kk+1kHTHPk+1kHT+R−1,Pk+1k=ΦPkkΦT+ΓQΓT,Pk+1k+1=In−Kk+1HPk+1k,where X^kk is the optimal estimate at the k-th sampling moment, Pkk is the error covariance of the current sampling time, and Kk+1k is the gain of the Kalman filter, which is used to update the values of X^kk and Pkk. In this paper, the estimated state matrix with unknown parameters is constructed:(25)x1k=a1k,x2k=a2k,⋮xnk=ank,xn+1k=b0k.Then the system’s state equation is obtained as follows:(26)Xk+1=Xk+Wk,where Xk is composed of xiki=1,2,…,n+1. We use the observed input and output data to construct the matrix Hk and state equation:(27)Hk=−yk−1,−yk−2,…,−yk−n,uk,Xk+1=Xk+Wk,yk=HkXk+ek.Finally, state equation (28) can be updated by equation (25) to obtain the actual system parameters. The SISO systems of four motors are combined to obtain the MIMO system of the linear part:(28)A=010⋯0001⋯0⋮⋮⋮⋱⋮000⋯1−an−an−1−an−2⋯−a1,(29)BT=00⋯0b0,(30)C=00⋯01,(31)Xk+1=Axa0000Aya0000Axb0000AybXk+Bxa0000Bya0000Bxb0000Bybuk,(32)Yk=Cxa0000Cya0000Cxb0000CybXk,where the linear part’s MIMO system is expressed by rewriting the state equations (32) and (33); Xk consists of samples of the output y before the k-th time step; Axa, Axb, Aya, and Ayb are composed of each motor model parameter matrix A; similarly, other parameters of the MIMO system can also be expressed by equations (30) and (31).


## 3.2. Nonlinear Part

The nonlinear part of the system mainly represents the relationship between the rotation of the puncture needle and the coordinates of the block. The relationship between them through equations (2)–(9) can be obtained as follows:(33)xe,ye,ze=fxa,ya,xb,yb,where f is their nonlinear mapping. Then, a first-order expansion of Taylor’s formula is performed for f to obtain the following linear relationship:(34)xe,ye,zeT=Jfxa−xarya−yarxb−xbryb−ybr+σ,(35)Jf=∂fxt∂xa∂fxt∂ya∂fxt∂xb∂fxt∂yb∂fyt∂xa∂fyt∂ya∂fyt∂xb∂fyt∂yb∂fzt∂xa∂fzt∂ya∂fzt∂xb∂fzt∂yb,where Yrk=xar,yar,xbr,ybrT is the target reference values obtained by the inverse kinematics model, σ is an infinitesimal quantity, and Jf is the linearized Jacobian matrix. Finally, the complete input and output linear system model of the surgical robot can be obtained through equations (33) and (35) as follows:(36)Xk+1=AsXk+Bsuk,(37)xe,ye,zeT=JfCsXk−Yrk,where(38)As=Axa0000Aya0000Axb0000Ayb,(39)Bs=Bxa0000Bya0000Bxb0000Byb,(40)Cs=Cxa0000Cya0000Cxb0000Cyb.


## 4. Model Predictive Control

Model predictive control (MPC) is now a well-known model-based control strategy, and it has gone through a period of development [41–43]. Taking the state-space model as an example, MPC uses the state of the model at the current time and the target state to estimate the control sequences at the next time under constraints. Compared with proportional-integral-derivative (PID) controller, MPC can predict the future behavior of the system. The optimal future control sequences are estimated by the known data. These sequences are evaluated at each sample time by the optimizer with constraints.MPC consists of three components [44]: (1) a loss function, (2) constraints in the form of equality and inequality, and (3) initial conditions. The purpose of the loss function is to minimize the distance between the target state and the state at the current time [45], thereby minimizing the energy required by the control system. The equality constraints consist of the system’s state equations. The limits on the state variables are considered as the inequality constraints. If all constraints are linear, the problem forms a convex set. Meanwhile, if the control system is also convex, MPC can be solved as a convex optimization problem. Similarly, the MPC can be solved as a quadratic problem if the control system is a quadratic problem.MPC uses the system’s model to predict the future control sequences [46]. As shown in Figure 5, the optimizer in MPC is to get the best control input. The linear time-invariant system will be considered in the discrete time domain as shown in equation (37). However, in equation (37), the state and input variables are added by two matrices to obtain the final output. To comprehensively consider the state and input variables, equations (37) and (38) are rewritten as follows:(41)ξk+1=Amξk+BmΔuk,(42)xe,ye,zeT+JfYrk=Cmξk,where(43)ξk=Xkuk−1,(44)Δuk=uk−uk−1,(45)Am=AsBs0I4nx4,(46)Bm=BsI4x4,(47)Cm=Cs,0.Figure 5
MPC controller internal rolling optimization process. The optimizer optimizes the current control input to obtain the optimal control input at the next step.Equations (42) and (43) are state-space equations that consider both state and input variables. We mainly discuss the optimization problem of equation (42), namely, the state space. This equation can make predictions about the future state, that is, the prediction step required for optimization can be obtained:(48)Yξ=Ψξk+ΘΔUk,where(49)Yξ=ξk+1,ξk+2,…,ξk+NpT,(50)Ψ=Am,Am2,…,AmNpT,(51)Θ=Bm00⋯0AmBmBm0⋅0Am2BmAmBmBm⋯0⋮⋮⋮⋱⋮AmNp−1BmAmNp−2BmAmNp−3Bm⋯AmNp−NcBm,where Np is the predicted step size of the system and Nc is the control step size of the system. Next, the reference trajectory ξr=Xr,urT is used to construct the following loss function:(52)Jξ=Yξ−YξrTQξYξ−Yξr,where Qξ is the weight matrix. Considering that the control input cannot change too much within a period of time, the following loss function is constructed:(53)Ju=ΔUTPuΔU,where Pu is the weight matrix. And the objective function to be optimized can be obtained by equations (53) and (54), which can be seen as a quadratic programming problem:(54)J=Jξ+Ju=Ψξ+ΘΔUTQξΨξ+ΘΔU−YξrTQξYξr+ΔUTPuΔU=ΨξTQξΨξ−YξrTQξYξr+ΘΔUTQξΘΔU+2ΨξTQξΘΔU+ΔUTPuΔU=ΔUTΘQξΘ+PuΔU+2ΨξTQξΘΔU.Finally, the constraints of the control variableΔU of the loss function to construct a standard form of quadratic programming will be set:(55)minΔUJs.t.lb≤ΔU≤ub,where lb and ub are the lower and upper bounds of the variable ΔU. Then, the best control sequences can be obtained by equation (45).


## 5. Simulation Setup and Results

This section mainly describes the simulated experimental settings and experimental results about this article. This article uses MATLAB to build a remote subsystem, which contains the MPC controller and parameter identification system. Under the function of the parameter identification system, the MPC controller obtains the robotic kinematics model realistically and outputs the optimal control sequence. In the first place, as shown in Figure2, there is actually a TD when the MPC controller is applied to the system; thus, in the experiment, a delay system will be considered for comparison. We will, in addition, divide it into two experiments: (1) order and parameter identification of the system’s linear part and (2) performance of MPC controller and PID controller in delay system.


### 5.1. Model Identification Experiment

Above all, only the model identification of the linear part of the system is considered. And the order needs to be identified through offline data. The system’s order can be obtained by equation (21). In this paper, the system’s order n^=2, the pitch of the screw ρ=0.02, and the largest movement distance of the block η=2. Next, the Kalman filter is designed to identify the system parameters. The state equation of the system is rewritten to identify the parameters of the system, and the result is represented by H in equation (28). The results of parameter identification are shown in Figure 6.Figure 6
The identification result of the system parameters, parametersa1 and a2 are the coefficients of the output term, and parameter b is the coefficient of the input term.It can be seen from Figure6 that the three parameters can stabilize at a certain value a1=0.0125, a2=−0.0257, and b=1.818 within the step size. However, it is not known whether a system composed of stable parameters can approximate the real system. Thus, as shown in Figure 7, the output of the actual system is compared with the output of the identified model. The figure shows the target curve, the output curve of the identification model, and the error curve between them. The value of the error curve floats above and below zero.Figure 7
Comparison of system parameter identification. The error is the difference between the target value and the estimated value.


### 5.2. MPC Performance

First, the model of the linear part of the control system can be obtained by equation (28), and the parameters of the model are obtained in the previous experiment, which contains the effective length of the needle h=0.05m and the distance between blocks d=0.03m. And the linearized model of the nonlinear part of the control system can be obtained by equation (35). Then, the two models are combined by equation (37) to obtain a complete model of the control system. In addition, in order to comprehensively consider the control input and system state, the state-space equation of the control system is rewritten by equations (42) and (43). Finally, the prediction equation (49) required by the MPC controller is obtained by equation (42), which is used for rolling optimization in the MPC controller. The constraints of quadratic programming lb=−0.002 and ub=0.002 are constraints on the difference values of control sequence. The optimizer of the MPC controller outputs the optimal control sequence through equation (55). In this paper, traditional PID controller is designed to compare with the MPC controller. In fact, as the controller takes time to run, there is a delay in the output of the controller, which is equivalent to the control delay system. Thus, we set different TDs in the control system. We set the weight matrix Qξ and Pu in the MPC controller as the identity matrix and the parameters of the PID controller Kp=13, Ki=2, and Kd=0.Figure8 shows the Y-axis motion trajectory of block-a, which illustrates that with increase in the delay time, the response time of the system also increases and the desired trajectory can be tracked by the MPC controller. As the delay increases, the response speed of the system also becomes slower, and the output of the MPC controller will produce 2% overshoot. However, the PID controller does not perform well in the case of TD. The output of the PID controller diverges when TD = 250ms, making the system unstable. When TD = 150ms, the PID controller makes the system gradually stable, but the setting time ts=4s and the 10% overshoot exceed the constraints of the surgical robot.Figure 8
The performance of theY-axis motion trajectory of block A under different delays and different controllers.Then, the output of the position of the tip of the puncture needle will be analyzed. As shown in Figure9, in the open-loop state without controller, the output value of the system has a large fluctuation around the desire trajectory. The closed-loop system formed by the MPC controller can track the desired trajectory with TD, which will produce 2% overshoot in the delay system. However, the closed-loop system formed by the PID controller is sensitive to TD. When there is a large TD in the system, it will produce a larger overshoot than the MPC controller and even make the system unstable.Figure 9
The performance of theZ-axis motion trajectory of the tip of puncture needle under different delays and different controllers.Finally, in order to more clearly show the advantages of the MPC controller, a differential operation is performed on the state value of the sine wave shown in Figure8 to obtain the distance from the target value shown in Figure 10. In the figure, the output of the MPC controller applied to the delay system produces phase shifts at different distances relative to the expected value. However, when the PID controller acts on a nondelayed system, phase shift occurs. As the delay increases, even the output diverges. The above experimental results show that the effect of MPC controller is better than that of the PID controller. The error variance is shown in Table 1, the desired error variance is zero, and the closer to the ideal error variance, the more ideal the system’s outputs are. The error variance of the MPC controller in the TD system is smaller than the error variance generated by the PID controller. And when TD=0, the error variance generated by the MPC controller is even smaller than the error variance generated by the nondelay system with noise. From the result, we can get that for the parallel mechanism robot involved in this article, MPC has a superior control effect because MPC can control the MIMO system.Figure 10
The performance of the coordinate difference in theY-axis direction of the block. The TD will cause the regular periodic error of the sine wave.Table 1
Error variance (cm2) under different TD.
Y1-MPC-250Y1-MPC-150Y1-MPC-50Y1-MPC-0Y1-pid-250Y1-pid-150Y1-pid-50Y1-PID-0Y1-reality-00.66250.40480.15120.07281.67300.70670.61780.68600.1077


## 5.1. Model Identification Experiment

Above all, only the model identification of the linear part of the system is considered. And the order needs to be identified through offline data. The system’s order can be obtained by equation (21). In this paper, the system’s order n^=2, the pitch of the screw ρ=0.02, and the largest movement distance of the block η=2. Next, the Kalman filter is designed to identify the system parameters. The state equation of the system is rewritten to identify the parameters of the system, and the result is represented by H in equation (28). The results of parameter identification are shown in Figure 6.Figure 6
The identification result of the system parameters, parametersa1 and a2 are the coefficients of the output term, and parameter b is the coefficient of the input term.It can be seen from Figure6 that the three parameters can stabilize at a certain value a1=0.0125, a2=−0.0257, and b=1.818 within the step size. However, it is not known whether a system composed of stable parameters can approximate the real system. Thus, as shown in Figure 7, the output of the actual system is compared with the output of the identified model. The figure shows the target curve, the output curve of the identification model, and the error curve between them. The value of the error curve floats above and below zero.Figure 7
Comparison of system parameter identification. The error is the difference between the target value and the estimated value.


## 5.2. MPC Performance

First, the model of the linear part of the control system can be obtained by equation (28), and the parameters of the model are obtained in the previous experiment, which contains the effective length of the needle h=0.05m and the distance between blocks d=0.03m. And the linearized model of the nonlinear part of the control system can be obtained by equation (35). Then, the two models are combined by equation (37) to obtain a complete model of the control system. In addition, in order to comprehensively consider the control input and system state, the state-space equation of the control system is rewritten by equations (42) and (43). Finally, the prediction equation (49) required by the MPC controller is obtained by equation (42), which is used for rolling optimization in the MPC controller. The constraints of quadratic programming lb=−0.002 and ub=0.002 are constraints on the difference values of control sequence. The optimizer of the MPC controller outputs the optimal control sequence through equation (55). In this paper, traditional PID controller is designed to compare with the MPC controller. In fact, as the controller takes time to run, there is a delay in the output of the controller, which is equivalent to the control delay system. Thus, we set different TDs in the control system. We set the weight matrix Qξ and Pu in the MPC controller as the identity matrix and the parameters of the PID controller Kp=13, Ki=2, and Kd=0.Figure8 shows the Y-axis motion trajectory of block-a, which illustrates that with increase in the delay time, the response time of the system also increases and the desired trajectory can be tracked by the MPC controller. As the delay increases, the response speed of the system also becomes slower, and the output of the MPC controller will produce 2% overshoot. However, the PID controller does not perform well in the case of TD. The output of the PID controller diverges when TD = 250ms, making the system unstable. When TD = 150ms, the PID controller makes the system gradually stable, but the setting time ts=4s and the 10% overshoot exceed the constraints of the surgical robot.Figure 8
The performance of theY-axis motion trajectory of block A under different delays and different controllers.Then, the output of the position of the tip of the puncture needle will be analyzed. As shown in Figure9, in the open-loop state without controller, the output value of the system has a large fluctuation around the desire trajectory. The closed-loop system formed by the MPC controller can track the desired trajectory with TD, which will produce 2% overshoot in the delay system. However, the closed-loop system formed by the PID controller is sensitive to TD. When there is a large TD in the system, it will produce a larger overshoot than the MPC controller and even make the system unstable.Figure 9
The performance of theZ-axis motion trajectory of the tip of puncture needle under different delays and different controllers.Finally, in order to more clearly show the advantages of the MPC controller, a differential operation is performed on the state value of the sine wave shown in Figure8 to obtain the distance from the target value shown in Figure 10. In the figure, the output of the MPC controller applied to the delay system produces phase shifts at different distances relative to the expected value. However, when the PID controller acts on a nondelayed system, phase shift occurs. As the delay increases, even the output diverges. The above experimental results show that the effect of MPC controller is better than that of the PID controller. The error variance is shown in Table 1, the desired error variance is zero, and the closer to the ideal error variance, the more ideal the system’s outputs are. The error variance of the MPC controller in the TD system is smaller than the error variance generated by the PID controller. And when TD=0, the error variance generated by the MPC controller is even smaller than the error variance generated by the nondelay system with noise. From the result, we can get that for the parallel mechanism robot involved in this article, MPC has a superior control effect because MPC can control the MIMO system.Figure 10
The performance of the coordinate difference in theY-axis direction of the block. The TD will cause the regular periodic error of the sine wave.Table 1
Error variance (cm2) under different TD.
Y1-MPC-250Y1-MPC-150Y1-MPC-50Y1-MPC-0Y1-pid-250Y1-pid-150Y1-pid-50Y1-PID-0Y1-reality-00.66250.40480.15120.07281.67300.70670.61780.68600.1077


## 6. Conclusion and Future Work

We introduced a surgical robotic puncture needle positioning platform that can improve the efficiency of biopsy. Kinematic analysis of the platform illustrates that the puncture needle can work in the working space through multiple angles that can be obtained by decoupling the robotic kinematic. In this paper, two constraints are defined for decoupling the kinematic, which ensure the robotic largest workspace. The designed MPC controller makes the control flow of the positioning device form a closed loop. And the state equation of the MPC controller needed is divided into two parts: linear part and nonlinear part. In order to guarantee the simulation of robotic system closer to the reality, the Kalman filter system parameters identification approach is used to obtain the system’s parameters in the linear part. In the nonlinear part, Taylor formula is used to linearize the system, and the linearization system only contains a Jacobian matrix. Next, the two parts of the system are combined into a complete robot system state equation. In simulation, the closed-loop control significantly improves the stability of robot needle positioning. And the performance of MPC controller is better than that of the PID controller. Finally, the system is open source in Github (https://github.com/tKsome/MPC-Puncture-Robot) to reduce the amount of development time for other researchers involved in the control of the medical robotic MIMO system. The system is flexible and may be useful for most image-based intervention procedures. In future work, we will test the performance of the proposed approach on the real-work robotic platform. Furthermore, a force control strategy will be proposed such that a force feedback can be provided to the surgeon through remote operation.



---

*Source: 5406084-2020-09-16.xml*