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

前期準備：完成機械臂數(shù)學模型的建立+計算機械臂工作空間

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

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

?

一、運動學分析

連桿變換是機器人進行運動學分析的基礎，其建立主要涉及到坐標變換，其中包括坐標旋轉和坐標平移。

坐標旋轉變換為繞坐標系的X、Y和Z軸的旋轉變換，一般情況下一個旋轉變換為幾個基本旋轉變換的合成。下面式1為繞X軸旋轉的基本旋轉矩陣，式2為繞Y軸旋轉的基本旋轉矩陣，式3為繞Z軸旋轉的基本旋轉矩陣。

??

?坐標平移變換為坐標系沿坐標軸X、Y和Z軸進行平移轉換，平移轉換后的兩個坐標系的原點不同，但坐標軸相對平行。式3-4為基本平移矩陣，、和為平移后的矩陣相對于原矩陣平移的距離。

?為在進行運動學分析過程中，簡化計算，一般將坐標轉換矩陣進行齊次變換，轉變?yōu)辇R次變換矩陣。

?通常情況下所有的連桿變換都可以通過坐標旋轉和坐標平移獲得，即可通過坐標旋轉變換和坐標平移變換的復合變換獲得連桿變換矩陣。如圖1為兩個坐標系的變換，其變換公式為下式

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? 圖1 旋轉和平移復合的一般坐標變換

根據(jù)前面博文建立的機械臂數(shù)學模型，按照各關節(jié)坐標系的旋轉和平移分別建立對應的齊次連桿變換矩陣：

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

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

?

二、逆解計算

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?圖2 機械臂計算逆解流程圖?

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

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

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

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

?

三、程序實現(xiàn)

%六軸機械臂幾何法反解計算 %流程：輸入末端點坐標，檢查是否在工作空間范圍內(nèi)，計算反解，正解驗證 clc; clear; format short;%保留精度%角度轉換 du=pi/180; %度 radian=180/pi; %弧度%% 模型導入 robot_hand;%% 工作空間計算 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};% 輸入末端點坐標, 末端姿態(tài)默認與初始狀態(tài)一致point_xyz=inputdlg({'X','Y','Z'},'末端點坐標',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%% 賦值A11=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è)機器人的軌跡規(guī)劃與控制系統(tǒng)研究[D].哈爾濱工業(yè)大學,2013.

邢婷婷. 上下料機械手的運動學及動力學分析與仿真[D].青島科技大學,2012.

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

總結

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

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