离散点曲线曲率计算方法大汇总

最新推荐文章于 2024-12-28 01:32:24 发布
最新推荐文章于 2024-12-28 01:32:24 发布
阅读量6.7k 收藏 67
点赞数 19
文章标签： 自动驾驶 机器人
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/beiyangxiaokai/article/details/135269999
版权

引言

在机器人、自动驾驶等领域中，规划与控制常常需要获取路径的曲率信息，而路径曲线的表示的方式一般为离散点的形式，这给曲率的求取带来一定难度。下面给出4种根据离散点曲线求曲率的方法。

方法

差分法

# 差分法
def calculate_curvature_difference(data, interval=1):
	# interval代表计算曲率时选取点的间隔，例如当需要计算data[i]的曲率时，选取的点为
	# data[i-2*interval],data[i-interval],data[i],data[i+interval],data[i+2*interval]这5个点来计算
	# 通过此操作可以缓解离散点不平滑的情况，后面的函数同理
    curvature_list = []
    
    for i in range(len(data)):
        # 初始化头尾的曲率均为0
        if i < 2 * interval:
            curvature_list.append(0)
        elif i > len(data) - 2*interval - 1:
            curvature_list.append(0)
        # 获取间隔为interval的5个点（二阶差分需要5个点）
        else:
            start, end = i - 2 * interval, i + 2 * interval + 1
            x, y = data[start:end:interval, 0], data[start:end:interval, 1]
        
        # 差分法计算曲率
            # 一阶差分
            dx = np.gradient(x)
            dy = np.gradient(y)

            # 二阶差分
            d2x = np.gradient(dx)
            d2y = np.gradient(dy)

            # 计算曲率, 并处理分母为0的情况
            denominator = (dx**2 + dy**2)**1.5
            curvature = np.where(denominator == 0, 0, (dx * d2y - d2x * dy) / denominator)
            # 只有中间点的曲率是准确计算的，所以只取中间点的曲率
            curvature_list.append(curvature[2])
    
    # 将头尾数据的曲率设置为最近可计算点的曲率
    curvature_list[0:2*interval] = [curvature_list[2*interval]] * (2*interval)
    curvature_list[-2*interval:] = [curvature_list[-2*interval-1]] * (2*interval)

    return curvature_list

注意：
1.差分法需要二阶差分，所以计算一个点的较为精确曲率需要周围5个点的信息。
2.差分法对于噪声较大，不平滑的离散点的曲率计算效果较差。
3.当相邻点之间的距离较小时，或者点的噪声比较大时，可以间隔选取点来计算曲率，可以提高精度。间隔个数通过interval参数设置。

三点参数方程法

# 三点参数方程法
def calculate_curvature_parameter(data,interval=1):
	# interval代表计算曲率时选取点的间隔，例如当需要计算data[i]的曲率时，选取的点为
	# data[i-interval],data[i],data[i+interval]这3个点来计算
	# 通过此操作可以缓解离散点不平滑的情况，后面的函数同理
    curvature_list=[]
    for i in range(len(data)):
        # 初始化头尾的曲率均为0
        if i < interval:
            curvature_list.append(0)
        elif i > len(data) - interval - 1:
            curvature_list.append(0)
        # 获取间隔为interval的3个点（二阶差分需要3个点）
        else:
            start, end = i -  interval, i +  interval + 1
            x, y = data[start:end:interval, 0], data[start:end:interval, 1]
    
            t_a = norm([x[1]-x[0], y[1]-y[0]])
            t_b = norm([x[2]-x[1], y[2]-y[1]])

            M = np.array([
                [1, -t_a, t_a**2],
                [1, 0,    0     ],
                [1,  t_b, t_b**2]
            ])

            # Add a small regularization term to the diagonal to prevent singularity
            M += np.eye(3) * 1e-6

            a = np.matmul(inv(M), x)
            b = np.matmul(inv(M), y)

            curvature = 2*(a[2]*b[1]-b[2]*a[1])/(a[1]**2.+b[1]**2.)**(1.5)
            curvature_list.append(curvature)
    # 将头尾数据的曲率设置为最近可计算点的曲率
    curvature_list[0:interval] = [curvature_list[interval]] * (interval)
    curvature_list[-interval:] = [curvature_list[-interval-1]] * (interval)
            
    return curvature_list

