（三）抛物线过渡的线性函数规划

最新推荐文章于 2022-09-22 19:30:40 发布

我什么都布吉岛

最新推荐文章于 2022-09-22 19:30:40 发布

阅读量2.9k

点赞数 5

分类专栏： # 运动规划文章标签：速度规划梯形加减速

本文链接：https://blog.csdn.net/weixin_39258979/article/details/106015531

版权

运动规划专栏收录该内容

10 篇文章 73 订阅

订阅专栏

前面说到，无论是三次还是五次多项式进行规划存在以下缺点：

位移往返
没有匀速段

这一节中，我们的研究对象是初速度和末速度都为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$ 对应的过渡时间 $t b$ 也相应确定，其大小为： $t_b=t/2-\sqrt{(a^2t^2-4a(\theta_f-\theta_0))}/2a$ ，相对于多项式插补，梯形规划方式既有匀速段且位置没有往返。