运动学模型的前向递推-IDM与自行车

2025年11月18日 3.9k 字大概 20 分钟

智能驾驶员模型（Intelligent Driver Model, IDM）
- 2. 动态期望安全距离 $s^*$
- 3. 模型输入与参数定义
- 简易实现
- 示例伪代码
运动学自行车模型的前向仿真
- 🚲 运动学自行车模型 (Kinematic Bicycle Model)
- 运动学方程
  - 欧拉法前向积分
  - 四阶龙格-库塔法（RK4）积分
    - 积分步骤

智能驾驶员模型（Intelligent Driver Model, IDM）

🚗 智能驾驶员模型 (IDM) 详解IDM 的核心目标是计算自车的期望加速度 $a$ ，这个加速度 $a$ 会在两个极端情况之间取得平衡：

自由流驾驶（Free Driving）：当前方没有车辆时，车辆倾向于加速至其期望速度 $v_0$ 。

跟车驾驶（Car Following）：当与前车距离较近时，车辆必须减速以保持一个动态的安全距离 $s^*$ 。

a_{\text{IDM}} = a_{\text{max}} \left[ 1 - \left(\frac{v}{v_0}\right)^\delta - \left(\frac{s^*}{s}\right)^2 \right]

公式主要由三部分组成：

① 自由流加速度项（Free-Driving Term）： $a_{\text{max}} \left[ 1 - \left(\frac{v}{v_0}\right)^\delta \right]$

目的： 描述车辆在没有前车阻碍时，如何加速到其目标速度 $v_0$ 。
行为：

当自车速度 $v$ 远小于期望速度 $v_0$ 时，该项接近 $a_{\text{max}}$ （最大加速度）。

当 $v$ 接近 $v_0$ 时，该项接近 0，加速度减小。

当 $v$ 超过 $v_0$ 时，该项变为负值，车辆开始减速。

② 跟车制动项（Interaction/Braking Term）： $-a_{\text{max}} \left(\frac{s^*}{s}\right)^2$

目的： 描述车辆为避免碰撞而减速的行为。
行为：

当实际距离 $s$ 远大于期望安全距离 $s^*$ 时，该项接近 0，跟车影响很小。

当 $s$ 接近 $s^*$ 时，该项急剧减小（变为较大的负值），产生强大的制动力。

2. 动态期望安全距离 $s^*$

在 IDM 中，安全距离 $s^*$ 不是一个固定值，它会根据自车速度 $v$ 和相对速度 $v_{\text{rel}}$ 动态调整，是 IDM 的关键所在。

s^* = s_0 + v T + \frac{v \cdot v_{\text{rel}}}{2 \sqrt{a_{\text{max}} b_{\text{max}}}}

其中：

符号	描述	组成部分	作用
$s_0$	静止安全距离	$s_0$	车辆静止时与前车应保持的最小距离（防止追尾）。
$v T$	速度相关距离	$v T$	确保在期望时距 $T$ 内，前车突然停车，自车有时间反应并制动。
$\frac{v \cdot v_{\text{rel}}}{...}$	相对速度距离	$\frac{v \cdot v_{\text{rel}}}{2 \sqrt{a_{\text{max}} b_{\text{max}}}}$	动态制动距离。如果 $v_{\text{rel}} > 0$ （自车比前车快），需要更大的安全距离来缓冲制动。

3. 模型输入与参数定义

要实现 IDM，需要以下输入（来自传感器）和参数（模型标定）：

类别	符号	含义	示例值
输入	$v$	自车当前速度	$15 \text{ m/s}$
	$v_{\text{front}}$	前车当前速度	$13 \text{ m/s}$
	$s$	自车与前车的净距	$20 \text{ m}$
参数	$v_0$	期望最大速度（目标速度）	$30 \text{ m/s}$
	$a_{\text{max}}$	最大舒适加速度	$1.5 \text{ m/s}^2$
	$b_{\text{max}}$	最大舒适减速度	$3.0 \text{ m/s}^2$
	$T$	期望时距（Time Headway）	$1.5 \text{ s}$
	$s_0$	静止安全距离	$2.0 \text{ m}$

简易实现

步骤 1：计算相对速度

v_{\text{rel}} = v - v_{\text{front}}

步骤 2：计算动态期望安全距离 $s^*$

使用 $v$ , $v_{\text{rel}}$ , 和模型参数计算 $s^*$ 。

# 计算 sqrt(a_max * b_max)
denom = 2 * sqrt(a_max * b_max)

