環(huán)境：MATLAB 2017B+Robotics Toolbox 9.10.0

前期準(zhǔn)備：完成機(jī)械臂數(shù)學(xué)模型的建立+計(jì)算機(jī)械臂工作空間

https://blog.csdn.net/Kalenee/article/details/81990130

注意：這里采用幾何法計(jì)算機(jī)械臂逆解，因此不一定適用于其他六軸機(jī)械臂構(gòu)型。

?

一、運(yùn)動(dòng)學(xué)分析

連桿變換是機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)分析的基礎(chǔ)，其建立主要涉及到坐標(biāo)變換，其中包括坐標(biāo)旋轉(zhuǎn)和坐標(biāo)平移。

坐標(biāo)旋轉(zhuǎn)變換為繞坐標(biāo)系的X、Y和Z軸的旋轉(zhuǎn)變換，一般情況下一個(gè)旋轉(zhuǎn)變換為幾個(gè)基本旋轉(zhuǎn)變換的合成。下面式1為繞X軸旋轉(zhuǎn)的基本旋轉(zhuǎn)矩陣，式2為繞Y軸旋轉(zhuǎn)的基本旋轉(zhuǎn)矩陣，式3為繞Z軸旋轉(zhuǎn)的基本旋轉(zhuǎn)矩陣。

??

?坐標(biāo)平移變換為坐標(biāo)系沿坐標(biāo)軸X、Y和Z軸進(jìn)行平移轉(zhuǎn)換，平移轉(zhuǎn)換后的兩個(gè)坐標(biāo)系的原點(diǎn)不同，但坐標(biāo)軸相對(duì)平行。式3-4為基本平移矩陣，、和為平移后的矩陣相對(duì)于原矩陣平移的距離。

?為在進(jìn)行運(yùn)動(dòng)學(xué)分析過程中，簡(jiǎn)化計(jì)算，一般將坐標(biāo)轉(zhuǎn)換矩陣進(jìn)行齊次變換，轉(zhuǎn)變?yōu)辇R次變換矩陣。

?通常情況下所有的連桿變換都可以通過坐標(biāo)旋轉(zhuǎn)和坐標(biāo)平移獲得，即可通過坐標(biāo)旋轉(zhuǎn)變換和坐標(biāo)平移變換的復(fù)合變換獲得連桿變換矩陣。如圖1為兩個(gè)坐標(biāo)系的變換，其變換公式為下式

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? 圖1 旋轉(zhuǎn)和平移復(fù)合的一般坐標(biāo)變換

根據(jù)前面博文建立的機(jī)械臂數(shù)學(xué)模型，按照各關(guān)節(jié)坐標(biāo)系的旋轉(zhuǎn)和平移分別建立對(duì)應(yīng)的齊次連桿變換矩陣：

根據(jù)各關(guān)節(jié)的齊次變換矩陣可計(jì)算機(jī)器人的位置方程，通過位置方程可以獲取關(guān)節(jié)變量對(duì)機(jī)械臂末端姿態(tài)的影響。在本課題中，將矩陣到按順序進(jìn)行相乘可獲得設(shè)計(jì)機(jī)械臂的位置方程，同時(shí)得到機(jī)械臂末端相對(duì)于基座坐標(biāo)系的位置與姿態(tài)。

關(guān)節(jié)機(jī)器人的逆運(yùn)動(dòng)學(xué)是根據(jù)末端位置及姿態(tài)求解機(jī)器人各關(guān)節(jié)的角度。與正運(yùn)動(dòng)學(xué)一組關(guān)節(jié)角度對(duì)應(yīng)一個(gè)末端位姿不同，逆運(yùn)動(dòng)學(xué)有概率存在機(jī)器人末端的某一位姿對(duì)應(yīng)多組關(guān)節(jié)角度，因此在進(jìn)行求解的過程中需加入約束條件。?

?

二、逆解計(jì)算

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?圖2 機(jī)械臂計(jì)算逆解流程圖?