注意：
1.三点参数方程法需要周围3个点的信息。
2.三点参数方程法对于噪声较大，不平滑的离散点的曲率计算效果较差。
3.当相邻点之间的距离较小时，或者点的噪声比较大时，可以间隔选取点来计算曲率，可以提高精度。间隔个数通过interval参数设置。

三点画圆法

# 三点画圆法
def calculate_curvature_circle(data,interval=1):
	# interval代表计算曲率时选取点的间隔，例如当需要计算data[i]的曲率时，选取的点为
	# data[i-interval],data[i],data[i+interval]这3个点来计算
	# 通过此操作可以缓解离散点不平滑的情况，后面的函数同理
    curvature_list=[]
    for i in range(len(data)):
        # 初始化头尾的曲率均为0
        if i < interval:
            curvature_list.append(0)
        elif i > len(data) - interval - 1:
            curvature_list.append(0)
        # 获取间隔为interval的3个点（二阶差分需要3个点）
        else:
            start, end = i -  interval, i +  interval + 1
            x, y = data[start:end:interval, 0], data[start:end:interval, 1]

            # 计算圆心
            x1,x2,x3=x
            y1,y2,y3=y
            denominator = 2 * (x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2))
            
            x_center = ((x1**2 + y1**2) * (y2 - y3) + (x2**2 + y2**2) * (y3 - y1) + (x3**2 + y3**2) * (y1 - y2)) / denominator
            y_center = ((x1**2 + y1**2) * (x3 - x2) + (x2**2 + y2**2) * (x1 - x3) + (x3**2 + y3**2) * (x2 - x1)) / denominator

            # 计算半径
            r = np.sqrt((x1 - x_center)**2 + (y1 - y_center)**2)

            # 计算曲率
            curvature = 1 / r
            
                # 判断曲率的正负
            vector1 = (x2 - x1, y2 - y1)
            vector2 = (x3 - x2, y3 - y2)
            cross_product = vector1[0] * vector2[1] - vector1[1] * vector2[0]

            if cross_product > 0:
                curvature *= 1  # 曲率为负
            else:
                curvature *= -1   # 曲率为正
            
            curvature_list.append(curvature)
            
    # 将头尾数据的曲率设置为最近可计算点的曲率
    curvature_list[0:interval] = [curvature_list[interval]] * (interval)
    curvature_list[-interval:] = [curvature_list[-interval-1]] * (interval)
            
    return curvature_list

注意：
1.三点画圆法需要周围3个点的信息。
2.三点画圆法对于噪声较大，不平滑的离散点的曲率计算效果较差。
3.当相邻点之间的距离较小时，或者点的噪声比较大时，可以间隔选取点来计算曲率，可以提高精度。间隔个数通过interval参数设置。

曲线拟合法

# 曲线拟合法
def Calculate_curvature_spline(data):
    x=data[:,0]
    y=data[:,1]

    # Cubic Spline 拟合
    cs = CubicSpline(x, y)

    # 求导
    y_prime = cs(x, 1)
    y_double_prime = cs(x, 2)

    # 计算曲率
    curvature_list = (y_double_prime) / np.sqrt(1 + y_prime**2)**3

    return curvature_list

注意：
1.本方法采用了三次样条曲线进行拟合，也可以采用其他方式进行拟合。
2.该方法对于离散点存在误差、不平滑的情况时具有很好的效果，相对于其他方法，该方法的综合效果最好。
3.该方法存在一个致命的弱点，当离散点的x坐标非单调时则无法采用此方法，因为函数要求x必须是单调的，否则不能成为函数。

完整代码与测试结果

import numpy as np
import matplotlib.pyplot as plt
from scipy.interpolate import CubicSpline
from numpy.linalg import norm, inv


# 差分法
def calculate_curvature_difference(data, interval=1):
    curvature_list = []
    
    for i in range(len(data)):
        # 初始化头尾的曲率均为0
        if i < 2 * interval:
            curvature_list.append(0)
        elif i > len(data) - 2*interval - 1:
            curvature_list.append(0)
        # 获取间隔为interval的5个点（二阶差分需要5个点）
        else:
            start, end = i - 2 * interval, i + 2 * interval + 1
            x, y = data[start:end:interval, 0], data[start:end:interval, 1]
        
        # 差分法计算曲率
            # 一阶差分
            dx = np.gradient(x)
            dy = np.gradient(y)

            # 二阶差分
            d2x = np.gradient(dx)
            d2y = np.gradient(dy)

            # 计算曲率, 并处理分母为0的情况
            denominator = (dx**2 + dy**2)**1.5
            curvature = np.where(denominator == 0, 0, (dx * d2y - d2x * dy) / denominator)
            # 只有中间点的曲率是准确计算的，所以只取中间点的曲率
            curvature_list.append(curvature[2])
    
    # 将头尾数据的曲率设置为最近可计算点的曲率
    curvature_list[0:2*interval] = [curvature_list[2*interval]] * (2*interval)
    curvature_list[-2*interval:] = [curvature_list[-2*interval-1]] * (2*interval)

    return curvature_list





