自动驾驶(七十三)———离散点计算曲率

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

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

         (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);

        

原创:https://www.panoramacn.com
源码网提供WordPress源码,帝国CMS源码discuz源码,微信小程序,小说源码,杰奇源码,thinkphp源码,ecshop模板源码,微擎模板源码,dede源码,织梦源码等。

专业搭建小说网站,小说程序,杰奇系列,微信小说系列,app系列小说

自动驾驶(七十三)---------离散点计算曲率

免责声明,若由于商用引起版权纠纷,一切责任均由使用者承担。

您必须遵守我们的协议,如果您下载了该资源行为将被视为对《免责声明》全部内容的认可-> 联系客服 投诉资源
www.panoramacn.com资源全部来自互联网收集,仅供用于学习和交流,请勿用于商业用途。如有侵权、不妥之处,请联系站长并出示版权证明以便删除。 敬请谅解! 侵权删帖/违法举报/投稿等事物联系邮箱:2640602276@qq.com
未经允许不得转载:书荒源码源码网每日更新网站源码模板! » 自动驾驶(七十三)———离散点计算曲率
关注我们小说电影免费看
关注我们,获取更多的全网素材资源,有趣有料!
120000+人已关注
分享到:
赞(0) 打赏

评论抢沙发

  • 昵称 (必填)
  • 邮箱 (必填)
  • 网址

您的打赏就是我分享的动力!

支付宝扫一扫打赏

微信扫一扫打赏