B样条 (折线路径平滑处理)(附MATLAB代码)

路径规划666

已于 2024-05-29 07:57:32 修改

阅读量467

点赞数 8

分类专栏：路径规划文章标签： matlab 算法

于 2024-04-28 20:24:11 首次发布

本文链接：https://blog.csdn.net/LJGHZ/article/details/138289796

版权

路径规划专栏收录该内容

7 篇文章 1 订阅

订阅专栏

一、代码

废话不多说直接上

代码：

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