简介

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

参考：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 = Mx;
b = My;
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)-1
Curvature(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('曲率');

结果

最后

以上就是时尚斑马为你收集整理的计算非等间隔离散曲线的曲率简介参考：link.代码结果的全部内容，希望文章能够帮你解决计算非等间隔离散曲线的曲率简介参考：link.代码结果所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。