自动驾驶基础一车辆模型

1. 模型概述：
   • 自行车动力学模型通常用于研究自行车在骑行过程中的行为，如稳定性、操控性和速度等。
   • 模型可以基于不同的简化假设和复杂度，从简单的二维模型到复杂的三维模型，甚至包括骑行者的动态。
2. 力学方程：
   • 基础物理学方程F=ma（力 = 质量×加速度）被应用于自行车运动。在恒定速度下，这简化为FP=FR，其中FP是推进力，FR是所有阻力的总和。
   • 阻力包括重力阻力（在斜坡上骑行时）、向前运动的空气阻力、滚动阻力、车轮轴承阻力和车轮转动的空气阻力等。
3. 动力学建模方法：
   • 自行车动力学模型可以使用不同的数学工具和方法来建立，如牛顿-欧拉方法、拉格朗日方程等。
   • 拉格朗日方程以能量和功作为力学基本量，特别适用于受约束系统的动力学问题。对于自行车一人体系统，由于刚体数目较少，经典的拉格朗日方程是一个合适的选择。
4. 空气动力学应用：
   • 空气动力学在自行车设计中非常重要，特别是在降低风阻和提高稳定性方面。
   • 通过优化车架结构、改变车手体位和车轮形状等措施，可以减少空气阻力的影响，提高自行车的速度和效率。
5. 模型参数：
   • 模型参数可以包括自行车的质量、车轮半径、转动惯量、阻力系数等。
   • 骑行者的体重、身高、骑行技能等也会影响模型的结果。
6. 动力学模型中，有几个重要的参数需要考虑

3. 算法流程

1. 质量参数：
   • 自行车和骑手的总质量（m）：这个参数影响重力阻力和其他阻力的计算。例如，在重力阻力的公式 Fg = mgsin(arctan(G)) 中，m 代表了自行车和骑手的总质量。
2. 阻力系数：
   • 空气阻力系数（Cd）：在向前运动的空气阻力 FA = 0.5 * Cd * A * ρ * vA^2 中，Cd 是阻力系数，代表了自行车和骑手形状的空气动力学特性。
   • 滚动阻力系数：这通常与轮胎材料和路面条件有关，影响滚动阻力 FR 的大小。
3. 几何尺寸：
   • 自行车和骑手的正面面积（A）：用于计算空气阻力。
   • 自行车长度（lf 和 lr）：质心到前后轮中心的距离，这些参数在车辆动态方程中用于计算轮胎的侧向力。
4. 物理常数：
   • 重力加速度（g）：通常取 9.81 m/s^2，用于计算重力阻力。
   • 空气密度（ρ）：影响空气阻力的计算，通常根据环境条件（如海拔、温度等）进行调整。
5. 运动学参数：
   • 期望速度（v_desired）：个体自由行驶时的速度，这反映了骑行者的意图和期望。
   • 反应系数：包括横向和纵向两类，描述了个体对于横向和纵向个人空间的需求程度。
   • 动作持续时间和避让力强度：这些参数描述了骑行者在面对障碍物或需要改变行驶方向时的反应。
6. 环境参数：
   • 坡度（G）：影响重力阻力的计算，陡峭的坡度会对骑行者的努力产生更大的需求。
   • 路面条件：这会影响滚动阻力和轮胎的侧向力。
7. 传输系统效率（η）：
   • 在将骑手的输入功率（P_in）转换为推进功率时，需要考虑传输系统的效率。这反映了自行车机械部件（如链条、齿轮等）的摩擦损失。
8. 车辆动态参数：
   • 车辆的动态方程可能会包含其他参数，如车辆的转动惯量、车轮的半径和宽度等，这些参数在更复杂的模型中可能会考虑。
9. 自行车尺寸参数：
   • 轴距（Wheelbase）：前后轮轴之间的距离，影响车辆的稳定性和操控性。
   • 车轮半径（Wheel radius）：用于计算车辆的滚动速度和角速度。
   • 自行车高度（Bicycle height）：从地面到自行车顶部的距离，可能影响空气动力学特性。
10. 自行车惯性参数：
    • 自行车质量分布（Mass distribution）：质心位置，影响车辆的操控稳定性和侧翻阈值。
    • 转动惯量（Moments of inertia）：车辆绕不同轴的转动惯量，影响车辆的旋转动态。
11. 轮胎参数：
    • 轮胎侧偏刚度（Cornering stiffness）：描述轮胎在侧向力作用下的抵抗变形能力，影响车辆的操控性和稳定性。
    • 轮胎滚动摩擦系数（Rolling friction coefficient）：影响滚动阻力和车辆的滑行特性。
12. 骑行者参数：
    • 骑行者体重（Rider weight）：影响车辆的总质量和重力阻力。
    • 骑行者身高（Rider height）：可能用于计算骑行者的舒适骑行姿势和空气动力学特性。
    • 骑行者骑行技能（Riding skill）：尽管这不是一个物理参数，但骑行者的技能水平会显著影响车辆的实际动态行为。
13. 操控参数：
    • 方向盘/手柄转角（Steering angle）：用于控制车辆的转向。
    • 踏板力/功率（Pedal force/power）：用于控制车辆的速度和加速度。
14. 外部干扰参数：
    • 风速和风向（Wind speed and direction）：影响车辆的空气动力学特性和稳定性。
    • 路面不平度（Road roughness）：影响车辆的振动和操控稳定性。
15. 控制策略参数（如果模型包含控制策略）：
    • 控制增益（Control gains）：用于调整控制器对车辆动态行为的影响程度。
    • 控制延迟（Control delay）：反映实际控制系统中的响应时间延迟。
