0.前言
逆運動學,是通過操作空間(笛卡爾空間)的位姿,求解關機空間的關節參數的過程。把它放在雅克比後面,是因為有些逆運動學算法需要用到雅克比矩陣。處理逆運動學的方法很多,我們這裡介紹一些常用的方法。
1 可解性
操作臂的逆運動學求解是一個非線性問題。已知位姿矩陣T,求解對應的關節角θ1,θ2,..,θn的值。我們這裡得到的是一些很難解的非線性超越方程。我們關係解的存在性、多解性和求解方法。
1.1 存在性
如果逆運動學有解,則目標點坐標必定落在工作空間內。操作空間是末端執行器能作用到的空間範圍。機械臂的操作空間用兩個很有用的概念:可達操作空間和靈活操作空間。
● 靈活操作空間:是機器人終端執行器在所有方位都能達到的空間範圍。
● 可達操作空間:是機器人終端執行器至少在一個方位上能夠達到的空間方位。
當機器人少於6個自由度時,它不能在三維空間內獲得一般的目標位置和方向。
1.2 多解性
當多解存在時,就會產生一個問題,我們選誰。一般選取較近的解是比較合理的(下圖:左),但是有時會遇到障礙(下圖:右),一般需要計算出所有的解以供取捨。
通常解的數目取決於關節數目、關節參數和運動範圍。如PUMA560就可能有8組解(下圖給出四種)。
通常非零關節連桿參數越多,解會越多。全是旋轉關節的6自由度機器臂,最多有16個可能解。
2 解析法
解析法通過解方程的辦法求解關節角。通過這些方程轉化或者幾何關係,找出對應的多個限制關係就可以解出對應的關節變量。解析法一般都效率比較高,並且可以求解出所有的可能解。但是,因為這些方程式非線性超越方程,並不一定能求解出解析解。而且具體求解公式依機械臂具體構型相關,一般沒有通用性(構型不同時)。
2.1 代數法
代數法就是直接通過位姿矩陣T0,..Tn中的項進行組合,定義中間變量(關節參數的組合),構造一元高次方程,求解(可以通過24x24的矩陣),最後求出所有關節角。
這裡用上面R3機械臂做個栗子:
對於上述3R機械臂(3個旋轉關節記為3R),通過DH法可以得到末端位姿和各關節的角度關係:
其中:x、y為平面坐標,φ代表末端方向和x0軸的夾角,將對應的變量聯立:
求解上面方方程這裡不再展開了,使用和角公式求解出對應的關節角度即可,計算中需要用到下面函數:
因為旋轉一周是360度,使用三角函數的反函數值域只有一半,因此這裡使用擴展的atan2函數。
2.2 幾何法(向量法)
我們試著把機械臂的空間幾何問題轉化為若干平面幾何問題。當多個連桿在一個平面中時,會出現關節角的和角形式,可以通過正餘弦定理建立條件求解。
同樣用上面R3的機械臂做個栗子:
這裡整個圖形都處在一個平面中,利用第三個關節的位置,通過餘弦定理得到下面方程:
可以求解θ2,再通過餘弦定理得到下面方程:
通過觀察可以計算出β:
這樣我們就可以求解θ1,通過三個角度和為末端姿態,可以求解出θ3。
多數情況下我們使用代數和幾何組合的方法求解。當使用幾何法求解釋,有時只要獲得第一個解就可以通過對稱性求解出其他解。
2.3 pipper準則
pipper給出了一些可以求解解析解的情況。有6個旋轉關節的機械臂,具有封閉解的充分條件是三個相鄰關節交於一點(一般是末端,這樣可以位置和姿態分開求解,簡化運算)。大多數的工業機器人都符合這一準則。
這裡我就不舉例求解了,求解過程中可以將位置和姿態關節的其次矩陣分開:T1T2T3,T4T5T6。然後通過上面的代數和幾何法求解。
3.數值法
逆運動學的數值解法,一般的書上討論的不多,相關論文比較多,這部分主要是參考論文和課件中的資料。
數值方法一般採用的都是迭代的方法,使計算出來的角度值不斷地逼近目標值。
迭代一般有三種模型:
● 每次運動控制,只迭代一次(可能距離目標點很遠),然後進行控制,然後進入下次迭代;適合距離較遠的兩點;
● 每次運動控制,一直迭代到距離目標值的閾值範圍內,然後進行控制,然後進入下次迭代;適合距離較近的兩點,如果距離較遠需要先計算中一些中間點;
● 每次運動控制,迭代固定次數(不考慮到目標距離),然後進行控制,然後進入下次迭代;(一般比較近的點迭代2~5次就可以計算到很高精度)
3.1 雅克比逆
因為雅克比是速度间的映射,分別在笛卡爾空間和關節空間對速度積分有:
因為我們在控制時使用的都是離散變量所以寫成如下形式:
通過求解雅克比矩陣的逆,我們就可以求出關節增量:
利用 θ = θ + △θ進行迭代。這裡的雅克比矩陣可以使用當前或者目標位置計算(目標位置的雅克比或出現較好效果)。
利用雅克比矩陣忽略了高階的無窮小量,奇異時非線性項不是可以忽略的部分,因此這種方法無法解決奇異時的情況。奇異附近的速度也會出現巨大比例的放縮。
3.2 雅克比轉置
上面的方法,在雅克比不是方陣或者雅克比不可逆的時候就無法進行求解,因此需要進行修正。
這裡使用雅克比的轉置替換雅克比的逆即可,成立的原因如下(設F為最小代價函數):
求梯度下降方向:
這裡α可以取值如下(e為笛卡爾末端的偏差,這裡取的比例為偏差的投影):
3.3 雅克比偽逆
一般情況下使用雅克比轉置效果並不好,很有肯恩不收斂或者收斂的太慢。
這裡使用廣義逆矩陣的一種(具體定義可以參考線性代數和矩陣論書籍)。通過下面代價函數得到:
得到的偽逆為:
這裡還有一個重要性質:
這裡後面的部分投影到雅克比矩陣的零空間,所以可以選取任意的φ(代價函數在零空間投影)進行不同的目標控制(可以對奇異構型或則冗餘關節進行限制)。
3.4 最小阻尼二乘(DLS)
上面的方法可以達到收斂,並且在奇異值附近效果也不錯,但是不能處理奇異的狀態(在不使用零空間時)。
構造代價函數如下:
其中λ 為非零阻尼係數,整理后的方程如下:
上式前半部分是偽逆,後半部分是轉置。這種方法的缺點是需要確定阻尼係數,一般選取奇異值分解后的最大值和最小值的比值(除了0),這樣的話計算量就會較大。
3.5 奇異值分解(SVD)分析
奇異值分解是非常強大的工具,這裡使用奇異值分解來觀察偽逆和DLS。
將雅克比矩陣(nxm)奇異值分解如下:
其中U為nxn的正交單位陣,V為mxm的正交單位陣,D是對角陣,其中對角線的元素為J的特征值:
設J的秩為r,則大於r的列投影到零空間。將U,V寫成列向量ui和vi則:
通過同樣地方法求出,偽逆的SVD為:
通過同樣地方法求出,DLS的SVD為:
通過上面的式子可以知道為什麼偽逆不能處理奇異狀態,而DLS卻可以。
通過不同的代價函數對應SVD下不同的特征值,也等價于在零空間選取不同的φ,這些方法都是相似的。在奇異或冗餘情況下(降維,有些維度投影到零空間),他們構成了新的約束(不加入約束條件方程無法求解)。
3.6 改進方案
當目標距離處於工作空間之外或者距離當前位置較遠時,會出現震動現象。
對弈上面個每種方法,這裡有一個通用的優化:對每次移動的偏差上限進行限制。
將每次迭代使用的偏差值,定義如下:
其中:
這裡的di為上次移動的距離,初始d0為無窮大,Dmax對應取關節長度的一般(每次行走的最大步長不超過30度)。
4 CCD(Cyclic Coordinate Descent)
在計算機仿真中經常使用的一種發發是CCD算法,這種算伐對每個關節獨立計算,在關節空間求解,避免了奇異的情況。
如上圖所示,計算過程沒建立求解關節和末端位置的向量r,求解關節和目標位置的向量g通過兩個向量的夾角求解對應的運動角度(可以使用內積求解)。
不過這種方法在處理多關節時,就不太好用了,計算量會變得比較大,控制性能也不是很好。
5 示教
很多工業機器人使用示教的方法運動到目標點。示教過程中,用手拖動機器人使其到達目標位姿,通過傳感器記錄路徑中間的關節坐標(稱為示教點),然後(可能利用插補)依次執行對應的關節角序列即可。這裡實在關節空間控制,不涉及逆運動學問題(不涉及任務空間)。這樣就涉及到重複定位精度。
如果任務規定在直角坐標系內,就需要使用逆運動學求解關節角,這樣可以到達未經過示教的點(稱為計算點)。機械臂能到達計算點的精確程度涉及到定位精度。
為了提升對應精度就涉及到參數識別和校準技術。
6 例子
這裡利用一個3R機械臂做個例子如下圖(左圖為初始位置,右圖為目標位置):
對應的各關節運動如下:
代碼如下(其中有動畫):
%% 定義參數 關節數+DH參數
JOINT_SIZE = 3+1;
a = [2, 2, 2]';
alpha = [0, 0, 0]'*pi/180.0;
d = [0, 0, 0]';
cta = [30, 30, 30]'*pi/180.0;
%% 定義數組
T = cell(JOINT_SIZE);
R = cell(JOINT_SIZE);
P = cell(JOINT_SIZE);
%% 計算正運動學
T{1} = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
P{1} = T{1}(1:3, 4);
R{1} = T{1}(1:3, 1:3);
for k=2:JOINT_SIZE
T{k} = T{k-1}*DH(a(k-1), alpha(k-1), d(k-1), cta(k-1));
P{k} = T{k}(1:3, 4);
R{k} = T{k}(1:3, 1:3);
end
% 繪圖
figure(1)
clf;
DrawCoordinate('0', P{1}, R{1});
for k=2:JOINT_SIZE
DrawLine(P{k-1}, P{k});
DrawCylinder(P{k-1}, R{k-1});
DrawCoordinate('0'+k-1, P{k}, R{k});
end
axis equal; % 顯示坐標軸比例
view(0,90); % 指定子图1的视点
%% 迭代法求逆解
target = [6/sqrt(2.0); 6/sqrt(2.0); 45*pi/180.0]; % 目標點
PATH_SIZE = 20;
save_cta = zeros(3,PATH_SIZE);
for i=1:PATH_SIZE
save_cta(1:3,i) = cta;
% 誤差
error = [target(1) - P{JOINT_SIZE}(1); target(2) - P{JOINT_SIZE}(2); target(3) - (cta(1)+cta(2)+cta(3))];
% 雅克比,手動求偏導
Jacob0 = [-a(1)*sin(cta(1))-a(2)*sin(cta(1)+cta(2))-a(3)*sin(cta(1)+cta(2)+cta(3)) -a(2)*sin(cta(1)+cta(2))-a(3)*sin(cta(1)+cta(2)+cta(3)) -a(3)*sin(cta(1)+cta(2)+cta(3))
a(1)*cos(cta(1))+a(2)*cos(cta(1)+cta(2))+a(3)*cos(cta(1)+cta(2)+cta(3)) a(2)*cos(cta(1)+cta(2))+a(3)*cos(cta(1)+cta(2)+cta(3)) a(3)*cos(cta(1)+cta(2)+cta(3))
1 1 1];
% 逆解迭代
cta = cta + pinv(Jacob0)*error;
% DH法正解
T{1} = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
P{1} = T{1}(1:3, 4);
R{1} = T{1}(1:3, 1:3);
for k=2:JOINT_SIZE
T{k} = T{k-1}*DH(a(k-1), alpha(k-1), d(k-1), cta(k-1));
P{k} = T{k}(1:3, 4);
R{k} = T{k}(1:3, 1:3);
end
% 数据更新
P_tool = T{JOINT_SIZE}(1:3, 4);
R_tool = T{JOINT_SIZE}(1:3, 1:3);
% 绘图
figure(2)
clf;
DrawCoordinate('0', P{1}, R{1});
for k=2:JOINT_SIZE
DrawLine(P{k-1}, P{k});
DrawCylinder(P{k-1}, R{k-1});
DrawCoordinate('0'+k-1, P{k}, R{k});
end
axis equal; % 顯示坐標軸比例
view(0,90); % 指定子图1的视点
drawnow;
end
figure(3)
subplot(2, 2, 1);
plot(1:PATH_SIZE, save_cta(1,1:PATH_SIZE));
title('关节1位置(弧度)');
subplot(2, 2, 2);
plot(1:PATH_SIZE, save_cta(2,1:PATH_SIZE));
title('关节2位置(弧度)');
subplot(2, 2, 3);
plot(1:PATH_SIZE, save_cta(3,1:PATH_SIZE));
title('关节3位置(弧度)');
7 參考文獻
《机器人学导论:分析控制及应用》(美)尼库 著
《机器人学导论》(美)克来格 著
《机器人学导论》(美)约翰 J 卡雷格 著
《机器人学:建模、规划与控制》布鲁诺·西西里安诺、洛伦索·夏维科、路易吉·维拉尼 、朱塞佩·奥里奥洛 著
《Springer Handbook of Robotics》布鲁诺·西西里安诺、歐莎瑪 哈提卜