离散点间曲率计算

本文转自知乎计算离散点的曲率(附Python, MATLAB代码)

在很多学科中的很多计算任务中都需要用到曲线的曲率(或者曲率半径),numpy库里和matlab build-in里都没有现成的能从离散点来算曲率的方法,网上找到的代码又不敢直接用,毕竟是要高频率用到自己科研上的工具,所以决定结合找到的资料自己推一下,并造出python和matlab的轮子,造福后人

公式很简单:

曲率:

离散点间曲率计算_第1张图片

在二维情况下,其标量形式为:

离散点间曲率计算_第2张图片

所以对于解析情况非常简单,可以直接对于曲线表达式进行解析求导,但是对于离散的点,情况反倒会比较复杂,因为这里的x和y的一阶和二阶导数如果直接用差分方法来计算的话会造成比较大的误差。

这里比较保险的做法是,使用三个点确定的二次曲线的的曲率作为我们估计的曲率:

然后使用中间这个点(x2,y2)处的曲率作为这三个点的曲率估计

具体方法是先表示成参数方程的样子,

对于曲线参数t,有:

离散点间曲率计算_第3张图片

 

6个未知数,三个点里有6个已知分量,列六个方程,解出这 (a1,a2,a3), (b1,b2,b3)即可。

这里使用两段矢量的长度来作为取值范围:

离散点间曲率计算_第4张图片

 

这里我们希望参数方程中的t满足如下条件:

离散点间曲率计算_第5张图片

 

则有:

离散点间曲率计算_第6张图片

以及

离散点间曲率计算_第7张图片

 

写成矩阵形式:

离散点间曲率计算_第8张图片

 

以及

离散点间曲率计算_第9张图片

 

简写为:

离散点间曲率计算_第10张图片

 

可以使用求矩阵逆的方式求解线性方程:

 

有了(a1,a2,a3)和(b1,b2,b3)就有了曲线的解析方程,接下来就和解析求曲率一样了,先算变量导数:

离散点间曲率计算_第11张图片

 

然后就是最终的曲率:

离散点间曲率计算_第12张图片

 

这样,任意给定三个点,都可以估计出这三个点是比较【弯的】还是比较【直的】,直的曲率小,van的曲率大。

随手生成两个例子算算:

(1)圆形:

好理解,圆形所有地方都是一样弯

(2)正弦

波峰和波谷最弯,0点附近最直。

具体实现源码:

python及matlab实现源码

你可能感兴趣的:(路径规划)