%均勻三次B樣條曲線插值

V?=?load（‘con_point‘）;%控制點坐標
V?=?V.‘;
n?=?length（V）;%控制點個數

%---畫出圖形---
hold?on
%控制多邊形，藍色，線
%plot（?V（:1）?V（:2）?‘black‘?）;

%V（:2）=V（:2）/180*pi;?%轉化成弧度

%畫出三次B樣條曲線，角度位移
for?i=1:（n-3）;?%n-3段
????u=0:0.1:1;?%?參數u；

????k0=1.0./6.*（1-u）.^3;?%?基函數k0；
????k1=1.0./6.*（3.*u.^3-6.*u.^2+4）;?%?基函數k1；
????k2=1.0./6.*（-3.*u.^3+3.*u.^2+3.*u+1）;?%?基函數k2；
????k3=1.0./6.*u.^3;?%?基函數k3；

????t=k0.*V（i1）+k1.*V（i+11）+k2.*V（i+21）+k3.*V（i+31）;?%?橫坐標時間t；
????dis=k0.*V（i2）+k1.*V（i+12）+k2.*V（i+22）+k3.*V（i+32）;?%?縱坐標角度位移dis；

????if?i==1
????????temp?=?t（1）;
????end
????
????t（:）?=?t（:）?-?temp;%調整時間從0開始
????
????cnt（i1）?=?t（1）;
????cnt（i2）?=?dis（1）;??
????if?i==（n-3）
????????cnt（i+11）?=?t（11）;
????????cnt（i+12）?=?dis（11）;
????end
????
????a1?=?-0.5.*V（i2）+1.5.*V（i+12）-1.5.*V（i+22）+0.5.*V（i+32）;
????a2?=?V（i2）-2.*V（i+12）+V（i+22）;
????a3?=?0.5.*V（i+22）-0.5*V（i2）;
????b1?=?-0.5.*V（i1）+1.5.*V（i+11）-1.5.*V（i+21）+0.5.*V（i+31）;
????b2?=?V（i1）-2.*V（i+11）+V（i+21）;
????b3?=?0.5.*V（i+21）-0.5*V（i1）;
????c1?=?2?*?b1;
????c2?=?b2;
????d1?=?2?*?a1;
????d2?=?a2;
????r1?=?（d1.*u+d2）.*（b1.*u.^2+b2.*u+b3）?-?（c1*u+c2）.*（a1*u.^2+a2*u+a3）;
????r2?=?（?b1*u.^2+b2*u+b3?）.^3;
????r3?=?d1*（?b1*u.^2+b2*u+b3?）.^2?-?3*（d1*u+d2）.*（c1*u+c2）.*（b1*u.^2+b2*u+b3）?+?3*（a1*u.^2+a2*u+a3）.*（?（c1*u+c2）.^2?）?-?（a1*u.^2+a2*u+a3）.*（b1*u.^2+b2*u+b3）*c1;
????r4?=?（?b1*u.^2+b2*u+b3?）.^5;
????
????vel?=?（a1.*u.^2+a2.*u+a3）?./?（b1.*u.^2+b2.*u+b3）;%速度vel
????acc?=?r1./r2;%加速度acc
????jer?=?r3./r4;%加加速度jer
????
????if?i>1
????????x（2）?=?t（1）;
????????y（2）?=?jer（1）;
????????plot（xy‘:b‘）;
????end
????x（1）?=?t（11）;
????y（1）?=?jer（11）;
????
????plot（tdis?tvel?tacc?tjer‘b‘）;
end

%legend（‘位移‘‘速度‘‘加速度‘‘加加速度‘）;

plot（?cnt（:1）?cnt（:2）?‘r.‘?）;

xlabel（‘時間/s‘）;
title（‘關節6‘）;

gtext（‘\leftarrow?位移‘）;
gtext（‘\leftarrow?速度‘）;
gtext（‘\leftarrow?加速度‘）;
gtext（‘\leftarrow?加加速度‘）;

hold?off;

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

-----------?---------??----------?-----??----

?????????????????2293????????????????????3