简介

由于需要计算车辆行驶轨迹的曲率，但由于车辆的车速不同，每次采集的轨迹点非等间隔，这里借鉴了诸多大神的经验，主要参考了下面的思路才得到最后结果。

参考：link.

代码

function Curvature = ThreePoint2Curvature(x,y,n)x = reshape(x,3,1);y = reshape(y,3,1);t_a = norm([x(2)-x(1),y(2)-y(1)]);t_b = norm([x(3)-x(2),y(3)-y(2)]);M = [1, -t_a, t_a^2;1,    0,     0;1,  t_b, t_b^2;];a = M\x;b = M\y;xd1 = a(2)+2*a(3)*[-t_a,0,t_b];xd2 = 2*a(3);yd1 = b(2)+2*b(3)*[-t_a,0,t_b];yd2 = 2*b(3);Curvature = (xd1(n)*yd2 - xd2*yd1(n))./((xd1(n).^2+yd1(n).^2).^(3/2));%k = (x"y'-x'y")/(((x')^2+(y')^2).^(3/2))
end

clear;clc;close all;
X_vehicle = randperm(8000,1000)/1000;
X_vehicle = sort(X_vehicle);
Y_vehicle = sin(X_vehicle).*cos(X_vehicle);
figure,
plot(X_vehicle,Y_vehicle);
%%
Curvature = zeros(size(X_vehicle));
Curvature(1)= ThreePoint2Curvature(X_vehicle(1:3),Y_vehicle(1:3),1);
for i = 2:length(X_vehicle)-1Curvature(i) = ThreePoint2Curvature(X_vehicle(i-1:i+1),Y_vehicle(i-1:i+1),2);
end
Curvature(end)= ThreePoint2Curvature(X_vehicle(end-2:end),Y_vehicle(end-2:end),3);
%%
xd1 = diff(X_vehicle);
xd2 = diff((X_vehicle(1:end-1)+X_vehicle(2:end))/2);
yd1 = diff(Y_vehicle)./xd1;
yd2 = diff(yd1)./xd2;
yd1 = [yd1(1) yd1];
yd2 = [yd2(1) yd2 yd2(end)];
Curvature2 = yd2./((1+yd1.^2).^(3/2));
%%
figure,
plot(1000*X_vehicle,Curvature);
hold on,
plot(1000*X_vehicle,Curvature2);
title('曲率');

结果

计算非等间隔离散曲线的曲率相关推荐

曲线积分与曲面积分总结_曲线、曲率、挠率以及离散曲线
曲线曲线,听起来很简单,看起来也很直观,但是如果我们看一下维基百科上关于曲线的定义: 设 I=[a,b]为一实数区间,即实数集的非空子集,那么曲线c 就是一个连续函数c : I → X 的映像,其中 ...
计算nurbs曲率NURBS曲线的曲率计算
计算nurbs曲率NURBS曲线的曲率计算 https://download.csdn.net/download/yishang44/10383324 NURBS曲线的计算和求导 ,主要看的书籍是&l ...
NURBS曲线的曲率计算
NURBS曲线的曲率计算这两天做课题需要用到NURBS曲线,仔细探究了NURBS曲线的计算和求导等,主要看的书籍是<The NURBS book>,代码主要参考nurbs工具箱-M语言, ...
光滑曲线_微分几何笔记(4) —— 二维三维空间中曲线的曲率以及环绕数
本篇文章我们从一般化的空间回到我们生活的空间,看看低维空间中的曲线有哪些性质,主要计算下在非弧长参数下的曲线,曲率挠率的一般表达式. 最后引入环绕数的概念,讲讲怎么数曲线转了多少圈. 4.1 ...
微分几何笔记(4) —— 二维三维空间中曲线的曲率以及环绕数
本篇文章我们从一般化的 Rn\mathbb{R}^nRn 空间回到我们生活的 R2,R3\mathbb{R}^2,\mathbb{R}^3R2,R3空间,看看低维空间中的曲线有哪些性质,主要计算下在非 ...
Hartley-Judd计算非均匀B样条节点矢量
Hartley-Judd计算非均匀B样条节点矢量在前面的文章介绍了用里森菲尔德法分别计算奇次和偶次B样条节点矢量,但是这种方法与实际情况存在出入,Hartley-Judd计算法与曲线的次数无关,可以 ...
pandas最大的时间间隔_pandas计算最大连续间隔的方法
pandas计算最大连续间隔的方法如下所示: 群里一朋友发了一个如上图的问题,解决方法如下 data = {'a':[1,1,1,1,1,1,1,1,2,2,2,2,2,2,2,2,2],'b':[ ...
光滑曲线_曲线的曲率
曲率(curvature)就是针对曲线上某个点的切线方向角对弧长的转动率, 是通过微分来定义的,表明曲线偏离直线的程度. 数学上表明曲线在某一点的弯曲程度的数值. 曲率越大,表示曲线的弯曲程度越大.曲 ...
Oracle 计算两日期间隔月数
Oracle 计算两日期之间月数实际应用中,有时候会计算两日期间隔天数.月数.如直接相减,可能会出问题,如: select 202110-202106 from dual; 结果为:4 看似正确,但 ...
高等数学笔记-乐经良老师-第四章-微分中值定理和导数的应用-第五节-曲线的曲率
高等数学笔记-乐经良第四章微分中值定理和导数的应用第五节曲线的曲率一.弧长和弧微分弧长曲线内接折线长度的极限 ( 组成折线的线段长 → 0 \rightarrow 0 →0 ) 设曲 ...

计算非等间隔离散曲线的曲率

计算非等间隔离散曲线的曲率

简介

参考：link.

代码

结果

计算非等间隔离散曲线的曲率相关推荐

最新文章

热门文章