代碼邏輯參考https://blog.csdn.net/fengyu19930920/article/details/81144042#comments

背景介紹：

本人想用 MATLAB+V-REP 仿真機器人，對機器人的正逆運動學簡單寫了下代碼實現部分。

首先是正運動學求解代碼（所用DH參數表為UR5）

function T = zhengyundongxue(theta)%已知關節角求變換矩陣a=[0,-0.425,-0.39225,0,0,0];d=[0.89159,0,0,0.10915,0.09465,0.08230];alpha=[pi/2,0,0,pi/2,-pi/2,0];T01=T_para(theta(1),d(1),a(1),alpha(1));T12=T_para(theta(2),d(2),a(2),alpha(2));T23=T_para(theta(3),d(3),a(3),alpha(3));T34=T_para(theta(4),d(4),a(4),alpha(4));T45=T_para(theta(5),d(5),a(5),alpha(5));T56=T_para(theta(6),d(6),a(6),alpha(6));T=T01*T12*T23*T34*T45*T56; end function T = T_para(theta,d,a,alpha)T=[ccc(theta),-sss(theta)*ccc(alpha),sss(theta)*sss(alpha),a*ccc(theta);sss(theta),ccc(theta)*ccc(alpha),-ccc(theta)*sss(alpha),a*sss(theta);0,sss(alpha),ccc(alpha),d;0,0,0,1]; end

好了，問題來了，這個ccc和sss函數就是cos和sin，我承認我SB了。。。

function ccc=ccc(a) % ccc=cos(a/180*pi);ccc=cos(a); end function sss=sss(a) % sss=sin(a/180*pi);sss=sin(a); end

至此，正運動學代碼部分貼完了，經驗證，和機器人工具箱所算結果一樣。

其次，是逆運動學代碼部分

function theta=niyundongxue(T)%變換矩陣T已知%SDH:標準DH參數表求逆解（解析解）%部分DH參數表如下，需要求解theta信息a=[0,-0.425,-0.39225,0,0,0];d=[0.89159,0,0,0.10915,0.09465,0.08230];alpha=[pi/2,0,0,pi/2,-pi/2,0];nx=T(1,1);ny=T(2,1);nz=T(3,1);ox=T(1,2);oy=T(2,2);oz=T(3,2);ax=T(1,3);ay=T(2,3);az=T(3,3);px=T(1,4);py=T(2,4);pz=T(3,4);%求解關節角1m=d(6)*ay-py; n=ax*d(6)-px; theta1(1,1)=atan2(m,n)-atan2(d(4),sqrt(m^2+n^2-(d(4))^2));theta1(1,2)=atan2(m,n)-atan2(d(4),-sqrt(m^2+n^2-(d(4))^2));%求解關節角5theta5(1,1:2)=acos(ax*sin(theta1)-ay*cos(theta1));theta5(2,1:2)=-acos(ax*sin(theta1)-ay*cos(theta1)); %求解關節角6mm=nx*sin(theta1)-ny*cos(theta1); nn=ox*sin(theta1)-oy*cos(theta1);theta6=atan2(mm,nn)-atan2(sin(theta5),0);%求解關節角3mmm=d(5)*(sin(theta6).*(nx*cos(theta1)+ny*sin(theta1))+cos(theta6).*(ox*cos(theta1)+oy*sin(theta1))) ...-d(6)*(ax*cos(theta1)+ay*sin(theta1))+px*cos(theta1)+py*sin(theta1);nnn=pz-d(1)-az*d(6)+d(5)*(oz*cos(theta6)+nz*sin(theta6));theta3(1:2,:)=acos((mmm.^2+nnn.^2-(a(2))^2-(a(3))^2)/(2*a(2)*a(3)));theta3(3:4,:)=-acos((mmm.^2+nnn.^2-(a(2))^2-(a(3))^2)/(2*a(2)*a(3)));%求解關節角2mmm_s2(1:2,:)=mmm;mmm_s2(3:4,:)=mmm;nnn_s2(1:2,:)=nnn;nnn_s2(3:4,:)=nnn;s2=((a(3)*cos(theta3)+a(2)).*nnn_s2-a(3)*sin(theta3).*mmm_s2)./ ...((a(2))^2+(a(3))^2+2*a(2)*a(3)*cos(theta3));c2=(mmm_s2+a(3)*sin(theta3).*s2)./(a(3)*cos(theta3)+a(2));theta2=atan2(s2,c2); %整理關節角1 5 6 3 2theta(1:4,1)=theta1(1,1);theta(5:8,1)=theta1(1,2);theta(:,2)=[theta2(1,1),theta2(3,1),theta2(2,1),theta2(4,1),theta2(1,2),theta2(3,2),theta2(2,2),theta2(4,2)]';theta(:,3)=[theta3(1,1),theta3(3,1),theta3(2,1),theta3(4,1),theta3(1,2),theta3(3,2),theta3(2,2),theta3(4,2)]';theta(1:2,5)=theta5(1,1);theta(3:4,5)=theta5(2,1);theta(5:6,5)=theta5(1,2);theta(7:8,5)=theta5(2,2);theta(1:2,6)=theta6(1,1);theta(3:4,6)=theta6(2,1);theta(5:6,6)=theta6(1,2);theta(7:8,6)=theta6(2,2); %求解關節角4theta(:,4)=atan2(-sin(theta(:,6)).*(nx*cos(theta(:,1))+ny*sin(theta(:,1)))-cos(theta(:,6)).* ...(ox*cos(theta(:,1))+oy*sin(theta(:,1))),oz*cos(theta(:,6))+nz*sin(theta(:,6)))-theta(:,2)-theta(:,3); end 在這里插入代碼片

Tip：

編寫代碼時，不要參考原博主https://blog.csdn.net/fengyu19930920/article/details/81144042#comments的結論部分，有錯誤，比如第一個m和n的定義與前述不同。

關于奇異位置，未作說明。

代碼有沒有硬核？名字無所謂，瞎起的，代碼的邏輯不知道別人能不能看懂，能用就完事了。

改進DH參數表求逆運動學，請參考https://blog.csdn.net/jldemanman/article/details/80785075，本人未驗證她的代碼是否可用（明天周五了，想了想這周進度，又是深深的難過。。。）

總結

以上是生活随笔為你收集整理的六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。