UUV三维路径跟踪的LOS制导与PID控制MATLAB仿真实现

拿到基于LOS制导PID控制的无人潜艇UUV三维路径跟踪这个需求时大多数人第一反应可能跟我一样三维路径听起来就得直接上模型预测控制或滑模控制才有底气。但把问题拆开之后你会发现用LOS视线法制导律加PID控制在MATLAB里做一套可复现的仿真代码反而是这个领域性价比最高、最容易调试、也最能把控制逻辑讲清楚的组合。这篇文章就把从需求到代码落地的完整路线整理出来内容包括UUV数学模型怎么简化、LOS制导的期望航向角和纵倾角怎么算、PID三个回路的角色与整定方法、MATLAB代码骨架怎么搭以及仿真调试里真正会咬人的几个细节。适合正在做无人水下航行器路径跟踪、水下机器人控制仿真、无人船相关课题的读者参考。1. 为什么三维路径跟踪首选LOS制导加PID这套组合1.1 LOS制导解决的是几何层的问题路径跟踪本质上是让UUV沿着一条给定的空间曲线移动而不是单纯跑到某个终点。LOS制导的思路很直白源自航海中的看着目标开船载体当前位置和前视点之间拉一条视线这条视线方向就是运动方向。制导层的任务只有一个就是根据当前位置和期望路径算出这条视线对应的基准角度它完全不关心UUV能不能瞬间转过去。这种几何制导底层控制的分层思想是整个方案的骨架。几何层只看位置关系动态层处理执行能力两者解耦之后代码结构非常清晰哪里出问题一眼就能定位。如果不用制导层直接让PID去跟踪路径点的绝对坐标那么路径点切换瞬间会跳出很大的误差阶跃控制器根本来不及平滑响应航迹容易冲出路径很远。LOS制导把绝对坐标跟踪转换成了角度基准跟踪本质上做了误差域的一层变换这是它比直接坐标跟踪稳的关键。1.2 PID解决的是动态层的问题制导层算出期望航向角和期望纵倾角之后接下来的问题是UUV这个刚体不可能瞬间转到那个朝向。因为水动力、惯性、舵效都会造成滞后没有底层控制的话UUV就会沿着当前方向一直飘出去。PID在这里扮演的就是把视线方向翻译成可执行指令的角色。航向误差进航向PID输出舵角纵倾误差进纵倾PID输出升降舵速度误差进速度PID输出推力。三个通道在弱扰动、定速巡航的场景下基本可以解耦处理PID的三个参数分别对应转得快不快有没有余差冲不冲工程语义非常明确调试时对着曲线就知道该动哪个参数不需要黑箱优化。1.3 这套组合的适用边界必须承认LOS加PID不是万能药。强时变海流、大范围模型不确定性、高机动避障场景下LOS会因为固定视线方向而出现稳态偏差PID的固定增益也难以兼顾全工况。但这种场景在课程设计、课题验证和工程第一版仿真里并不常见多数情况下UUV巡航速度近似恒定环境扰动较弱路径也以直线段为主经典LOS加PID已经完全够用。而且这套组合还有个隐藏优势它可以作为更高级算法的基线。常规LOS升级成积分视线法ILOS就可以抗海流PID升级成抗饱和PID或模糊自整定PID就能提升鲁棒性路径上加光滑样条就能缩短转弯超调。先跑通经典版本后面所有改进都有对比基准这是直接用重型算法开局拿不到的好处。2. 模型与误差把UUV运动学和三维路径跟踪问题形式化2.1 本文使用的简化运动学模型及其离散化做三维路径跟踪仿真不需要一上来就用六自由度完整水动力模型那会淹没控制设计的重点。为了把LOS制导和PID的控制逻辑讲清楚这里采用简化运动学模型加一阶舵响应的组合x方向位置变化率由前向速度在水平面上的分量决定y方向同理z方向由纵倾角决定。舵效和推进器响应用一阶惯性环节近似即实际的艏摇角速度和纵倾角速度都滞后于指令。完整的连续时间模型可以写作dx/dt u * cos(psi) * cos(theta)dy/dt u * sin(psi) * cos(theta)dz/dt -u * sin(theta)dpsi/dt (1 / T_psi) * (psi_cmd - psi)dtheta/dt (1 / T_theta) * (theta_cmd - theta)du/dt (1 / T_u) * (u_cmd - u)这里的T_psi、T_theta、T_u分别表示航向、纵倾、速度通道的惯性时间常数取值依据对象特性而定。仿真中流动模型的时候我用固定步长dt离散化状态更新写成% 状态向量 state [x; y; z; psi; theta; u] % 控制向量 cmd [psi_cmd; theta_cmd; u_cmd] function state_new uuv_model(state, cmd, dt) x state(1); y state(2); z state(3); psi state(4); theta state(5); u state(6); dx u * cos(psi) * cos(theta); dy u * sin(psi) * cos(theta); dz -u * sin(theta); dpsi (cmd(1) - psi) / 1.5; % 航向惯性时间常数 dtheta (cmd(2) - theta) / 2.0; % 纵倾惯性时间常数 du (cmd(3) - u) / 3.0; % 速度惯性时间常数 state_new state dt * [dx; dy; dz; dpsi; dtheta; du]; end这个模型虽然简化但保留了路径跟踪系统最重要的动态特性舵指令到姿态变化存在滞后速度不是瞬间到位。所以后续调PID时观察到的超调和振荡现象都是真实系统中会遇到的仿真结果有参考意义。2.2 路径、航路点与前视点的表示方法三维路径以航路点序列表示每两行是一个直线段waypoints [ 0, 0, -20; 200, 50, -35; 350, 180, -50; 500, 220, -40; ];第一列为x坐标第二列为y坐标第三列为z坐标。UUV需要依次经过这些航路点。为了让轨迹贴合路径不能直愣愣把当前航路点作为跟踪目标否则距离越近制导方向越飘会产生绕圈效应。正确做法是引入切换半径R_switch当UUV当前位置与当前目标航路点的距离小于R_switch时目标点切换到下一个航路点。切换半径一般取2到3倍艇长太小则航路点切换频繁且方向突变剧烈太大则会在转角处提前切弯导致轨迹偏离路径。前视点的概念更有讲究。经典LOS制导中前视点定位在路径当前线段上与UUV当前位置的直线距离等于前视距离L。它的计算方法是把当前位置投影到当前线段上再沿路径方向向前推进一个距离L。以线段起点P_i、终点P_{i1}为例单位方向向量e (P_{i1} - P_i) / norm(P_{i1} - P_i)当前位置P到起点P_i的向量r P - P_i投影位置P_proj P_i dot(r, e) * e前视点P_los P_proj L * e。这样得到的前视点始终在路径前方UUV的视线方向会平滑地拉向路径偏差越大视线越斜修正作用越强。2.3 三维跟踪误差的定义与符号约定三维路径跟踪误差不是简单的欧氏距离。如果只看三维直线距离控制器完全不知道偏差是在水平面还是垂直面修正动作就是混乱的。正确做法是把误差沿路径的切线方向和法平面方向分解。简化实现里我建议只算两个核心误差水平横向偏差和垂直偏差。水平横向偏差定义为当前位置在以路径方向为x轴的平面坐标系里的y分量垂直偏差就是z方向差值。代码可以这么写% 当前线段起点 pp, 终点 pn, 当前点 pc e (pn - pp) / norm(pn - pp); r pc - pp; proj pp dot(r, e) * e; err_xy proj(1:2) - pc(1:2); % 水平偏差向量 err_h norm(err_xy); % 水平横向偏差大小 err_v proj(3) - pc(3); % 垂直偏差误差符号需要配合制导方向约定。水平面上如果视线方向在路径左侧横向偏差为正还是为负直接影响PID输出舵角的方向建议在代码里固定符号约定并注释清楚避免后面调方向时反复试错。3. LOS制导核心水平面航向角、垂直面纵倾角与前视距离设计3.1 水平面LOS制导律与角度跳变处理水平面LOS制导律的表达式非常简洁期望航向角就是前视点到当前位置连线的方位角psi_d atan2(P_los.y - y, P_los.x - x)这里一定要用atan2而不是atan因为atan2返回四象限的[-pi, pi]范围能正确区分东北、东南、西南、西北四个方向的夹角。用atan(x/y)的话在x为负时会彻底算错方向UUV会朝相反方向跑。更大的坑是角度跳变。当psi_d从正方向越过±pi边界时数值上会从接近pi突然跳到接近-pi而载体航向还在pi附近两者之差看起来就会变成接近2pi的假误差导致PID输出一个巨大的反向舵角航迹瞬间失控。解决办法是角度误差归一化把所有角度差先包到[-pi, pi]区间再进入PID。我通常写一个wrap函数function e wrap_to_pi(angle) e mod(angle pi, 2*pi) - pi; end航向误差的计算因此写成e_psi wrap_to_pi(psi_d - psi);别小看这一行它是航向类仿真里最常见的隐性bug来源。路径从第三象限跨越到第二象限时不写wrap函数分数几乎一定出问题。3.2 垂直面制导深度误差向纵倾指令的转换垂直面的引导不能照搬水平面直接给航向角的方式。因为UUV的z坐标是深度垂直面路径跟踪的目标是一个深度值如果直接把深度误差丢给PID去输出升降舵虽然也能工作但物理意义不清晰而且深度变化速率受纵倾角限制容易出现深度方向的超调和拉锯。更合理的做法是设计一个纵倾角指令。假设当前水位z目标深度z_target水平面内到前视点的距离为rho sqrt((P_los.x - x)^2 (P_los.y - y)^2)那么期望纵倾角可以写成theta_d atan2(z - z_target, rho)注意atan2的第一个参数是z差负号决定了抬头为正还是俯冲为正。实际因为深度向下为正目标深度比当前深度更深时z_target zz - z_target为负此时纵倾角为负表示需要下潜符号逻辑需要和运动学模型里的定义对齐。直接用这个公式在距离很近时会出现纵倾角指令趋向±90度的情况这既不真实也容易引起数值发散所以要加限幅theta_cmd_raw atan2(z - z_target, rho); theta_cmd max(-0.5, min(0.5, theta_cmd_raw)); % 限幅约±28度工程上还有一种双回路方案外层深度PID输出期望纵倾角内层纵倾PID输出升降舵角。好处是深度误差不会直接驱动舵面而是先经过纵倾通道平滑抗浪涌能力更好。第一版仿真建议直接用公式给纵倾指令加限幅代码量少且效果直观等跑通之后再换双回路。3.3 前视距离的自适应调整固定前视距离L的经典LOS有一个内在矛盾。L小视线方向拉得近收敛速度快但容易在路径两侧振荡L大视线方向看得远轨迹平滑但偏差消除慢拐弯容易飘。很多论文采用固定L比如取2倍艇长效果尚可但不是最优。我建议在第一版代码里就写成自适应前视距离公式用横向偏差的递减函数L L_min (L_max - L_min) / (1 k * err_h^2)偏差大时L变小视线方向大幅偏向路径快速收敛偏差小时L变大视线方向基本平行路径避免振荡。这里L_min要大于UUV的最小转弯能力否则制导层给出的转角要求执行机构根本做不到控制器会一直饱和。我常用的初始值对于模型全长2米、巡航速度1.5米/秒的UUVL_min取6米L_max取20米k取0.01左右。前视距离的调整直接改变闭环系统的主导极点在路径切线方向和法线方向的投影是制导层最重要的可调参数。我调试时的习惯是先固定PID参数只调L的自适应参数直到横向偏差曲线没有明显振荡再去动控制器的增益这样可以避免两个环节的参数混在一起找不出问题。4. PID三个回路的角色与整定从参数到抗饱和处理4.1 航向、纵倾、速度三个控制通道的设计分工三个PID通道在系统里各司其职。航向PID输入是期望航向与实际航向的角度差输出是垂直舵角直接控制UUV在水平面内的转动纵倾PID输入是期望纵倾与实际纵倾的差值输出是升降舵角控制UUV在垂直面内的姿态速度PID输入是期望速度与实际速度的差值输出是推力指令控制巡航速度。三个通道之间存在耦合航向改变会引起速度和深度轻微波动纵倾改变也会影响水平面投影速度。工程上的处理原则是先按解耦设计再通过仿真观察耦合造成的误差如果路径跟踪误差在可接受范围内就不做补偿。对于巡航速度恒定的UUV这个假设在多数工况下成立。4.2 参数整定的顺序与初始值范围参数整定顺序建议严格按照速度、航向、纵倾来。先调速度回路因为速度和航向、纵倾都存在运动学耦合速度不稳会直接污染另外两个通道的误差曲线。再调航向回路水平面响应通常比垂直面灵敏参数容易观察。最后调纵倾垂直面动态慢超调改进余地小要耐心。手动整定坐标时可以按照经典PID调整口诀先把积分和微分项设为0只留比例项从小到大增加K_p直到响应出现持续等幅振荡再取当前K_p的一半作为基准值然后加入微分项设置为K_p的1/5到1/3用来抑制超调最后加入积分项从很小的Ki开始用于消除稳态偏差。整个调整过程每改一次参数就要跑一次仿真并观察误差曲线不要指望一次到位。下表是我在这类仿真中常用的初始参数范围不同模型尺度可在此基准上上下浮动控制通道KpKiKd输出限幅航向环1.0 ~ 3.50.05 ~ 0.20.5 ~ 2.0±35度纵倾环0.5 ~ 1.50.02 ~ 0.10.3 ~ 1.0±20度速度环50 ~ 3002 ~ 105 ~ 20推力上限需要特别提醒的是PID参数不是越大越好。Kp过大会导致航向角快速瞄准但带出持续振荡Ki过大会让舵角饱和并产生积分超调Kd过大则会让控制量受噪声严重污染。4.3 PID代码里的积分限幅与抗饱和处理许多人在写PID时只做比例微分加积分不处理积分饱和实际跑路径跟踪马上就会遇到问题。UUV在路径初始偏差较大时航向误差长时间保持一个大值积分项不断累积等UUV接近路径时积分器已经积累了很大输出导致它冲过路径再反冲回来来回振荡好几轮才稳定。标准的抗饱和处理是积分分离加积分限幅。误差超过阈值时不累积积分误差回到阈值内恢复累积同时积分项输出本身被限幅。我常用这样的通用PID实现function u pid_step(e, e_dot, integ, dt, param) % param [Kp, Ki, Kd, out_min, out_max, int_max] Kp param(1); Ki param(2); Kd param(3); out_min param(4); out_max param(5); int_max param(6); if abs(e) 0.5 % 积分分离阈 integ integ e * dt; end integ max(-int_max, min(int_max, integ)); u Kp * e Ki * integ Kd * e_dot; u max(out_min, min(out_max, u)); end积分分离阈值跟具体控制通道相关航向环取0.1到0.3弧度纵倾环取0.05到0.15弧度。输出限幅则是对执行机构物理能力的模拟舵角不可能无限大推进器推力也不可能无限大提前在控制器里限幅比让模型数值发散好得多。微分项处理上我建议使用测量值微分而不是误差微分也就是说微分作用在输出信号上而不仅限于误差的变化率这样可以避免期望角度跳变时微分项产生巨大尖峰。实现时用角速度反馈替代误差微分效果会平滑很多。5. MATLAB代码骨架制导、控制、模型三块怎么组织5.1 主仿真循环的搭建MATLAB代码实现的关键是模块化。我习惯把所有参数放进一个结构体PARAM里主循环只负责调用三块函数制导、控制、模型。这样后换算法时只需要替换对应函数主循环基本不用动。主循环骨架如下% 参数初始化 PARAM.waypoints [0,0,-20; 200,50,-35; 350,180,-50; 500,220,-40]; PARAM.L_min 6; PARAM.L_max 20; PARAM.k_los 0.01; PARAM.R_switch 10; PARAM.pid_psi [1.8, 0.1, 0.8, -35, 35, 5]; PARAM.pid_theta [0.8, 0.03, 0.4, -20, 20, 3]; PARAM.pid_u [150, 5, 10, -50, 100, 30]; PARAM.dt 0.02; PARAM.T_end 400; % 初始状态 state [x y z psi theta u] state [0, 0, -20, 0, 0, 1.5]; wp_idx 1; integ_psi 0; integ_theta 0; integ_u 0; % 存储数组 n_steps round(PARAM.T_end / PARAM.dt); traj zeros(n_steps, 6); err_hist zeros(n_steps, 2); for k 1:n_steps x state(1); y state(2); z state(3); psi state(4); theta state(5); u state(6); % 1. 制导层 [psi_d, theta_d, wp_idx] los_guidance(...); % 2. 控制层 e_psi wrap_to_pi(psi_d - psi); e_theta theta_d - theta; e_u PARAM.ud - u; delta_r pid_step(e_psi, 0, integ_psi, PARAM.dt, PARAM.pid_psi); delta_e pid_step(e_theta, 0, integ_theta, PARAM.dt, PARAM.pid_theta); tau_u pid_step(e_u, 0, integ_u, PARAM.dt, PARAM.pid_u); % 3. 模型更新 state uuv_model(state, [delta_r; delta_e; tau_u], PARAM.dt); % 4. 记录 traj(k, :) state; err_hist(k, 1) err_h; err_hist(k, 2) err_v; end主循环里的顺序不要变制导永远先于控制因为控制层需要制导层的最新输出作为期望值。存储数组在初始化时预先分配好内存不要每步动态拼接数组不然仿真时间长时会特别慢。5.2 三个核心子函数制导函数封装了航路点切换、前视点计算、期望角度计算的全部逻辑。接口设计上输入当前状态和控制器参数输出期望角度和更新后的航路点索引。等速度之类与时间强相关的状态量也一并输出方便控制层直接使用。function [psi_d, theta_d, wp_idx] los_guidance(state, waypoints, wp_idx, PARAM) x state(1); y state(2); z state(3); % 航路点切换 target waypoints(wp_idx, :); if norm([target(1) - x, target(2) - y, target(3) - z]) PARAM.R_switch wp_idx size(waypoints, 1) wp_idx wp_idx 1; target waypoints(wp_idx, :); end % 当前线段上的前视点 p_start waypoints(max(wp_idx - 1, 1), :); p_end waypoints(wp_idx, :); e (p_end - p_start) / norm(p_end - p_start); r [x, y, z] - p_start; proj p_start dot(r, e) * e; % 前视距离自适应 err_h norm(proj(1:2) - [x, y]); L PARAM.L_min (PARAM.L_max - PARAM.L_min) / (1 PARAM.k_los * err_h^2); los_pt proj L * e; % 期望角度 psi_d atan2(los_pt(2) - y, los_pt(1) - x); rho sqrt((los_pt(1) - x)^2 (los_pt(2) - y)^2); theta_cmd atan2(z - los_pt(3), rho); theta_d max(-0.5, min(0.5, theta_cmd)); end控制函数就是前面给出的pid_step模型函数就是uuv_model。注意力求每个函数职责单一制导函数里不出现PID参数控制函数里不出现航路点信息模型函数里不出现任何控制逻辑。这样后面做参数扫描时只需要在脚本里循环改PARAM字段不用改动函数内部代码。5.3 继续演进Simulink验证方案单文件脚本适合学习和调试缺点是跑参数扫描和矩阵测试比较费劲。后续如果要批量验证多组参数、或者在路径上加入扰动模型可以考虑切换到Simulink方案。Simulink的搭建思路和脚本完全对应制导函数放在MATLAB Function块里PID使用Simulink自带的PID Controller模块并导成离散形式UUV模型用State-Space或S-Function实现数据输出通过To Workspace集。我个人的体会是不建议第一版就直接用Simulink因为模块连线本身会引入很多连接错误排错时间比脚本高得多。脚本跑通且参数基本确定之后再平移成Simulink模型做可视化展示和后续扩展是更稳妥的推进路径。另外Simulink的变步长求解器配合PID模块时要注意采样时间的显式设置否则控制器的离散采样与模型积分步长不一致结果会非常诡异。6. 仿真结果解读三维轨迹、误差曲线与参数对比6.1 三维轨迹与航路点绘制plot3的基本用法仿真跑完之后第一步就是画三维轨迹。用plot3绘制UUV实际轨迹用scatter3绘制航路点配合grid on和axis equal能明显看出跟踪质量。figure; plot3(traj(:,1), traj(:,2), traj(:,3), b-, LineWidth, 1.5); hold on; scatter3(waypoints(:,1), waypoints(:,2), waypoints(:,3), 60, r, filled); plot3(waypoints(:,1), waypoints(:,2), waypoints(:,3), r--, LineWidth, 1); xlabel(X (m)); ylabel(Y (m)); zlabel(Z (m)); grid on; view(45, 30); axis equal;判断轨迹质量的标准有两条。第一直线段部分轨迹要贴合期望路径不能来回摆动轨迹偏差慢慢收敛到小范围第二航路点转弯附近允许出现少量超调这与制导切换和前视距离有关但超调不应扩散到后续直线段。如果轨迹看起来绕来绕去大概率是前视距离太小或航向Kp偏大。6.2 误差曲线应该长什么样掉飘了是哪里出了问题误差曲线比轨迹图更容易定位问题。将水平横向偏差err_h和垂直偏差err_v分别画在两张子图里横轴为时间。正常的误差曲线应该是这样的形态初始阶段有一个较大的偏差峰值然后单调或略带阻尼地收敛到零附近的稳定范围稳态误差的控制目标建议在1米以内。如果水平横向偏差呈现持续振荡说明航向环P过大或者前视距离过短振荡幅度逐渐增大则是D项不足或者存在时滞稳态时偏离零点且不消除是积分项不足或前视距离偏大导致的稳态残差。垂直偏差的响应通常比水平慢这是纵倾通道惯性大的正常表现。如果垂直误差长期不收敛优先检查制导律里theta_d的符号定义和限幅值很多时候不是PID的问题而是纵倾角指令的正负号反了导致升降舵在往反方向打。6.3 一组典型参数对比实验的结果分析用参数扫描的方式验证调试效果最直观。固定路径和模型参数只改变PID的Kp或前视距离参数跑三组仿真并统计关键指标就能清楚看到参数变化的规律。举例说明对同样的折线路径三组参数的结果可能如下参数方案最大横向偏差收敛时间稳态误差航向振荡Kp0.6 前视固定12m7.8m85s1.2m轻微Kp1.8 前视自适应6~20m3.5m45s0.3m无Kp4.0 前视固定6m2.1m25s0.4m明显振荡从表中能看出一个规律Kp增大确实能加快收敛、缩小最大偏差但代价是稳定性下降出现持续振荡前视自适应则能在不牺牲稳定性的前提下显著提升收敛速度。因此我推荐的调参路线是先固定前视自适应参数把PID调稳再缩短L_min或者增大Kp去压收敛时间不要一上来就用大Kp小前视这种极限组合。参数对比实验的脚本实现很简单外层循环改PARAM字段内层跑完整仿真统计最大偏差和收敛时间最后汇总成表。这个流程是仿真调试的核心工作流建议一开始就写好后面每次改参数都直接复用。7. 调试中真正咬人的几个细节与解决办法7.1 误差角度的±pi跳变这个坑在前面已经提过一次但值得单独强调。只要路径或载体经历某些几何位置期望航向角就可能从pi跳到-pi即使实际误差很小不经过wrap处理的角度差也会表现为2pi级别的假误差。小则出现尖峰大则直接发散。排查方法很简单把psi_d、psi、wrap前后的误差曲线同时打印出来看到哪个时间点出现跳变检查该处是否需要wrap。我见过不少人一开始在仿真里用wrap后来加上扰动后又忘了新加的环节导致问题反复出现。这里建议把wrap_to_pi封装成一个公共函数所有角度差计算统一调用不要重复写mod逻辑。7.2 微分项的噪声放大PID的微分项本质上是高通滤波器会把传感器测量噪声、数值计算噪声放大几十倍。在纯仿真环境里噪声主要来自数值积分离散化和误差曲线的微小抖动模型越复杂噪声越明显。表现是控制量输出高频抖动舵角指令在很小范围内来回跳动轨迹上出现细微毛刺。解决方法是给微分通道加一阶低通滤波截止频率取5~10Hz只让真实低频误差趋势进入D项。更简单粗暴的做法是直接限制D项输出幅值让它在正常摆动范围内不会影响P项主导的响应。实际工程里还可以用反馈微分结构对输出量微分而不是对误差微分能避免期望信号改变时D项跳变。7.3 仿真步长与离散化写法手动离散化状态下仿真步长必须同时满足控制系统采样需求和模型动态需求。船体惯性时间常数通常有几秒到十几秒理论上0.1秒步长就够但在航路点切换瞬间会有尖锐的几何变化步长太大会漏掉短时响应轨迹看起来失真。我通常选0.02秒作为固定步长数值离散没有出现震荡又不至于让仿真跑太慢。另一个容易被忽略的坑是离散PID里微分项的时间尺度。连续域里Kd的单位是秒离散化后实际起作用的是Kd/dt如果忘了除以步长dm微分作用会被放大50倍导致系统剧烈振荡。之前我见过有人把Kd取0.5配上0.01秒步长效果就像Kd取了50控制器瞬间饱和。7.4 一点后续扩展建议这套代码跑通之后后续扩展的方向很明确。一是加海流扰动项在速度方程里混入常值干扰这能立刻暴露LOS制导在常值扰动下的稳态偏差问题也可以顺势把LOS升级为ILOS来对比效果。二是改路径类型加入曲线路径或B样条光滑路径观察制导层前视点计算是否能平滑过渡。三是做控制算法的横向对比把PID换成模糊PID或滑模控制器跑同样的路径和参数比较误差峰值和稳态误差。我个人在最开始搭这套仿真时习惯是先把制导层和控制层写成两个独立函数所有参数集中在结构体里路径和仿真条件单独放在脚本最上面。这样每次换路径、换控制算法都是改参数或者换函数接口的事完全不用重写主循环。后面做课题对比实验时这个设计节省了非常多的时间和调试成本算是给后来者一个小建议。