自动驾驶—车辆运动学模型推导及Matlab验证

一、车辆运动学模型概述

运动学是从几何学的角度研究物体的运动规律，包括物体在空间的位置、速度等随时间而产生的变化，因此，车辆运动学模型应该能反映车辆位置、速度、加速度等与时间的关系。在车辆轨迹规划过程中应用运动学模型，可以使规划出的轨迹更切合实际，满足行驶过程中的运动学几何约束，且基于运动学模型设计出的控制器也能具有更可靠的控制性能。

《Vehicle Dynamics and Control》书中，将机体坐标系原点设置在质心位置，假设前后轮都存在偏角，推导了一般方程。

《无人驾驶车辆模型预测控制》书中，将机体坐标系原点设置在后轴中心，假设只存在前轮偏角，推导了以后轴为原点的运动学模型。

建模方式不同，机体坐标系原点选择不同，前驱/后驱和前轮/后轮转向都会导致运动学模型结果不一致。复杂的模型并不一定是最好的选择，对车辆运动学模型进行合理简化并且选择行使工况的约束条件才是研究的重点，能够表达控制对象的真实物理限制即可。

根据阿克曼转向几何（阿克曼转向几何(Ackerman Turning Geometry)是一种为了解决交通工具转弯时，内外转向轮路径指向的圆心不同的几何学）设计的汽车，可以认为左前轮和右前轮的偏角为同一角度，且瞬时转向中心相交于一点。根据这个原理，可以将四轮模型简化为两轮（单车）模型。

二、以后轴中心为原点的车辆运动学模型推导

将机体坐标系原点定义在后轴中心，平面运动存在三个自由度，即三个状态量：

State=[X,\ Y,\Psi] \\ 实际控制的话是两个输入, v_r 是后轮速度，由油门刹车控制； \delta_f 是前轮偏角，由方向盘转角控制：

Input=[v_r,\delta_f] \\ 运动学模型就是建立状态量和输入的关系。

首先分析机体坐标系上的状态量，前轮速度 v_f ，后轮速度 v_r 。假设只存在车轮滚动，所以后轮速度只存在于机体坐标系x方向。在不考虑轮胎力的前提下，前轮速度 v_f 只存在前路偏角 \delta_f 方向上。（由于两个假设过于理想，所以仅使用运动学约束的控制策略局限较大）。将两个速度分解，沿机体坐标系x轴方向：

v_{rx}=v_r \\ v_{fx}=v_f*cosδ_f y轴方向：

v_{ry}=0\\v_{fy}=v_f*sinδ_f

因为假设机体是刚体模型，所以x轴方向不会发生形变,前后轮沿机体坐标系x轴方向速度相同：

v_{rx}=v_{fx}\\ 而前轮在y轴方向有速度，所以模型存在平面转动，前轮转动的角速度数值上等于切线速度和轴距的比值：

\omega_f=\frac{v_{fy}}{L}=\frac{v_rtan\delta_f}{L}\\ 接着，建立机体坐标系oxy和大地坐标系OXY的关系，其实就是坐标系的转换。将后轮速度 v_r 分解到大地坐标系：

\dot{X}=v_rcosψ\\\dot{Y}=v_rsinψ

因为整个模型是刚体，刚体对于任意基点的角速度都是相同的，即大地坐标系下的 \dot{\psi} 和机体上各点的角速度是相同的：

\dot{\psi}=\omega_f=\frac{v_rtan\delta_f}{L}\\ 总结一下最后的运动学模型：

\dot{X}=v_rcosψ\\\dot{Y}=v_rsinψ\\\dot{ψ}=\frac{v_rtan\delta_f}{L}

个人认为这是最单纯的运动学模型，但是大部分参考资料中，都会在运动学模型中引入车辆瞬时转向半径、道路曲率半径，这里也接着推导。假设转向过程中车辆质心侧偏角保持不变，则车辆瞬时转向半径R与道路曲率半径相同。分别做前轮速度和后轮速度的垂线，相交于P点，P点即为瞬时转向中心。根据三角形角度互补关系，可以得到两个垂线的夹角等于前轮偏角 \delta_f ，进一步可建立三角函数关系：

\delta_f=arctan(L/R)\\ 最后，建立了前轮偏角和道路曲率半径、轴距的几何关系,纯跟踪算法(pure pursuit)以及Apollo横向控制中的曲率半径前馈等策略也是根据这种纯几何关系设计的。

三、matlab实现

function vehicle_state_new = kinematic_state(vehicle_state, x, dt)
    
    % 状态量
    X = vehicle_state(1,1);      % 水平位移 m 大地坐标系 
    Y = vehicle_state(1,2);      % 垂直位移 m 大地坐标系
    Psi = vehicle_state(1,3);    % 航向角 rad 大地坐标系
    % 输入量
    delta = x(1,1);              % steering wheel
    v_r = x(1,2);                % pedal 
    % 车辆参数
    len_wheel = 2.7;             % 车辆轴距 m    
    
    % 状态更新
    Psi_new = Psi + v_r * tan(delta) / len_wheel;
    X_new = X + v_r * cos(Psi_new) * dt;
    Y_new = Y + v_r * sin(Psi_new) * dt;

    vehicle_state_new = [X_new, Y_new, Psi_new];

end

运动学模型输出为大地坐标系下位移的变化率（速度），航向角的变化率（航向角速度），输入为前轮偏角和后轮速度，严格按照公式编写状态更新函数。函数验证时给定恒定速度和正弦变化的前轮偏角，验证结果如下：

四、小结

1. 根据实际场景选择对应的模型：建模方式、机体坐标系原点选择、前驱/后驱、前轮/后轮转向等前提都会导致运动学模型结果不一致。复杂的模型并不一定是最好的选择，对车辆运动学模型进行合理简化并且选择行使工况的约束条件才是研究的重点，能够表达控制对象的真实物理限制即可。推导过程主要是机体坐标系和大地坐标系的转换，机体坐标系原点不管设置在质心还是前/后轴中心，做好坐标系的角度变换即可完成推导。
2. 运动学模型就是建立状态量（水平速度、航向角速度）和输入量(后轮速度、前轮倾角)的关系，进而根据数学模型设计控制器。但是运动学模型基于诸多假设，忽略力和力矩的平衡，忽略执行机构特性，所以只适用于部分场景。在实际操作时，如果发现控制效果较差，可以关注哪一项的假设与实际场景差别较大。