机械臂轨迹规划篇（一）MATLAB测试三次多项式样条插值

MATLAB三次多项式样条插值

前言
机械臂运动空间
三次多项式插值原理
MATLAB仿真测试

前言

最近这一段时间有点忙，所以博客这方面也更的少了，不过我有学到新的知识，还是会尽量更的，由于之前一直也在Matlab里仿真过机械臂，所以这次就准备更下Matlab里使用D-H模型搭建机械臂，用三次、五次多项式样条插值规划机械臂轨迹算法。

机械臂运动空间

机械臂运动轨迹规划在空间可以分为两大类：关节空间轨迹规划与笛卡尔空间轨迹规划。机械臂关节空间的轨迹规划解决机械臂从起始位姿到终止位姿去取放物体的问题。机械臂末端移动的过程并不重要，只要求运动是平滑的且没有碰撞产生。而笛卡尔空间轨迹规划要解决的是机械臂末端确定的轨迹规划问题；在关节空间中进行轨迹规划时，算法简单、工具移动效率高、关节空间与直角坐标空间连续的对应关系是不存在的，因此机构的奇异性问题一般不会发生。对于无路径的要求，应尽量在关节空间进行轨迹规划。

三次多项式插值原理

三次多项式插值适用于起点和终点速度为零的情况。约束关节在起点和终点为零的角度值，规定轨迹端点位置角速度为定值。
设关节角满足下式：
{ θ ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 θ ( t ) ˙ = a 1 + 2 a 2 t + 3 a 3 t 2 θ ( t ) ¨ = 2 a 2 + 6 a 3 t \begin{cases} \theta(t)=a_0+a_1t+a_2t^2+a_3t^3\\\dot{\theta(t)}=a_1+2a_2t+3a_3t^2\\\ddot{\theta(t)}=2a_2+6a_3t\\ \end{cases} ⎩⎪⎨⎪⎧θ(t)=a0+a1t+a2t2+a3t3θ(t)˙=a1+2a2t+3a3t2θ(t)¨=2a2+6a3t
将相邻两个点看作是一小段轨迹的起点与终点，分别用 θ 0 \theta_0 θ0和 θ f \theta_f θf表示，设起始速度 v 0 v_0 v0,终止速度v_f。
{ θ ( t 0 ) = θ 0 θ ( t f ) = θ f θ ( t 0 ) ˙ = v 0 θ ( t f ) ˙ = v f \begin{cases} \theta(t_0)=\theta_0\\\theta(t_f)=\theta_f\\\dot{\theta(t_0)}=v_0\\\dot{\theta(t_f)}=v_f \end{cases} ⎩⎪⎪⎪⎨⎪⎪⎪⎧θ(t0)=θ0θ(tf)=θfθ(t0)˙=v0θ(tf)˙=vf
将约束条件代入函数，可以求得系数（设t_0=0）
{ a 0 = θ 0 a 1 = v 0 a 2 = 3 t f 2 ( θ f − θ 0 ) − 1 t f ( 2 v 0 + v f ) a 3 = 2 t f 3 ( θ 0 − θ f ) + 1 t f 2 ( v 0 + v f ) \begin{cases} a_0=\theta_0\\a_1=v_0\\a_2=\frac{3}{t_f^2}(\theta_f-\theta_0)-\frac{1}{t_f}(2v_0+v_f)\\a_3=\frac{2}{t_f^3}(\theta_0-\theta_f)+\frac{1}{t_f^2}(v_0+v_f) \end{cases} ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧a0=θ0a1=v0a2=tf23(θf−θ0)−tf1(2v0+vf)a3=tf32(θ0−θf)+tf21(v0+vf)
三次多项式轨迹规划函数如下：
{ θ ( t ) = θ 0 + v 0 t + a 2 t 2 + [ 3 t f 2 ( θ f − θ 0 ) − 1 t f ( 2 v 0 + v f ) ] t 2 + [ 2 t f 3 ( θ 0 − θ f ) + 1 t f 2 ( v 0 + v f ) ] t 3 θ ( t ) ˙ = v 0 + 2 [ 3 t f 2 ( θ f − θ 0 ) − 1 t f ( 2 v 0 + v f ) ] t + 3 [ 2 t f 3 ( θ 0 − θ f ) + 1 t f 2 ( v 0 + v f ) ] t 2 θ ( t ) ¨ = 2 [ 3 t f 2 ( θ f − θ 0 ) − 1 t f ( 2 v 0 + v f ) ] + 6 [ 2 t f 3 ( θ 0 − θ f ) + 1 t f 2 ( v 0 + v f ) ] t \begin{cases} \theta(t)=\theta_0+v_0t+a_2t^2+[\frac{3}{t_f^2}(\theta_f-\theta_0)-\frac{1}{t_f}(2v_0+v_f)]t^2+[\frac{2}{t_f^3}(\theta_0-\theta_f)+\frac{1}{t_f^2}(v_0+v_f)]t^3\\\dot{\theta(t)}=v_0+2[\frac{3}{t_f^2}(\theta_f-\theta_0)-\frac{1}{t_f}(2v_0+v_f)]t+3[\frac{2}{t_f^3}(\theta_0-\theta_f)+\frac{1}{t_f^2}(v_0+v_f)]t^2\\\ddot{\theta(t)}=2[\frac{3}{t_f^2}(\theta_f-\theta_0)-\frac{1}{t_f}(2v_0+v_f)]+6[\frac{2}{t_f^3}(\theta_0-\theta_f)+\frac{1}{t_f^2}(v_0+v_f)]t \end{cases} ⎩⎪⎪⎨⎪⎪⎧θ(t)=θ0+v0t+a2t2+[tf23(θf−θ0)−tf1(2v0+vf)]t2+[tf32(θ0−θf)+tf21(v0+vf)]t3θ(t)˙=v0+2[tf23(θf−θ0)−tf1(2v0+vf)]t+3[tf32(θ0−θf)+tf21(v0+vf)]t2θ(t)¨=2[tf23(θf−θ0)−tf1(2v0+vf)]+6[tf32(θ0−θf)+tf21(v0+vf)]t

MATLAB仿真测试

基于上面的信息，我们就可以用Matlab来写三项式插值的脚步代码，设机械臂某关节在5s内由初始点A经过中间点B到达目标点C的位置、速度、加速度变化情况，设
θ A = 25 , θ B = 70 , θ C = 50 ； θ A ˙ = 30 , θ B ˙ = 20 , θ C ˙ = 30 ; θ A ¨ = 2 , θ B ¨ = 4 , θ C ¨ = 3 \theta_A=25 ,\theta_B=70,\theta_C=50；\\\dot{\theta_A}=30,\dot{\theta_B}=20,\dot{\theta_C}=30;\\ \ddot{\theta_A}=2,\ddot{\theta_B}=4,\ddot{\theta_C}=3 θA=25,θB=70,θC=50；θA˙=30,θB˙=20,θC˙=30;θA¨=2,θB¨=4,θC¨=3

clear;clc;close all;
q_array=[25,70,50];%指定起止位置
t_array=[0,2,5];%指定起止时间
v_array=[30,20,30];%指定起止速度
a_array=[2,4,3];%指定起止加速度
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=(q_array(i+1)-q_array(i))*3/(T^2)-(2*v_array(i)+v_array(i+1))/T;
a3=(q_array(i)-q_array(i+1))*2/(T^3)+(v_array(i)+v_array(i+1))/(T^2);
ti=t_array(i):0.02: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(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,'*','color','r'),grid on;
subplot(3,1,2),plot(t,v,'b'),xlabel('t'),ylabel('velocity');hold on;plot(t_array,v_array,'o','color','g'),grid on;
subplot(3,1,3),plot(t,a,'g'),xlabel('t'),ylabel('accelerate');hold on;plot(t_array,a_array,'^','color','b'),grid on;

下图为Matlab测试结果

从曲线可以看出，红色关节角度曲线平滑，而蓝色速度曲线在中间点2s处出现突
变，绿色加速度曲线突变的更加明显。