L1制导算法的研究以及MATLAB仿真

参考文件以及MATLAB代码：

链接：https://pan.baidu.com/s/1NmBVm-_jFle7oZMEkOmFOw 
提取码：fvzf

包括以下两个文件

原理:

具体原理和推导过程看第一个文件，大体思路是：

1、找参考点

无人机想要跟踪一条路径，首先选取这条路径上的一些点作为参考点（参考点一般可以任意取）

2、法向加速度的更新

然后更新角速度（线速度可以保持不变）：

3、位置更新，航向角更新

代码：

clear all

v(1,1) = 1; %线速度
theta(1,1) = 0;%航向
p(1,:) = [0,0];%位置
% dp = [1,1.5,0;2.5,1,0;4.5,1.5,0;6.5,3.5,0;5.5,2.5,0;2.5,5.5,0;4.5,5.5,0];%路径点
dp = [1,2,0;2,3,0;3,4,0;4,3,0;5,2,0;3,3,0;1,1,0];%路径点
w(1,1) = 0;%角速度

dt = 0.01;%
n = 3000;%仿真步长
j = 1;

for i = 2:n
    l1d = sqrt((dp(j,2)-p(i-1,2))^2+(dp(j,1)-p(i-1,1))^2); %L1距离
    dtheta = atan((dp(j,2)-p(i-1,2))/(dp(j,1)-p(i-1,1))); %目标点与无人机间的方向角：目标角
    if (dp(j,1)-p(i-1,1)) < 0
        dtheta = dtheta + pi;
    elseif (dp(j,2)-p(i-1,2)) < 0
        dtheta = dtheta + pi*2;        
    end
    eta = dtheta - theta(i-1,1); %航向角与目标角差值
     
    a_y = 2 * v(i-1,1)^2/l1d*sin(eta); %横向加速度
    w(i,1) = 2*a_y/v(i-1,1); %更新角速度
    v(i,1) = v(i-1,1);%更新线速度
    
    theta(i,1) = theta(i-1,1) + w(i,1)*dt; %更新航向角
    p(i,1) = p(i-1,1) + v(i,1)*dt * cos(theta(i,1));%更新位置
    p(i,2) = p(i-1,2) + v(i,1)*dt * sin(theta(i,1));
    
   if l1d < 0.01%更新目标点
        if j <7
            j = j + 1;
        else
            break;%到达最后目标点后终止运行
        end
    end

end

figure(1)
subplot(211),plot(p(:,1),p(:,2))
hold on
subplot(211),plot(dp(:,1),dp(:,2),'ro')
hold off
n = length(p(:,1));
t = 0:dt:(n-1)*dt;
subplot(212),plot(t,theta)

但是经过编程发现，在更新角速度的时候并不能很好地跟踪路径，于是可以在w前面乘上一个系数，使之更新更快，来达到更好地跟踪地目的。