Matlab中计算道路曲率的几种方法

我使用Prescan采集到的道路中心线数据,都是离散点(x,y,z),但在作研究时,通常都是道路曲率,这时需要将离散点坐标转换为曲率,但通过计算得到的曲率与实际曲率有一些误差,以下是几种方法。

注意:这里代码中的xunizhuang_x和xunizhuang_y是我在道路中心线上每隔两米设置的虚拟桩,可以直接替换成你自己的道路中心线坐标(但离散点的疏密程度可能会影响到结果)

1. 三点公式法

这种方法使用连续三个点的坐标来计算曲率,可以较为简单地应用于离散点数据,如样本间隔均匀的路径数据。

假设有三个连续的点 ,曲率 K 可以近似计算为:

matlab代码如下:

% 计算三点法曲率
curvature_3pt = zeros(1, length(xunizhuang_x) - 2);
for i = 2:length(xunizhuang_x) - 1
    x1 = xunizhuang_x(i - 1); y1 = xunizhuang_y(i - 1);
    x2 = xunizhuang_x(i); y2 = xunizhuang_y(i);
    x3 = xunizhuang_x(i + 1); y3 = xunizhuang_y(i + 1);
    
    area = abs(x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)) / 2;  
    d12 = sqrt((x2 - x1)^2 + (y2 - y1)^2);
    d23 = sqrt((x3 - x2)^2 + (y3 - y2)^2);
    d31 = sqrt((x3 - x1)^2 + (y3 - y1)^2);
    curvature_3pt(i - 1) = 4 * area / (d12 * d23 * d31);  % 曲率计算公式
end

结果:

2.时间参数化法

时间参数化法将曲线表示为时间的函数,通过时间的导数来计算曲率。

如果数据点是按时间序列采集的, x和 y表示为时间的函数:

计算一阶导数和二阶导数,之后根据导数计算曲率 K:

matlab代码如下:

% 时间参数化法曲率计算+平滑
windowSize = 5;  
xunizhuang_x_smooth = movmean(xunizhuang_x, windowSize);
xunizhuang_y_smooth = movmean(xunizhuang_y, windowSize);
dx = gradient(xunizhuang_x_smooth) ./ gradient(xunizhuang_t);
dy = gradient(xunizhuang_y_smooth) ./ gradient(xunizhuang_t);
ddx = gradient(dx) ./ gradient(xunizhuang_t);
ddy = gradient(dy) ./ gradient(xunizhuang_t);
curvature_time_smooth = abs(dx .* ddy - dy .* ddx) ./ ((dx.^2 + dy.^2).^(3/2));

结果

3.多项式拟合法(不推荐)

如果曲线较为光,可以使用多项式f(x)拟合,尤其适用于噪声较低的数据。拟合一条二次或三次多项式曲线,然后求导来计算曲率。二次多项式曲率公式为:

其中,f(x)为拟合多项式

matlab代码如下:

% 二次多项式拟合曲率计算
p = polyfit(xunizhuang_x, xunizhuang_y, 10); 
f_prime = polyder(p); 
f_double_prime = polyder(f_prime);  
curvature_polyfit = abs(polyval(f_double_prime, xunizhuang_x)) ./ ...
                   (1 + polyval(f_prime, xunizhuang_x).^2).^(3/2);

以下是2阶多项拟合结果

以下是5阶多项拟合结果

以下是20阶多项拟合结果

可以看出此方法不太适用

4. 弧长参数化法

这种方法适合在任意路径上计算曲率。路径使用弧长(路径的累积距离)重新参数化,通过弧长的导数计算曲率。其公式为:

初始matlab代码如下:

