自动驾驶（七十三）---------离散点计算曲率

最新推荐文章于 2024-07-02 08:42:29 发布

一实相印

最新推荐文章于 2024-07-02 08:42:29 发布

阅读量2.2w

点赞数 26

分类专栏：自动驾驶

本文链接：https://blog.csdn.net/zhouyy858/article/details/108057703

版权

自动驾驶专栏收录该内容

87 篇文章 1101 订阅

订阅专栏

在自动驾驶中，很常见的一个问题是通过离散点计算曲率，奇怪的是网上居然找不到我需要的资料，也许是我描述的不对，总之找了很久都不是很满意，所以干脆自己来总结一下吧。

网上也不是找不到资料，但是大部分都是这样：

（1）通过曲率公式，首先你要知道曲线的表达式，然后代入公式计算。

（2）离散点也需要先polyfit拟合成多项式，再利用上面的公式计算曲率。

（3）在matlab中，用csape()对离散点进行Spline插值，然后用fnder()对得到曲线求导，最后用fnval()对导数求值即可。

（4）通过标量公式，计算曲率。

（5）三个点外接圆：但是三个点计算，不好推广。

总之以上的一些方法都不直接，拐弯抹角计算曲率，曲率直接理解就是角度变化率，近似对应于曲率半径的倒数，利用一串点鲁棒的计算对应的曲率就应该很简单才对，不要什么拟合多项式再代公式求曲率，简直是隔靴搔痒、舍本逐末。这里我总结一种直观的方法，抛砖引玉：

1. 离散点拟合圆

曲率的倒数就是半径，那我先拟合圆，知道半径也就知道曲率了，圆的拟合可以使多个点，拟合也有足够的鲁棒性。

用圆的展开公式：。圆心为,圆的半径就是。

误差方程为：对于自变量a、b、c求偏导为0：

得到：为了直观和看代码方便，我们假设

上式可以描述为：

通过矩阵知识可以解出：


void FitCenterByLeastSquares(std::vector<Point3D> mapPoint, Point3D &centerP, double &radius) {
    double sumX = 0, sumY = 0;
    double sumXX = 0, sumYY = 0, sumXY = 0;
    double sumXXX = 0, sumXXY = 0, sumXYY = 0, sumYYY = 0;

    int pCount = mapPoint.size();
    for (int i = 0; i<pCount; i++){ 
        sumX += mapPoint[i].x;
        sumY += mapPoint[i].y;
        sumXX += pow(mapPoint[i].x,2);
        sumYY += pow(mapPoint[i].y,2);
        sumXY += mapPoint[i].x * mapPoint[i].y;
        sumXXX += pow(mapPoint[i].x,3);
        sumXXY += pow(mapPoint[i].x,2) * mapPoint[i].y;
        sumXYY += mapPoint[i].x * pow(mapPoint[i].y,2);
        sumYYY += pow(mapPoint[i].y,3);
    }

    double M1 = pCount * sumXY - sumX * sumY;
    double M2 = pCount * sumXX - sumX * sumX;
    double M3 = pCount * (sumXXX + sumXYY) - sumX * (sumXX + sumYY);
    double M4 = pCount * sumYY - sumY * sumY;
    double M5 = pCount * (sumYYY + sumXXY) - sumY * (sumXX + sumYY);

    double a = (M1 * M5 - M3 * M4) / (M2 * M4 - M1 * M1);
    double b = (M1 * M3 - M2 * M5) / (M2 * M4 - M1 * M1);
    double c = -(a * sumX + b * sumY + sumXX + sumYY) / pCount;

    //圆心XY 半径
    double xCenter = -0.5 * a;
    double yCenter = -0.5 * b;
    radius = 0.5 * sqrt(a * a + b * b - 4 * c);
    centerP.x = xCenter;
    centerP.y = yCenter;
}

2. 计算正负号

通过上面我们计算出圆的半径，倒数就是曲率，那么曲率的正负号呢？这里我们也需要一种方法优雅的计算它的正负号，不然有失风度，目前我能给出的方法是直线与圆心的关系：

计算起始点与终点的直线，判断该直线与圆心的关系从而判断曲率的正负号，又因为前面已经计算了很多最小二乘法的中间变量，要是不用也可惜了，干脆拟合直线算了：

类似的误差方程：偏导为0得到：和

联立解方程得到：最后把圆心代入方程判断正负即可。

这里给出新加的代码，只需要在原来的基础上只要加3行代码即可，基本上还算优雅吧：

    //计算曲率正负号
	a = (sumXY-((sumX*sumY)/pCount))/(sumXX-(sumX*sumX/pCount));
	b = (sumY-a*sumX)/pCount;
	centerP.z = (xCenter*a+b<yCenter)?(1/radius):(-1/radius);