![](https://img2022.cnblogs.com/blog/1667991/202205/1667991-20220524233744054-1963064400.png)
\[K=\frac{|y''|}{(1+y'^2)^{\frac{3}{2}}}
\]
在任一点\((a,b)\)的曲率,带入即可:
\[y'|_{x=1}=-1\\
y''|_{x=1}=2\\
K=\frac{|y''|}{(1+y'^2)^{\frac{3}{2}}}
=\frac{2}{(1+(-1)^2)^{\frac{3}{2}}}
=\frac{\sqrt{2}}{2}
\]
代码示例:
//s_为曲线
Eigen::Vector2d dxy = s_(s0, 1); //在s0处的一阶导数
Eigen::Vector2d ddxy = s_(s0, 2);//在s0处的二阶导数
double dx = dxy.x();
double dy = dxy.y();
phi = atan2(dy, dx);
double ddx = ddxy.x();
double ddy = ddxy.y();
double dphi = (ddy * dx - dy * ddx) / (dx * dx + dy * dy);
//根据曲率计算参考速度
double curvature = std::fabs(dphi / std::sqrt(dx * dx + dy * dy));