【无人水面艇路径跟随控制8】（Matlab）USV代码阅读：LOS通过视线引导算法和PID控制器来实现无人水面艇的直线路径跟踪

2024-11-20 来源：个人技术集锦

?你好呀！我是 ? 2024每日百字篆刻时光，感谢你的陪伴与支持 ~ ? 欢迎一起踏上探险之旅，挖掘无限可能，共同成长！

写在最前面

阅读代码：https://github.com/quyinsong/USV-path-following
运行效果：

LOS.m

这段代码实现了一个用于直线路径跟踪的视线引导（Line of Sight, LOS）控制算法，使用了PID控制器。

仿真循环：每个状态更新步骤使用欧拉积分 (euler2) 来更新状态。

需要实现或导入 USV 和 euler2 函数，即USV.m 和 euler2.m文件。

代码思路

这段 MATLAB 代码实现了一种基于 视线引导法（Line-of-Sight, LOS） 的路径跟随控制算法，适用于自主水面艇（USV）的直线路径跟随。整个代码由 USV 的运动学和动力学模型构成，使用了 PID 控制器来调节航向。下面逐步解释代码的主要部分：

1. 参数初始化

USV 的运动学和动力学参数初始化：m11, m22, m33 等是质量矩阵的相关元素，描述了 USV 的动力学特性。Xu, Yv, Nv 等是水动力系数，表示不同方向上的阻力和力矩的影响。
xk, yk：定义了直线路径的两个点。afak 计算了路径的方向角，基于起点和终点的坐标，通过 atan2 函数得到。

2. 仿真时间设置

ts：仿真时间步长，tfinal：仿真总时间，Ns：仿真步骤数量。初始状态 x=[0.1, 0.1, 0, 5, 5, 0] 对应了初始的速度、位置和航向角。

3. 仿真主循环

在这个循环中，代码执行以下操作：

3.1 视线引导法（LOS）

Kp1 是 LOS 控制器的增益。
ye 是横向偏差，表示 USV 在当前时刻相对于目标路径的偏离量。
beta 是当前 USV 的航向角。
psaid 计算出期望航向角，基于 LOS 规则。

3.2 控制律

使用 PID 控制器来控制航向，ek 表示当前航向误差，Kd 和 Kp2 是控制器的参数，分别用于调整控制器对误差的响应。

3.3 动力学模型

M 是 USV 的质量矩阵，Crb 是与科氏力和离心力相关的刚体力矩矩阵。
Nvr 是与水动力相关的阻尼矩阵，表示水流对 USV 的影响。
fr 是摩擦力模型。

3.4 PID 控制输出

tpid 是 PID 控制器的输出力矩，用于控制 USV 的航向。
tao 是控制输入向量，包含横向力、纵向力和航向力矩。

3.5 状态更新

使用 euler2 函数进行欧拉法数值积分，更新 USV 的状态。
xdot = USV(x,tao,[0 0]',[0,0]',d) 通过调用 USV 函数计算 USV 的状态导数，d 是随机噪声，模拟环境干扰。

4. 结果绘制

仿真结束后，代码生成多组图表，分别展示了 USV 的位置、航向角、速度、控制输入等随时间的变化。
modelplot 函数用于在仿真过程中绘制 USV 的路径。

5. 控制器调节

在仿真过程中，Kp2 和 Kd 这两个参数根据不同的航向误差动态调整，以增强系统的响应速度和稳定性。PID 控制器根据航向误差和其导数生成控制信号来调整 USV 的航向。

6. 总结

视线引导法（LOS）：该方法是一个经典的路径跟随方法，通过计算目标路径与 USV 当前位姿之间的偏差，生成期望的航向角。
PID 控制器：用于调整航向，控制系统依据当前的航向误差来调整 USV 的姿态和速度。
运动模型：考虑了水动力、摩擦力和环境干扰，使用欧拉法进行数值积分来模拟 USV 的运动。

这个代码实现了 USV 在水面上跟随指定直线路径的仿真，并且展示了控制器的设计和性能。

代码解释

以下是代码的详细解释：

全部代码

% line of sight guidence used in straight line path following

% control law use PID
clc
clear all
% USV parameters 
m11 = 25.8; m22 = 33.8; m33 = 2.76; m23 = 1.095; m32 =1.095;
Xu=0.72253;          Yv=-0.88965;          Nv=0.0313;
Xuu=-1.32742;        Yr=-7.25;             Nr=-1.9;
                     Yvv=-36.47297;        Nvv=3.95645;
                     Yrv=-0.805;           Nrv=0.13;
                     Yvr=-0.845;           Nvr=0.08;
                     Yrr=-3.45;            Nrr=-0.75;


