好的确定车和路之间的关系，我们通常将车辆的在大地坐标坐标转化为车辆和道路之间的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,yvector<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;}想要源代码的朋友可以在评论区留下联系方式。
无人驾驶——对frenet坐标的理解
原文地址：https://www.cnblogs.com/fuhang/p/9848992.html