路径已经找到!
函数或变量 'D' 无法识别。
出错 fivespline (第 52 行)
P(i,:) = N * D(idx_start:idx_start+k, :);
出错 mainrrt (第 63 行)
new_Path = fivespline(mypos_iter, rSave, circleCenterSave, source, goal); % 调用修改后的五次B样条函数
new_Path = fivespline(mypos_iter, rSave, circleCenterSave, source, goal);
% 后续逆运动学计算保持不变
n = length(new_Path);
a=zeros(4,4,n);
for num1=1:n
x_taget = new_Path(n+1-num1,1);
y_taget = new_Path(n+1-num1,2);
z_taget = new_Path(n+1-num1,3);
a(1,1,num1)=1;
a(2,2,num1)=-1;
a(3,3,num1)=1;
a(1,4,num1)=x_taget;
a(2,4,num1)=y_taget;
a(3,4,num1)=z_taget;
a(4,4,num1)=1;
end
q=PUMA.ikunc(a);
figure(2)
PUMA.plot(q);
figure(3)
plot(q);
legend('关节1','关节2','关节3','关节4','关节5','关节6');
xlabel('t/s');
ylabel('关节值'); function P = fivespline(Pp, radius, circleCenter, start, goal)
pt = Pp;
% 可视化部分保持不变...
[row, column] = size(pt);
n = size(pt, 1); % 型值点数量
k = 5; % 五次样条
% === 1. 参数检查 ===
if n < 6
error('五次B样条需要至少6个型值点');
end
% === 2. 积累弦长参数化 ===
if column == 2 % 2D
dists = sqrt(diff(pt(:,1)).^2 + diff(pt(:,2)).^2);
else % 3D
dists = sqrt(diff(pt(:,1)).^2 + diff(pt(:,2)).^2 + diff(pt(:,3)).^2);
end
total_length = sum(dists);
cum_dists = [0; cumsum(dists)/total_length]; % 归一化累积距离
% === 3. 构建节点矢量 ===
U = zeros(1, n + k + 1); % 节点矢量长度 = n + k + 1
U(1:k+1) = 0; % 起始重复度 k+1
U(end-k:end) = 1; % 结束重复度 k+1
% 内节点赋值(关键修正)
num_inner_nodes = n - k; % 需要的内节点数量
if num_inner_nodes > 0
% 从累积距离中提取内节点(避开首尾点)
inner_indices = round(linspace(2, numel(cum_dists)-1, num_inner_nodes));
U(k+2:k+1+num_inner_nodes) = cum_dists(inner_indices);
end
% 确保节点矢量单调递增
U = sort(U);
% === 4. 反算控制点 ===
% ...(保持之前提供的控制点计算代码不变)...
% === 5. 生成曲线 ===
dt = 0.01;
t = 0:dt:1;
P = zeros(numel(t), column);
for i = 1:numel(t)
u = t(i);
span = find_span(u, U, n+k-1, k); % 找到节点区间
N = basis_funs(span, u, k, U); % 计算基函数
idx_start = span - k;
P(i,:) = N * D(idx_start:idx_start+k, :);
end
% 可视化部分保持不变...
end
% 辅助函数:find_span(保持不变)
function span = find_span(u, U, n, p)
if u >= U(n+2), span = n+1; return; end
if u <= U(p+1), span = p+1; return; end
low = p+1; high = n+2;
while high > low + 1
mid = floor((low+high)/2);
if u < U(mid)
high = mid;
else
low = mid;
end
end
span = low;
end
% 辅助函数:basis_funs(保持不变)
function N = basis_funs(span, u, p, U)
N = zeros(1, p+1);
left = zeros(1, p+1);
right = zeros(1, p+1);
N(1) = 1;
for j = 1:p
left(j+1) = u - U(span+1-j);
right(j+1) = U(span+j) - u;
saved = 0;
for r = 0:j-1
temp = N(r+1)/(right(r+2) + left(j-r+1));
N(r+1) = saved + right(r+2)*temp;
saved = left(j-r+1)*temp;
end
N(j+1) = saved;
end
end