LOS曲线路径跟踪

一、参考文献

Path Following for Marine Surface Vessels

 二、实现效果

三、源代码

%% main
%parameter curve---sin
t=0:0.1:20*pi;
x=t;
y=4*sin(0.1*t);
curve_value=0;

%parameter ship
ship_position=[0;2;pi/4];
ship_velocity=[1;0;0];
delta_t=0.1;
ship_position_storage=[];
%circle
iteration=0;
while iteration~=500
    ship_position_storage=[ship_position_storage,ship_position];
    [curve_update,expect_angle]=los(curve_value,ship_position,ship_velocity);
    curve_value=curve_value+curve_update*delta_t;
    ship_position=[ship_position(1)+delta_t*cos(ship_position(3));...
                   ship_position(2)+delta_t*sin(ship_position(3));...
                   expect_angle];
    iteration=iteration+1;
end

%draw
plot(x,y);
hold on
plot(ship_position_storage(2,:),ship_position_storage(1,:),'r-');

%% function
function [curve_update,expect_angle]=los(curve_value,ship_position,ship_velocity)
%los parameter
delta=1;
gamma=0.8;
%algorithm
curve_position=[4*sin(0.1*curve_value);curve_value;pi/2-atan(0.4*cos(0.1*curve_value))];
d_curve_position=[0.4*cos(0.1*curve_value);1];
rotation_matrix=[cos(curve_position(3)),-sin(curve_position(3));...
    sin(curve_position(3)),cos(curve_position(3))];
error=rotation_matrix'*(ship_position(1:2)-curve_position(1:2));
%output
curve_update=(norm(ship_velocity(1:2))*cos(ship_position(3)-curve_position(3))+gamma*error(1))/...
              norm(d_curve_position);
expect_angle=curve_position(3)+atan(-error(2)/delta);
end

四、原理

1、参数化路径——sin曲线

2、误差分析

期望点坐标系下误差

 李亚普洛夫稳定性

 3、控制器设计