无人驾驶——对frenet坐标的理解

时间：2018-10-25 14:21:51 收藏：0 阅读：7992

好的确定车和路之间的关系，我们通常将车辆的在大地坐标坐标转化为车辆和道路之间的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;
}

想要源代码的朋友可以在评论区留下联系方式。