4. 路感模拟电机力矩计算

任务定义

本案例实现线控转向系统中路感模拟电机的目标力矩计算模块。输入为车速 \(v\)（km/h）与方向盘转角 \(\delta\)（deg），输出为理想路感力矩 \(T_{feel}\)（N·m）。采用二维查表模型：车速决定路感增益曲线，转角决定基础回正力矩，二者叠加后经饱和限幅得到最终力矩指令。

实现方案

路感力矩模型采用工程中常用的“车速增益 × 转角回正力矩”结构：

\[
T_{feel} = K_{speed}(v) \cdot T_{align}(\delta)
\]

其中 \(K_{speed}(v)\) 为随车速增加而递减的增益（低速轻便、高速稳重），\(T_{align}(\delta)\) 为与转角近似线性、带饱和特性的回正力矩基值。查表模型通过离散车速点与转角点建立二维网格，运行时采用双线性插值获得连续输出，避免在线计算复杂三角函数，适合嵌入式实时实现。

代码实现

#include <stdio.h>
#include <stdint.h>

#define N_SPEED 5
#define N_ANGLE 7

/* 车速断点：0, 20, 40, 60, 100 km/h */
static const float speed_breakpoints[N_SPEED] = {0.0f, 20.0f, 40.0f, 60.0f, 100.0f};

/* 转角断点：-540, -360, -180, 0, 180, 360, 540 deg */
static const float angle_breakpoints[N_ANGLE] = {-540.0f, -360.0f, -180.0f, 0.0f, 180.0f, 360.0f, 540.0f};

/* 路感力矩查表数据 [车速索引][转角索引]，单位 N·m */
static const float feel_map[N_SPEED][N_ANGLE] = {
    /* 0 km/h   */
    {-3.50f, -3.20f, -2.20f, 0.00f, 2.20f, 3.20f, 3.50f},
    /* 20 km/h  */
    {-2.80f, -2.50f, -1.70f, 0.00f, 1.70f, 2.50f, 2.80f},
    /* 40 km/h  */
    {-2.20f, -1.90f, -1.30f, 0.00f, 1.30f, 1.90f, 2.20f},
    /* 60 km/h  */
    {-1.70f, -1.50f, -1.00f, 0.00f, 1.00f, 1.50f, 1.70f},
    /* 100 km/h */
    {-1.20f, -1.00f, -0.70f, 0.00f, 0.70f, 1.00f, 1.20f}
};

/* 输出饱和限幅 */
#define T_MAX  3.50f
#define T_MIN -3.50f

static float clampf(float val, float lo, float hi) {
    if (val < lo) return lo;
    if (val > hi) return hi;
    return val;
}

/* 一维线性插值 */
static float interp1d(float x, const float *xp, const float *yp, int n) {
    if (x <= xp[0]) return yp[0];
    if (x >= xp[n-1]) return yp[n-1];
    int i = 0;
    while (i < n-2 && x > xp[i+1]) i++;
    float t = (x - xp[i]) / (xp[i+1] - xp[i]);
    return yp[i] + t * (yp[i+1] - yp[i]);
}

/* 二维查表：双线性插值 */
static float interp2d(float speed, float angle) {
    /* 在车速方向定位 */
    int si = 0;
    while (si < N_SPEED-2 && speed > speed_breakpoints[si+1]) si++;
    float s_frac = (speed - speed_breakpoints[si]) /
                   (speed_breakpoints[si+1] - speed_breakpoints[si]);

    /* 在转角方向定位 */
    int ai = 0;
    while (ai < N_ANGLE-2 && angle > angle_breakpoints[ai+1]) ai++;
    float a_frac = (angle - angle_breakpoints[ai]) /
                   (angle_breakpoints[ai+1] - angle_breakpoints[ai]);

    float f00 = feel_map[si][ai];
    float f10 = feel_map[si+1][ai];
    float f01 = feel_map[si][ai+1];
    float f11 = feel_map[si+1][ai+1];

    /* 先沿车速插值，再沿转角插值 */
    float c0 = f00 + s_frac * (f10 - f00);
    float c1 = f01 + s_frac * (f11 - f01);
    return c0 + a_frac * (c1 - c0);
}

/* 路感力矩计算主函数 */
float calculate_feel_torque(float vehicle_speed, float steering_angle) {
    float torque = interp2d(vehicle_speed, steering_angle);
    return clampf(torque, T_MIN, T_MAX);
}

/* 简单自测 */
int main(void) {
    /* 用例1：低速大转角，期望接近饱和值 */
    float t1 = calculate_feel_torque(0.0f, 540.0f);
    /* 用例2：中速中转角 */
    float t2 = calculate_feel_torque(40.0f, 180.0f);
    /* 用例3：高速小转角 */
    float t3 = calculate_feel_torque(100.0f, 90.0f);
    /* 用例4：边界外插（车速120，转角600） */
    float t4 = calculate_feel_torque(120.0f, 600.0f);

    printf("T(0km/h, 540deg) = %.2f N·m\n", t1);
    printf("T(40km/h, 180deg) = %.2f N·m\n", t2);
    printf("T(100km/h, 90deg) = %.2f N·m\n", t3);
    printf("T(120km/h, 600deg) = %.2f N·m\n", t4);
    return 0;
}

落地与验证

文件与编译：将上述代码保存为 feel_torque_lookup.c，使用任意C编译器编译（如 gcc -o feel_torque feel_torque_lookup.c -lm），运行可执行文件即可。

验证方法：程序输出四个测试点的力矩值。判定标准：

• 用例1输出应为 3.50（查表边界值，经限幅不变）；
• 用例2输出应为 1.30（40km/h、180°处查表值）；
• 用例3输出约为 0.35（100km/h下90°位于0°与180°之间线性插值，即0.7/2）；
• 用例4输出应为 1.20（超出车速与转角范围时取边界值，不发生越界）。

若输出与上述一致，说明查表、插值与限幅逻辑正确。实际工程中可将该函数嵌入周期性中断（如1ms任务），输入车速与转角信号，输出直接作为路感电机电流环的力矩指令。