Adaptive Kalman filter algorithm (AKF) is utilized to improve the performance of the robot's speed and heading angle, which can eliminate the system state noise and error, generated by the sudden change of formation in the multi-robot leader-follower system. The approach guarantees that the robots can reach the desired position quickly and accurately and keep a predetermined formation. The motion parameters are adjusted to maintain the formation configuration and help to complete the predetermined route with a desirable formation, providing an effective efficiency to the robot collaboration system. Player/Stage is used as the simulation platform to verify this approach, and the results have demonstrated the effectiveness of the proposed optimization method of formation control for multi-robot system.