Probe is an important component of the optical coherence tomography (OCT) system. Electrothermally-actuated micro-electromechanical (MEMS) mirrors are especially suitable for OCT endoscopic imaging due to their large scan angle, low driving voltage and high fill factor. However, endoscopic OCT imaging quality would be seriously affected by the nonlinear scanning characteristics of electrothermal MEMS mirrors as well as the distortion due to the need of placing the MEMS mirror on a 45° slope. In this work, an open-loop control method based on the transfer function of the MEMS mirror is proposed to optimize the overall optical trajectory scanned on the sample under a MEMS OCT probe. The transfer function of the electrothermal MEMS mirror is established and the relation between the MEMS mirror’s tilt angle and the final scanning trajectory on the sample is calculated. A PID control model based on the transfer function is developed and employed to generate the driving voltage signals that can be applied to the MEMS mirror for obtaining undistorted scanning trajectories on the sample. Experiments show that the proposed method can correct the scanning nonlinearity and the distortion without adding extra hardware. For instance, the linear scanning ratio is improved from 72% up to 98%; the fan-shaped distortion is reduced by a factor 3–5. This method is also applied to a swept-source OCT system and the image distortions are compensated effectively.