六轴机器人轨迹规划之五次多项式插值

1.轨迹规划的定义
轨迹规划(trajectory planning)是运动规划(motion planning)研究的主要内容。运动规划指的是运动插补，在起始点和终止点之间插入中间点序列，实现沿着轨迹的平稳运动。运动控制包含路径规划(path planning)和轨迹规划，路径规划是规划位置，在起终点之间经过的路径点，轨迹规划是规划时间，将路径点与时间相对应。
对于我们的六轴机器人而言轨迹规划可以分为：关节空间轨迹规划和笛卡尔空间轨迹规划。关节空间轨迹规划是把机器人的关节变量变换成跟时间的函数，然后对角速度和角加速度进行约束。笛卡尔空间轨迹规划是把机器人末端在笛卡尔空间的位移、速度和加速度变换成跟时间的函数关系。
五次多项式插值能够解决三次多项式插值的角速度变化不平滑且加速度存在跳变的情况。
2.数学基础
五次多项式插值，角位移、角速度和角加速度的函数表达式为：

⎧⎩⎨⎪⎪⎪⎪θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5θ˙(t)=a1+2a2t+3a3t2+4a4t3+5a5t4θ¨(t)=2a2+6a3t+12a4t2+20a5t3 { θ ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 + a 5 t 5 θ ˙ ( t ) = a 1 + 2 a 2 t + 3 a 3 t 2 + 4 a 4 t 3 + 5 a 5 t 4 θ ¨ ( t ) = 2 a 2 + 6 a 3 t + 12 a 4 t 2 + 20 a 5 t 3

\begin{eqnarray*}\left\{ \begin{array}{l} \theta(t)=a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5\\ \dot{\theta}(t)=a_1+2a_2t+3a_3t^2+4a_4t^3+5a_5t^4\\ \ddot{\theta}(t)=2a_2+6a_3t+12a_4t^2+20a_5t^3 \end{array}\right. \end{eqnarray*}
约束条件如下，相对三次多项式插值，增加了对起止点角速度的约束（为简便计算设 t0 t 0 t_0为0）

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪θ(t0)=θ0=a0θ(tf)=θf=a0+a1tf+a2t2f+a3t3f+a4t4f+a5t5fθ˙(t0)=θ˙0=a1θ˙(tf)=θ˙f=a1+2a2tf+3a3t2f+4a4t3f+5a5t4fθ¨(t0)=θ¨0=2a2θ¨(tf)=θ¨f=2a2+6a3tf+12a4t2f+20a5t3f { θ ( t 0 ) = θ 0 = a 0 θ ( t f ) = θ f = a 0 + a 1 t f + a 2 t f 2 + a 3 t f 3 + a 4 t f 4 + a 5 t f 5 θ ˙ ( t 0 ) = θ ˙ 0 = a 1 θ ˙ ( t f ) = θ ˙ f = a 1 + 2 a 2 t f + 3 a 3 t f 2 + 4 a 4 t f 3 + 5 a 5 t f 4 θ ¨ ( t 0 ) = θ ¨ 0 = 2 a 2 θ ¨ ( t f ) = θ ¨ f = 2 a 2 + 6 a 3 t f + 12 a 4 t f 2 + 20 a 5 t f 3

\begin{eqnarray*}\left\{ \begin{array}{l} \theta(t_0)=\theta_0=a_0\\ \theta(t_f)=\theta_f=a_0+a_1t_f+a_2t_f^2+a_3t_f^3+a_4t_f^4+a_5t_f^5\\ \dot{\theta}(t_0)=\dot{\theta}_0=a_1\\ \dot{\theta}(t_f)=\dot{\theta}_f=a_1+2a_2t_f+3a_3t_f^2+4a_4t_f^3+5a_5t_f^4\\ \ddot{\theta}(t_0)=\ddot{\theta}_0=2a_2\\ \ddot{\theta}(t_f)=\ddot{\theta}_f=2a_2+6a_3t_f+12a_4t_f^2+20a_5t_f^3 \end{array}\right. \end{eqnarray*}
求解得

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪a0=θ0a1=θ˙0a2=θ¨02a3=20θf−20θ0−(8θ˙f+12θ˙0)tf−(3θ¨0−θ¨f)t2f2t3fa4=30θ0−30θf+(14θ˙f+16θ˙0)tf+(3θ¨0−2θ¨f)t2f2t4fa5=12θf−12θ0−(6θ˙f+6θ˙0)tf−(θ¨0−θ¨f)t2f2t5f { a 0 = θ 0 a 1 = θ ˙ 0 a 2 = θ ¨ 0 2 a 3 = 20 θ f − 20 θ 0 − ( 8 θ ˙ f + 12 θ ˙ 0 ) t f − ( 3 θ ¨ 0 − θ ¨ f ) t f 2 2 t f 3 a 4 = 30 θ 0 − 30 θ f + ( 14 θ ˙ f + 16 θ ˙ 0 ) t f + ( 3 θ ¨ 0 − 2 θ ¨ f ) t f 2 2 t f 4 a 5 = 12 θ f − 12 θ 0 − ( 6 θ ˙ f + 6 θ ˙ 0 ) t f − ( θ ¨ 0 − θ ¨ f ) t f 2 2 t f 5

\begin{eqnarray*}\left\{ \begin{array}{l} 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}{2t_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}{2t_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}{2t_f^5}\\ \end{array}\right. \end{eqnarray*}
3.matlab代码实现

序号	位移	速度	加速度	时间
1	0	0	0	0
2	50	10	20	3
3	150	20	30	6
4	100	-15	-20	12
5	0	0	0	14

clear;
clc;
q_array=[0,50,150,100,40];%指定起止位置
t_array=[0,3,6,12,14];%指定起止时间
v_array=[0,10,20,-15,0];%指定起止速度
a_array=[0,20,30,-20,0];%指定起止加速度
t=[t_array(1)];q=[q_array(1)];v=[v_array(1)];a=[a_array(1)];%初始状态
for i=1:1:length(q_array)-1;%每一段规划的时间T=t_array(i+1)-t_array(i)a0=q_array(i);a1=v_array(i);a2=a_array(i)/2;a3=(20*q_array(i+1)-20*q_array(i)-(8*v_array(i+1)+12*v_array(i))*T-(3*a_array(i)-a_array(i+1))*T^2)/(2*T^3);a4=(30*q_array(i)-30*q_array(i+1)+(14*v_array(i+1)+16*v_array(i))*T+(3*a_array(i)-2*a_array(i+1))*T^2)/(2*T^4);a5=(12*q_array(i+1)-12*q_array(i)-(6*v_array(i+1)+6*v_array(i))*T-(a_array(i)-a_array(i+1))*T^2)/(2*T^5);%计算五次多项式系数 ti=t_array(i):0.001:t_array(i+1);qi=a0+a1*(ti-t_array(i))+a2*(ti-t_array(i)).^2+a3*(ti-t_array(i)).^3+a4*(ti-t_array(i)).^4+a5*(ti-t_array(i)).^5;vi=a1+2*a2*(ti-t_array(i))+3*a3*(ti-t_array(i)).^2+4*a4*(ti-t_array(i)).^3+5*a5*(ti-t_array(i)).^4;ai=2*a2+6*a3*(ti-t_array(i))+12*a4*(ti-t_array(i)).^2+20*a5*(ti-t_array(i)).^3;t=[t,ti(2:end)];q=[q,qi(2:end)];v=[v,vi(2:end)];a=[a,ai(2:end)];
end
subplot(3,1,1),plot(t,q,'r'),xlabel('t'),ylabel('position');hold on;plot(t_array,q_array,'o','color','g'),grid on;
subplot(3,1,2),plot(t,v,'b'),xlabel('t'),ylabel('velocity');hold on;plot(t_array,v_array,'*','color','y'),grid on;
subplot(3,1,3),plot(t,a,'g'),xlabel('t'),ylabel('accelerate');hold on;plot(t_array,a_array,'^','color','r'),grid on;

PS:相对于三次多项式插值，加速度也是平滑的曲线，并没有出现跳变的情况。然而在机器人系统中，单纯的多项式规划有一个非常严重的问题：没有匀速段，无法根据期望速度提供匀速控制，而在大部分机器人应用中，对加工的速度控制都是有要求的。另一个问题就是，次数越高的多项式，加速过程越慢，整个运动过程中的平均速度越小，影响效率。下次将会为大家介绍能约束速度的三段s曲线加减速直线插值方式。

六轴机器人轨迹规划之五次多项式插值相关推荐

六轴机器人轨迹规划（直线轨迹规划，弧线轨迹规划）——C#实现+ABB为例（规划直接下发离线程序运动）
机器人直线插补算法,弧线插补算法+离线编程转换(空间直线插补规划.空间弧线插补规划.离线编程.ABB二次开发.六轴机器人控制.C#) 一,通过对空间点的插补,形成空间点轨迹 1.插补算法原理简述: ( ...
六轴机器人轨迹规划之五段位置s曲线插补
1.原理五段s曲线相较于三段s曲线而言加速度也是连续变化的,能适用于平稳性要求更高的场合.分为加加速.加减速.匀速.减加速.减减速这五段. 设除匀速段以为,其余四段的时间相等都为TaTaT_a,总时 ...
六轴机器人轨迹规划之空间直线插补
1.原理简述直线插补时,指定起止坐标与速度. 确定要插直线的周期增量,分解到xyz轴. 2.matlab代码 clear; clc; p0=[1,2,3]; pf=[2,4,5]; %指定起止位置 ...
matlab机器人轨迹规划仿真程序,基于MATLAB的六自由度机器人轨迹规划与仿真.pdf...
基于MATLAB的六自由度机器人轨迹规划与仿真学兔兔 l 訇似基于MATLAB的六自由度机器人轨迹规划与仿真 Trajectory planning and simulation of six- ...
03论文阅读与分享————2020年中国知网六自由度机器人轨迹规划相关论文统计
一.2020年知网论文统计 1.刘劲松. 基于时间最优的六轴关节机械手轨迹规划研究[D].湖北工业大学,2020. 创新点:遗传算法+粒子群处理时间最优基本方法:样条插值/笛卡尔空间规划/matla ...
6轴机器人运动学逆解matlab,六轴机器人建模方法、正逆解、轨迹规划实例与Matalb Robotic Toolbox 的实现...
摘要本文主要是给大家一个系统的概念,如何用Matlab实现六轴机器人的建模和实现轨迹规划.以后将会给大家讲解如何手写正逆解以及轨迹插补的程序.程序是基于Matlab2016a,工具箱版本为Robot ...
matlab最优轨迹规划,matlab机器人轨迹规划
运动仿真 $@ %" 运动仿真( ')在对上述规划轨迹进行仿真前, 先输入机器人的参数, 并命名 -$.$/&&)" 命令如下: " @ A 连杆的前四 ...
机器人轨迹规划（熊友伦）
机器人轨迹规划(熊友伦) http://blog.csdn.net/jyc1228/article/details/3991881 http://blog.csdn.net/wx545644217/a ...
css3运动后留下轨迹尾巴_机器人轨迹规划简介
轨迹规划,往往称为机器人轨迹规划,属于低层规划,基本上不涉及工人智能问题,而是在机械手运动学和动力学的基础上,讨论在关节空间和笛卡儿空间中机器人运动的轨迹规划和轨迹生成方法.所谓轨迹,是指机械手在运动 ...

六轴机器人轨迹规划之五次多项式插值

六轴机器人轨迹规划之五次多项式插值相关推荐

最新文章

热门文章