無人駕駛——對frenet坐標的理解

好的確定車和路之間的關系，我們通常將車輛的在大地坐標坐標轉化為車輛和道路之間的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;
}

想要源代碼的朋友可以在評論區留下聯系方式。