# 计算动态项 (相对速度项)
dynamic_term = (v * v_rel) / denom

# 计算 s*
s_star = s0 + v * T + dynamic_term

步骤 3：计算 IDM 加速度 $a_{\text{IDM}}$

将 $v_0$ , $v$ , $s$ , 和 $s^*$ 代入主公式：

# 1. 自由流项
free_flow_term = 1 - (v / v0)**delta 

# 2. 跟车制动项
braking_term = -(s_star / s)**2

# 3. 最终加速度 (基础值)
a_idm_base = a_max * (free_flow_term + braking_term)

步骤 4：限制输出加速度

计算出的 $a_{\text{IDM}}$ 必须限制在物理和舒适性范围之内。

a_{\text{final}} = \text{clip}(a_{\text{IDM}}, -b_{\text{max}}, a_{\text{max}})

示例伪代码

#include <cmath>
#include <algorithm>

// 定义 IDM 模型的参数结构体
struct IDM_Parameters {
    // 自由流参数
    double a_max;       // 最大舒适加速度 [m/s^2]
    double v0;          // 期望最大速度 [m/s]
    double delta;       // 加速度指数 (通常为 4)
    
    // 跟车参数
    double s0;          // 静止安全距离 [m]
    double T;           // 期望时距 (Time Headway) [s]
    double b_max;       // 最大舒适减速度 [m/s^2]
};

// 智能驾驶员模型类
class IntelligentDriverModel {
public:
    // 构造函数，初始化模型参数
    IntelligentDriverModel(const IDM_Parameters& params) : params_(params) {}

    /**
     * @brief 计算自车的期望加速度 a_IDM
     * * @param v_ego 自车当前速度 [m/s]
     * @param v_front 前车当前速度 [m/s]
     * @param s_gap 自车与前车的净距 (实际距离) [m]
     * @return double 期望加速度 [m/s^2]
     */
    double calculate_acceleration(double v_ego, double v_front, double s_gap) const {
        // 防止除以零或其他数值问题，确保速度和距离是合理的正值
        v_ego = std::max(v_ego, 0.0);
        s_gap = std::max(s_gap, 0.1); // 保持最小距离避免除零

        // 步骤 1: 计算相对速度 v_rel (正值表示接近前车)
        double v_rel = v_ego - v_front;

        // 步骤 2: 计算动态期望安全距离 s*
        // s* = s0 + v * T + (v * v_rel) / (2 * sqrt(a_max * b_max))
        
        // 计算动态制动项的分母
        double denom = 2.0 * std::sqrt(params_.a_max * params_.b_max);
        
        // 相对速度项
        double dynamic_term = (v_ego * v_rel) / denom;

        // 期望安全距离 s_star
        double s_star = params_.s0 + params_.T * v_ego + dynamic_term;

        // 步骤 3: 计算 IDM 基础加速度 a_IDM_base
        
        // 3.1 自由流加速度项 (Free-Driving Term)
        double free_flow_term;
        if (params_.v0 > 1e-6) {
            free_flow_term = 1.0 - std::pow(v_ego / params_.v0, params_.delta);
        } else {
            // 期望速度为零，保持最大加速度 (或根据实际需求设置为零)
            free_flow_term = 1.0; 
        }

        // 3.2 跟车制动项 (Braking Term)
        // 使用实际距离 s_gap 和期望距离 s_star
        double braking_term = -std::pow(s_star / s_gap, 2.0);

        // 3.3 最终加速度 (基础值)
        double a_idm_base = params_.a_max * (free_flow_term + braking_term);

        // 步骤 4: 限制输出加速度在舒适范围 [-b_max, a_max] 内
        // 在实际应用中，如果 IDM 模型计算出低于 -b_max 的加速度，通常意味着需要紧急制动。
        // 但 IDM 本身是设计为舒适的，所以直接 clip 即可。
        double a_final = std::max(-params_.b_max, a_idm_base);
        a_final = std::min(params_.a_max, a_final);
        
        return a_final;
    }

private:
    IDM_Parameters params_;
};

// --- 示例用法 ---
void main_example() {
    IDM_Parameters default_params = {
        1.5,   // a_max (m/s^2)
        30.0,  // v0 (m/s)
        4.0,   // delta
        2.0,   // s0 (m)
        1.5,   // T (s)
        3.0    // b_max (m/s^2)
    };

    IntelligentDriverModel idm(default_params);

    // 场景: 自车 15 m/s 接近 13 m/s 的前车，距离 20 m
    double v_ego = 15.0;
    double v_front = 13.0;
    double s_gap = 20.0;

    double acceleration = idm.calculate_acceleration(v_ego, v_front, s_gap);

    // 输出结果
    // ...
}