16. 车辆状态参数：
    • 纵向速度（Longitudinal velocity）：车辆前进的速度。
    • 横向速度（Lateral velocity）：车辆侧向移动的速度，通常较小但在曲线行驶时很重要。
    • 偏航角（Yaw angle）：车辆前进方向与绝对坐标系的夹角。
    • 偏航率（Yaw rate）：车辆偏航角的变化率，即车辆的转向速度。

17. 1. 车辆动力学模型

    车辆动力学模型是描述汽车运动规律的微分方程，它通常基于牛顿第二定律（F=ma）来推导。在这个模型中，车辆的运动受到多种力的影响，包括驱动力、制动力、阻力和转弯力等。

    2. 关键参数和公式

18. 质量（m）：车辆的质量，影响加速度和动能的计算。
19. 速度（v）：车辆的速度，可以分解为纵向速度（v_x）和横向速度（v_y）。
20. 加速度（a）：车辆的加速度，描述了速度的变化率。
21. 扭矩（T）：发动机或电动机产生的扭矩，与转速（n）和马力（P）的关系为 T = P × 5252 / n。
22. 马力（P）：发动机或电动机的功率，与扭矩和转速的关系为 P = T × n / 5252。
23. 动能（KE）：KE = 0.5 × m × v^2，描述了车辆由于运动而具有的能量。
24. 动量（p）：p = m × v，描述了车辆的质量和速度的乘积。
25. 刹车距离（d）：d = (初速度 - 终速度)^2 / (2 × 刹车减速度)，用于估算车辆的制动距离。
26. 车辆建模：根据车辆的结构和参数，建立车辆动力学模型。
27. 数据采集：通过车辆传感器和控制单元实时采集车速、油门位置、制动状态等数据。
28. 目标优化：根据驾驶情况和用户需求，确定优化目标，如最小化燃油消耗、最大化加速性能或最小化排放。
29. 控制策略：基于动力系统模型和优化目标，采用先进的控制策略来调整引擎功率、电机输出、换挡时机等。
30. 实时优化：在车辆运行过程中，根据实时采集的数据和控制策略，不断优化车辆的动力性能。

31. RK4（Runge-Kutta 4阶方法）是一种用于解决常微分方程的数值方法，以下是对其的详细解释：

32. 定义与原理：
    • RK4是一种高精度的算法，用于求解微分方程的数值解。
    • 它是一种迭代方法，通过逐步逼近精确解来得到数值解。
    • RK4方法基于泰勒级数展开的思想，通过在差商区间内取多点斜率加权平均代替导数方法来提高精度。
33. 算法步骤：
    • 假设要求解初值问题：(y' = f(t, y), y(t_0) = y_0)。
    • RK4的迭代公式为：
      (y_{n+1} = y_n + \frac{h}{6}(k_1 + 2k_2 + 2k_3 + k_4))
      其中，(h) 是仿真步长，满足 (h < \text{某个给定值})，而 (k_1, k_2, k_3, k_4) 是基于当前函数值和导数值的加权斜率。
    • (k_1, k_2, k_3, k_4) 的计算方式如下：
      • (k_1 = f(t_n, y_n))
      • (k_2 = f(t_n + \frac{h}{2}, y_n + \frac{h}{2}k_1))
      • (k_3 = f(t_n + \frac{h}{2}, y_n + \frac{h}{2}k_2))
      • (k_4 = f(t_n + h, y_n + hk_3))

34. def rk4(func: Callable, state: np.ndarray, dt: float = 0.01, t: float = 0, **kwargs):
        """
        single-step fourth-order numerical integration (RK4) method
        func: system of first order ODEs
        state: current state vector [y1, y2, y3, ...]
        dt: discrete time step size
        t: current time
        **kwargs: additional parameters for ODE system
        returns: y evaluated at time k+1
        """
    
        # evaluate derivative at several stages within time interval
        f1 = func(t, state, **kwargs)
        f2 = func(t + dt / 2, state + (f1 * (dt / 2)), **kwargs)
        f3 = func(t + dt / 2, state + (f2 * (dt / 2)), **kwargs)
        f4 = func(t + dt, state + (f3 * dt), **kwargs)
        return state + (dt / 6) * (f1 + (2 * f2) + (2 * f3) + f4)
    
    
    class BicycleVehicle(Vehicle):
        """
        A dynamical bicycle model, with tire friction and slipping.
    
        See Chapter 2 of Lateral Vehicle Dynamics. Vehicle Dynamics and Control. Rajamani, R. (2011)
        """
    
        MASS: float = 1  # [kg]
        LENGTH_A: float = Vehicle.LENGTH / 2  # [m]
        LENGTH_B: float = Vehicle.LENGTH / 2  # [m]
        INERTIA_Z: float = (
            1 / 12 * MASS * (Vehicle.LENGTH**2 + Vehicle.WIDTH**2)
        )  # [kg.m2]
        FRICTION_FRONT: float = 15.0 * MASS  # [N]
        FRICTION_REAR: float = 15.0 * MASS  # [N]
    
        MAX_ANGULAR_SPEED: float = 2 * np.pi  # [rad/s]
        MAX_SPEED: float = 15  # [m/s]
    
        def __init__(
            self, road: Road, position: Vector, heading: float = 0, speed: float = 0
        ) -> None:
            super().__init__(road, position, heading, speed)
            self.lateral_speed = 0
            self.yaw_rate = 0
            self.theta = None
            self.A_lat, self.B_lat = self.lateral_lpv_dynamics()