空间曲线引导算法

一、参考文献

Principles of Guidance-Based Path Following in 2D and 3D

二、实现效果

三、源代码

clear;clc;close all;
%const parameter
delta_t=0.1;
%parameter curve--spiral line
a=10;
b=10;
h=-4;
t=0:0.1:10*pi;
curve_parameter=[a,b,h];

%north-east coordinate
% x=r*cos(t);
% y=r*sin(t);
y=a*cos(t);
x=b*sin(t);
z=h*t/(2*pi);

curve_value=0;

%parameter ship
position=[0,8,0,0,0,0]';%[x,y,z,Fai,Theta,Psi]
velocity=[2,0,0,0,0,0]';%[u,v,w,p,q,r]
position_storage=[];
%circle
iteration=0;
while iteration~=500
    position_storage=[position_storage,position];
    [curve_update,expect_azimuth_angle,expect_elevation_angle]=spital_los(curve_parameter,curve_value,position,velocity);
    curve_value=curve_value+curve_update*delta_t;
    rotation_azimuth_matrix=[cos(position(6)),-sin(position(6)),0;
                         sin(position(6)),cos(position(6)),0;
                         0,0,1];
    rotation_elevation_matrix=[cos(position(5)),0,sin(position(5));
                           0,1,0;
                           -sin(position(5)),0,cos(position(5))];
    rotation_matrix=rotation_azimuth_matrix*rotation_elevation_matrix;
    position(1:3)=position(1:3)+rotation_matrix*velocity(1:3)*delta_t;
    position(5:6)=[expect_elevation_angle,expect_azimuth_angle]';
    iteration=iteration+1;
end

%draw
plot3(y,x,z);
axis equal;grid on;hold on;
plot3(position_storage(2,:),position_storage(1,:),position_storage(3,:),'r-');

%% function
function [curve_update,expect_azimuth_angle,expect_elevation_angle]=spital_los(curve_parameter,curve_value,ship_position,ship_velocity)
%parameter curve--spiral line
a=curve_parameter(1);
b=curve_parameter(2);
h=curve_parameter(3);
%los parameter
delta_e=0.5;
delta_h=0.5;
gamma=1.2;
%algorithm
curve_position=[b*sin(curve_value),a*cos(curve_value),h*curve_value/(2*pi)]';
d_curve_position=[b*cos(curve_value),-a*sin(curve_value),h/(2*pi)]';
curve_azimuth_angle=atan2(d_curve_position(2),d_curve_position(1));
curve_elevation_angle=atan2((-d_curve_position(3)),(d_curve_position(1)^2+d_curve_position(2)^2)^0.5);

rotation_azimuth_matrix=[cos(curve_azimuth_angle),-sin(curve_azimuth_angle),0;
                         sin(curve_azimuth_angle),cos(curve_azimuth_angle),0;
                         0,0,1];
rotation_elevation_matrix=[cos(curve_elevation_angle),0,sin(curve_elevation_angle);
                           0,1,0;
                           -sin(curve_elevation_angle),0,cos(curve_elevation_angle)];
rotation_matrix=rotation_azimuth_matrix*rotation_elevation_matrix;

error=rotation_matrix'*(ship_position(1:3)-curve_position(1:3));
%output
curve_update=(norm(ship_velocity(1:3))*cos(ship_position(5)-curve_azimuth_angle)*cos(ship_position(6)-curve_elevation_angle)+gamma*error(1))/...
              norm(d_curve_position);
expect_azimuth_angle=curve_azimuth_angle+atan2(-error(2),delta_e);
expect_elevation_angle=curve_elevation_angle+atan2(error(3),delta_h);
end