最近在实验室折腾Matlab的轨迹控制，发现这玩意儿真能玩出花。咱们直接从二维无人机航迹跟踪开整。上回给四旋翼写PID跟踪，核心就这几行

在Matlab下编程实现二维与三维的航迹跟踪控制、路径跟踪控制和轨迹跟踪控制，实现编队集群控制与避障控制。 研究对象有空中无人机、地面机器人、水面无人艇、水下机器人以及多智能体等。

function [u] = pid_controller(x_ref, x_current, Kp, Ki, Kd, dt) persistent integral error_prev if isempty(integral), integral = 0; error_prev = 0; end error = x_ref - x_current; integral = integral + error*dt; derivative = (error - error_prev)/dt; u = Kp*error + Ki*integral + Kd*derivative; error_prev = error; end

调参时候发现Kd超过0.8就开始抖，后来发现是传感器噪声被放大了。这时候上卡尔曼滤波预处理数据效果立竿见影，实测轨迹标准差降了62%。

转战三维水面无人艇时，改用模型预测控制（MPC）。定义动力学模型时注意把流体动力参数化：

A = [0 0 cos(theta); 0 0 sin(theta); 0 0 -D/m]; % D为水阻力系数 B = [0 0; 0 0; 1/m 1/m]; Q = diag([10,10,1]); % 状态权重 R = 0.1*eye(2); % 输入权重

这里有个坑：预测步长超过3秒后解算容易发散。后来改用自适应步长策略，在转向时缩短预测时长，直线段再拉长，运算效率提升40%。

多机器人编队用领航-跟随法实现时，用结构体数组管理个体状态贼方便：

bots(1).x = [0;0]; % 领航者 for i=2:N bots(i).x = bots(1).x + formation_offset(:,i); bots(i).u = compute_ctrl(bots(i), bots(1)); end

实测时发现跟随者会"抢跑"，原来是领航者状态更新时序问题。加个状态缓冲区后，编队位置误差从0.5m降到0.1m以下。

在Matlab下编程实现二维与三维的航迹跟踪控制、路径跟踪控制和轨迹跟踪控制，实现编队集群控制与避障控制。 研究对象有空中无人机、地面机器人、水面无人艇、水下机器人以及多智能体等。

避障控制试过势场法，斥力场函数这么写：

function F = repulsion_force(p, p_obs, k_rep, d_safe) d = norm(p - p_obs); if d < d_safe F = k_rep*(1/d - 1/d_safe)*(p - p_obs)/d^3; else F = [0;0]; end end

但动态障碍物处理要加预测模块，有次demo时无人机差点怼墙上，紧急改成滚动时域预测才稳住。现在这套框架已经移植到水下机器人，把流体参数换成水动力系数照样跑得欢。

调试轨迹跟踪时发现微分平坦特性真好用，把状态空间映射到平坦输出空间，复杂度直接砍半。不过遇到非完整约束时得小心，有回在阿克曼转向的无人车上翻车，后来加了运动学层约束才解决。

这些控制算法最终要集成到同一个架构里，我们现在用状态机切换控制模式。当传感器检测到障碍时自动切入避障模式，平时走轨迹跟踪。实测多模式切换时控制量会有跳变，后来在交接班时做了指令平滑过渡，看着舒服多了。