好的确定车和路之间的关系，我们通常将车辆的在大地坐标坐标转化为车辆和道路之间的frenet坐标。
可能有人会疑问为什么转换后就方便了呢？我们来看一个例子。
在大地坐标下：
无人车首先要知道红色车的位置。通过传感器得到目标在车辆坐标系下的坐标，车辆的笛卡尔坐标系下坐标可以由惯导得到，可以推出目标在笛卡尔坐标下的位置信息，然后再和道路坐标比较，判断红色车辆在哪条车道内。
在frenet坐标下：
可以看出在frenet坐标下，车相对于道路的位置信息更加清楚。
给出笛卡尔坐标和frenet坐标相互转换的代码：
vector<double> getFrenet(double x, double y, double theta, vector<double> maps_x, vector<double> maps_y, vector<double> maps_s) { int next_wp = NextWaypoint(x,y, theta, maps_x,maps_y); int prev_wp; prev_wp = next_wp-1; if(next_wp == 0) { prev_wp = maps_x.size()-1; } double n_x = maps_x[next_wp]-maps_x[prev_wp]; double n_y = maps_y[next_wp]-maps_y[prev_wp]; double x_x = x - maps_x[prev_wp]; double x_y = y - maps_y[prev_wp]; // find the projection of x onto n double proj_norm = (x_x*n_x+x_y*n_y)/(n_x*n_x+n_y*n_y); double proj_x = proj_norm*n_x; double proj_y = proj_norm*n_y; double frenet_d = distance(x_x,x_y,proj_x,proj_y); //see if d value is positive or negative by comparing it to a center point double center_x = 1000-maps_x[prev_wp]; double center_y = 2000-maps_y[prev_wp]; double centerToPos = distance(center_x,center_y,x_x,x_y); double centerToRef = distance(center_x,center_y,proj_x,proj_y); if(centerToPos <= centerToRef) { frenet_d *= -1; } // calculate s value double frenet_s = maps_s[0]; for(int i = 0; i < prev_wp; i++) { frenet_s += distance(maps_x[i],maps_y[i],maps_x[i+1],maps_y[i+1]); } frenet_s += distance(0,0,proj_x,proj_y); return {frenet_s,frenet_d}; } // Transform from Frenet s,d coordinates to Cartesian x,y vector<double> getXY(double s, double d, vector<double> maps_s, vector<double> maps_x, vector<double> maps_y) { int prev_wp = -1; while(s > maps_s[prev_wp+1] && (prev_wp < (int)(maps_s.size()-1) )) { prev_wp++; } int wp2 = (prev_wp+1)%maps_x.size(); double heading = atan2((maps_y[wp2]-maps_y[prev_wp]),(maps_x[wp2]-maps_x[prev_wp])); // the x,y,s along the segment double seg_s = (s-maps_s[prev_wp]); double seg_x = maps_x[prev_wp]+seg_s*cos(heading); double seg_y = maps_y[prev_wp]+seg_s*sin(heading); double perp_heading = heading-pi()/2; double x = seg_x + d*cos(perp_heading); double y = seg_y + d*sin(perp_heading); return {x,y}; }int NextWaypoint(double x, double y, double theta, vector<double> maps_x, vector<double> maps_y) { int closestWaypoint = ClosestWaypoint(x,y,maps_x,maps_y); double map_x = maps_x[closestWaypoint]; double map_y = maps_y[closestWaypoint]; double heading = atan2( (map_y-y),(map_x-x) ); double angle = abs(theta-heading); if(angle > pi()/4) { closestWaypoint++; } return closestWaypoint; }
想要源代码的朋友可以在评论区留下联系方式。
