4. 力矩反馈在路径规划中的应用：势场法改进、DWA算法中的力矩约束、TEB算法中的力矩项

路径规划这事儿，说白了就是让机器人知道「怎么走」。但光知道怎么走还不够，你得让它走得稳、走得安全。我刚开始做导航的时候，总觉得路径规划就是算出一条无碰撞的线就完事了。结果呢？机器人是没撞墙，但遇到个斜坡就原地打滑，遇到个软地面就偏得离谱。

后来我才明白——路径规划必须考虑力矩。你想想看，规划出来的路径再漂亮，如果执行器根本使不上劲，那就是一张废纸。这一章，我就把三种主流算法里怎么塞进力矩感知的实战经验，掰开了讲给你听。

4.1 势场法改进：给引力/斥力加上力矩权重

传统势场法有个老毛病——引力场和斥力场都是纯几何的。机器人离目标近就往那拉，离障碍物近就往那推。但问题是，它不管机器人当前能不能出那个力。

我记得有一次做轮式机器人爬坡，势场法规划出来的路径直接对着一个30度的坡就冲过去了。理论上没障碍，路径最短。但实际呢？电机扭矩不够，爬到一半就溜回来了。这就是典型的「几何可行，物理不可行」。

4.1.1 力矩加权势场

我的改进思路很简单：在势场函数里加一个力矩权重系数。具体来说，引力势场不再只跟距离有关，还要考虑当前姿态下机器人能输出的最大牵引力。

# 改进后的引力势场函数（Python伪代码）
def attractive_potential_with_torque(pose, goal, torque_available):
    # 传统距离项
    dist = np.linalg.norm(pose[:2] - goal[:2])
    # 力矩可行性因子：当前方向所需力矩 / 可用力矩
    required_torque = estimate_required_torque(pose, goal)
    torque_factor = min(1.0, torque_available / (required_torque + 1e-6))
    # 加权后的势场
    return 0.5 * k_att * dist**2 * (1.0 / torque_factor)

4.1.2 地面反作用力修正

另一个坑是地面条件。同样的路径，在水泥地上能走，在沙地上就陷进去了。我在斥力场里加了一项「地面反作用力势场」——如果某个区域的地面支撑力不足，或者摩擦系数太低，斥力场会自动增大。

地面类型	摩擦系数	力矩修正系数	势场影响
水泥/沥青	0.7 - 0.9	1.0	无修正
草地/泥土	0.4 - 0.6	0.6 - 0.8	中等排斥
沙地/雪地	0.2 - 0.3	0.3 - 0.5	强排斥
冰面/湿滑	0.1 - 0.2	0.1 - 0.3	极高排斥

4.2 DWA算法中的力矩约束

DWA（动态窗口法）的核心是速度空间采样。传统DWA只考虑速度、加速度限制，然后从采样轨迹里选一条代价最小的。但代价函数里通常只有：距离障碍物、朝向目标、速度大小这几项。

你发现问题了吗？——没有力矩项。这就导致DWA经常选出一条「速度很快但力矩不够」的轨迹。我踩过这个坑，当时机器人在一个缓坡上反复横跳，就是因为DWA选的速度组合需要很大的加速度，但电机根本响应不过来。

4.2.1 力矩代价函数

我在DWA的代价函数里加了一项 torque_cost。具体做法是：对每个采样速度对 (v, ω)，估算出对应的轮子力矩需求，然后跟当前可用力矩做比较。

# DWA代价函数中的力矩项
def torque_cost(v, omega, torque_available):
    # 估算所需力矩（简化模型）
    torque_required = abs(v) * k_v + abs(omega) * k_w + torque_bias
    # 超出可用力矩越多，代价越大
    if torque_required <= torque_available:
        return 0.0
    else:
        excess = (torque_required - torque_available) / torque_available
        return 10.0 * excess**2  # 二次惩罚

4.2.2 动态窗口的力矩边界

除了在代价函数里加惩罚，我还建议直接修改动态窗口的边界。传统DWA的动态窗口由速度边界、加速度边界决定。我加了一个「力矩可行性窗口」——只有那些在当前力矩限制下能稳定执行的速度组合，才进入采样空间。

# 力矩可行性窗口计算
def torque_feasible_window(v_current, omega_current, torque_max):
    # 根据当前力矩输出，反推下一时刻可行的速度范围
    v_min = v_current - delta_v_torque(torque_max)
    v_max = v_current + delta_v_torque(torque_max)
    omega_min = omega_current - delta_omega_torque(torque_max)
    omega_max = omega_current + delta_omega_torque(torque_max)
    return (v_min, v_max, omega_min, omega_max)

4.3 TEB算法中的力矩项

TEB（Timed Elastic Band）算法把路径规划建模成一个稀疏优化问题。它的核心是超图（hypergraph），节点是位姿和时间，边是各种约束（速度、加速度、障碍物、运动学等）。

说实话，TEB是我个人最喜欢的局部规划器。因为它灵活——你想加什么约束，就往超图里加一条边就行了。力矩约束也不例外。

4.3.1 力矩超图边

我在TEB的超图里加了一种新的边类型：TorqueEdge。这条边连接相邻的两个位姿节点，约束的是从位姿i到位姿i+1所需的力矩不能超过当前可用力矩。

// C++伪代码：TEB中的力矩边
class TorqueEdge : public BaseEdge<1, double> {
public:
    TorqueEdge(const PoseSE2& pose_i, const PoseSE2& pose_j,
               double torque_limit) {
        // 计算从pose_i到pose_j所需的力矩
        double dt = pose_j.time() - pose_i.time();
        double curvature = compute_curvature(pose_i, pose_j);
        double required_torque = k_t * curvature / dt;
        
        // 误差 = 所需力矩超出限制的部分
        double error = std::max(0.0, required_torque - torque_limit);
        setMeasurement(error);
    }
    
    void computeError() override {
        // 优化过程中自动计算误差
        _error[0] = ...;  // 力矩超限部分
    }
};

4.3.2 执行器饱和约束

除了力矩上限，还有一个容易被忽略的问题——执行器饱和。电机不是理想器件，当力矩指令超过额定值时，输出会进入非线性区，甚至可能过热保护。

我在TEB里加了一个「饱和惩罚项」：如果某个时间段的力矩需求持续超过额定值的90%，就在代价里加一个指数惩罚。这样TEB会自动把高力矩需求分散到更长的时间段里，避免执行器长时间过载。

# 饱和惩罚项（Python风格）
def saturation_penalty(torque_profile, torque_rated):
    # 计算连续超限时间
    exceed_time = 0.0
    penalty = 0.0
    for t, tau in enumerate(torque_profile):
        if tau > 0.9 * torque_rated:
            exceed_time += dt
            penalty += np.exp(exceed_time / tau_sat) - 1.0
        else:
            exceed_time = 0.0  # 冷却
    return penalty

4.4 三种方法的对比与选型建议

讲了三种方法，你可能会问：到底用哪个？我的建议是看场景。

算法	力矩集成方式	计算开销	适用场景
势场法改进	加权势场 + 地面修正	低	简单环境、地面变化明显的场景
DWA力矩约束	代价函数 + 可行性窗口	中	动态环境、需要快速响应的场景
TEB力矩项	超图边 + 饱和惩罚	高	复杂地形、需要精细优化的场景

我个人习惯是：简单场景用势场法改进，中等场景用DWA加力矩约束，复杂场景上TEB。但不管用哪个，记住一点——力矩感知不是锦上添花，而是让路径规划从「纸上谈兵」变成「真刀真枪」的关键一步。