简介：面向无人艇运动控制研究者的LOS视线法路径跟踪代码包，解决欠驱动无人艇在有限控制输入下的航迹跟踪难题。代码基于目标点与艇体中心线的几何关系计算视线角，通过调节航向角使艇沿预设路径行驶，并针对风浪流干扰引入滤波与稳定策略，提升跟踪鲁棒性。资源共24个文件，包含12个MATLAB脚本与7个Simulink模型，另有PDF说明、MAT数据等，压缩包仅806KB；m文件覆盖路径规划、LOS控制律、航向/速度控制器及仿真运行，slx模型提供路径跟踪、制导与控制闭环仿真框架，便于二次开发。目前已有7790人学习下载。通过阅读并运行这套代码，可完整掌握无人艇建模、控制器设计、滤波及Simulink仿真验证的闭环流程，适合教学演示、课题研究与算法对比，为海洋监测、环境采样等自主航行场景提供直接参考。 做无人艇控制也快五年了，从最早在仿真里对着直线路径调PID，到后来在水池里试真艇，踩过的坑比写过的代码还多。最近好多朋友在问无人艇路径跟踪的LOS视线法怎么实现、代码怎么写，今天就把我手里这套能跑的LOS代码和调试心得整理出来，给需要入门路径跟踪的朋友一个能直接抄作业的参考。
LOS（Line of Sight）视线法在无人艇路径跟踪里属于最经典的制导方案，核心思路特别像人开车看路：盯住前方路径上的某个目标点，船头对着它开过去，随着船靠近路径，横向偏差逐渐收敛到零。相比直接跟踪航路点、或者上MPC这类重型算法，LOS实现成本低、参数直观、鲁棒性好，特别适合无人艇这种执行机构带宽有限的平台。
这篇文章面向的对象是做无人艇或类似水面机器人控制的同学，也适合刚接触路径跟踪、想搞懂LOS原理和代码细节的新手。下面直接从原理推导、仿真代码、参数整定、常见坑这几个维度拆开讲。
1. 理解LOS视线法：路径跟踪的底层逻辑
1.1 无人艇路径跟踪到底在解决什么问题
先想清楚我们在解决什么问题。无人艇在水面航行，导航系统会给出一条参考路径，比如从A点出发经过B点再到C点的折线路径，或者一条曲线路径。路径跟踪任务是：让无人艇的实际轨迹尽可能贴近这条参考路径，并且最终沿路径稳定航行，而不是简单地从A点开到C点就完事。这跟航路点跟踪的本质区别在于，路径跟踪关注的是"在路径上"，而不是"到目标点"。
如果只把航路点当作目标去做纯跟踪，船很容易在折线拐角处“切内角”，轨迹又顿又扭，这在狭窄水域里非常危险。LOS的原理恰好解决这个问题：它把注意力放在船当前正前方的一段“虚拟视线”上，通过选取合适的前视距离，让船平滑地趋向参考路径，而不是死盯着远方的航路点猛打舵。
1.2 视线角的推导与几何意义
LOS视线法的核心是“视线角”的计算。假设无人艇当前坐标为(P_x, P_y)，参考路径为一条直线段，路径方向角为alpha_p。我们把船到路径的垂直距离记为横向偏差e（带正负号，船在路径左侧或右侧）。此时选定的LOS参考点，不是船正前方的最近点，而是在路径上沿前进方向，距离船足点前视距离Delta_horiz处的一个点。
于是期望航向角psi_d的公式可以写成：
psi_d = alpha_p - atan(e / Delta_horiz)
这个式子很有深意：当横向偏差e很大时，atan(e/Delta)趋向±90度，也就是说船会几乎垂直于路径方向猛打舵往路径上靠；当e接近零时，atan趋近0，船就顺着路径方向往前开。前视距离Delta越大，趋近路径的过程越平滑，但收敛越慢；Delta越小，收敛越快，但越容易振荡。这个参数是整个LOS算法的灵魂，后面我会专门讲怎么调。
如果参考路径是曲线，则需要在线寻找路径上距离船当前点前方固定弧长的点，或者直接参数化路径（比如样条、多项式），求期望视线点坐标后，用atan2计算视线角即可。这里为了不把基础讲太复杂，先从直线路径讲起，代码框架后面可以很容易扩展成多段折线和曲线。
2. 从公式到代码：搭建无人艇仿真环境
2.1 简化运动学模型的选择与理由
写LOS代码之前，得有个模拟船运动的对象。实际无人艇动力学模型非常复杂，有纵荡、横荡、艏摇三个自由度，还涉及水动力系数。但在路径跟踪制导律设计阶段，我们更关心的是“制导环”和“控制环”的配合，因此通常采用两种模型：运动学模型或者含一阶舵响应的简化动力学模型。
运动学模型用三自由度简化：
x_dot = u * cos(psi) y_dot = u * sin(psi) psi_dot = r
其中u是纵向速度（常取恒定），psi是艏向角，r是艏摇角速度。更进一步，为了模拟真实的舵机延迟和惯量，可以把控制量（期望舵角或期望转角速度）和被控量之间加一阶惯性环节：
r_dot = (K * delta - r) / T
式中delta是舵角指令，K是舵效系数，T是时间常数。这样做的好处是，代码里能体现出"船不能瞬间转到期望航向"这个物理限制，后续你上真艇时心理落差会小很多。
我的建议是，第一版仿真直接用运动学模型把LOS逻辑跑通，然后再加上一阶舵响应模型，看看控制器性能变化。这也是很多论文在仿真章节采用的标准套路。
2.2 路径表示与投影点计算
直线路径在代码里用两个航路点表示：P_start = (x_start, y_start)，P_end = (x_end, y_end)。路径方向向量v = [x_end - x_start, y_end - y_start]。
船当前点P = (x, y)在路径上的投影，用向量点积计算：
s = dot(P - P_start, v) / dot(v, v)
s就是船在路径方向上的归一化投影距离（0到1之间）。这样我们可以得到：
- 最近的路径点P_proj = P_start + s * v
- 横向偏差e = cross(P - P_start, v)（二维向量叉积），正负可定义船在路径左侧为正或右侧为正，按自己习惯来。
LOS参考点则在投影点前方“延伸”一个前视距离Delta，即沿路径方向：
P_los = P_start + (s + Delta / |v|) * v
注意如果s + Delta/|v|大于1，说明船接近路径末端，LOS参考点可能越出当前段。此时要么切换到下一段路径，要么把LOS参考点直接钳制在路径终点，根据控制策略决定。
这段代码是整个制导律里最容易出边界问题的地方，尤其是多段路径切换时，投影点的s必须限制在[0,1]区间，否则在拐角附近会有跳变。后面我会在常见问题里细说这个坑。
3. 完整代码实现与参数标定
3.1 主程序结构与仿真流程
我用MATLAB写的这套代码，结构非常清晰，适合理解和修改。你也可以轻松改成Python版，逻辑完全一致。主程序包括几个部分：参数初始化、航路点定义、仿真主循环、数据可视化。
% 参数初始化 dt = 0.1; % 仿真步长（秒） T_total = 200; % 总仿真时长（秒） u = 2.0; % 船速（m/s） Delta = 10; % LOS前视距离（m） K = 0.5; % 舵效系数 T_phi = 2.0; % 舵响应时间常数（s） % 航路点定义 waypoints = [0, 0; 100, 0; 150, 50; 150, 100]; % 初始状态 x = 5; y = -20; psi = 0.3; r = 0; delta = 0; % 存储轨迹 x_log = []; y_log = []; psi_log = []; e_log = []; % 仿真主循环 for k = 1:T_total/dt [psi_d, e, los_pt] = los_guidance(x, y, waypoints, Delta); psi_error = wrap_to_pi(psi_d - psi); delta_cmd = kp * psi_error - kd * r; % 这里kp、kd需在参数区定义 % 舵机一阶惯性 delta = delta + dt * (delta_cmd - delta) / T_phi; % 更新船状态 r = r + dt * (K * delta - r) / T_phi; psi = psi + dt * r; x = x + dt * u * cos(psi); y = y + dt * u * sin(psi); % 记录数据 x_log(end+1) = x; y_log(end+1) = y; e_log(end+1) = e; end注意代码里的wrap_to_pi函数，用于把角度差限定在[-pi, pi]区间，这个非常重要。如果不做角度归一化，当期望航向在+179度而当前航向在-179度时，角度差会算成358度，控制器会错误地反向打大舵。这个问题在纯跟踪里特别常见，一定要处理。
3.2 LOS制导律实现：直线段路径
接下来是核心的LOS函数。我把它封装成一个独立函数，方便调用和复用。这个函数接受当前船位、航路点数组和前视距离，返回期望航向角、横向偏差和LOS参考点。
function [psi_d, e, los_pt] = los_guidance(x, y, waypoints, Delta) % 找到当前所在路径段（简化为逐段搜索） seg_idx = 1; % 省略找最近段的判断，可以用最近投影距离实现 x1 = waypoints(seg_idx, 1); y1 = waypoints(seg_idx, 2); x2 = waypoints(seg_idx+1, 1); y2 = waypoints(seg_idx+1, 2); vx = x2 - x1; vy = y2 - y1; % 路径方向角 alpha_p = atan2(vy, vx); % 投影比例s wx = x - x1; wy = y - y1; s = (wx * vx + wy * vy) / (vx^2 + vy^2); s = max(0, min(1, s)); % 限制在段内 % 横向偏差（定义左侧为正） e = wx * vy - wy * vx; % 叉积 % LOS参考点：在投影点前方Delta处 los_x = x1 + (s * vx) + (Delta * vx / sqrt(vx^2 + vy^2)); los_y = y1 + (s * vy) + (Delta * vy / sqrt(vx^2 + vy^2)); % 期望视线角 psi_d = atan2(los_y - y, los_x - x); los_pt = [los_x, los_y]; end这里有个细节值得注意：上面代码里的e我定义为叉积值，量纲是米，但正负直接对应船在路径的左侧或右侧。如果要更标准的横向偏差表达式，需要除以路径长度：
e_normalized = e / sqrt(vx^2 + vy^2)
在后面的控制律里，我建议用归一化后的横向偏差来算LOS角，而不是直接用叉积。不过在实际仿真中，只要路径长度单位是米，叉积值就是米乘以米，最终会对舵角产生一个无意识的放大作用，导致整定参数时产生困惑。
更稳妥的做法是单独维护一个横向偏差变量：
e_distance = (wx * vy - wy * vx) / sqrt(vx^2 + vy^2); psi_d = alpha_p - atan2(e_distance, Delta);这个版本是从上一小节公式直接翻译过来的，物理意义清晰，也方便后续打印调试信息。
3.3 控制律设计：从期望航向到舵角
LOS算出的期望航向psi_d，还需要一个航向控制器去执行。这里用最简单的PD控制：
delta_cmd = kp * psi_error - kd * r
其中kp是比例增益，kd是角速度阻尼系数，r是艏摇角速度（可从陀螺或差分求得）。这个控制器本质上就是一个PD航向控制器，目的是让实际艏向快速光滑地收敛到期望视线角。
kp和kd的值我常用的起点是kp=1.5、kd=0.8（对应于舵角饱和±30度、时间常数2秒的船），在此基础上根据仿真曲线微调。如果船响应慢、舵效低，适当增大kp；如果艏向振荡明显，增大kd。切忌一上来就大幅调kp，舵角饱和后会出现极限环振荡，轨迹会变成锯齿状。
3.4 参数整定参考表
参数整定是LOS算法落地最花时间的环节。下面是我在多次仿真和实艇试验中总结的经验参考值，适用于船长5米左右、船速1-3节的小型无人艇。
| 参数 | 推荐范围 | 我的常用值 | 备注 |
|---|---|---|---|
| 前视距离Delta | 1.5~3倍船长 | 10~15 m | 太大收敛慢，太小振荡 |
| 舵效系数K | 0.3~0.8 | 0.5 | 取决于舵面积和船型 |
| 舵响应时间常数T_phi | 1~3 s | 2.0 s | 大舵角响应慢，要实测 |
| 比例增益kp | 0.8~2.5 | 1.5 | 与舵角饱和匹配 |
| 微分增益kd | 0.3~1.2 | 0.8 | 抑制艏向振荡 |
| 仿真步长dt | 0.05~0.2 s | 0.1 s | 尽量与控制周期一致 |
这里重点说一下Delta。它取值非常讲究：Delta太小，LOS视线会变得很陡，船会在路径两侧来回穿越，轨迹形成“蛇形”；Delta太大，船会明显切内角，尤其在急转弯处轨迹偏差很大。我的经验是，先按船长2倍起步，观察轨迹收敛情况，再以1米为单位微调。
4. 调试实录与常见问题排查
4.1 横向偏差不收敛：投影计算越界
第一次跑仿真时最容易遇到的问题就是横向偏差不收敛——船在路径附近来回绕圈子，迟迟不回到路径上。检查下来，90%的情况是因为投影比例s没有限制在[0,1]区间。当船越过当前路径段的终点时，s大于1，按公式计算的LOS参考点跑到了下一段路径的延长线上，视线角指向一个错误的方向。这就是典型的“投影越界”问题。
解决办法就是前面代码里写的：
s = max(0, min(1, s));但当船在多段路径切换时，只做钳制还不够，更稳妥的方案是逐段判断“船是否进入下一段路径的接引范围”，当投影距离超过某个阈值时主动切换当前参考路径段。我在代码里实现的是先算所有路径段的投影距离，取最小值对应的段作为当前段，再在该段上计算LOS。
4.2 艏向角跳变：wrap_to_pi函数不可省
另一个高频坑是角度跳变。当船在正北方向附近航行时，艏向角可能在+179度到-179度之间跳变，如果不做角度归一化，PD控制器会产生一个巨大的误差信号，导致舵角瞬间饱和，船猛打一个满舵，轨迹瞬间失控。我见过很多初学者的仿真录像里，船快到终点时突然原地转一圈，就是这个原因。
所以，角度差计算必须写成：
psi_error = atan2(sin(psi_d - psi), cos(psi_d - psi));
这一步在任何角度控制场景里都是必须的，不只是LOS。建议把它封装成wrap_to_pi函数，所有航向误差统一走这个入口。
4.3 前视距离怎么选：实践中的调参经验
前视距离Delta的调参，我踩过很深的一次坑。之前做实艇试验，船长3米，我参照论文把Delta设成船长2倍，也就是6米，结果船在路径跟踪时横向偏差振荡非常明显，轨迹始终压不到预设直线上。后来把Delta调到12米后，曲线明显平滑了，但转弯半径又变得太大，路径终点转向前经常冲过头。
后来总结出一个经验法则：Delta的最小值必须大于船在一个控制周期内前进的距离，同时要考虑转角误差的控制带宽。数学上，LOS趋近路径的动态特性近似一个一阶环节，时间常数约为Delta/u，如果u=2 m/s，Delta=10 m，那么趋近时间常数是5秒，对无人艇来说这个动态是可以接受的；如果Delta只有5米，时间常数2.5秒，控制回路和舵响应时间常数接近，就容易耦合振荡。
所以我的调参建议是：先设Delta为船速的4-5倍（单位米），然后逐步减小直到轨迹开始出现轻微振荡，再往回退20%，这个值基本就是最优工作点。比如船速2 m/s，Delta从10米开始调，逐渐减到8米、7米，到6米时开始振荡，那就回到8米左右作为最终参数。
4.4 多段路径切换时的拐角策略
多段折线路径的拐角处理是个容易忽视的问题。船即将通过航路点时，如果LOS参考点还在前一段路径上，船会沿着前一段的延长线开出去；切换太早，船又会直接切内角。我的做法是设置一个切换半径：当船到当前段终点的距离小于某个阈值（比如1.5倍船长）时，直接切换参考路径段到下一段。
如果任务对路径精度要求很高，不想切内角，可以考虑在拐点处加一个“圆弧过渡段”，把折线路径做圆滑化处理。这一块属于路径规划的内容，不展开细讲，但实现LOS时你一定会遇到，知道有这几种思路就行。
5. 从仿真到实艇：还要补哪些功课
仿真跑通只是第一步，上艇前还有几个关键差异要补齐。
第一，定位数据要换坐标系。仿真里用的是平面直角坐标，实际GPS输出是经纬度，LOS公式中所有的距离和角度计算都要在平面坐标系下进行。工程上通常先把经纬度投影到UTM坐标或当地北东坐标系，再做路径跟踪。如果你直接用经纬度算atan2，纬度方向与经度方向的比例尺不一样，偏差会非常可笑。
第二，导航信号有噪声和延迟。GPS的横向偏差测量噪声一般在米级，航向测量如果用双天线GPS动基准站，噪声小一些；如果靠磁罗盘，还会有磁偏角和船体磁场干扰。实艇调试时，我会在仿真里加入高斯白噪声和0.5秒的信号延迟，看看控制器鲁棒性如何，再做实艇试验。
第三，舵角指令要经过执行机构限幅。船用舵机一般最大舵角±35度，且舵机响应速度有限。仿真里如果不加限幅，实艇上舵机会一直处于饱和状态，发热磨损很快。所以代码里要加饱和环节，并确保控制器在饱和时积分不无限累积（我这里用的是PD控制，没有积分项，会少一个坑；如果换PID，别忘记加抗积分饱和）。
6. 我自己的体会与扩展方向
这套代码我前前后后改过几十个版本，最深的一点体会是：LOS视线法看着简单，真正调好并不容易，因为问题往往不出在LOS公式本身，而在与它耦合的航向控制器、执行器响应、路径表示这些外围环节上。任何一处有bug，最后的表现都是船不走直线，但定位问题却要一层层排查。
如果想让这个项目再进一步，可以从几个方向扩展：一是在LOS基础上加自适应增益，根据横向偏差大小实时调整前视距离，提高收敛速度同时减少超调；二是引入S面控制器或模糊PID替换现在的线性PD，提升在风浪干扰下的表现；三是把路径从直线扩展到B样条曲线，通过参数化路径表达式直接求LOS点，船在连续转弯时的轨迹会更自然。
最后分享一个调试技巧：把LOS参考点也画到图上，用圆圈标记。当船运行异常时，先看LOS参考点是否在路径上、是否在船前方合理位置——如果LOS点都算错了，那后面控制器调得再好都白搭。这一步能帮你省下大量找bug的时间。
本文还有配套的精品资源，点击获取
