6. 运动模型

本文描述 Autonomy common/motion_model 库中的运动学模型，供 Prediction 模块做轨迹前向传播。

6.1 接口设计

MotionModelInterface 采用 CRTP 模式：

template <typename Derived, typename StateT>
class MotionModelInterface {
public:
    StateT predict(const StateT& state, const std::chrono::nanoseconds& dt) {
        return static_cast<Derived*>(this)->crtp_predict(state, dt);
    }
    typename StateT::Matrix jacobian(const StateT& state, const std::chrono::nanoseconds& dt);
};

方法	说明
`predict(state, dt)`	状态前向传播 \(\mathbf{x}_{k+1} = f(\mathbf{x}_k, \Delta t)\)
`jacobian(state, dt)`	雅可比 \(F = \partial f / \partial \mathbf{x}\)，供卡尔曼使用

6.2 LinearMotionModel（恒速/恒加速）

文件：linear_motion_model.hpp / .cpp

状态：GenericState，典型为 \([x, y, v_x, v_y]\) 或含加速度扩展。

转移矩阵（CV）：

\[\begin{split} F = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{split}\]

预测：

State predicted = model.predict(state, std::chrono::milliseconds(100));

适用：行人、缓慢移动障碍、短期预测。

6.3 DifferentialDriveMotionModel

文件：differential_drive_motion_model.hpp / .cpp

状态：\([x, y, \theta, v, \omega]\) 或简化形式。

动力学：

\[\begin{split} \begin{aligned} \dot{x} &= v \cos\theta \\ \dot{y} &= v \sin\theta \\ \dot{\theta} &= \omega \end{aligned} \end{split}\]

离散化（欧拉）：

\[\begin{split} \begin{aligned} x_{k+1} &= x_k + v \cos\theta_k \Delta t \\ y_{k+1} &= y_k + v \sin\theta_k \Delta t \\ \theta_{k+1} &= \theta_k + \omega \Delta t \end{aligned} \end{split}\]

适用：差速移动机器人作为动态障碍时的预测。

6.4 StationaryMotionModel

文件：stationary_motion_model.hpp

预测：\(\mathbf{x}_{k+1} = \mathbf{x}_k\)（状态不变）

适用：静态障碍、停车车辆、未知动态障碍的保守假设。

6.5 与卡尔曼滤波的配合

// 预测步
auto x_pred = motion_model.predict(x_est, dt);
auto F = motion_model.jacobian(x_est, dt);
P_pred = F * P_est * F.transpose() + Q;

// 更新步（观测到达后）
K = P_pred * H.transpose() * (H * P_pred * H.transpose() + R).inverse();
x_est = x_pred + K * (z - H * x_pred);
P_est = (I - K * H) * P_pred;

6.6 模型选型指南

障碍类型	推荐模型	预测时域
行人	CV	2–5 s
车辆（道路）	CTRV / CA	3–8 s
差速机器人	DiffDrive	2–5 s
未知/静态	Stationary	—
高速机动	CA + 大过程噪声	1–3 s

6.7 单元测试

测试文件	验证内容
`linear_motion_model_test.cpp`	CV 前向传播正确性
`differential_motion_model_test.cpp`	非完整约束轨迹
`stationary_motion_model_test.cpp`	状态不变性

6.8 扩展：CTRV 实现建议

当前仓库无独立 CTRV 类，可按以下方式扩展：

class CTRVMotionModel : public MotionModelInterface<CTRVMotionModel, State5D> {
    State5D crtp_predict(const State5D& s, const std::chrono::nanoseconds& dt) const {
        double v = s.v(), theta = s.theta(), omega = s.omega();
        double dt_s = std::chrono::duration<double>(dt).count();
        if (std::abs(omega) < 1e-6) {
            // CV 退化
        } else {
            // 解析 CTRV 公式，见 03_math.md §3.3
        }
    }
};