機器人逆運動學(xué)

1、精選優(yōu)質(zhì)文檔-傾情為你奉上clear;clc;L1 = Link('d', 0, 'a', 0, 'alpha', pi/2); %Link 類函數(shù)L2 = Link('d', 0, 'a', 0.5, 'alpha', 0,'offset',pi/2);L3 = Link('d', 0, 'a', 0, 'alpha', pi/2,'offset',pi/4);L4 = Link('d', 1, 

2、9;a', 0, 'alpha', -pi/2);L5 = Link('d', 0, 'a', 0, 'alpha', pi/2);L6 = Link('d', 1, 'a', 0, 'alpha', 0);b=isrevolute(L1); %Link 類函數(shù)robot=SerialLink(L1,L2,L3,L4,L5,L6); %SerialLink 類函數(shù)='帶球形腕的擬人臂' %SerialLink 屬性值robot.manuf=

3、'飄零過客' %SerialLink 屬性值robot.display(); %Link 類函數(shù)theta=0 0 0 0 0 0;robot.plot(theta); %SerialLink 類函數(shù)theta1=pi/4,-pi/3,pi/6,pi/4,-pi/3,pi/6;p0=robot.fkine(theta);p1=robot.fkine(theta1);s=robot.A(4 5 6,theta);cchain=robot.trchain;q=robot.getpos();q2=robot.ikine(p1); %逆運動學(xué)j0=robot.jacob0(q2); %

4、雅可比矩陣p0 = -0.7071 -0.0000 0.7071 1.4142 0.0000 -1.0000 -0.0000 -0.0000 0.7071 0.0000 0.7071 1.9142 0 0 0 1.0000p1 = 0.9874 0.1567 0.0206 1.0098 0.0544 -0.4593 0.8866 1.8758 0.1484 -0.8743 -0.4621 0.0467 0 0 0 1.0000>> ss = 1 0 0 0 0 1 0 0 0 0 1 2 0 0 0 1cchain =Rz(q1)Rx(90)Rz(q2)Tx(0.5)Rz(q3)R

5、x(90)Rz(q4)Tz(1)Rx(-90)Rz(q5)Rx(90)Rz(q6)Tz(1)q = 0 0 0 0 0 0q2 = 1.0e+04 * 0.0003 0.0180 -0.0399 1.1370 0.0002 0.0536j0 = -0.1100 0.0707 0.3577 -0.0114 0.5092 0 -0.8329 -0.0448 -0.2267 -0.6224 0.1813 0 -0.0000 0.7623 0.3956 -0.1410 -0.8413 0 -0.0000 0.5354 0.5354 0.3374 -0.0178 -0.8605 0.0000 0.844

6、6 0.8446 -0.2139 -0.9751 0.1275 1.0000 0.0000 0.0000 0.9168 -0.2209 -0.4933作者：fly qq鏈接：來源：知乎著作權(quán)歸作者所有，轉(zhuǎn)載請聯(lián)系作者獲得授權(quán)。matlab機器人工具箱求串聯(lián)機器人運動學(xué)逆解一般是采用ikine()函數(shù)，所以剛打開matlab看了一下源碼。（貌似新版本還加了其他求解算法，這邊先不說，先解決題主的問題。）我把它的主要步驟和貼出：% 初始化% 定義目標(biāo)T，迭代次數(shù)，誤差等；% 初始化當(dāng)前誤差e,while true % 計算誤差 Tq = robot.fkine(q'); e(1:3) = t

7、ransl(T - Tq); Rq = t2r(Tq); th,n = tr2angvec(Rq'*t2r(T); e(4:6) = th*n; J = jacob0(robot, q); % 計算雅克比 % 根據(jù)末端誤差求取關(guān)節(jié)變化 if opt.pinv % 雅克比偽逆法 dq = opt.alpha * pinv( J(m,:) ) * e(m); else % 雅克比轉(zhuǎn)置法 dq = J(m,:)' * e(m); dq = opt.alpha * dq; end % 更新關(guān)節(jié)值 q = q + dq' % 判斷誤差是否小于容許誤差tolerance nm =

8、norm(e(m); if nm <= opt.tol break endend所以，很簡單，Matlab工具箱就是采用雅克比矩陣偽逆（或轉(zhuǎn)置）來確定迭代方向，并通過迭代的方法使關(guān)節(jié)值收斂至目標(biāo)位置。這應(yīng)該是串聯(lián)機器人運動學(xué)逆解數(shù)值解的最常用方法。· 再回答為什么這么簡單就可以求解逆解？首先，只要給了D-H參數(shù)，雅克比矩陣很容易就可以推導(dǎo)出來（如果不會請參考各種機器人學(xué)教材，推薦John Craig的Introduction to robotics: mechanics and control）。<img src=" data-rawwidth="57

9、5" data-rawheight="315" class="origin_image zh-lightbox-thumb" width="575" data-original="它就是關(guān)節(jié)速度與末端速度的線性關(guān)系。<img src=" data-rawwidth="658" data-rawheight="332" class="origin_image zh-lightbox-thumb" width="658" d

10、ata-original="于是，世界坐標(biāo)系中的運動可以近似用關(guān)節(jié)的運動疊加得到；越小，線性關(guān)系越準(zhǔn)確，迭代求解的軌跡越接近圖中直線；但是，我們并不關(guān)心求解過程的運動方向，只關(guān)心最后求解的誤差大小。所以，一般迭代求解會對求解的步長做處理，也即增加一個變量，這樣可以加快求解速度。所以，雅克比偽逆的思路是讓機械臂末端往目標(biāo)點方向移動。使用偽逆是為了應(yīng)對非6-dof情況。當(dāng)然，看上面的代碼，matlab機器人工具箱似乎可以用雅克比的轉(zhuǎn)置代替雅克比偽逆。 % 根據(jù)末端誤差求取關(guān)節(jié)變化 if opt.pinv % 雅克比偽逆法 dq = opt.alpha * pinv( J(m,:) ) *

11、 e(m); else % 雅克比轉(zhuǎn)置法 dq = J(m,:)' * e(m); dq = opt.alpha * dq; end這也很有趣，而且可以從很多角度來解釋：1）從力的角度：根據(jù)虛功原理，可以有如下關(guān)系：<img src=" data-rawwidth="740" data-rawheight="407" class="origin_image zh-lightbox-thumb" width="740" data-original="其中，是每個關(guān)節(jié)的力（力矩），F(xiàn)是

12、末端受力。所以，雅克比轉(zhuǎn)置的方法是給機械臂每個關(guān)節(jié)一個朝目標(biāo)點移動的力矩，這樣就相當(dāng)于一直拉著機械臂末端往目標(biāo)點拖，機械臂最終將收斂到目標(biāo)位置。2）從優(yōu)化角度：<img src=" data-rawwidth="746" data-rawheight="443" class="origin_image zh-lightbox-thumb" width="746" data-original="設(shè)定優(yōu)化目標(biāo)函數(shù)F，逆解求解就成了一個非線性優(yōu)化問題。利用梯度下降法可以推導(dǎo)得到相同的公式，殊途同

13、歸的感覺真棒！所以，雅克比轉(zhuǎn)置的思路是給機械臂末端一個朝著目標(biāo)點方向的力。當(dāng)然，兩種方法各有優(yōu)劣：1）偽逆法可能會遇到矩陣奇異等問題；2）轉(zhuǎn)置法收斂比較慢。于MATLAB_robotic_toolbox的機器人逆解教程  (2014-08-15 22:07:20)標(biāo)簽： 機器人工具箱使用大致分為三步：一、構(gòu)件模型；二、規(guī)劃軌跡及使用逆解函數(shù)；三、圖形輸出。 一、建立機器人數(shù)學(xué)模型（連桿機構(gòu)），如下： >> l1=Link(0 0 0 pi/2);     l2=Link(0 0 0.4318

14、0);     l3=Link(-pi/2 0.1505 0.02032 -pi/2);     l4=Link(0 0.4318 0 pi/2);     l5=Link(0 0 0 -pi/2);     l6=Link(0 0 0 0);     pm560=SerialLink(l1 l2 l3 l4 l5 l6);其中Link（ d a

15、）里的表示兩個連桿的夾角，d表示z方向的長度（z為選轉(zhuǎn)軸），a為連桿的長度即x方向的長度（參考的坐標(biāo)為桿首段坐標(biāo)即前一桿尾端坐標(biāo)。每個桿的兩端放置一個笛卡爾坐標(biāo)，通過坐標(biāo)變換實現(xiàn)求解。）。為桿兩端兩個坐標(biāo)的夾角（主要指兩個選擇軸z的夾角）。機器人的正解如下：機器人逆解的如下：其中ikine6s與ikine函數(shù)解不一樣。但是再正解后結(jié)果是一樣的？。二、下面為逆解過程：即從坐標(biāo)點各關(guān)節(jié)角度值的過程。  >>t=0:0.2:2;      >> T2=transl(0.4521,0,0.4318)

16、;      >> T=ctraj(T1,T2,length(t);      >> Q=pm560.ikine6s(T); >> pm560.plot(Q)             %顯示機器人三維圖動畫過程。 >> tranimate(T)   &

17、#160;              %動畫演示坐標(biāo)系自初始點運動到目標(biāo)點的過程 三、演示如何畫圖： >> s(1)=subplot(3,2,1);         %一共分三行，每行兩個，現(xiàn)在畫出第一行第一個。      >> plot(t,Q(:,1)              %畫出對應(yīng)時間t的關(guān)節(jié)角度變化，Q（：，1）表示一個關(guān)節(jié)角度值。>> xlabel(s(1),'times')            %為圖表添加橫坐標(biāo)。>> ylabel(s(1),