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

是Yu欸2024-10-15 16:56

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

  • 写在最前面
  • LOS.m
    • 代码思路
      • [1. **参数初始化**](#1. 参数初始化)
      • [2. **仿真时间设置**](#2. 仿真时间设置)
      • [3. **仿真主循环**](#3. 仿真主循环)
        • [3.1 **视线引导法（LOS）**](#3.1 视线引导法（LOS）)
        • [3.2 **控制律**](#3.2 控制律)
        • [3.3 **动力学模型**](#3.3 动力学模型)
        • [3.4 **PID 控制输出**](#3.4 PID 控制输出)
        • [3.5 **状态更新**](#3.5 状态更新)
      • [4. **结果绘制**](#4. 结果绘制)
      • [5. **控制器调节**](#5. 控制器调节)
      • [6. **总结**](#6. 总结)
    • 代码解释
    • 全部代码


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

写在最前面

USV-path-following

USV路径跟踪LOS控制算法仿真

阅读代码：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 在水面上跟随指定直线路径的仿真，并且展示了控制器的设计和性能。

代码解释

以下是代码的详细解释：

  1. 初始化和清理：

    objective-c 复制代码 
    clc
clear all

    清除命令窗口和工作区中的所有变量。

  2. 定义USV（无人水面艇）参数：

    objective-c 复制代码 
    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;

    定义了USV的质量矩阵和阻尼系数等参数。

  3. 生成两个目标点：

    objective-c 复制代码 
    xk =[5 5]';yk =[300 150]';
afak=atan2(yk(2)-xk(2),yk(1)-xk(1));

    定义了两个目标点，并计算了它们之间的角度afak。

  4. 初始化变量：

    objective-c 复制代码 
    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;

    初始化了仿真时间步长、总时间、状态变量x、误差ek_1和一些控制参数。

  5. 仿真循环：

    objective-c 复制代码 
    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);

    进行仿真，计算每个时间步的状态更新：

    • LOS控制律 ：计算横向误差ye，并根据误差计算期望航向psaid。
    • 控制律 ：根据当前状态x计算控制误差ek，并调整控制增益Kd和Kp2。
    • 矩阵计算：计算阻尼矩阵的各个元素。

全部代码

matlab 复制代码 
% 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，我是 是Yu欸。如果你喜欢我的文章，欢迎三连给我鼓励和支持：👍点赞 📁 关注 💬评论，我会给大家带来更多有用有趣的文章。

原文链接 👉 ，⚡️更新更及时。

欢迎大家添加好友交流。

相关推荐
不去幼儿园17 分钟前
【MARL】深入理解多智能体近端策略优化（MAPPO）算法与调参
人工智能·python·算法·机器学习·强化学习
Mr_Xuhhh19 分钟前
重生之我在学环境变量
linux·运维·服务器·前端·chrome·算法
Ajiang28247353041 小时前
对于C++中stack和queue的认识以及priority_queue的模拟实现
开发语言·c++
盼海1 小时前
排序算法(五)--归并排序
数据结构·算法·排序算法
幽兰的天空1 小时前
Python 中的模式匹配：深入了解 match 语句
开发语言·python
Theodore_10224 小时前
4 设计模式原则之接口隔离原则
java·开发语言·设计模式·java-ee·接口隔离原则·javaee
网易独家音乐人Mike Zhou5 小时前
【卡尔曼滤波】数据预测Prediction观测器的理论推导及应用 C语言、Python实现（Kalman Filter）
c语言·python·单片机·物联网·算法·嵌入式·iot
----云烟----6 小时前
QT中QString类的各种使用
开发语言·qt
lsx2024066 小时前
SQL SELECT 语句：基础与进阶应用
开发语言
开心工作室_kaic7 小时前
ssm161基于web的资源共享平台的共享与开发+jsp(论文+源码)_kaic
java·开发语言·前端
热门推荐
01(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明02PyTorch机器学习实现液态神经网络03【HarmonyOS】HUAWEI DevEco Studio 下载地址汇总04玄机平台应急响应—webshell查杀05Coze扣子平台完整体验和实践（附国内和国际版对比）06Ubuntu 20.04使用Livox mid 360 测试 FAST_LIO07【目标跟踪】相机运动补偿08Unity中PICO实现 隔空取物 和 接触抓取物体09RAG 实践- Ollama+RagFlow 部署本地知识库10怎样让音频速度变慢？请跟随以下方法进行操作