Dual-EKF System Identification and Model Predictive Path Integral Control of a Retinal Microsurgical Robot

Abstract

This study presents a modeling and control framework for a retinal microsurgical robot, the Improved Integrated Robotic Intraocular Snake (I²RIS), which accounts for the coupling between pitch and yaw degrees of freedom. The system is modeled using a multi-input multi-output (MIMO) mass-spring-damping (MSD) formulation, with parameters and states jointly estimated through a dual-extended Kalman filter (dual-EKF) approach. Unlike analytical methods such as Cosserat-based models, the proposed approach is computationally efficient, requires minimal training data, and enables real-time control. The coupled MSD model captures the interdependence between pitch and yaw dynamics using experimentally optimized parameters. An optimal stochastic controller, the Model Predictive Path Integral (MPPI), is then designed and compared with a Linear Quadratic Regulator (LQR) in a trajectory-tracking control problem. Experimental results demonstrate that MPPI achieves superior performance and robustness in controlling the coupled dynamics of the I²RIS robot, offering a promising solution for efficient real-time control of snake-like robotic systems.

1. Introduction

Epiretinal membrane (ERM) is a fibrocellular tissue that develops on the inner surface of the retina, most frequently over the macula, where it generates tangential traction. This traction can distort the retinal architecture and increase macular thickness, leading to visual symptoms such as reduced visual acuity and metamorphopsia. Reported prevalence rates of ERM vary substantially across different racial groups and geographic regions, with estimates ranging from approximately 7% to 11.8% [1]. Epidemiological estimates from a 1994 study indicate that epiretinal membranes affect approximately 30 million individuals (among adults aged 43 to 86 years) in the United States, with involvement of at least one eye [2]. ERM peeling is a surgical procedure, in which surgeons delaminate the membrane using a microforceps. Performing this procedure with straight handheld forceps is consistently challenging, and the risk of retina damage is significant [3].

Challenges include physiological hand tremor ($182\mu \text{m}$) [4], small thickness of the retina ($160\text{to}291\mu \text{m}$) [5], retina motion induced by respiration and heartbeat ($81\pm 3.5\mu \text{m}$ at about 1 Hz) [6], improper depth perception in axial microscopic views, and the fragile, non-regenerative nature of retina tissue.

To address these issues, several robotic-assisted surgical systems have been developed, such as Preceyes [7], the Stereotaxical Microtelemanipulator for Ocular Surgery (SMOS) [8], the Robot Assisted Microsurgery (RAMS) [9], and the Steady-Hand Eye Robot (SHER) [10], and others [11]. The Preceyes system has features such as remote center of motion (RCM) constraint, motion scaling, tremor filtering, virtual depth constraints, and instrument freeze functions. The SMOS is primarily designed for ocular vitrectomy and focuses on implementing an RCM to enable safe intraocular access. The RAMS prioritizes clinical compatibility through a compact, patient-mounted design that provides high stiffness, intuitive RCM adjustment, and seamless operating-room integration without the need for patient motion tracking. The SHER is a five-degree-of-freedom (5-DoF) manipulator developed at Johns Hopkins University, which can be controlled based on cooperative and teleoperation strategies and is integrated with advanced sensing modalities such as force sensing needles and optical coherence tomography (OCT) imaging. Despite achieving hand tremor removal and micromillimeter-scale positioning accuracy, these robotic manipulators lack the necessary dexterity required for ERM peeling, due to their traditional rigid-link design.

Continuum robots can provide such dexterity in the small, confined space inside the human eye [12]. The Improved Integrated Robotic Intraocular Snake (I²RIS) [13] is a cable-driven continuum robot designed to be integrated with the SHER robots’ end-effector (three versions of SHER, including 2.0, 2.1, and 3.0), and to increase SHER’s DoFs from 5 to 7, to enable surgeons to perform ERM peeling with optimum approach angle while minimizing applied forces that might result in tractional injury or hemorrhage (see Fig. 1).

Fig. 1:

The I²RIS attached to the SHER 3.0 handle. The integrated robotic system has 7 DoFs (5 by SHER and 2 by I²RIS) and a gripping DoF, which provides the necessary dexterity to perform complex tasks such as ERM peeling.

Accurate positioning control of the I²RIS robot is challenging, due to its small size (0.9 mm in diameter); there is no embedded sensor to provide state feedback. Therefore, it is essential to utilize model-based state estimation methods for real-time control of the I²RIS robot. Several models have been developed to represent continuum robot kinematics, such as rigid link transformations [13], constant curvature model [14], finite element method [15], Cosserat-based models [16,17], combined models [18, 19], and others [20]; however, these models rely on ideal geometrical assumptions, neglecting structural nonlinearities such as hysteresis [21], coupled motion, cable tension condition [22], and friction, etc. Many vitreoretinal surgical tasks require sub-millimeter accuracy and stable manipulation of extremely delicate tissue. For instance, ERM peeling involves delaminating a thin layer ($61\pm 28\mu \text{m}$) [23] without causing trauma to the underlying retina tissue. These operations need precise tool positioning and force regulation to avoid unintended contact, excessive deformation, or collateral damage to adjacent structures. Consequently, mechanical nonlinearities—such as motion coupling and hysteresis—can significantly degrade control performance and adversely affect ERM peeling outcomes [24].