% 路径弧长法计算曲率 
segment_lengths = sqrt((diff(xunizhuang_x).^2 + diff(xunizhuang_y).^2)).';
s = cumsum([0; segment_lengths]);
dx_ds = gradient(xunizhuang_x) ./ gradient(s);
dy_ds = gradient(xunizhuang_y) ./ gradient(s);
ddx_ds2 = gradient(dx_ds) ./ gradient(s);
ddy_ds2 = gradient(dy_ds) ./ gradient(s);
curvature_arclength = abs(dx_ds .* ddy_ds2 - dy_ds .* ddx_ds2) ./ ...
                      ((dx_ds.^2 + dy_ds.^2).^(3/2));

结果

改进后,matlab代码如下:

% 路径弧长法计算曲率 - 加强平滑
segment_lengths = sqrt((diff(xunizhuang_x).^2 + diff(xunizhuang_y).^2)).';
s = cumsum([0; segment_lengths]);
smooth_x = smoothdata(xunizhuang_x, 'movmean', 10); % 平滑 x 数据
smooth_y = smoothdata(xunizhuang_y, 'movmean', 10); % 平滑 y 数据

% 确保 smooth_x, smooth_y, s 均为列向量
smooth_x = smooth_x(:);
smooth_y = smooth_y(:);
s = s(:);

% 计算一阶导数 dx_ds 和 dy_ds
dx_ds = diff(smooth_x) ./ diff(s);
dy_ds = diff(smooth_y) ./ diff(s);

% 计算二阶导数 ddx_ds2 和 ddy_ds2
ddx_ds2 = diff(dx_ds) ./ diff(s(1:end-1)); % 保证 s 的长度匹配
ddy_ds2 = diff(dy_ds) ./ diff(s(1:end-1)); % 同上

% 计算曲率
curvature_arclength = abs(dx_ds(1:end-1) .* ddy_ds2 - dy_ds(1:end-1) .* ddx_ds2) ./ ...
                      ((dx_ds(1:end-1).^2 + dy_ds(1:end-1).^2).^(3/2));

改进几点:

1、添加 smoothdata 函数进行平滑,修改窗口尺寸,以获得更平滑的曲线

2、gradient 在计算不规则数据的二阶导数时可能会导致误差,使用 diff 辅助计算高阶导数,确保结果平滑、稳定                  
但需 注意:diff 操作返回的数组长度比原数组短1,因此,一阶导数 dx_ds 和 dy_ds 的长度比原数据少 1,二阶导数 ddx_ds2 和 ddy_ds2 比一阶导数少 1,曲率计算时使用 1:end-1 来对齐长度。

改进后的结果

5. 离散曲线拟合法(样条曲线)

对于复杂、噪声较多的数据,通过拟合样条曲线或高阶多项式曲线,可以得到平滑的曲线,然后使用数值求导计算曲率。这种方法对复杂曲线效果较好。公式与上述弧长参数化法相似。matlab代码如下:

% 样条拟合法曲率计算 
% 使用 csape 对 x 和 y 进行单独的三次样条拟合
spline_fit_x = csape(xunizhuang_t, xunizhuang_x, 'not-a-knot');
spline_fit_y = csape(xunizhuang_t, xunizhuang_y, 'not-a-knot');

% 计算一阶和二阶导数
dx_ds_spline = fnder(spline_fit_x, 1); % x 的一阶导数
dy_ds_spline = fnder(spline_fit_y, 1); % y 的一阶导数
ddx_ds2_spline = fnder(spline_fit_x, 2); % x 的二阶导数
ddy_ds2_spline = fnder(spline_fit_y, 2); % y 的二阶导数

% 计算曲率
dx_ds = ppval(dx_ds_spline, xunizhuang_t);
dy_ds = ppval(dy_ds_spline, xunizhuang_t);
ddx_ds2 = ppval(ddx_ds2_spline, xunizhuang_t);
ddy_ds2 = ppval(ddy_ds2_spline, xunizhuang_t);

curvature_spline = abs(dx_ds .* ddy_ds2 - dy_ds .* ddx_ds2) ./ ...
                   ((dx_ds.^2 + dy_ds.^2).^(3/2));

结果:

进行平滑后,matlab代码如下:

