MATLAB機器人工具箱中機器人逆解是如何求出來的？

12-28

MATLAB機器人工具箱中機器人逆解是如何求出來的？

我就喜歡這樣知其然又要知其所以然的問題。

先回答怎麼樣？

matlab機器人工具箱求串聯機器人運動學逆解一般是採用ikine()函數，所以剛打開matlab看了一下源碼。
（貌似新版本還加了其他求解演算法，這邊先不說，先解決題主的問題。）
我把它的主要步驟和貼出：

% 初始化 % 定義目標T，迭代次數，誤差等； % 初始化當前誤差e,


while true

    % 計算誤差

    Tq = robot.fkine(q");

    e(1:3) = transl(T - Tq);

    Rq = t2r(Tq);

    [th,n] = tr2angvec(Rq"*t2r(T));

    e(4:6) = th*n;
    J = jacob0(robot, q);   % 計算雅克比

    % 根據末端誤差求取關節變化

    if opt.pinv  % 雅克比偽逆法

        dq = opt.alpha * pinv( J(m,:) ) * e(m);

    else         % 雅克比轉置法

        dq = J(m,:)" * e(m);

        dq = opt.alpha * dq;

    end
    % 更新關節值

    q = q + dq";

% 判斷誤差是否小於容許誤差tolerance nm = norm(e(m)); if nm &<= opt.tol break end end

所以，很簡單，Matlab工具箱就是採用雅克比矩陣偽逆（或轉置）來確定迭代方向，並通過迭代的方法使關節值收斂至目標位置。這應該是串聯機器人運動學逆解數值解的最常用方法。

再回答為什麼這麼簡單就可以求解逆解？

首先，只要給了D-H參數，雅克比矩陣很容易就可以推導出來（如果不會請參考各種機器人學教材，推薦John Craig的Introduction to robotics: mechanics and control）。

它就是關節速度與末端速度的線性關係。
$dx = Jcdot dq Rightarrow dq = J^{dagger } cdot dx$

於是，世界坐標系中的運動 $Delta x$ 可以近似用關節的運動 $Delta q$ 疊加得到；
$Delta x$ 越小，線性關係越準確，迭代求解的軌跡越接近圖中直線；
但是，我們並不關心求解過程的運動方向，只關心最後求解的誤差大小。
所以，一般迭代求解會對求解的步長做處理，也即增加一個變數 $alpha$ ，這樣可以加快求解速度。
$Delta heta = alpha J^{dagger } Delta x$
所以，雅克比偽逆的思路是讓機械臂末端往目標點方向移動。使用偽逆是為了應對非6-dof情況。當然，看上面的代碼，matlab機器人工具箱似乎可以用雅克比的轉置代替雅克比偽逆。

% 根據末端誤差求取關節變化 if opt.pinv % 雅克比偽逆法 dq = opt.alpha * pinv( J(m,:) ) * e(m); else % 雅克比轉置法 dq = J(m,:)" * e(m); dq = opt.alpha * dq; end

這也很有趣，而且可以從很多角度來解釋：
1）從力的角度：
根據虛功原理，可以有如下關係：
$au =J^{T}cdot F$

其中， $au$ 是每個關節的力（力矩），F是末端受力。
$Delta heta = alpha J^{T } Delta x$
所以，雅克比轉置的方法是給機械臂每個關節一個朝目標點移動的力矩，這樣就相當於一直拉著機械臂末端往目標點拖，機械臂最終將收斂到目標位置。

2）從優化角度：

設定優化目標函數F，逆解求解就成了一個非線性優化問題。
利用梯度下降法可以推導得到相同的公式，殊途同歸的感覺真棒！
所以，雅克比轉置的思路是給機械臂末端一個朝著目標點方向的力。當然，兩種方法各有優劣：
1）偽逆法可能會遇到矩陣奇異等問題；
2）轉置法收斂比較慢。

它是通過newton迭代法求解的，最好用的是9.8版本。下面給一個6自由度求解示例：
clear;clc;close all;
x1=-pi/2; a1=121.16; d1=191.99;
x2=0; a2=850.94; th2=82.19*pi/180;
x3=0; a3=482.6;
x4=pi/2; a4=133.35;
x5=-pi/2; a5=0; th5=pi/2;
x6=0; d6=380.9;
L(1)=Link([0 d1 a1 x1]); L(1).qlim=[-pi,pi];
L(2)=Link([0 0 a2 x2]); L(2).qlim=[-pi/6,pi/2];
L(3)=Link([0 0 a3 x3]); L(3).qlim=[-pi/2,pi/2];
L(4)=Link([0 0 a4 x4]); L(4).qlim=[-pi/2,pi/2];
L(5)=Link([0 0 a5 x5]); L(5).qlim=[-pi/2,pi/2];
L(6)=Link([0 d6 0 x6]); L(6).qlim=[-pi,pi];
robot6=SerialLink(L,"name","robot6","offset",[0 -th2 th2 0 -th5 0]);
robot6.teach("degrees");
下面給出一個8自由度機械手逆解運動視頻：
視頻封面

Matlab機器人工具箱求機械手運動學逆解—在線播放—優酷網，視頻高清在線觀看視頻

matlab robotic toolbox-9.8 可以在Guestbook: Robotics Toolbox下載。這是國外學者peter corke開發的一個工具箱，作者原著教材pdf網上也有的。此外這位學者開發了matlab的機器視覺工具箱。

其實matlab現在最新版本自帶了mathworks公司自主開發的Robotics System Toolbox，比那個peter corke的工具箱要完善和強大很多

同樣很想問。似乎是通過數值解法求解的。