【机器人算法】轨迹规划

---

本篇博文主要是从robotic toolbox的源码出发，理解机器人学中的轨迹规划。

参考

• 点对点轨迹规划: CSDN.
• 点对点轨迹规划2: CSDN.
• 多轴插补为什么普遍使用梯形速度曲线？: 知乎.

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    

这只是简单粗暴的点对点的一般线性插补，下一步应当主要研究两个点

• 运动学逆解
• B样线条
  • 链接: Matlab.
• 路径规划
• 关于插补: csdn.

---

关节端轨迹规划

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曲线轨迹规划

主要是在考虑到了速度的平滑过度，保证机械臂不会发生卡顿。仿真结果见下图：