【无人水面艇路径跟随控制8】（Matlab）USV代码阅读：通过视线引导算法和PID控制器来实现无人水面艇的直线路径跟踪
- 写在最前面
- LOS.m
- 代码思路
- 1. **参数初始化**
- 2. **仿真时间设置**
- 3. **仿真主循环**
- 3.1 **视线引导法（LOS）**
- 3.2 **控制律**
- 3.3 **动力学模型**
- 3.4 **PID 控制输出**
- 3.5 **状态更新**
- 4. **结果绘制**
- 5. **控制器调节**
- 6. **总结**
- 代码解释
- 全部代码
写在最前面
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 在水面上跟随指定直线路径的仿真，并且展示了控制器的设计和性能。
代码解释
以下是代码的详细解释：
-
初始化和清理：
clc clear all清除命令窗口和工作区中的所有变量。
-
定义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的质量矩阵和阻尼系数等参数。
-
生成两个目标点：
xk =[5 5]';yk =[300 150]'; afak=atan2(yk(2)-xk(2),yk(1)-xk(1));定义了两个目标点，并计算了它们之间的角度
afak。 -
初始化变量：
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和一些控制参数。 -
仿真循环：
disp('Simulation ...'); for k=1:1:Nstime(k)=(k-1)*ts;% LOS lawKp1=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 lawu = 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% matrixn11=-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。 - 矩阵计算：计算阻尼矩阵的各个元素。
- LOS控制律：计算横向误差
全部代码
% 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:Nstime(k)=(k-1)*ts;% LOS lawKp1=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 lawu = 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% matrixn11=-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;% USVd = [1*randn(1,1) 2*randn(1,1) 2*randn(1,1)]';xdot=USV(x,tao,[0 0]',[0,0]',d);% state updatex=euler2(xdot,x,ts);% store time seriesxout(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:Nspos =[N(k) E(k)]';if k==1modelplot(pos,psai(k));endif rem(k*ts,5)==0modelplot(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欸 。如果你喜欢我的文章，欢迎三连给我鼓励和支持：👍点赞 📁 关注 💬评论，我会给大家带来更多有用有趣的文章。
原文链接 👉 ，⚡️更新更及时。
欢迎大家添加好友交流。
