Matlab中计算道路曲率的几种方法

我使用Prescan采集到的道路中心线数据，都是离散点（x，y，z），但在作研究时，通常都是道路曲率，这时需要将离散点坐标转换为曲率，但通过计算得到的曲率与实际曲率有一些误差，以下是几种方法。

注意：这里代码中的xunizhuang_x和xunizhuang_y是我在道路中心线上每隔两米设置的虚拟桩，可以直接替换成你自己的道路中心线坐标（但离散点的疏密程度可能会影响到结果）

1. 三点公式法

这种方法使用连续三个点的坐标来计算曲率，可以较为简单地应用于离散点数据，如样本间隔均匀的路径数据。

假设有三个连续的点，曲率 K 可以近似计算为：

matlab代码如下：

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代码如下：

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代码如下：

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代码如下：

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代码如下：

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代码如下：

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代码如下：

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 = [];

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
% % % % % % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 二次多项式拟合曲率计算
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;

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