# 三点参数方程法
def calculate_curvature_parameter(data,interval=1):

    curvature_list=[]
    for i in range(len(data)):
        # 初始化头尾的曲率均为0
        if i < interval:
            curvature_list.append(0)
        elif i > len(data) - interval - 1:
            curvature_list.append(0)
        # 获取间隔为interval的3个点（二阶差分需要3个点）
        else:
            start, end = i -  interval, i +  interval + 1
            x, y = data[start:end:interval, 0], data[start:end:interval, 1]
    
            t_a = norm([x[1]-x[0], y[1]-y[0]])
            t_b = norm([x[2]-x[1], y[2]-y[1]])

            M = np.array([
                [1, -t_a, t_a**2],
                [1, 0,    0     ],
                [1,  t_b, t_b**2]
            ])

            # Add a small regularization term to the diagonal to prevent singularity
            M += np.eye(3) * 1e-6

            a = np.matmul(inv(M), x)
            b = np.matmul(inv(M), y)

            curvature = 2*(a[2]*b[1]-b[2]*a[1])/(a[1]**2.+b[1]**2.)**(1.5)
            curvature_list.append(curvature)
    # 将头尾数据的曲率设置为最近可计算点的曲率
    curvature_list[0:interval] = [curvature_list[interval]] * (interval)
    curvature_list[-interval:] = [curvature_list[-interval-1]] * (interval)
            
    return curvature_list


# 三点画圆法
def calculate_curvature_circle(data,interval=1):
    curvature_list=[]
    for i in range(len(data)):
        # 初始化头尾的曲率均为0
        if i < interval:
            curvature_list.append(0)
        elif i > len(data) - interval - 1:
            curvature_list.append(0)
        # 获取间隔为interval的3个点（二阶差分需要3个点）
        else:
            start, end = i -  interval, i +  interval + 1
            x, y = data[start:end:interval, 0], data[start:end:interval, 1]

            # 计算圆心
            x1,x2,x3=x
            y1,y2,y3=y
            denominator = 2 * (x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2))
            
            x_center = ((x1**2 + y1**2) * (y2 - y3) + (x2**2 + y2**2) * (y3 - y1) + (x3**2 + y3**2) * (y1 - y2)) / denominator
            y_center = ((x1**2 + y1**2) * (x3 - x2) + (x2**2 + y2**2) * (x1 - x3) + (x3**2 + y3**2) * (x2 - x1)) / denominator

            # 计算半径
            r = np.sqrt((x1 - x_center)**2 + (y1 - y_center)**2)

            # 计算曲率
            curvature = 1 / r
            
                # 判断曲率的正负
            vector1 = (x2 - x1, y2 - y1)
            vector2 = (x3 - x2, y3 - y2)
            cross_product = vector1[0] * vector2[1] - vector1[1] * vector2[0]

            if cross_product > 0:
                curvature *= 1  # 曲率为负
            else:
                curvature *= -1   # 曲率为正
            
            curvature_list.append(curvature)
            
    # 将头尾数据的曲率设置为最近可计算点的曲率
    curvature_list[0:interval] = [curvature_list[interval]] * (interval)
    curvature_list[-interval:] = [curvature_list[-interval-1]] * (interval)
            
    return curvature_list






# 曲线拟合法
def Calculate_curvature_spline(data):
    x=data[:,0]
    y=data[:,1]

    # Cubic Spline 拟合
    cs = CubicSpline(x, y)

    # 求导
    y_prime = cs(x, 1)
    y_double_prime = cs(x, 2)

    # 计算曲率
    curvature_list = (y_double_prime) / np.sqrt(1 + y_prime**2)**3

    return curvature_list



