简介面向无人艇运动控制方向的研究生与开发者这是一套基于 LOS 视线法的欠驱动无人艇路径跟踪 MATLAB/Simulink 工程可解决推进自由度受限条件下的航线跟踪与航向调节问题。压缩包共 24 个文件整体约 806KB以 .m 脚本和 .slx 模型为主另有 PDF 说明、MAT 航路点文件及 README 文档代码覆盖航向控制、速度控制、路径生成、LOS 导引与跟踪仿真等模块便于按层次拆解学习。目前已有 7790 人浏览学习属于该领域较受关注的代码包。从文件结构和运行入口看读者可先通过主脚本了解整体流程再结合视线角计算函数与相关 Simulink 模型理解 LOS 导引律、制导指令生成及系统搭建方法同时可参考实验说明理清背景借助航路点数据复现指定航线逐步掌握欠驱动无人艇建模、控制器设计与滤波稳定模块的工程实现。这些模块能帮助读者进一步将相关方法迁移到海洋科学、环境监测等无人艇应用场景提升对完整控制链路与仿真验证方法的认识。 我之前帮一个做无人艇路径跟踪的朋友排过一次问题他的船在湖试里走直线航段总是在目标点附近反复画圈无论怎么调控制器增益都压不住。后来我让他把控制目标从一个“固定目标点”换成“路径前方一定距离的点”船立刻变乖了。这就是LOS视线法的核心思想也是无人艇路径跟踪里最经典、最实用的导引方法之一。这篇文章会把LOS从原理、MATLAB代码到Simulink建模整个链路讲透适合正在做无人艇、无人船相关课题的学生以及刚开始接触路径跟踪算法、被各种文献公式绕晕的工程师。你放心我不会堆公式所有数学推导都会落到可复现的代码里。你拿到手之后把参数改成自己的船型就能跑起来。1. 为什么无人艇“对准目标点”开反而走不出直线1.1 无人艇的欠驱动特性和路径跟踪难点无人艇在地面上看跟普通小车有点像但它有一个本质区别它是欠驱动的。小车可以靠前轮转向实现横向位移无人艇在水面上没有横向推进器只能靠尾部螺旋桨推力和差速产生的转艏力矩来改变前进方向。也就是说船不能直接“横着走”想要靠近一条路径只能通过调整航向一点点逼近。这就是路径跟踪问题的核心难点控制输入是航向和速度控制效果却是位置和横向误差。如果只盯着目标点做比例控制当船离目标点越来越近的时候几何上需要的航向修正角度会越来越大控制器就会越来越“紧张”很容易出现超调、画圈甚至绕回原点。另外要区分一个概念路径跟踪Path Following和轨迹跟踪Trajectory Tracking不是一回事。轨迹跟踪要求船在指定时刻到达指定位置对时间严格约束很难实现工程上很少用。路径跟踪只要求船收敛到期望路径上并沿着路径走对“什么时候到达”没有硬性要求这非常符合无人艇的航行特性也是LOS能成为主流方案的原因。1.2 三种典型导引思路对比为什么LOS胜出我做这一块的时候对比过三种常见的导引方案这里把结论直接给出来导引方案基本原理优点缺点纯跟踪瞄目标点把控制目标设为当前目标航路点实现简单概念直观接近目标点时航向变化剧烈易振荡画圈LOS视线法把控制目标设为路径前方固定距离的视线点收敛平滑抗扰动强计算量小需要合理整定前视距离参数横向偏差PID直接控制直接用横向误差e_y做PID得到舵令反馈路径清晰缺少航向前馈动态响应慢大误差时容易饱和纯跟踪虽然实现简单但它的“视距”是不断缩短的到终点附近视线角急剧变化船就会像追着自己尾巴跑的狗一样。PID直接反馈横向误差的问题在于误差变化本身滞后于船的实际运动容易产生相位延迟。LOS法的巧妙之处在于始终看向路径前方一个固定距离的点既保证了航向的连续性又把横向误差收敛过程变成了一阶惯性过程天然稳定。2. 从几何直觉到MATLAB代码LOS导引率的落地思路2.1 坐标变换与关键误差量先建立基础坐标系。大地坐标系用NED北东地系的北向和东向船的位置记为(x, y)艏向角记为ψ。期望路径由一系列航路点构成当前段从Pk出发指向Pk1。路径方向角可以这样算chi_p atan2(Pk1(2) - Pk(2), Pk1(1) - Pk(1));这个角度就是期望路径的方位角。接下来要把当前船位投影到这段路径上得到沿路径方向的距离s和垂直方向的横向误差e_ye_x (x - Pk(1)) * cos(chi_p) (y - Pk(2)) * sin(chi_p); e_y -(x - Pk(1)) * sin(chi_p) (y - Pk(2)) * cos(chi_p);e_x是船在路径方向上的投影位置e_y是船偏离路径的横向距离。注意这个e_y是有正负的船在路径右侧为正左侧为负这个符号后面很关键很多人调LOS调不出来就是这里正负搞反了。2.2 视线点与前视距离的数学角色有了e_x和e_y视线点PLOS的定义就很简单了PLOS Pk (e_x Delta) * [cos(chi_p); sin(chi_p)];也就是说先从船的位置沿路径方向投影到路径上再沿着路径往前走一段Delta那个点就是视线点。然后期望艏向角就是船指向视线点的角度psi_d atan2(PLOS(2) - y, PLOS(1) - x);Delta这个参数叫前视距离它的物理意义可以类比成开车时看多远。你开车不会一直盯着车头前面的地面线而是看远处某个点这样方向盘才稳。Delta太小修偏很急船容易左右摆头Delta太大收敛路径慢走大弯道时容易切内线。工程经验上Delta通常取船长的一到三倍但具体多少要看控制周期和船速后面参数整定部分会展开。LOS其实还有一种等价写法就是解析公式psi_d chi_p - atan(e_y / Delta)这个公式和视线点法是等价的但实际写代码时我更推荐直接算视线点角度因为视线点法对后续扩展比如动态拦截、避障更自然而且不用纠结符号约定。2.3 核心函数代码与逐行说明下面是LOS导引率的核心函数我用MATLAB写的方便你直接抄function [psi_d, e_y, PLOS] los_guidance(x, y, Pk, Pk1, Delta) % 输入船位(x,y)当前航路点Pk下一航路点Pk1前视距离Delta % 输出期望艏向角psi_d横向误差e_y视线点PLOS % 路径方位角 chi_p atan2(Pk1(2) - Pk(2), Pk1(1) - Pk(1)); % 投影到路径坐标系 e_x (x - Pk(1)) * cos(chi_p) (y - Pk(2)) * sin(chi_p); e_y -(x - Pk(1)) * sin(chi_p) (y - Pk(2)) * cos(chi_p); % 视线点 PLOS Pk (e_x Delta) * [cos(chi_p); sin(chi_p)]; % 期望艏向 psi_d atan2(PLOS(2) - y, PLOS(1) - x); psi_d wrapToPi(psi_d); % 把角度归一化到[-pi, pi] end注意wrapToPi这一步很重要。艏向角如果不做归一化当实际角度从179度变到-179度时误差会变成358度控制器会以为要转一整圈实际转2度就够了。这个坑我在初版代码里踩过跑出来的轨迹是疯狂自旋。3. 多航路点场景下的完整仿真切换逻辑与全套可运行代码3.1 航路点切换制度实际路径通常不是一条直线而是多个航路点组成的折线。这时候必须处理航路点切换问题。最常用的切换制度是“圆切换法”当船进入当前目标点周围半径R_switch的圆内时认为已经到达该点把当前目标切到下一个航路点。dist norm([x - Pk1(1), y - Pk1(2)]); if dist R_switch idx idx 1; % 切换到下一个航路点 endR_switch怎么选我一般取船长的两到三倍或者比Delta略小一些。如果切换半径太大船会在离航路点很远的地方就转向路径的转角处会被“切角”如果太小船可能会冲过头再折返造成不必要的航向震荡。比较稳妥的做法是在最后一个航路点进入圆后停止切换让船最终泊定在目标点附近。3.2 完整仿真脚本下面是一个完整的MATLAB仿真脚本模型用简化的三自由度运动学控制量是转艏角速度r_cmd适合先验证LOS逻辑后续再升级成更真实的动力学模型% 无人艇LOS路径跟踪完整仿真脚本 clear; clc; close all; % 参数设置 L 3; % 艇长 [m] U 2; % 前进速度 [m/s]恒定 Delta 2 * L; % 前视距离 R_switch 2 * L; % 航路点切换半径 % 航路点可自行修改 wps [0, 0; 200, 0; 400, 150; 400, 400]; wp_cur wps(1, :); wp_next wps(2, :); idx 2; % 初始状态 x 0; y -20; psi 0; r 0; dt 0.1; t_end 600; N t_end / dt; % 控制器参数 Kp 2.0; Kd 1.5; r_max 0.8; % 转艏角速度限幅 [rad/s] % 数据记录 x_log zeros(N,1); y_log zeros(N,1); psi_log zeros(N,1); e_y_log zeros(N,1); psi_d_log zeros(N,1); idx_log ones(N,1); for k 1:N % 航路点切换 dist_to_wp norm([x - wp_next(1), y - wp_next(2)]); if dist_to_wp R_switch idx idx 1; if idx size(wps, 1) idx size(wps, 1); end wp_cur wps(idx-1, :); wp_next wps(idx, :); end % LOS导引 [psi_d, e_y, ~] los_guidance(x, y, wp_cur, wp_next, Delta); % 航向控制器PD e_psi wrapToPi(psi_d - psi); r_cmd Kp * e_psi - Kd * r; r_cmd max(-r_max, min(r_max, r_cmd)); % 运动学更新简化的欠驱动模型 x x U * cos(psi) * dt; y y U * sin(psi) * dt; psi psi r_cmd * dt; psi wrapToPi(psi); r r_cmd; % 记录 x_log(k) x; y_log(k) y; psi_log(k) psi; e_y_log(k) e_y; psi_d_log(k) psi_d; idx_log(k) idx; end % 画图 figure; plot(wps(:,1), wps(:,2), k--o, LineWidth, 1.2); hold on; plot(x_log, y_log, b-, LineWidth, 1.2); xlabel(东向 [m]); ylabel(北向 [m]); legend(期望路径, 实际轨迹); axis equal; grid on; figure; subplot(2,1,1); plot((1:N)*dt, x_log, b); hold on; plot((1:N)*dt, y_log, r); xlabel(时间 [s]); ylabel(位置 [m]); legend(x, y); grid on; subplot(2,1,2); plot((1:N)*dt, e_y_log, m); xlabel(时间 [s]); ylabel(横向误差 [m]); grid on;这段代码我故意简化了模型没有加侧滑、海流和风浪就是为了让LOS的逻辑主线足够清晰。动力学模型这块正式做课题时建议把运动学模型换成一阶Nomoto模型再加海流但导引层逻辑不需要改动这也正是分层设计的优势。3.3 仿真结果怎么解读跑完这段代码你会看到船从初始横向偏差20米的地方开始收敛到路径然后沿路径走过了第一个航路点后平滑转向下一段。看结果图重点看两个地方横向误差e_y收敛曲线理想情况下应该单调收敛并稳定在0附近如果看到e_y在0附近反复穿越、振幅不衰减说明Delta太小或者Kd不够。实际轨迹转折处正常情况是船在接近航路点前就开始转向轨迹是一条平滑的弧线从上一段切到下一段不应该出现明显的“冲过头再拉回来”的现象。如果发现船绕路或者在某一段反复打转优先检查切换逻辑idx有没有正确推进以及psi_d有没有被wrapToPi归一化。4. Simulink建模时应该注意的模块划分与代数环问题4.1 Simulink模块划分建议用Simulink做这个系统建议按以下模块层次划分方便后面改成半实物仿真路径管理模块内部存航路点表接收当前船位输出当前路径段的Pk、Pk1和切换标志。LOS导引模块用MATLAB Function块写输入x、y、Pk、Pk1、Delta输出psi_d同时可选输出e_y供记录。航向控制器模块输入psi_d、psi、r输出r_cmd内部做角度归一化和限幅。船舶动力学模块可以用S函数或者直接用Integrator模块搭运动学方程输入r_cmd和U输出x、y、psi、r。显示与记录模块Scope、To Workspace。这个分层的好处是每一层都可以单独调试。我做的时候习惯先单独测LOS模块给定一系列静态位置看psi_d输出是否符合几何直觉再接入控制器闭环调参。4.2 MATLAB Function块与代数环避坑LOS导引在Simulink里最自然的做法是用MATLAB Function块。但有一个经典坑如果动力学模块输出的x、y没有经过积分器或者Memory延迟直接反馈到LOS的输入Simulink会报代数环错误。我遇到过的情况是这样的我在LOS模块里为了省事直接用了一些临时状态变量来算路径投影结果这些状态变量跟x、y构成了一条没有时间延迟的反馈回路Simulink立刻报错“Algebraic loop detected”。解决办法有二在反馈回路上加一个Memory块或者Unit Delay块把连续状态改成离散状态打破代数环。更推荐的做法是在LOS模块里不要保存任何内部状态所有输出都只由当前输入x、y、Pk、Pk1决定这样动力学积分器本身就是天然的延迟环节一般不会形成代数环。另外MATLAB Function块里尽量别用复杂的类对象保持纯函数风格Simulink的代码生成器对纯函数支持最好。4.3 固定步长与离散求解器设置Simulink参数设置里面建议把求解器类型改成固定步长步长0.01到0.1秒跟实际控制器执行周期对齐。为什么不建议用变步长变步长求解器虽然精度高但MATLAB Function块内部如果有离散状态操作变步长会导致触发时刻不确定仿真结果和实际控制器对不上。我用固定步长ode4跑出来的结果跟真船上的离散控制周期基本一致。如果模型里用了PWM调制或通信时延这类离散环节步长还要综合考虑一般不低于10毫秒。5. 参数整定经验与海流工况下的改进方向5.1 前视距离与控制器增益的整定步骤调参顺序比参数本身重要。我总结了一套流程照着调基本不会乱第一步固定Delta取一个比较大的值比如5倍船长这样航向响应最平滑先把航向控制器的Kp和Kd调出来。第二步观察航向跟踪情况。ekf差减小即可如果psi振荡加Kd如果psi有稳态偏差加Kp或加积分环节。第三步慢慢减小Delta观察横向误差e_y的收敛速度。e_y收敛越来越快直到开始出现超调或振荡就退回到上一个合适的值。第四步如果发现低速大偏差场景下收敛太慢可以上自适应Delta让船离路径远时看近些、离路径近时看远些。一个典型的Delta自适应公式是Delta Delta_min (Delta_max - Delta_min) * exp(-k_adapt * e_y^2);这里e_y大时Delta小修偏动作更激进e_y小时Delta大路径跟踪更平滑。k_adapt一般取0.1到1之间具体看船的响应速度。这个公式实现起来就一行但效果比固定Delta好不少。5.2 我踩过的三个坑下面三个坑都是真实调试中遇到过的写出来帮你省时间。第一个坑是e_y符号反了。我用解析公式psi_d chi_p - atan(e_y / Delta)时没有仔细验证e_y在路径左侧和右侧时的符号导致船往错误方向修偏轨迹直接画了一个大圆圈。排查方法是把船手动放在路径右侧几米物理直觉告诉我们应该左转如果代码算出来的psi_d是往右偏那肯定符号反了。视线点法天然避开了这个坑所以我推荐先用视线点法。第二个坑是视线点“穿”到了终点之后。当船超过当前航路点的投影位置时e_x加大视线点可能跑到下一段路径外面船就会过度转向甚至绕路。解决办法是加切换逻辑在投影点离终点距离小于某个阈值时强制把当前段终点作为临时视线点或者提前切换到下一段。多航路点场景里这个bug非常隐蔽因为有时候看起来轨迹只是“绕远了一下”不容易想到是这里出了问题。第三个坑是Delta太小加上海浪激励导致极限环振荡。在湖试中风浪会给船一个持续的扰动如果Delta定得很小、控制器增益又高船会进入一种“修正—超调—反向修正—再超调”的极限环看起来就像在海面上画正弦波。这种振荡靠调Kp和Kd往往压不下去因为根因在导引层的视野太短。把Delta从0.8倍船长调到2倍船长振荡立刻消失。这个经验后面做真船时起了大作用。5.3 有海流时怎么办ILOS与漂角补偿上述代码都是基于无风无流的理想假设实际湖试或者海上海流是绕不开的。海流会导致两个问题一是船即使对准了期望艏向也会被流推离路径二是艇体本身有漂角β艇速方向和艏向不一致只用地速和艏向算LOS会有稳态误差。解决思路有两条路线积分LOSILOS在LOS导引率中加入横向误差的积分项让系统在常值海流下也能消除稳态误差。修正后的期望艏向写成psi_d chi_p - atan(e_y / Delta k_int * e_y_int)其中e_y_int是e_y的积分k_int取值很小一般0.01到0.1量级。实现时注意要对e_y_int做限幅防止积分饱和。漂角补偿如果船上有流速传感器或能估计漂角β可以直接在期望艏向的基础上修正psi_d_corrected psi_d - beta。这个思路更直接但依赖状态估计精度。我实际测试下来ILOS在不知道海流大小的情况下依然有效代码改动很小所以工程上优先推荐。先跑通ILOS再谈漂角估计。做完整套算法和仿真之后我最大的一个感受是LOS看着简单工程里真正做好靠的是对几何关系的直觉和对细节的执着。那些画圈的轨迹、振荡的曲线、跳变的航向背后往往都是很小的问题。如果你在自己调试时遇到类似的情况先把前视距离调大两倍试一下很多时候困扰你的“疑难杂症”就这么简单——这个参数是LOS法的灵魂也是最容易被忽视的地方。本文还有配套的精品资源点击获取 无人艇LOS视线法路径跟踪：原理、MATLAB仿真与Simulink建模
简介面向无人艇运动控制方向的研究生与开发者这是一套基于 LOS 视线法的欠驱动无人艇路径跟踪 MATLAB/Simulink 工程可解决推进自由度受限条件下的航线跟踪与航向调节问题。压缩包共 24 个文件整体约 806KB以 .m 脚本和 .slx 模型为主另有 PDF 说明、MAT 航路点文件及 README 文档代码覆盖航向控制、速度控制、路径生成、LOS 导引与跟踪仿真等模块便于按层次拆解学习。目前已有 7790 人浏览学习属于该领域较受关注的代码包。从文件结构和运行入口看读者可先通过主脚本了解整体流程再结合视线角计算函数与相关 Simulink 模型理解 LOS 导引律、制导指令生成及系统搭建方法同时可参考实验说明理清背景借助航路点数据复现指定航线逐步掌握欠驱动无人艇建模、控制器设计与滤波稳定模块的工程实现。这些模块能帮助读者进一步将相关方法迁移到海洋科学、环境监测等无人艇应用场景提升对完整控制链路与仿真验证方法的认识。 我之前帮一个做无人艇路径跟踪的朋友排过一次问题他的船在湖试里走直线航段总是在目标点附近反复画圈无论怎么调控制器增益都压不住。后来我让他把控制目标从一个“固定目标点”换成“路径前方一定距离的点”船立刻变乖了。这就是LOS视线法的核心思想也是无人艇路径跟踪里最经典、最实用的导引方法之一。这篇文章会把LOS从原理、MATLAB代码到Simulink建模整个链路讲透适合正在做无人艇、无人船相关课题的学生以及刚开始接触路径跟踪算法、被各种文献公式绕晕的工程师。你放心我不会堆公式所有数学推导都会落到可复现的代码里。你拿到手之后把参数改成自己的船型就能跑起来。1. 为什么无人艇“对准目标点”开反而走不出直线1.1 无人艇的欠驱动特性和路径跟踪难点无人艇在地面上看跟普通小车有点像但它有一个本质区别它是欠驱动的。小车可以靠前轮转向实现横向位移无人艇在水面上没有横向推进器只能靠尾部螺旋桨推力和差速产生的转艏力矩来改变前进方向。也就是说船不能直接“横着走”想要靠近一条路径只能通过调整航向一点点逼近。这就是路径跟踪问题的核心难点控制输入是航向和速度控制效果却是位置和横向误差。如果只盯着目标点做比例控制当船离目标点越来越近的时候几何上需要的航向修正角度会越来越大控制器就会越来越“紧张”很容易出现超调、画圈甚至绕回原点。另外要区分一个概念路径跟踪Path Following和轨迹跟踪Trajectory Tracking不是一回事。轨迹跟踪要求船在指定时刻到达指定位置对时间严格约束很难实现工程上很少用。路径跟踪只要求船收敛到期望路径上并沿着路径走对“什么时候到达”没有硬性要求这非常符合无人艇的航行特性也是LOS能成为主流方案的原因。1.2 三种典型导引思路对比为什么LOS胜出我做这一块的时候对比过三种常见的导引方案这里把结论直接给出来导引方案基本原理优点缺点纯跟踪瞄目标点把控制目标设为当前目标航路点实现简单概念直观接近目标点时航向变化剧烈易振荡画圈LOS视线法把控制目标设为路径前方固定距离的视线点收敛平滑抗扰动强计算量小需要合理整定前视距离参数横向偏差PID直接控制直接用横向误差e_y做PID得到舵令反馈路径清晰缺少航向前馈动态响应慢大误差时容易饱和纯跟踪虽然实现简单但它的“视距”是不断缩短的到终点附近视线角急剧变化船就会像追着自己尾巴跑的狗一样。PID直接反馈横向误差的问题在于误差变化本身滞后于船的实际运动容易产生相位延迟。LOS法的巧妙之处在于始终看向路径前方一个固定距离的点既保证了航向的连续性又把横向误差收敛过程变成了一阶惯性过程天然稳定。2. 从几何直觉到MATLAB代码LOS导引率的落地思路2.1 坐标变换与关键误差量先建立基础坐标系。大地坐标系用NED北东地系的北向和东向船的位置记为(x, y)艏向角记为ψ。期望路径由一系列航路点构成当前段从Pk出发指向Pk1。路径方向角可以这样算chi_p atan2(Pk1(2) - Pk(2), Pk1(1) - Pk(1));这个角度就是期望路径的方位角。接下来要把当前船位投影到这段路径上得到沿路径方向的距离s和垂直方向的横向误差e_ye_x (x - Pk(1)) * cos(chi_p) (y - Pk(2)) * sin(chi_p); e_y -(x - Pk(1)) * sin(chi_p) (y - Pk(2)) * cos(chi_p);e_x是船在路径方向上的投影位置e_y是船偏离路径的横向距离。注意这个e_y是有正负的船在路径右侧为正左侧为负这个符号后面很关键很多人调LOS调不出来就是这里正负搞反了。2.2 视线点与前视距离的数学角色有了e_x和e_y视线点PLOS的定义就很简单了PLOS Pk (e_x Delta) * [cos(chi_p); sin(chi_p)];也就是说先从船的位置沿路径方向投影到路径上再沿着路径往前走一段Delta那个点就是视线点。然后期望艏向角就是船指向视线点的角度psi_d atan2(PLOS(2) - y, PLOS(1) - x);Delta这个参数叫前视距离它的物理意义可以类比成开车时看多远。你开车不会一直盯着车头前面的地面线而是看远处某个点这样方向盘才稳。Delta太小修偏很急船容易左右摆头Delta太大收敛路径慢走大弯道时容易切内线。工程经验上Delta通常取船长的一到三倍但具体多少要看控制周期和船速后面参数整定部分会展开。LOS其实还有一种等价写法就是解析公式psi_d chi_p - atan(e_y / Delta)这个公式和视线点法是等价的但实际写代码时我更推荐直接算视线点角度因为视线点法对后续扩展比如动态拦截、避障更自然而且不用纠结符号约定。2.3 核心函数代码与逐行说明下面是LOS导引率的核心函数我用MATLAB写的方便你直接抄function [psi_d, e_y, PLOS] los_guidance(x, y, Pk, Pk1, Delta) % 输入船位(x,y)当前航路点Pk下一航路点Pk1前视距离Delta % 输出期望艏向角psi_d横向误差e_y视线点PLOS % 路径方位角 chi_p atan2(Pk1(2) - Pk(2), Pk1(1) - Pk(1)); % 投影到路径坐标系 e_x (x - Pk(1)) * cos(chi_p) (y - Pk(2)) * sin(chi_p); e_y -(x - Pk(1)) * sin(chi_p) (y - Pk(2)) * cos(chi_p); % 视线点 PLOS Pk (e_x Delta) * [cos(chi_p); sin(chi_p)]; % 期望艏向 psi_d atan2(PLOS(2) - y, PLOS(1) - x); psi_d wrapToPi(psi_d); % 把角度归一化到[-pi, pi] end注意wrapToPi这一步很重要。艏向角如果不做归一化当实际角度从179度变到-179度时误差会变成358度控制器会以为要转一整圈实际转2度就够了。这个坑我在初版代码里踩过跑出来的轨迹是疯狂自旋。3. 多航路点场景下的完整仿真切换逻辑与全套可运行代码3.1 航路点切换制度实际路径通常不是一条直线而是多个航路点组成的折线。这时候必须处理航路点切换问题。最常用的切换制度是“圆切换法”当船进入当前目标点周围半径R_switch的圆内时认为已经到达该点把当前目标切到下一个航路点。dist norm([x - Pk1(1), y - Pk1(2)]); if dist R_switch idx idx 1; % 切换到下一个航路点 endR_switch怎么选我一般取船长的两到三倍或者比Delta略小一些。如果切换半径太大船会在离航路点很远的地方就转向路径的转角处会被“切角”如果太小船可能会冲过头再折返造成不必要的航向震荡。比较稳妥的做法是在最后一个航路点进入圆后停止切换让船最终泊定在目标点附近。3.2 完整仿真脚本下面是一个完整的MATLAB仿真脚本模型用简化的三自由度运动学控制量是转艏角速度r_cmd适合先验证LOS逻辑后续再升级成更真实的动力学模型% 无人艇LOS路径跟踪完整仿真脚本 clear; clc; close all; % 参数设置 L 3; % 艇长 [m] U 2; % 前进速度 [m/s]恒定 Delta 2 * L; % 前视距离 R_switch 2 * L; % 航路点切换半径 % 航路点可自行修改 wps [0, 0; 200, 0; 400, 150; 400, 400]; wp_cur wps(1, :); wp_next wps(2, :); idx 2; % 初始状态 x 0; y -20; psi 0; r 0; dt 0.1; t_end 600; N t_end / dt; % 控制器参数 Kp 2.0; Kd 1.5; r_max 0.8; % 转艏角速度限幅 [rad/s] % 数据记录 x_log zeros(N,1); y_log zeros(N,1); psi_log zeros(N,1); e_y_log zeros(N,1); psi_d_log zeros(N,1); idx_log ones(N,1); for k 1:N % 航路点切换 dist_to_wp norm([x - wp_next(1), y - wp_next(2)]); if dist_to_wp R_switch idx idx 1; if idx size(wps, 1) idx size(wps, 1); end wp_cur wps(idx-1, :); wp_next wps(idx, :); end % LOS导引 [psi_d, e_y, ~] los_guidance(x, y, wp_cur, wp_next, Delta); % 航向控制器PD e_psi wrapToPi(psi_d - psi); r_cmd Kp * e_psi - Kd * r; r_cmd max(-r_max, min(r_max, r_cmd)); % 运动学更新简化的欠驱动模型 x x U * cos(psi) * dt; y y U * sin(psi) * dt; psi psi r_cmd * dt; psi wrapToPi(psi); r r_cmd; % 记录 x_log(k) x; y_log(k) y; psi_log(k) psi; e_y_log(k) e_y; psi_d_log(k) psi_d; idx_log(k) idx; end % 画图 figure; plot(wps(:,1), wps(:,2), k--o, LineWidth, 1.2); hold on; plot(x_log, y_log, b-, LineWidth, 1.2); xlabel(东向 [m]); ylabel(北向 [m]); legend(期望路径, 实际轨迹); axis equal; grid on; figure; subplot(2,1,1); plot((1:N)*dt, x_log, b); hold on; plot((1:N)*dt, y_log, r); xlabel(时间 [s]); ylabel(位置 [m]); legend(x, y); grid on; subplot(2,1,2); plot((1:N)*dt, e_y_log, m); xlabel(时间 [s]); ylabel(横向误差 [m]); grid on;这段代码我故意简化了模型没有加侧滑、海流和风浪就是为了让LOS的逻辑主线足够清晰。动力学模型这块正式做课题时建议把运动学模型换成一阶Nomoto模型再加海流但导引层逻辑不需要改动这也正是分层设计的优势。3.3 仿真结果怎么解读跑完这段代码你会看到船从初始横向偏差20米的地方开始收敛到路径然后沿路径走过了第一个航路点后平滑转向下一段。看结果图重点看两个地方横向误差e_y收敛曲线理想情况下应该单调收敛并稳定在0附近如果看到e_y在0附近反复穿越、振幅不衰减说明Delta太小或者Kd不够。实际轨迹转折处正常情况是船在接近航路点前就开始转向轨迹是一条平滑的弧线从上一段切到下一段不应该出现明显的“冲过头再拉回来”的现象。如果发现船绕路或者在某一段反复打转优先检查切换逻辑idx有没有正确推进以及psi_d有没有被wrapToPi归一化。4. Simulink建模时应该注意的模块划分与代数环问题4.1 Simulink模块划分建议用Simulink做这个系统建议按以下模块层次划分方便后面改成半实物仿真路径管理模块内部存航路点表接收当前船位输出当前路径段的Pk、Pk1和切换标志。LOS导引模块用MATLAB Function块写输入x、y、Pk、Pk1、Delta输出psi_d同时可选输出e_y供记录。航向控制器模块输入psi_d、psi、r输出r_cmd内部做角度归一化和限幅。船舶动力学模块可以用S函数或者直接用Integrator模块搭运动学方程输入r_cmd和U输出x、y、psi、r。显示与记录模块Scope、To Workspace。这个分层的好处是每一层都可以单独调试。我做的时候习惯先单独测LOS模块给定一系列静态位置看psi_d输出是否符合几何直觉再接入控制器闭环调参。4.2 MATLAB Function块与代数环避坑LOS导引在Simulink里最自然的做法是用MATLAB Function块。但有一个经典坑如果动力学模块输出的x、y没有经过积分器或者Memory延迟直接反馈到LOS的输入Simulink会报代数环错误。我遇到过的情况是这样的我在LOS模块里为了省事直接用了一些临时状态变量来算路径投影结果这些状态变量跟x、y构成了一条没有时间延迟的反馈回路Simulink立刻报错“Algebraic loop detected”。解决办法有二在反馈回路上加一个Memory块或者Unit Delay块把连续状态改成离散状态打破代数环。更推荐的做法是在LOS模块里不要保存任何内部状态所有输出都只由当前输入x、y、Pk、Pk1决定这样动力学积分器本身就是天然的延迟环节一般不会形成代数环。另外MATLAB Function块里尽量别用复杂的类对象保持纯函数风格Simulink的代码生成器对纯函数支持最好。4.3 固定步长与离散求解器设置Simulink参数设置里面建议把求解器类型改成固定步长步长0.01到0.1秒跟实际控制器执行周期对齐。为什么不建议用变步长变步长求解器虽然精度高但MATLAB Function块内部如果有离散状态操作变步长会导致触发时刻不确定仿真结果和实际控制器对不上。我用固定步长ode4跑出来的结果跟真船上的离散控制周期基本一致。如果模型里用了PWM调制或通信时延这类离散环节步长还要综合考虑一般不低于10毫秒。5. 参数整定经验与海流工况下的改进方向5.1 前视距离与控制器增益的整定步骤调参顺序比参数本身重要。我总结了一套流程照着调基本不会乱第一步固定Delta取一个比较大的值比如5倍船长这样航向响应最平滑先把航向控制器的Kp和Kd调出来。第二步观察航向跟踪情况。ekf差减小即可如果psi振荡加Kd如果psi有稳态偏差加Kp或加积分环节。第三步慢慢减小Delta观察横向误差e_y的收敛速度。e_y收敛越来越快直到开始出现超调或振荡就退回到上一个合适的值。第四步如果发现低速大偏差场景下收敛太慢可以上自适应Delta让船离路径远时看近些、离路径近时看远些。一个典型的Delta自适应公式是Delta Delta_min (Delta_max - Delta_min) * exp(-k_adapt * e_y^2);这里e_y大时Delta小修偏动作更激进e_y小时Delta大路径跟踪更平滑。k_adapt一般取0.1到1之间具体看船的响应速度。这个公式实现起来就一行但效果比固定Delta好不少。5.2 我踩过的三个坑下面三个坑都是真实调试中遇到过的写出来帮你省时间。第一个坑是e_y符号反了。我用解析公式psi_d chi_p - atan(e_y / Delta)时没有仔细验证e_y在路径左侧和右侧时的符号导致船往错误方向修偏轨迹直接画了一个大圆圈。排查方法是把船手动放在路径右侧几米物理直觉告诉我们应该左转如果代码算出来的psi_d是往右偏那肯定符号反了。视线点法天然避开了这个坑所以我推荐先用视线点法。第二个坑是视线点“穿”到了终点之后。当船超过当前航路点的投影位置时e_x加大视线点可能跑到下一段路径外面船就会过度转向甚至绕路。解决办法是加切换逻辑在投影点离终点距离小于某个阈值时强制把当前段终点作为临时视线点或者提前切换到下一段。多航路点场景里这个bug非常隐蔽因为有时候看起来轨迹只是“绕远了一下”不容易想到是这里出了问题。第三个坑是Delta太小加上海浪激励导致极限环振荡。在湖试中风浪会给船一个持续的扰动如果Delta定得很小、控制器增益又高船会进入一种“修正—超调—反向修正—再超调”的极限环看起来就像在海面上画正弦波。这种振荡靠调Kp和Kd往往压不下去因为根因在导引层的视野太短。把Delta从0.8倍船长调到2倍船长振荡立刻消失。这个经验后面做真船时起了大作用。5.3 有海流时怎么办ILOS与漂角补偿上述代码都是基于无风无流的理想假设实际湖试或者海上海流是绕不开的。海流会导致两个问题一是船即使对准了期望艏向也会被流推离路径二是艇体本身有漂角β艇速方向和艏向不一致只用地速和艏向算LOS会有稳态误差。解决思路有两条路线积分LOSILOS在LOS导引率中加入横向误差的积分项让系统在常值海流下也能消除稳态误差。修正后的期望艏向写成psi_d chi_p - atan(e_y / Delta k_int * e_y_int)其中e_y_int是e_y的积分k_int取值很小一般0.01到0.1量级。实现时注意要对e_y_int做限幅防止积分饱和。漂角补偿如果船上有流速传感器或能估计漂角β可以直接在期望艏向的基础上修正psi_d_corrected psi_d - beta。这个思路更直接但依赖状态估计精度。我实际测试下来ILOS在不知道海流大小的情况下依然有效代码改动很小所以工程上优先推荐。先跑通ILOS再谈漂角估计。做完整套算法和仿真之后我最大的一个感受是LOS看着简单工程里真正做好靠的是对几何关系的直觉和对细节的执着。那些画圈的轨迹、振荡的曲线、跳变的航向背后往往都是很小的问题。如果你在自己调试时遇到类似的情况先把前视距离调大两倍试一下很多时候困扰你的“疑难杂症”就这么简单——这个参数是LOS法的灵魂也是最容易被忽视的地方。本文还有配套的精品资源点击获取