**
1.軌跡規劃的定義
軌跡規劃(trajectory planning)是運動規劃(motion planning)研究的主要內容。運動規劃指的是運動插補，在起始點和終止點之間插入中間點序列，實現沿著軌跡的平穩運動。運動控制包含路徑規劃(path planning)和軌跡規劃，路徑規劃是規劃位置，在起終點之間經過的路徑點，軌跡規劃是規劃時間，將路徑點與時間相對應。
對於我們的六軸機械人而言軌跡規劃可以分為：關節空間軌跡規劃和笛卡爾空間軌跡規劃。關節空間軌跡規劃是把機械人的關節變數變換成跟時間的函式，然後對角速度和角加速度進行約束。笛卡爾空間軌跡規劃是把機械人末端在笛卡爾空間的位移、速度和加速度變換成跟時間的函式關係。
2.數學基礎
三次多項式插值（適用於起點和終點速度為零的情況，約束關節在起點和終點的角度值，規定軌跡兩端點位置角速度為定值）。
設關節角滿足下式 ⎩⎨
⎪⎪⎪⎪
θ(t)
=a0+
a1t+
a2t2
+a3t
3θ˙(
t)=a
1+2a
2t+3
a3t2
θ¨(t
)=2a
2+6a
3t{θ(t)=a0+a1t+a2t2+a3t3θ˙(t)=a1+2a2t+3a3t2θ¨(t)=2a2+6a3t0θ0
fθf0v0
fvf⎩⎨
⎪⎪⎪⎪
⎪⎪⎪⎪
⎪⎪θ(
t0)=
θ0θ(
tf)=
θfθ˙
(t0)
=v0θ
˙(tf
)=vf
{θ(t0)=θ0θ(tf)=θfθ˙(t0)=v0θ˙(tf)=vf0=
0t0=0⎩⎨
⎪⎪⎪⎪
⎪⎪⎪⎪
⎪⎪⎪⎪
a0=θ
0a1=
v0a2
=3t2
f(θf
−θ0)
−1tf
(2v0
+vf)
a3=2
t3f(
θ0−θ
f)+1
t2f(
v0+v
f){a0=θ0a1=v0a2=3tf2(θf−θ0)−1tf(2v0+vf)a3=2tf3(θ0−θf)+1tf2(v0+vf)⎩⎨
⎪⎪⎪⎪
⎪⎪⎪⎪
⎪⎪θ(
t)=θ
0+v0
t+[3
t2f(
θf−θ
0)−1
tf(2
v0+v
f)]t
2+[2
t3f(
θ0−θ
f)+1
t2f(
v0+v
f)]t
3θ˙(
t)=v
0+2[
3t2f
(θf−
θ0)−
1tf(
2v0+
vf)]
t+3[
2t3f
(θ0−
θf)+
1t2f
(v0+
vf)]
t2θ¨
(t)=
2[3t
2f(θ
f−θ0
)−1t
f(2v
0+vf
)]+6
[2t3
f(θ0
−θf)
+1t2
f(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
位置速度時間1
0002
1000
3
clear;
clc;
q0=0;
q1=100; %指定起止位置
t0=0;
t1=3;%指定起止時間
v0=0;
v1=0;%指定起止速度
a0=q
0;a1=v0;
a2=(3/(t1)^2)*(q1-q
0)-(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
0002
501033
1502064
100-15125
0014
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];%初始狀態
fori=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];
endsubplot(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...
六軸機械人軌跡規劃之空間直線插補
1.原理簡述 直線插補時，指定起止座標與速度。確定要插直線的週期增量，分解到xyz軸。2.matlab clear clc p0 1,2,3 pf 2,4,5 指定起止位置 v 0.1 指定速度 x p0 1 y p0 2 z p0 3 l pf 1 p0 1 2 pf 2 p0 2 2 pf 3 ...
規劃帶偏置六軸機械人直線運動軌跡
clc clear all 描述六軸機械人d h引數xz類轉換關係 l 1 link revolute d 587.5,a 0,alpha 0,modified l 2 link revolute d 157.5,a 150,alpha pi 2,modified l 3 link revolute...