运动学自行车模型的前向仿真

🚲 运动学自行车模型 (Kinematic Bicycle Model)

模型定义车辆状态 $S$ 和控制输入 $U$ :

状态 $S$ :
$S = [x, y, \psi, v]^{\text{T}}$
$x, y$ : 后轴中心的世界坐标。
$\psi$ (yaw): 车辆的航向角（相对于 $x$ 轴）。
$v$ : 车辆的纵向速度。

控制输入 $U$ :
$U = [a, \delta]^{\text{T}}$
$a$ (acceleration): 纵向加速度。
$\delta$ (steering_angle): 前轮转向角。

运动学方程

车辆状态随时间变化的一阶微分方程如下：

\begin{align*} \dot{x} &= v \cos(\psi) \\ \dot{y} &= v \sin(\psi) \\ \dot{\psi} &= \frac{v}{L} \tan(\delta) \\ \dot{v} &= a \end{align*}

其中 $L$ 是轴距（Wheelbase）。

知道 $x_t$ ，要得到 $x_{t+1}$ ，怎么实现呢？
状态的导数方程我们已经知道 $\dot{S} = [\dot{x}, \dot{y}, \dot{\psi}, \dot{v}]^{\text{T}}$

欧拉法前向积分

欧拉法的基本思想是利用泰勒级数展开的一阶近似，假设在很短的时间步长 $\Delta t$ 内，状态的变化率（导数） $\dot{S}(t_k)$ 保持不变。

此时 $S(t_{k+1}) \approx S(t_k) + \text{Slope} \cdot (t_{k+1} - t_k)$

// 状态结构体
struct State {
    double x;         // x 坐标 (m)
    double y;         // y 坐标 (m)
    double yaw;       // 航向角 (rad)
    double v;         // 速度 (m/s)
};

// 控制输入结构体
struct ControlInput {
    double acceleration;  // 加速度 a (m/s^2)
    double steering_angle; // 前轮转向角 delta (rad)
};

class KinematicBicycleModel {
public:
    const double L; // 轴距 (Wheelbase) [m]

    KinematicBicycleModel(double wheelbase) : L(wheelbase) {}

    /**
     * @brief 使用欧拉法计算下一时刻状态 S_{t+1}
     * * @param current_state 当前时刻的状态 S_t
     * @param input_u 控制输入 U_t
     * @param dt 仿真时间步长 (s)
     * @return State 下一时刻的状态 S_{t+1}
     */
    State predict_euler(const State& current_state, const ControlInput& input_u, double dt) const {
        // --- 1. 计算当前时刻的状态导数 (S_dot) ---

        // x_dot = v * cos(yaw)
        double x_dot = current_state.v * std::cos(current_state.yaw);

        // y_dot = v * sin(yaw)
        double y_dot = current_state.v * std::sin(current_state.yaw);

        // yaw_dot = (v / L) * tan(delta)
        double yaw_dot = 0.0;
        if (std::abs(L) > 1e-6) { // 避免除以零
            yaw_dot = (current_state.v / L) * std::tan(input_u.steering_angle);
        }
        
        // v_dot = a
        double v_dot = input_u.acceleration;


        // --- 2. 使用欧拉法积分 S_{t+1} = S_t + S_dot * dt ---
        State next_state;

        next_state.x = current_state.x + x_dot * dt;
        next_state.y = current_state.y + y_dot * dt;
        
        // 积分航向角
        next_state.yaw = current_state.yaw + yaw_dot * dt; 
        
        // 积分速度
        next_state.v = current_state.v + v_dot * dt;

        // --- 3. 状态约束与归一化 ---
        
        // 确保速度不小于 0 (物理限制)
        next_state.v = std::max(0.0, next_state.v);

        // 归一化航向角到 [-PI, PI]
        next_state.yaw = normalize_angle(next_state.yaw);

        return next_state;
    }

private:
    /**
     * @brief 归一化角度到 [-PI, PI] 范围
     */
    double normalize_angle(double angle) const {
        // 这是一个常见的角度归一化实现
        angle = std::fmod(angle + M_PI, 2.0 * M_PI);
        if (angle < 0) {
            angle += 2.0 * M_PI;
        }
        return angle - M_PI;
    }
};

