【Matlab】B样条曲线机器人二维动态仿真（2020Robocon）_b样条曲线路径规划算完教学matlab仿真实验

1. 基础知识

本文采用的是非均匀B样条曲线，节点矢量的确定采用Hartley-Judd方法。

1.1 B样条曲线方程：

其中，d_i(i=0,1,…,n)为控制顶点，又称德布尔点。顺序连成的折线称为B样条控制多边形。N_i,k(u)(i=0,1,…,n)成为k次规范B样条基函数，其中每一个称为规范基函数，简称B样条。它是由一个称为节点矢量的非递减参数u的序列U：u₀ ≤ u₁ ≤ … ≤ u_n+k+1所决定的k次分段多项式，也即是k次多项式样条。

1.2 节点矢量的确定

非均匀B样条曲线通常将曲线的定义域取成规范参数域，即使u∈[ u_k , u_n+1 ] = [0,1]。于是，u₀ = u₁ = … u_k = 0, u_n+1 = u_n+2 = … u_n+k+1 = 1。剩下需要确定的就只有u_k+1，u_k+2，… ，…u_n，那些内节点，共n - k个。
Hartley-Judd方法，公式如下：

可得节点值：

代码：

% U_HartleyJudd.m文件 
% B样条 Hartley Judd 算法 非连续节点求法
function NodeVector = U_HartleyJudd(n, k,P ) 
if  (~mod(k, 1) && k>=1)   % 满足n是k的整数倍且k为正整数
    NodeVector = zeros(1, n+k+2);   % 节点矢量长度为n+k+2
    NodeVector(1, n+2 : n+k+2) = ones(1, k+1);  % 右端节点置1  
   den =0;
   mem =0;
   % 求分母   从1开始  n+1个控制点  n条曲线
   for  i = k+1 : n+1
       for j = i - k : i -1          
            den = den + norm(P(:,j) - P(:,j+1));   %求长度
       end
   end
   %计算分子
   for i = k + 2 : n+1          %节点值是从1开始的，故i从k+1+1开始循环
       mem=0;
       for j= i-k : i - 1
           mem = mem + norm(P(:,j) - P(:,j+1)) ;
       end
       NodeVector(1,i)= mem/den + NodeVector(1,i-1);
   end
else
    fprintf('error!\n');
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

1.3 B样条基函数

采用德布尔算法，公式如下：

其中i为节点序号，k是基函数的次数，共有n+1个控制顶点，代码如下：

% BaseFunction.m文件
function Nik_u = BaseFunction(i, k , u, NodeVector)
% 计算基函数Ni,k(u),NodeVector为节点向量

if k == 0       % 0次B样条
    if (u >= NodeVector(i+1)) && (u < NodeVector(i+2))
        Nik_u = 1.0;
    else
        Nik_u = 0.0;
    end
else
    Length1 = NodeVector(i+k+1) - NodeVector(i+1);
    Length2 = NodeVector(i+k+2) - NodeVector(i+2);      % 支撑区间的长度
    if Length1 == 0.0 % 规定0/0 = 0
        Length1 = 1.0;
    end
    if Length2 == 0.0
        Length2 = 1.0;
    end
    Nik_u = (u - NodeVector(i+1)) / Length1 * BaseFunction(i, k-1, u, NodeVector) ...
        + (NodeVector(i+k+2) - u) / Length2 * BaseFunction(i+1, k-1, u, NodeVector);
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

2.1 运行流程

function BPathPlanning() %B样条轨迹规划
fig = figure(1);
Robot = RobotParameter();         %Robot参数设置

%Bpath 所有u节点绘制
Drawmap_Center_xoy();             %Robocon地图描绘
Robot.PR.State = DrawRobot(Robot);%Robot描绘

plot(Robot.PR.BSpline.P(1,1:Robot.PR.BSpline.n+1), ...
          Robot.PR.BSpline.P(2, 1:Robot.PR.BSpline.n+1),...
        'o','LineWidth',1,'MarkerEdgeColor','k',...
        'MarkerFaceColor','g', 'MarkerSize',6);          %B样条顶点描绘

    
set(gcf,'WindowButtonDownFcn',@ButttonDownFcn);    %回调函数,鼠标点击显示坐标
set(fig,'windowkeypressfcn',{'Keypressfcn',Robot});%回调函数，按空格执行函数

end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

2.2 运行结果

1. 全局图如下：
   
2. 仿真时
   
3. 细节1
   

完整程序链接：2020Robocon Matlab B样条曲线仿真

参考资料：
《计算机辅助几何设计与非均匀有理B样条》施法中
《基于三次非均匀B样条曲线的机器人轨迹规划算法研究》赖永林
相关链接：
https://blog.csdn.net/Mr_Grit/article/details/45603627
https://zhuanlan.zhihu.com/p/50626506

文中如有错误请私信，不吝赐教！