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