清空筛选条件
清空筛选条件

How to put a spline through points representing a path

13 次查看（过去 30 天）
Amritz Ansara
Amritz Ansara 2021-3-23
评论： darova 2021-3-24
I have this path here. The waypoints are marked in red. The line going through each point (linearly) is in blue. The path was made programmatically - if you'd like the code to make the path it is:
%% Create Path
%have it come from the south going north
r = 15;
start_pos = [-r * 7 0 0];
%initial approach to curve
waypoints = [start_pos;
-r*5 1.9 -0;
-r*3 1.7 0;
-r*2.5 1.8 0;
-r*2 2.4 0;
-r*1.5 3.4 0;
-r*1 3.7 0;
-r*0.5 3.4 0];
%Set constant radius curve
angle_req = 90;
a = linspace(0, angle_req*pi/180, 10);
round_x = r * sin(a) + waypoints(end, 1);
round_y = r * cos(a) - r + waypoints(end, 2);
for i = 1:length(a)
waypoints = [waypoints; round_x(i) round_y(i) 0];
end
%Set exit
last_point = waypoints(end, :);
waypoints = [waypoints;
last_point(1) + r*1*cos(a(end)) last_point(2) - r*1*sin(a(end)) 0;
last_point(1) + r*2.5*cos(a(end)) last_point(2) - r*2.5*sin(a(end)) 0;
last_point(1) + max(r*5, 100)*cos(a(end)) last_point(2) - max(r*5, 100)*sin(a(end)), 0];
%last_point(1) + r*1*cos(a(end))- 0.2*r*sin(a(end)) last_point(2) - r*1*sin(a(end)) 0;
%last_point(1) + r*2.5*cos(a(end)) - 0.5*r*sin(a(end)) last_point(2) - r*2.5*sin(a(end)) 0;
%last_point(1) + max(r*5, 100)*cos(a(end)) - 0.5*r*sin(a(end)) last_point(2) - max(r*5, 100)*sin(a(end)), 0];
%plot
plot(waypoints(:, 2), waypoints(:, 1), 'LineWidth', 5)
hold on;
plot(waypoints(:, 2), waypoints(:, 1), 'ro', 'LineWidth', 3)
hold off;
st = sprintf("A %d degree turn (anticlockwise) of %d metre radius", angle_req, r);
grid on;
title(st)
ylabel('X Distance (m)'); xlabel('Y Distance(m)')
I am worried about the fact that there could be discontinuous curvature, when I put the waypoints into the driving scenario and draw up the vehicle's trajectory.
I would like to put a smooth spline through these points (which will guarantee a curve with continuous curvature) and then resample my waypoints from there. How do I do this, considering most spline fitting tools rely on a one-to-one correspondence between x-values and y-values? I've tried the curve fitting toolbox but it sends me an error relating to this correspondence.
  2 个评论
Bruno Luong
Bruno Luong 2021-3-24
Cubic spline provides a C^2 interpolation result, meaning the second derivatve is continuous (in fact continuity is enrured for all derivative of order <= 2). So the curvature is continuous, it might change quickly but it is contnuous.

请先登录，再进行评论。

请先登录，再回答此问题。

采纳的回答

Bruno Luong
Bruno Luong 2021-3-24
编辑：Bruno Luong 2021-3-24
Put this next-below to your code
waypoints=unique(waypoints,'rows','stable'); % <- this is required because your example as duplicated points
% Spline interpolation
t=cumsum([0;sqrt(sum(diff(waypoints,1,1).^2,2))]);
ti=linspace(t(1),t(end),512);
xyzi=interp1(t,waypoints,ti,'spline');
%plot
figure
plot(waypoints(:,2),waypoints(:,1),'or',xyzi(:,2),xyzi(:,1),'b');
axis equal
You don't have to think what kind of angle to turn and it can work for multiple winding path.

更多回答（1 个）

darova
darova 2021-3-24
What about rotation?
R = @(a)[cosd(a) -sind(a);sind(a) cosd(a)]; % rotation maitrx
v1 = R(45)*[x(:) y(:)]'; % rotate data to make exclusive X coordinate
x1 = linspace(v1(1,1),v1(1,end),100); % denser mesh
y1 = spline(v1(1,:),v1(2,:),x1); % interpolation with spline
v2 = R(-45)*[x1(:) y1(:)]'; % rotate to original position

类别

在 Help Center 和 File Exchange 中查找有关 Vehicle Scenarios 的更多信息

标签

产品


版本

R2020b

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by