matlab机器人工具箱 轨迹生成函数jtraj代码详解

Matlab中nargin变量是函数输入参数的个数。nargout变量是函数输出的个数

轨迹生成函数jtraj()的代码详解：

function [qt,qdt,qddt] = jtraj(q0, q1, tv, qd0, qd1)

 

一般情况下输入参数：q0初始各个关节的角度。q1最终点的各个关节的角度，tv：可以理解为时间，也可以理解为步数。一般用这三个就够了。函数会将qd0，qd1置零。

输出qt：不同时间内各个关节的角度，qdt，角速度，qddt，角加速度。

函数的核心是五次多项式插值。每一个角度的变化轨迹是个5次函数。

对于一次从静止点到静止点的运动，约束条件方程左边的值我们是知道的。一般情况下初始点的速度与加速度为0.即：

 

约束方程的解为：

根据上面所说的初始点与终止点速度，加速度为0.同时jtraj做了归一化，将 归一化为1.所以解化简为：

 

现在回过头来看代码：

function [qt,qdt,qddt] = jtraj(q0, q1, tv, qd0, qd1)
    if length(tv) > 1
        tscal = max(tv);
        t = tv(:)/tscal;
    else
        tscal = 1;
        t = (0:(tv-1))'/(tv-1); % normalized time from 0 -> 1 将时间或者步数缩放到[0,1]
    end

    q0 = q0(:);
    q1 = q1(:);    

    if nargin == 3        %一般输入参数为3，qd0,qd1置0
        qd0 = zeros(size(q0));
        qd1 = qd0;
    elseif nargin == 5
        qd0 = qd0(:);
        qd1 = qd1(:);
    else
        error('incorrect number of arguments')
    end

    %最基本的情况，加速度，速度都为0，下面的多项式系数求解，将加速度置0，保留了速度
    %中间运动，速度可以不为0，tscal一般为1
    % compute the polynomial coefficients
    A = 6*(q1 - q0) - 3*(qd1+qd0)*tscal;
    B = -15*(q1 - q0) + (8*qd0 + 7*qd1)*tscal;
    C = 10*(q1 - q0) - (6*qd0 + 4*qd1)*tscal;
    %这里没没有D,D其实是初始点角加速度的1/2.初始加速度为0，所以D为0
    E = qd0*tscal; % as the t vector has been normalized
    F = q0;

     %这里面是通过矩阵运算，将各个时间的角度一块算了出来。
    tt = [t.^5 t.^4 t.^3 t.^2 t ones(size(t))];
    c = [A B C zeros(size(A)) E F]';    %系数矩阵，需要转置一下
    
    qt = tt*c;  %求得角度

    % compute optional velocity
    if nargout >= 2
        c = [ zeros(size(A)) 5*A 4*B 3*C  zeros(size(A)) E ]';     %求速度系数矩阵发生了改变
        qdt = tt*c/tscal;                                                        %对轨迹函数求一次导
    end

    % compute optional acceleration
    if nargout == 3
        c = [ zeros(size(A))  zeros(size(A)) 20*A 12*B 6*C  zeros(size(A))]';    %求加速度，求二次导
        qddt = tt*c/tscal^2;
    end