TD

TD的每一时刻都在重新决策，没有预设轨迹，只有实时最优跟踪

TD 相比传统速度规划强的一点，是TD可“自愈”。fhan 的本质是闭环解算。即使在“卡顿”那一瞬间错过了最佳刹车点，一旦系统恢复，在卡顿结束后的第一个周期，td_update 会读到新的 \(x_1\)（离目标更近了）和新的 \(x_2\)（速度超标了），此时计算出的 \(a\) 会远大于 \(d\)，输出会立即从 \(+r\) 切换为 \(-r\)（全力反向制动），虽然可能还是会有轻微超调，但 TD 会以物理极限能力（\(r\)）去挽回损失。这比那些只会按预设曲线“死跑”的算法（由于步数对不上，可能直接发疯）要鲁棒得多。

/* @param x1 位置误差
 * @param x2 速度
 * @param r 快速跟踪因子（相当于最大加速度）
 * @param h 积分步长
 * @param h0 滤波因子（用于输入滤波，典型值为h的5-10倍）
 */
float td_fhan(float x1, float x2, float r, float h, float h0) {
    float d = r * h;                                // 单步速度变化量 (delta v)
    float d0 = h0 * d;                              // 线性区宽度

    float y = x1 + h * x2;                          /* 预测：当前位置 + h*速度 = 不加控制时的未来位置 */
    float a0 = sqrtf(d * d + 8.0f * r * fabsf(y));  /* 离散时间下的刹车轨迹方程的逆运算 */

如何推导 \(y\) 和 \(a_0\)？

对于一个在 \(t\) 时刻 \(n\) 阶可导的函数 \(x(t)\)，它在 \(t+h\) 时刻的值可以通过泰勒级数展开为：

其中：\(x(t)\) 是当前值。\(h\) 是时间增量（步长）。\(R_n(h)\) 是高阶无穷小余项。

在工程实现（尤其是离散控制）中，我们通常只取了泰勒展开的一阶近似（First-order Approximation）。

韩京清教授的最速控制研究对象是双积分系统，其物理模型如下：

我们将 \(x_1(t+h)\) 代入泰勒级数：

根据系统方程 \(\dot{x}_1 = x_2\) 和 \(\ddot{x}_1 = \dot{x}_2 = u\)，公式变为：

所以利用一阶泰勒展开，预测下一个采样周期 \(h\) 时的位置的公式就变成了：

由于 \(\dot{x}_1 = x_2\)（速度），故：

意义：\(y\) 代表了"如果不施加额外控制，下一时刻系统会到达的位置"。

作用：它让控制器产生了"超前意识"，有效地抵消了离散系统带来的相位滞后，是不超调的第一道防线。

切换指标 \(a_0\)

在代码中：\(a_0 = \sqrt{d^2 + 8r|y|}\)

在连续时间的最速控制（Time-Optimal Control）中，已知加速度极限为 \(r\)，根据牛顿力学 \(v^2 = 2as\)，刹车曲线方程为：

在相平面上，这是一条对称的抛物线。

相平面：将系统的状态变量作为坐标轴，把微分方程的解直观映射为平面上的运动轨迹。简单来说，对于二阶系统（如 \(\ddot{x}=f(x,\dot{x})\)），通常选取：

• 横轴：位置/误差 \(x\)
• 纵轴：速度 \(\dot{x}\)

每一点 \((x,\dot{x})\) 唯一确定系统的瞬时状态，无需显式知道时间 \(t\)。

此外韩教授指出，离散系统不能直接套用连续公式，必须考虑"步长 \(h\)"的离散跳跃。

假设系统以最大加速度 \(r\) 减速，经过 \(n\) 步回到原点：

• 每步速度变化量：\(d = r \cdot h\)
• 速度序列（倒推）：\(d, 2d, 3d, \dots, nd\)
• 总位移 \(y\)（各步位移之和）：

代入 \(d = rh\)：

为了解出 \(n\)（剩余步数），我们需要解上述二次方程。

第一步：整理为二次方程标准型

为了解出 \(n\)，我们将方程重组。为了方便计算，我们取 \(y\) 的绝对值 \(|y|\)：

整理得：

第二步：利用求根公式解出 \(n\)

这是一个关于 \(n\) 的一元二次方程 \(ax^2 + bx + c = 0\)，其中 \(a=1, b=1, c=-\frac{2|y|}{rh^2}\)。

根据求根公式 \(n = \frac{-b + \sqrt{b^2 - 4ac}}{2a}\)（取正根）：

简化得到：

在离散系统中，速度的变化量是按"步"计算的。定义单步最大速度变化量 \(d = rh\)。

系统如果要停止，它当前的理想速度 \(v_{ideal}\) 应该正比于剩余步数 \(n\)。

我们令一个中间速度指标 \(V = n \cdot d\)：

将括号外的 \(d\) 移入根号内（注意 \(d^2 = (rh)^2 = r^2 h^2\)）：

消掉分母中的 \(rh^2\)：

观察上面的公式，根号下的表达式正是我们要找的 \(a_0\)：

代回 \(V\) 的表达式：

公式：\(a_0 = \sqrt{d^2 + 8r|y|}\)

易晕解释：\(y\) 在代码逻辑上是基于"当前"测得的数据计算出来的，但它在算法逻辑中扮演的是"未来"的角色。

    float a;
    if (fabsf(y) <= d0) {                           // 线性区
        a = x2 + y / h;
    } else {                                        // 非线性区（赶路）
        a = x2 + 0.5f * (a0 - d) * ((y > 0) ? 1.0f : -1.0f);
    }

    float fhan_out;                                 // 输出
    if (fabsf(a) <= d) {                            // 线性插值区（接近停稳）
        fhan_out = -r * a / d;
    } else {                                        // 饱和区（满速加速/刹车）
        fhan_out = -r * ((a > 0) ? 1.0f : -1.0f);
    }

    return fhan_out;
}

