【无人水面艇路径跟随控制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欸。如果你喜欢我的文章，欢迎三连给我鼓励和支持：👍点赞 📁 关注 💬评论，我会给大家带来更多有用有趣的文章。

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

欢迎大家添加好友交流。

相关推荐
A懿轩A12 分钟前
C/C++ 数据结构与算法【数组】 数组详细解析【日常学习，考研必备】带图+详细代码
c语言·数据结构·c++·学习·考研·算法·数组
古希腊掌管学习的神13 分钟前
[搜广推]王树森推荐系统——矩阵补充&最近邻查找
python·算法·机器学习·矩阵
云边有个稻草人17 分钟前
【优选算法】—复写零（双指针算法）
笔记·算法·双指针算法
半盏茶香18 分钟前
在21世纪的我用C语言探寻世界本质 ——编译和链接（编译环境和运行环境）
c语言·开发语言·c++·算法
忘梓.1 小时前
解锁动态规划的奥秘：从零到精通的创新思维解析（3）
算法·动态规划
Evand J1 小时前
LOS/NLOS环境建模与三维TOA定位，MATLAB仿真程序，可自定义锚点数量和轨迹点长度
开发语言·matlab
LucianaiB1 小时前
探索CSDN博客数据：使用Python爬虫技术
开发语言·爬虫·python
Ronin3051 小时前
11.vector的介绍及模拟实现
开发语言·c++
计算机学长大白2 小时前
C中设计不允许继承的类的实现方法是什么？
c语言·开发语言
PieroPc3 小时前
Python 写的 智慧记 进销存 辅助 程序 导入导出 excel 可打印
开发语言·python·excel
热门推荐
01〔AI 绘画〕Stable Diffusion 之 解决绘制多人或面部很小的人物时面部崩坏问题 篇02ARM学习（31）编译器对overlay方式的支持03玄机平台应急响应—webshell查杀04DisplayPort AltMode On Type-c协议_Ver2.0(学习笔记)05校验 GPT-4 真实性的三个经典问题：快速区分 GPT-3.5 与 GPT-4，并提供免费测试网站06Docker 夺命连环 15 问076个主流的工业3D管道设计软件08基于大语言模型（LLM）的合成数据生成、策展和评估的综述09(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明10树莓派 5 AI 套件（Hailo-8L）使用教程