精华内容
下载资源
问答
  • matlabB样条曲线求导求曲率

    千次阅读 2019-05-07 10:58:35
    原文:...function result = BspCurv( C,U,t,k) % %CURVATURE B样条曲线在t点曲率 %C控制点,U节点序列,U(k)<=t<U(k+1) % %see also http://www.matlabsky.com % dev1=0; dev2=0; %%计算...

    原文:http://www.matlabsky.com/thread-665-1-1.html

    function result = BspCurv( C,U,t,k)

    %

    %CURVATURE B样条曲线在t点曲率

    %C控制点,U节点序列,U(k)<=t<U(k+1)

    %

    %see also http://www.matlabsky.com

    %

    dev1=0;

    dev2=0;

    %%计算一阶导数

    for i=k-2:k

    dev1=dev1+DeBoorDv(C,U,t,i,1)*Bbase(i,2,U,t);
    

    end

    %计算二阶导数

    for i=k-1:k

    dev2=dev2+DeBoorDv(C,U,t,i,2)*Bbase(i,1,U,t);
    

    end

    %计算曲率

    temp=norm(dev1)^3;

    if temp==0

    result=NaN;
    

    else

    result=det( cat(1,dev1,dev2))/temp;
    

    end

    &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&

    %

    %

    function y=DeBoorDv(d,u,kn,i,L)

    %递归求导数控制点,德布尔递推算法

    %

    %d控制点序列,u节点序列,kn插入的节点,u(i)<kn<u(i+1),L递推层次

    % k B样条次数

    %

    %see also http://www.matlabsky.com

    %

    k=3;

    if L==0

     y=d(i,:);
    
     return;
    

    end

    temp=(u(i+k+1-L)-u(i));

    if temp==0

     y=0;
    

    else

     y=(k+1-L)*(DeBoorDv(d,u,kn,i,L-1)-DeBoorDv(d,u,kn,i-1,L-1))/temp;
    

    end

    展开全文
  • matlab三次样条函数的绘制(spline和csape函数详解)样条函数是工程中常用的插值函数。早期工程师制图时,把富有弹性的细长木条...由于样条曲线具有连续的二阶导数,所以光滑性好。matlab里有两个函数可以绘制样条曲...

    matlab三次样条函数的绘制(spline和csape函数详解)

    样条函数是工程中常用的插值函数。早期工程师制图时,把富有弹性的细长木条(所谓样条)用压铁固定在样点上,在其他地方让它自由弯曲,然后沿木条画下曲线。对于样条本身,可以利用材料力学的大柔度梁理论建立梁的挠度方程,根据理论,样条可以用分段插值三次函数表示。

    由于样条曲线具有连续的二阶导数,所以光滑性好。matlab里有两个函数可以绘制样条曲线,一个是spline,一个是csape。虽然interp1中也可以绘制,优点是代码简单而且可以方便更换其它插值方法,但是功能也比较简单,对于边界条件的无法设置。

    关于interp1可以参见官方帮助https://ww2.mathworks.cn/help/matlab/ref/interp1.html

    三次样条曲线有4种边界条件。

    自然边界条件,二阶导数在边界处为0,可视为简支梁,是最常用的边界条件。

    第一边界条件,二阶导在边界处已知的边界条件。自然边界条件可视为特例。

    第二边界条件,一阶导在边界处已知的边界条件。

    循环边界条件,一阶导与二阶导在边界处相等的边界条件,适用于封闭或循环的图形。

    如果既有第一边界由于第二边界,称为混合边界条件。

    1.spline函数详解

    spline函数只能实现自然边界条件和第二边界条件,可以实现一维或者高维的曲线插值。

    官方spline实例可以参见:https://ww2.mathworks.cn/help/matlab/ref/spline.html

    1.一维自然边界条件

    格式cs=spline(x,y);

    输入:x自变量,y函数值

    输出:cs为三维样条插值函数构建的结构体。

    cs结构体调用方法为yy=ppval(cs,xx);

    xx为插值点,yy为插值得到的函数值。

    %正弦曲线

    r=pi*linspace(-1,1,100);

    yr=sin(xr);

    %1自然边界条件

    x = pi*linspace(-1,1,5);%设置5个控制点

    y = sin(x);

    cs = spline(x,y);%样条函数

    xx = linspace(x(1),x(end),100);%插值点

    yy=ppval(cs,xx);%插值

    figure(1)

    plot(x,y,'bo',xr,yr,'b--',xx,yy,'r-');%绘图

    这里自然条件下,y’’(-pi)=y’’(pi)=0。红线为插值函数,蓝线为实际正弦函数。

    2.第二边界条件

    曲线依然选用上面的曲线,我们使用第二边界条件。

    这里依然用上一个xr和yr正弦曲线。y’(-pi)=1,y’(pi)=0。

    第二边界条件(导数条件)格式为:

    cs=spline(x , [y’1, y, y’2]);

    当y比x的长度多2个时,把第一个值和最后一个值当做函数边界的导数。

    比如下面代码中,这里x有5个值,y在spline函数中在第一和最后分别增加了1个值作为导数值,第一个值为1,对应y’(-pi)=1;最后一个值为0,对应y’(pi)=0。

    %2第二种边界条件,导数确定的

    cs2 = spline(x,[1,y,0]);

    xx = linspace(x(1),x(end),100);

    yy=ppval(cs2,xx);

    figure(2)

    plot(x,y,'bo',xr,yr,'b--',xx,yy,'r-');

    可以看到插值函数被大大改变。

    如果想要验证y’(-pi)=1,取(yy(2)-yy(1))/(xx(2)-xx(1)),得0.87(这里不是1的原因是xx太稀疏,xx间隔取越密,越接近1)。

    3.高维无约束

    这里以二维为例。这里选用两端无约束的自然边界条件。

    二维格式为:

    cs=spline(t,XY);

    输入t为一个正向序列,需要单调。

    输入XY为2行n列的矩阵,第一行为x,第二行为y。

    cs依然是输出的结构体,但是后面利用ppval插值时要代入关于t的序列值。

    XY=[1.1,1,0,-1,0,1,1.1;

    2,1,2,0,-2,-1,-2];

    t=1:length(XY);%输入XY和t

    cs3=spline(t,XY);%样条函数

    yy=ppval(cs3,linspace(t(1),t(end),101));%插值

    figure(3)

    plot(XY(1,:),XY(2,:),'bo',yy(1,:),yy(2,:))

    4.高维第二边界

    由于1维不能表述当y’(x)=inf的情况(如果输入inf时函数会报错),所以可以利用二维来实现导数为无穷大时的约束。

    对于二维情况,导数的表达用向量的形式代替,比如k=1可以表示为(0.707,0.707),k=∞可以用(0,1)表示。

    表达格式为

    cs=spline(t , [xy’1, xy, xy’2]);

    t为序列

    xy为坐标,xy’1为第一个点的导数列向量,xy’2为最后一个点的导数列向量

    这里第一个点和最后一个点都取负无穷(0 ; -1)

    %4高维有斜率约束

    %这里用3的t和XY

    cs4=spline(t,[[0;-1],XY,[0;-1]]);

    yy=ppval(cs4,linspace(t(1),t(end),101));

    figure(4)

    plot(XY(1,:),XY(2,:),'bo',yy(1,:),yy(2,:))

    可以看到有负无穷约束的条件下,改善了图形向外弯折。

    5.利用第二边界条件绘制圆

    虽然圆是一个封闭图形,需要用到循环边界条件,但是如果我们已知圆一点处的导数,还是可以间接的利用这点的导数去做一个伪循环条件的。

    比如在(1,0)点处作为初始点,逆时针一圈回到(1,0)点。两边一阶导均为正无穷。算法实现如下:

    %5圆形绘制

    t = pi*linspace(0,2,7);

    XY=[cos(t);sin(t)];

    cs5=spline(t,[[0;1],XY,[0;1]]);

    yy=ppval(cs5,linspace(t(1),t(end),101));

    figure(5)

    plot(XY(1,:),XY(2,:),'bo',yy(1,:),yy(2,:))

    2.csape函数详解

    csape函数可以实现三次样条曲线的各种条件,包括第一边界、第二边界、循环边界、混合边界等。可以实现一维至多维的各种情况。

    1.自然边界条件

    调用格式同spline函数,这里就不展开了。

    cs =csape(x,y)

    2。第一边界条件

    调用格式:

    cs = csape(x , [y’‘1 , y , y’‘2] , [2 , 2] )

    其中x、y和y’'1(2)和之前定义都一样,是约束值,只是这个函数多了一项。

    第三项,即[2,2]代表第一个值和最后一个值,都是样条函数的2阶导数值约束条件。

    举例为

    %采用cs1的x和y值,即正弦曲线值

    cs6 = csape(x,[5,y,5],[2,2]);%样条插值

    xx = linspace(x(1),x(end),100);

    yy=ppval(cs6,xx);

    figure(6)

    plot(x,y,'bo',xr,yr,'b--',xx,yy,'r-');

    3.第二边界条件&混合边界条件

    这里第三项取[1,1]即为第二边界条件,即1阶导数约束点。

    如果取[1,2]或[2,1]即为混合边界条件。

    这里仅用第二边界举例

    cs7 = csape(x,[-1,y,-1],[1,1]);

    xx = linspace(x(1),x(end),100);

    yy=ppval(cs7,xx);

    figure(7)

    plot(x,y,'bo',xr,yr,'b--',xx,yy,'r-');

    可以看到和之前用二阶导等于0约束相比,用一阶导都等于-1的精度要更高。

    4.二维循环边界条件

    这里csape函数高维的调用格式依然同spline,具体参见前面的例子。

    这里第三项为[0,0]时,代表循环边界条件,不仅适用于1维,还适用于更高维。

    比如还是那绘制圆形举例:

    %9二维自然条件

    t = pi*linspace(0,2,5);

    XY=[cos(t);sin(t)];

    cs9=csape(t,XY);

    yy=ppval(cs9,linspace(t(1),t(end),101));

    figure(9)

    plot(XY(1,:),XY(2,:),'bo',yy(1,:),yy(2,:))

    axis equal

    %10二维循环

    cs10=csape(t,XY,[0,0]);%只多了第三项

    yy=ppval(cs10,linspace(t(1),t(end),101));

    figure(10)

    plot(XY(1,:),XY(2,:),'bo',yy(1,:),yy(2,:))

    在(1,0)点自然无约束条件↑↑↑。

    在(1,0)点循环约束↑↑↑。

    展开全文
  • 这就需要进行带时间参数的轨迹规划处理,也就是对这条空间轨迹进行速度、加速度约束,并且计算运动到每个路点的时间,高级的算法有TOPP等,一般的呢就是贝塞尔、三次准/非/均匀B、五次及三次样条等。下面从最简单的...

    12ff0e6c7ea6aed6793721f937b2b598.png

    写在前面

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

    三次样条曲线性质

    当给出n+1个点时,可以使用n个p次多项式(通常较低)代替唯一的n次插值多项式,每个多项式定义一段轨迹。以这种方式定义的总函数s(t)称为p阶的样条曲线。p的值是根据所需的样条连续度来选择的。例如,为了在两个连续段之间发生过渡的时刻tk获得速度和加速度的连续性,可以假定多项式的阶数p=3(三次多项式)。

    9995f67679408691fb0a69c052cc404c.png

    定义三次样条曲线的函数形式为:

    7b113c57a35dd948f922cec2a58753d4.png

    这段轨迹由n个三次多项式构成,并且每个多项式需要计算四个参数。由于n个多项式是定义一条通过n+1点的轨迹所必需的,因此需要确定的系数总数为4n。为了解决这个问题,必须考虑以下条件:

    • 给定点插值的2n条件,因为每一个三次函数必须在其极值处穿过点。
    • n-1个条件,过渡点的速度要连续;
    • n-1个条件,过渡点的加速度要连续;

    这样的话,就已经限制了2n+2(n-1)个条件,还剩下2个自由度还未限制。通过前面分析,还需要两个限制条件才行,这里讨论的就是初始点和终点的速度以及加速度。下面是几种可能的选择,可以任意选择:

    7aecac4a26080b04d89a1d81e12a66c8.png

    240019bb96448472f3a7425f953048d5.png

    通常情况,样条曲线具有如下几个特性:

    • 对于由给定点(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,就有如下几个条件成立:

    1f8b0f6576388762f5fa05d75220fded.png

    可以最终确定样条曲线的函数s(t)为

    ae8397510ca8c5ba0a789dc7449478c4.png

    系数ak,i可以由以下算法进行确定:

    第一种情况,如果中间点(插补点)的速度我们已知,也就是vk,k=1,…,n-1,对于每段三次样条曲线,有

    462a41eec6b3283ea26501174209f120.png

    其中,$T_{k}=t_{k+1}-t_{k}$。通过解上面的方程,可以得到

    c1292f75252d1172e30f2f9145d1af9a.png

    这就可以把每一段曲线的系数都求出来,从而得到样条曲线。这是最简单的情况!

    子函数如下:

    % 三次样条:指定初始速度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均未知,然而这些值是必须被计算的。为此,考虑了中间加速度的连续条件:

    0e0ab0944c0d41f09675b898d2bdc622.png

    在这些条件下,通过考虑参数$a_{k, 2}, quad a_{k, 3}, quad a_{k+1,2}$的表达式乘以$left(T_{k} T_{k+1}right) / 2$,在简单计算整理之后能够得到

    a71eb28ee4a0fdc4cffea3c25038e5b8.png

    其中k=0,…,n-2。

    上面的关系可以整理成矩阵的形式,$A^{prime} v^{prime}=c^{prime}$,其中

    d7683f4237b89f1f518e71d8ff4a9bef.png

    d6eeb925aa0d2e5476b4f357001182cb.png

    其中常数项ck仅取决于中间位置和已知的样条曲线段的持续时间Tk。由于速度v0和vn也是已知的,所以可以消除矩阵A‘的相应列并获得

    709cf54515f5c9986c9c54c0cfef00be.png

    也就是

    95cd8f39f1e67158a92cd2077f775bd9.png

    84d0d67349a2de96e1a1d7251732c4ea.png

    例子

    d37a2b6d35c266bfe796971fa32582f8.png

    另外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('加速度')

    51b4235aaaef3b0872439c17989682a0.png

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

    在许多应用中,要执行的运动是周期性的,即初始位置和最终位置相同。在这种情况下,利用为计算样条曲线而指定的最后两个自由度,以使曲线具有初始和最终速度和加速度的连续性。因此,计算系数的方法与先前报告的略有不同。事实上,在这种情况下,必须考虑代替任意选择的初始和最终速度v0和vn的条件。

    bbbad5d6856356044f532301e2460895.png

    最后一个公式可以写成:

    7c2680b639eb52d63a7c29fc025679e1.png

    代入系数表达式后,从(4.8)中得到

    50d86371bf1d3af6dd1ceed732715d72.png

    通过将该方程添加到系统(4.10)中,并考虑到在这种情况下,速度vn等于v0但未知(因此在(4.10)中,必须在左侧移动$T_{n-2}v_n$和$T_1v_0$),计算速度的线性系统变为

    1235c8993b0dc97745078d70f29311b9.png

    系统的矩阵不再是三对角的。这种情况被称为循环三对角系统,也存在有效的求解计算方法。一旦获得速度v0,…,vn−1,就可以通过(4.8)计算样条曲线的系数。

    例子

    b9379d9829ae87dfad281e7e792a657c.png
    % 三次样条:周期三次样条,没有指定初始速度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('加速度')

    ff48f00336a08a210ced12be23c8d57f.png

    具有指定初始速度v0和最终速度vn的三次样条(v0和vn已知):基于加速度的计算

    定义三次样条还有一种方法:样条曲线的一般多项式qk(t)可以表示为在其端点处计算的二阶导数的函数,也就是加速度$ddot{q}left(t_{k}right)=omega_{k}{k=0,...,n}$,,而不是速度$v_k$。可以计算得到此时的多项式表达式为

    1023c8e776064c3f331485ff4c97d7da.png

    这样的话,速度和加速度就是如下计算式

    f1baca305b46fa40374a6876e8b179f8.png

    这样的话,未知数就是加速度$omega_k$,因为从上面的计算式可以发现,曲线的未知参数就是加速度了,因此它是唯一定义样条曲线的。由于中间点的速度和加速度的连续性,我们得到

    4f4a5ae6f10472613ee5df3ca78db8bc.png

    结合式(4.15)、(4.17)和(4.18),可以得到

    9846560fae351d1fca80094046317621.png

    其中k=1,…,n-1。另外,初始速度和最终速度有如下条件

    2e02c967291551dd1f4ddc623adf24b9.png

    由此可以推导得到

    9bc427f2a0774ea05db1901b3cdd64d8.png

    结合(4.19)-(4.21),可以得到如下线性系统

    897d274c312c028d1bc4f8013d5522a0.png

    其中,(n+1)阶三对角对称矩阵A(其中未知参数$omega=[omega_0,omega_1,...,omega_n]^T$)为

    fdbaf91717c29fed1724c951b6c261e8.png

    8cf36a862cfa8fe85dbe509e99f88d6e.png

    同样是利用追赶法解三对角线方程组,通过将解方程得到的$omega _k$带入(4.14)中,最终得到样条曲线。除了利用(4.14)来描述三次样条曲线之外,显然,还是可以根据初始定义来描述三次样条曲线,即

    8774d0d9d91e9e22ba72b9be409ee2d6.png

    其中样条曲线的参数a呢就可以通过已知的点$q_k$和已经求得的加速度$omega _k$来计算,计算式如下

    be794ed9e2efab678ac33f06a832879c.png

    这个方法主要是为下面的方法做准备,因此不写例子。

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

    样条曲线是一个二阶连续导数的函数,但通常不可能同时指定初始速度和最终速度以及加速度。因此,样条曲线在其末端的特征是速度或加速度的不连续性,一般情况下我们会指定初始和最终速度,则此时初始和最终加速度难以保证连续,会出现突变。下面需要做的就是需要保证在指定初始速度和最终速度的前提下,还要保证初始、最终加速度从0开始连续变化。如果这些不连续代表一个问题,可以采用不同的方法:

    • 一个5次多项式函数可以用于第一个和最后一个域,其缺点是允许在这些段中有更大的超调,并且稍微增加了计算负担;
    • 在第一段和最后一段中添加两个自由额外点(从这个意义上说,这些点不能先验地固定),并通过施加所需的速度和加速度的初始值和最终值来计算它们的值。

    后一种方法现在详细说明。让我们考虑一个要插值的n-1点向量,这两个向量中都没有第二个值以及倒数第二个值,也就是q1、t1和qn-1、tn-1(目前我还不知道为啥要这么做。。。)

    d858d78e7cb2c55e9e09e7217a6462b6.png

    对应的时间节点为

    6bb64a3a9b0f13a7cf38fdbe76026d63.png

    以及同时考虑速度v0,vn和加速度a0,an的边界条件。为了施加所需的加速度,增加了两个额外的点$bar{q}{1}$ 和 $bar{q}{n-1}$。时间瞬间$overline{t_{1}}$ 和 $bar{t}_{n-1}$分别位于t0和t2之间以及tn-2和tn之间。不过可以分析得到,这种处理办法虽然能够对一条轨迹施加轨迹长度、初始速度、终止速度、初始加速度、终止加速度这五种约束条件,但是前提是额外增加两个轨迹点以及时间点,这样可能破坏时间最优规划的初衷,额外增加约束可能也会导致轨迹灵活性变差。。。

    9532f6742464170386c6746498331fb5.png

    增加的这两个点,可以通过已知的变量去表达这两个点,即初始/最终的位置、速度以及加速度(q0/qn,v0/vn,a0/an)同时包括在这些点上的加速度w1和wn-1(其中边界点的加速度是用a0和an来表示,而中间点的加速度是用w来表示)。这样,就可以考虑初始加速度和最终加速度的约束。

    7bea52ea86bfc2e4bf3cae8af2fb6c3f.png

    将(4.26)和(4.27)替换掉(4.24)中的有关项,通过重新排列n-1方程,我们得到一个线性系统

    be00fa2265627c34f35e8419ba969834.png

    其中

    b9bbfedf9610d314f26a194b9c7feb59.png

    80bb0ad9120cf186c109b5549c809914.png

    注意,T0、T1和Tn-2、Tn-1分别是$overline{t_{1}}$ 和 $bar{t}_{n-1}$的函数,可以在间隔(t0、t2)和(tn-2、tn)中任意选择,例如

    8efd34f774676540bf3d5cc0ec6b3b6f.png

    通过求解方程组(4.28)可以得到中间插补点的加速度为

    9e742df64911efd73b9cabc9adb1fe27.png

    与边界值a0和an一起,就可以根据(4.14)或者(4.25)计算整体的样条曲线。

    例子

    bc9c540af2369d432e674874b8a3beae.png

    其中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('加速度')

    a40964111a193d725e9594c4b85777b7.png

    三次样条部分还有平滑三次样条,后面再写,今天就到这里~

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

    Trajectory Planning for Automatic Machines and Robots

    展开全文
  • 这就需要进行带时间参数的轨迹规划处理,也就是对这条空间轨迹进行速度、加速度约束,并且计算运动到每个路点的时间,高级的算法有TOPP等,一般的呢就是贝塞尔、三次准/非/均匀B、五次及三次样条等。下面从最简单的...

    写在前面

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

    三次样条曲线性质

    当给出n+1个点时,可以使用n个p次多项式(通常较低)代替唯一的n次插值多项式,每个多项式定义一段轨迹。以这种方式定义的总函数s(t)称为p阶的样条曲线。p的值是根据所需的样条连续度来选择的。例如,为了在两个连续段之间发生过渡的时刻tk获得速度和加速度的连续性,可以假定多项式的阶数p=3(三次多项式)。

    定义三次样条曲线的函数形式为:

    这段轨迹由n个三次多项式构成,并且每个多项式需要计算四个参数。由于n个多项式是定义一条通过n+1点的轨迹所必需的,因此需要确定的系数总数为4n。为了解决这个问题,必须考虑以下条件:给定点插值的2n条件,因为每一个三次函数必须在其极值处穿过点。

    n-1个条件,过渡点的速度要连续;

    n-1个条件,过渡点的加速度要连续;

    这样的话,就已经限制了2n+2(n-1)个条件,还剩下2个自由度还未限制。通过前面分析,还需要两个限制条件才行,这里讨论的就是初始点和终点的速度以及加速度。下面是几种可能的选择,可以任意选择:

    通常情况,样条曲线具有如下几个特性:对于由给定点(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,就有如下几个条件成立:

    可以最终确定样条曲线的函数s(t)为

    系数ak,i可以由以下算法进行确定:

    第一种情况,如果中间点(插补点)的速度我们已知,也就是vk,k=1,…,n-1,对于每段三次样条曲线,有

    其中,$T_{k}=t_{k+1}-t_{k}$。通过解上面的方程,可以得到

    这就可以把每一段曲线的系数都求出来,从而得到样条曲线。这是最简单的情况!

    子函数如下:

    % 三次样条:指定初始速度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均未知,然而这些值是必须被计算的。为此,考虑了中间加速度的连续条件:

    在这些条件下,通过考虑参数$a_{k, 2}, \quad a_{k, 3}, \quad a_{k+1,2}$的表达式乘以$\left(T_{k} T_{k+1}\right) / 2$,在简单计算整理之后能够得到

    其中k=0,…,n-2。

    上面的关系可以整理成矩阵的形式,$A^{\prime} v^{\prime}=c^{\prime}$,其中

    其中常数项ck仅取决于中间位置和已知的样条曲线段的持续时间Tk。由于速度v0和vn也是已知的,所以可以消除矩阵A‘的相应列并获得

    也就是

    例子

    另外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('加速度')

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

    在许多应用中,要执行的运动是周期性的,即初始位置和最终位置相同。在这种情况下,利用为计算样条曲线而指定的最后两个自由度,以使曲线具有初始和最终速度和加速度的连续性。因此,计算系数的方法与先前报告的略有不同。事实上,在这种情况下,必须考虑代替任意选择的初始和最终速度v0和vn的条件。

    最后一个公式可以写成:

    代入系数表达式后,从(4.8)中得到

    通过将该方程添加到系统(4.10)中,并考虑到在这种情况下,速度vn等于v0但未知(因此在(4.10)中,必须在左侧移动$T_{n-2}v_n$和$T_1v_0$),计算速度的线性系统变为

    系统的矩阵不再是三对角的。这种情况被称为循环三对角系统,也存在有效的求解计算方法。一旦获得速度v0,…,vn−1,就可以通过(4.8)计算样条曲线的系数。

    例子

    % 三次样条:周期三次样条,没有指定初始速度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('加速度')

    具有指定初始速度v0和最终速度vn的三次样条(v0和vn已知):基于加速度的计算

    定义三次样条还有一种方法:样条曲线的一般多项式qk(t)可以表示为在其端点处计算的二阶导数的函数,也就是加速度$\ddot{q}\left(t_{k}\right)=\omega_{k}{k=0,...,n}$,,而不是速度$v_k$。可以计算得到此时的多项式表达式为

    这样的话,速度和加速度就是如下计算式

    这样的话,未知数就是加速度$\omega_k$,因为从上面的计算式可以发现,曲线的未知参数就是加速度了,因此它是唯一定义样条曲线的。由于中间点的速度和加速度的连续性,我们得到

    结合式(4.15)、(4.17)和(4.18),可以得到

    其中k=1,…,n-1。另外,初始速度和最终速度有如下条件

    由此可以推导得到

    结合(4.19)-(4.21),可以得到如下线性系统

    其中,(n+1)阶三对角对称矩阵A(其中未知参数$\omega=[\omega_0,\omega_1,...,\omega_n]^T$)为

    同样是利用追赶法解三对角线方程组,通过将解方程得到的$\omega _k$带入(4.14)中,最终得到样条曲线。除了利用(4.14)来描述三次样条曲线之外,显然,还是可以根据初始定义来描述三次样条曲线,即

    其中样条曲线的参数a呢就可以通过已知的点$q_k$和已经求得的加速度$\omega _k$来计算,计算式如下

    这个方法主要是为下面的方法做准备,因此不写例子。

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

    样条曲线是一个二阶连续导数的函数,但通常不可能同时指定初始速度和最终速度以及加速度。因此,样条曲线在其末端的特征是速度或加速度的不连续性,一般情况下我们会指定初始和最终速度,则此时初始和最终加速度难以保证连续,会出现突变。下面需要做的就是需要保证在指定初始速度和最终速度的前提下,还要保证初始、最终加速度从0开始连续变化。如果这些不连续代表一个问题,可以采用不同的方法:一个5次多项式函数可以用于第一个和最后一个域,其缺点是允许在这些段中有更大的超调,并且稍微增加了计算负担;

    在第一段和最后一段中添加两个自由额外点(从这个意义上说,这些点不能先验地固定),并通过施加所需的速度和加速度的初始值和最终值来计算它们的值。

    后一种方法现在详细说明。让我们考虑一个要插值的n-1点向量,这两个向量中都没有第二个值以及倒数第二个值,也就是q1、t1和qn-1、tn-1(目前我还不知道为啥要这么做。。。)

    对应的时间节点为

    以及同时考虑速度v0,vn和加速度a0,an的边界条件。为了施加所需的加速度,增加了两个额外的点$\bar{q}{1}$ 和 $\bar{q}{n-1}$。时间瞬间$\overline{t_{1}}$ 和 $\bar{t}_{n-1}$分别位于t0和t2之间以及tn-2和tn之间。不过可以分析得到,这种处理办法虽然能够对一条轨迹施加轨迹长度、初始速度、终止速度、初始加速度、终止加速度这五种约束条件,但是前提是额外增加两个轨迹点以及时间点,这样可能破坏时间最优规划的初衷,额外增加约束可能也会导致轨迹灵活性变差。。。

    增加的这两个点,可以通过已知的变量去表达这两个点,即初始/最终的位置、速度以及加速度(q0/qn,v0/vn,a0/an)同时包括在这些点上的加速度w1和wn-1(其中边界点的加速度是用a0和an来表示,而中间点的加速度是用w来表示)。这样,就可以考虑初始加速度和最终加速度的约束。

    将(4.26)和(4.27)替换掉(4.24)中的有关项,通过重新排列n-1方程,我们得到一个线性系统

    其中

    注意,T0、T1和Tn-2、Tn-1分别是$\overline{t_{1}}$ 和 $\bar{t}_{n-1}$的函数,可以在间隔(t0、t2)和(tn-2、tn)中任意选择,例如

    通过求解方程组(4.28)可以得到中间插补点的加速度为

    与边界值a0和an一起,就可以根据(4.14)或者(4.25)计算整体的样条曲线。

    例子

    其中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('加速度')

    三次样条部分还有平滑三次样条,后面再写,今天就到这里~

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

    Trajectory Planning for Automatic Machines and Robots

    展开全文
  • 这就需要进行带时间参数的轨迹规划处理,也就是对这条空间轨迹进行速度、加速度约束,并且计算运动到每个路点的时间,高级的算法有TOPP等,一般的呢就是贝塞尔、三次准/非/均匀B、五次及三次样条等。下面从最简单的...
  • Trajectory Generation and Following参考资料贝塞尔曲线基本概念一阶贝塞尔曲线二阶贝塞尔曲线三阶贝塞尔曲线问题 参考资料 matlab2019b 开源代码 Matlab官网: matlab. 贝塞尔曲线 基本概念 参考资料: 深入理解...
  • 样条曲线反算公式

    千次阅读 2018-04-22 10:47:02
    %------------------非均匀B样条拟合MATLAB程序-----------------cleark=3;x=load('data.txt');[n,m]=size(x);%-----------弦长参数化--------------------------------------u(k+n)=0;for i=1:n-1 u(k+i+1)=u(k+i)...
  • 所以 B 样条曲线(B-Spline)为了解决贝塞尔曲线的缺陷应运而生。 不了解贝塞尔曲线的同学,可以去看我以前写的另外一篇文章《深入理解贝塞尔曲线》,后面的内容会假设你已经了解并掌握贝塞尔曲线的相关内容。什么是 B...
  • 早期工程师制图时,把富有弹性的细长木条(所谓样条)用压铁固定在样点上,在其他地方让它自由弯曲,然后沿木条画下曲线,成为样条曲线。 设函数S(x)∈C2[a,b] ,且在每个小区间[xj, xj+1]上是三次多项式,其中a=x0...
  • y=3.36 3.30 3.33 2.75 2.43若直接采用matlab绘制曲线得到不是很完美的一组曲线:为了更好的绘制该曲线,采用插值的方式来绘制。matlab中常用的插值方式有以下几种:'nearest'最近项插值; 'linear'线性插值 ;'...
  • 三次样条函数csapi 插值生成三次样条函数csape 生成给定约束条件下的三次样条函数csaps 平滑生成三次样条函数cscvn 生成一条内插参数的三次样条曲线getcurve 动态生成三次样条曲线2. 分段多项式样条函数ppmak 生成...
  • MATLAB样条工具箱可以通过节点获得样本函数值,但不能根据x求y或z,也不能求得样本曲线方程。例如:ctrlpoints=[0 -1.2 -1.6 -1.4 -1 -0.5 -0.35-0.6 -1.6-0.2 -0.5-1 -1.5-2.2 -2.7-3.2 -3.7-4.2];knots=[0 0 0 0 1...
  • 如何使用MATLAB绘制平滑曲线

    万次阅读 2013-07-26 05:56:12
    MATLAB中绘制平滑曲线一般使用最小二乘法或者B样条插值。  最小二乘法实际上是函数拟合,可以得到目标函数(这里为多项式)的系数,对outliers相对不敏感,缺点是需要预先设置目标函数的阶数,且有时不容易找到最优...
  • http://zhan.renren.com/3caddesignHigh-speed,high-precision and high-efficiency A1-CNC.MATLAB 样条工具箱可以通过节点获得样本函数值,但不能根据x求y或z,也不能求得样本曲线方程。例如:ctrlpoints=[0 -1.2...
  • 在做相关项目需要解决B样条插值问题,记录如下 感谢以下参考资料的帮助 % ref: 闭合 B 样条曲线控制点的快速求解算法及应用 % http://www.doc88.com/p-5714423317458.html % ...
  • 2)用多项式样条进行B样条回归;3) 进行非线性回归。在此示例中,这三个中的每一个都将找到基本相同的最佳拟合曲线。多项式回归多项式回归实际上只是多元回归的一种特殊情况。对于线性模型(lm),调整后的R平方...
  • Matlab三次均匀B样条曲线插值函数

    热门讨论 2014-03-25 11:25:23
    对给定的点进行三次B样条插值,得到插值曲线,这里给定的点可以是二维平面上的点或三维点,注意输入的点矩阵要每行为一个点坐标,里面都有注释,可以自己简单修改封装成自己想要的带参函数,里面有测试的点数据,...
  • 今天给大家分享的是SolidWorks节能灯的建模,之前给大家分享过一个节能灯,相比之下,这个更难一点,主要用到螺旋线和3D草图、样条曲线等功能,希望大家可以实践一下,提高自己的建模水平,如果你感觉比较吃力,那么...
  • 1.B-样条曲线教程(B-spline Curves Notes)目录 2.运动规划——B样条曲线 3.B样条曲线(B-spline Curves) 4.基于B样条曲线的路径规划(含matlab代码) 5.B样条曲线拟合原理
  • 基于遗传算法优化B样条曲线一、B样条曲线学习二、遗传算法学习 一、B样条曲线学习 B-样条曲线入门 B-spline Curves 定义 二、遗传算法学习 详解遗传算法(含MATLAB代码)
  • matlab实现b样条线拼接

    2012-05-31 08:39:58
    jlu的作业必备啊,*chexueyuan做b样条作业,要的,你懂的.用用Matlab实现了3次样条曲线插值的算法边界条件取为自然.doc ,就是三次b样条线的算法,下了就知道
  • 又到了金九银十的月份了,收获的季节。今天CAD易学堂书写一篇关于【窗帘】绘制的完整大纲,感兴趣的同学可以练习、评论、收藏...教学重难点教学重点:学会运用样条曲线命令绘制窗帘。教学难点:结合对象捕捉运用样条...
  • B样条曲线反算.zip

    2019-07-25 10:35:15
    给定数据点,反算节点矢量和控制点,构造三次B样条插值曲线,使得曲线通过数据点。理论参考《计算机辅助几何设计与非均匀有理B样条》254页-262页。程序语言为matlab,main为主程序,Bbasis为计算B样条基函数程序。
  • 五次B样条曲线

    2020-12-04 19:38:37
    五次B样条曲线 MATLAB 的实现 ...MATLABB样条插值实现的函数是spapi() spline = spapi(knots,x,y) returns the spline f (if any) of order ​ k = length(knots) - length(x) with knot seq
  • 第36章 样条曲线的端点与节点处理类函数 第37章 解线性方程组类函数 第38章 样条GUI函数 第五篇 信号处理工具箱 第39章 采样与波形发生 第40章 模拟滤波器设计 第41章 数字滤波器设计 第42章 滤波器分析 第43章 随机...
  • MATLAB 插值+计算离散点曲率

    千次阅读 2020-07-10 21:03:58
    样条曲线B-Spline)插值得到拟合曲线 diff、gradient 函数求拟合曲线的一、二阶导数 最后通过公式求得曲率 公式: 例:余弦函数取 8 个点,用 B-Spline 插值 x = 0:1:7; y = cos(x*0.5*pi); xx=0:0.01:7...
  • Lagrange插值 Hermite插值 Runge现象和分段插值 分段插值 样条插值的MATLAB表示 多项式拟合 函数线性组合的曲线拟合方法 最小二乘曲线拟合 B样条函数及其MATLAB表示
  • B b bar 二维直方图 bar3 三维直方图 bar3h 三维水平直方图 barh 二维水平直方图 base2dec X进制转换为十进制 bin2dec 二进制转换为十进制 blanks 创建空格串 bone 蓝色调黑白色图阵 box 框状坐标轴 ...
  • 三次b样条画图程序

    2014-03-03 18:49:20
    matlab编写的三次b样条绘图程序,对于初学者来讲是一难得的入门程序。通过该程序,初始研究样条曲线的新手,可以体会节点、控制点及次数是怎样来改变曲线形状的。
  • % MATLAB数学建模工具箱 % % 本工具箱主要包含三部分内容 % 1. MATLAB常用数学建模工具的中文帮助 % 2. 贡献MATLAB数学建模工具(打*号) % 3. 中国大学生数学建模竞赛历年试题MATLAB程序 % 数据拟合 % interp1 - ...

空空如也

空空如也

1 2 3
收藏数 42
精华内容 16
关键字:

matlabb样条曲线

matlab 订阅