机器人轨迹规划:三次样条曲线

文章目录

  • 机器人轨迹规划:三次样条曲线
    • 写在前面
      • 学习代码都记录在[个人github](https://github.com/xuuyann/RobotLearningCode)上,欢迎关注~
    • 三次样条曲线性质
    • 当指定初始速度v0和最终速度vn时的参数计算(也就是v0和vn已知)
      • 例子
    • 周期三次样条:没有指定初始速度v0和最终速度vn(也就是v0和vn未知)
      • 例子
    • 具有指定初始速度v0和最终速度vn的三次样条(v0和vn已知):基于加速度的计算
    • 具有指定初始、最终速度以及加速度的三次样条曲线
      • 例子
    • 参考文献(实际上我就是翻译了一下下。。。)

写在前面

学习代码都记录在个人github上,欢迎关注~

在一些避障的应用场景下,一般都是先在任务空间中对多轴机械臂的末端进行路径规划,得到的是末端的运动路径点数据。这条轨迹只包含位置关系,并没有告诉机器人应该以怎样的速度、加速度运动,这就需要进行带时间参数的轨迹规划处理,也就是对这条空间轨迹进行速度、加速度约束,并且计算运动到每个路点的时间,高级的算法有TOPP等,一般的呢就是贝塞尔、三次准/非/均匀B、五次及三次样条等。下面从最简单的三次样条开始讨论。

三次样条曲线性质

当给出n+1个点时,可以使用n个p次多项式(通常较低)代替唯一的n次插值多项式,每个多项式定义一段轨迹。以这种方式定义的总函数s(t)称为p阶的样条曲线。p的值是根据所需的样条连续度来选择的。例如,为了在两个连续段之间发生过渡的时刻tk获得速度和加速度的连续性,可以假定多项式的阶数p=3(三次多项式)。   1   定义三次样条曲线的函数形式为:   2   这段轨迹由n个三次多项式构成,并且每个多项式需要计算四个参数。由于n个多项式是定义一条通过n+1点的轨迹所必需的,因此需要确定的系数总数为4n。为了解决这个问题,必须考虑以下条件:
  • 给定点插值的2n条件,因为每一个三次函数必须在其极值处穿过点。
  • n-1个条件,过渡点的速度要连续;
  • n-1个条件,过渡点的加速度要连续;
这样的话,就已经限制了2n+2(n-1)个条件,还剩下2个自由度还未限制。通过前面分析,还需要两个限制条件才行,这里讨论的就是初始点和终点的速度以及加速度。下面是几种可能的选择,可以任意选择:   3     通常情况,样条曲线具有如下几个特性:
  • 对于由给定点(tk,qk),k=0,…n得到的p阶样条曲线s(t),[n(p+1)]个参数可以确定
  • 给定n+1个点,并且给定边界条件,则p阶插值样条曲线s(t)能被唯一确定
  • 用于构造样条曲线的多项式的阶数p不取决于数据点的数目
  • 函数s(t)p-1阶连续可导
  • 自然样条曲线是指初始加速度和最终加速度均为0的样条曲线

当指定初始速度v0和最终速度vn时的参数计算(也就是v0和vn已知)

在定义自动机械的轨迹时,速度剖面的连续性条件至关重要。因此,计算样条曲线的典型选择是指定初始和最终速度v0和vn。因此,给定点(tk,qk),k=0,…n以及速度的边界条件(初始速度和最终速度)v0,vn,就有如下几个条件成立:   4   可以最终确定样条曲线的函数s(t)为   5   系数ak,i可以由以下算法进行确定: 第一种情况,如果中间点(插补点)的速度我们已知,也就是vk,k=1,…,n-1,对于每段三次样条曲线,有 6   7   这就可以把每一段曲线的系数都求出来,从而得到样条曲线。这是最简单的情况! 子函数如下:  
% 三次样条:指定初始速度v0和终止速度vn,并且中间插补点的速度已知,这是最简单的情况
% Input:
%   q:给定点的位置
%   t:给定点位置对应的时间
%   v:包括给定起始、中间及终止速度的速度向量
%   tt:插补周期
% Output:
%   yy dyy ddyy:样条曲线函数值、速度、加速度值
function [yy dyy ddyy] = cubicSpline_1(q, t, v, tt)
if length(q) ~= length(t)
    error('输入的数据应成对')
end
n = length(q);
T = t(n) - t(1); % 运行总时长
nn = T / tt; % 总点数
yy = zeros(1, nn);
dyy = zeros(1, nn);
ddyy = zeros(1, nn);
j = 1;
for i = 1: n-1
    Tk = t(i+1) - t(i);
    a0 = q(i);
    a1 = v(i);
    a2 = (1/Tk) * ((3*(q(i+1)-q(i)))/Tk - 2*v(i) - v(i+1));
    a3 = (1/(Tk*Tk)) * ((2*(q(i)-q(i+1)))/Tk + v(i) + v(i+1));
    
    for tk = t(i): tt: t(i+1)
        if i > 1 && tk == t(i)
            continue
        end
        yy(j) = a0 + a1*(tk-t(i)) + a2*power(tk-t(i), 2) + a3*power(tk-t(i), 3);
        dyy(j) = a1 + 2*a2*(tk-t(i)) + 3*a3*power(tk-t(i), 2);
        ddyy(j) = 2*a2 + 6*a3*(tk-t(i));
        j = j + 1;
    end
end

end


  第二种情况,如果中间点的速度我们未知,也就是vk,k=1,…,n-1均未知,然而这些值是必须被计算的。为此,考虑了中间加速度的连续条件:   8   9   其中常数项ck仅取决于中间位置和已知的样条曲线段的持续时间Tk。由于速度v0和vn也是已知的,所以可以消除矩阵A‘的相应列并获得   10   11   另外v0=2,v6=-3。  
% 三次样条:指定初始速度v0和终止速度vn,但是中间点速度未知
% Input:
%   q:给定点的位置
%   t:给定点位置对应的时间
%   v0:初始速度
%   vn:终止速度
%   tt:插补周期
% Output:
%   yy dyy ddyy:样条曲线函数值、速度、加速度值
function [yy dyy ddyy] = cubicSpline_2(q, t, v0, vn, tt);
if length(q) ~= length(t)
    error('输入的数据应成对');
end
n = length(q);
c = zeros(n-2, 1);
% 矩阵A是个(n-2)*(n-2)的对角占优矩阵
A = zeros(n-2);
for i = 1: n-2
    Tk_1 = t(i+2) - t(i+1);
    Tk = t(i+1) - t(i);
    if i == 1
        A(i, i) = 2*(Tk + Tk_1);
        A(i, i+1) = Tk;
        c(i, 1) = (3/(Tk*Tk_1))*(Tk^2*(q(i+2)-q(i+1))+Tk_1^2*(q(i+1)-q(i))) - Tk_1*v0;
    elseif i == n-2
        A(i, i-1) = Tk_1;
        A(i, i) = 2*(Tk + Tk_1);
        c(i, 1) = (3/(Tk*Tk_1))*(Tk^2*(q(i+2)-q(i+1))+Tk_1^2*(q(i+1)-q(i))) - Tk*vn;
    else
        A(i, i-1) = Tk_1;
        A(i, i) = 2*(Tk + Tk_1);
        A(i, i+1) = Tk;
        c(i, 1) = (3/(Tk*Tk_1))*(Tk^2*(q(i+2)-q(i+1))+Tk_1^2*(q(i+1)-q(i)));
        
    end
end
% 经过上述步骤得到对角占优矩阵A和c
% vk = A \ c; % 这一步matlab计算很慢,应换成追赶法求vk
for i = 1: n-2
    a(i) = A(i, i); % 对角线
    if i == n-3
        b(i) = A(i, i+1); % 上边
        d(i) = A(i+1, i); % 下边
        continue;
    elseif i < n-2
        b(i) = A(i, i+1); % 上边
        d(i) = A(i+1, i); % 下边
    end
end
[~, ~, vk] = crout(a, b, d, c); % 追赶法

% 得到中间插补点的速度vk,然后调用cubicSpline_1即可
v_ = [v0, vk', vn];
[yy dyy ddyy] = cubicSpline_1(q, t, v_, tt);

end


%追赶法求解三对角线性方程组,Ax=b,A用一维数组a,c,d存储。
function [L,U,x]=crout(a,c,d,b)%数组a存储三角矩阵A的主对角线元素,c、d存储主对角线上边下边带宽为1的元素
    n=length(a);
    n1=length(c);
    n2=length(d);
    %错误检查
    if n1~=n2%存储矩阵的数组维数错误
        error('MATLAB:Crout:不是三对角矩阵,参数数组中元素个数错误.');
    elseif n~=n1+1
        error('MATLAB:Crout:不是三对角矩阵,参数数组中元素个数错误.');
    end
   
    %初始化
    L=zeros(n);%生成n*n的全零矩阵
    U=zeros(n);
    p=1:n;
    q=1:n-1;
    x=1:n;
    y=1:n;
   
    %追赶法程序主体
    p(1)=a(1);
    for i=1:n-1
        q(i)=c(i)/p(i);
        p(i+1)=a(i+1)-d(i)*q(i);%d的下标改为1到n-1
    end
    %正解y
    y(1)=b(1)/p(1);%用x存储y
    for i=2:n
        y(i)=(b(i)-d(i-1)*y(i-1))/p(i);
    end
    %倒解x
    x(n)=y(n);
    for i=(n-1):-1:1
        x(i)=y(i)-q(i)*x(i+1);
    end
    %L,U矩阵
    for i=1:n
        L(i,i)=p(i);
        U(i,i)=1;
    end
    for i=1:n-1
        L(i+1,i)=d(i);
        U(i,i+1)=q(i);
    end
end %end of function

  测试:  
%% 自写cubicSpline_2函数测试
q = [3, -2, -5, 0, 6, 12, 8];
t = [0, 5, 7, 8, 10, 15, 18];
n = length(t);
v0 = 2; vn = -3; tt = 0.1;
[yy dyy ddyy] = cubicSpline_2(q, t, v0, vn, tt);
subplot(3, 1, 1)
plot(t, q, 'o');
ylabel('位置')
grid on
hold on
plot([t(1):tt:t(n)], yy);
subplot(3, 1, 2)
plot([t(1), t(n)], [v0, vn], 'o');
grid on
hold on
plot([t(1):tt:t(n)], dyy);
ylabel('速度')
subplot(3, 1, 3)
grid on
hold on
plot([t(1):tt:t(n)], ddyy);
ylabel('加速度')
    13  

周期三次样条:没有指定初始速度v0和最终速度vn(也就是v0和vn未知)

在许多应用中,要执行的运动是周期性的,即初始位置和最终位置相同。在这种情况下,利用为计算样条曲线而指定的最后两个自由度,以使曲线具有初始和最终速度和加速度的连续性。因此,计算系数的方法与先前报告的略有不同。事实上,在这种情况下,必须考虑代替任意选择的初始和最终速度v0和vn的条件。   14   15   16   系统的矩阵不再是三对角的。这种情况被称为循环三对角系统,也存在有效的求解计算方法。一旦获得速度v0,…,vn−1,就可以通过(4.8)计算样条曲线的系数。

例子

  17  
% 三次样条:周期三次样条,没有指定初始速度v0和终止速度vn,也就是v0和vn未知
% Input:
%   q:给定点的位置
%   t:给定点位置对应的时间
%   tt:插补周期
% Output:
%   yy dyy ddyy:样条曲线函数值、速度、加速度值
function [yy dyy ddyy] = cubicSpline_3(q, t, tt)
if length(q) ~= length(t)
    error('输入的数据应成对');
end
n = length(q);
c = zeros(n-1, 1);
% 矩阵A是个(n-1)*(n-1)的循环三对角矩阵
A = zeros(n-1);
for i = 1: n-1
    if i == 1
        Tn_1 = t(n) - t(n-1);
        T0 = t(i+1) - t(i);
        A(i, i) = 2*(Tn_1 + T0);
        A(i, i+1) = Tn_1;
        A(i, n-1) = T0;
        c(i, 1) = (3/(Tn_1*T0))*((Tn_1^2)*(q(i+1)-q(i))+(T0^2)*(q(n)-q(n-1)));
    else
        Tk_1 = t(i+1) - t(i);
        Tk = t(i) - t(i-1);
        c(i, 1) = (3/(Tk*Tk_1))*(Tk^2*(q(i+1)-q(i))+Tk_1^2*(q(i)-q(i-1)));
        if i == n-1
            A(i, 1) = Tk_1;
            A(i, i-1) = Tk_1;
            A(i, i) = 2*(Tk + Tk_1);
        else
            A(i, i-1) = Tk_1;
            A(i, i) = 2*(Tk + Tk_1);
            A(i, i+1) = Tk;
        end
    end
            
end
% 经过上述步骤得到矩阵A和c
vk = A \ c; % 这一步matlab计算很慢,应换成追赶法求vk
% 这个vk的第一个值为v0,然后v0和vn相等
% 得到中间插补点的速度vk,然后调用cubicSpline_1即可
v_ = [vk', vk(1)];
% v_ = [-2.28 -2.78 2.99 5.14 2.15 -1.8281 -2.28]
[yy dyy ddyy] = cubicSpline_1(q, t, v_, tt);

end

  测试:  
%% 自写cubicSpline_3函数测试
q = [3, -2, -5, 0, 6, 12, 3];
t = [0, 5, 7, 8, 10, 15, 18];
t1 = [18, 23, 25, 26, 28, 33, 36];
n = length(t);
tt = 0.1;
[yy dyy ddyy] = cubicSpline_3(q, t, tt);
[yy_, dyy_, ddyy_] = cubicSpline_3(q, t1, tt);
subplot(3, 1, 1)
plot(t, q, 'o');
ylabel('位置')
grid on
hold on
plot([t(1):tt:t(n)], yy);
plot([t1(1):tt:t1(n)], yy_);
subplot(3, 1, 2)
% plot([t(1), t(n)], [v0, vn], 'o');
grid on
hold on
plot([t(1):tt:t(n)], dyy);
plot([t1(1):tt:t1(n)], dyy_);
ylabel('速度')
subplot(3, 1, 3)
grid on
hold on
plot([t(1):tt:t(n)], ddyy);
plot([t1(1):tt:t1(n)], ddyy_);
ylabel('加速度')
    19   20   21   22 23   24   这个方法主要是为下面的方法做准备,因此不写例子。

具有指定初始、最终速度以及加速度的三次样条曲线

样条曲线是一个二阶连续导数的函数,但通常不可能同时指定初始速度和最终速度以及加速度。因此,样条曲线在其末端的特征是速度或加速度的不连续性,一般情况下我们会指定初始和最终速度,则此时初始和最终加速度难以保证连续,会出现突变。下面需要做的就是需要保证在指定初始速度和最终速度的前提下,还要保证初始、最终加速度从0开始连续变化。如果这些不连续代表一个问题,可以采用不同的方法:
  • 一个5次多项式函数可以用于第一个和最后一个域,其缺点是允许在这些段中有更大的超调,并且稍微增加了计算负担;
  • 在第一段和最后一段中添加两个自由额外点(从这个意义上说,这些点不能先验地固定),并通过施加所需的速度和加速度的初始值和最终值来计算它们的值。
后一种方法现在详细说明。让我们考虑一个要插值的n-1点向量,这两个向量中都没有第二个值以及倒数第二个值,也就是q1、t1和qn-1、tn-1(目前我还不知道为啥要这么做。。。)   25   26   增加的这两个点,可以通过已知的变量去表达这两个点,即初始/最终的位置、速度以及加速度(q0/qn,v0/vn,a0/an)同时包括在这些点上的加速度w1和wn-1(其中边界点的加速度是用a0和an来表示,而中间点的加速度是用w来表示)。这样,就可以考虑初始加速度和最终加速度的约束。   28   将(4.26)和(4.27)替换掉(4.24)中的有关项,通过重新排列n-1方程,我们得到一个线性系统   29   其中   30   31   32  

例子

  33   其中v0=2,vn=-3,a0=0,an=0。额外增加的两个时间点t1=2.5,t7=16.5,当然也可以任意选择。  
% 三次样条:指定初始、终止速度以及加速度,也就是v0,vn,a0,an已知
% 这个方法需要增加两个额外的点q1_和qn-1_
% q1_在原有q1和q2之间,qn-1_在原有的qn-1和qn之间
% 这两个额外点对应的时间t1_和tn-1_需要计算,可以任意选择,本程序选择取平均值
% Input:
%   q:给定点的位置
%   t:给定点位置对应的时间
%   v0:初始速度
%   vn:终止速度
%   a0:初始加速度
%   an:终止加速度
%   tt:插补周期
% Output:
%   yy dyy ddyy:样条曲线函数值、速度、加速度值
function [yy dyy ddyy q1 qn_1] = cubicSpline_4(q, t, v0, vn, a0, an, tt)
if length(q) ~= length(t)
    error('输入的数据应成对');
end
n = length(q); % 原来的点数量
% 增加两个额外点q1_和qn-1_,计算两点对应的时间点
t1_ = (t(1)+t(2)) / 2;
tn_1_ = (t(n-1)+t(n)) / 2;
% 更新时间向量
t = [t(1), t1_, t(2: n-1), tn_1_, t(n)];
% 更新点的数量
n = n + 2;
% 矩阵A是个(n-2)阶对角占优矩阵
A = zeros(n-2);
c = zeros(n-2, 1);
for i = 1: n-2
    Tk_1 = t(i+2) - t(i+1);
    Tk = t(i+1) - t(i);
    if i == 1
        A(i, i) = 2*Tk_1 + Tk*(3+Tk/Tk_1);
        A(i, i+1) = Tk_1;
        c(i, 1) = 6*((q(2)-q(1))/Tk_1-v0*(1+Tk/Tk_1)-a0*(0.5+Tk/(3*Tk_1))*Tk);
    elseif i == 2
        T0 = t(2)-t(1);
        A(i, i-1) = Tk - (T0^2)/Tk;
        A(i, i) = 2*(Tk + Tk_1);
        A(i, i+1) = Tk_1;
        c(i, 1) = 6*((q(3)-q(2))/Tk_1-(q(2)-q(1))/Tk+v0*(T0/Tk)+a0*(T0^2)/(3*Tk));
    elseif i == n-2-1
        Tn_1 = t(n) - t(n-1);
        A(i, i-1) = Tk;
        A(i, i) = 2*(Tk + Tk_1);
        A(i, i+1) = Tk_1 - (Tn_1^2)/Tk_1;
        c(i, 1) = 6*((q(n-2)-q(n-3))/Tk_1-(q(n-3)-q(n-4))/Tk-vn*(Tn_1/Tk_1)+an*(Tn_1^2)/(3*Tk_1));
    elseif i == n-2
        A(i, i) = 2*Tk + Tk_1*(3+Tk_1/Tk);
        A(i, i-1) = Tk;
        c(i, 1) = 6*((q(n-3)-q(n-2))/Tk+vn*(1+Tk_1/Tk)-an*(0.5+Tk_1/(3*Tk))*Tk_1);
    else
        A(i, i-1) = Tk;
        A(i, i) = 2*(Tk + Tk_1);
        A(i, i+1) = Tk_1;
        c(i, 1) = 6*((q(i+1)-q(i))/Tk_1-(q(i)-q(i-1))/Tk);
    end
end
% 经过上述步骤得到对角占优矩阵A和c
wk = A \ c; % 这一步matlab计算很慢,应换成追赶法求vk
% for i = 1: n-2
%     a(i) = A(i, i); % 对角线
%     if i == n-3
%         b(i) = A(i, i+1); % 上边
%         d(i) = A(i+1, i); % 下边
%         continue;
%     elseif i < n-2
%         b(i) = A(i, i+1); % 上边
%         d(i) = A(i+1, i); % 下边
%     end
% end
% [~, ~, wk_] = crout(a, b, d, c); % 追赶法
n_ = length(wk);
q1 = q(1) + T0*v0 + ((T0^2)/3)*a0 + ((T0^2)/6)*wk(1);
Tn_1 = t(n) - t(n-1);
qn_1 = q(n-2) - Tn_1*vn + ((Tn_1^2)/3)*an + ((Tn_1^2)/6)*wk(n_);
% 更新位置q向量
q = [q(1), q1, q(2: n-3), qn_1, q(n-2)];
% 更新加速度w向量
w = [a0, wk', an];

% 规划样条轨迹
T = t(n) - t(1); % 运行总时长
nn = T / tt; % 总点数
yy = zeros(1, nn);
dyy = zeros(1, nn);
ddyy = zeros(1, nn);
j = 1;
for i = 1: n-1
    Tk = t(i+1) - t(i);
    a0 = q(i);
    a1 = (q(i+1)-q(i))/Tk-(Tk/6)*(w(i+1)+2*w(i));
    a2 = w(i) / 2;
    a3 = (w(i+1)-w(i))/(6*Tk);
    
    for tk = t(i): tt: t(i+1)
        if i > 1 && tk == t(i)
            continue
        end
        yy(j) = a0 + a1*(tk-t(i)) + a2*power(tk-t(i), 2) + a3*power(tk-t(i), 3);
        dyy(j) = a1 + 2*a2*(tk-t(i)) + 3*a3*power(tk-t(i), 2);
        ddyy(j) = 2*a2 + 6*a3*(tk-t(i));
        j = j + 1;
    end
end

end
  测试:  
%% 自写cubicSpline_4函数测试
q = [3, -2, -5, 0, 6, 12, 8];
t = [0, 5, 7, 8, 10, 15, 18];
v0 = 2; vn = -3; a0 = 0; an = 0;
tt = 0.1;
n = length(t);
[yy dyy ddyy q1 qn_1] = cubicSpline_4(q, t, v0, vn, a0, an, tt);
% 增加两个额外点q1_和qn-1_,计算两点对应的时间点
t1_ = (t(1)+t(2)) / 2;
tn_1_ = (t(n-1)+t(n)) / 2;
% 更新时间向量
t = [t(1), t1_, t(2: n-1), tn_1_, t(n)];
n = length(t);% 更新n 
% 更新q
q = [q(1), q1, q(2: n-3), qn_1, q(n-2)];
subplot(3, 1, 1)
plot(t, q, 'o');
ylabel('位置')
grid on
hold on
plot([t(1):tt:t(n)], yy);
hold on
plot(t(2), q(2), 'r*');
plot(t(n-1), q(n-1), 'r*');
subplot(3, 1, 2)
% plot([t(1), t(n)], [v0, vn], 'o');
grid on
hold on
plot([t(1):tt:t(n)], dyy);
ylabel('速度')
subplot(3, 1, 3)
grid on
hold on
plot([t(1):tt:t(n)], ddyy);
ylabel('加速度')
  34   三次样条部分还有平滑三次样条,后面再写,今天就到这里~

参考文献(实际上我就是翻译了一下下。。。)

Trajectory Planning for Automatic Machines and Robots