运动包络面-机器人

包络面绘制程序
function [X,Y,Z]=Enveloping_surface_rendering(x,y,z,a,b,A1)
s=A1(:,3); 
b=[find(diff(s));length(s)];
c=b-[0;b(1:end-1)];
out=mat2cell(A1,c);
ct=[]; 
rd=[]; 
for k=1:length(out)
x=out{k}(:,1); y=out{k}(:,2); z=out{k}(:,3);
xg=mean(x); yg=mean(y); zg=z(1); 
dist=sqrt((x-xg).^2+(y-yg).^2);
if length(x)~=1, rho=max(dist); end 
ct=[ct; xg yg zg];
rd=[rd;rho];
end
cp=spline(1:length(ct),ct‘,1:0.2:length(ct))‘; %圆心轨迹样条插值
rp=spline(1:length(rd),rd‘,1:0.2:length(rd))‘; 
t=0:pi/18:2*pi;
X=[]; Y=[]; Z=[];
for i=1:length(rp)
X=[X;cp(i,1)+rp(i)*cos(t)];
Y=[Y;cp(i,2)+rp(i)*sin(t)];
end
Z=repmat(cp(:,3),1,length(t));

end