代码基本思路
1、路径为直线路径,已预先给出;
2、预先设定期望速度u,再由los艏向角引导程序求解期望艏向角;
3、由于为欠驱船舶控制量只有推进力和转向力矩,所以pid控制模块依据上述期望速度和期望艏向角求解出控制量。
%% initial paramter
position=[0;5;pi/4];
condition=[0;0;0];
force=[0;0;0];
predict_velocity=2;
%pid paramter
k_p=diag([6000,0,1000]);
k_i=diag([2,0,0]);
k_d=diag([200,0,35000]);
err_current=[0;0;0];
err_integral=[0;0;0];
delta_t=0.5;
%% trace
%坐标点
x=[0,100,200,400];
y=[0,100,300,400];
len=length(x);
trace=[x',y'];
point_storage=[position(1),position(2)];
angle_storage=position(3);
expect_angle_storage=0;
%los paramter
delta=1;
%% loop
i=1;
for j=1:2400
%los
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)];
appendage_coordinate=trans'*[position(2)-trace(i,2);position(1)-trace(i,1)];
rang=abs(trans'*[position(2)-trace(i+1,2);position(1)-trace(i+1,1)]);
predict_path_angle=whole_angle-atan(appendage_coordinate(2)/delta);
predict_path_angle=pi/2-predict_path_angle;
if rang(1)<delta
i=i+1;
err_integral=[0;0;0];
end
%% pid-->force
err_angle=(predict_path_angle-position(3))/pi*180;
err_velocity=predict_velocity-condition(1);
err_last=err_current;
err_current=[err_velocity;0;err_angle];
err_integral=err_integral+err_current;
force_before=[force(1:2,1);0];
force=k_p*err_current+k_i*err_integral+k_d*(err_current-err_last);
%condition & position
acceleration=ship_model(position,condition,force);
condition=condition+acceleration*delta_t;
angle=position(3);
J=[cos(angle),-sin(angle),0;
sin(angle),cos(angle),0;
0,0,1];
position=position+J*condition*delta_t;
if position(3)>pi
position(3)=position(3)-2*pi;
end
if position(3)<-pi
position(3)=2*pi+position(3);
end
point_storage=[point_storage;position(1),position(2)];
angle_storage=[angle_storage;position(3)];
expect_angle_storage=[expect_angle_storage;predict_path_angle];
end
%% draw
figure(1)
plot(trace(:,1),trace(:,2),'b.-');
hold on;
plot(point_storage(:,1),point_storage(:,2),'r');
figure(2)
len_angle=length(angle_storage);
angle_2=0:len_angle-1;
angle_storage=[angle_storage,angle_2'];
expect_angle_storage=[expect_angle_storage,angle_2'];
plot(angle_storage(:,2),angle_storage(:,1),'r');
hold on;
plot(expect_angle_storage(:,2),expect_angle_storage(:,1),'b');
%% simplified ship mathematical model
function acceleration=ship_model(position,condition,force)
%poistion=[x;y;angle]---condition=[u;v;r]--force=[F1;F2;M]
%ship basic paramter
ship_length=38;%m
ship_m=118000;%kg
m_11=120000;
m_22=177900;
m_33=63600000;%kg.m^2
d_11=21500;%kg/s
d_22=147000;%kg/s
d_33=8020000;%kg.m^2/s
angle=position(3);
u=condition(1);
v=condition(2);
%matrix
J=[cos(angle),-sin(angle),0;
sin(angle),cos(angle),0;
0,0,1];
M=[m_11,0,0;
0,m_22,0;
0,0,m_33];
C=[0,0,-m_22*v;
0,0,m_11*u;
m_22*v,-m_11*u,0];
D=[d_11,0,0;
0,d_22,0;
0,0,d_33];
acceleration=M\(-C*condition-D*condition+force);
end