（三）拋物線過渡的線性函數規劃

前面說到，無論是三次還是五次多項式進行規劃存在以下缺點：

• 位移往返
• 沒有勻速段

這一節中，我們的研究對象是初速度和末速度都爲0關節規劃運動。

一、無過渡線性函數

假設時刻$t$和角度$\theta$是線性關係，其係數爲$v$，有：
$\theta(t)=v*t$
在整一段運動中，其速度恆定爲$v$,假設在這段運動前的速度爲$v_{pre}$,如果速度差$\Delta v=v-0=v\not=0$,根據加速度計算公式，在一個極短的時間$\Delta t$有：
$a=\Delta v/\Delta t=\infty$

二、拋物線擬合的線性函數

爲了避免端點處加速度的"衝擊"，在運動的起末兩點採用用兩段拋物線進行平滑地改變速度大小。記兩端待求拋物線的時間爲$t_b$,總持續時間爲$t$，加速度爲$a$，速度從零變化到勻速$v$，起末角爲$\theta_0$和$\theta_f$，作出關節的角速度曲線：

紅色面積代表運動的角度變化量即$\theta_f-\theta_0$，有梯形面積公式有：$S_{red}=(t-2t_b+t)\cdot a\cdot t_b/2$，整理得：
$a\cdot t_b^2-a\cdot t\cdot tb+\theta_f-\theta_0=0$
二次方程有解的條件是：$\Delta=a^2t^2-4a(\theta_f-\theta_0)\ge0$，即加速度需要滿足$a\ge4(\theta_f-\theta_0)/t^2$,對應的解$t_b=t/2-\sqrt{(a^2t^2-4a(\theta_f-\theta_0))}/2a$

三、Matlab驗證程序

% Velocity plan according accelerate 
% duration: total time of the motion

function [real_a,line_v,line_duration]=plan(set_a,delta_x,duration)
    while(1)
        min_acc=4*delta_x/(duration^2); %minimun acc
        if set_a<min_acc
            duration=duration+0.5;
            
        else
            real_a=set_a;
            break;
        end
    end
    tb=duration/2-sqrt(real_a^2*duration^2-4*real_a*(delta_x))/(2*real_a);
    line_duration=duration-2*tb;
    line_v=real_a*tb;
end

function x=move(start_x,end_x,ts,te,t,set_a)
    duration=te-ts;
    delta_x=end_x-start_x;
    [real_a,line_v,line_durasion]=plan(set_a,delta_x,duration);
    
    acc_duration=(duration-line_durasion)/2;
    tt=t-ts;
    if tt>=0&&tt<=acc_duration
        dx=1/2*real_a*tt^2;
    elseif tt>acc_duration&&tt<line_durasion+acc_duration
        dx=1/2*real_a*acc_duration^2+line_v*(tt-acc_duration);
    else
        dx=1/2*real_a*acc_duration^2+line_v*line_durasion+(line_v*(tt-acc_duration-line_durasion)-1/2*real_a*(tt-acc_duration-line_durasion)^2);
    end
    x=start_x+dx;
end

clear;
clc;
close all;

ts=5;
te=10;
theta1=5;
theta2=185;

h = figure(1);
axis tight manual % this ensures that getframe() returns a consistent size
filename = 'testAnimated.gif';
for set_a=50:50:1500
    theta=[]
    for t=ts:0.01:te
        theta=[theta,move(theta1,theta2,ts,te,t,set_a)];
    end
   % figure(1);
    t=ts:0.01:te;
    subplot(3,1,1);
    plot(t,theta,'r','LineWidth',1.2);
    ylabel('position')
    subplot(3,1,2);
    plot(t(1:end-1),diff(theta/0.01),'g','LineWidth',1.2);
    ylabel('velocity')
    subplot(3,1,3);
    plot(t(1:end-2),diff(diff(theta/0.01)),'b','LineWidth',1.2);
    ylabel('acceleration') 
    xlabel('time(t/s)')
 
    drawnow
    % Capture the plot as an image
    frame = getframe(h);
    im = frame2im(frame);
    [imind,cm] = rgb2ind(im,256);
    % Write to the GIF File
    if set_a==50
        imwrite(imind,cm,filename,'gif', 'Loopcount',inf);
    else
        imwrite(imind,cm,filename,'gif','WriteMode','append');
    end
end

仿真的結果如下：

隨着加速度的增加，角位移的曲線逐漸變成線性，同時所需要的加速度也隨之增加。

總結

爲了防止起始和末端點速度的“衝擊”，我們在兩端添加兩段拋物線進行過渡，拋物線加速度有最小值$4(\theta_f-\theta_0)/t^2$,對於確定的加速度$a$對應的過渡時間$tb$也相應確定，其大小爲：$t_b=t/2-\sqrt{(a^2t^2-4a(\theta_f-\theta_0))}/2a$，相對於多項式插補，梯形規劃方式既有勻速段且位置沒有往返。

[1] M. G , Rodd. Introduction to robotics: Mechanics and control: John J. Craig[J]. Automatica, 1987.