上圖為機(jī)械臂計(jì)算逆解流程圖，逆解計(jì)算首先獲取需要機(jī)械臂末端執(zhí)行器到達(dá)的位置的坐標(biāo)及其姿態(tài)，判斷其位置坐標(biāo)是否在機(jī)械臂的工作空間中，如果是才開始進(jìn)行逆解的計(jì)算。

然后將六個(gè)關(guān)節(jié)角度分為兩組，先使用幾何法計(jì)算關(guān)節(jié)角度一、二和三，因?yàn)檫@三個(gè)角度可在不涉及角度四、五和六的情況下進(jìn)行求解。角度一主要控制機(jī)械臂整體的旋轉(zhuǎn)，可投影到XOY坐標(biāo)系進(jìn)行計(jì)算，角度二和角度三與機(jī)械臂整體旋轉(zhuǎn)無關(guān)，可投影到坐標(biāo)系XOZ或者YOZ進(jìn)行計(jì)算。此處單純采用幾何法完成關(guān)節(jié)角的計(jì)算，所以受機(jī)械臂構(gòu)型影響較大，無法通用，對(duì)其余構(gòu)型機(jī)械臂參考價(jià)值不大，因此此處不列出計(jì)算具體流程。完成角度一、二和三計(jì)算后，將其結(jié)果代入機(jī)械臂的DH矩陣中，為后續(xù)計(jì)算做準(zhǔn)備。

接著，計(jì)算和的乘積，并在代入角度一、二和三后計(jì)算?、 、 和的乘積，兩次計(jì)算所獲得的結(jié)果應(yīng)該是相等的，因此，根據(jù)計(jì)算所獲得的矩陣，比較兩邊矩陣內(nèi)部的各元素，獲取一邊為常數(shù)，另一邊為單個(gè)未知數(shù)的兩個(gè)元素，通過求解該單一未知數(shù)分別求解角度四、五和六。

因所獲得的解有可能并不唯一，所以最后根據(jù)約束條件，對(duì)所獲得的解進(jìn)行篩選，排除不在約束條件內(nèi)部的解，并根據(jù)路徑最短原則選取最優(yōu)解作為最終的結(jié)果。

?

三、程序?qū)崿F(xiàn)