% 样条拟合法曲率计算 
% 使用 csape 对 x 和 y 进行单独的三次样条拟合
spline_fit_x = csape(xunizhuang_t, xunizhuang_x, 'not-a-knot');
spline_fit_y = csape(xunizhuang_t, xunizhuang_y, 'not-a-knot');

% 计算一阶和二阶导数
dx_ds_spline = fnder(spline_fit_x, 1); % x 的一阶导数
dy_ds_spline = fnder(spline_fit_y, 1); % y 的一阶导数
ddx_ds2_spline = fnder(spline_fit_x, 2); % x 的二阶导数
ddy_ds2_spline = fnder(spline_fit_y, 2); % y 的二阶导数

% 计算并平滑导数
dx_ds = smoothdata(ppval(dx_ds_spline, xunizhuang_t), 'movmean', 5);
dy_ds = smoothdata(ppval(dy_ds_spline, xunizhuang_t), 'movmean', 5);
ddx_ds2 = smoothdata(ppval(ddx_ds2_spline, xunizhuang_t), 'movmean', 5);
ddy_ds2 = smoothdata(ppval(ddy_ds2_spline, xunizhuang_t), 'movmean', 5);

% 计算曲率
curvature_spline = abs(dx_ds .* ddy_ds2 - dy_ds .* ddx_ds2) ./ ...
                   ((dx_ds.^2 + dy_ds.^2).^(3/2));

结果

总结

总的代码如下:

道路数据自己加入,分别是道路中心线横坐标纵坐标和对应时间

xunizhuang_x = [];  
xunizhuang_y = []; 
xunizhuang_t = []; 






% 计算三点法曲率

curvature_3pt = zeros(1, length(xunizhuang_x) - 2);
for i = 2:length(xunizhuang_x) - 1
    x1 = xunizhuang_x(i - 1); y1 = xunizhuang_y(i - 1);
    x2 = xunizhuang_x(i); y2 = xunizhuang_y(i);
    x3 = xunizhuang_x(i + 1); y3 = xunizhuang_y(i + 1);
    
    area = abs(x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)) / 2;  
    d12 = sqrt((x2 - x1)^2 + (y2 - y1)^2);
    d23 = sqrt((x3 - x2)^2 + (y3 - y2)^2);
    d31 = sqrt((x3 - x1)^2 + (y3 - y1)^2);
    curvature_3pt(i - 1) = 4 * area / (d12 * d23 * d31);  % 曲率计算公式
end
% % % % % % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 二次多项式拟合曲率计算
p = polyfit(xunizhuang_x, xunizhuang_y, 3); 
f_prime = polyder(p); 
f_double_prime = polyder(f_prime);  
curvature_polyfit = abs(polyval(f_double_prime, xunizhuang_x)) ./ ...
                   (1 + polyval(f_prime, xunizhuang_x).^2).^(3/2);

% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 
% 路径弧长法计算曲率 - 加强平滑
segment_lengths = sqrt((diff(xunizhuang_x).^2 + diff(xunizhuang_y).^2)).';
s = cumsum([0; segment_lengths]);
smooth_x = smoothdata(xunizhuang_x, 'movmean', 10); % 平滑 x 数据
smooth_y = smoothdata(xunizhuang_y, 'movmean', 10); % 平滑 y 数据
% 确保 smooth_x, smooth_y, s 均为列向量
smooth_x = smooth_x(:);
smooth_y = smooth_y(:);
s = s(:);

% 计算一阶导数 dx_ds 和 dy_ds
dx_ds = diff(smooth_x) ./ diff(s);
dy_ds = diff(smooth_y) ./ diff(s);

% 计算二阶导数 ddx_ds2 和 ddy_ds2
ddx_ds2 = diff(dx_ds) ./ diff(s(1:end-1)); % 保证 s 的长度匹配
ddy_ds2 = diff(dy_ds) ./ diff(s(1:end-1)); % 同上

