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

于 2024-10-09 14:00:00 发布
阅读量552 收藏 9
点赞数 11
分类专栏： 路径跟踪 # 机器学习实战（matlab版本） 文章标签： matlab 算法 开发语言 无人机 目标跟踪
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/wtyuong/article/details/142724425
版权

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

请添加图片描述

🌈你好呀！我是 是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. 初始化和清理：

    clc
clear all

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

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

    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. 生成两个目标点：

    xk =[5 5]';yk =[300 150]';
afak=atan2(yk(2)-xk(2),yk(1)-xk(1));

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

  4. 初始化变量：

    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. 仿真循环：

    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。
    • 矩阵计算：计算阻尼矩阵的各个元素。

全部代码

% 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欸 。如果你喜欢我的文章，欢迎三连给我鼓励和支持：👍点赞 📁 关注 💬评论，我会给大家带来更多有用有趣的文章。
原文链接 👉 ，⚡️更新更及时。

欢迎大家添加好友交流。

是Yu欸
关注
专栏目录
【无人水面艇路径跟随控制6】（Matlab）USV代码阅读：简介+效果预览
定期分享我的发现和想法，感谢你的陪伴和支持
10-08 608
路径跟踪是机器人、自动驾驶车辆和无人机等自主系统的一项基本能力，它要求系统能够沿着预定的路径准确地移动。Line-of-Sight (LOS) 控制算法是一种广泛应用于路径跟踪控制的策略，它通过计算当前位置与目标路径之间的虚拟线，以及这条线与前进方向之间的角度偏差，来指导系统调整航向和速度，以保持在预定路径上。为了评估和优化LOS控制算法的性能，仿真成为了一个不可或缺的工具。
【无人水面艇路径跟随控制5】（C++）USV代码阅读： 如何声明消息、服务和动作
定期分享我的发现和想法，感谢你的陪伴和支持
10-07 1159
这个CMake构建脚本配置了一个名为 publish_waypoints的ROS包，定义了包的依赖关系、编译选项和如何声明消息、服务和动作。通过这些配置，可以确保项目在编译时正确地找到所需的头文件和库文件，并生成所需的目标文件。这个 package.xml文件定义了 publish_waypoints包的基本信息、维护者、许可证和依赖关系。需要注意的是，某些占位符（如许可证信息）需要替换为实际内容。这个文件对于ROS包的构建和管理至关重要。
【无人水面艇】USV路径跟踪LOS控制算法仿真（Matlab实现）
weixin_61181717的博客
07-24 423
路径跟踪是机器人、自动驾驶车辆和无人机等自主系统的一项基本能力，它要求系统能够沿着预定的路径准确地移动。Line-of-Sight (LOS) 控制算法是一种广泛应用于路径跟踪控制的策略，它通过计算当前位置与目标路径之间的虚拟线，以及这条线与前进方向之间的角度偏差，来指导系统调整航向和速度，以保持在预定路径上。为了评估和优化LOS控制算法的性能，仿真成为了一个不可或缺的工具。
【无人水面艇】USV路径跟踪LOS控制算法仿真（Matlab代码实现）
Ke_Yan_She的博客
08-10 408
路径跟踪是机器人、自动驾驶车辆和无人机等自主系统的一项基本能力，它要求系统能够沿着预定的路径准确地移动。Line-of-Sight (LOS) 控制算法是一种广泛应用于路径跟踪控制的策略，它通过计算当前位置与目标路径之间的虚拟线，以及这条线与前进方向之间的角度偏差，来指导系统调整航向和速度，以保持在预定路径上。为了评估和优化LOS控制算法的性能，仿真成为了一个不可或缺的工具。
在ROS（Robot Operating System）中为无人水面艇（USV）开发路径跟随控制的方法（包含详细的完整的程序
10-07
内容概要：本文聚焦于如何在ROS环境下开发无人水面艇（USV）的路径跟随控制算法，并展示了具体的项目实例，涵盖了从设置CMakeLists.txt到编译ROS消息和服务的关键流程。 适用人群：具有一定ROS基础知识和技术背景的...
Thinking_轨迹规划_航行轨迹_基于LOS无人水面艇的路径跟踪_own3oh_水面无人艇
09-11
本资源"Thinking_轨迹规划_航行轨迹_基于LOS无人水面艇的路径跟踪_own3oh_水面无人艇"显然是关于如何通过Los（Line-of-Sight）方法来实现无人水面艇的精确轨迹规划和跟踪控制。让我们深入探讨这些关键概念。 **轨迹...
matlab实现USV路径跟踪LOS控制算法仿真源码+全部数据（大作业课设）.zip
11-16
matlab实现USV路径跟踪LOS控制算法仿真源码+全部数据（大作业课设）.zip 下载即用无需修改，项目完整确保可以运行。 matlab实现USV路径跟踪LOS控制算法仿真源码+全部数据（大作业课设）.zip 下载即用无需修改，...
课程设计USV路径跟踪LOS控制算法matlab仿真源码（下载即用）.zip
03-02
课程设计USV路径跟踪LOS控制算法matlab仿真源码（下载即用）.zip课程设计USV路径跟踪LOS控制算法matlab仿真源码（下载即用）.zip课程设计USV路径跟踪LOS控制算法matlab仿真源码（下载即用）.zip课程设计USV路径跟踪...
课程作业-USV路径跟踪LOS控制算法matlab仿真源码.zip
09-21
USV路径跟踪LOS控制算法matlab仿真源码.zip课程作业-USV路径跟踪LOS控制算法matlab仿真源码.zip课程作业-USV路径跟踪LOS控制算法matlab仿真源码.zip课程作业-USV路径跟踪LOS控制算法matlab仿真源码.zip课程作业-USV...
蓝牙定位的MATLAB仿真程序（基于信号强度，平面内的定位，四个蓝牙基站）
MATLAB卡尔曼的博客
10-05 228
这段代码通过RSSI信号强度实现了蓝牙定位，展示了如何使用锚点位置和测量的信号强度来估计未知点的位置。它涵盖了信号衰减模型、距离计算和最小二乘法估计等基本概念。通过图形化输出，用户可以直观地看到真实位置与估计位置的关系。
基于matlab的滤波器设计
2401_86539719的博客
10-08 594
在无线脉冲响应（IIR）数字滤波器设计中，先进行模拟滤波器的设计，然后进行模拟-数字滤波器转换，即采用脉冲响应不变法及双线性Z变化法设计数字滤波器，最后进行滤波器的频带转换。，生成一个新的信号。执行程序后听到的音频依旧含有噪音信号，不如原始语音清晰，但比加噪后的语音有了明显的改进，可以比较清晰的听到原语音。当执行上述程序时，会听到嘈杂的声音层，且频率高于原始语音很多，由加噪后的频谱图与加噪前的频谱图相对比会明显发现在原时间内多了很多频率变化，观察到的频谱图变化与我们的目的是一致的，加噪成功。
基于方块编码的图像压缩matlab仿真,带GUI界面
最新发布
MATLAB,verilog,python,opencv,tensorflow,caffe,C,C++
10-08 473
随着信息技术的飞速发展，图像数据在各个领域中的应用越来越广泛。然而，图像数据通常具有较大的数据量，这给存储、传输和处理带来了很大的挑战。为了解决这个问题，图像压缩技术应运而生。图像压缩的目的是在不损失图像质量的前提下，尽可能地减少图像数据的存储空间和传输带宽。方块编码是一种基于区域划分的图像压缩方法，它将图像划分为若干个大小相同的方块，然后对每个方块进行编码。方块编码具有简单、高效、易于实现等优点，在图像压缩领域得到了广泛的应用。（一）方块的划分。
【3D目标检测】激光雷达和相机联合标定（二）——MATLAB联合标定工具使用
qq_44703886的博客
10-05 1071
MATLAB激光雷达与相机联合标定，生成KITTI标准化标定文件，点云与图像配准映射
（11）MATLAB莱斯（Rician）衰落信道仿真2
weixin_45333185的博客
09-30 2210
首先给出莱斯衰落信道仿真模型，该模型由直射路径分量和反射路径分量组成，其中反射路径分量由瑞利衰落信道模型构成，该模型可以很方便地建立瑞利衰落信道的仿真。给出莱斯衰落信道的MATLAB仿真代码，并对比了莱斯衰落信道和瑞利衰落信道，揭示了两者之间的内在关系。
MATLAB - 机械臂手眼标定（眼在手内） - 估计安装在机器人上的移动相机的姿态
weixin_46300916的博客
10-04 1314
执行手眼校准有助于操作配备末端执行器（简称 “手”）的机械臂，该末端执行器依赖于摄像头提供的视觉数据。一旦完成了眼在手外的校准，机械臂就能准确地移动到摄像头识别的特定像素位置。这种能力是执行精确拾放任务（如分类、堆垛和分拣）的基础，即使在摄像机的确切位置和方向未知的情况下也是如此。机器人-摄像机系统有两种构型：眼在手内和眼在手外。在眼在手内构型中，摄像头直接安装在机器人手臂的末端执行器上。相反，在眼在手的构型中，摄像头固定在一个静止的物体上，机器人手臂在其视野内。
matlab初学习记录
soleiltree的博客
10-02 1411
看入门之旅先计算等号右边再计算等号左边。工作区记录等号右边的变量。;表示的是抑制输出。不输出结果，但是工作区还是会记录的。上下箭头能够返回之前的命令。并没有重新计算。matlab 区分大小写，数字不能开头当变量。save + 文件名字，可以将工作区的变量保存到 mat 文件中就是将工作区里的变量保存到文件中clear 将工作区清空load + 文件名字，能将文件中的变量加载到工作区中输入变量名称能在控制台输出变量里面的内容。clc 清空命令行窗口也能够保存或加载部分变量。
（14）MATLAB莱斯（Rician）衰落信道仿真4
weixin_45333185的博客
10-03 2393
本文通过将接收信号总功率设置为1，重写了莱斯衰落信道上接收信号幅度的理论PDF式。然后用MATLAB代码生成了在具有不同莱斯因子K的Ricean平坦衰落信道下接收到的信号样本，并计算了PDF的估计值。最终通过画图给出仿真结果。
数字通信中不同信道类型对通信系统性能影响matlab仿真分析,对比AWGN,BEC,BSC以及多径信道
MATLAB,verilog,python,opencv,tensorflow,caffe,C,C++
10-02 636
在数字通信系统中，信道是传输信息的媒介。不同的信道类型会对通信系统的性能产生显著的影响。本章节将详细介绍AWGN（加性高斯白噪声）信道、BEC（二进制擦除信道）、BSC（二进制对称信道）以及多径信道，并通过数学公式和原理来分析它们对通信系统性能的影响。AWGN信道是最常见的信道模型之一，它假设噪声是独立于信号的加性高斯白噪声。在AWGN信道中，接收到的信号 y 可以表示为：y=x+n其中，x 是发送信号，n 是均值为0、方差为 σ2的高斯白噪声。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

是Yu欸

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值