3. 运动学与动力学建模

轨迹重规划与优化的第一步，是把“车或机器人到底怎么动”写成可计算的数学对象。若模型选错，后续优化器即使收敛，也可能给出物理不可行、控制不可跟踪的轨迹。本节围绕刚体运动表示、车辆/机器人模型、微分平坦与约束建模四条主线，建立从几何位姿到动力学约束的完整建模链路。

一、刚体运动表示：位姿、旋转与速度

平面移动机器人常用位姿 q = (x, y, θ) 描述，其中 (x, y) 为参考点位置，θ 为航向角。三维空间则用 SE(3) 表示：旋转矩阵 R ∈ SO(3) 与平移向量 p ∈ R³ 组成齐次变换矩阵。旋转的三种常用表示各有取舍：欧拉角直观但有万向节死锁，旋转矩阵无奇异但冗余且需正交约束，四元数紧凑且插值平滑，适合姿态优化。

速度层面需区分参考点速度与刚体角速度。对平面刚体，若参考点选在后轴中心，则侧向速度近似为零，这直接导出自行车模型的非完整约束。建模时务必明确：位姿定义在哪个坐标系、速度是体坐标系还是世界坐标系、航向角以哪条轴为基准。这三项不统一，是轨迹跟踪误差最常见的来源。

二、车辆与机器人模型：从运动学到动力学

常用模型按保真度递增可分为四类：质点模型、自行车运动学模型、自行车动力学模型、以及带轮胎侧偏的完整车辆模型。运动学自行车模型假设无侧滑，状态为 (x, y, θ, v)，控制为前轮转角 δ 与加速度 a：

x_dot   = v * cos(theta)
y_dot   = v * sin(theta)
theta_dot = v / L * tan(delta)
v_dot   = a

其中 L 为轴距。该模型适合低速泊车、园区物流机器人。当速度升高，轮胎侧偏力不可忽略，需引入动力学模型，状态扩展为 (x, y, θ, v, β, ω)，其中 β 为质心侧偏角，ω 为横摆角速度，控制量变为前轮转角与纵向力。差速驱动机器人则用两轮角速度 (ω_L, ω_R) 直接驱动，模型更简单但同样受非完整约束限制。

三、微分平坦：把轨迹生成降维

微分平坦是轨迹优化中的“降维利器”。若系统存在一组平坦输出 z，使得所有状态与控制都可由 z 及其有限阶导数代数表示，则轨迹规划只需在平坦输出空间进行。对平面自行车运动学模型，取 z = (x, y) 为平坦输出，可推导出：

theta = atan2(y_dot, x_dot)
v     = sqrt(x_dot^2 + y_dot^2)
delta = atan(L * (x_dot*y_ddot - y_dot*x_ddot) / (x_dot^2 + y_dot^2)^(3/2))

这意味着：只要给出一条光滑的 (x(t), y(t)) 曲线，就能解析地反算出航向、速度和前轮转角，无需在优化中显式处理非完整约束。工程上常把轨迹参数化为五次或七次多项式、B 样条或贝塞尔曲线，保证 C² 以上连续，从而得到连续可导的曲率与转角。微分平坦的代价是：它要求模型精确，且平坦输出需满足边界条件；对含侧滑的动力学模型，平坦性往往不再成立，需退回数值优化。

四、约束建模：把物理边界写进优化

约束建模决定优化问题的可行域。常见约束分为四类：

• 运动学约束：非完整约束 x_dot·sinθ − y_dot·cosθ = 0，以及转角极限 |δ| ≤ δ_max。
• 动力学约束：加速度、横摆角速度、轮胎附着椭圆 (a_x/a_max)² + (a_y/a_max)² ≤ 1。
• 几何约束：车身轮廓与障碍物不碰撞，常用圆、矩形或多边形近似，配合有符号距离场。
• 舒适性与执行器约束：加加速度 jerk 上限、转角速率上限、速度区间。

约束的数学形式直接影响求解器选择：线性约束可用二次规划（QP），非线性约束需序列二次规划（SQP）或内点法。工程中常把曲率约束转化为对控制量的线性约束，把碰撞约束用软约束加惩罚项处理，以提升数值稳定性。

五、案例与复盘：一个换道场景的建模选择

场景：城市快速路 20 m/s 换道避障。若用运动学模型，优化器会给出曲率突变轨迹，实车跟踪时横向误差超过 0.5 m；改用动力学模型并加入轮胎侧偏约束后，轨迹曲率更平缓，但求解时间从 8 ms 升至 35 ms。复盘结论有三点：第一，模型保真度要与速度区间匹配；第二，微分平坦适合生成初值，再用动力学模型做局部精修；第三，约束建模宁可用软约束加惩罚，也不要因硬约束过强导致优化不可行。

综上，建模链路可归纳为：先用刚体运动表示统一位姿与速度语义，再按速度区间选择车辆/机器人模型，利用微分平坦把轨迹生成降维到平坦输出空间，最后用四类约束把物理边界写进优化问题。模型、平坦性与约束三者匹配，轨迹重规划才能在精度与实时性之间取得平衡。