Matlab中的轨迹规划函数
本篇博文主要是从robotic toolbox的源码出发,理解机器人学中的轨迹规划。
参考
robotics toolbox一键安装
-
官方下载链接->petercorke
-
添加工具箱->链接
笛卡尔空间轨迹规划
ctraj
可以理解为输入“起点位姿”、“终点位姿”和“步长”,求出过程的齐次变换矩阵。
源代码
%CTRAJ Cartesian trajectory between two poses
%
% TC = CTRAJ(T0, T1, N) is a Cartesian trajectory (4x4xN) from pose T0 to T1
% with N points that follow a trapezoidal velocity profile along the path.
% The Cartesian trajectory is a homogeneous transform sequence and the last
% subscript being the point index, that is, T(:,:,i) is the i'th point along
% the path.
%
% TC = CTRAJ(T0, T1, S) as above but the elements of S (Nx1) specify the
% fractional distance along the path, and these values are in the range [0 1].
% The i'th point corresponds to a distance S(i) along the path.
%
% Notes::
% - If T0 or T1 is equal to [] it is taken to be the identity matrix.
% - In the second case S could be generated by a scalar trajectory generator
% such as TPOLY or LSPB (default).
% - Orientation interpolation is performed using quaternion interpolation.
%
% Reference::
% Robotics, Vision & Control, Sec 3.1.5,
% Peter Corke, Springer 2011
%
% See also LSPB, MSTRAJ, TRINTERP, UnitQuaternion.interp, SE3.ctraj.
% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
function traj = ctraj(T0, T1, t)
% T0 = SE3.check(T0);
% T1 = SE3.check(T1);
% distance along path is a smooth function of time
if isscalar(t)
s = lspb(0, 1, t);
else
s = t(:);
end
for i=1:length(s)
traj(:,:,i) = trinterp(T0, T1, s(i));
end
end
基于球形四元数的姿态插补
%TRINTERP Interpolate SE(3) homogeneous transformations
%
% T = TRINTERP(T0, T1, S) is a homogeneous transform (4x4) interpolated
% between T0 when S=0 and T1 when S=1. T0 and T1 are both homogeneous
% transforms (4x4). Rotation is interpolated using quaternion spherical
% linear interpolation (slerp). If S (Nx1) then T (4x4xN) is a sequence of
% homogeneous transforms corresponding to the interpolation values in S.
%
% T = TRINTERP(T1, S) as above but interpolated between the identity matrix
% when S=0 to T1 when S=1.
%
% See also CTRAJ, SE3.interp, UnitQuaternion, trinterp2.
% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
function T = trinterp(A, B, C)
if nargin == 3
% TR = TRINTERP(T0, T1, r)
T0 = A; T1 = B; r = C(:)';
if length(r) == 1 && r > 1 && (r == floor(r))
% integer value
r = [0:(r-1)] / (r-1);
end
assert(all(r>=0 & r<=1), 'RTB:trinterp:badarg', 'values of S outside interval [0,1]');
q0 = UnitQuaternion(T0);
q1 = UnitQuaternion(T1);
p0 = transl(T0);
p1 = transl(T1);
for i=1:length(r)
qr = q0.interp(q1, r(i));%spherical linear interpolation (slerp) 在此先不做赘述
pr = p0*(1-r(i)) + r(i)*p1;%线性插补
T(:,:,i) = rt2tr(qr.R, pr);
end
elseif nargin == 2
% TR = TRINTERP(T, r)
T0 = A; r = B(:)';
if length(r) == 1 && r > 1 && (r == floor(r))
% integer value
r = linspace(0, 1, r);
elseif any(r<0 | r>1)
error('RTB:trinterp', 'values of S outside interval [0,1]');
end
q0 = UnitQuaternion(T0);
p0 = transl(T0);
for i=1:length(r)
qr = q0.interp(r(i));
pr = r(i)*p0;
T(:,:,i) = rt2tr(qr.R, pr);
end
else
error('RTB:trinterp:badarg', 'must be 2 or 3 arguments');
end
这只是简单粗暴的点对点的一般线性插补,下一步应当主要研究两个点
关节端轨迹规划
jtraj(五次多项式的轨迹规划)
五次多项式的曲线轨迹规划不能够限制最大速度和加速度。
源码
%JTRAJ Compute a joint space trajectory
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, M) is a joint space trajectory Q (MxN) where the joint
% coordinates vary from Q0 (1xN) to QF (1xN). A quintic (5th order) polynomial is used
% with default zero boundary conditions for velocity and acceleration.
% Time is assumed to vary from 0 to 1 in M steps. Joint velocity and
% acceleration can be optionally returned as QD (MxN) and QDD (MxN) respectively.
% The trajectory Q, QD and QDD are MxN matrices, with one row per time step,
% and one column per joint.
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, M, QD0, QDF) as above but also specifies
% initial QD0 (1xN) and final QDF (1xN) joint velocity for the trajectory.
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, T) as above but the number of steps in the
% trajectory is defined by the length of the time vector T (Mx1).
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, T, QD0, QDF) as above but specifies initial and
% final joint velocity for the trajectory and a time vector.
%
% Notes::
% - When a time vector is provided the velocity and acceleration outputs
% are scaled assumign that the time vector starts at zero and increases
% linearly.
%
% See also QPLOT, CTRAJ, SerialLink.jtraj.
% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
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
end
q0 = q0(:);
q1 = q1(:);
if nargin == 3
qd0 = zeros(size(q0));
qd1 = qd0;
elseif nargin == 5
qd0 = qd0(:);
qd1 = qd1(:);
else
error('incorrect number of arguments')
end
% 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;
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
- Time is assumed to vary from 0 to 1 in M steps.
- M可以为步数,1秒内走的步数,也可以为向量,其中每个元素代表计算的时间点。
- 可以输入初始的速度和最后时刻的速度,但是加速度为零。
- % compute the polynomial coefficients
- 见下代码
syms y; syms A B C D E F; syms t; syms T; syms eq1 eq2 eq3 eq4 eq5 eq6 syms y0 dy0 ddy0 yT dyT ddyT % y=A*t^5+B*t^4+C*t^3+D*t^2+E*t+F; dy=diff(y,t) ddy=diff(dy,t) % % y0=subs(y,t,0) % dy0=subs(dy,t,0) % ddy0=subs(ddy,t,0) % % yT=subs(y,t,T) % dyT=subs(dy,t,T) % ddyT=subs(ddy,t,T) eq1=subs(y,t,0)-y0 eq2=subs(dy,t,0)-dy0 eq3=subs(ddy,t,0)-ddy0 eq4=subs(y,t,1)-yT eq5=subs(dy,t,1)-dyT eq6=subs(ddy,t,1)-ddyT [A,B,C,D,E,F]=solve(eq1, eq2, eq3, eq4, eq5, eq6,A,B,C,D,E,F)
- 测试代码
clear; clc; init_ang=0; targ_ang=pi/4; step=40 [q,qd,qdd] = Myjtraj(init_ang, targ_ang, step); subplot(3,1,1); i=1; plot(q(:,i)); title('位置'); grid on; subplot(3,1,2); i=1; plot(qd(:,i)); title('速度'); grid on; subplot(3,1,3); i=1; plot(qdd(:,i)); title('加速度'); grid on;
lspb(梯形轨迹规划)
可以理解为输入:起点位置、终点位置、步数和最大速度,求出梯形轨迹
源代码
%LSPB Linear segment with parabolic blend
%
% [S,SD,SDD] = LSPB(S0, SF, M) is a scalar trajectory (Mx1) that varies
% smoothly from S0 to SF in M steps using a constant velocity segment and
% parabolic blends (a trapezoidal velocity profile). Velocity and
% acceleration can be optionally returned as SD (Mx1) and SDD (Mx1)
% respectively.
%
% [S,SD,SDD] = LSPB(S0, SF, M, V) as above but specifies the velocity of
% the linear segment which is normally computed automatically.
%
% [S,SD,SDD] = LSPB(S0, SF, T) as above but specifies the trajectory in
% terms of the length of the time vector T (Mx1).
%
% [S,SD,SDD] = LSPB(S0, SF, T, V) as above but specifies the velocity of
% the linear segment which is normally computed automatically and a time
% vector.
%
% LSPB(S0, SF, M, V) as above but plots S, SD and SDD versus time in a single
% figure.
%
% Notes::
% - If M is given
% - Velocity is in units of distance per trajectory step, not per second.
% - Acceleration is in units of distance per trajectory step squared, not
% per second squared.
% - If T is given then results are scaled to units of time.
% - The time vector T is assumed to be monotonically increasing, and time
% scaling is based on the first and last element.
% - For some values of V no solution is possible and an error is flagged.
%
% References::
% - Robotics, Vision & Control, Chap 3,
% P. Corke, Springer 2011.
%
% See also TPOLY, JTRAJ.
% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
%TODO
% add a 'dt' option, to convert to everything to units of seconds
function [s,sd,sdd] = lspb(q0, q1, t, V)
t0 = t;
if isscalar(t)
t = (0:t-1)';
else
t = t(:);
end
plotsargs = {'Markersize', 16};
tf = max(t(:));
if nargin < 4
% if velocity not specified, compute it
V = (q1-q0)/tf * 1.5;
else
V = abs(V) * sign(q1-q0);
if abs(V) < abs(q1-q0)/tf
error('V too small');
elseif abs(V) > 2*abs(q1-q0)/tf
error('V too big');
end
end
if q0 == q1
s = ones(size(t)) * q0;
sd = zeros(size(t));
sdd = zeros(size(t));
return
end
tb = (q0 - q1 + V*tf)/V;
a = V/tb;
p = zeros(length(t), 1);
pd = p;
pdd = p;
for i = 1:length(t)
tt = t(i);
if tt <= tb
% initial blend
p(i) = q0 + a/2*tt^2;
pd(i) = a*tt;
pdd(i) = a;
elseif tt <= (tf-tb)
% linear motion
p(i) = (q1+q0-V*tf)/2 + V*tt;
pd(i) = V;
pdd(i) = 0;
else
% final blend
p(i) = q1 - a/2*tf^2 + a*tf*tt - a/2*tt^2;
pd(i) = a*tf - a*tt;
pdd(i) = -a;
end
end
switch nargout
case 0
if isscalar(t0)
% for scalar time steps, axis is labeled 1 .. M
xt = t+1;
else
% for vector time steps, axis is labeled by vector M
xt = t;
end
clf
subplot(311)
% highlight the accel, coast, decel phases with different
% colored markers
hold on
%plot(xt, p);
k = t<= tb;
plot(xt(k), p(k), 'r.-', plotsargs{:});
k = (t>=tb) & (t<= (tf-tb));
plot(xt(k), p(k), 'b.-', plotsargs{:});
k = t>= (tf-tb);
plot(xt(k), p(k), 'g.-', plotsargs{:});
grid; ylabel('$s$', 'FontSize', 16, 'Interpreter','latex');
hold off
subplot(312)
plot(xt, pd, '.-', plotsargs{:});
grid;
if isscalar(t0)
ylabel('$ds/dk$', 'FontSize', 16, 'Interpreter','latex');
else
ylabel('$ds/dt$', 'FontSize', 16, 'Interpreter','latex');
end
subplot(313)
plot(xt, pdd, '.-', plotsargs{:});
grid;
if isscalar(t0)
ylabel('$ds^2/dk^2$', 'FontSize', 16, 'Interpreter','latex');
else
ylabel('$ds^2/dt^2$', 'FontSize', 16, 'Interpreter','latex');
end
if ~isscalar(t0)
xlabel('t (seconds)')
else
xlabel('k (step)');
for c=findobj(gcf, 'Type', 'axes')
set(c, 'XLim', [1 t0]);
end
end
shg
case 1
s = p;
case 2
s = p;
sd = pd;
case 3
s = p;
sd = pd;
sdd = pdd;
end
求分段时间
%求分段时间
syms S1 S2 S3
syms V
syms T Ts
syms q1 q0
syms eq;
S1=0.5*(V/T)*T^2;
S2=V*(Ts-2*T);
S3=V*T-0.5*(V/T)*T^2;
eq=S1+S2+S3-q0-q1
[T]=solve(eq,T)
梯形轨迹规划加速度不连续对于工业机器人而言会发生卡顿等现象,因此提出
S曲线轨迹规划
主要是在考虑到了速度的平滑过度,保证机械臂不会发生卡顿。仿真结果见下图:
![](https://i-blog.csdnimg.cn/blog_migrate/410c64eff7902f1e83c61e92eebb5465.png)