【无人水面艇路径跟随控制11】（Matlab）USV代码阅读：USV返回USV的状态导数（即状态随时间的变化率），仿真 USV 的运动过程。函数考虑了风、洋流的影响，并结合了船体的动力学参数和控制输入

---

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

---

写在最前面

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

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

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

USV.m

这段 MATLAB 代码定义了一个用于 USV（Unmanned Surface Vehicle，无人水面艇） 动力学模型的函数 USV。该函数返回 USV 的状态导数（即状态随时间的变化率），用于仿真 USV 的运动过程。函数考虑了风、洋流的影响，并结合了船体的动力学参数和控制输入。

总结

• 该函数实现了一个 USV 动力学模型，考虑了风、洋流和控制输入的影响，返回状态的导数，供仿真过程中更新 USV 的状态。
• 函数通过矩阵运算描述船舶的动力学行为，并结合了物理参数，如船体质量、风和洋流对运动的影响。

代码详解

下面是详细的解析：

1. 函数输入与输出

输入：

• x：USV 当前的状态向量 [u, v, r, x, y, psai]，其中：

  • u：纵向速度（m/s）
  • v：横向速度（m/s）
  • r：航向角速度（rad/s）
  • x：船舶在全球坐标系下的 x 方向位置（m）
  • y：船舶在全球坐标系下的 y 方向位置（m）
  • psai：航向角（rad）

• tao：控制输入 [tx, ty, tn]'，分别是横向力、纵向力和航向力矩。

• wind：风速和风向 [Vw, betaw]'，其中 Vw 是风速，betaw 是风向角。

• current：洋流速度和方向 [Vc, betac]'，其中 Vc 是洋流速度，betac 是洋流方向角。

• d：环境扰动（如噪声）。

输出：

• xdot：状态导数 [udot, vdot, rdot, xdot, ydot, psaidot]'，分别是纵向速度、横向速度、航向角速度、位置和航向角随时间的变化率。

2. 状态和输入检查

代码首先检查输入的数量和维度，确保输入的状态、控制向量、风速和水流的大小都是预期的。

3. USV 当前状态提取

u = x(1);  % 纵向速度
v = x(2);  % 横向速度
r = x(3);  % 航向角速度
psai = x(6);  % 航向角

• 提取 USV 当前的速度（u, v, r）和航向角 psai。

4. 坐标变换矩阵

Rbn = [cos(psai) -sin(psai) 0;
       sin(psai) cos(psai)  0;
       0          0         1];

• Rbn 是从船体坐标系 {b} 到全球坐标系 {n} 的旋转矩阵，利用 psai（航向角）将船体上的位置和速度转换为全球坐标系下的表示。

5. 风的影响

Vw = wind(1);  % 风速
betaw = wind(2);  % 风向角
uwb = Vw * cos(betaw - psai);  % 风在船体纵向的影响
vwb = Vw * sin(betaw - psai);  % 风在船体横向的影响
urw = u - uwb;  % 纵向相对风速
vrw = v - vwb;  % 横向相对风速
Vrw = sqrt(urw^2 + vrw^2);  % 风相对速度的合速度
gamaw = -atan2(vrw, urw);  % 风相对于船的角度
twind = 1/2 * 0.01 * Vrw^2 * [-0.5 * cos(gamaw), -0.7 * sin(gamaw), -0.05 * sin(2 * gamaw)]';

• 这部分代码计算了风对船体运动的影响：
  • uwb 和 vwb 是风在船体纵向和横向上的投影。
  • urw 和 vrw 是船舶相对于风的速度。
  • twind 是风对船体产生的力和力矩，它取决于相对风速的平方和风的方向。

6. 洋流的影响

Vc = current(1);  % 洋流速度
betac = current(2);  % 洋流方向
Vcn = Vc * [cos(betac), sin(betac), 0]';  % 洋流在全球坐标系下的表示
Vcb = Rbn' * Vcn;  % 洋流转换到船体坐标系下的表示
ucb = Vcb(1);  % 洋流对纵向的影响
vcb = Vcb(2);  % 洋流对横向的影响
ur = u - ucb;  % 相对洋流的纵向速度
vr = v - vcb;  % 相对洋流的横向速度
Vcr = [ur, vr, r]';  % 相对于洋流的速度向量

• Vc 和 betac 分别表示洋流的速度和方向。
• 通过坐标变换，将洋流从全球坐标系转换到船体坐标系下，然后计算船舶相对于洋流的速度 ur 和 vr。

