上文介绍了车辆的运动学模型, 以及如何生成后轮行车轨迹和辅助线. 本文介绍如何生成车辆的前轮轨迹.

前轮轨迹

自行车模型中, 前轮和后轮围绕同一中心旋转, 区别是旋转半径不同. 为了计算前轮轨迹, 我们需要调整前轮角速度的计算方式:

  1. 后轮角速度 = V / R
  2. 而tan(δ) = L / R
  3. 联立(1)(2)可得: 后轮角速度 = V * tan(δ) / L

需要修改的是:

  1. 前轮角速度 = V / R’
  2. 而sin(δ) = L / R’
  3. 联立(1)(2)可得: 前轮角速度 = V * sin(δ) / L

其中R’是前轮的转弯半径.

因此, 前轮的轨迹可以通过如下公式计算:

def tractor(front_steer_angle=0.0, current_phi=float(math.pi / 2), vehicle_length=15.4, wheelbase=7.7):
    """
    :param front_steer_angle: 前轮转角
    :param current_phi: 车头朝向: 向前, 车体坐标, 摄像头位置为原点
    :param vehicle_length: 超过一个车长(15.4)米时停止计算
    :param wheelbase: 轴距
    :return: 轨迹
    """
 
    current_x = current_y = 0.0
    delta_distance = 0.1  # 每0.1米计算一个点
 
    total_distance = 0
    (x, y, phi) = ([], [], [])  # (轨迹坐标, 车体朝向)
    while total_distance < vehicle_length:
        next_x = current_x + delta_distance * math.cos(current_phi)
        next_y = current_y + delta_distance * math.sin(current_phi)
        next_phi = current_phi + delta_distance / wheelbase * math.sin(front_steer_angle)
        x.append(next_x)
        y.append(next_y)
        phi.append(next_phi)
 
        current_x = next_x
        current_y = next_y
        current_phi = next_phi
        total_distance += delta_distance
 
    return [x, y, phi]

其余和后轮轨迹生成类似.