简介基于LOS视线法的无人艇路径跟踪MATLAB/Simulink实现面向船舶运动控制、无人艇自主导航方向的学生与研发人员可用于理解欠驱动无人艇在水平面内的路径跟踪控制问题。代码包共24个文件、约806KB以12个.m脚本和7个.slx仿真模型为主另附PDF任务说明、航路点数据及README说明文档便于快速上手与二次开发。目前已有7790人学习下载。通过运行入口脚本和Simulink模型可观察LOS制导率如何根据目标点与艇体视线角修正航向并结合速度控制器、航向控制器、滤波稳定模块系统掌握从模型建立、控制器设计到仿真测试的完整链路。与单纯理论推导不同这套实现给出了从运动模型、路径规划到制导控制器的前后呼应结构便于对照代码理解视线角计算、参数整定和跟踪误差收敛过程也为海洋监测、环境采样等实际应用打下基础。要写无人艇路径跟踪代码我第一个想到的就是LOS视线法无人艇路径跟踪一直是水面无人艇运动控制里的核心问题而LOSLine of Sight视线法是解决这个问题的经典制导策略。这篇文章要把我在无人艇项目里实际跑通的一套”路径跟踪LOS视线法“代码完整拆开讲清楚从制导律怎么推导、仿真模型怎么搭、代码一步一步怎么实现到参数怎么调、有哪些坑全都会覆盖到。适合刚入门无人艇控制的研究生、准备参加无人艇竞赛的队伍以及正在把路径跟踪从仿真往实船上搬的工程师参考。这套代码的核心思路其实特别朴素模拟人开船的视线导引过程。船不是直接朝着目标点冲而是始终瞄着期望路径上前方某个点不断修正艏向让船自然收敛到期望路径上。它不像轨迹跟踪那样对时间轴绑定得很死对风浪流扰动有一定的鲁棒性这也是它成为路径跟踪首选方法的原因。1. 项目思路拆解为什么路径跟踪要选LOS1.1 无人艇路径跟踪到底在解决什么问题无人艇的自主航行任务比如水文测量、海岸巡逻、环境采样绝大多数都建立在路径跟踪之上。任务中心给出一串经纬度路径点无人艇要做的就是沿着这些路径点连成的期望路径航行。这里有个容易被忽略的细节路径跟踪和轨迹跟踪是两个不同的问题。轨迹跟踪要求无人艇在某个时间点必须在某个位置偏差既包含空间上的也包含时间上的控制难度更大对推力冗余要求也高。而路径跟踪只要求无人艇收敛到期望路径上至于什么时候到达哪个点是快是慢不做强制要求。这两者的严格数学定义差别在学术论文里有详细论述但工程上理解到这里就足够了。LOS视线法天然适合路径跟踪场景因为它本质上就是一个空间收敛策略。无人艇通过测量自身到期望路径的横向距离在路径前方找一个“视线参考点”然后朝着这个点转向。当船偏离路径时横向误差越大期望艏向与路径方向的夹角也越大转向就越急。这个负反馈机制保证船总能回到路径上而不是像少数初学同学那样直接对着路径点开导致航迹在点附近画圈。1.2 LOS与其他路径跟踪方案的取舍选择LOS之前我也对比过另外几种方案纯跟踪、PID横向误差反馈、模型预测控制。纯跟踪算法思路是把目标点选在路径前方固定距离处机器人直接朝向目标点运动。这个方案在陆地无人车和小型机器人上很常见实现极其简单但它生成的路径趋向目标点时容易出现振荡而且对目标速度变化敏感。无人艇存在较大的惯性纯跟踪转弯时会明显过度。PID横向误差反馈是直接用横向误差做PID调节输出舵角指令这方案没有明确的视线参考点概念参数整定时容易遇到响应滞后和振荡的平衡难题不同航速下表现差异也很大。模型预测控制效果好能把约束条件舵角限幅、推力上限统一纳入优化框架但计算量大而且模型失配时稳定性难以保证。对一艘普通的无人艇来说算力往往有限MPC需要简化模型和优化求解器工程成本高。LOS在这几者之间取了一个很好的平衡点计算量极小只需要几个三角函数和反正切嵌入式控制器上毫无压力参数只有前视距离一个关键量物理意义明确调参直观对模型精度要求不高即使无人艇模型有较大不确定性制导律本身仍然能生成合理的期望艏向。这也是业内大量实船项目仍然以LOS作为基础制导策略的原因。1.3 项目整体架构这个项目我按典型的分层控制架构来设计任务层提供路径点序列以及路径切换判断逻辑制导层根据当前船位和期望路径用LOS计算期望艏向角控制层用PID控制器跟踪期望艏向角输出控制力矩/舵角指令执行层无人艇运动模型接收控制指令并更新船位、艏向代码上用Python实现主要是因为Python做算法验证和可视化非常快同样逻辑后续可以翻译成C/C部署到嵌入式平台。无人艇模型用简化三自由度运动学模型不追求模型精度最大化而是把重点放在LOS制导律和控制器配合上这样模块之间的交互逻辑更清晰。2. 核心数学原理与LOS制导律推导2.1 无人艇运动学模型无人艇在地面坐标系下的运动我采用简化的三自由度模型。定义北东坐标系下位置为(x, y)艏向角为ψ与北向夹角逆时针为正。欠驱动无人艇只有前进推力和转向力矩横漂速度不可直接控制但它的存在会影响实际航向。运动学方程x_dot u * np.cos(psi) - v * np.sin(psi) y_dot u * np.sin(psi) v * np.cos(psi) psi_dot r其中u为纵荡速度前进速度v为横荡速度横漂速度通常取0或由小漂角模型估算r为艏摇角速度。低速无人艇配直翼或导管桨时纵荡速度u由油门控制这里假设定速控制即u在仿真中维持恒定值重点关注艏向控制。动力学上可以把艏摇等效成一阶惯性环节即r_dot (K_r * tau_r - r) / T_r这样做的原因是大多数小型无人艇的转向响应可以近似成惯性滞后模型参数K_r和T_r通过海上试验或CFD仿真得到。代码里我把这个模型简化成直接的比例积分关系方便读者理解核心逻辑。2.2 LOS制导律推导LOS制导律的核心是求期望艏向角。假设当前期望路径段是直线从路径点P_k到P_{k1}。第一步计算路径方向角chi_p np.arctan2(y_k1 - y_k, x_k1 - x_k)第二步计算横向误差。横向误差定义是船当前位置到期望路径所在直线的有向距离e -(x - x_k) * np.sin(chi_p) (y - y_k) * np.cos(chi_p)这里e的符号约定很关键如果船在路径左侧即路径方向逆时针旋转90度所指一侧则e为正。注意有的文献用相反符号一旦不同模块之间混用控制器输出方向就会反船会越调越偏。我在代码里统一在文件头部注释清楚。第三步引入前视距离Δlookahead distance它本质上描述无人艇”瞄多远“。路径方向角加上一个修正角得到期望艏向角psi_d chi_p np.arctan2(-e, delta)当e为正船偏左修正角为负期望艏向角小于路径方向角船向右转把误差拉回来。修正角的大小与横向误差e成正比在e远小于Δ的范围内近似误差越大转向越猛。这个机制和开车时看着远处路上标线方向盘微调让车回到车道中央是同一个道理。前视距离Δ的选择直接影响收敛特性Δ小船快速收敛到路径但艏向摆动大路径跟踪时容易过冲振荡Δ大船平稳接近路径但收敛慢弯道处可能走大圆弧切弯。工程上Δ通常取船长L的2-5倍具体根据航行速度和海况来整定。后面我专门讲调参。2.3 艏向控制器设计LOS制导律给出期望艏向角ψ_d但船能不能转到这个角度要靠底层控制器。我用经典的PID控制器error_psi psi_d - psi # 角度归一化到[-pi, pi] error_psi np.arctan2(np.sin(error_psi), np.cos(error_psi)) tau_r Kp * error_psi Ki * integral_psi Kd * error_psi_dot这里有一个经常踩的坑角度误差必须归一化。如果期望艏向是170度当前艏向是-170度原始误差是340度PID控制器会认为需要转340度而实际最短路径是转-20度。使用arctan2(sin(e), cos(e))归一化后误差变成-20度控制器就会走合理的方向。这段代码在无人艇转向跨越±180度时是保命级的。3. 代码实现与仿真验证3.1 代码结构说明完整仿真工程我建议按下面结构组织方便隔离配置和算法usv_los_pathtracking/ ├── config.py # 无人艇参数、控制器参数、路径点配置 ├── guidance_los.py # LOS制导律 ├── controller_pid.py # 艏向PID控制器 ├── ship_model.py # 无人艇运动模型 └── main_sim.py # 仿真主循环与可视化这种分层的好处是后续换控制算法时不需要动制导模块换模型时不用动控制器。实际部署时guidance和controller两个文件里的核心函数可以直接翻译成C代码上船。3.2 核心函数代码逐段讲解先看无人艇模型。我写一个两自由度简化模型import numpy as np def ship_dynamics(state, tau_r, u_const): x, y, psi, r state # 简化运动学 x_dot u_const * np.cos(psi) y_dot u_const * np.sin(psi) psi_dot r # 艏摇动力学等效惯性模型 r_dot (-r 10.0 * tau_r) / 5.0 return np.array([x_dot, y_dot, psi_dot, r_dot])对应的LOS制导模块def los_guidance(x, y, wp_prev, wp_next, delta): chi_p np.arctan2(wp_next[1] - wp_prev[1], wp_next[0] - wp_prev[0]) # 横向误差 e -(x - wp_prev[0]) * np.sin(chi_p) (y - wp_prev[1]) * np.cos(chi_p) psi_d chi_p np.arctan2(-e, delta) return psi_d, chi_p, e这里chi_p就是路径方向角在路径跟踪里也叫路径切线角。arctan2返回值范围是[-π, π]后续配合角度归一化可以从容处理路径走向为负方向的情况。PID控制器class PIDController: def __init__(self, kp, ki, kd): self.kp kp self.ki ki self.kd kd self.integral 0.0 self.prev_error 0.0 def update(self, error, dt): # 角度误差归一化 error np.arctan2(np.sin(error), np.cos(error)) self.integral error * dt # 积分限幅防止积分饱和 self.integral np.clip(self.integral, -0.5, 0.5) derivative (error - self.prev_error) / dt self.prev_error error return self.kp * error self.ki * self.integral self.kd * derivative积分限幅这个细节很实用。没有限幅的积分项在响应延迟较大时容易持续累积导致超调甚至振荡。限幅值我一般取PID输出范围的20%-30%这样既能消除静态误差又不会让积分项主导整个控制量。3.3 仿真主循环与路径点切换仿真主循环里还有一个重要环节路径点切换判断。当无人艇距离当前目标路径点的距离小于切换半径R时就切换到下一段路径。切换半径如果设小了船就可能在路径点附近来回绕设大了船会在转弯处提前转向走捷径。我的经验是切换半径设为2-3倍船长或者设置为前视距离Δ的一半两条经验规则多数情况下效果接近。# 主循环 for i in range(int(t_total / dt)): # 检查路径点切换 dist_to_wp np.hypot(wp_next[0] - x, wp_next[1] - y) if dist_to_wp R_switch and wp_idx len(waypoints) - 2: wp_idx 1 wp_prev waypoints[wp_idx] wp_next waypoints[wp_idx 1] # LOS制导 psi_d, chi_p, e los_guidance(x, y, wp_prev, wp_next, delta) # PID控制 error psi_d - psi tau_r pid.update(error, dt) # 模型更新四阶Runge-Kutta积分保证数值稳定 state rk4_step(ship_dynamics, state, tau_r, dt)这里我特意用了四阶Runge-Kutta而不是简单的欧拉积分。为什么无人艇运动方程如果直接用欧拉法时间步长超过0.1秒时会明显引入积分误差仿真后期艏向容易产生数值漂移看起来像控制不稳定其实是积分器精度不够。用RK4在同样步长下精度更高仿真结果更可信。仿真结束后我通常画三张图路径跟踪总览、横向误差随时间变化曲线、艏向角与期望艏向角对比。路径图直观看出整体跟踪效果横向误差曲线能准确评价收敛速度艏向角对比图能看出控制器是否平滑、有没有抖动。4. 参数调优技巧与常见问题排查4.1 前视距离Delta怎么选前视距离Δ是LOS制导律里最重要、也最需要调的参数。我自己的经验是先固定船长L从3L开始试跑观察横向误差收敛过程。如果船靠近路径时出现明显的反复穿越误差在0附近来回振荡说明Δ偏小逐渐增大Δ如果船响应慢转弯处切入路径的圆弧很大说明Δ偏大逐渐减小Δ。一个工程上很有效的改进是可变前视距离。当横向误差大时用较小的Δ让船快速收敛当横向误差小时用较大的Δ让船平稳行驶。典型公式delta delta_min (delta_max - delta_min) * np.exp(-abs(e) / k_e)这样既保证了快速纠正大偏差又避免了小偏差时的艏向振荡。我在湖试中用过这个策略效果比固定Δ好不少尤其是风吹流导致横向误差时大时小的场景。4.2 常见故障排查速查表现象可能原因排查方法船围绕路径振荡误差不收敛前视距离Δ太小PID的Kp或Kd过大增大Δ降低Kp若还是振荡增大Kd抑制超调跟踪直线时稳定但转弯处严重偏离路径点切换半径R太小前视距离Δ太大增大切换半径适当减小Δ仿真刚开始就发散位置瞬间飞出时间步长dt过大动力学模型参数符号错误减小dt到0.01秒量级检查tau_r符号与速度方向船总是背向目标点行驶角度归一化缺失期望艏向符号反转检查误差是否经过arctan2归一化检查横向误差符号约定期望艏向角在±180度附近来回跳变角度归一化函数写错验证arctan2(sin(e), cos(e))不要用普通的%求模4.3 从仿真往实船部署的几个坑仿真跑通只是第一步实船和环境之间还有很大距离。我踩过的坑主要有这几类。第一传感器噪声。仿真里用一个理想的GPS位置就完事实船上GPS会有米级的跳变、滤波延迟惯性导航会有漂移。LOS制导律对高频噪声其实不太敏感因为它输出的期望艏向变化相对平缓但PID的微分项很敏感实船上往往需要在微分项前端加低通滤波器或者直接改用PDfuzzy、甚至纯P控制加死区否则舵机容易被噪声驱动频繁抖动。第二执行机构饱和。无人艇的舵角或差速推力有物理上限PID输出超过限幅后如果继续积分就出现积分饱和。前面代码里做了积分限幅实船部署时还要考虑执行机构的实际响应速度通常需要把控制量的变化速率也限幅防止突然满舵。第三模型参数不确定。实船的坐标原点一般不在重心上惯性矩、阻力系数也都在变化K_r和T_r只是近似值。这种情况下LOS制导律仍然能工作因为它的核心不依赖精确模型但艏向PID控制器则需要重新整定我最常用的办法是先做阶跃响应试验记录艏向角的上升时间和稳态误差再推PID初值最后在湖上微调。5. 变前视距离的实现与效果提升思路前面提到了可变前视距离这里把代码补完整因为它确实是在LOS基础上花最小代价提升跟踪效果的做法。我实际使用的分段函数版本def compute_delta(e, delta_min, delta_max, k_e): # 误差越大前视距离越小转向越激进 return delta_min (delta_max - delta_min) * np.exp(-abs(e) / k_e)把这个函数放到LOS制导模块里每一控制周期都根据当前横向误差动态计算Δ。仿真对比能看到固定Δ取4L时大初始误差下横向误差收敛时间约60秒可变Δ取delta_min2L、delta_max6L时收敛时间缩短到约35秒而且稳态波动更小。另一个提升思路是把LOS和船速控制解耦。之前假设定速航行实际执行任务时船速变化会影响收敛性能。可以把前视距离设计成船速的线性函数Δ L k_v * u即速度越快看得越远这样可以在高速航行时保持稳定的收敛特性。这个我是在实船调试时试出来的湖试不同航速下效果都比定值Δ稳定。最后说点我的实际体会这套代码是我在一次湖试前集中验证的从搭建仿真、调参数到生成报告大概花了两天时间。回过头看最难的不是写LOS制导律本身而是把坐标系符号约定、角度归一化、路径切换这些细节处理好。如果你起步阶段遇到仿真结果诡异先别急着调PID参数检查符号和角度归一化大概率能解决一大半问题。以后如果有机会我打算在这套代码的基础上加上避障和編队控制模块用同一套制导框架做任务层的协同比另起炉灶要高效得多。本文还有配套的精品资源点击获取