// --- 示例用法 ---
void main_example() {
    // 假设轴距 L = 2.8 m
    KinematicBicycleModel model(2.8);
    double dt = 0.1; // 100 ms 采样时间

    State current_state = {
        0.0, 0.0,           // 初始位置 (0, 0)
        M_PI / 4,           // 初始航向角 45度
        5.0                 // 初始速度 5 m/s
    };

    ControlInput input_u = {
        0.5,                // 加速度 0.5 m/s^2 
        0.1745              // 转向角 10度 (左转)
    };

    std::cout << "初始状态: x=" << current_state.x << ", y=" << current_state.y 
              << ", yaw=" << current_state.yaw << ", v=" << current_state.v << std::endl;

    // 仿真 10 步 (1秒)
    for (int i = 0; i < 10; ++i) {
        current_state = model.predict_euler(current_state, input_u, dt);
    }

    std::cout << "1秒后状态: x=" << current_state.x << ", y=" << current_state.y 
              << ", yaw=" << current_state.yaw << ", v=" << current_state.v << std::endl;
}

四阶龙格-库塔法（RK4）积分

比欧拉法精度高得多。四阶方法，意味着其局部误差与 $(\Delta t)^5$ 成正比，全局误差与 $(\Delta t)^4$ 成正比。

积分步骤

1.算四个斜率 $k_1, k_2, k_3, k_4$ $\begin{align*} k_1 &= f(S_k, U_k) \\ k_2 &= f\left( S_k + k_1 \frac{\Delta t}{2}, U_k \right) \\ k_3 &= f\left( S_k + k_2 \frac{\Delta t}{2}, U_k \right) \\ k_4 &= f\left( S_k + k_3 \Delta t, U_k \right) \end{align*}$

2.计算加权平均变化率

四个斜率按照 $1:2:2:1$ 的比例加权平均：

\dot{S}\_{\text{avg}} = \frac{1}{6} (k\_1 + 2k\_2 + 2k\_3 + k\_4)

3.更新状态

S_{k+1} = S_k + \dot{S}_{\text{avg}} \cdot \Delta t

#include <cmath>
#include <algorithm>
#include <iostream>
#include <vector>

// 定义 PI 常量
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

// --- 1. 状态和输入结构体 ---

// 车辆当前状态
struct State {
    double x;         // x 坐标 (m)
    double y;         // y 坐标 (m)
    double yaw;       // 航向角 (rad)
    double v;         // 速度 (m/s)
};

// 控制输入
struct ControlInput {
    double acceleration;  // 加速度 a (m/s^2)
    double steering_angle; // 前轮转向角 delta (rad)
};

// 状态导数 (S_dot)
// 注意：该结构体与 State 结构体具有相同的成员，但存储的是变化率
struct StateDerivative {
    double x_dot;
    double y_dot;
    double yaw_dot;
    double v_dot;
};

class KinematicBicycleModelRK4 {
public:
    const double L; // 轴距 (Wheelbase) [m]

    KinematicBicycleModelRK4(double wheelbase) : L(wheelbase) {}

    /**
     * @brief 使用四阶龙格-库塔法 (RK4) 计算下一时刻状态 S_{t+1}
     * @param current_state 当前时刻的状态 S_t
     * @param input_u 控制输入 U_t
     * @param dt 仿真时间步长 (s)
     * @return State 下一时刻的状态 S_{t+1}
     */
    State predict_rk4(const State& current_state, const ControlInput& input_u, double dt) const {
        // --- 1. 计算四个斜率 K1, K2, K3, K4 ---
        
        // K1 = f(S_t, U_t)
        StateDerivative k1 = calculate_s_dot(current_state, input_u);

        // K2 = f(S_t + k1 * (dt/2), U_t)
        State s_temp1 = add_state_derivative(current_state, k1, dt / 2.0);
        StateDerivative k2 = calculate_s_dot(s_temp1, input_u);

        // K3 = f(S_t + k2 * (dt/2), U_t)
        State s_temp2 = add_state_derivative(current_state, k2, dt / 2.0);
        StateDerivative k3 = calculate_s_dot(s_temp2, input_u);

        // K4 = f(S_t + k3 * dt, U_t)
        State s_temp3 = add_state_derivative(current_state, k3, dt);
        StateDerivative k4 = calculate_s_dot(s_temp3, input_u);

        // --- 2. 计算加权平均变化率 S_dot_avg ---
        // S_dot_avg = (k1 + 2*k2 + 2*k3 + k4) / 6
        StateDerivative s_dot_avg;
        s_dot_avg.x_dot = (k1.x_dot + 2.0 * k2.x_dot + 2.0 * k3.x_dot + k4.x_dot) / 6.0;
        s_dot_avg.y_dot = (k1.y_dot + 2.0 * k2.y_dot + 2.0 * k3.y_dot + k4.y_dot) / 6.0;
        s_dot_avg.yaw_dot = (k1.yaw_dot + 2.0 * k2.yaw_dot + 2.0 * k3.yaw_dot + k4.yaw_dot) / 6.0;
        s_dot_avg.v_dot = (k1.v_dot + 2.0 * k2.v_dot + 2.0 * k3.v_dot + k4.v_dot) / 6.0;

        // --- 3. 更新状态 S_{t+1} = S_t + S_dot_avg * dt ---
        State next_state = add_state_derivative(current_state, s_dot_avg, dt);

        // --- 4. 状态约束与归一化 ---
        next_state.v = std::max(0.0, next_state.v);
        next_state.yaw = normalize_angle(next_state.yaw);

        return next_state;
    }

private:
    // --- 辅助函数 ---