% 计算曲率
curvature_arclength = abs(dx_ds(1:end-1) .* ddy_ds2 - dy_ds(1:end-1) .* ddx_ds2) ./ ...
                      ((dx_ds(1:end-1).^2 + dy_ds(1:end-1).^2).^(3/2));
% gradient 在计算不规则数据的二阶导数时可能会导致误差,使用 diff 辅助计算高阶导数,确保结果平滑、稳定                  
% 注意:diff 操作返回的数组长度比原数组短1,因此,一阶导数 dx_ds 和 dy_ds 的长度比原数据少 1,二阶导数 ddx_ds2 和 ddy_ds2 比一阶导数少 1,
% 因此曲率计算时使用 1:end-1 来对齐长度。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 时间参数化法曲率计算+平滑
windowSize = 5;  
xunizhuang_x_smooth = movmean(xunizhuang_x, windowSize);
xunizhuang_y_smooth = movmean(xunizhuang_y, windowSize);
dx = gradient(xunizhuang_x_smooth) ./ gradient(xunizhuang_t);
dy = gradient(xunizhuang_y_smooth) ./ gradient(xunizhuang_t);
ddx = gradient(dx) ./ gradient(xunizhuang_t);
ddy = gradient(dy) ./ gradient(xunizhuang_t);
curvature_time_smooth = abs(dx .* ddy - dy .* ddx) ./ ((dx.^2 + dy.^2).^(3/2));

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 样条拟合法曲率计算 
% 使用 csape 对 x 和 y 进行单独的三次样条拟合
spline_fit_x = csape(xunizhuang_t, xunizhuang_x, 'not-a-knot');
spline_fit_y = csape(xunizhuang_t, xunizhuang_y, 'not-a-knot');

% 计算一阶和二阶导数
dx_ds_spline = fnder(spline_fit_x, 1); % x 的一阶导数
dy_ds_spline = fnder(spline_fit_y, 1); % y 的一阶导数
ddx_ds2_spline = fnder(spline_fit_x, 2); % x 的二阶导数
ddy_ds2_spline = fnder(spline_fit_y, 2); % y 的二阶导数

% 计算并平滑导数
dx_ds = smoothdata(ppval(dx_ds_spline, xunizhuang_t), 'movmean', 5);
dy_ds = smoothdata(ppval(dy_ds_spline, xunizhuang_t), 'movmean', 5);
ddx_ds2 = smoothdata(ppval(ddx_ds2_spline, xunizhuang_t), 'movmean', 5);
ddy_ds2 = smoothdata(ppval(ddy_ds2_spline, xunizhuang_t), 'movmean', 5);

% 计算曲率
curvature_spline = abs(dx_ds .* ddy_ds2 - dy_ds .* ddx_ds2) ./ ...
                   ((dx_ds.^2 + dy_ds.^2).^(3/2));



% 绘制曲率结果并添加标签
figure;
plot(1:length(curvature_time_smooth), curvature_time_smooth, 'LineWidth', 1.5, 'DisplayName', '时间平滑法');
hold on;
plot(1:length(curvature_3pt), curvature_3pt, 'LineWidth', 1.5, 'DisplayName', '三点法');
plot(1:length(curvature_polyfit), curvature_polyfit, 'LineWidth', 1.5, 'DisplayName', '二次多项式拟合法');
plot(1:length(curvature_arclength), curvature_arclength, 'LineWidth', 1.5, 'DisplayName', '弧长法');
plot(1:length(curvature_spline), curvature_spline, 'LineWidth', 1.5, 'DisplayName', '样条拟合法');
hold off;

% 设置图形属性 
title('道路曲率计算比较');
xlabel('虚拟桩序号');
ylabel('对应虚拟桩下的道路曲率');
legend show;
grid on;

