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

转:https://blog.csdn.net/jldemanman/article/details/79303021

1.轨迹规划的定义 

轨迹规划(trajectory planning)是运动规划(motion planning)研究的主要内容。运动规划指的是运动插补,在起始点和终止点之间插入中间点序列,实现沿着轨迹的平稳运动。运动控制包含路径规划(path planning)和轨迹规划,路径规划是规划位置,在起终点之间经过的路径点,轨迹规划是规划时间,将路径点与时间相对应。 
对于我们的六轴机器人而言轨迹规划可以分为:关节空间轨迹规划和笛卡尔空间轨迹规划。关节空间轨迹规划是把机器人的关节变量变换成跟时间的函数,然后对角速度和角加速度进行约束。笛卡尔空间轨迹规划是把机器人末端在笛卡尔空间的位移、速度和加速度变换成跟时间的函数关系。 
2.数学基础 
三次多项式插值(适用于起点和终点速度为零的情况,约束关节在起点和终点的角度值,规定轨迹两端点位置角速度为定值)。 
设关节角满足下式 
θ(t)=a0+a1t+a2t2+a3t3θ˙(t)=a1+2a2t+3a3t2θ¨(t)=2a2+6a3t{θ(t)=a0+a1t+a2t2+a3t3θ˙(t)=a1+2a2t+3a3t2θ¨(t)=2a2+6a3t

将相邻两个点看作一小段轨迹的起点和终点分别用 θ0θ0 θfθf 表示,约束起始速度为 v0v0 ,终止速度为 vfvf  
θ(t0)=θ0θ(tf)=θfθ˙(t0)=v0θ˙(tf)=vf{θ(t0)=θ0θ(tf)=θfθ˙(t0)=v0θ˙(tf)=vf

将约束条件代入函数,可以求得系数(为简便计算,设 t0=0t0=0 ) 
a0=θ0a1=v0a2=3t2f(θfθ0)1tf(2v0+vf)a3=2t3f(θ0θf)+1t2f(v0+vf){a0=θ0a1=v0a2=3tf2(θf−θ0)−1tf(2v0+vf)a3=2tf3(θ0−θf)+1tf2(v0+vf)

三次多项式规划轨迹如下 
θ(t)=θ0+v0t+[3t2f(θfθ0)1tf(2v0+vf)]t2+[2t3f(θ0θf)+1t2f(v0+vf)]t3θ˙(t)=v0+2[3t2f(θfθ0)1tf(2v0+vf)]t+3[2t3f(θ0θf)+1t2f(v0+vf)]t2θ¨(t)=2[3t2f(θfθ0)1tf(2v0+vf)]+6[2t3f(θ0θf)+1t2f(v0+vf)]t{θ(t)=θ0+v0t+[3tf2(θf−θ0)−1tf(2v0+vf)]t2+[2tf3(θ0−θf)+1tf2(v0+vf)]t3θ˙(t)=v0+2[3tf2(θf−θ0)−1tf(2v0+vf)]t+3[2tf3(θ0−θf)+1tf2(v0+vf)]t2θ¨(t)=2[3tf2(θf−θ0)−1tf(2v0+vf)]+6[2tf3(θ0−θf)+1tf2(v0+vf)]t

当速度为零时适用于一段轨迹的起终点,不为零时适用于一段轨迹的经过点。 
3.matlab代码实现

序号位置速度时间
1000
210003
clear;
clc;
q0=0;
q1=100; %指定起止位置
t0=0;
t1=3;%指定起止时间
v0=0;
v1=0;%指定起止速度
a0=q0;
a1=v0;
a2=(3/(t1)^2)*(q1-q0)-(1/t1)*(2*v0+v1);
a3=(2/(t1)^3)*(q0-q1)+(1/t1^2)*(v0+v1);%计算三次多项式系数
t=t0:0.01:t1;
q=a0+a1*t+a2*t.^2+a3*t.^3;%三次多项式插值的位置
v=a1+2*a2*t+3*a3*t.^2;%三次多项式插值的速度
a=2*a2+6*a3*t;%三次多项式插值的加速度
subplot(3,1,1),plot(t,q),xlabel('t'),ylabel('position');grid on;
subplot(3,1,2),plot(t,v),xlabel('t'),ylabel('velocity');grid on;
subplot(3,1,3),plot(t,a),xlabel('t'),ylabel('accelerate');grid on;
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19

下图为插值结果 
这里写图片描述 
4.含经过点的插值

序号位置速度时间
1000
250103
3150206
4100-1512
50014
clear;
clc;
q_array=[0,50,150,100,0];%指定起止位置
t_array=[0,2,4,8,10];%指定起止时间
v_array=[0,10,20,-15,0];%指定起止速度
t=[t_array(1)];q=[q_array(1)];v=[v_array(1)];a=[0];%初始状态
for i=1:1:length(q_array)-1;%每一段规划的时间
     a0=q_array(i);
     a1=v_array(i);
     a2=(3/(t_array(i+1)-t_array(i))^2)*(q_array(i+1)-q_array(i))-(1/(t_array(i+1)-t_array(i)))*(2*v_array(i)+v_array(i+1));
     a3=(2/(t_array(i+1)-t_array(i))^3)*(q_array(i)-q_array(i+1))+(1/(t_array(i+1)-t_array(i))^2)*(v_array(i)+v_array(i+1));%计算三次多项式系数 
     ti=t_array(i)+0.001: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;
     vi=a1+2*a2*(ti-t_array(i))+3*a3*(ti-t_array(i)).^2;
     ai=2*a2+6*a3*(ti-t_array(i));
     t=[t,ti];q=[q,qi];v=[v,vi];a=[a,ai];
end
subplot(3,1,1),plot(t,q,'r'),xlabel('t'),ylabel('position');grid on;
subplot(3,1,2),plot(t,v,'b'),xlabel('t'),ylabel('velocity');grid on;
subplot(3,1,3),plot(t,a,'g'),xlabel('t'),ylabel('accelerate');grid on;
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 7
    点赞
  • 52
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值