Path planning and collision avoidance during autonomous navigation in unknown environments is a crucial issue for unmanned surface vehicles (USVs). This paper improves the traditional D* Lite algorithm and achieves multi-goal path planning and collision avoidance for USVs in unknown and complex environments. By expanding the adjacent search range and setting a safe distance for USVs, we solve the issue of limited steering maneuverability in USVs with fewer DOF during autonomous navigation. We propose an approach to optimize the planned path during navigation by comparing the estimated distance with the actual distance between the current waypoint and the goal waypoint. A minimum binary heap is used to optimize the priority queue of the D* Lite and significantly reduce the path search time. Simulation results show that the improved D * Lite can significantly reduce the path planning time, optimize the planned path and solve the issue of limited steering maneuverability in USVs. We apply the algorithm to a real USV for further tests. Experimental results show that the USV can plan an optimized path while avoiding both static and dynamic obstacles in complex environments with a safe distance during autonomous navigation.