误差优化方向-1

你现在的 10 cm 左转横向误差，主要有两个“结构性”来源：

1. 前视点是沿切线“直着看”的（线性前视），在圆弧上会天然产生一个几何偏差 ≈ ½·κ·Ld²（κ 为曲率，Ld 为前视距离）；
2. 曲率与航向参考有滞后（κ 滤波、ψ̇ 近似），会让 δ_ff 与 r_ref 稍慢半拍，形成稳态偏差。

下面给出最小侵入式的 4 处修改 + 2 处小参数，基本不改变你的控制结构，但能把圆弧段的 |e1| 明显压低。按顺序做就行（每条都是“可以单独回退”的独立改动）。

---

✅ 改动 A（强烈建议）：把“线性前视”改为“圆弧前视”

位置：adrc_controller.m 中 %%% NEW --- 预瞄（前视点） 这一段。

替换这几行：

cpsi = cos(psi_d); spsi = sin(psi_d);
xd_p = xd_e + Ld * cpsi;
zd_p = zd_e + Ld * spsi;
psi_p = wrapToPiLocal( psi_d + k_e * Ld );

为下面的“圆弧一致”写法（在常曲率圆弧上无几何偏差）：

cpsi = cos(psi_d); spsi = sin(psi_d);
th   = k_e * Ld;                        % 预视角
if abs(k_e) > 1e-6s_k = sin(th)/k_e;                  % Δx_bodyc_k = (1 - cos(th))/k_e;            % Δy_bodyxd_p = xd_e + s_k * cpsi - c_k * spsi;zd_p = zd_e + s_k * spsi + c_k * cpsi;
elsexd_p = xd_e + Ld * cpsi;            % 直线退化zd_p = zd_e + Ld * spsi;
end
psi_p = wrapToPiLocal( psi_d + th );

为什么有效：你原来沿切线“直看”Ld 米，在圆弧上会少看“向心”那一小截，误差大概就是
(\frac{1}{2}\kappa L_d^2)（很多时候就是几厘米到十几厘米），改成圆弧前视后，这个几何偏差基本消失。

---

✅ 改动 B：让 yaw ESO 里的 r̂ 估计用 ψ̇_p（而不是 ψ̇_d）

位置：adrc_controller.m 中 “%%% NEW — 航向角速度参考与前馈”。

1. 先在状态初始化里加一行：

persistent Ld_last   % 记录上一步的前视距离
...
if isempty(initialized)...Ld_last = 0;     % 初始化
end

2. 用 ψ̇_p（预视航向导数）修正 r_hat 计算。把原来的：

psi_d_dot_ref = v_ref * k_e;      % 参考 ψ̇
r_hat = eso_yaw_z2 + psi_d_dot_ref;

替换为：

% 预视航向导数：ψ̇_p = ψ̇_d + κ·L̇d（忽略 κ̇·Ld, 圆弧足够好）
Ld_dot = (Ld - Ld_last) / max(Ts,1e-6);
Ld_last = Ld;
psi_p_dot_ref = v_ref * k_e + k_e * Ld_dot;r_hat = eso_yaw_z2 + psi_p_dot_ref;   % 让 r̂ ≈ r

说明：u_r 的目标 r_ref = vx * kappa_filt 可以保持不变；我们只是让估计 r̂ 与“e2 的定义（相对 ψ_p）”一致，减少系统性相位/幅值偏差。

---

✅ 改动 C：把曲率滤波的时间常数减半（减少 δ_ff 滞后）

位置：参数区

把

tau_kappa = 0.20;

改为

tau_kappa = 0.08;   % 0.08~0.12 皆可，0.08 圆弧跟随更紧

---

✅ 改动 D：e1/e2 误差滤波略快一点（少一点“拖尾”）

位置：参数区

把

tau_e = 0.1;

改为

tau_e = 0.08;   % 让 e1_filt/e2_filt 略灵敏（不会放大噪声太多）

---

☑️ 参数小调整 1：更早让 Mz 参与（入弯更稳）

位置：参数区“模式切换与控制门限参数”

