无人车的轨迹优化，都是通过建立优化问题，是非线性规划问题，通过IPopt来求解，

1. 这个过程有个重要的前提，那就是f(x)是连续可导。如果f(x)这个函数不可导，或者不可微呢？比如说，在SLAM中需要处理姿态估计的问题。而姿态是通过旋转矩阵和平移向量表示的，这显然是不连续的。但是，由于旋转矩阵是一个李群，那么我们可以将其映射到其李代数上。而且，李代数是可以求导的。所以我们就还可以利用非线性优化的方法对姿态进行估计。

2. 上面的解法求得的只是局部最优解，不一定是全局最优解。因为它只看到下一时刻，在邻域内的最小值，很容易陷入到局部最优，或者说求出的最优值是跟初始位置相关的。即使初始值在同一个位置，也很有可能落入到两个不同的局部最优值，也就是说这个解是很不稳定的。如何解决这个问题，至今仍然存在很大的挑战。

Apollo高精地图给出的道路中心线的平滑性往往都不能满足规划模块的需求，因此规划中是不能拿来直接用的，需要对其进行平滑操作。Apollo目前有三种平滑算法如下，默认为离散点平滑。

离散点平滑三种求解方式，分别是利用不同求解器。如果考虑参考线的曲率约束，其优化问题是非线性的，可以使用ipopt非线性求解器求解（内点法），也可以将曲率约束线性化后使用osqp求解器来用SQP方法求解；如果不考虑曲率约束，则直接用osqp求解二次规划问题。目前Apollo 6.0中默认的参数为不考虑曲率约束-

获取 anchor_s

设置中间点 anchor points

从原始的reference_line来选取中间点，即根据reference_line的anchor_s值进行采样，采样的间隔为0.25m。采样完毕后，就能得到一系列的ref_points，而每个anchor_point就包含了一个ref_point以及横纵向的裕度。

横纵向裕度的默认参数为：

longitudinal_boundary_bound : 2.0

max_lateral_boundary_bound : 0.5

min_lateral_boundary_bound : 0.1

然后再根据自车宽度，车道边界类型（是否为curb）等再对横向裕度进行收缩。AnchorPoint结构中的enforced变量表示该点是否为强约束，在参考线平滑中只有首尾两点为强约束。

计算完anchor points后，将其赋值到smoother对象中。

// 省略不重要部分
class ReferencePoint : public hdmap::MapPathPoint
{
     public:
      common::PathPoint ToPathPoint(double s) const;

      double kappa() const;
      double dkappa() const;

      static void RemoveDuplicates(std::vector<ReferencePoint>* points);

     private:
      double kappa_ = 0.0;
      double dkappa_ = 0.0;
};

struct LaneWaypoint {
  LaneWaypoint() = default;
  LaneWaypoint(LaneInfoConstPtr lane, const double s)
      : lane(CHECK_NOTNULL(lane)), s(s) {}
  LaneWaypoint(LaneInfoConstPtr lane, const double s, const double l)
      : lane(CHECK_NOTNULL(lane)), s(s), l(l) {}

  LaneInfoConstPtr lane = nullptr;
  double s = 0.0;
  double l = 0.0;

  std::string DebugString() const;
};

// lane_info_ptr->accumulate_s(); 的来源
for (size_t i = 0; i + 1 < points_.size(); ++i)
{
    segments_.emplace_back(points_[i], points_[i + 1]);
    accumulated_s_.push_back(s);
    unit_directions_.push_back(segments_.back().unit_direction());
    s += segments_.back().length();
}