Tremor Estimation and Removal in Robot-Assisted Surgery Using Lie Groups and EKF

摘要

Summary This paper aims at estimating the tremor torque using extended Kalman filter (EKF) applied to a two-link 3-DOF robot with nonlinear dynamics modelled using Lie-group and Lie-algebra theory. Later, it is generalised to d number of links with ( d + 1) -DOF. The configuration of each link at any time is described by its rotation relative to the preceding link. Using this formulation, an elegant formula for the kinetic energy of the ( d + 1) -DOF system is obtained as a quadratic form in the angular velocities with coefficients being highly nonlinear trigonometric functions of the angles. Properties of the Lie algebra generators and the Lie adjoint map are used to arrive at this expression. Further, the gravitational potential energy and the torque potential energy are expressed as nonlinear trigonometrical functions of the angles using properties of the SO(3) group. The input torque comprises a nonrandom intentional torque component and a highly nonlinear tremor torque component. The tremor torque is modelled as a stochastic differential equation (sde) satisfying Ornstein–Uhlenbeck (OU) process with diffusion and damping coefficients. Further, the tremor is treated as the disturbance. The Euler–Lagrange equations for the angles are derived. These form a system of sdes, and the EKF is used to get a more accurate disturbance estimate than that provided by the usual disturbance observer. The EKF is based on noisy angle measurements and yields as a bonus the angle and angular velocity estimates on a real-time basis. The parameters in the OU process model of the tremor torque, and parameters of the Fourier components of the intentional torque have also been estimated.