% generate two points
xk =[5 5]';yk =[300 150]';
afak=atan2(yk(2)-xk(2),yk(1)-xk(1));
%initial
ts =0.01;
tfinal=50;
Ns=tfinal/ts;
x=[0.1 0.1 0 5 5 0]';x0=x;
ek_1=1; Ek(1)=ek_1;

psaid_1 = 0.1; psaid_2 = 0.05;
% simulation
disp('Simulation ...');
for k=1:1:Ns
    time(k)=(k-1)*ts;
    
    % LOS law
    Kp1=0.1;
    ye=-(x(4)-xk(1))*sin(afak)+(x(5)-xk(2))*cos(afak);
    YE(k)=ye;
    beta=atan2(x(2),x(1));
    psaid=afak+atan2(-Kp1*ye,1)-beta;
    % control law
    u = x(1);v=x(2);r= x(3);
    ek=x(6)-psaid;
    if k*ts<=0.5, Kd=0.8; else ,Kd=10;end 
    if ek<=0.05, Kp2=3;else,Kp2=1;end


    % matrix
    n11=-Xu-Xuu*abs(u);
    n22=-Yv-Yvv*abs(v)-Yrv*abs(r);
    n23=-Yr-Yvr*abs(v)-Yrr*abs(r);
    n32=-Nv-Nvv*abs(v)-Nrv*abs(r);
    n33=-Nr-Nvr*abs(v)-Nrr*abs(r);
    M=[m11  0    0;
       0   m22 m23;
      0   m32 m33];
    Crb=[0             0       -m22*v-m23*r;
         0             0        m11*u;
         m22*v+m23*r  -m11*u       0        ];
%     Nvr=[n11   0     0;
%           0   n22  n23;
%           0   n32  n33 ];
    c13=Crb(1,3);
    c23=Crb(2,3);
    c31=-c13;c32=-c23;
    m0 = m22*m33-m23*m32;
    fr = (m32*(c23*r+n22*v+n23*r)+m22*(c13*u+c23*v-n32*v-n33*r))/m0;
    psaidd = (psaid-2*psaid_1+psaid_2)/ts^2;
    psaid_2=psaid_1; psaid_1 = psaid;
    tpid=-Kp2*ek-Kd*(ek-ek_1)/ts-fr*m0/m22+psaidd; 
    ek_1=ek;
    tao=[20 0 tpid]';
    Ttao(k,:)=tao';
    Ek(k)=ek;
    % USV
    d = [1*randn(1,1) 2*randn(1,1) 2*randn(1,1)]';
    xdot=USV(x,tao,[0 0]',[0,0]',d);
    % state update
    x=euler2(xdot,x,ts);
    % store time series
    xout(1,:)=x0;
    xout(k,:)=x';
end
u=xout(:,1);
v=xout(:,2);
r=xout(:,3);
N=xout(:,4);
E=xout(:,5);
psai=xout(:,6);
% plot
disp('Plot ...');
for k=1:1:Ns
    pos =[N(k) E(k)]';
    if k==1
        modelplot(pos,psai(k));
    end
    if rem(k*ts,5)==0
        modelplot(pos,psai(k));
    end   
end
plot(E,N,'r','linewidth',2)
plot([xk(2) yk(2)],[xk(1) yk(1)],'b',E,N,'r--','linewidth',2)
hold off;
figure(2);
plot([xk(2) yk(2)],[xk(1) yk(1)],'b',E,N,'r--','linewidth',2)
xlabel('E');ylabel('N');
figure(3);
plot(time,psai*180/pi,'r','linewidth',2);
xlabel('time/s');ylabel('psai/deg');
figure(4);
plot(time,u,'r','linewidth',2)
xlabel('time/s');ylabel('u (m/s)');
figure(5);
plot(time,Ttao(:,1),'r',time,Ttao(:,3),'b','linewidth',2)
legend('surge force','yaw torch');
figure(6);
plot(time,YE,'r','linewidth',2)
xlabel('time/s');ylabel('YE (m)');

hello，我是。如果你喜欢我的文章，欢迎三连给我鼓励和支持：?点赞 ? 关注 ?评论，我会给大家带来更多有用有趣的文章。
原文链接 ? ，⚡️更新更及时。

欢迎大家添加好友交流。

显示全文

全部栏目