Moreover, these analytical models are computationally intensive, limiting their applicability in real-time control scenarios. Data-driven models, such as Gaussian Mixture Models (GMM) [25], have the potential to generate optimal, custom characterizations of continuum robot kinematics based on experimental data; however, they typically require a significant amount of data collected through robot motion. Prolonged data collection can introduce mechanical issues, such as gradual cable loosening and hysteresis, which affect the robot’s kinematic response. As a result, maintaining repeatable device behavior and ensuring data reproducibility becomes increasingly challenging with extended robot use. For example, the lifespan for the Intuitive Da Vinci Xi Maryland Bipolar Forceps and the innovative shaft-actuated tip articulation (SATA) grasper are limited to 14 and 50 uses, respectively [26].

In a previous study, we simulated a control algorithm for the I²RIS robot under a decoupled motion assumption [27]; however, the real robot exhibits coupled motion, in part due to asymmetric cable tension, relative slippage between the rolling disks (see Fig. 3), and other potential assembly errors. In this work, we focus on controlling the I²RIS robot by accounting for the coupled motions between the pitch and yaw motions (see Fig. 3-b). The model is based on a multi-input multi-output mass-spring-damping (MIMO-MSD) dynamical system with a dual-extended Kalman filter (dual-EKF) parameter and state estimator. This approach, unlike analytical methods such as Cosserat-based models, is computationally efficient and fast and does not require significant data collection during the training phase. These properties enable an efficient model identification, parameter optimization, and real-time control of the I²RIS robot.

Fig. 3:

The CAD model of the dexterous distal unit and gripper of the I²RIS root. (a) Presents the mechanical components: including two fixed disks (component①) and eleven rolling disks between them (component②), and fixed and moving gripper jaws (③, ④). (b) Describes a cross-sectional view of a single rolling disk. (c) Depicts the dimensions of all components in millimeters (left), a magnified view highlighting the disks, wire routing, and actuation pulleys (middle), and how the wires are fixed to the final disk using adhesive (right).

The contributions of this work are as follows:

• We characterized the I²RIS robot kinematics using a coupled-dynamics mass-spring-damper (MSD) model to account for the coupling effects between pitch and yaw motions. The model’s parameters are optimized using a dual-extended Kalman filter (dual-EKF) method and experimental data of the I²RIS robot motion.

• We designed a stochastic optimal controller, a smooth Model Predictive Path Integral (MPPI), for the I²RIS robot and compared its performance with that of a classical Linear Quadratic Regulator (LQR) in a trajectory-tracking problem.

This paper is organized as follows. Materials and methods are explained in Section 2. Experimental setup and procedures are described in Section 3. Results are analyzed in Section 4 and discussed in Section 5. Section 6 concludes the paper.

2. MATERIALS AND METHODS

2.1. Hybrid SHER-I²RIS Robotic System

Figure 1 shows the hybrid robotic platform for retinal microsurgery applications. The system includes a five-degree-of-freedom (5-DoF) robot manipulator called Steady-Hand Eye Robot (SHER 3.0) (see Fig. 1-a), integrated with the 2-DoF I²RIS robot attached to its end-effector (Fig. 1-b). The position and orientation of the I²RIS robot can be controlled by SHER 3.0 to reach a target point. At the same time, the articulated distal end of the I²RIS provides wrist-like dexterity (pitch and yaw) for optimum approach angle and gripping capability for membrane peeling procedures. Both robots can be controlled in cooperative and teleportation modalities with human-in-the-loop trajectory corrections [28]. Yet, controlling them in an autonomous mode requires proper modeling and control strategies. Control of SHER 3.0 is less of a challenge, due to its conventional structure design and availability of the joint variables measured by encoders; however, modeling and control of the I²RIS robot is challenging, due in part to mechanical nonlinearities such as asymmetric cable tensions, coupled motions, and lack of embedded sensory feedback. The control performance of the SHER 3.0 is studied in a previous work [29]; and this work focuses on modeling and controlling the I²RIS robot.

2.2. Mechanical Design of I²RIS Robot

Figure 3 illustrates the conceptual configuration of the I²RIS robot’s dexterous distal unit (DDU). The DDU consists of thirteen disk-shaped components with outer diameter of 0.9 mm (machined from SUS 303 stainless steel), including two fixed disks at both ends and eleven rolling disks in between (see Fig. 3-a), actuated by two pairs of interconnected tensioning wires with 0.15 mm diameter (SUS 304) (Fig. 3-b), enabling two DoF, bending along the pitch and yaw axes. Each disk features cylindrical upper and lower surfaces and five through-holes. The central hole allows the passage of driving wire for a microgripper (see Fig. 4). The disks are sequentially arranged so that adjacent elements make contact through alternately oriented cylindrical surfaces (the rolling axis between each pair of disks alternates 90° for all adjacent pairs), forming an orthogonally distributed structure that generates flexible pitch and yaw bending motions at the tip. For sterilization, the DDU is designed to be easily attached to and detached from the actuation unit (see Fig. 2).

Fig. 4:

Assembly of the gripper wire and jaws of the I²RIS robot. The gripper is normally open because the nitinol wire has a restoring force that tends to straighten it. The gripper is closed by pulling the nitinol wire. The open and closed stages of the gripper are shown in the side view (a) and perspective view (b). The assembly of the pitch and yaw wires (colored), the gripper wire (black), the fixed and moving disks, and the jaws are depicted in (c).

Fig. 2:

Overview of the actual I²RIS robot. (a) The locked mode: when the actuation unit and the instrument unit are locked together and the actuation pulleys are fully engaged with the instrument pulleys using actuation pins (fully actuated), (b) the unlocked mode: where the connection lever is released and the actuation pins of the actuation pulleys are disengaged from the instrument pulleys (no actuation), but the connection shafts are still engaged, holding the instrument unit in place, and (c) the detached mode: where both the actuation pins and the connection shafts are disengaged. The pitch, yaw, and gripping motions are activated using three motors attached to the actuation pulleys (d) when they are locked to their instrument pulley counterparts (e). The dexterous distal unit can provide a wrist-like motion as shown in (f).

2.3. I²RIS Robot Model

To address the cross-coupling effects in the I²RIS system, a Multi-Input Multi-Output model based on a mass-spring-damper framework is employed, shown in Fig. 5. This model captures the dynamic interactions between the yaw and pitch axes, enabling the representation of coupled angular responses to motor inputs. The MSD system effectively models the internal cable tension and damping behavior inherent in the wire-driven structure. For simplicity, higher-order nonlinear stiffness terms are omitted. The following equations describe the simplified model, where $x_1$ and $x_2$ denote the angular output states (the pitch and yaw angles), $u_1$ and $u_2$ represent the encoder-driven motor inputs, and $f_1$ and $f_2$ correspond to the cable tensions associated with the pitch and yaw motions. The angle $\beta$ accounts for a rotational alignment variable that determines the degree of motion coupling between pitch and yaw, and $h_1,h_2$ could be general nonlinear functions that could potentially model hysteresis behavior; however, in this model, for simplicity, they are set as identity functions, and also the mass is normalized to $m=1$. $c_1,c_2$ are damping coefficients and $k_1,k_2$ are stiffness constants. The equations of the model are shown as follows:

$$m\ddot{x_1}+c_1\dot{x_1}+k_1{x}_1=f_1\text{cos}\left(\beta \right)-f_2\text{sin}(\beta )$$ (1)

$$m\ddot{x_2}+c_2\dot{x_2}+k_2{x}_2=f_2\text{cos}\left(\beta \right)+f_1\text{sin}(\beta )$$ (2)

which can be expressed in a standard state-space representation as:

$$\begin{gathered} \dot{\mathbf{\text{x}}}=A\mathbf{\text{x}}+B\mathbf{\text{u}}, \\ \mathbf{\text{y}}=C\mathbf{\text{x}}, \\ A=\left[\begin{array}{cccc}0& 1& 0& 0\\ -\frac{k_1}{m}& -\frac{c_1}{m}& 0& 0\\ 0& 0& 0& 1\\ 0& 0& -\frac{k_2}{m}& -\frac{c_2}{m}\end{array}\right], \\ B=\left[\begin{array}{cc}0& 0\\ \frac{\text{cos}\beta }{m}& -\frac{\text{sin}\beta }{m}\\ 0& 0\\ \frac{\text{sin}\beta }{m}& \frac{\text{cos}\beta }{m}\end{array}\right],C=\left[\begin{array}{llll}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right], \end{gathered}$$ (3)

where $\mathbf{\text{x}}={\left[{\mathbf{\text{x}}}_1,{\mathbf{\text{x}}}_2,{\mathbf{\text{x}}}_3,{\mathbf{\text{x}}}_4\right]}^T={\left[x_1,{\dot{x}}_1,x_2,{\dot{x}}_2\right]}^T\in {\mathbb{R}}^4$ represents the four state variables of the I²RIS robot gripper tip, namely, [pitch angle, pitch angular velocity, yaw angle, yaw angular velocity]^T; $\mathbf{\text{y}}={\left[{\mathbf{\text{x}}}_1,{\mathbf{\text{x}}}_3\right]}^T\in {\mathbb{R}}^2$ is the output of the system, which includes the pitch and yaw angles of the robot that we want to control; and $\mathbf{\text{u}}={\left[u_1,u_2\right]}^T={\left[f_1,f_2\right]}^T\in {\mathbb{R}}^2$ stands for the two control input signals, that is, the encoder values of the pitch and yaw motors, respectively.

Fig. 5:

Schematic of the MIMO mass-spring-damper model. $u_1$ and $u_2$ are the two control inputs (pitch motor and yaw motor encoder values); $x_1$ and $x_2$ represent the model outputs (the pitch and yaw angles of the robot tip).

To implement this model in system identification and control algorithms, we first discretized the continuous state-space equation in (3) using the fourth-order Runge–Kutta method as follows:

$$\begin{gathered} \mathbf{\text{x}}(k+1)={\mathbf{\text{A}}}_d(k)\mathbf{\text{x}}(k)+{\mathbf{\text{B}}}_d(k)\mathbf{\text{u}}(k) \\ {\mathbf{\text{A}}}_d=\frac{1}{24}{A}^4{T}_s^4+\frac{1}{6}{A}^3{T}_s^3+\frac{1}{2}{A}^2{T}_s^2+A{T}_s+I \\ {\mathbf{\text{B}}}_d=\frac{1}{24}{A}^{3}B{T}_s^4+\frac{1}{6}{A}^{2}B{T}_s^3+\frac{1}{2}AB{T}_s^2+B{T}_s \end{gathered}$$ (4)

2.4. Dual Extended Kalman Filter for Joint State–Parameter Estimation

Kalman Filters (KFs) [30] provide an optimal framework for state estimation in linear systems subject to Gaussian noise. The resulting estimates are optimal in both the Minimum Mean Square Error (MMSE) and Maximum A Posteriori (MAP) senses. Extended Kalman Filters (EKF) extend this framework to nonlinear systems of the following form [31]:

