機器人中的軌跡規劃（Trajectory Planning ）

 

 Figure. Several possible path shapes for a single joint

五次多項式曲線(quintic polynomial)

 

$$\theta(t)=a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$$

 　　考慮邊界條件：

$$\begin{align*} 
\theta_0&=a_0\\
\theta_f&=a_0+a_1t+a_2{t_f}^2+a_3{t_f}^3+a_4{t_f}^4+a_5{t_f}^5\\
\dot{\theta_0}&=a_1\\
\dot{\theta_f}&=a_1+2a_2t_f+3a_3{t_f}^2+4a_4{t_f}^3+5a_5{t_f}^4\\
\ddot{\theta_0}&=2a_2\\
\ddot{\theta_f}&=2a_2+6a_3{t_f}+12a_4{t_f}^2+20a_5{t_f}^3\\
\end{align*}$$

 　　這6組約束構成了一個6個未知數的線性方程組，能夠求出係數爲：

$$\begin{align*} 
a_0&=\theta_0\\
a_1&=\dot{\theta_0}\\
a_2&=\frac{\ddot{\theta_0}}{2}\\
a_3&=\frac{20\theta_f-20\theta_0-(8\dot{\theta_f}+12\dot{\theta_0})t_f-(3\ddot{\theta_0}-\ddot{\theta_f}){t_f}^2}{2{t_f}^3}\\
a_4&=\frac{30\theta_0-30\theta_f+(14\dot{\theta_f}+16\dot{\theta_0})t_f+(3\ddot{\theta_0}-2\ddot{\theta_f}){t_f}^2}{2{t_f}^4}\\
a_5&=\frac{12\theta_f-12\theta_0-(6\dot{\theta_f}+6\dot{\theta_0})t_f-(\ddot{\theta_0}-\ddot{\theta_f}){t_f}^2}{2{t_f}^5}
\end{align*}$$

 

 　　在MATLAB機器人工具箱中函數tpoly能夠用於計算並生成機器人單軸的五次多項式軌跡曲線。當$t \in [0,T]$時，五次多項式曲線以及其一階導數、二階導數都是連續光滑的多項式曲線：

$$\begin{align*} 
S(t)&=At^5+Bt^4+Ct^3+Dt^2+Et+F\\
\dot{S}(t)&=5At^4+4Bt^3+3Ct^2+2Dt+E\\
\ddot{S}(t)&=20At^3+12Bt^2+6Ct+2D
\end{align*}$$

　　根據約束條件

　　能夠寫出矩陣方程以下：

　　利用MATLAB提供的左除（反除）操做符，能夠方便的求解線性方程組：Ax=b → x=A\b（表示矩陣A的逆乘以b)

　　 tpoly.m主要內容以下：

%TPOLY Generate scalar polynomial trajectory

% [S,SD,SDD] = TPOLY(S0, SF, T, SD0, SDF) as above but specifies initial 
% and final joint velocity for the trajectory and time vector T.

