【无人水面艇路径跟随控制10】（Matlab）USV代码阅读：仿真无人水面艇在一定时间内的运动，使用欧拉法对状态进行积分，并绘制仿真结果
- 写在最前面
- testUSV.m
- 总结
- 代码详解
- 1. **初始化部分**
- 2. **仿真循环**
- 3. **仿真数据提取**
- 4. **绘制仿真结果**
- 5. **绘制其他图像**
- 全部代码
写在最前面
版权声明：本文为原创，遵循 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:Nstime(1) = 0;time(k+1) = k * ts;if x(6)*180/pi >= 360x(6) = x(6) - 2*pi;endif x(6)*180/pi <= -360x(6) = x(6) + 2*pi;endif k * ts >= 5tao = [10 0 1 * pi / 180]'; % 调整控制力矩，增加航向力矩endTtao(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:Nspos = [N(k) E(k)]';if k == 1modelplot(pos, psai(k)); % 初始位置的船舶模型绘制endif rem(k,10) == 0modelplot(pos, psai(k)); % 每 10 步绘制一次end
endplot(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]'; % currenttao=[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:Nstime(1)=0;time(k+1)=k*ts;if x(6)*180/pi>=360x(6)=x(6)-2*pi;endif x(6)*180/pi<=-360x(6)=x(6)+2*pi;endif k*ts>=5tao=[10 0 1*pi/180]';endTtao(1,:)=tao0';Ttao(k+1,:)=tao';% time derivativesxdot=USV(x,tao,wind,current,d);% update statesx=euler2(xdot,x,ts);% store time seriesxout(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:Nspos =[N(k) E(k)]';if k==1modelplot(pos,psai(k));endif rem(k,10)==0modelplot(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)');hello，我是 是Yu欸 。如果你喜欢我的文章，欢迎三连给我鼓励和支持：👍点赞 📁 关注 💬评论，我会给大家带来更多有用有趣的文章。
原文链接 👉 ，⚡️更新更及时。
欢迎大家添加好友交流。