x=np.linspace(0,2*np.pi,10000)
y=np.sin(x)
data=np.array([x,y]).T
#curvature=calculate_curvature_difference(data,1)
#curvature=calculate_curvature_parameter(data,4)# 这块需要设置为4效果才好，因为点的间距很小
#curvature=calculate_curvature_circle(data,1)
curvature=Calculate_curvature_spline(data)
plt.figure(1)
plt.plot(x,y)
plt.show()
plt.figure(2)
plt.plot(curvature)
plt.show()

  • 函数曲线图
    在这里插入图片描述- 曲线曲率图-差分法
    在这里插入图片描述

  • 曲线曲率图-参数方程法
    在这里插入图片描述

  • 曲线曲率图-三点画圆法
    在这里插入图片描述

  • 曲线曲率图-曲线拟合法
    在这里插入图片描述

北洋树人
关注
【局部规划模块曲线拟合算法】曲线拟合的实现路径插值与平滑（附C++代码）
qq_35635374的博客
04-28 683
系列文章目录 提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加 TODO:写完再整理 文章目录系列文章目录前言（1）双圆弧轨迹插值拟合算法一、双圆弧轨迹插值拟合算法的应用场景二、双圆弧轨迹插值拟合算法的实现流程三、实现工具（2）正余弦曲线插值拟合算法一、正余弦曲线插值拟合算法--路径规划二、正余弦曲线插值拟合算法--速度规划（3）基于三次、五次多项式曲线拟合算法的路径插值及平滑1、直线插值、二次样条插值、三次样条插值生成路径效果对比2、三次样条插值的路径生成实现1.人为数学推导过程（代数的
TPAMI 2024 | 黎曼流形上的概率主曲线
小白学视觉
08-11 123
本文研究了一种新的 Riemannian 流形上数据的曲线拟合方法。我们基于观察和未观测潜在变量的混合模型定义了一种主曲线，并提出了一种新的算法来估计给定数据点在 Riemannian 流形上主曲线。本文介绍了黎曼对称空间上主曲线的概率表述。为此，我们研究了将正态分布和曲线的平滑度惩罚扩展到黎曼流形上。此外，我们提出了一个实用的算法，使用展开、解包和打包的过程来估计所提出的PPC。所提出的PPC有一些局限性。
散乱点求特定点高斯曲率和平均曲率
08-15
求散乱点中特定点的高斯曲率和平均曲率，使用点最近的五个点计算
离散点曲率计算C++源码
10-31
离散点曲率计算离散点曲率计算离散点曲率计算离散点曲率计算
自动驾驶（七十三）---------离散点计算曲率
zhouyy858
08-17 2万+
在自动驾驶中，很常见的一个问题是通过离散点计算曲率，奇怪的是网上居然找不到我需要的资料，也许是我描述的不对，总之找了很久都不是很满意，所以干脆自己来总结一下吧。 网上也不是找不到资料，但是大部分都是这样： （1）通过曲率公式，首先你要知道曲线的表达式，然后代入公式计算。 （2）离散点也需要先polyfit拟合成多项式，再利用上面的公式计算曲率。 （3）在matlab中，用csape()对离散点进行Spline插值，然...
计算离散点曲率的方法
不想学好数学的汽车工程师不是一个好的程序员
02-19 1万+
以三点外接圆方式计算： 以三个离散点的外接圆半径的倒数作为中间点的曲率计算公式为: k=4Sabck = \frac{4S}{abc}k=abc4S​ 其中S为三点构成三角形的面积。 a、b、c分别为三角形的三个边长。 如下图所示： 三点计算三角形面积的计算公式为： S=∣x1y2+x2y3+x3y1−y1x2−y2x3−y3x12∣S=|\frac{x_1y_2+x_2y_3+x_3y_1-y_1x_2-y_2x_3-y_3x_1}{2}|S=∣2x1​y2​+x2​y3​+x3​y1​−y1​x2​−y
离散点的曲率的计算,离散点求曲率,matlab
09-10
计算nurbs曲率NURBS曲线的曲率计算
离散点曲率计算
最新发布
qq_37423490的博客
12-28 1013
离散点曲率计算方法
离散点的曲率计算
weixin_42156097的博客
08-08 9287
描述 我有一个点集，里面都是[x,y]这样的二维点，这个点集能形成一个曲线。 在并不想去求这个曲线的表达式时，怎么得到某一点的曲率呢 相关知识 曲率的定义是： 针对曲线上某个点的切线方向角对弧长的转动率，而曲率的倒数就是曲率半径 K  =  ∣Δθl∣  =  ∣1r∣  K\; =\; \left| \frac{\Delta \theta }{l} \right|\; =\; \left| \frac{1}{r} \right|\; K=∣∣∣∣​lΔθ​∣∣∣∣​=∣∣∣∣​r1​∣∣∣∣​ 式子中KK
离散点曲率计算方法c语言,自动驾驶（七十三）---------离散点计算曲率
weixin_35708531的博客
05-22 4096
在自动驾驶中，很常见的一个问题是通过离散点计算曲率，奇怪的是网上居然找不到我需要的资料，也许是我描述的不对，总之找了很久都不是很满意，所以干脆自己来总结一下吧。网上也不是找不到资料，但是大部分都是这样：(1)通过曲率公式，首先你要知道曲线的表达式，然后代入公式计算。 (2)离散点也需要先polyfit拟合成多项式，再利用上面的公式计算曲率。(3)在matlab中，用csape()对离散点进行Spl...
离散点的曲率的计算
热门推荐
无敌三脚猫的专栏
04-11 2万+
from：http://www.ilovematlab.cn/thread-67207-1-1.html 曲率公式： 注意： ①曲率半径为曲率的倒数 ②如果是离散点，先用polyfit和polyval拟合出曲线 程序：   clc; clear all; close all; x0 = linspace(0, 1); y0 = sin(x0).*cos
离散点求曲率
weixin_44028636的博客
08-23 1141
曲率公式： 注意： ①曲率半径为曲率的倒数 ②如果是离散点，先用polyfit和polyval拟合出曲线 程序： clc; clear all; close all; x0 = linspace(0, 1); y0 = sin(x0).*cos(x0); h = abs(diff([x0(2), x0(1)])); % 模拟一阶导 figure; box on; hold on; ythe1 = cos(x0).^2 - sin(x0).^2; %理论一阶导 yapp1 = gradient(y0, h)
NURBS曲线参数化法实现汇总+分段贝塞尔曲线拟合+一些简单改进的方法
m0_68783592的博客
12-04 559
NURBS曲线，曲线参数化法，曲线压缩，svg矢量图解析
【曲线曲率分析与优化】：从理论到实践的详细流程指导
本文首先介绍了曲线曲率的基本概念和数学定义，随后探讨了多种计算曲线曲率的方法，包括解析方法和数值方法。文中详细介绍了数学软件工具、编程语言及开源工具在曲率分析中的应用，并通过多个实践案例展示了曲率分析...
Bev任务issue汇总
@bangbang的博客
06-01 728
在做update的话，需要等异步任务结束后再去做update。将每次backward结果存起来，等所有任异步务结束后，再做update。主干训练完的话，进行冻结，然后针对各个head进行训练。多任务更偏向于算法框架，需要解决:(1)（前）, 超过110米其实效果就已经很差了，90米距离效果也比较差。单orin有两个DLA， DLA 通常是放backbone。scale更新的频率和网络参数更新的频率是1:1。的问题，是很难做好的，尤其是。学深度分布depth。lift这种几何投影。不同源的数据是处理的。
平滑曲线算法研究
Gypsyy的专栏
06-02 9234
一  样条概述 在绘图术语中样条是通过一组指定点集而生成平滑曲线的柔性带 。 术语 样条曲线 spline curve 绘制样条曲线的方法是给定一组称为控制点的坐标点，可以得到一条样条曲线。 样条曲线分为: 1 插值样条曲线(interpolate)  生成的样条曲线通过这组控制点。（这是我们详细研究的）2 逼近样条曲线(approximate)  生面的样条曲线不通过或通过部分控制点。
Spline导数及曲率计算
OneSea的专栏
01-21 1万+
参考资料 [1]三次B样条曲率与导数算法Matlab代码
离散点（离散序列）曲率计算那点事
izhxxx的博客
01-16 2万+
声明：本文曾发表至智能车制作论坛，非抄袭！（离散点曲率计算那点事，曾经的辣鸡破车*策略）各位大佬若需转载请注明出处！感谢！ 　　　 曲率计算Ａ方案：（三角形外接圆曲率法） 　　此方法据说在智能车上的应用最早出自东北大学的技术报告（本人未验证），即在一组离散点中选取三点，计算以此三点所围成的三角形的外接圆曲率即可。示意图及公式如下： 　　个人评价：显然，该方案实现起来较为简单，但三点选取位置可能会对...
评论 3
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值