Udacity 6.udacity_highway02

udacity_highway02

高速上的自动驾驶2

需要给路径持续的添加新的点

vector next_x_vals;
vector next_y_vals;

double pos_x;
double pos_y;
double angle;
int path_size = previous_path_x.size();//上一条路径的个数

for (int i = 0; i < path_size; ++i) {
next_x_vals.push_back(previous_path_x[i]);
next_y_vals.push_back(previous_path_y[i]);
}

if (path_size == 0) {//如果之前的路径不存在，代表刚刚启动，将
//汽车初始点作为路径的起点
pos_x = car_x;
pos_y = car_y;
angle = deg2rad(car_yaw);//角的单位从度转换为弧度
} else {//否则将之前路径的最后个点作为起始点
pos_x = previous_path_x[path_size-1];
pos_y = previous_path_y[path_size-1];
//最后的倒数第二个点作为第二个点
double pos_x2 = previous_path_x[path_size-2];
double pos_y2 = previous_path_y[path_size-2];
angle = atan2(pos_y-pos_y2,pos_x-pos_x2);
//两个点
}

double dist_inc = 0.5;
for (int i = 0; i < 50-path_size; ++i) {//不足50个，需要再添加50个坐标进去
next_x_vals.push_back(pos_x+(dist_inc) cos(angle+(i+1) (pi()/100)));
next_y_vals.push_back(pos_y+(dist_inc) sin(angle+(i+1) (pi()/100)));
pos_x += (dist_inc) cos(angle+(i+1) (pi()/100));
pos_y += (dist_inc) sin(angle+(i+1) (pi()/100));
}

msgJson["next_x"] = next_x_vals;
msgJson["next_y"] = next_y_vals;

仿真循环

坐标系是以自身为坐标，车头朝向为x，左侧为y。

仿真器的循环是每20ms(一秒50帧)。所写程序至少能提供一个20ms之后的路径。
这意味着当涉及更高的延迟时，使用先前的路径数据变得更加重要。
只要新路径包含之前路径的足够长度，过渡仍将是平滑的。

高速地图

784.6001 1135.571 0 -0.02359831 -0.9997216
815.2679 1134.93 30.6744785308838 -0.01099479 -0.9999396
844.6398 1134.911 60.0463714599609 -0.002048373 -0.9999979
875.0436 1134.808 90.4504146575928 -0.001847863 -0.9999983
905.283 1134.799 120.689735412598 0.004131136 -0.9999915

高速地图的坐标信息存储在csv文件里，从左到右分别是：x,y,s,vector d

这个(x,y)是全球地图坐标，s是Frenet坐标，d是中线的法向向量，行驶方向的右手侧，模是1. 这个坐标给的是地图六个车道(双向车道，各三条)的双黄线中间的坐标。然后每个车道是4m, 所以如果想得到行驶方向最左侧车道的坐标，即 (x，y)需要加上，2乘以向量d. 如果是中间车道(行驶方向),则是(x，y)加上6(2+4)乘以法向向量d。

需要完成坐标之间转换的代码，从frenet到（x,y）

插值法

如果想要获得平滑的，或者估计在两点之间的坐标，就需要用插值法。

插值法，可以用贝塞尔去拟合，或者用样条插值（spline fititng） 可以生成曲线过所有的点。

在c++里可以使用 简单的，便捷的https://kluge.in-chemnitz.de/opensource/spline/.
spline tool. 只需要一个简单的头文件。

使用这个可以达到 用 Cubic C^2 twice continuously differentiable或者 C^1 Hermite once continuously differentiable插值 或者 C^1 monotonic插值.

头文件：

https://kluge.in-chemnitz.de/opensource/spline/spline.h

代码：

#include <cstdio>
#include <vector>
#include "spline.h"

int main(int, char**) {
   std::vector<double> X = {0.1, 0.4, 1.2, 1.8, 2.0}; // must be increasing
   std::vector<double> Y = {0.1, 0.7, 0.6, 1.1, 0.9};

   tk::spline s(X,Y);
   double x=1.5, y=s(x), deriv=s.deriv(1,x);

   printf("spline at %f is %f with derivative %f\n", x, y, deriv);
}

输出：spline at 1.500000 is 0.915345 with derivative 1.223601