文章目錄
三次多項式
具有中間點路徑的三次多項式軌跡
五次多項式
例子1：簡單三次曲線和五次曲線
（a）是三次多項式曲線，已知用戶指定初始點和終止點的位置和速度及整個運動時間，則根據公式（7-1）~（7-5）進行計算，代碼及運算結果如下
% 單關節多項式關節空間軌跡
%% 三階多項式theta(t) = a0 + a1*t + a2*t^2 + a3*t^3
% 指定初始和終止點的位置theta_s theta_f，同時速度均爲0
theta_s = 120; theta_f = 60; tf = 1;
a0_3 = theta_s;
a1_3 = 0;
a2_3 = (3/tf^2)*(theta_f - theta_s);
a3_3 = (-2/tf^3)*(theta_f - theta_s);
j = 1;
for t = 0: 0.02: 1
theta_3(j) = a0_3 + a1_3*t + a2_3*t^2 + a3_3*t^3;
theta_3d(j) = a1_3 + 2*a2_3*t + 3*a3_3*t^2;
theta_3dd(j) = 2*a2_3 + 6*a3_3*t;
theta_3ddd(j) = 6*a3_3;
j = j + 1;
end
figure(1)
subplot(4, 1, 1)
plot([0:0.02:1], theta_3)
grid on
title('關節角(°)')
subplot(4, 1, 2)
plot([0:0.02:1], theta_3d)
grid on
title('角速度(°/s)')
subplot(4, 1, 3)
plot([0:0.02:1], theta_3dd)
grid on
title('角加速度(°/s^2)')
subplot(4, 1, 4)
plot([0:0.02:1], theta_3ddd)
grid on
title('角加速度變化率')
hold on
由上圖可以看出，三次曲線在初始時刻和終止時刻時，加速度不連續，會存在衝擊，這是三次曲線的缺點。五次曲線可以解決這個問題。
（b）是五次曲線，此時用戶給定初始時刻和終止時刻的位置、速度和加速度，這是該曲線的約束條件，同時給定運動時間。代碼及運算結果如下：
%% 五階多項式theta(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5
% 指定初始和終止點的位置，另外速度和加速度均爲0
theta_s = 120; theta_f = 60; tf = 1;
theta_fd = 0; theta_fdd = 0; theta_sd = 0; theta_sdd = 0;
a0_5 = theta_s; a1_5 = theta_sd; a2_5 = theta_sdd / 2;
a3_5 = (20*theta_f - 20*theta_s - (8*theta_fd + 12*theta_sd)*tf - (3*theta_sdd - theta_fdd)*tf^2) / (2*tf^3);
a4_5 = (30*theta_s - 30*theta_f + (14*theta_fd + 16*theta_sd)*tf + (3*theta_sdd - 2*theta_fdd)*tf^2) / (2*tf^4);
a5_5 = (12*theta_f - 12*theta_s - (6*theta_fd + 6*theta_sd)*tf - (theta_sdd - theta_fdd)*tf^2) / (2*tf^5);
k = 1;
for t = 0: 0.02: 1
theta_5(k) = a0_5 + a1_5*t + a2_5*t^2 + a3_5*t^3 + a4_5*t^4 + a5_5*t^5;
theta_5d(k) = a1_5 + 2*a2_5*t + 3*a3_5*t^2 + 4*a4_5*t^3 + 5*a5_5*t^4;
theta_5dd(k) = 2*a2_5 + 6*a3_5*t + 12*a4_5*t^2 + 20*a5_5*t^3;
theta_5ddd(k) = 6*a3_5 + 24*a4_5*t + 60*a5_5*t^2;
k = k + 1;
end
figure(2)
subplot(4, 1, 1)
plot([0:0.02:1], theta_5)
grid on
title('關節角(°)')
subplot(4, 1, 2)
plot([0:0.02:1], theta_5d)
grid on
title('角速度(°/s)')
subplot(4, 1, 3)
plot([0:0.02:1], theta_5dd)
grid on
title('角加速度(°/s^2)')
subplot(4, 1, 4)
plot([0:0.02:1], theta_5ddd)
grid on
title('角加速度變化率')
由上圖可知，五次曲線解決了三次曲線在初始時刻和終止時刻加速度不連續的問題。
（c）是兩段帶有中間點路徑的三次曲線。此時用戶給定初始時刻和終止時刻的位置、速度和加速度，給定了中間點的位置，但是沒有指定中間時刻的速度，因此在兩條三次曲線的連接處，用速度和加速度均連續作爲新的約束條件，並根據公式（7-12）~（7-15）進行計算。代碼及計算結果如下：
%% 兩段帶有中間點的三項多項式(約束條件爲連接點的速度和加速度相等)
% theta(t)_1 = a10 + a11*t1 + a12*t1^2 + a13*t1^3
% theta(t)_2 = a20 + a21*t2 + a22*t2^2 + a23*t2^3
theta_s_ = 60; theta_v_ = 120; theta_f_ = 30;
t = 1; tf = 2;
theta_s_d_ = 0; theta_s_dd_ = 0;
theta_f_d_ = 0; theta_f_dd_ = 0;
a10 = theta_s_; a11 = 0;
a12 = (12*theta_v_ - 3*theta_f_ - 9*theta_s_) / (4*t^2);
a13 = (-8*theta_v_ + 3*theta_f_ + 5*theta_s_) / (4*t^3);
a20 = theta_v_;
a21 = (3*theta_f_ - 3*theta_s_) / (4*t);
a22 = (-12*theta_v_ + 6*theta_f_ + 6*theta_s_) / (4*t^2);
a23 = (8*theta_v_ - 5*theta_f_ - 3*theta_s_) / (4*t^3);
s = 1;
for T = 0: 0.02: 1
theta_1(s) = a10 + a11*T + a12*T^2 + a13*T^3;
theta_d_1(s) = a11 + 2*a12*T + 3*a13*T^2;
theta_dd_1(s) = 2*a12 + 6*a13*T;
theta_ddd_1(s) = 6*a13;
s = s + 1;
end
s = 1;
for T = 0: 0.02: 1
theta_2(s) = a20 + a21*T + a22*T^2 + a23*T^3;
theta_d_2(s) = a21 + 2*a22*T + 3*a23*T^2;
theta_dd_2(s) = 2*a22 + 6*a23*T;
theta_ddd_2(s) = 6*a23;
s = s + 1;
end
% 去掉首尾
theta_ = [theta_1, theta_2(2: 51)];
theta_d_ = [theta_d_1, theta_d_2(2: 51)];
theta_dd_ = [theta_dd_1, theta_dd_2(2: 51)];
theta_ddd_ = [theta_ddd_1, theta_ddd_2(2: 51)];
figure(3)
subplot(4, 1, 1)
plot([0:0.02:2], theta_)
grid on
title('關節角(°)')
subplot(4, 1, 2)
plot([0:0.02:2], theta_d_)
grid on
title('角速度(°/s)')
subplot(4, 1, 3)
plot([0:0.02:2], theta_dd_)
grid on
title('角加速度(°/s^2)')
subplot(4, 1, 4)
plot([0:0.02:2], theta_ddd_)
grid on
title('角加速度變化率')
由上圖可知，三次曲線本身的問題仍然存在。
例子2：多段帶有中間點的三次多項式
用戶指定初始點、終止點、多箇中間點的時刻以及各點對應的位置、速度
指定各點的時間及其對應的位置、速度：
t0 = 0, t1 = 2, t2 = 4, t3 = 8, t4 = 10
p0 = 10, p1 = 20, p2 = 0, p3 = 30, p4 = 40
v0 = 0, v1 = -10, v2 = 10, v3 = 3, v4 = 0
此時用戶指定了中間點的速度，因此可以根據公式（7-9）~（7-11）確定各項係數，並得到軌跡。代碼及運算結果如下：
%% 多段帶有中間點的三次多項式
% 指定各點的時間及其對應的位置、速度
% t0 = 0, t1 = 2, t2 = 4, t3 = 8, t4 = 10
% p0 = 10, p1 = 20, p2 = 0, p3 = 30, p4 = 40
% v0 = 0, v1 = -10, v2 = 10, v3 = 3, v4 = 0
t = [0, 2, 4, 8, 10];
p = [10, 20, 0, 30, 40];
v = [0, -10, 10, 3, 0];
% 初始化
T = t(1); P = p(1); V = v(1);
for i = 1:4
% 四個三階多項式的各項參數
a0(i) = p(i);
a1(i) = v(i);
Tf = t(i+1) - t(i);
a2(i) = (3/Tf^2)*(p(i+1) - p(i)) - (2/Tf)*v(i) - (1/Tf)*v(i+1);
a3(i) = (-2/Tf^3)*(p(i+1) - p(i)) + (1/Tf^2)*(v(i+1) + v(i));
% 時間均分100份,並累加時間橫座標
N = linspace(t(i), t(i+1), 100);
T = [T, N(2: 100)];% 累加時間座標
% 計算各多項式
pk = a0(i) + a1(i)*(N - N(1)) + a2(i)*power(N - N(1), 2) + a3(i)*power(N - N(1), 3);
vk = a1(i) + 2*a2(i)*(N - N(1)) + 3*a3(i)*power(N - N(1), 2);
qk = 2*a2(i) + 6*a3(i)*(N - N(1));
P = [P, pk(2: 100)];
V = [V, vk(2: 100)];
if (i == 1)
Q = 2*a2(i);
end
Q = [Q, qk(2: 100)];
end
figure(4)
subplot(3, 1, 1);
plot(T, P, 'r')
grid on
hold on
plot(t, p, 'or')
title('關節角(°)')
subplot(3, 1, 2)
plot(T, V, 'b')
grid on
hold on
plot(t, v, 'ob')
title('角速度(°/s)')
subplot(3, 1, 3)
plot(T, Q, 'g')
grid on
title('角加速度(°/s^2)')
由上圖可知，三次曲線中各時間點的加速度不連續，存在衝擊，同時速度波動太大，這可能是中間點速度選取不合理造成、下面利用五次多項式，提出一種簡單的解決辦法。
例子3：多段帶中間點的五次多項式，同時中間速度平滑處理（和三次多項式進行比較）
用戶指定初始點、終止點、多箇中間點的時刻以及各點對應的位置、速度
指定各點的時間及其對應的位置、速度。使用五次多項式，還需要指定各點的加速度：
t0 = 0, t1 = 2, t2 = 4, t3 = 8, t4 = 10
p0 = 10, p1 = 20, p2 = 0, p3 = 30, p4 = 40
v0 = 0, v1 = -10, v2 = 10, v3 = 3, v4 = 0
a0 = 0, a1 = 0, a2 = 0, a3 = 0, a4 = 0
針對例子2中速度選取不得當導致波動較大的問題，本例採用如下辦法。一般情況下不會指定中間點的速度，只指定起點和終點的速度，這時候就可以使用下面方法規劃軌跡。有時候定義軌跡時，指定的中間點的速度不合理，會導致速度曲線波動過大，這是時候如果不要求中間位置的速度都必須與指定相等，也可以使用下面的規劃方式。
代碼及運算結果如下：
%% 多段帶中間點的五次多項式，同時中間速度平滑處理（和三次多項式進行比較）
% 該方法由於對中間點的速度進行平滑處理，因此中間點速度不一定是期望速度，適用於對中間點速度無要求的情況
% 指定各點的時間及其對應的位置、速度
% t0 = 0, t1 = 2, t2 = 4, t3 = 8, t4 = 10
% p0 = 10, p1 = 20, p2 = 0, p3 = 30, p4 = 40
% v0 = 0, v1 = -10, v2 = 10, v3 = 3, v4 = 0
% 初始、中間點及終止點的加速度均爲0
t = [0, 2, 4, 8, 10];
p = [10, 20, 0, 30, 40];
v = [0, -10, 10, 3, 0];
a = [0, 0, 0, 0, 0];
% 初始化
T = t(1); P_ = p(1); V_ = v(1); A_ = a(1);
% 處理中間速度
for k = 1: 4
if (k == 1)
v2(k) = v(k);
else
dk1 = (p(k) - p(k-1)) / (t(k) - t(k-1));
dk2 = (p(k+1) - p(k)) / (t(k+1) - t(k));
if (dk1 >= 0 && dk2 >= 0 || dk1 <= 0 && dk2 <= 0)
v2(k) = (1/2)*(dk1 + dk2);
else
v2(k) = 0;
end
end
end
v2(5) = v(5);
for i = 1: 4
% 計算四個五次多項式的各項係數
tf = t(i+1) - t(i);
a0(i) = p(i);
a1(i) = v2(i);
a2(i) = a(i) / 2;
a3(i) = (20*p(i+1) - 20*p(i) - (8*v2(i+1) + 12*v2(i))*tf - (3*a(i) - a(i+1))*tf^2) / (2*tf^3);
a4(i) = (30*p(i) - 30*p(i+1) + (14*v2(i+1) + 16*v2(i))*tf + (3*a(i) - 2*a(i+1))*tf^2) / (2*tf^4);
a5(i) = (12*p(i+1) - 12*p(i) - (6*v2(i+1) + 6*v2(i))*tf - (a(i) - a(i+1))*tf^2) / (2*tf^5);
% 時間均分100份,並累加時間橫座標
N = linspace(t(i), t(i+1), 100);
T = [T, N(2: 100)]; % 累加時間座標
% 計算各多項式theta(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5
% position
pk = a0(i) + a1(i)*(N - N(1)) + a2(i)*power(N - N(1), 2) + a3(i)*power(N - N(1), 3) + a4(i)*power(N - N(1), 4) + a5(i)*power(N - N(1), 5);
% velocity
vk = a1(i) + 2*a2(i)*(N - N(1)) + 3*a3(i)*power(N - N(1), 2) + 4*a4(i)*power(N - N(1), 3) + 5*a5(i)*power(N - N(1), 4);
% acceleration
ak = 2*a2(i) + 6*a3(i)*(N - N(1)) + 12*a4(i)*power(N - N(1), 2) + 20*a5(i)*power(N - N(1), 3);
P_ = [P_, pk(2: 100)];
V_ = [V_, vk(2: 100)];
A_ = [A_, ak(2: 100)];
end
figure(5)
subplot(3, 1, 1);
plot(T, P_, 'r')
grid on
hold on
plot(T, P, 'r--')
plot(t, p, 'or')
title('關節角(°)')
subplot(3, 1, 2)
plot(T, V_, 'b')
grid on
hold on
plot(T, V, 'b--')
plot(t, v2, 'ob')
title('角速度(°/s)')
subplot(3, 1, 3)
plot(T, A_, 'g')
hold on
plot(T, Q, 'g--')
grid on
title('角加速度(°/s^2)')
與帶有多箇中間點的三次多項式軌跡進行比較，圖中實線爲本例方法，虛線爲三次多項式方法。
改進之後，可以發現速度變化更爲平滑，各時間點的加速度衝擊得到改善。
附加
關於拐點對稱的拋物線軌跡
%% 關於拐點對稱的拋物線軌跡
% 已知起始和結束時刻的位置和速度
t0 = 0; t = 8; T = t - t0;
tf = T / 2; % 拐點
v0 = 0; v1 = 0;
theta_s = 0; theta = 10; theta_f = (theta_s + theta) / 2;
h = theta - theta_s;
% theta_a(t) = a10 + a11*(t - t0) + a12*(t - t0)^2 加速段
a10 = theta_s; a11 = v0; a12 = (2/T^2)*(h - v0*T);
% matlab中這樣計算是很方便的，但是c裏面無法直接合並向量，得藉助循環遍歷
Ta = linspace(t0, tf, 400);
theta_a = a10 + a11*(Ta - t0) + a12*power(Ta - t0, 2);
theta_ad = a11 + 2*a12*(Ta - t0);
theta_add = 2*a12;
% theta_b(t) = a20 + a21*(t - tf) + a22*(t - tf)^2 減速段
Tb = linspace(tf, t, 400);
Tn = [Ta, Tb(2: 400)];
a20 = theta_f; a21 = 2*(h/T) - v1; a22 = (2/T^2)*(v1*T - h);
theta_b = a20 + a21*(Tb - tf) + a22*power(Tb - tf, 2);
theta_bd = a21 + 2*a22*(Tb - tf);
theta_bdd = 2*a22;
theta = [theta_a, theta_b(2: 400)];
theta_d = [theta_ad, theta_bd(2: 400)];
figure(1)
subplot(3, 1, 1)
plot(Tn, theta, 'r')
ylabel('position')
hold on
plot(tf, theta_f, 'or')
grid on
subplot(3, 1, 2)
plot(Tn, theta_d, 'b')
ylabel('velocity')
hold on
plot(tf, theta_d(400), 'ob')
grid on
for i = 1: 799
theta_dd(i) = theta_add;
if (i > 400)
theta_dd(i) = theta_bdd;
end
end
subplot(3, 1, 3)
plot(Tn, theta_dd, 'g')
ylabel('acceleration')
hold on
plot(tf, theta_dd(400), 'og')
grid on
更一般的情況，拐點兩邊不對稱，但是加速度對稱恆定
% 已知起始和結束時刻的位置和速度，同時拐點的位置和速度具有連續性
t0 = 0; t1 = 8; T = t - t0; tf = 4;
ta = tf - t0; tb = t - tf;
v0 = 0.1; v1 = -1;
q0 = 0; q1 = 10; h = q1 - q0;
% theta_a(t) = a10 + a11*(t - t0) + a12*(t - t0)^2 加速段
a10 = q0; a11 = v0; a12 = (2*h - v0*(T + ta) - v1*tb) / (2*T*tb);
Ta = linspace(t0, tf, (ta/T)*800);
q_a = a10 + a11*(Ta - t0) + a12*power(Ta - t0, 2);
q_ad = a11 + 2*a12*(Ta- t0);
q_add = 2*a12;
% theta_b(t) = a20 + a21*(t - tf) + a22*(t - tf)^2 減速段
a20 = (2*q1*ta + tb*(2*q0 + ta*(v0 - v1))) / (2*T);
a21 = (2*h - v0*ta - v1*tb) / T;
a22 = -(2*h - v0*ta - v1*(T + tb)) / (2*T*tb);
Tb = linspace(tf, t1, (tb/T)*800);
q_b = a20 + a21*(Tb - tf) + a22*power(Tb - tf, 2);
q_bd = a21 + 2*a22*(Tb - tf);
q_bdd = 2*a22;
Tn = [Ta, Tb(2: (tb/T)*800)];
q = [q_a, q_b(2: (tb/T)*800)];
q_d = [q_ad, q_bd(2: (tb/T)*800)];
for i = 1:799
q_dd(i) = q_add;
if (i > (ta/T)*800)
q_dd(i) = q_bdd;
end
end
figure(2)
subplot(3, 1, 1)
plot(Tn, q, 'r');
hold on
plot(tf, q_a((ta/T)*800), 'or')
grid on
ylabel('position')
subplot(3, 1, 2)
plot(Tn, q_d, 'b')
hold on
plot(tf, q_ad((ta/T)*800), 'ob')
grid on
subplot(3, 1, 3)
plot(Tn, q_dd, 'g')
grid on
