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

路径规划666

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

阅读量1.1k

点赞数 8

CC 4.0 BY-SA版权

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

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

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

一、代码

废话不多说直接上

代码：

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)=a