寫在前面
Matlab機器人工具箱版本9.10
機械臂用的是五自由度的，我測試時發現逆解精度存在一些問題，目前還沒找到是求解析解時出錯還是編程過程有問題，還是算法本身考慮不周到，歡迎有研究過的大神們批評指正！感謝~
常規插補算法
假設機器人末端由P1點沿直線運動到P2點，和分別爲起點P1的位置和RPY角，和分別爲終點P2的位置和RPY角，和分別爲中間插補點Pi的位置和RPY角。
各個插值點位置座標向量和RPY角度向量可以表示爲：
其中，和表示插值點的位置座標向量和RPY角度向量，爲歸一化因子，採用拋物線過渡的線性函數（即梯形加減速）以保證整段軌跡上的位移和速度都連續。和分別表示首末位置間的位置和RPY角度的增量，其求解爲：
所以問題被轉變爲對歸一化因子的求解，求解方法後面會講。下面先了解記錄兩種常見的插補算法：直線插補和空間圓弧插補。
直線插補算法
一般最簡單的做法爲，已知空間直線的起點S位置座標爲，終點D位置座標爲和插補次數N，則有
對於該直線上任一點i(1<= i <= N)有
這種方法很簡單，對於機械臂的運動而言只需要加上RPY角的變化即可，此處不詳述。
基於拋物線過渡（梯形加減速）的空間直線插補的核心在於歸一化參數的計算，同時加入速度規劃的物理條件和約束。因此得到的直線插補算法需要初始化的參數有機械臂末端線速度、加速度、減速度，需要計算得到的插值參數有總位移S和插值點數N。
對於空間直線插補而言，總位移計算公式如下：
插值點數的計算公式如下：
其中，爲插值參數，可以根據情況進行調整。從公式（7）可以看出，插值點數N和總位移成正比，與機械臂末端的線速度成反比。也就是說，如果總位移越大，線速度越小，則插值點數應該越多。原因在於，如果位移越大，在速度不變的情況下，一定要保證更多的插值點才能對規劃曲線進行有效的擬合。同樣的，如果位移不變，爲了得到大的運動速度，則要降低插值點數，因爲插值過程是一個等時間間隔的過程，插值點數越多，意味着所需時間越長。
空間圓弧插補算法
1.圓心和半徑
如圖所示，{O-XYZ}爲機器人基座座標系。P1、P2、P3爲空間中任意不共線的三個點，其中P1、P2、P3爲三點在基座標系中的座標。那麼這三個點可以唯一確定一個外接圓，該園所在平面的方程如下：
該外接圓方程的一般形式如下：
其中，
P1P2的垂直平分面M2的方程如下：
其中，
P2P3的垂直平分面M3的方程如下：
其中，
綜上，聯立M1、M2、M3三個平面的方程，可得到如下算式：
公式（12）有且僅有唯一一組解，可解得三點外接圓圓心P0的位置座標爲，根據圓心座標可求得外接圓半徑爲
2.座標變換
在以圓心P0爲原點的基礎上建立新的座標系{P0-X0Y0Z0}。新座標系中的Z0爲平面M1的法矢量，從上文可得Z0軸的方向餘弦爲：
其中，
此處，規定新座標系{P0-X0Y0Z0}的X0爲P0P1的矢量方向，故可得X0軸的方向餘弦如下：
其中，i= x， y， z。
確定Z0和X0的方向後，根據右手定則即可確定Y0的方向，如下：
在得到新座標系三個軸的方向餘弦後，根據旋轉矩陣的定義，可得到新座標系{P0-X0Y0Z0}相對於基座標系{O-XYZ}的齊次變換矩陣
因此，可通過齊次變換矩陣T將基座標系內的空間圓弧變換到新座標系內的平面圓弧，簡化求解過程。
3.轉換爲平面圓弧問題
根據齊次變換矩陣，P1、P2、P3在新座標系下的座標Q1、Q2、Q3可通過下面公式計算得到：
在平面內進行圓弧插補，實際上就是對圓心角進行插補，因此核心就是求出圓弧的圓心角，同時還需要注意順逆時針的問題。
4.圓心角和插值點數N
由於atan2函數返回的是原點至點(x,y)的方位角，即與 x 軸的夾角。也可以理解爲複數 x+yi 的輻角。返回值的單位爲弧度。在數學座標系中，結果爲正表示從 X 軸逆時針旋轉的角度，結果爲負表示從 X 軸順時針旋轉的角度。此時爲了判斷圓弧是順時針還是逆時針，需要將角度值轉換成0~2的範圍。
假設爲與正向的夾角，有
其中是P2在新座標系下的座標值。此時存在如下兩種情況：
- 當時，；
- 否則，。
同理，假設爲與正向的夾角，有
其中是P2在新座標系下的座標值。此時存在如下兩種情況：
- 當時，；
- 否則，。
通過比較和的大小可以確定圓弧的方向，同時可以求出圓弧圓心角：
- 當時，圓弧爲逆時針，圓心角;
- 當時，;
對應基於拋物線過渡（梯形加減速）的空間圓弧插補而言，需要初始化的運動參數有機械臂末端角速度和角加速度a，需要計算的插值參數爲圓心角和插值點數N。上文已經求得圓心角，插值點數計算公式如下：
歸一化因子的求解
預備知識
機器人學回爐重造（6）：關節空間規劃方法——梯形加減速（與拋物線擬合的線性函數）、S型曲線規劃
設機械臂在勻速運動時的線速度爲，拋物線段的加減速度均爲，可以計算出加速階段和減速階段的時間和位移爲
設機械臂末端運動的總位移爲，總時間爲：
將位移、速度和加速度進行歸一化處理得到上述計算量的歸一化參數：
結合梯形加減速的位移公式，可以推導出歸一化因子的計算公式如下：
式中，t表示時間，，其中N表示總的插值點數，i = 1、2、3、……、N-1、N。每個插值點的時間值，都有一個與之對應，結合公式（1）（2）（3）可以得到各個插值點位置座標向量和RPY角度向量，最後就是解逆運動學方程，得到每個插值點對應的關節變量，生成軌跡。
Matlab實現代碼
預備知識：
模型是我買的五自由度機械臂，之前已經推導過逆運動學的解析解，這裏直接拿來用。
先是預備程序：
- 高斯列主元消去法，解圓心方程；
- 五自由度逆運動學解析解推導（前面博客有）；
- 歸一化因子推導。
%% 高斯列主元消去法
function x = Gauss_lie(n, A, b)
for k = 1: n-1
a_max = abs(A(k, k));
cnt = k;
for i = k: n
if (abs(A(i, k)) > a_max)
a_max = abs(A(i, k));
cnt = i; % 確定下標i
end
end
if (A(cnt, k) == 0)
fprintf('Gauss_lie: no unique solution\n');
return;
end
% 換行
if (cnt ~= k)
t = 0; s = 0;
for j = k: n
t = A(k, j);
A(k, j) = A(cnt, j);
A(cnt, j) = t;
s = b(cnt);
b(cnt) = b(k);
b(k) = s;
end
end
% step 5
for i = k+1: n
L(i) = A(i, k) / A(k, k);
for j = k+1: n
A(i, j) = A(i, j) - L(i)*A(k, j);
end
b(i) = b(i) - L(i)*b(k);
end
end
for i = 1: n
if (A(i, i) == 0)
fprintf('Gauss_lie no unique solution\n');
return;
end
end
% 回代
x(n) = b(n) / A(n, n);
for i = n-1: -1: 1
sum_a = 0;
for j = i+1: n
sum_a = sum_a + A(i, j)*x(j);
end
x(i) = (b(i) - sum_a) / A(i, i);
end
end
% 歸一化處理
% 梯形加減速曲線
% 輸入參數：機械臂末端運動總位移（角度）pos
% 機械臂末端線速度（角速度）vel
% 加速度減速度accl（設定加減速段accl相同）
% 插值點數N
function lambda = Normalization(pos, vel, accl, N)
% 加減速段的時間和位移
T1 = vel / accl;
S1 = (1/2) * accl * T1^2;
% 總時間
Te = 2*T1 + (pos - 2*S1) / vel;
% 歸一化處理
S1_ = S1 / pos;
T1_ = T1 / Te;
T2_ = 1 - T1_;
accl_ = 2*S1_ / T1_^2;
% lambda求解公式
for i = 0: N
t = i / N;
if (t >= 0 && t <= T1_)
lambda(i+1) = (1/2) * accl_ * t^2;
elseif (t > T1_ && t <= T2_)
lambda(i+1) = (1/2)*accl_*T1_^2 + accl_*T1_*(t - T1_);
elseif (t > T2_ && t <= 1)
lambda(i+1) = (1/2)*accl_*T1_^2 + accl_*T1_*(T2_ - T1_) + (1/2)*accl_*power(t - T2_, 2);
end
end
end
核心程序：
% 空間直線位置插補與RPY角姿態插補 + 梯形加減速歸一化處理
% 參數：起點S位置， 終點D位置, 末端線速度vs， 加減速度a
% 起點S的RPY角、終點D的RPY角
% 返回值：插值點（不包括起點S和終點D）
function [x y z alp beta gama N] = SpaceLine(S, D, S_, D_, vs, a)
x1 = S(1); y1 = S(2); z1 = S(3);
x2 = D(1); y2 = D(2); z2 = D(3);
alp1 = S_(1); beta1 = S_(2); gama1 = S_(3);
alp2 = D_(1); beta2 = D_(2); gama2 = D_(3);
P = 1; % 插值參數，增加插值點數，避免過小
% 總位移S
s = sqrt(power(x2 - x1, 2) + power(y2 - y1, 2) + power(z2 - z1, 2))
% 插值點數N
N = ceil(P*s / vs)
% 求歸一化參數
% function lambda = Normalization(pos, vel, accl, N)
lambda = Normalization(s, vs, a, N)
delta_x = x2 - x1;
delta_y = y2 - y1;
delta_z = z2 - z1;
delta_alp = alp2 - alp1;
delta_beta = beta2 - beta1;
delta_gama = gama2 - gama1;
for i = 1: N+1
x(i) = x1 + delta_x*lambda(i);
y(i) = y1 + delta_y*lambda(i);
z(i) = z1 + delta_z*lambda(i);
alp(i) = alp1 + delta_alp*lambda(i);
beta(i) = beta1 + delta_beta*lambda(i);
gama(i) = gama1 + delta_gama*lambda(i);
end
end
% 空間圓弧位置插補與RPY角姿態插補 + 梯形加減速歸一化處理
% 參數： 起點S位置和RPY角, 中間點M位置, 終點D位置和RPY角，末端角速度，角加減速度
% 方便起見，角速度和角加速度均爲角度制
function [x y z alp beta gama N] = SpaceCircle(S, M, D, S_, D_, ws, a)
x1 = S(1); x2 = M(1); x3 = D(1);
y1 = S(2); y2 = M(2); y3 = D(2);
z1 = S(3); z2 = M(3); z3 = D(3);
alp1 = S_(1); beta1 = S_(2); gama1 = S_(3);
alp2 = D_(1); beta2 = D_(2); gama2 = D_(3);
A1 = (y1 - y3)*(z2 - z3) - (y2 - y3)*(z1 - z3);
B1 = (x2 - x3)*(z1 - z3) - (x1 - x3)*(z2 - z3);
C1 = (x1 - x3)*(y2 - y3) - (x2 - x3)*(y1 - y3);
D1 = -(A1*x3 + B1*y3 + C1*z3);
A2 = x2 - x1;
B2 = y2 - y1;
C2 = z2 - z1;
D2 = -((x2^2 - x1^2) + (y2^2 - y1^2) + (z2^2 - z1^2)) / 2;
A3 = x3 - x2;
B3 = y3 - y2;
C3 = z3 - z2;
D3 = -((x3^2 - x2^2) + (y3^2 - y2^2) + (z3^2 - z2^2)) / 2;
A = [A1, B1, C1; A2, B2, C2; A3, B3, C3]
b = [-D1, -D2, -D3]'
% 圓心
C = Gauss_lie(3, A, b)
x0 = C(1); y0 = C(2); z0 = C(3);
plot3(x0, y0, z0, 'bo')
hold on
% 外接圓半徑
r = sqrt(power(x1 - x0, 2) + power(y1 - y0, 2) + power(z1 - z0, 2));
% 新座標系Z0的方向餘弦
L = sqrt(A1^2 + B1^2 + C1^2);
ax = A1 / L; ay = B1 / L; az = C1 / L;
% 新座標系X0的方向餘弦
nx = (x1 - x0) / r;
ny = (y1 - y0) / r;
nz = (z1 - z0) / r;
% 新座標系Y0的方向餘弦
o = cross([ax, ay, az], [nx, ny, nz]);
ox = o(1);
oy = o(2);
oz = o(3);
% 相對於基座標系{O-XYZ}， 新座標系{C-X0Y0Z0}的座標變換矩陣
T = [nx ox ax x0;
ny oy ay y0;
nz oz az z0;
0 0 0 1]
T_ni = T^-1
% 求在新座標系{C-X0Y0Z0}下S、M和D的座標
S_ = (T^-1)*[S'; 1]
M_ = (T^-1)*[M'; 1]
D_ = (T^-1)*[D'; 1]
x1_ = S_(1) , y1_ = S_(2), z1_ = S_(3)
x2_ = M_(1), y2_ = M_(2), z2_ = M_(3)
x3_ = D_(1), y3_ = D_(2), z3_ = D_(3)
% 判斷圓弧是順時針還是逆時針，並求解圓心角
if (atan2(y2_, x2_) < 0)
angle_SOM = atan2(y2_, x2_) + 2*pi;
else
angle_SOM = atan2(y2_, x2_);
end
if (atan2(y3_, x3_) < 0)
angle_SOD = atan2(y3_, x3_) + 2*pi;
else
angle_SOD = atan2(y3_, x3_);
end
% 逆時針
if (angle_SOM < angle_SOD)
flag = 1;
theta = angle_SOD % 圓心角
end
% 順時針
if (angle_SOM >= angle_SOD)
flag = -1;
theta = 2*pi - angle_SOD % 圓心角
end
% 插補點數N
P = 2; %插補參數，增加插值點數，避免過小
ws = ws*pi / 180; % 角度換成弧度
a = a*pi / 180;
N = P*theta / ws;
% 求歸一化參數
lambda = Normalization(theta, ws, a, N);
% 插補原理: 在新平面上進行插補（簡化）
% 在新座標系下z1_,z2_,z3_均爲0，即外接圓在新座標系的XOY平面內
% 此時轉化爲平面圓弧插補問題
delta_ang = theta;
delta_alp = alp2 - alp1
delta_beta = beta2 - beta1;
delta_gama = gama2 - gama1;
for i = 1: N+1
x_(i) = flag * r * cos(lambda(i)*delta_ang);
y_(i) = flag * r * sin(lambda(i)*delta_ang);
P = T*[x_(i); y_(i); 0; 1];
x(i) = P(1);
y(i) = P(2);
z(i) = P(3);
alp(i) = alp1 + delta_alp*lambda(i);
beta(i) = beta1 + delta_beta*lambda(i);
gama(i) = gama1 + delta_gama*lambda(i);
end
% % figure(1)
% % plot(x_, y_)
% 插補原理： 在原圓弧上進行插補
% 圓弧上任一點處沿前進方向的切向量
% x(1) = x1; y(1) = y1; z(1) = z1;
% for i = 1: N+1
% m(i) = flag*(ay*(z(i) - z0) - az*(y(i) - y0));
% n(i) = flag*(az*(x(i) - x0) - ax*(z(i) - z0));
% l(i) = flag*(ax*(y(i) - y0) - ay*(x(i) - x0));
% delta_s = delta_ang * r;
% E = delta_s / (r*sqrt(ax^2 + ay^2 + az^2));
% G = r / sqrt(r^2 + delta_s^2);
% x(i+1) = x0 + G*(x(i) + E*m(i) - x0);
% y(i+1) = y0 + G*(y(i) + E*n(i) - y0);
% z(i+1) = z0 + G*(z(i) + E*l(i) - z0);
% end
end
測試主程序
% Standard DH
% five_dof robot
% 在關節4和關節5之間增加一個虛擬關節，便於逆運動學計算
clear;
clc;
th(1) = 0; d(1) = 0; a(1) = 0; alp(1) = pi/2;
th(2) = 0; d(2) = 0; a(2) = 1.04;alp(2) = 0;
th(3) = 0; d(3) = 0; a(3) = 0.96; alp(3) = 0;
th(4) = 0; d(4) = 0; a(4) = 0; alp(4) = 0;
th(5) = pi/2; d(5) = 0; a(5) = 0; alp(5) = pi/2;
th(6) = 0; d(6) = 0; a(6) = 0; alp(6) = 0;
th(7) = 0; d(7) = 1.63; a(7) = 0.28; alp(7) = 0;
% DH parameters th d a alpha sigma
L1 = Link([th(1), d(1), a(1), alp(1), 0]);
L2 = Link([th(2), d(2), a(2), alp(2), 0]);
L3 = Link([th(3), d(3), a(3), alp(3), 0]);
L4 = Link([th(4), d(4), a(4), alp(4), 0]);
L5 = Link([th(5), d(5), a(5), alp(5), 0]);
L6 = Link([th(6), d(6), a(6), alp(6), 0]);
L7 = Link([th(7), d(7), a(7), alp(7), 0]);
robot = SerialLink([L1, L2, L3, L4, L5, L6, L7]);
robot.name='MyRobot-5-dof';
robot.display()
% 起點關節角[0 90 30 60 90 0 0]*pi/180
% 終點關節角[0 90 60 60 90 0 0]*pi/180
theta_S = [0 120 100 40 90 0 0]*pi/180;
theta_M = [60 100 100 30 90 0 0]*pi/180;
theta_D = [100 120 100 40 90 0 0]*pi/180;
% robot.teach();
% robot.plot(theta_D);
% hold on
T_S = robot.fkine(theta_S) %起點末端執行器位姿
T_M = robot.fkine(theta_M)
T_D = robot.fkine(theta_D)
% ik_T = five_dof_ikine(T_D)
[S_RPY(1) S_RPY(2) S_RPY(3)] = RPY_angle(T_S); % 起點對應的RPY角
[D_RPY(1) D_RPY(2) D_RPY(3)] = RPY_angle(T_D); % 終點對應的RPY角
S = T_S(1: 3, 4); % 起點對應的位置座標
M = T_M(1: 3, 4);
D = T_D(1: 3, 4); % 終點對應的位置座標
vs = 0.06; a = 0.02; % 直線插補速度參數
ws = 5; a = 2.5; % 空間圓弧插補速度參數
% [x y z alp beta gama N] = SpaceLine(S, D, S_RPY, D_RPY, vs, a); % 直線插補，得到插值點（不包括起點和終點）
[x y z alp beta gama N] = SpaceCircle(S', M', D', S_RPY, D_RPY, ws, a);
plot3(x, y, z)
hold on
th1(1) = theta_S(1); th2(1) = theta_S(2); th3(1) = theta_S(3);
th4(1) = theta_S(4); th5(1) = theta_S(5); th6(1) = theta_S(6); th7(1) = theta_S(7);
% t = [0 0 0 0 0];
T = {1, N};
for i = 1: N+1
R = RPY_rot(alp(i), beta(i), gama(i));
R(1, 1:3);
R(2, 1:3);
R(3, 1:3);
T{i} = [R(1, 1:3), x(i);
R(2, 1:3), y(i);
R(3, 1:3), z(i);
0 0 0 1];
theta = five_dof_ikine(T{i});
th = theta(2, 1:5); % 簡單取1行逆解
th1(i+1) = th(1);
th2(i+1) = th(2);
th3(i+1) = th(3);
th4(i+1) = th(4);
th5(i+1) = pi/2;
th6(i+1) = th(5);
th7(i+1) = 0;
end
for i = 1: N+1
T_ = robot.fkine([th1(i), th2(i), th3(i), th4(i), th5(i), th6(i), th7(i)]);
traj = T_(1: 3, 4);
a(i) = traj(1);
b(i) = traj(2);
c(i) = traj(3);
plot3(traj(1), traj(2), traj(3), 'r*');
hold on
robot.plot([th1(i), th2(i), th3(i), th4(i), th5(i), th6(i), th7(i)])
end
參考文獻
[1]楊淞. 一種六自由度機械臂的運動控制系統設計[D].上海交通大學,2014.
[2]陳國良,黃心漢,王敏.機械手圓周運動的軌跡規劃與實現[J].華中科技大學學報(自然科學版),2005(11):69-72.
[3]卓揚娃,白曉燦,陳永明.機器人的三種規則曲線插補算法[J].裝備製造技術,2009(11):27-29.
[4]林威,江五講.工業機器人笛卡爾空間軌跡規劃[J].機械工程與自動化,2014(05):141-143.
[5]Chen Z, Wang H, Lu X, et al. Kinematics analysis and application of 5-DOF manipulator with special joint[C]//2017 Chinese Automation Congress (CAC). IEEE, 2017: 7421-7426.