【无人水面艇路径跟随控制10】（Matlab）USV代码阅读：testUSV仿真无人水面艇在一定时间内的运动，使用欧拉法对状态进行积分，并绘制仿真结果

news2024/11/26 15:36:23

【无人水面艇路径跟随控制10】（Matlab）USV代码阅读：仿真无人水面艇在一定时间内的运动，使用欧拉法对状态进行积分，并绘制仿真结果

写在最前面
testUSV.m
- 总结
- 代码详解
- - 1. **初始化部分**
  - 2. **仿真循环**
  - 3. **仿真数据提取**
  - 4. **绘制仿真结果**
  - 5. **绘制其他图像**
- 全部代码

请添加图片描述

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

写在最前面

版权声明：本文为原创，遵循 CC 4.0 BY-SA 协议。转载请注明出处。

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

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

在这里插入图片描述

请添加图片描述

testUSV.m

这段 MATLAB 代码用于测试 USV（Unmanned Surface Vehicle，无人水面艇） 的运动模型。代码通过仿真无人水面艇在一定时间内的运动，使用欧拉法对状态进行积分，并绘制仿真结果。

总结

该代码实现了对无人水面艇（USV）的运动仿真，模拟了船舶在风、流等干扰下的运动过程。
使用 PID 控制器来调节船舶的推进力和航向角力矩，通过调用 USV 函数计算船舶状态导数，再使用欧拉法更新状态。
仿真过程中动态调整控制输入（如航向力矩），并实时绘制船舶的运动轨迹和相关状态变量的变化。

代码详解

下面逐步解释代码的各个部分：

1. 初始化部分

ts = 0.1;                 % 采样时间
tfinal = 20;              % 仿真结束时间
Ns = tfinal/ts;           % 仿真步数
Vw = 0; betaw = 30 * pi / 180;
wind = [Vw betaw]';       % 风的速度和角度
Vc = 0; betac = 30 * pi / 180;
current = [Vc betac]';    % 水流的速度和角度

tao = [10 0 0]';          % 初始控制输入力矩
tao0 = tao;
d = [0 0 0]';             % 扰动（假设无扰动）
x = [0 0 0 2 5 0]';       % 初始状态：[u v r x y psai]
x0 = x;

ts：仿真时间步长。
tfinal：仿真总时间，单位是秒。
Ns：仿真步数，即仿真总时长除以步长。
wind 和 current：分别表示风速和水流速度，风和水流的角度（弧度制）通过 betaw 和 betac 指定。
tao：控制输入向量，分别是船舶的推进力、横向力和航向力矩。
x：初始状态向量，包含速度（u, v, r）以及位置（x, y）和航向角 psai。

2. 仿真循环

disp('Simulation ... ');
for k = 1:1:Ns
    time(1) = 0;
    time(k+1) = k * ts;
    
    if x(6)*180/pi >= 360
        x(6) = x(6) - 2*pi;
    end
    if x(6)*180/pi <= -360
        x(6) = x(6) + 2*pi;
    end

    if k * ts >= 5
        tao = [10 0 1 * pi / 180]';  % 调整控制力矩，增加航向力矩
    end
    Ttao(1,:) = tao0';
    Ttao(k+1,:) = tao';
    
    % 计算状态导数
    xdot = USV(x, tao, wind, current, d);
    
    % 使用欧拉法更新状态
    x = euler2(xdot, x, ts);
    
    % 存储每步状态
    xout(1,:) = x0;
    xout(k+1,:) = x';
end

time：记录每一步的仿真时间。
当 x(6) 的航向角超过 360 度或小于 -360 度时，通过减去或加上 2*pi 来归一化航向角。
当时间超过 5 秒时，控制输入 tao 中的航向力矩被调整为 1 * pi / 180，引入了一个小的航向力矩。
USV(x, tao, wind, current, d)：调用 USV 函数计算状态导数 xdot。
euler2(xdot, x, ts)：使用欧拉法数值积分，更新状态 x。
xout：用于存储每一步的状态。

3. 仿真数据提取

u = xout(:,1);
v = xout(:,2);
r = xout(:,3);
N = xout(:,4);
E = xout(:,5);
psai = xout(:,6);

