%% 1. 创建三维轨迹点
theta = linspace(0, 6*pi, 30);
points = [cos(theta')*2, sin(theta')*2, theta'/5];
%% 2. 弦长参数化(归一化)
diffs = diff(points);
distances = sqrt(sum(diffs.^2, 2));
t_original = [0; cumsum(distances)];
t_original = t_original / t_original(end); % 归一化到[0,1]
%% 3. 创建三次B样条
spline_x = csape(t_original, points(:,1)', 'second');
spline_y = csape(t_original, points(:,2)', 'second');
spline_z = csape(t_original, points(:,3)', 'second');
%% 4. 计算总弧长(关键修改)
% 在密集点上采样计算总弧长
t_temp = linspace(0, 1, 1000);
pts_temp = [fnval(spline_x, t_temp); fnval(spline_y, t_temp); fnval(spline_z, t_temp)]';
diffs_temp = diff(pts_temp);
total_length = sum(sqrt(sum(diffs_temp.^2, 2)));
%% 5. 生成等距插值点(核心算法)
num_points = 200; % 目标点数
target_spacing = total_length / (num_points-1); % 目标间距
% 初始化
t_equal = zeros(1, num_points);
t_equal(1) = 0; % 起点参数
current_length = 0;
interpolated_points = zeros(num_points, 3);
interpolated_points(1,:) = points(1,:); % 起点
% 迭代计算等距点参数
for i = 2:num_points
% 牛顿迭代法求解参数
t_guess = t_equal(i-1) + 0.01; % 初始猜测
for iter = 1:5 % 5次迭代通常足够
% 计算当前位置
pt_current = [fnval(spline_x, t_guess);
fnval(spline_y, t_guess);
fnval(spline_z, t_guess)]';
% 计算与前一点的距离
segment_vec = pt_current - interpolated_points(i-1,:);
current_dist = norm(segment_vec);
% 计算导数(速度向量)
deriv_x = fnval(fnder(spline_x,1), t_guess);
deriv_y = fnval(fnder(spline_y,1), t_guess);
deriv_z = fnval(fnder(spline_z,1), t_guess);
speed = norm([deriv_x, deriv_y, deriv_z]);
% 牛顿法更新参数
t_guess = t_guess - (current_dist - target_spacing)/speed;
end
t_equal(i) = t_guess;
interpolated_points(i,:) = pt_current;
end
%% 6. 验证等距性(可选)
diffs_final = diff(interpolated_points);
distances_final = sqrt(sum(diffs_final.^2, 2));
fprintf('平均间距: %.4f\n标准差: %.4f\n最大偏差: %.4f\n', ...
mean(distances_final), std(distances_final), max(abs(distances_final - target_spacing)));
%% 7. 可视化
figure;
plot3(points(:,1), points(:,2), points(:,3), 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r');
hold on;
plot3(interpolated_points(:,1), interpolated_points(:,2), interpolated_points(:,3), 'b-o', 'LineWidth', 1.5);
title('等距B样条插值轨迹');
grid on; axis equal;
legend('原始点', '等距插值点', 'Location', 'best');
%% 8. 输出插值点
csvwrite('equidistant_points.csv', interpolated_points); 转换成使用C++代码实现