modules/planning/planning_base/common/ego_info.cc

1.CalculateFrontObstacleClearDistance

void EgoInfo::CalculateFrontObstacleClearDistance(
    const std::vector<const Obstacle*>& obstacles) {

  //获取自车(后轴中心)在笛卡尔坐标系下的坐标
  Vec2d position(vehicle_state_.x(), vehicle_state_.y());

  const auto& param = ego_vehicle_config_.vehicle_param();


  //计算自车中心点在自车坐标系(后轴中心为原点)下坐标
  Vec2d vec_to_center(
      (param.front_edge_to_center() - param.back_edge_to_center()) / 2.0,
      (param.left_edge_to_center() - param.right_edge_to_center()) / 2.0);

  //先将自车中心点自车坐标系下的坐标转换为笛卡尔坐标系上的坐标，然后在加上自车(后轴中心)笛卡尔坐标系下的坐标，得到自车中心点在笛卡尔坐标系下的坐标
  Vec2d center(position + vec_to_center.rotate(vehicle_state_.heading()));

  Vec2d unit_vec_heading = Vec2d::CreateUnitVec2d(vehicle_state_.heading());

  // Due to the error of ego heading, only short range distance is meaningful
  static constexpr double kDistanceThreshold = 50.0;
  static constexpr double buffer = 0.1;  // in meters

  //关注的自车前方区域长度
  const double impact_region_length =
      param.length() + buffer + kDistanceThreshold;

  //关注的自车前方矩形区域
  Box2d ego_front_region(center + unit_vec_heading * kDistanceThreshold / 2.0,
                         vehicle_state_.heading(), impact_region_length,
                         param.width() + buffer);

  //跳过虚拟障碍物和与关注的前方矩形区域不重叠的障碍物
  for (const auto& obstacle : obstacles) {
    if (obstacle->IsVirtual() ||
        !ego_front_region.HasOverlap(obstacle->PerceptionBoundingBox())) {
      continue;
    }
    //计算自车与障碍物中心点间的距离并减去了自车矩形框对角线的一半
    double dist = ego_box_.center().DistanceTo(
                      obstacle->PerceptionBoundingBox().center()) -
                  ego_box_.diagonal() / 2.0;
    //获取到与关注区域重叠的最近障碍物障碍物距离
    if (front_clear_distance_ < 0.0 || dist < front_clear_distance_) {
      front_clear_distance_ = dist;
    }
  }
}

modules/map/pnc_map/pnc_map_base.cc
2.LookForwardDistance

double PncMapBase::LookForwardDistance(const double velocity) {
  //8s自车向前行驶的距离速度单位m/s
  auto forward_distance = velocity * FLAGS_look_forward_time_sec;
  //如果行驶距离大于180m,返回250m，否则返回190m
  return forward_distance > FLAGS_look_forward_short_distance
             ? FLAGS_look_forward_long_distance
             : FLAGS_look_forward_short_distance;
}

源码地址

源码解析pdf

Frenet坐标系:[$s$,$\dot{s}$,$\ddot{s}$,l,$\dot{l}$,$\ddot{l}$,${l}'$,${l}''$]

Cartesian坐标系:[$\vec{x} $,$v_{x}$,$a_{x}$,$\theta_{x}$,$k_{x}$]

 

$s$：Frenet纵坐标

$\dot{s}$=$\frac{ds}{dt} $：Frenet纵坐标对时间的导数，纵向速度

$\ddot{s}$=$\frac{\dot{ds}}{dt} $：纵向加速度

$l$：Frenet横坐标

$\dot{l}$=$\frac{dl}{dt}$：Frenet横向速度

$\ddot{l}$=$\frac{d\dot{l}}{dt}$：Frenet横向加速度

${l}'$=$\frac{dl}{ds}$：Frenet横向坐标对纵向坐标的导数

${l}''$=$\frac{\dot{dl}}{ds}$：Frenet横向坐标对纵向坐标的二阶导

$\vec{x} $：Cartesian坐标系下的一个向量

$\theta_{x}$：Cartesian坐标系下的朝向

$k_{x}$=$\frac{d{\theta_{x}}}{ds}$：曲率

$v_{x}$=$\left \| \dot{\vec{x} } \right \| _{2}$：Cartesian坐标系下的线速度

$a_{x}$=$\frac{dv_{x}}{dt}$：Cartesian坐标系下的加速度

推导过程：

笛卡尔坐标系与Frenet坐标系的转换

使用五阶多项式来表示随时间变化的位置：
$s(t)=c_{1} t^{4}+c_{2} t^{3}+c_{3} t^{2}+c_{4} t+c_{5}$

可以推导出:

速度(一阶导数):
$v(t)=\frac{d s}{d t}=4 c_{1} t^{3}+3 c_{2} t^{2}+2 c_{3} t+ c_{4}$

加速度(二阶导数)
$a(t)=\frac{d v}{d t}=12c_{1} t^{2}+6 c_{2} t+2 c_{3}$
约束条件:
1.初始条件：

在初始时刻$t_0$和终止时刻$t_1$分别设置约束：

$t_0=0$

初始位置：$s(t_0)=c_5=s_0$
初始速度：$v(t_0)=c_4=v_0$
初始加速度：$a(t_0)=2c_3=a_0$

2.终止条件约束：

终止位置：$s(t_1)=c_1t_1^4+c_2t_1^3+c_3t_1^2+c_4t_1+c_5=s_1$
终止速度：$v(t_1)=4c_1t_1^3+3c_2t_1^2+2c_3t_1+c_4=v_1$
终止加速度：$a(t_1)=12c_1t_1^2+6c_2t_1+2c_3=a_1$
 

方程组求解：

6个方程，可以解出6个系数$c_1$到$c_6$：
$c_5=s_0$（直接得出)
$c_4=v_0$   (直接得出)
$2c_3=a_0\Rightarrow c_3=\frac{a_0}{2} $
 