function [s,sd,sdd] = tpoly(q0, qf, t, qd0, qdf)

    if isscalar(t)
        t = (0:t-1)';
    else
        t = t(:);
    end
    if nargin < 4
        qd0 = 0;
    end
    if nargin < 5
        qdf = 0;
    end
                
    tf = max(t);
    % solve for the polynomial coefficients using least squares
    X = [
        0           0           0         0       0   1
        tf^5        tf^4        tf^3      tf^2    tf  1
        0           0           0         0       1   0
        5*tf^4      4*tf^3      3*tf^2    2*tf    1   0
        0           0           0         2       0   0
        20*tf^3     12*tf^2     6*tf      2       0   0
    ];
    coeffs = (X \ [q0 qf qd0 qdf 0 0]')';

    % coefficients of derivatives 
    coeffs_d = coeffs(1:5) .* (5:-1:1);
    coeffs_dd = coeffs_d(1:4) .* (4:-1:1);

    % evaluate the polynomials
    p = polyval(coeffs, t);
    pd = polyval(coeffs_d, t);
    pdd = polyval(coeffs_dd, t);

　　在MATLAB中輸入下面命令生成從位置0運動到1的五次多項式曲線（時間步數爲50步）：

>>  [s, sd, sdd] = tpoly(0, 1, 50);

　　其位置、速度、加速度曲線以下圖所示：

　　雖然這三條曲線都是連續且光滑的，但卻存在一個很實際的問題。從速圖曲線中能夠看出在t=25時速度達到最大值，沒有勻速段，其它時刻速度都小於最大值。平均速度除以最大速度的值爲：mean(sd) / max(sd) = 0.5231，即平均速度只有最大速度的一半左右，速度利用率較低。對於大多數實際伺服系統，電機的最大速度通常是固定的，所以但願速度曲線在最大速度的時間儘量長。

梯形速度曲線 / Linear segment with parabolic blend (LSPB) trajectory 

 　　高次多項式軌跡曲線的計算量比較大，咱們也能夠考慮用直線段來構造簡單的軌跡曲線，可是在不一樣直線段的交接處會發生速度跳變的狀況（位移曲線不光滑），若是用拋物線（parabolic blend）進行拼接就能夠獲得光滑的軌跡。以下圖所示，單軸從$t_0$開始勻加速運動（位移曲線爲拋物線）；$t_b$時刻達到最大速度，進行勻速直線運動（位移曲線爲直線段）；從$t_f-t_b$時刻開始進行勻減速運動，$t_f$時刻減速爲零併到達目標位置。曲線關於時間中點$t_h$對稱，因爲這種軌跡的速度曲線是梯形的，所以也稱爲梯形速度（trapezoidal velocity trajectory）曲線，在電機驅動器中被普遍使用。

Figure. Linear segment with parabolic blends

　　MATLAB機器人工具箱中函數lspb能夠用於計算並生成梯形速度曲線，下面的命令生成從位置0運動到1的梯形速度軌跡曲線，時間步數爲50步，最大速度爲默認值：

>>   [s, sd, sdd] = lspb(0, 1, 50);

　　另外也能夠指定最大速度（In fact the velocity cannot be chosen arbitrarily
, too high or toolow a value for the maximum velocity will result in an infeasible trajectory ）：

>> s = lspb(0, 1, 50, 0.025);
>> s = lspb(0, 1, 50, 0.035);

　　下圖a是默認最大速度的曲線，圖b是指定不一樣速度的對比。

  　　lspb.m的主要內容以下：

%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.

function [s,sd,sdd] = lspb(q0, q1, t, V)

    if isscalar(t)
        t = (0:t-1)';
    else
        t = t(:);
    end

    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;

    s = zeros(length(t), 1);
    sd = s;
    sdd = s;
    
    for i = 1:length(t)
        tt = t(i);

        if tt <= tb           % 勻加速段
            % initial blend
            s(i) = q0 + a/2*tt^2;
            sd(i) = a*tt;
            sdd(i) = a;
        elseif tt <= (tf-tb)  % 勻速段
            % linear motion
            s(i) = (q1+q0-V*tf)/2 + V*tt;
            sd(i) = V;
            sdd(i) = 0
        else                  % 勻減速段
            % final blend
            s(i) = q1 - a/2*tf^2 + a*tf*tt - a/2*tt^2;
            sd(i) = a*tf - a*tt;
            sdd(i) = -a;
        end
    end

 

 多自由度軌跡規劃 

 　　機器人工具箱中的函數mtraj能夠在內部調用單自由度軌跡生成函數，來生成多個軸的運動軌跡。mtraj第一個參數爲單自由度軌跡生成函數的句柄，q₀和q_f分別爲起始和目標點的座標（是一個多維向量）。

function [S,Sd,Sdd] = mtraj(tfunc, q0, qf, M) if ~isa(tfunc, 'function_handle') error('first argument must be a function handle'); end M0 = M; if ~isscalar(M) M = length(M); end
    if numcols(q0) ~= numcols(qf) error('must be same number of columns in q0 and qf') end s = zeros(M, numcols(q0)); sd = zeros(M, numcols(q0)); sdd = zeros(M, numcols(q0)); for i=1:numcols(q0) % for each axis [s(:,i),sd(:,i),sdd(:,i)] = tfunc(q0(i), qf(i), M); end

　　mtraj能夠調用tpoly或lspb，在50步內生成從(0, 2)運動到(1, -1)的軌跡。返回值x是一個50×2的矩陣，每一列表明一個軸的數據，每一行表明一個時間點。

>> x = mtraj(@tpoly, [0 2], [1 -1], 50);
>> x = mtraj(@lspb, [0 2], [1 -1], 50);
>> plot(x) 

　　在指定的時間內x₁從0運動到1，x₂從2運動到-1：

 

 

 

參考：

V-rep學習筆記：Reflexxes Motion Library 4

多軸插補爲何廣泛使用梯形速度曲線？

工業機器人運動軌跡規劃方法簡述

Introduction to Robotics - Mechanics and Control. Chapter 7 Trajectory generation

Robotics, vision and control fundamental algorithms in MATLAB Chapter 3 Time and Motion