一、代码
废话不多说直接上
代码:
data_x1=[1
3
6
2
8]; %放x坐标
data_x2=[7
5
3
4.5
1]; %放y坐标
X=[data_x1,data_x2];
[n,m]=size(X);
k=5;
for i=1:k+1
u(i)=0;
u(n-1+k+i)=1;
end
u(k+1)=0;
L=0;
for i=1:n-1
L=abs((X(i+1,1)-X(i,1)))+L;
end
for i=k+2:k+n-1
u(i)=u(i-1)+(abs(X(i-k,1)-X(i-k-1,1)))/(L);
end
A=zeros(n+k-1);
A(1,1)=1;A(n,n+k-1)=1;
for i=2:n-1
for j=i:i+4
A(i,j)=Bbase(j,k,u,u(i+5));
end
end
tall=1;
v01=-5/(u(7)-u(2))/tall;
v02=5/(u(7)-u(2))/tall;
ve1=-5/(u(n+9)-u(n+4))/tall;
ve2=5/(u(n+9)-u(n+4))/tall;
a01=20/((u(7)-u(3))*(u(7)-u(2)))/tall^2;
a02=-(20/((u(7)-u(3))*(u(8)-u(3)))+20/((u(7)-u(3))*(u(7)-u(2))))/tall^2;
a03=20/((u(7)-u(3))*(u(8)-u(3)))/tall^2;
ae1=20/((u(n+8)-u(n+4))*(u(n+8)-u(n+3)))/tall^2;
ae2=-(20/((u(n+8)-u(n+4))*(u(n+9)-u(n+4)))+20/((u(n+8)-u(n+4))*(u(n+8)-u(n+3))))/tall^2;
ae3=20/((u(n+8)-u(n+4))*(u(n+9)-u(n+4)))/tall^2;
A(n+1,1)=v01;
A(n+1,2)=v02;
A(n+2,n+k-2)=ve1;
A(n+2,n+k-1)=ve2;
A(n+3,1)=a01;
A(n+3,2)=a02;
A(n+3,3)=a03;
A(n+4,n+k-3)=ae1;
A(n+4,n+k-2)=ae2;
A(n+4,n+k-1)=ae3;
ttall=X(end,1)-X(1,1);
clear e;
e=0;
dt_v_start=0;
dt_v_end=0;
dt_a_start=0;
dt_a_end=0;
e(n+k-4,1)=10;
e(n+k-4,2)=dt_v_start*ttall;
e(n+k-3,1)=data_x1(n);
e(n+k-3,2)=dt_v_end*ttall;
e(n+k-2,1)=0;
e(n+k-2,2)=dt_a_start*ttall^2;
e(n+k-1,1)=data_x1(n);
e(n+k-1,2)=dt_a_end*ttall^2;
for i=1:n
e(i,:)=X(i,:);
end
d=inv(A)*e;
t=0;angle=0;velocity=0;acceleration=0;jerk=0;
x=0;y=0;down=0;
for j=1:n-1
uuu=u(j+5):0.001:u(j+6);
Len=(X(j+1,1)-X(j,1))/(length(uuu)-1);
xx=X(j,1):Len:X(j+1,1);
if t==0
t=[xx];
else
t=[t,xx];
end
for uu=u(j+5):0.001:u(j+6)
down=down+1;
x=Bbase(j,5,u,uu)*d(j,:)+Bbase(j+1,5,u,uu)*d(j+1,:)+Bbase(j+2,5,u,uu)*d(j+2,:)+Bbase(j+3,5,u,uu)*d(j+3,:)+Bbase(j+4,5,u,uu)*d(j+4,:)+Bbase(j+5,5,u,uu)*d(j+5,:);
angle(down)=x(2);
end
end
plot(t,angle,'r-','linewidth',1.5);
代码中的Bbase包
function result = Bbase(i,k,u,t)
if k==0
if ((u(i)<=t && t<u(i+1)))
result=1;
return;
else
result=0;
return;
end
end
if u(i+k)-u(i)==0
alpha=0;
else
alpha=(t-u(i))/(u(i+k)-u(i));
end
if u(i+k+1)-u(i+1)==0
beta=0;
else
beta=(u(i+k+1)-t)/(u(i+k+1)-u(i+1));
end
result=alpha*Bbase(i,k-1,u,t)+beta*Bbase(i+1,k-1,u,t);
end
二、代码讲解
我逐步解释代码的计算过程:
-
计算样条节点
u
:
for i=1:k+1
u(i)=0;
u(n-1+k+i)=1;
end
u(k+1)=0;
L=0;
for i=1:n-1
L=abs((X(i+1,1)-X(i,1)))+L;
end
for i=k+2:k+n-1
u(i)=u(i-1)+(abs(X(i-k,1)-X(i-k-1,1)))/(L);
end
-
- 首先,为了进行样条插值,需要确定插值节点。代码中通过等距离分布的方式确定了插值节点
u
。 - 首先初始化节点为0和1,然后计算出其他节点的位置,确保节点在数据点之间均匀分布。
- 首先,为了进行样条插值,需要确定插值节点。代码中通过等距离分布的方式确定了插值节点
-
构建样条插值矩阵
A
:
A=zeros(n+k-1);
A(1,1)=1;A(n,n+k-1)=1;
for i=2:n-1
for j=i:i+4
A(i,j)=Bbase(j,k,u,u(i+5));
end
end
- 构建了一个方阵
A
,其中包含了样条插值的系数。 - 首先将
A
初始化为零矩阵,然后根据三次样条插值的基函数,填充了A
的其他元素。
3计算边界条件:
v01=-5/(u(7)-u(2))/tall;
v02=5/(u(7)-u(2))/tall;
ve1=-5/(u(n+9)-u(n+4))/tall;
ve2=5/(u(n+9)-u(n+4))/tall;
a01=20/((u(7)-u(3))*(u(7)-u(2)))/tall^2;
a02=-(20/((u(7)-u(3))*(u(8)-u(3)))+20/((u(7)-u(3))*(u(7)-u(2))))/tall^2;
a03=20/((u(7)-u(3))*(u(8)-u(3)))/tall^2;
ae1=20/((u(n+8)-u(n+4))*(u(n+8)-u(n+3)))/tall^2;
ae2=-(20/((u(n+8)-u(n+4))*(u(n+9)-u(n+4)))+20/((u(n+8)-u(n+4))*(u(n+8)-u(n+3))))/tall^2;
ae3=20/((u(n+8)-u(n+4))*(u(n+9)-u(n+4)))/tall^2;
- 这里计算了样条插值的边界条件,包括起始和结束点的速度和加速度。
4、构建等式约束矩阵 e
:
e=0;
e(n+k-4,1)=10;
e(n+k-4,2)=dt_v_start*ttall;
e(n+k-3,1)=data_x1(n);
e(n+k-3,2)=dt_v_end*ttall;
e(n+k-2,1)=0;
e(n+k-2,2)=dt_a_start*ttall^2;
e(n+k-1,1)=data_x1(n);
e(n+k-1,2)=dt_a_end*ttall^2;
for i=1:n
e(i,:)=X(i,:);
end
- 初始化了等式约束矩阵
e
,并设置了边界条件和数据点。
5、解线性方程组得到样条系数 d
:
d=inv(A)*e;
- 这里使用了线性代数中的求逆运算,解出了样条插值的系数。
6、进行插值计算:
for j=1:n-1
uuu=u(j+5):0.001:u(j+6);
Len=(X(j+1,1)-X(j,1))/(length(uuu)-1);
xx=X(j,1):Len:X(j+1,1);
if t==0
t=[xx];
else
t=[t,xx];
end
for uu=u(j+5):0.001:u(j+6)
down=down+1;
x=Bbase(j,5,u,uu)*d(j,:)+Bbase(j+1,5,u,uu)*d(j+1,:)+Bbase(j+2,5,u,uu)*d(j+2,:)+Bbase(j+3,5,u,uu)*d(j+3,:)+Bbase(j+4,5,u,uu)*d(j+4,:)+Bbase(j+5,5,u,uu)*d(j+5,:);
angle(down)=x(2);
end
end
- 这里通过插值节点和样条系数,计算了在每个节点处的插值结果,并将结果存储在
angle
中。
7、绘制插值结果:
plot(t,angle,'r-','linewidth',1.5);
-
- 最后,使用
plot
函数将插值结果以红色线条的形式绘制出来。
- 最后,使用
总的来说,这段代码实现了三次样条插值的计算过程,从确定插值节点到计算插值结果,最后绘制出了插值曲线。