代码编织梦想

控制率设计-比例导引法的Matlab与Simulink实现

基本原理

利用 q ˙ = k ϵ ˙ \dot{q} = k \dot\epsilon q˙=kϵ˙ 其中 q q q 为目标与观测点的夹角, ϵ \epsilon ϵ为飞行器与观测点的夹角

q q q可用目标与观测点的 X X X方向距离, Y Y Y方向距离进行表示, ϵ \epsilon ϵ 同理。

Simulink实现

输入目标的速度 V V V,飞行角度 θ \theta θ,利用积分器分别对 V x , V y V_x,V_y Vx,Vy进行积分求得 S x , S y S_x,S_y Sx,Sy,反解出此时的角度
求解角度

对角度进行微分求得 q ˙ \dot{q} q˙,并设定增益值,求得 ϵ ˙ \dot{\epsilon} ϵ˙在这里插入图片描述

积分后输入飞行器的飞行角度控制中,求得飞行的 X , Y X,Y X,Y方向速度,积分得到 X , Y X,Y X,Y方向的位移在这里插入图片描述

利用X-Y Graph绘制相应的曲线。
在这里插入图片描述

利用OUT输出至workspace,进行绘图重现

输出的为目标位移与飞行器位移
在这里插入图片描述

注:

  • 此处设置目标角度为0,沿 X X X轴方向飞行
  • 积分初值对应的起始位置点
  • 由于飞机速度快,基本会飞到目标前面

Matlab 进行绘图重现

实现效果

在这里插入图片描述

源代码

 
 %读取数据
x1= out.yout{1}.Values.Data;
y1=out.yout{2}.Values.Data;
x2 = out.yout{3}.Values.Data;
y2 = out.yout{4}.Values.Data;
%绘制图像
for t =1:1:1000
    xlim([0,1500]);
    ylim([0,2700]) ;
    if (abs(x1(t)-x2(t))+abs(y1(t)-y2(t))>40) %设定跟踪阈值
        scatter(x1(t),y1(t),5,'k','*');   
        hold on 
        grid on
        scatter(x2(t),y2(t),10,'r','o');   
        legend(' Target' ,'UAV') ;
        xlabel('X');
        ylabel('Y');
        pause(0.00000001);  
        title('Tracing Guidance');
    else
        warndlg('Target Ready!!','Warning');%显示完成
        break
    end
end

注意:由于加入了跟踪阈值,会导致绘图越来越慢,可根据需要删去

版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/weixin_46268065/article/details/131027867