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

matlab机器人工具箱 轨迹生成函数jtraj代码详解Matlab中nargin变量是函数输入参数的个数。nargout变量是函数输出的个数轨迹生成函数jtraj()的代码详解:function[qt,qdt,qddt]=jtraj(q0,q1,tv,qd0,qd1)一般情况下输入参数:q0初始各个关节的角度。q1最终点的各个关节的角度,tv:可以理解为时间,也可以理解为步数。一般用这三个就够了。函数会将qd0,qd1…

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

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

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

 

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

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

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

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

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

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

约束方程的解为:

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

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

根据上面所说的初始点与终止点速度,加速度为0.同时jtraj做了归一化,将matlab机器人工具箱 轨迹生成函数jtraj代码详解 归一化为1.所以解化简为:

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

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

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

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

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

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

现在回过头来看代码:

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

 

今天的文章matlab机器人工具箱 轨迹生成函数jtraj代码详解分享到此就结束了,感谢您的阅读,如果确实帮到您,您可以动动手指转发给其他人。

版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。
如需转载请保留出处:https://bianchenghao.cn/28807.html

(0)
编程小号编程小号

相关推荐

发表回复

您的电子邮箱地址不会被公开。 必填项已用*标注