To further improve machining accuracy under the constrained conditions of multi-axis dynamic response, current research is focusing on the control of CNC machine toolpaths, with contour error as the target. While extant approaches analyze positions solely at PLC sampling nodes, they neglect inter-sample toolpath fluctuations induced by velocity deviations. This paper proposes a servo state-based polynomial interpolation model predictive control that predicts real-time toolpath behavior by utilizing servo axis states. The polynomial interpolation of servo states (e.g., position/velocity feedback) enables high-fidelity toolpath prediction between PLC nodes, overcoming the limitation imposed by the sampling gap. Experimental validation on a five-axis motion platform with S-shaped trajectories demonstrates that, without extending the prediction horizon of the model predictive control method, the proposed method reduces contour error by approximately 20% at the tool tip and 40% in tool orientation, while decreasing contour error fluctuations by around 60% compared to conventional model predictive control method.