    /**
     * @brief 状态导数函数 f(S, U)：计算当前状态 S 的变化率 S_dot
     * Kinematic Bicycle Model 方程
     */
    StateDerivative calculate_s_dot(const State& s, const ControlInput& u) const {
        StateDerivative s_dot;
        
        // 避免除以零
        const double L_safe = (std::abs(L) > 1e-6) ? L : 1e-6; 

        // x_dot = v * cos(yaw)
        s_dot.x_dot = s.v * std::cos(s.yaw);

        // y_dot = v * sin(yaw)
        s_dot.y_dot = s.v * std::sin(s.yaw);

        // yaw_dot = (v / L) * tan(delta)
        s_dot.yaw_dot = (s.v / L_safe) * std::tan(u.steering_angle);
        
        // v_dot = a
        s_dot.v_dot = u.acceleration;

        return s_dot;
    }

    /**
     * @brief 计算临时状态： S_temp = S_current + S_dot * delta_t
     */
    State add_state_derivative(const State& s, const StateDerivative& s_dot, double delta_t) const {
        State s_new;
        s_new.x = s.x + s_dot.x_dot * delta_t;
        s_new.y = s.y + s_dot.y_dot * delta_t;
        s_new.yaw = s.yaw + s_dot.yaw_dot * delta_t;
        s_new.v = s.v + s_dot.v_dot * delta_t;
        
        // 在计算 RK4 的中间状态时，速度也应保证非负
        s_new.v = std::max(0.0, s_new.v); 
        return s_new;
    }

    /**
     * @brief 归一化角度到 [-PI, PI] 范围
     */
    double normalize_angle(double angle) const {
        // 使用标准库函数进行归一化
        angle = std::fmod(angle + M_PI, 2.0 * M_PI);
        if (angle < 0) {
            angle += 2.0 * M_PI;
        }
        return angle - M_PI;
    }
};

// --- 示例用法 ---
int main() {
    // 假设轴距 L = 2.8 m
    KinematicBicycleModelRK4 model(2.8);
    double dt = 0.1; // 100 ms 采样时间

    State current_state = {
        0.0, 0.0,           // 初始位置 (0, 0)
        M_PI / 4,           // 初始航向角 45度
        5.0                 // 初始速度 5 m/s
    };

    ControlInput input_u = {
        0.5,                // 加速度 0.5 m/s^2 
        0.1745              // 转向角 10度 (左转)
    };

    std::cout << "--- RK4 运动学模型仿真 ---" << std::endl;
    std::cout << "初始状态: x=" << current_state.x << ", y=" << current_state.y 
              << ", yaw=" << current_state.yaw << " rad, v=" << current_state.v << " m/s" << std::endl;

    // 仿真 10 步 (1秒)
    for (int i = 0; i < 10; ++i) {
        current_state = model.predict_rk4(current_state, input_u, dt);
    }

    std::cout << "\n1秒后状态 (RK4):" << std::endl;
    std::cout << "x=" << current_state.x << " m, y=" << current_state.y << " m" << std::endl;
    std::cout << "yaw=" << current_state.yaw << " rad (" << current_state.yaw * 180.0 / M_PI << " deg)" << std::endl;
    std::cout << "v=" << current_state.v << " m/s" << std::endl;

    return 0;
}