方法优点缺点适用情况
三点公式法计算简单、快速,适用于局部曲率计算 对曲线变化大时,结果可能不准确 样本间隔均匀的路径数据
时间参数化法 适用于不规则路径,灵活性强,适合处理带有时间序列的数据 需考虑时间参数化的适用性,计算较复杂 实际行驶轨迹或时间序列数据
多项式拟合法 适合光滑且噪声较低的数据,提供全局特征 高阶多项式易导致过拟合,计算复杂 平滑且连续的数据
弧长参数化法 适合任意路径,考虑了曲线的整体形状, 能较好地处理复杂曲线 计算相对复杂,需保证数据的平滑性 各类路径数据,尤其是不规则数据
离散曲线拟合法 对复杂、噪声较多的数据拟合效果显著, 提供光滑曲线,便于后续计算 计算开销较大,需要合理选择拟合参数 非常复杂或不规则的曲线

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:/a/899604.html

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

sentinel原理源码分析系列(八)-熔断

限流为了防止过度使用资源造成系统不稳,熔断是为了识别出”坏”资源,避免好的资源受牵连(雪崩效应),是保证系统稳定性的关键,也是资源有效使用的关键,sentinel熔断插槽名称Degrade(降级),本人觉得应该改为熔…

怎么提取pdf的某一页?批量提取pdf的某一页的简单方法

怎么提取pdf的某一页?在日常工作与学习中,我们经常会遇到各式各样的PDF文件,它们以其良好的兼容性和稳定性,成为了信息传输和存储的首选格式。然而,在浩瀚的文档海洋中,有时某个PDF文件中的某一页内容尤为重…

一篇文章进阶MySQL数据库

一,MySQL数据库体系结构 层级说明连接层主要完成一些类似于连接处理,授权认证,及相关的安全方案。服务器也会为安全接入的每个客户端验证它所具有的操作权限服务层完成大多数的核心服务功能,如SQL接口,并完成缓存的查询…

使用 Pake 一键打包网页为桌面应用 / 客户端

项目 项目:https://github.com/tw93/Pake/ 免费ICO图片:https://icon-icons.com/zh/ 设置环境 以下教程仅针对windows系统适用 请确保您的 Node.js 版本为 18 或更高版本 文档:https://v1.tauri.app/zh-cn/v1/guides/getting-started/prerequ…

java小游戏实战-星空大战(接口、继承、多态等多种方法)