$$\begin{gathered} {\mathbf{\text{x}}}_{k+1}=F\left({\mathbf{\text{x}}}_k,{\mathbf{\text{u}}}_k,\mathbf{\text{w}}\right)+{\nu }_k \\ {\mathbf{\text{y}}}_k=H\left({\mathbf{\text{x}}}_k,\mathbf{\text{w}}\right)+{\eta }_k \end{gathered}$$ (5)

where $F(.)$ and $H(.)$ are nonlinear functions of dynamic systems, $\mathbf{\text{w}}$ is a set of unknown parameters of the system (in this work $\mathbf{\text{w}}=\left[k_1,c_1,k_2,c_2\right]$ representing the model parameters in (3)), and ${\nu }_k,{\eta }_k$ are process and observation (or measurement) noises, respectively [32, 33].

When direct access to the system state is unavailable, joint estimation of the state and unknown model parameters can be performed using a dual extended Kalman filter (dual-EKF) method [34]. The dual-EKF consists of two coupled EKFs operating in parallel: a state filter that estimates the system state given the current parameter estimate ${\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}_k$, and a parameter filter that updates the model parameters using the latest state estimate ${\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_k$. The dual-EKF method has three main steps: a prediction step, a correction step (for both states and parameters), and a recursive sensitivity propagation step [35].

2.4.1. Prediction Step

The parameter evolution is modeled as a stationary process [35]:

$${\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}_k^{-}={\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}_{k-1},$$ (6)

$${\mathbf{\text{P}}}_{{\mathbf{\text{w}}}_k}^{-}={\lambda }^{-1}{\mathbf{\text{P}}}_{{\mathbf{\text{w}}}_{k-1}},$$ (7)

where $\lambda$ is a forgetting factor.

Using the predicted parameter estimate, the state prediction is formulated as follows:

$${\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_k^{-}=F\left({\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_{k-1},{\mathbf{\text{u}}}_k,{\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}_k^{-}\right),$$ (8)

$${\mathbf{\text{P}}}_{{\mathbf{\text{x}}}_k}^{-}={\mathbf{\text{A}}}_{k-1}{\mathbf{\text{P}}}_{{\mathbf{\text{x}}}_{k-1}}{\mathbf{\text{A}}}_{k-1}^T+{\mathbf{\text{R}}}_{\nu },$$ (9)

where ${\mathbf{\text{A}}}_{k-1}$ is calculated by linearizing the process model as follows:

$${\mathbf{\text{A}}}_{k-1}={\left.\frac{\partial F\left(\mathbf{\text{x}},{\mathbf{\text{u}}}_k,{\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}_k^{-}\right)}{\partial \mathbf{\text{x}}}\right|}_{\mathbf{\text{x}}={\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_{k-1}}.$$ (10)

2.4.2. Correction Step: State Filter

The measurement-update equations for the state Kalman filter are formulated in the following equation:

$${\mathbf{\text{K}}}_k^{\mathbf{\text{x}}}={\mathbf{\text{P}}}_{{\mathbf{\text{x}}}_k}^{-}{\mathbf{\text{C}}}_k^T{\left({\mathbf{\text{C}}}_k{\mathbf{\text{P}}}_{{\mathbf{\text{x}}}_k}^{-}{\mathbf{\text{C}}}_k^T+{\mathbf{\text{R}}}_{\eta }\right)}^{-1},$$ (11)

$${\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_k={\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_k^{-}+{\mathbf{\text{K}}}_k^{\mathbf{\text{x}}}\left({\mathbf{\text{y}}}_k-H\left({\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_k^{-},{\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}_k^{-}\right)\right),$$ (12)

$${\mathbf{\text{P}}}_{{\mathbf{\text{x}}}_k}=\left(\mathbf{\text{I}}-{\mathbf{\text{K}}}_k^{\mathbf{\text{x}}}{\mathbf{\text{C}}}_k\right){\mathbf{\text{P}}}_{{\mathbf{\text{x}}}_k}^{-}.$$ (13)

where $R_{\nu }$ and $R_{\eta }$ are the covariances of the process noise ${\nu }_k$ and the observation noise ${\eta }_k$, respectively, and ${\mathbf{\text{C}}}_k$ is calculated by linearizing the measurement model as follows:

$${\mathbf{\text{C}}}_k={\left.\frac{\partial H\left(\mathbf{\text{x}},{\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}_k^{-}\right)}{\partial \mathbf{\text{x}}}\right|}_{\mathbf{\text{x}}={\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_k^{-}}.$$ (14)

2.4.3. Correction Step: Parameter Filter

The measurement-update equations for the parameter Kalman filter are expressed in the following relations:

$${\mathbf{\text{K}}}_k^{\mathbf{\text{w}}}={\mathbf{\text{P}}}_{{\mathbf{\text{w}}}_k}^{-}{\left({\mathbf{\text{C}}}_k^{\mathbf{\text{w}}}\right)}^T{\left({\mathbf{\text{C}}}_k^{\mathbf{\text{w}}}{\mathbf{\text{P}}}_{{\mathbf{\text{w}}}_k}^{-}{\left({\mathbf{\text{C}}}_k^{\mathbf{\text{w}}}\right)}^T+{\mathbf{\text{R}}}_{\mathbf{\text{w}}}\right)}^{-1},$$ (15)

$${\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}_k={\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}_k^{-}+{\mathbf{\text{K}}}_k^{\mathbf{\text{w}}}{\mathit{\epsilon }}_k,$$ (16)

where ${\mathit{\epsilon }}_k$ is the measurement error expressed as:

$${\mathit{\epsilon }}_k={\mathbf{\text{y}}}_k-H\left({\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_k^{-},{\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}_k^{-}\right),$$ (17)

and ${\mathbf{\text{C}}}_k^{\mathbf{\text{w}}}$ is the parameter sensitivity matrix, which is required for parameter estimation and is defined as follows:

$${\mathbf{\text{C}}}_k^{\mathbf{\text{w}}}=-{\left.\frac{\partial {\mathit{\epsilon }}_k}{\partial \mathbf{\text{w}}}\right|}_{{\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}_k^{-}}={\left.{\mathbf{\text{C}}}_k\frac{\partial {\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_k^{-}}{\partial \mathbf{\text{w}}}\right|}_{{\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}_k^{-}}.$$ (18)

2.4.4. Recursive Sensitivity Propagation

Because the state estimate depends recursively on $\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}$, the sensitivity $\partial {\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_k^{-}/\partial \stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}$ is propagated as:

$$\frac{\partial {\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_{k+1}^{-}}{\partial \stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}=\frac{\partial F(\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}},\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}})}{\partial {\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_k}\frac{\partial {\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_k}{\partial \stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}+\frac{\partial F(\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}},\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}})}{\partial {\stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}_k},$$ (19)

$$\frac{\partial {\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_k}{\partial \stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}=\left(\mathbf{\text{I}}-{\mathbf{\text{K}}}_k^{\mathbf{\text{x}}}{\mathbf{\text{C}}}_k\right)\frac{\partial {\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_k^{-}}{\partial \stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}+\frac{\partial {\mathbf{\text{K}}}_k^{\mathbf{\text{x}}}}{\partial \stackrel{\mathbf{ˆ}}{\mathbf{\text{w}}}}\left({\mathbf{\text{y}}}_k-{\mathbf{\text{C}}}_k{\stackrel{\mathbf{ˆ}}{\mathbf{\text{x}}}}_k^{-}\right).$$ (20)

in which the second term of (20) can be set to zero assuming that the Kalman gain ${\mathbf{\text{K}}}_k^{\mathbf{\text{x}}}$ is independent of the parameter $\mathbf{\text{w}}$ [35].

2.5. Discrete-Time Smooth-MPPI Controller

After identifying the parameters of the discrete-time dynamical model of the robot (${\mathbf{\text{x}}}_{k+1}={\mathbf{\text{A}}}_d{\mathbf{\text{x}}}_k+{\mathbf{\text{B}}}_d{\mathbf{\text{u}}}_k$) using the dual-EKF method in the previous step, this section designs a model predictive path integral controller for the robot model. The advantage of the MPPI control algorithm is that it does not require the gradient of the cost function and can be applied to non-convex optimization problems [27]. However, the sampling-based nature of MPPI can lead to chattering when the sampled trajectories deviate from the optimal solution. To avoid chattering in real-time control implementation on the I²RIS robot, we smooth the weights of the sampled trajectories as well as the control command [36].

2.5.1. Cost Function

The cost function on a finite horizon $T$ is defined as follows:

$$S(\tau )=\sum _{t=0}^{T-1}\left[{\left({\mathbf{\text{x}}}_t-{\mathbf{\text{x}}}_{\text{target}}\right)}^{\top }Q\left({\mathbf{\text{x}}}_t-{\mathbf{\text{x}}}_{\text{target}}\right)+{\mathbf{\text{u}}}_t^{\top }R{\mathbf{\text{u}}}_t\right]+{\left({\mathbf{\text{x}}}_T-{\mathbf{\text{x}}}_{\text{target}}\right)}^{\top }{Q}_H\left({\mathbf{\text{x}}}_T-{\mathbf{\text{x}}}_{\text{target}}\right)$$ (21)

2.5.2. MPPI Control Law

To find the optimal control solution in the MPPI algorithm, we sample $K$ control sequences as $\delta {\mathbf{\text{u}}}_k~𝒩\left(0,{\Sigma }_u\right)$, simulate (estimate) potential trajectories up to a finite horizon $T$ (using the dual-EKF-optimized model of the robot) for these sampled control sequences, and compute the corresponding cost for each simulated trajectory. We then compute the weights of all sampled trajectories as follows [27]:

$${\rho }_k=\frac{\text{exp}\left(-\frac{S_k-{\text{min}}_i{S}_i}{\lambda }\right)}{{\sum }_{j=1}^K\text{exp}\left(-\frac{S_j-{\text{min}}_i{S}_i}{\lambda }\right)}$$ (22)

where ${\text{min}}_i{S}_i$ is the minimum cost of all sampled trajectories, and subtracting the cost of each trajectory from the minimum cost guarantees that the sampling weight of at least one trajectory remains non-negligible [36].

The optimal MPPI control solution can be calculated as a weighted average of the sampled control sequences:

$${\mathbf{\text{u}}}_{\text{MPPI}}={\mathbf{\text{u}}}_{\text{nominal}}+\sum _{k=1}^K{\rho }_k\delta {\mathbf{\text{u}}}_k$$ (23)

where ${\mathbf{\text{u}}}_{\text{nominal}}$ is a nominal PID controller with proportional, derivative, and integral gains $K_P,K_D,K_I\in {\mathbb{R}}^{2\times 4}$. Then, ${\mathbf{\text{u}}}_{\text{MPPI}}\left[0\right]$, as the first element of the control value in the control horizon ($t=0,\dots ,T-1$), is typically applied to the system to move it one step forward. These procedures (from (21) to (23)) are repeated at each time step (calculate a new ${\mathbf{\text{u}}}_{\text{MPPI}}\left[0\right]$ and apply it to the real system) until the system state is close enough (within a predefined threshold) to the target state. From a technical viewpoint, to avoid chatter and handle actuator saturation, additional smoothing and clipping to the current MPPI control signal are recommended before applying it to the actual robotic system.

2.5.3. Control Smoothing and Actuator Saturation

To remove the chattering effects of the MPPI control signal (${\mathbf{\text{u}}}_{\text{MPPI}}\left[0\right]$), we apply an exponential moving average filter, further smoothing the control action using the following relation:

$${\mathbf{\text{u}}}_{\text{f}}(t)=\alpha {\mathbf{\text{u}}}_{\text{MPPI}}\left[0\right](t)+(1-\alpha ){\mathbf{\text{u}}}_{\text{f}}(t-1)$$ (24)

where ${\mathbf{\text{u}}}_{\text{f}}(t)$ and ${\mathbf{\text{u}}}_{\text{f}}(t-1)$ are the filtered control values at the current and previous time steps, respectively, and $0<\alpha <1$ is a constant filter parameter $(\alpha =0.05)$. Then, to avoid saturating the motors and over-tensioning the robot cables, we clipped the filtered control signal to keep it within actuator limits:

$${\mathbf{\text{u}}}_{\text{final}}=\text{clip}\left({\mathbf{\text{u}}}_{\text{f}}(t),{\mathbf{\text{u}}}_{\text{min}},{\mathbf{\text{u}}}_{\text{max}}\right)$$ (25)

where ${\mathbf{\text{u}}}_{\text{max}}=1300$ and ${\mathbf{\text{u}}}_{\text{min}}=-1300$ are the maximum and minimum values for the motors’ encoder values. Finally, the control signal, ${\mathbf{\text{u}}}_{\text{final}}$, is applied to the I²RIS motors.

3. Experiments

3.1. Experimental Setup

The I²RIS robot has two degrees of freedom, enabling motion in both pitch and yaw, with a range of approximately ±45°. It has a highly compact distal section, measuring 0.9 mm in diameter and 3 mm in length, and is equipped with a miniature gripper at its end-effector. Actuation is achieved using four tendon wires (0.15 mm diameter, SUS 304) and two DC servo motors (DCX08M EB KL 4.2V, Maxon Motor Inc.), each coupled with a gearbox (GPX08 A, gear ratio 64:1) and magnetic encoder (ENX 8 MAG, 256 pulses per revolution).

The complete data collection setup for the I²RIS robot and the interconnections between components are shown in Fig. 6. Two digital microscopes (AD407, Andonstar) are positioned orthogonally to the robot’s primary motion planes: the top-view microscope monitors pitch motion, while the side-view microscope captures yaw motion [27].

Fig. 6:

Experimental setup. The workstation sends actuation commands to the controller to drive the robot. After the motion is completed, the cameras capture images. The workstation records both the camera images and the issued command data.

All microscopes and motor controllers (EPOS 24/2, Maxon Motor Inc.) are connected to a laptop computer (with Ubuntu 20.04), which manages both command transmission to the actuation system and data acquisition from sensors. Real-time image capture and storage are performed using the OpenCV Python library. Communication between the workstation, controllers, and motors is established via USB serial interfaces implemented through the manufacturer’s C++ API and the rospy Python library, enabling approximate synchronization of data streams.

To measure the pitch and yaw angles, a cube with ArUco markers is mounted at the tip of the I²RIS robot. The markers are detected in the image streams to reconstruct the robot’s orientation and tip position over time.

3.2. Experimental Procedures

To analyze the kinematics of the I²RIS robot, we investigated the relationship between the encoder counts of the two motors and the resulting pitch and yaw bending angles at the robot tip. The home position is when the I²RIS robot is straight (pitch = yaw = 0°). The experimental procedure for the kinematic model characterization involves actuating the two motors, causing the I²RIS robot to bend along the pitch and yaw angles. This procedure is done for three types of input signals for the motors, including constant-amplitude sinusoidal, sawtooth, and decaying sinusoidal signals at amplitudes of 10°, 20°, and 30°, and frequencies of 0.05 Hz, 0.1 Hz, and 0.15 Hz. For each signal, we collected the input-output data for 5 alternating cycles.

The data collection process runs the image capture script and the actuation unit control script in parallel, allowing the time-synchronized storage of data in a .csv file and real-time images in a local folder. After completion of a predefined number of back-and-forth motion cycles, images are subsequently processed by a Python script for angle calculation, which employs the OpenCV ArUco library to compute the relative angle for each image. The processed data, including angles derived from images captured by both microscopes, encoder readings, and time frames, are stored, which are then used in the modeling phase.

3.2.1. Parameter Estimation

The collected datasets are used to estimate parameters of the mass-spring-damper model that describes the coupled motion between the motor encoder counts (inputs) and the resulting pitch and yaw angles (outputs). A Python-based implementation of the Dual-EKF is used for parameter estimation. This approach produces temporal trends of estimated parameters, a representative example of which is shown in Fig. 7. These trends typically converge to values that approximate the system’s actual physical parameters.

Fig. 7:

An example of parameters estimation for the MIMO model in (3) using dual-EKF algorithm. $k_1,c_1$ are the stiffness and damping coefficients for the pitch motion, and $k_2,c_2$ are the stiffness and damping coefficients for the yaw motion. Here, the term count refers to the measured motor encoder count. Although it does not represent a physical torque, it is treated as a proxy for the applied actuation effort during the parameter optimization stage.

Due to inherent model mismatch—primarily caused by unmodeled hysteresis in the actuation mechanism—the initially estimated parameters are further refined through manual tuning. Each tuned parameter set is validated against testing data by comparing the root mean square error (RMSE) between the model’s predictions and the experimental measurements, and the set that minimizes the RMSE is selected.

4. Results

This work addresses the coupling motion problem in the I²RIS robot kinematics, enabling accurate, repeatable control performance. This section presents the experimental results for both an offline system identification phase and an online trajectory tracking control implementation. During the offline system identification phase, the parameters of an MIMO-MSD dynamic system are identified using a dual-EKF algorithm to characterize the robot’s kinematics behavior. Then, a smooth MPPI control algorithm is designed for the optimized model and implemented on the I²RIS robot, and tested in a tracking control problem with several trajectories that include straight and curved lines, such as a rectangle, an oval, and an infinity sign, following strategies comparable to those explored in [37, 38]. Of note, the manipulator operates within a hemispherical three-dimensional workspace, where each ($x,y$) position corresponds to a unique $z$-coordinate; consequently, reference trajectories are specified as two-dimensional projections onto the $x-y$ plane. The performance of the MPPI controller is also compared with a classical LQR controller in terms of RMSE and mean absolute error (MAE) for both pitch and yaw degrees of freedom.

4.1. Dual-EKF System Identification

For the model identification step, the robot motors were activated by sinusoidal velocity signals as the system input. The system’s output, i.e., the pitch and yaw bending angles of the snake robot’s gripper, was measured using two microscopes that captured the coordinate frames of two AruCo markers attached to a 3D-printed cube mounted on the robot tip. (see Fig. 6).

The MIMO model parameters were optimized offline using the dual-EKF algorithm. The modeling results of the implemented model were evaluated using sawtooth motion (Fig. 8) and decaying sine wave motion (Fig. 9), both with an amplitude of 1300 encoder counts and a frequency of 0.1 Hz. During the sawtooth motion, both motors were actuated. In contrast, only one motor was actuated during the decaying sine wave motion, meaning that only the motion of the corresponding motor was recorded. The model fitting results are summarized in Table 1. The results demonstrate that the model accurately predicts both single- and dual-motor motions for pitch and yaw angles.

Fig. 8:

Dual-EKF model evaluation with sawtooth motion.

Fig. 9:

Dual-EKF model evaluation with decaying sinusoidal inputs.

Table 1:

Comparison between predicted angles from the model and the actual data.

Motion	Error	Pitch (deg)	Yaw (deg)
Sawtooth	RMSE	5.43	3.78
	MAE	4.24	2.61
Decaying sine wave	RMSE	3.66	2.59
	MAE	2.89	2.24

4.2. Control Implementation on I²RIS

In this experiment, we implemented the proposed MPPI controller on the MIMO-MSD dynamic model of the I²RIS robot, which was trained using the dual-EKF algorithm in the previous step.

The tracking results for each controller across different designated trajectories, represented in the space of bending angles, are shown in Fig. 10. As shown in Fig. 10(a), the MPPI controller demonstrates superior tracking performance for the desired rectangular trajectory. In addition, MPPI-generated trajectories exhibit higher repeatability, consistently retracing the same path across multiple tracking cycles. In contrast, trajectories produced by the LQR controller appear less consistent, with noticeable cycle-to-cycle variations and gradual drift.

Fig. 10:

Tracking results of bending angles of (a) rectangular trajectory (b) oval trajectory (c) infinity-sign trajectory with LQR and MPPI controller.

During tracking of the oval trajectory (Fig. 10(b)), abrupt discontinuities were observed in the yaw motion of the robot at each cycle (second row of Fig. 11(b)), whereas the pitch motion closely and smoothly followed the desired reference (first row of Fig. 11(b)). These yaw discontinuities resulted in locally sharp corners in the end-effector path when executing oval or circular trajectories. Such behavior may be caused by the slippage of the rolling disks due to an imperfect curved contact surface (manufacturing issue) or by mechanical assembly inaccuracies. In particular, improper sequencing of the rolling disks—where consecutive disks are intended to be oriented with a 90° rotation about the shaft axis to ensure smooth pitch–yaw coupling—could introduce sudden motion discontinuities in the affected degree of freedom. Deviations from this intended assembly pattern may therefore explain the observed yaw jumps.

Fig. 11:

Tracking results of pitch and yaw angles and the relative errors of (a) rectangular trajectory (b) oval trajectory (c) infinity-sign trajectory with LQR and MPPI controller.

Overall, the MPPI controller exhibits better performance in tracking the desired trajectories. The detailed tracking performance, decomposed into pitch angle, yaw angle, and their respective errors, is presented in Fig. 11. The corresponding error analyses and $p$-values are summarized in Table 2. For example, on the oval trajectory, the MPPI controller reduced the pitch RMSE from 2.78° ± 1.06° (for LQR) to 2.17° ± 0.40° and reduced the pitch MAE from 2.24° ± 0.97° to 1.55° ± 0.31°. Similarly, for the infinity-sign path, MPPI achieved a yaw MAE of 1.48° ± 0.35°, compared to 1.75° ± 0.63° with LQR. During tracking of the infinity-shaped trajectory (Fig. 10(c)), a systematic shift between the desired path and the robot tip trajectory can be observed. This deviation is mainly due to the nature of the current open-loop controller, where no human-in-the-loop or sensory feedback is available to correct the robot motion online. Additional discrepancies may also result from hysteresis effects in the mechanical structure. Despite these factors, the proposed modeling approach combined with the open-loop control strategy is able to capture the overall shape and orientation of the desired trajectory within an acceptable error range. This indicates that the controller effectively resolves coupling-induced motion effects, even in the absence of feedback, and provides a foundation for future closed-loop implementations.

Table 2:

Comparison of the MPPI and LQR controllers’ performance in terms of tracking desired pitch and yaw signals for square, circle, and infinity trajectories.

Trajectory	Pitch (deg)						Yaw (deg)
	RMSE			MAE			RMSE			MAE
	LQR	MPPI	$p$	LQR	MPPI	$p$	LQR	MPPI	$p$	LQR	MPPI	$p$
Rectangle	2.54±0.78	2.65±1.08	0.8748	1.82±0.67	1.88±0.83	0.9143	1.66±0.37	1.85±0.23	0.4228	1.34±0.24	1.29±0.11	0.7232
Oval	2.78±1.06	2.17±0.40	0.2325	2.24±0.97	1.55±0.31	0.1480	2.81±0.68	2.88±0.62	0.8559	2.35±0.72	2.30±0.49	0.8913
Infinity	2.83±1.14	2.47±1.34	0.4861	2.45±1.13	1.95±1.29	0.3237	2.19±0.68	1.83±0.34	0.1202	1.75±0.63	1.48±0.35	0.2115

5. Discussions and Future Works

The MPPI controller performs superiorly on the oval and infinity-shaped trajectories, which are nonlinear, thereby improving in the control accuracy. In comparison, the LQR controller yields more minor errors for the rectangular trajectory, such as a yaw RMSE of 1.66 ± 0.37 versus 1.85 ± 0.23 for MPPI. This outcome is consistent with the expected characteristics of each controller, where LQR performs better for linear motions, and MPPI is more effective in handling nonlinear dynamics [39, 40].

Some sources of tracking error include mechanical nonlinearities such as hysteresis and cable friction, and a lack of state feedback. Hysteresis in the cable actuation mechanism contributes significantly to tracking inaccuracy. Due to the elastic properties of the cables, motion reversal introduces a lag or dead zone that prevents the system from immediately following the command signals, resulting in a hysteresis behavior in the I²RIS robot’s tip motion.

One limitation of the current controller for the I²RIS trajectory tracking is the lack of state feedback, due to the small size of the robot, which leaves no space for integrating embedded sensing capabilities such as FBG and EM tracking sensors. To resolve this issue, we plan to use vision-based shape estimation and reconstruction using an iOCT microscope and deep learning methods. Another limitation is the lack of hysteresis compensation in the current robot model. For future work, we plan to improve the kinematics model of the I²RIS robot, taking into account structural nonlinearities such as hysteresis and cable friction.

This study is also limited to open-loop control and is intended to evaluate whether the proposed modeling and control framework can resolve coupling-induced motion effects and reliably track the orientation of desired trajectories, thereby assessing system controllability. While the controller successfully achieved this objective, no feedback was incorporated to close the control loop. In particular, the current implementation does not include human-in-the-loop supervision to adapt the robot tip trajectory during execution. Future work will focus on closing the control loop within a teleoperation framework, enabling expert user supervision during trajectory tracking. This will allow real-time trajectory adjustment to reduce the drift observed in the present study (see Fig. 10-c) and improve precision during execution. Additionally, ERM peeling requires delaminating a membrane along a spiral trajectory [41], which requires a three-dimensional motion, including pitch, yaw, and axial retraction of the I²RIS robot. Since the current platform does not provide active retraction, we plan to investigate spiral trajectories in future studies by integrating the I²RIS robot with the Steady-Hand Eye Robot to provide the required axial motion. This extension will again necessitate human-in-the-loop control to allow expert-guided trajectory update during membrane peeling. We plan to conduct membrane peeling experiments on artificial membranes and tissue phantoms, following approaches similar to [42, 43]. Finally, future efforts will incorporate image-based reconstruction of the snake robot shape, similar to [44], to provide feedback for online trajectory correction. Such sensing is expected to enable this robotic platform to perform autonomously. In the absence of sensory or human feedback, some trajectory deviation is anticipated; nevertheless, the primary contribution of this work is demonstrating that the proposed controller can effectively mitigate coupling effects and follow commanded trajectories in terms of orientation and direction.

6. Conclusions

In this work, we developed a MIMO mass-spring-damper model to characterize the motion coupling between the pitch and yaw angles of a snake robot gripper. The model was optimized using a dual extended Kalman filter (Dual-EKF) for simultaneous parameter and state estimation. With parameters identified from experimental data, we implemented a model predictive path integral (MPPI) controller and compared its trajectory tracking performance with that of a classical LQR controller. The results demonstrate the effectiveness of the Dual-EKF modeling approach and highlight the advantages of MPPI in handling coupled dynamics for improved trajectory tracking.