%坐标点 x=[0,10,28,40,50,40,80]; y=[0,10,20,30,60,80,100]; len=length(x); trace=[x',y']; %船舶初始位置 ship_point=[0,5]; %船舶速度 ship_velocity=[1,1]; delta=1; los_step=0.8; point_storage=[]; angle_storage=[]; for i=1:len-1 % 使用北东坐标系 err_y=trace(i+1,1)-trace(i,1); err_x=trace(i+1,2)-trace(i,2); whole_angle=atan2(err_y,err_x); trans=[cos(whole_angle),-sin(whole_angle);sin(whole_angle),cos(whole_angle)]; rang=(ship_point(1)-trace(i+1,1))^2+(ship_point(2)-trace(i+1,2))^2; n=1; while rang>delta^2 appendage_coordinate=trans'*[ship_point(2)-trace(i,2);ship_point(1)-trace(i,1)]; expect_path_angle=whole_angle-atan(appendage_coordinate(2)/delta); ship_next_point=[ship_point(1)+sin(expect_path_angle)*los_step,ship_point(2)+cos(expect_path_angle)*los_step]; point_storage=[point_storage;ship_point]; ship_point=ship_next_point; rang=sqrt((ship_point(1)-trace(i+1,1))^2+(ship_point(2)-trace(i+1,2))^2); n=n+1; end end figure(1) plot(trace(:,1),trace(:,2),'b.-'); hold on; plot(point_storage(:,1),point_storage(:,2),'r');

这段代码是一个路径跟踪的 Matlab 程序，主要实现的功能是在一个已知的路径上，让一艘船舶按照一定的速度和方向进行行驶，直到到达下一个路径点。

程序中，首先定义了路径上的一些坐标点 `x` 和 `y`，用 `trace` 变量存储了这些点的坐标。然后定义了船舶的初始位置 `ship_point` 和速度 `ship_velocity`。接下来，程序通过计算路径上两个相邻点之间的角度，来确定船舶应该朝向哪个方向。在每个路径点之间，程序使用了一个 while 循环，不断计算船舶下一个位置，直到船舶到达下一个路径点。在这个 while 循环中，程序首先计算了船舶当前位置相对于路径点的坐标差，然后通过计算船舶期望的行驶方向，来确定船舶下一个位置。每次计算出新的位置后，程序将该位置存储在 `point_storage` 变量中。最后，程序使用 `plot` 函数将路径和船舶的行驶轨迹绘制出来。

需要注意的是，程序中使用了北东坐标系，因此在计算船舶位置时，需要进行相应的坐标转换。