选取路线曲率半径的计算

首先，将选取路线分成一定数量的点。然后用三个相邻的数据点来构建一个圆，例如选择当前点以及其相邻的两个点。

接下来，使用最小二乘法拟合圆的方法来估计该圆的圆心和半径。构建方程：对于三个点(x1, y1)、(x2, y2)和(x3, y3)，圆心为 (a, b) 和半径为 r 的圆满足以下方程：

(x1 - a)^2 + (y1 - b)^2 = r^2

(x2 - a)^2 + (y2 - b)^2 = r^2

(x3 - a)^2 + (y3 - b)^2 = r^2

通过计算该圆的曲率，可以得到当前点上的曲率半径。曲率半径的计算公式为 R = 1/r。

对每个点重复上述步骤，即可计算每个点上的曲率半径。

关于解方程，在matlab中可以用lsqnonlin函数实现。

需要注意的是，该方法解出来的曲率均为正，实际过程中，我们应当自定义正负方向。同时该方法解出来的的曲率半径可能存在过大等情况，我们需要进行一定处理。

另外nx这款软件可以直接解曲线曲率半径，效果还不错，b站有教程。