Abstract: The target velocity jump has caused the low-velocity stability and fuel economy in the whole process of autonomous agricultural machinery operation. In this study, optimal velocity planning was proposed for the field operation of agricultural machinery in the constantly variable transmission (CVT) tractor. The global and local planning objectives and constraints were collected from the autonomous agricultural machinery. The minimum-jerk polynomial velocity planning was then designed using the Bellman optimality principle and optimal control theory. The velocity-following control system was also developed to realize the stable velocity cruise of autonomous agricultural machinery in the field. The velocity planning was modelled as multi-stage decision-making, according to the time series. The optimal decision was achieved in the jerk optimal control at each stage to realize the state transition. Bellman optimality principle and optimal control theory were selected to design the minimum-jerk polynomial velocity planning. The minimum jerk cost function was subject to hard constraints, such as the maximum and minimum velocity, acceleration, and jerk. The soft constraints were utilized to realize the driving task in the shortest possible time. Numerical solutions were used to reduce the difficulty of the model. Polynomial piecework fitting was generated to fully meet the smooth and continuous optimal speed reference curve under the hard constraints. The performance of the control system was significantly improved to prevent the velocity jump and varying conditions. The velocity stability and fuel economy were realized in the CVT tractor. The feasible solution space was obtained in the numerical solution. The end states of tractor motion were taken to heuristically search for the optimal solution under hard and soft constraints. The numerical solution was obtained after sampling. The unexecuted portion of the previous cycle’s solution in each planning cycle was utilized to achieve the time consistency of the unmanned tractor. The executive layer controller of the unmanned tractor usually tracked the reference curve generated by the planning algorithm with high accuracy and responsiveness. However, the replanning was performed to discard the remaining part of the previous solution, when the motion state deviated significantly from the target trajectory under external disturbances. The U-turn field experiment showed that the mean absolute error (MAE) and the root mean square error (RMSE) of speed decreased by 42.31%, and 50.75%, respectively, compared with the control group. The average absolute value and the variance of acceleration decreased by 8.26%, and 16.36%, respectively, while their jerk decreased by 7.65% and 14.23%, respectively. The variance of engine speed, torque percentage, instantaneous fuel consumption, and total fuel consumption decreased by 63.36%, 60.26%, 71.25%, and 2.37%, respectively. The straight-line navigation velocity adjustment experiment showed a similar optimization trend. The MAE and the RMSE of speed decreased by 9.45%, and 11.14%, while the average absolute value and variance of acceleration decreased by 6.03% and 13.68%, respectively. The average absolute value and variance of jerk decreased by 1.55% and 3.59%, respectively. The variance of engine speed decreased by 31.78%, the variance of engine torque percentage decreased by 25.13%, and the variance of instantaneous fuel consumption decreased by 31.82%. The total fuel consumption decreased by 2.48%. The stability of speed regulation was significantly improved to reduce fuel consumption during operation. The smooth control of velocity switching can fully meet the requirement for autonomous CVT tractors.