对于剩余的三个系数$c_1, c_2$,需要解以下矩阵方程：
$\begin{bmatrix}
  t_1^4& t_1^3\\
  4t_1^3&  3t_1^2\\
\end{bmatrix}\begin{bmatrix}
 c_1\\
 c_2\\
\end{bmatrix}=\begin{bmatrix}
 s_1-c_3t_1^2-c_4t_1-c_5\\
 v_1-2c_3t_1-c_4\\
\end{bmatrix}$


使用 LAPACK 库的 LU 分解法，解方程
python代码示例：

import numpy as np

def solve_quartic_coefficients(t1, s0, v0, a0, s1, v1, a1):
    """
    求解四次多项式 s(t) = c1*t^4 + c2*t^3 + c3*t^2 + c4*t + c5 的系数
    输入参数：
        t1: 终止时间
        s0, v0, a0: 初始位置、速度、加速度
        s1, v1, a1: 终止位置、速度、加速度
    返回：
        系数列表 [c1, c2, c3, c4, c5]
    """
    # 直接已知的系数
    c5 = s0
    c4 = v0
    c3 = a0 / 2

    # 构建矩阵 A 和向量 b
    A = np.array([
        [t1**4, t1**3],
        [4*t1**3, 3*t1**2]
    ])

    # 终止时刻的加速度方程：12*c1*t1^2 + 6*c2*t1 + 2*c3 = a1
    # 速度方程：4*c1*t1^3 + 3*c2*t1^2 + 2*c3*t1 + c4 = v1
    # 位移方程：c1*t1^4 + c2*t1^3 + c3*t1^2 + c4*t1 + c5 = s1
    b = np.array([
        s1 - c3*t1**2 - c4*t1 - c5,
        v1 - 2*c3*t1 - c4
    ])

    # 解线性方程组 Ax = b
    c1, c2 = np.linalg.solve(A, b)

    return [c1, c2, c3, c4, c5]

# 示例参数（与五次多项式示例一致）
t1 = 1.0
s0, v0, a0 = 0.0, 0.0, 0.0
s1, v1, a1 = 1.0, 0.0, 0.0

# 求解系数
coefficients = solve_quartic_coefficients(t1, s0, v0, a0, s1, v1, a1)
print("系数 c1 到 c5:", coefficients)

# 验证终止条件
t = t1
c1, c2, c3, c4, c5 = coefficients

s = c1*t**4 + c2*t**3 + c3*t**2 + c4*t + c5
v = 4*c1*t**3 + 3*c2*t**2 + 2*c3*t + c4
a = 12*c1*t**2 + 6*c2*t + 2*c3

print(f"验证 t={t}: s={s}, v={v}, a={a}")

使用五阶多项式来表示随时间变化的位置：
$s(t)=c_{1} t^{5}+c_{2} t^{4}+c_{3} t^{3}+c_{4} t^{2}+c_{5} t+c_{6}$

可以推导出:

速度(一阶导数):
$v(t)=\frac{d s}{d t}=5 c_{1} t^{4}+4 c_{2} t^{3}+3 c_{3} t^{2}+2 c_{4} t+c_{5}$

加速度(二阶导数)
$a(t)=\frac{d v}{d t}=20 c_{1} t^{3}+12 c_{2} t^{2}+6 c_{3} t+2 c_{4}$
约束条件:
1.初始条件：

在初始时刻$t_0$和终止时刻$t_1$分别设置约束：

$t_0=0$

初始位置：$s(t_0)=c_6=s_0$
初始速度：$v(t_0)=c_5=v_0$
初始加速度：$a(t_0)=2c_4=a_0$

2.终止条件约束：

终止位置：$s(t_1)=c_1t_1^5+c_2t_1^4+c_3t_1^3+c_4t_1^2+c_5t_1+c_6=s_1$
终止速度：$v(t_1)=5c_1t_1^4+4c_2t_1^3+3c_3t_1^2+2c_4t_1+c_5=v_1$
终止加速度：$a(t_1)=20c_1t_1^3+12c_2t_1^2+6c_3t_1+2c_4=a_1$
 

方程组求解：

6个方程，可以解出6个系数$c_1$到$c_6$：
$c_6=s_0$（直接得出)
$c_5=v_0$   (直接得出)
$2c_4=a_0\Rightarrow c_4=\frac{a_0}{2} $
 

对于剩余的三个系数$c_1, c_2, c_3$,需要解以下矩阵方程：
$\begin{bmatrix}
  t_1^5& t_1^4& t_1^3\\
  5t_1^4&  4t_1^3& 3t_1^2\\
  20t_1^3&  12t_1^2& 6t_1
\end{bmatrix}\begin{bmatrix}
 c_1\\
 c_2\\
 c_3
\end{bmatrix}=\begin{bmatrix}
 s_1-c_4t_1^2-c_5t_1-c_6\\
 v_1-2c_4t_1-c_5\\
 a_1-2c_4
\end{bmatrix}$


使用 LAPACK 库的 LU 分解法，解方程
python代码示例：

import numpy as np

def solve_quintic_coefficients(t1, s0, v0, a0, s1, v1, a1):
    """
    求解五阶多项式 s(t) = c1*t^5 + c2*t^4 + c3*t^3 + c4*t^2 + c5*t + c6 的系数
    输入参数：
        t1: 终止时间
        s0, v0, a0: 初始位置、速度、加速度
        s1, v1, a1: 终止位置、速度、加速度
    返回：
        系数列表 [c1, c2, c3, c4, c5, c6]
    """
    # 直接已知的系数
    c6 = s0
    c5 = v0
    c4 = a0 / 2

    # 构建矩阵 A 和向量 b
    A = np.array([
        [t1**5,      t1**4,      t1**3],
        [5*t1**4,    4*t1**3,    3*t1**2],
        [20*t1**3,   12*t1**2,   6*t1]
    ])

    b = np.array([
        s1 - c4*t1**2 - c5*t1 - c6,
        v1 - 2*c4*t1 - c5,
        a1 - 2*c4
    ])

    # 解线性方程组 Ax = b
    c1, c2, c3 = np.linalg.solve(A, b)

    return [c1, c2, c3, c4, c5, c6]

# 示例参数（与之前数学示例一致）
t1 = 1.0
s0, v0, a0 = 0.0, 0.0, 0.0
s1, v1, a1 = 1.0, 0.0, 0.0

# 求解系数
coefficients = solve_quintic_coefficients(t1, s0, v0, a0, s1, v1, a1)
print("系数 c1 到 c6:", coefficients)

# 验证终止条件
t = t1
c1, c2, c3, c4, c5, c6 = coefficients

s = c1*t**5 + c2*t**4 + c3*t**3 + c4*t**2 + c5*t + c6
v = 5*c1*t**4 + 4*c2*t**3 + 3*c3*t**2 + 2*c4*t + c5
a = 20*c1*t**3 + 12*c2*t**2 + 6*c3*t + 2*c4

print(f"验证 t={t}: s={s}, v={v}, a={a}")

http://hardpurus.site/usr/image/sim.png

http://hardpurus.site/usr/image/eigenvalue_eigenvector.png

真因数:除了x本身，x的所有正因数 质数:是指在大于1的自然数中，除了1和它本身以外不再有其他因数的自然数。 质数的平方恰好有两个真因数

CyberRt时序图
http://hardpurus.site/usr/image/cyberrt.svg