Trajectory Tracking Control for Flexible-Joint Robot Based on Extended Kalman Filter and PD Control

Fast and robust localization of surgical array using Kalman filter

PROBLEM

Intraoperative tracking of surgical instruments is an inevitable task of computer-assisted surgery. An optical tracking system often fails to precisely reconstruct the dynamic location and pose of a surgical tool due to the acquisition noise and measurement variance. Embedding a Kalman filter (KF) or any of its extensions such as extended and unscented Kalman filters (EKF and UKF) with the optical tracker resolves this issue by reducing the estimation variance and regularizing the temporal behavior. However, the current KF implementations are computationally burdensome and hence takes long execution time which hinders real-time surgical tracking.

AIM

This paper introduces a fast and computationally efficient implementation of linear KF to improve the measurement accuracy of an optical tracking system with high temporal resolution.

METHODS

Instead of the surgical tool as a whole, our KF framework tracks each individual fiducial mounted on it using a Newtonian model. In addition to simulated dataset, we validate our technique against real data obtained from a high frame-rate commercial optical tracking system. We also perform experiments wherein a diffusive material (such as a drop of blood) blocks one of the fiducials and show that KF can substantially reduce the tracking error.

RESULTS

The proposed KF framework substantially stabilizes the tracking behavior in all of our experiments and reduces the mean-squared error (MSE) by a factor of 26.84, from the order of [Formula: see text] to [Formula: see text] mm[Formula: see text]. In addition, it exhibits a similar performance to UKF, but with a much smaller computational complexity.

Trajectory Tracking Control for Flexible-Joint Robot Based on Extended Kalman Filter and PD Control

The robot arm with flexible joint has good environmental adaptability and human robot interaction ability. However, the controller for such robot mostly relies on data acquisition of multiple sensors, which is greatly disturbed by external factors, resulting in a decrease in control precision. Aiming at the control problem of the robot arm with flexible joint under the condition of incomplete state feedback, this paper proposes a control method based on closed-loop PD (Proportional-Derivative) controller and EKF (Extended Kalman Filter) state observer. Firstly, the state equation of the control system is established according to the non-linear dynamic model of the robot system. Then, a state prediction observer based on EKF is designed. The state of the motor is used to estimate the output state, and this method reduces the number of sensors and external interference. The Lyapunov method is used to analyze the stability of the system. Finally, the proposed control algorithm is applied to the trajectory control of the flexible robot according to the stability conditions, and compared with the PD control algorithm based on sensor data acquisition under the same experimental conditions, and the PD controller based on sensor data acquisition under the same test conditions. The experimental data of comparison experiments show that the proposed control algorithm is effective and has excellent trajectory tracking performance.