%六軸機(jī)械臂幾何法反解計(jì)算 %流程：輸入末端點(diǎn)坐標(biāo)，檢查是否在工作空間范圍內(nèi)，計(jì)算反解，正解驗(yàn)證 clc; clear; format short;%保留精度%角度轉(zhuǎn)換 du=pi/180; %度 radian=180/pi; %弧度%% 模型導(dǎo)入 robot_hand;%% 工作空間計(jì)算 figurews;%% DH參數(shù)矩陣 R=[0;0;0]; syms theta1 theta2 theta3 theta4 theta5 theta6 % theta d a alpha L=[0 L1 0 -pi/2 ;pi/2 0 -L2 0 ;0 0 -L3 pi/2 ;0 L4 0 -pi/2 ;pi/2 0 0 pi/2 ; 0 L5 0 0 ]; T_start=six_link.fkine([0 0 0 0 0 0]); q_test=[pi/4 pi/3 pi/3 pi/6 pi/3 pi/4]; T_test_end=six_link.fkine(q_test);A{1}=trotz(theta1)*transl(0,0,L(1,2))*trotx(L(1,4))*transl(L(1,3),0,0); A{2}=trotz(theta2)*transl(0,0,L(2,2))*trotx(L(2,4))*transl(L(2,3),0,0); A{3}=trotz(theta3)*transl(0,0,L(3,2))*trotx(L(3,4))*transl(L(3,3),0,0); A{4}=trotz(theta4)*transl(0,0,L(4,2))*trotx(L(4,4))*transl(L(4,3),0,0); A{5}=trotz(theta5)*transl(0,0,L(5,2))*trotx(L(5,4))*transl(L(5,3),0,0); A{6}=trotz(theta6)*transl(0,0,L(6,2))*trotx(L(6,4))*transl(L(6,3),0,0);T6=A{1}*A{2}*A{3}*A{4}*A{5}*A{6};% 輸入末端點(diǎn)坐標(biāo), 末端姿態(tài)默認(rèn)與初始狀態(tài)一致point_xyz=inputdlg({'X','Y','Z'},'末端點(diǎn)坐標(biāo)',1,{'52.7','0','48.75'});point_x=str2double(point_xyz{1});point_y=str2double(point_xyz{2});point_z=str2double(point_xyz{3});%% target point X=point_x; Y=point_y; Z=point_z; Judge_Point_in_WS=0;if (X<X_max)&&(X>X_min)if (Y<Y_max)&&(Y>Y_min)if (Z<Z_max)&&(Z>Z_min)if (X^2+Y^2+Z^2)<R_max_squreJudge_Point_in_WS=1;endendend end%% ikine if Judge_Point_in_WS==1T06=[T_start(1:3,1:3) [X;Y;Z];0 0 0 1] T5=T06*inv(A{6});X_t=double(T5(1,4));Y_t=double(T5(2,4));Z_t=double(T5(3,4));%T45=A{3}*A{4};%double(T45(3,4));r_squre=X_t^2+Y_t^2;r_gen=sqrt(r_squre);S=Z_t-L1;R_squre=S^2+r_squre;R_gen=sqrt(R_squre);%% solve theta1theta11=atan2(Y_t,X_t);theta11=double(real(theta11));%% solve theta3a33=sqrt(L3^2+L4^2);a33_angle=atan(L4/L3);cos_theta3_bu=(L2^2+a33^2-R_squre)/(2*L2*a33);theta33=pi-acos(cos_theta3_bu)-a33_angle;theta33=double(theta33);%% solve theta2cos_theta2_bu=(L2^2+R_squre-a33^2)/(2*L2*R_gen);if S>0theta22=pi/2-acos(cos_theta2_bu)-atan(S/r_gen);elsetheta22=pi/2-acos(cos_theta2_bu)+atan(-S/r_gen);endtheta22=double(theta22); % end % %% solve to theta4 theta5 theta6%% 賦值A(chǔ)11=trotz(L(1,1)+theta11)*transl(0,0,L(1,2))*trotx(L(1,4))*transl(L(1,3),0,0);A22=trotz(L(2,1)+theta22)*transl(0,0,L(2,2))*trotx(L(2,4))*transl(L(2,3),0,0);A33=trotz(L(3,1)+theta33)*transl(0,0,L(3,2))*trotx(L(3,4))*transl(L(3,3),0,0);%% solve theta4 theta5 theta6T321=inv(A11*A22*A33);T45=simplify(A{4}*A{5});kimejer=simplify(T321*T5);%% solve theta6str_solve=kimejer(3,2);%解出theta6 的方程式,if theta6 is right, theta45 then will rightAA=double(subs(str_solve,[cos(theta6),sin(theta6)],[1,0]));BB=double(subs(str_solve,[cos(theta6),sin(theta6)],[0,1]));theta66=atan(-AA/BB);%% solve theta4 and theta5kimejer2=simplify(subs(kimejer,theta6,theta66));%% solve theta4 theta441=double(atan(kimejer2(2,1)/kimejer2(1,1)));if (theta441*radian)<q4_end&&(theta441*radian)>q4_stheta44=theta441;end%% solve theta5theta551=double(atan2(-kimejer2(3,1),kimejer2(3,3))-pi/2);if (theta551*radian)<q5_end&&(theta551*radian)>q5_stheta55=theta551;end%% checktheta_ikine=[theta11,theta22,theta33,theta44,theta55,theta66]*radian T_check=six_link.fkine([theta11 theta22 theta33 theta44 theta55 theta66])figure (2)hold on six_link.plot([theta11,theta22,theta33,theta44,theta55,theta66], plotopt{:});hold off end

?

參考：

宋金華. 六軸工業(yè)機(jī)器人的軌跡規(guī)劃與控制系統(tǒng)研究[D].哈爾濱工業(yè)大學(xué),2013.

邢婷婷. 上下料機(jī)械手的運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)分析與仿真[D].青島科技大學(xué),2012.

李洪波. 冗余七自由度串并聯(lián)擬人手臂的設(shè)計(jì)研究[D].河北工業(yè)大學(xué),2003.

總結(jié)

以上是生活随笔為你收集整理的机械臂——六轴机械臂逆解的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。