Apollo5.5规划代码分析（1）_pointsmath

modules目录下的子目录

├─ calibration// 标定模块，现在已经实现自动标定的功能
├─ canbus // can总线相关的模块
├─common // 公共源码模块如日志，工厂模式的实现，日志系统，监控模块，数学算法等
├── control // 幅度和度的相互转换
├── data // 数据的收集，存储和处理
├── dreamview // 可视化模块，查看规划的轨迹及实时的转向刹车油门信息
├── drivers // GNSS设备驱动，包括NovAtel, Applanix, u-blox， velodyne驱动
├── e2e // 端到端模块
├── elo // 利用高精地图的自定位模块
├── guardian // 监护进程，主要是为了安全
├── localization // 定位模块，输入GPS和IMU信息输出自车定位信息
├── map // 高精地图模块，输出结构化地图信息，如车道线，十字路口等
├── monitor // 监控模块,监控硬件状态，同时把状态发给hmi
├── perception // 感知模块，输入激光点云，高精地图，变换坐标
├── planning // 决策规划模块
├── prediction // 预测模块，输出感知的障碍物信息及自定位信息输出障碍物未来的轨迹
├── routing // 全局导航模块，输入包括地图信息各起点终点地址，输出一个全局的导航信息
├── third_party_perception // 车辆姿态模拟线程
└── tools // 第三方的感知模块

https://www.cnblogs.com/longjiang-uestc/p/10020729.html

message PathPoint {
  // coordinates
  optional double x = 1;
  optional double y = 2;
  optional double z = 3;

  // direction on the x-y plane
  optional double theta = 4;
  // curvature on the x-y planning
  optional double kappa = 5;
  // accumulated distance from beginning of the path
  optional double s = 6;

  // derivative of kappa w.r.t s.
  optional double dkappa = 7;
  // derivative of derivative of kappa w.r.t s.
  optional double ddkappa = 8;
  // The lane ID where the path point is on
  optional string lane_id = 9;

  // derivative of x and y w.r.t parametric parameter t in CosThetareferenceline
  optional double x_derivative = 10;
  optional double y_derivative = 11;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

curve_math.h

计算曲率和曲率微分

// kappa = (dx * d2y - dy * d2x) / [(dx * dx + dy * dy)^(3/2)]
double CurveMath::ComputeCurvature(const double dx, const double d2x,
                                   const double dy, const double d2y) {
  const double a = dx * d2y - dy * d2x;
  auto norm_square = dx * dx + dy * dy;
  auto norm = std::sqrt(norm_square);
  const double b = norm * norm_square;
  return a / b;
}

double CurveMath::ComputeCurvatureDerivative(const double dx, const double d2x,
                                             const double d3x, const double dy,
                                             const double d2y,
                                             const double d3y) {
  const double a = dx * d2y - dy * d2x;
  const double b = dx * d3y - dy * d3x;
  const double c = dx * d2x + dy * d2y;
  const double d = dx * dx + dy * dy;

  return (b * d - 3.0 * a * c) / (d * d * d);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

discrete_points_math.h

成员函数作用：输入点x，y坐标，输出headings（角度）、accumulated_s（里程）、kappas（曲率）、dkappas（曲率微分）

#pragma once

#include <utility>
#include <vector>

namespace apollo {
namespace planning {

class DiscretePointsMath {
 public:
  DiscretePointsMath() = delete;

  static bool ComputePathProfile(
      const std::vector<std::pair<double, double>>& xy_points,
      std::vector<double>* headings, std::vector<double>* accumulated_s,
      std::vector<double>* kappas, std::vector<double>* dkappas);
};

}  // namespace planning
}  // namespace apollo
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

polynomial_xd.h

这个头文件不知道在干什么，有点难懂

#pragma once

#include <cinttypes>
#include <vector>

namespace apollo {
namespace planning {

class PolynomialXd {
 public:
  PolynomialXd() = default;
  explicit PolynomialXd(const std::uint32_t order);
  explicit PolynomialXd(const std::vector<double>& params);
  double operator()(const double value) const;
  double operator[](const std::uint32_t index) const;
  void SetParams(const std::vector<double>& params);

  static PolynomialXd DerivedFrom(const PolynomialXd& base);
  static PolynomialXd IntegratedFrom(const PolynomialXd& base,
                                     const double intercept = 0.0);

  std::uint32_t order() const;
  const std::vector<double>& params() const;

 private:
  std::vector<double> params_;
};

}  // namespace planning
}  // namespace apollo
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30