【无人水面艇路径跟随控制9】（Matlab）USV代码阅读：modelplot模拟和显示船舶实时运动的可视化函数。接受船舶在某一时刻的 位置 和 航向角 作为输入，并在二维坐标系中绘制船舶的几何形状

【无人水面艇路径跟随控制9】（Matlab）USV代码阅读：modelplot模拟和显示船舶实时运动的可视化函数。接受船舶在某一时刻的 位置 和 航向角 作为输入，并在二维坐标系中绘制船舶的几何形状

• 写在最前面
• modelplot.m

• 总结
• 代码详解

• 1. **函数说明**
• 2. **输入验证**
• 3. **定义船舶的几何形状**
• 4. **从船体坐标系到全球坐标系的旋转**
• 5. **坐标变换**
• 6. **绘制船舶模型**
• 7. **坐标轴标签**

• 全部代码

🌈你好呀！我是 是Yu欸

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

写在最前面

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

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

modelplot.m

这段 MATLAB 代码实现了一个实时绘制船舶几何模型的函数 modelplot。它接受船舶在某一时刻的 位置 和 航向角 作为输入，并在二维坐标系中绘制船舶的几何形状。这是典型的用来模拟和显示船舶运动的可视化函数。

总结

• 这个函数用于实时绘制船舶的几何模型，显示其在某个时刻的具体位置和航向。
• 船体的形状通过 5 个顶点定义，并通过旋转矩阵将这些点从船体坐标系变换到全球坐标系。
• 函数使用 MATLAB 的绘图功能，显示船舶的位置和几何模型，随着时间的推进，船舶的模型可以在航线上动态移动。

这个函数通常用于模拟船舶的运动行为，实时显示船舶如何沿着预定路径或控制律航行。

代码详解

下面是详细的代码解析：

1. 函数说明

function  modelplot( pos, psai )

• modelplot 是用来绘制船舶模型的函数。
• 输入：

• pos：船舶的当前位置，[x, y]（二维坐标）。
• psai：船舶当前的航向角（yaw angle），即船舶的旋转角度，单位是弧度。

2. 输入验证

if nargin~=2 
    error('the number of input must be 2');
end
if length(pos)~=2 
    error('the number of pos must be 2');
end
if length(psai)~=1 
    error('the number of psai must be 1');
end

• 这部分代码检查输入参数的数量和大小：

• 需要两个输入参数，pos（二维位置）和 psai（标量航向角）。
• pos 必须是一个二维向量，而 psai 必须是标量。

3. 定义船舶的几何形状

xb1=[0.35 -0.375]'; xb2=[0.65 0]'; xb3=[0.35 0.375]'; xb4=[-0.35 0.375]'; xb5=[-0.35 -0.375]';

• 这里定义了船舶模型的 5 个顶点在船体坐标系 {b} 下的坐标（即相对于船体本身的坐标）。

• xb1, xb2, xb3, xb4, xb5：分别是船舶轮廓上的五个顶点。
• 这些点一起形成一个大致的船体几何轮廓，船长 4.5 米，宽 1.5 米。

4. 从船体坐标系到全球坐标系的旋转

Rb_n=[cos(psai) -sin(psai); sin(psai) cos(psai)];

• Rb_n 是从船体坐标系 {b} 到全球坐标系 {n} 的 旋转矩阵。

• psai 表示船舶的航向角，矩阵 Rb_n 根据 psai 的值将船舶坐标系中的顶点转换到全球坐标系中。

5. 坐标变换

xn1=Rb_n*xb1+pos;
xn2=Rb_n*xb2+pos;
xn3=Rb_n*xb3+pos;
xn4=Rb_n*xb4+pos;
xn5=Rb_n*xb5+pos;

• 通过旋转矩阵 Rb_n 和当前位置 pos，将船体坐标系 {b} 下的 5 个顶点变换到全球坐标系 {n} 下。

• xn1, xn2, xn3, xn4, xn5 分别是这 5 个顶点在全球坐标系下的对应位置。

6. 绘制船舶模型

figure(1);
N=pos(1); E=pos(2);
plot(E,N,'r-','linewidth',2);  hold on % plot the navagation line

• figure(1)：指定使用图形窗口 1。
• plot(E,N,'r-','linewidth',2)：绘制当前船舶位置 pos 的红色点，代表船舶的当前位置。

plot([xn1(2) xn2(2)],[xn1(1) xn2(1)],'k-',...
     [xn2(2) xn3(2)],[xn2(1) xn3(1)],'k-',...
     [xn3(2) xn4(2)],[xn3(1) xn4(1)],'k-',...
     [xn4(2) xn5(2)],[xn4(1) xn5(1)],'k-',...
     [xn5(2) xn1(2)],[xn5(1) xn1(1)],'k-','linewidth',4);

• 这部分代码依次连接船舶模型的 5 个顶点，使用黑色线条将它们连接成一个封闭的船舶轮廓。
• 每个顶点通过 xn1, xn2, xn3 等依次连接，绘制出船体的形状，'k-' 表示黑色线条，'linewidth',4 设置了线条的宽度。

7. 坐标轴标签

xlabel('E/��'); ylabel('N/��');

• 设置图形的坐标轴标签。这里 E 代表东向坐标，N 代表北向坐标。

全部代码

function  modelplot( pos, psai )
% MODELPLOT modelplot(pos,psai) :plot the geometrical model of ship in real time
%
% Input:  pos; ship real time position in {n}, pos=[x y]'; 
%         psai: real time yaw angle
% Output: none
%
% Auther: Quyinsong
% Data:   12nd Jan 2022

% check of input dimensions
if nargin~=2 
    error('the number of input must be 2');end
if length(pos)~=2 
    error('the number of pos must be 2');end
if length(psai)~=1 
    error('the number of psai must be 1');end
% ship geometrical shape is characterized by five point:
% xb1,xb2,xb3,xb4,xb5, their coodinates are described in {b} 
% ship  length: 4.5m   width: 1.5m
xb1=[0.35 -0.375]';xb2=[0.65 0]';xb3=[0.35 0.375]';xb4=[-0.35 0.375]';xb5=[-0.35 -0.375]';
% rotation matrix from {b} to {n}
Rb_n=[cos(psai) -sin(psai);
      sin(psai) cos(psai)]; 
% trasfer the five point in {b} to {n}
xn1=Rb_n*xb1+pos;
xn2=Rb_n*xb2+pos;
xn3=Rb_n*xb3+pos;
xn4=Rb_n*xb4+pos;
xn5=Rb_n*xb5+pos;
% plot
figure(1);
N=pos(1); E=pos(2);
plot(E,N,'r-','linewidth',2);  hold on % plot the navagation line
plot([xn1(2) xn2(2)],[xn1(1) xn2(1)],'k-',[xn2(2) xn3(2)],[xn2(1) xn3(1)],'k-',...
[xn3(2) xn4(2)],[xn3(1) xn4(1)],'k-',[xn4(2) xn5(2)],[xn4(1) xn5(1)],'k-',...
[xn5(2) xn1(2)],[xn5(1) xn1(1)],'k-','linewidth',4); % plot ship model

% set(gca,'xTick',0:10:100);
% set(gca,'yTick',0:10:100);
% axis([0 50,0 50]);
xlabel('E/��');ylabel('N/��');
end