LOS循迹算法matlab实现

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');