LOS算法相当于pid控制中的p控制,根据与目标轨迹的横向偏差来计算艏向角
%坐标点
x=[0,5,10,28,40,50,40,80];
y=[0,5,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)];
%[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');