环境:Windows系统Eclipse/idea、jdk8 1.创建英雄类 2.创建飞机类 3.创建敌人接口 package com.plane;public interface Enemy { /* *得分的方法 */ public int getScore(); } 4.创建小蜜蜂类 5.创建奖励接口 package com.plane;public interface Award {public …

【Linux笔记】Linux命令与使用

博文将不断学习补充 学习参考博文: Linux命令大全:掌握常用命令,轻松使用Linux操作系统-CSDN博客 文件或目录操作命令 zip # zip是使用最多的文档压缩格式 # 方便跨平台使用,但是压缩率不是很高 zip指令未安装 安装zip yum ins…

深度学习相关知识点

文章目录 epoch/batch/batch_size的关系dense visual predictionlogits epoch/batch/batch_size的关系 Epoch:模型在整个数据集上完成一次训练。一个epoch后,模型已经看过所有的训练数据,执行了正向传播和反向传播。通常训练需要多个epoch&a…

【C#】搭建环境之CSharp+OpenCV

在我们使用C#编程中,对图片处理时会用到OpenCV库,以及其他视觉厂商提供的封装库,这里因为OpenCV是开源库,所以在VS资源里可以直接安装使用,这里简单说明一下搭建的步骤及实现效果,留存。 1. 项目创建 1.1…

055_基于python摄影平台交流系统

目录 系统展示 开发背景 代码实现 项目案例 获取源码 博主介绍:CodeMentor毕业设计领航者、全网关注者30W群落,InfoQ特邀专栏作家、技术博客领航者、InfoQ新星培育计划导师、Web开发领域杰出贡献者,博客领航之星、开发者头条/腾讯云/AW…

【笔试题】字节秋招笔试 TODO

🔗 参考地址 亮灭 🔗 亮灭 🎉 模拟 import java.util.Scanner;public class Main {// 亮灯数组:a[1][2][3] 表示 数字1的第2行第3列,1 表示亮static int[][][] a new int[10][10][10];public static void main(Str…

python机器人编程——用python调用API控制wifi小车的实例程序

目录 一、前言二、一个客户端的简单实现2.1 首先定义一个类及属性2.2 其次定义连接方法2.3 定义一些回调函数2.4 定义发送小车指令方法2.5 定义一个正常关闭方法 三、python编程控制小车的demo实现四、小结PS.扩展阅读ps1.六自由度机器人相关文章资源ps2.四轴机器相关文章资源p…

从0开始linux(19)——如何写一个linux环境下运行的shell程序

欢迎来到博主的专栏:从0开始Linux 博主ID:代码小豪 文章目录 bashmyshell源码 bash 什么?我写bash?bash作为一个大型的shell程序,甚至已经成为一种语言。博主当然没能力复刻。 博主这里写了一个仿bash的shell程序。主…

Linux:文件系统基础命令扫盲

目录 查看目录下的文件 创建目录文件 删除目录文件 打印当前工作目录 切换工作目录 删除文件 复制文件或目录 移动文件或目录 创建文件 🚀主页:R6bandito_ ✈往期:《Linux与Windows文件共享》 查看目录下的文件 命令:ls …

2024年【流动式起重机司机】考试技巧及流动式起重机司机模拟考试题库

题库来源:安全生产模拟考试一点通公众号小程序 流动式起重机司机考试技巧是安全生产模拟考试一点通生成的,流动式起重机司机证模拟考试题库是根据流动式起重机司机最新版教材汇编出流动式起重机司机仿真模拟考试。2024年【流动式起重机司机】考试技巧及…

正确的功能可将热晶体管风速计线性化

处理传感器电路输出信号的电路或计算公式必须生成传感器响应的反函数。例如,如果传感器响应是对数函数,则线性化部分的响应必须是指数的。 这项工作首先获取传感器响应的 46 个离散点(参见参考论文中的图 4)。刚开始时&#xff0…

若依前后分离版集成积木报表进行token传递

若依分离板集成积木报表就不说了需要的请移步:若依前后分离版集成积木报表-CSDN博客 考虑到前端摸鱼不干活,所以一般都是前后端都干,我这里前后端都搞上,你们直接抄,抄完接着去摸鱼,代码不美观,轻喷 一、…

【景观生态学实验】实验一 ArcGIS地理数据处理及制图基础

实验目的 1.掌握ArcGIS软件基本操作:通过实验操作与学习,熟练掌握ArcGIS软件相关的基本操作,包括界面熟悉、工具栏使用、数据的加载和保存、基本数据处理操作等; 2.掌握如何使用ArcGIS进行影像拼接及裁剪:通过实验操作与学习&am…

ABAP SMARTFORMS(2)

1、表单接口 方法一:导入结构、内表,给全局定义传入结构体 方法二:只关联表,不关联结构,给全局定义传入结构体 GW_XYXX存的表头信息 GW_XYKQ存考勤信息,表中的每一行 初始化学员信息表的第一条数据作为表头 2、创建表头模板 该…

x-cmd mod | x sd - 搭配 fzf 实时预览文本替换效果,打造更直观高效的编辑体验

目录 介绍子命令使用案例 介绍 sd(search & displace)是一种查找和替换文本工具,使用常见的正则表达式语法,类似于 sed,但专注于替换操作,从而使用起来更直观、更易读。 该模块主要通过 fzf 以交互方式…

单片机STC8H8K64U开发板_RA6809开发板 驱动彩屏显示

单片机STC8H8K64U开发板,型号RT8H8K001 预留Type C接口,可供电SWD下载: RA6809开发板,型号RT6809CNN01 预留Type C接口供电,预留MCU接口、电容触摸屏接口、液晶屏接口: 双臂合一,驱动和控…