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
今天的文章matlab机器人工具箱 轨迹生成函数jtraj代码详解分享到此就结束了,感谢您的阅读,如果确实帮到您,您可以动动手指转发给其他人。
版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。
如需转载请保留出处:https://bianchenghao.cn/28807.html