7. 动力学模型

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 的动力学参数，包括质量矩阵的分量（如 m11, m22, m33），以及阻尼力矩系数（如 Xu, Yv, Nv）。

8. 矩阵定义

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];

• M 是 USV 的质量矩阵，Crb 是刚体的科氏力矩阵，Nvr 是阻尼矩阵。
• 这些矩阵用于描述 USV 的动力学行为。

9. 计算状态导数

Vdot = M \ (-Crb * V - Nvr * Vcr + tao + d);  % 计算速度导数
Xdot = Rbn * V;  % 计算位置导数
xdot = [Vdot; Xdot];  % 返回状态导数

• Vdot 是船体速度（u, v, r）的导数，通过动力学方程计算。
• Xdot 是船舶位置（x, y）和航向角 psai 的导数，通过坐标变换和当前速度计算。
• xdot 包含了所有状态的导数，供仿真过程使用。

全部代码

function xdot = USV( x,tao,wind,current,d )
% USV xdot = USV( x,tao,taod ) returns the time derivative of 
% the state vector: x = [ u v r x y psi]'  for USV, where
% INPUT: 
% u=x(1): surge velocity (m/s)
% v=x(2): sway velocity (m/s)
% r=x(3): yaw velocity (rad/s)
% x=x(4): position in {n} x-direction (m)
% y=x(5): position in {n} y-direction (m)
% psai=x(6): yaw angle (rad)
% tao=[tx ty tn]':
% wind=[Vw betaw]'
% current=[Vc betac]'
% OUTPUT: 
% xdot=[udot vdot rdot xdot ydot psaidot Vcdot]':time derivative of state vector

% Author: Quyinsong
% Data: 13rd Jan 2022
% model equitions: kinetics: M*vdot+Crb*v+Nvr*vr=tao+wind+d  (vr=v-vc)
%                   xdot=Rbn*v

% check input and state dimentions
if nargin ~=5,error('input number must be 5!');end
if length(x) ~=6,error('state x number must be 6!');end
if length(tao) ~=3,error('ctr input tao number must be 3!');end
if length(wind) ~=2,error('wind input tao number must be 2!');end
if length(current) ~=2,error('current input tao number must be 2!');end
if length(d) ~=3,error('diturbance taod number must be 3!');end

% USV state:
u=x(1);
v=x(2);
r=x(3);
psai=x(6);
V=[u v r]';
% transfer matrix
Rbn=[ cos(psai) -sin(psai) 0;
      sin(psai) cos(psai)  0;
      0          0         1];
% wind
Vw=wind(1);
betaw=wind(2);
uwb=Vw*cos(betaw-psai);
vwb=Vw*sin(betaw-psai);
urw=u-uwb; vrw=v-vwb;
Vrw=sqrt(urw^2+vrw^2);
gamaw=-atan2(vrw,urw);
twind=1/2*0.01*Vrw^2*[-0.5*cos(gamaw)*1 
                      -0.7*sin(gamaw)*1 
                      -0.05*sin(2*gamaw)*1];
% curremt
Vc=current(1);
betac= current(2);
Vcn=Vc*[cos(betac) sin(betac) 0]';
Vcb=Rbn'*Vcn;
ucb=Vcb(1);
vcb=Vcb(2);
ur=u-ucb;
vr=v-vcb;
Vcr=[ur vr r]';
% 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;
n11=-Xu-Xuu*abs(ur);
n22=-Yv-Yvv*abs(vr)-Yrv*abs(r);
n23=-Yr-Yvr*abs(vr)-Yrr*abs(r);
n32=-Nv-Nvv*abs(vr)-Nrv*abs(r);
n33=-Nr-Nvr*abs(vr)-Nrr*abs(r);
% matrix
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];
% yaw force limit
% tu = tao(1);
% tv = tao(2);
% tr = tao(3);
% if tr >= 100000
%     tr_limit = 100000;
% else
%     tr_limit = tr;
% end
% tao = [tu tv tr_limit]';
% time derivatives

Vdot=M\(-Crb*V-Nvr*Vcr+tao+d) ;
Xdot=Rbn*V;
xdot=[Vdot ;Xdot];

% components expression
% detM2=m22*m33-m23*m32;
% xdot=[ ];

end

---

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

欢迎大家添加好友交流。