计算非等间隔离散曲线的曲率简介参考：link.代码结果

121 阅读 0 评论 80 点赞

我是靠谱客的博主时尚斑马，这篇文章主要介绍计算非等间隔离散曲线的曲率简介参考：link.代码结果，现在分享给大家，希望可以做个参考。

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

简介
参考：[link](https://zhuanlan.zhihu.com/p/72083902/).
代码
结果

简介

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

参考：link.

代码

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
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

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
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('曲率');