G01 trajectory is widely-adopted for CNC machining. The piecewise linear trajectory is geometrically discontinuous, bringing about drastic fluctuation of feedrate and acceleration, and causing low machining efficiency and quality. Many methods of trajectory smoothing are developed and applied in CNC system. However some shortcomings exist when these methods were applied in CNC machining. In the existing methods, the following requirements are not satisfied simultaneously: not less than G2 continuity, confined chord error, shape preservation, interpolating G01 points, spatial applicability, real-time performance of algorithm, free geometric form. Considering various requirements of CNC machining, this work develops an interpolation scheme with local repairing function to deal with the possible shape deficiencies of trajectory. The resulting tool path has G2 continuity and is optimized in bending energy under shape-preserving and chord error constraints. Combining with jerk-limited feedrate scheduling, a real-time tool path processing strategy is developed. The simulation demonstrates the proposed method eliminates shape deficiencies, and has apparent advantage in the reduction of contour error, curvature, bending energy and machining time, compared with Zhao’s and Fan’s methods. The experiment demonstrates the advantages of the proposed algorithm in improving the quality of machined surface and machining efficiency, compared with previous methods.