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

This article introduces the design and control performance of a lightweight, ﬂexible, 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 ﬁlter 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 eﬃciency is improved while reducing the burden on the surgeon.


Introduction
e 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][3][4]. Nowadays, the robotic systems have been developed and used for various surgeries such as biopsy, brachytherapy, and tumor ablation [5][6][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. e needle positioning system of the surgical assistant robot can be independent of the insertion action [8][9][10]. e 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][12][13][14][15][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][20][21][22]. Navarro-Alarcon et al. [12] developed a 3-DOF needle driver for biopsy. e 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. e 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].
is 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][24][25][26][27]. Sampling and delay are involved in this problem, which is a challenge for remote controlling of the robot. e solution to the delay problem in remote is discussed in [28][29][30][31]. However, the local optimizer on the slave side of the robot will also cause a delay [32,33]. is 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. e 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]. us, 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. e medical robot can provide feedback of the end-effector pose and force to the surgeon. e size of the designed robot is 16 cm high, 37.6 cm long, and 37.6 cm 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. is 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. e structure of the paper is organized as follows: in Section 2, 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. e end-effector position of the slave robot is updated via medical imaging or other sensors, and such position feedback is sent to master.
ere are many ways to solve the remote communication scheme between the master and slave robots [28][29][30][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. us, 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. e proposed slave robot control system can be seen in Figure 2(a). e surgeon uses CT to mark the target lesion area or trajectory in the robot workspace. e surgical robot obtains the control trajectory which serves as the reference input for the MPC controller through kinematic model. e 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. e 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. e order judger is used to judge the order of the linear system. e Kalman filter is selected as the parameter estimator to estimate the parameters of the linear system.

Kinematic Modeling
In this section, the modeling process of surgical robot kinematics will be described. e 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. e 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.
In order to obtain a suitable kinematics model, the coordinate relationship as shown in Figure 4 is established. e fixed world coordinate system is at the initial center position of block-b. w T a 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 w T b is the moving distance of block-b. Coordinate system w T e indicates the position of the tip of the puncture needle.

Forward Kinematic.
e purpose of forward kinematics is to obtain the position of the tip of the puncture needle x e , y e , and z e , 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. e moving distance of the two blocks x a , x b , y a , and y b will change the angle of the puncture needle θ.
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  Figure 3: e 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. Figure 4: e 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.

Complexity 3
is similar; α is the angle generated by two blocks on the X-Y plane; next, for coordinate system w T b , 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: Δz � h · cos(θ).
Finally, the position of the puncture needle's tip x e , y e , and z e relative to the fixed world coordinate system can be obtained as follows:

Inverse Kinematic.
e 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. us, 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 the X-Y plane is of more significance. In order to obtain a feasible solution for the motor control input, the second constraint is defined. at is, when block-a moves to the limit position and still cannot reach the lesion area, block-b is then moved. e inverse kinematics model is constructed using geometric theorem based on the constraints: and where η is the largest distance that the tip of the puncture needle can reach when only block-a is in motion; x t and y t are the lesion's target point coordinates marked by the surgeon. However, no complete feasible solution can be obtained yet. en, the solution of the coordinates of block-a within the limit η is discussed: where θ is the angle between the block and puncture needle: So far, a feasible solution within the limit η from equations (10) and (12) is obtained: 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. erefore, according to the angle θ between the block and the puncture needle, the relationship between x b and x b can be acquired: e solution of coordinate value of block-b can be acquired by equations (15) and (16): where E, EA, EB, and EC are known parameters as follows: en, the feasible solution that exceeds the limit η is obtained by equations (11) and (17): 4 Complexity Now, the complete solution of the kinematic model of the surgical robot has been obtained by equations (14) and (19). e corresponding motor control angle can be obtained through the lesion area marked by the surgeon.

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. e 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. e 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. us, the MIMO system is equivalent to four single-input single-output (SISO) systems. e order and parameters of the SISO system are considered: where e(k) is the Gaussian white noise with mean zero, u(k) is the input signal, and y(k) is the output signal. e input and output signals at each moment are known, but parameters are unknown. en, input signal and output signal are used to construct determinants for comparison to obtain the ratio DR * (n): where n is the estimated order and H * is a matrix composed of input and output and has the following relationship: 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 parameters a i (i � 1, 2, . . . , n) and b 0 . e Kalman filter is usually used to optimally estimate the state value of the state-space equation, which is composed of the unknown parameters: where both V(k) and W(k) are noise matrix composed of Gaussian white noise with zero mean. Γ is the process noise drive matrix. X(k) is the state matrix at the k-th sampling moment. Φ is the state transition matrix at the k-th sampling moment. Y(k) is the output matrix at the k-th sampling moment. And we assume that the variances of the two noises are Q and R. en, the update equation of Kalman filter can be derived based on Euclidean theorem: where X(k | k) is the optimal estimate at the k-th sampling moment, P(k | k) is the error covariance of the current sampling time, and K(k + 1 | k) is the gain of the Kalman filter, which is used to update the values of X(k | k) and Complexity P(k | k). In this paper, the estimated state matrix with unknown parameters is constructed: x n (k) � a n (k), en the system's state equation is obtained as follows: where X(k) is composed of x i (k)(i � 1, 2, . . . , n + 1). We use the observed input and output data to construct the matrix H(k) and state equation:
e SISO systems of four motors are combined to obtain the MIMO system of the linear part: −a n −a n−1 −a n−2 · · · −a 1 where the linear part's MIMO system is expressed by rewriting the state equations (32) and (33); X(k) consists of samples of the output y before the k-th time step; A x a , A x b , A y a , and A y b 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).

Nonlinear
Part. e nonlinear part of the system mainly represents the relationship between the rotation of the puncture needle and the coordinates of the block. e relationship between them through equations (2)-(9) can be obtained as follows: x e , y e , z e � f x a , y a , x b , y b , where f is their nonlinear mapping. en, a first-order expansion of Taylor's formula is performed for f to obtain the following linear relationship: x e , y e , z e T � J f x a − x ar y a − y ar where Y r (k) � [x ar , y ar , x br , y br ] T is the target reference values obtained by the inverse kinematics model, σ is an infinitesimal quantity, and J f 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: x e , y e , z e where 6 Complexity

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][42][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. e optimal future control sequences are estimated by the known data. ese 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. e 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. e equality constraints consist of the system's state equations. e 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. e linear timeinvariant 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: x e , y e , z e where 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. is equation can make predictions about the future state, that is, the prediction step required for optimization can be obtained: where where N p is the predicted step size of the system and N c is the control step size of the system. Next, the reference trajectory ξ r � [X r , u r ] T is used to construct the following loss function: 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: where P u 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: Finally, the constraints of the control variable ΔU of the loss function to construct a standard form of quadratic programming will be set: where lb and ub are the lower and upper bounds of the variable ΔU. en, the best control sequences can be obtained by equation (45).

Simulation Setup and Results
is section mainly describes the simulated experimental settings and experimental results about this article. is 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 Figure 2, 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.

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. e 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. e state equation of the system is rewritten to identify the parameters of the system, and the result is represented by H in equation (28). e results of parameter identification are shown in Figure 6.
It can be seen from Figure 6 that the three parameters can stabilize at a certain value a 1 � 0.0125, a 2 � −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. us, as shown in Figure 7, the output of the actual system is compared with the output of the identified model. e figure shows the target curve, the output curve of the identification model, and the error curve between them. e value of the error curve floats above and below zero.

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.05 m and the distance between blocks d � 0.03 m. And the linearized model of the nonlinear part of the control system can be obtained by equation (35). en, 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. e constraints of quadratic programming lb � −0.002 and ub � 0.002 are constraints on the difference values of control sequence. e 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. us, we set different TDs in the control system. We set the weight matrix Q ξ and P u in the MPC controller as the identity matrix and the parameters of the PID controller K p � 13, K i � 2, and K d � 0. Figure 8 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. e output of the PID controller diverges when TD � 250 ms, making the system unstable. When TD � 150 ms, the PID controller makes the system gradually stable, but the setting time ts � 4 s and the 10% overshoot exceed the constraints of the surgical robot.
en, the output of the position of the tip of the puncture needle will be analyzed. As shown in Figure 9, in the openloop state without controller, the output value of the system  Figure 5: MPC controller internal rolling optimization process. e optimizer optimizes the current control input to obtain the optimal control input at the next step.
8 Complexity  Figure 6: e identification result of the system parameters, parameters a 1 and a 2 are the coefficients of the output term, and parameter b is the coefficient of the input term.   has a large fluctuation around the desire trajectory. e 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.
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 Figure 8 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. e above experimental results show that the effect of MPC controller is better than that of the PID controller. e 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. e 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.

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. e 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. e system is flexible and may be useful for most imagebased 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.

Data Availability
e data included in this study are available upon proper request by contact with the corresponding author.

Conflicts of Interest
e authors declare that they have no conflicts of interest.