e2_yaw_on = 0.8*pi/180;        % 原来 1° -> 0.8°
kappa_yaw_on = 8e-4;           % 原来 1e-3 -> 8e-4
e2_yaw_on_lo=0.5*e2_yaw_on; e2_yaw_on_hi=1.5*e2_yaw_on;
kappa_yaw_on_lo=0.5*kappa_yaw_on; kappa_yaw_on_hi=1.5*kappa_yaw_on;

这会让 s_turn 更早抬起，yaw 通道与 δ 的融合更快，“贴弧”更紧。

---

☑️ 参数小调整 2：“入弯软限”再温和一点（避免 δ 上限太保守）

位置：%%% NEW --- 入弯软限 那一段，把：

turn_soft_timer = min(turn_soft_timer + Ts, 0.4);
...
soft_fac = 0.65 + 0.35 * (turn_soft_timer/0.4);

改为：

turn_soft_timer = min(turn_soft_timer + Ts, 0.25);   % 缩短软启动时长
...
soft_fac = 0.80 + 0.20 * (turn_soft_timer/0.25);     % 基线从 0.65 -> 0.80

这样在入弯最初 0.25 s 内也不会把 δ 限得过低，减少“入弯初期”横向尾差。

---

（可选）改动 E：只在圆弧段给个侧向加速度限速（轨迹端）

如果你的 δ_max、R、速度组合接近侧向饱和，控制器“再聪明”也会留下一点尾差。这时在轨迹函数里把弧段的 v_ref 限一下，收益很大。

位置：traj_fixed.m，Mode 2 计算完 vref 之后、圆弧段掩码 m2 处，加入：

if any(m2)a_lat_lim = 1.2;  % [m/s^2] 视抓地力/甲板工况定，1.0~1.5 常用vcap = sqrt( a_lat_lim ./ max(abs(k_e(m2)), 1e-6) );vref(m2) = min(vref(m2), vcap);   % 只影响控制侧 v_ref，不改几何 S(t)
end

这会同步降低 adrc_controller 里的 v_ref、r_ref，把“要跟的弧”做得更可达，误差自然更小。

---

预期效果 & 调参提示

• 仅做 A+B+C+D，在 R≈20 m、v≈2–4 m/s、Ld≈0.8–1.2 m 的左转弯里，典型 |e1| 会从 ~0.10 m 降到 ~0.04–0.06 m，峰值降低 30–60%（不同工况会有差异）。
• 若还看到入弯起始那 0.2–0.3 s 有一点点偏差，做上面的“入弯软限”调整（参数小调整 2）通常能把那一撮也抹平。
• 如果 δ 仍频繁打到动态上限（看 dbg(40) 及 delta_cmd 对比），强烈建议加上改动 E（轨迹端的弧段限速）。
• 保持 k_r = 0.6*Mz_max 先不动；如果你侧向依旧偏“欠跟”，可以试着到 0.7~0.8*Mz_max，注意别让 Mz 过度起伏（看 dbg(42)/dbg(46)）。

---

为什么我优先改“几何一致性”和“小滞后”

• 你原始控制律已经很完整（双模、锁向、b 分离、ESO、r_ref 前馈等），在圆弧这种“稳态+少扰动”的工况下，误差地板往往由几何近似与滤波滞后决定，不是靠再堆增益能完全抹掉的。
• 把前视点改成“沿弧看”、让 yaw 估计与“预视航向”一致，再把 κ/误差滤波的时间常数稍微压一压，就能在不增拐点噪声的前提下把稳态偏差干掉大半。剩下的，再用轻微的门限/软限调整把入弯第一瞬间的“保守动作”放开一点即可。

---

一次性补丁清单（便于你检视）

• adrc_controller.m

  • 圆弧前视（替换前视点求法）
  • r_hat 使用 psi_p_dot_ref（新增 Ld_last）
  • tau_kappa = 0.08;
  • tau_e = 0.08;
  • e2_yaw_on = 0.8*pi/180; kappa_yaw_on = 8e-4; 并更新 lo/hi
  • 入弯软限：0.4s→0.25s，soft_fac: 0.65→0.80 起步

• traj_fixed.m（可选）

  • Mode 2 的 m2 圆弧段：vref(m2) = min(vref(m2), sqrt(a_lat_lim/|k_e|))

---

如果你愿意，我也可以把以上改动合并成两份完整的函数版本贴给你，直接替换即可（保持 dbg 向量长度不变，不破坏你现有的记录/可视化）。