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

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] endq0 = 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*Czeros(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*Czeros(size(A))]'; %求加速度，求二次导 qddt = tt*c/tscal^2; end

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