从仿真结果 xout 中提取各个状态变量的时间序列，包括：
- u：纵向速度。
- v：横向速度。
- r：航向角速度。
- N：北向位置。
- E：东向位置。
- psai：航向角。

4. 绘制仿真结果

disp('plot ...');
for k = 1:1:Ns
    pos = [N(k) E(k)]';
    if k == 1
        modelplot(pos, psai(k));  % 初始位置的船舶模型绘制
    end
    if rem(k,10) == 0
        modelplot(pos, psai(k));  % 每 10 步绘制一次
    end
end

plot(E, N, 'r', 'linewidth', 2)  % 绘制航迹线
hold off;

modelplot(pos, psai(k))：调用 modelplot 函数绘制船舶模型，在第一个位置和每 10 步时绘制一次船舶的位置。
plot(E, N, 'r', 'linewidth', 2)：绘制仿真过程中船舶的航迹，红色线条表示船舶在仿真过程中的运动轨迹。

5. 绘制其他图像

figure(2);
plot(time, psai * 180 / pi, 'r', 'linewidth', 2);  % 绘制航向角随时间变化
xlabel('time/s'); ylabel('psai/deg');

figure(3);
plot(E, N, 'r', 'linewidth', 2)  % 绘制东、北坐标的变化
xlabel('E'); ylabel('N');

figure(4);
plot(time, u, 'r', 'linewidth', 2)  % 绘制纵向速度随时间变化
xlabel('time/s'); ylabel('u (m/s)');

figure(5);
plot(time, Ttao(:,1), 'r', time, Ttao(:,2), 'k', time, Ttao(:,3), 'b', 'linewidth', 2)  % 绘制控制输入
xlabel('time/s'); ylabel('u (m/s)');

图 2：绘制航向角 psai 随时间的变化，显示船舶的转向行为。
图 3：再次绘制东向 E 和北向 N 的变化轨迹。
图 4：绘制纵向速度 u 随时间的变化。
图 5：绘制控制输入 tao 的变化，包括三个力矩分量：推进力（红色）、横向力（黑色）和航向力矩（蓝色）。

全部代码

% Author: Quyinsong
% Data: 14th Jan 2022
% test the USV function
clc 
clear all
close all
% initial
ts=0.1;                 % sample time
tfinal =20;             % simulation final time
Ns =tfinal/ts;          % step number of simulation
Vw=0; betaw=30*pi/180;
wind=[Vw betaw]';       % wind
Vc=0; betac=30*pi/180;
current=[Vc betac]';    % current

tao=[10 0 0]'; 
tao0=tao;
d=[0 0 0]';
x=[0 0 0 2 5 0]';
x0=x;
% simulation start
disp('Simulation ... ');
for k=1:1:Ns
    time(1)=0;
    time(k+1)=k*ts;
    
    if x(6)*180/pi>=360
        x(6)=x(6)-2*pi;
    end
    if x(6)*180/pi<=-360
        x(6)=x(6)+2*pi;
    end

    if k*ts>=5
        tao=[10 0 1*pi/180]';
    end
    Ttao(1,:)=tao0';
    Ttao(k+1,:)=tao';
    % time derivatives
    xdot=USV(x,tao,wind,current,d);
    % update states
    x=euler2(xdot,x,ts);
    % store time series
    xout(1,:)=x0;
    xout(k+1,:)=x';
    
end
u=xout(:,1);
v=xout(:,2);
r=xout(:,3);
N=xout(:,4);
E=xout(:,5);
psai=xout(:,6);
% testUSV plot
disp('plot ...');
for k=1:1:Ns
    pos =[N(k) E(k)]';
    if k==1
        modelplot(pos,psai(k));
    end
    if rem(k,10)==0
        modelplot(pos,psai(k));
    end   
end
plot(E,N,'r','linewidth',2)
hold off;
figure(2);
plot(time,psai*180/pi,'r','linewidth',2);
xlabel('time/s');ylabel('psai/deg');
figure(3);
plot(E,N,'r','linewidth',2)
xlabel('E');ylabel('N');
figure(4);
plot(time,u,'r','linewidth',2)
xlabel('time/s');ylabel('u (m/s)');
figure(5);
plot(time,Ttao(:,1),'r',time,Ttao(:,2),'k',time,Ttao(:,3),'b','linewidth',2)
xlabel('time/s');ylabel('u (m/s)');