In the process of formation control of multi-heterogeneous USVs, the problems of response delay and inertia factors, as well as different control parameters of heterogeneous USVs will be encountered. Based on the Leader-follower algorithm, this paper introduces two innovative improvement. On the one hand, a response delay and inertia control item is added to the algorithm model, which make it more suitable for the engineering application of the USVs formation control. On the other hand, based on Lyapunov stability theory, a path-following control law based on position and course tracking has been designed for the formation control of multi-heterogeneous USVs, then the formation control problem is transformed into a path tracking problem, making it easy to adjust the control parameters of different dynamic characteristics of unmanned boats separately, improve the control efficiency of parameter optimization in practical engineering. Finally the improved algorithm and software is applied to the control of multi-heterogeneous USVs in lake, and the effectiveness and rationality of the improved algorithm and control software is verified.