在非线性区，系统离目标还远。为了在最短时间内停在原点，系统必须始终保持在最速控制切换曲线（那条抛物线）上。

我们推导出的理想刹车速度 \(V = \frac{a_0 - d}{2}\)，为了在最短时间内停在原点，系统必须始终保持在最速控制切换曲线（那条抛物线）上。

\(0.5 \cdot (a_0 - d)\) 就是基于当前剩余距离 \(y\)，计算出的当前时刻你应该具有的"临界速度"。

在线性区，误差 \(y\) 进入 \(d_0\) 范围内，系统已经非常靠近目标了。此时如果还用复杂的平方根计算，由于数值精度和离散步长限制，会产生细微的颠簸。

这里的逻辑是："预测下一时刻刚好归零"。

我们看输出逻辑：如果 \(a\) 很小，输出 \(fhan_{out} = -r \cdot \frac{a}{d}\)。

因为 \(d = rh\)，代入得：

如果你仔细看这个结构，它其实变成了一个 PD 控制器：

• 比例项 (P)：\(-\frac{1}{h^2} \cdot y\)
• 微分项 (D)：\(-\frac{1}{h} \cdot x_2\)

它不再追求加速度最大化，而是在最后一步，利用位置和速度的线性组合，像拉弹簧一样，平滑地把物体拉回到 \(y=0\)。

代码最后一部分，分为饱和区和线性插值区。

当偏差 \(a\) 很大，超过了单步改变能力 \(d\) 时，输出 \(fhan_{out} = -r \cdot \text{sgn}(a)\)，此时系统处于 Bang-Bang 状态，油门踩到底或刹车踩到底。这是为了保证最快。

快速补充：Bang-Bang 状态是指只有满输出/满输入两种状态。

当偏差 \(a\) 进入单步能力 \(d\) 之内时，输出 \(fhan_{out} = -r \cdot \frac{a}{d}\)，这是为了不超调。

如果不做这个线性处理，系统在最后一步可能会输出一个过大的加速度，导致跨过原点。

通过 \(\frac{a}{d}\) 这个比例缩放，加速度会随着靠近目标而逐渐减小，最终在目标点刚好减为 0。

请注意，输出 fhan_out 的正负号完全取决于 a。

• 如果 \(a > 0\)：输出加速度为 \(-r\)（减速/刹车）。
• 如果 \(a < 0\)：输出加速度为 \(+r\)（加速/追赶）。

所以，当 \(a\) 从负数变成正数的那一瞬间，就是系统“决定刹车”的时刻。

当 \(a=0\) 的时候，我们仔细来看看非线性区 \(a\) 的公式（假设目标在前方，\(y > 0\)）： $\(a = x_2 + \frac{a_0 - d}{2}\)$ 当 \(a = 0\) 时，意味着： $\(x_2 = -\frac{a_0 - d}{2}\)$ 这行等式的物理意义是：“我当前的速度 \(x_2\)，刚好等于我这辆车在当前距离下，所能允许的最大安全速度。”

• 如果 \(x_2\) 还没达到这个值：\(a < 0\)，系统觉得“还能再快点”，于是输出 \(+r\) 继续加速。

• 如果 \(x_2\) 刚好等于这个值：\(a = 0\)，系统意识到“刹车临界点到了”。

• 如果 \(x_2\) 超过了这个值：\(a > 0\)，系统惊觉“太快了，再不刹车就撞线了”，于是输出 \(-r\) 全力刹车。

评论