01 正运动学：DH表示法

1955年, Denavit和Hartenberg在“ASME Journal of Applied Mechanic”发表了一篇论文,这篇论文介绍了一种机器人表示和建模的方法,并导出了它们的运动方程,目前已成为机器人表示表示和机器人运动建模的标准方法

Denavit-Hartenberg（D-H）模型表示了对机器人连杆和关节进行建模的一种非常简单的方法,可用于任何机器人构型,而不管机器人的结构顺序和复杂程度。

Link()

参数可从DH参数表获得

sigma 为0，表示转动关节

offset 表示初始角度

modified 表示采用改进D-H法

qlim() 设定关节运动范围

SerialLink()

连接机械臂各杆件，并且返回机械臂名字，供后续使用

plot()

显示出机械臂位置。姿态

teach()

示教

tool()指定工具端长度

base()指定基座位置

说明：

位姿矩阵保存为se3格式，与（4×4）矩阵的替换方法为：

（若A1为前者，A2为后者）

A2=A1.T;

A1=SE3(A2);工具端和基座都可以通过 ``transl```确定，

工具端是 右乘 ， 基座是 左乘L(2).A(pi/3)

获得pi/3时，连杆2的转换矩阵

1.1 建立机械臂模型

% mdl_5dof.m% 5自由度机械臂-单臂结构参数d=[ 0,0, 0, 0, 0];a=[ 0, 13, 233.24, 175.64, 0];alpha=[ 0, pi/2, 0, 0, pi/2];%使用offsetL(1)=Link('d',d(1),'a',a(1),'alpha',alpha(1),'modified'); L(2)=Link('d',d(2),'a',a(2),'alpha',alpha(2),'offset',pi/2,'modified');L(3)=Link('d',d(3),'a',a(3),'alpha',alpha(3),'modified');L(4)=Link('d',d(4),'a',a(4),'alpha',alpha(4),'offset',pi/2,'modified');L(5)=Link('d',d(5),'a',a(5),'alpha',alpha(5),'modified');du=pi/180;ra=180/pi;%定义关节范围L(1).qlim =[-170, 170]*du;L(2).qlim =[60-70, 60+70]*du;%-10,130L(3).qlim =[-70-70,-70+70]*du;%-140,0L(4).qlim =[-70,70]*du;L(5).qlim =[-170, 170]*du;bot=SerialLink(L,'name','五自由度机械臂');%bot.tool= transl(0, 0, tool)% bot.teach()% bot.plot([0 0 0 0 0])

1.2 计算位姿矩阵

化简

% 总转换矩阵计算% v10clc;clear;syms th1 th2 th3 th4 th5;syms a1 a2 a3 a4 a5;syms d1 d2 d3 d4 d5;syms alp1 alp2 alp3 alp4 alp5;d=[ 0,0, 0, 0, 0];a=[ 0, 13, 233.24, 175.64, 0];alpha=[ 0, pi/2, 0, 0, pi/2];% theta d a alpha% L(1) = Link([0 d1 a1 alp1 th1],'modified');% L(2) = Link([0 d2 a2 alp2 th2],'modified');% L(3) = Link([0 d3 a3 alp3 th3],'modified');% L(4) = Link([0 d4 a4 alp4 th4],'modified');% L(5) = Link([0 d5 a5 alp5 th5],'modified');[d1,d2,d3,d4,d5]=deal(0);[alp1,alp2,alp3,alp4,alp5]=deal(alpha(1),alpha(2),alpha(3),alpha(4),alpha(5));a1=0;L(1) = Link([th1 d1 a1 alp1 0],'modified');L(2) = Link([th2 d2 a2 alp2 0],'modified');L(3) = Link([th3 d3 a3 alp3 0],'modified');L(4) = Link([th4 d4 a4 alp4 0],'modified');L(5) = Link([th5 d5 a5 alp5 0],'modified');bot=SerialLink(L,'name','my robot');%计算总转换矩阵T=L(1).A(th1)*L(2).A(th2)*L(3).A(th3)*L(4).A(th4)*L(5).A(th5)%简化simplify(T)

02 逆运动学

逆运动学可以分为迭代法和解析法（封闭解）

求封闭解有两个充分条件：

三个相邻关节轴相交与一点三个相邻关节轴平行

MATLAB机器人工具箱提供了两种逆运动学函数：

ikine6s()

6轴机械臂的封闭解法ikine()

机械臂的迭代解法

默认是六轴

可以用于非六轴的情况

eg:

对于这个五轴机械臂

q2 = bot.ikine(T2,'mask',[1 1 1 1 0 1],'q0',q1);

其中

mask的“1”标记有那些关节

q0表示迭代的初始位置是那里（影响迭代速度和正确性）

迭代法往往计算速度比较慢，所以需要计算五轴机械臂封闭解

封闭解的计算方法，关键就是比较矩阵中每个位置，具体可以参考一个硕士论文《协作机器人零力控制与碰撞检测技术研究》

2.1 五轴机械臂封闭解

对比矩阵1、2

按照

theta1 → theta5→ theta2→ (theta2+theta3)→ theta3→ (theta2+theta3+theta4) → theta4

的顺序计算

theta1 和 theta5 的计算方法如下图

注意求解过程一定要使用atan2()

不要用arctan()，arcsin()之类的

以此结果得到逆运动学函数（共四组解）：

% 逆运动学求解函数% 输入 机器人名称，转换矩阵（SE3）% 输出关节角度% 说明：函数执行过程中打印4组解（deg），目前函数返回第三组解（rad）function [ th ] = MDH_IK(robot,T)TT=SE3(T);%矩阵转换为SE3格式T = inv(robot.base) * TT * inv(robot.tool);%对于多个连续的问题，可设置k% T = inv(robot.base) * TT(k) * inv(robot.tool);%将函数变量导入% T是SE3类型函数，所以不能用下列写法(也可以转换为T=T.T）% [nx ny nz]=deal(T(1,1),T(2,1),T(3,1));% [ox oy oz]=deal(T(1,2),T(2,2),T(3,2));% [ax ay az]=deal(T(1,3),T(2,3),T(3,3));% [px py pz]=deal(T(1,4),T(2,4),T(3,4));n=T.n;o=T.o;a=T.a;p=T.t;[nx ny nz]=deal(n(1),n(2),n(3));[ox oy oz]=deal(o(1),o(2),o(3));[ax ay az]=deal(a(1),a(2),a(3));[px py pz]=deal(p(1),p(2),p(3));% 结构参数syms theta1 theta2 theta3 theta4 theta5 ;syms a1 a2 a3 d5 ;%alpha,a,d,thetaDH_Tab =[ 0 0 0 0; 9013 0 0;0 233.24 0 0; 0 175.64 0 0; 90 0 0 0];%参数赋值a0=robot.a(1);a1=robot.a(2);a2=robot.a(3);a3=robot.a(4);a4=robot.a(5);%角度转弧度% DH_Tab(1:5,1)=DH_Tab(1:5,1)/180*pi;% alp0=DH_Tab(1,1); alp1=DH_Tab(2,1);alp2=DH_Tab(3,1);alp3=DH_Tab(4,1);alp4=DH_Tab(5,1);%% 求解过程1theta1_1=atan2(-py,-px);theta1_2=atan2(py,px);theta1=theta1_1;theta5=atan2(nx*sin(theta1)-ny*cos(theta1),ox*sin(theta1)-oy*cos(theta1));theta234=atan2(nz,nx*cos(theta1)+ny*sin(theta1));A=px*cos(theta1)+py*sin(theta1)-a1;B=pz;theta2_1=atan2(A^2+B^2+a2^2-a3^2,sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);theta2_2=atan2(A^2+B^2+a2^2-a3^2,-sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);theta2=theta2_1;theta23=atan2(pz-a2*sin(theta2),px*cos(theta1)+py*sin(theta1)-a1-a2*cos(theta2));theta3=theta23-theta2;theta4=theta234-theta23;%theta4不一样只是一种表示而已，差别360°，就是转一圈，位置是一样的if(theta4>pi)theta4=theta4-2*pi;endif(theta4<-pi)theta4=theta4+2*pi;endth1=[theta1,theta2,theta3,theta4,theta5];for j=1:robot.n th1(j) = th1(j) - robot.offset(j);end %输出th1=th1*180/pi%% 求解过程2theta1_1=atan2(-py,-px);theta1_2=atan2(py,px);theta1=theta1_2;theta5=atan2(nx*sin(theta1)-ny*cos(theta1),ox*sin(theta1)-oy*cos(theta1));theta234=atan2(nz,nx*cos(theta1)+ny*sin(theta1));A=px*cos(theta1)+py*sin(theta1)-a1;B=pz;theta2_1=atan2(A^2+B^2+a2^2-a3^2,sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);theta2_2=atan2(A^2+B^2+a2^2-a3^2,-sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);theta2=theta2_1;theta23=atan2(pz-a2*sin(theta2),px*cos(theta1)+py*sin(theta1)-a1-a2*cos(theta2));theta3=theta23-theta2;theta4=theta234-theta23;%theta4不一样只是一种表示而已，差别360°，就是转一圈，位置是一样的if(theta4>pi)theta4=theta4-2*pi;endif(theta4<-pi)theta4=theta4+2*pi;endth2=[theta1,theta2,theta3,theta4,theta5];for j=1:robot.n th2(j) = th2(j) - robot.offset(j);end %输出th2=th2*180/pi%% 求解过程3theta1_1=atan2(-py,-px);theta1_2=atan2(py,px);theta1=theta1_1;theta5=atan2(nx*sin(theta1)-ny*cos(theta1),ox*sin(theta1)-oy*cos(theta1));theta234=atan2(nz,nx*cos(theta1)+ny*sin(theta1));A=px*cos(theta1)+py*sin(theta1)-a1;B=pz;theta2_1=atan2(A^2+B^2+a2^2-a3^2,sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);theta2_2=atan2(A^2+B^2+a2^2-a3^2,-sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);theta2=theta2_2;theta23=atan2(pz-a2*sin(theta2),px*cos(theta1)+py*sin(theta1)-a1-a2*cos(theta2));theta3=theta23-theta2;theta4=theta234-theta23;%theta4不一样只是一种表示而已，差别360°，就是转一圈，位置是一样的if(theta4>pi)theta4=theta4-2*pi;endif(theta4<-pi)theta4=theta4+2*pi;endth3=[theta1,theta2,theta3,theta4,theta5];for j=1:robot.n th3(j) = th3(j) - robot.offset(j);end %输出th3=th3*180/pi%% 求解过程4theta1_1=atan2(-py,-px);theta1_2=atan2(py,px);theta1=theta1_2;theta5=atan2(nx*sin(theta1)-ny*cos(theta1),ox*sin(theta1)-oy*cos(theta1));theta234=atan2(nz,nx*cos(theta1)+ny*sin(theta1));A=px*cos(theta1)+py*sin(theta1)-a1;B=pz;theta2_1=atan2(A^2+B^2+a2^2-a3^2,sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);theta2_2=atan2(A^2+B^2+a2^2-a3^2,-sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);theta2=theta2_2;theta23=atan2(pz-a2*sin(theta2),px*cos(theta1)+py*sin(theta1)-a1-a2*cos(theta2));theta3=theta23-theta2;theta4=theta234-theta23;%theta4不一样只是一种表示而已，差别360°，就是转一圈，位置是一样的if(theta4>pi)theta4=theta4-2*pi;endif(theta4<-pi)theta4=theta4+2*pi;endth4=[theta1,theta2,theta3,theta4,theta5];for j=1:robot.n th4(j) = th4(j) - robot.offset(j);end %输出th4=th4*180/pi%% 函数输出，转换为rad形式th=th3*pi/180;end

2.2 ikine6s源码

%SerialLink.ikine6s Analytical inverse kinematics%% Q = R.ikine(T) are the joint coordinates (1xN) corresponding to the robot% end-effector pose T which is an SE3 object or homogenenous transform% matrix (4x4), and N is the number of robot joints. This is a analytic% solution for a 6-axis robot with a spherical wrist (the most common form% for industrial robot arms).%% If T represents a trajectory (4x4xM) then the inverse kinematics is% computed for all M poses resulting in Q (MxN) with each row representing% the joint angles at the corresponding pose.%% Q = R.IKINE6S(T, CONFIG) as above but specifies the configuration of the arm in% the form of a string containing one or more of the configuration codes:%% 'l' arm to the left (default)% 'r' arm to the right% 'u' elbow up (default)% 'd' elbow down% 'n' wrist not flipped (default)% 'f' wrist flipped (rotated by 180 deg)%% Trajectory operation::%% In all cases if T is a vector of SE3 objects (1xM) or a homogeneous% transform sequence (4x4xM) then R.ikcon() returns the joint coordinates% corresponding to each of the transforms in the sequence.%% Notes::% - Treats a number of specific cases:% - Robot with no shoulder offset% - Robot with a shoulder offset (has lefty/righty configuration)% - Robot with a shoulder offset and a prismatic third joint (like Stanford arm)% - The Puma 560 arms with shoulder and elbow offsets (4 lengths parameters)% - The Kuka KR5 with many offsets (7 length parameters)% - The inverse kinematics for the various cases determined using ikine_sym.% - The inverse kinematic solution is generally not unique, and% depends on the configuration string.% - Joint offsets, if defined, are added to the inverse kinematics to% generate Q.% - Only applicable for standard Denavit-Hartenberg parameters%% Reference::% - Inverse kinematics for a PUMA 560,% Paul and Zhang,% The International Journal of Robotics Research,% Vol. 5, No. 2, Summer 1986, p. 32-44%% Author::% - The Puma560 case: Robert Biro with Gary Von McMurray,% GTRI/ATRP/IIMB, Georgia Institute of Technology, 2/13/95% - Kuka KR5 case: Gautam Sinha,% Autobirdz Systems Pvt. Ltd., SIDBI Office,% Indian Institute of Technology Kanpur, Kanpur, Uttar Pradesh.%% See also SerialLink.fkine, SerialLink.ikine, SerialLink.ikine_sym.function thetavec = ikine6s(robot, TT, varargin)if robot.mdh ~= 0error('RTB:ikine:notsupported','Solution only applicable for standard DH conventions');endif robot.n ~= 6error('RTB:ikine:notsupported','Solution only applicable for 6-axis robot');end%%% recurse over all poses in a trajectory%if ndims(T) == 3% theta = zeros(size(T,3),robot.n);% for k=1:size(T,3)% theta(k,:) = ikine6s(robot, T(:,:,k), varargin{:});% end% return;%end%%%if ~ishomog(T)% error('RTB:ikine:badarg', 'T is not a homog xform');%endTT = SE3(TT);L = robot.links;if ~robot.isspherical()error('RTB:ikine:notsupported', 'wrist is not spherical');end% The configuration parameter determines what n1,n2,n4 values are used% and how many solutions are determined which have values of -1 or +1.if nargin < 3configuration = '';elseconfiguration = lower(varargin{1});end% default configurationsol = [1 1 1]; % left, up, noflipfor c=configurationswitch ccase 'l'sol(1) = 1;case 'r'sol(1) = 2;case 'u'sol(2) = 1;case 'd'sol(2) = 2;case 'n'sol(3) = 1;case 'f'sol(3) = 2;endend% determine the arm structure and the relevant solution to useif isempty(robot.ikineType)if is_simple(L)robot.ikineType = 'nooffset';elseif is_puma(L)robot.ikineType = 'puma';elseif is_offset(L)robot.ikineType = 'offset';elseif is_rrp(L)robot.ikineType = 'rrp';elseerror('RTB:ikine6s:badarg', 'This kinematic structure not supported');endendfor k=1:length(TT)% undo base and tool transformationsT = inv(robot.base) * TT(k) * inv(robot.tool);% drop back to matrix formT = T.T;%% now solve for the first 3 joints, based on position of the spherical wrist centreswitch robot.ikineTypecase 'puma'% Puma model with shoulder and elbow offsets%% - Inverse kinematics for a PUMA 560,% Paul and Zhang,% The International Journal of Robotics Research,% Vol. 5, No. 2, Summer 1986, p. 32-44%% Author::% Robert Biro with Gary Von McMurray,% GTRI/ATRP/IIMB,% Georgia Institute of Technology% 2/13/95a2 = L(2).a;a3 = L(3).a;d1 = L(1).d;d3 = L(3).d;d4 = L(4).d;% The following parameters are extracted from the Homogeneous% Transformation as defined in equation 1, p. 34Ox = 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) - d1;%% Solve for theta(1)%% r is defined in equation 38, p. 39.% theta(1) uses equations 40 and 41, p.39,% based on the configuration parameter n1%r = sqrt(Px^2 + Py^2);if sol(1) == 1theta(1) = atan2(Py,Px) + pi - asin(d3/r);elsetheta(1) = atan2(Py,Px) + asin(d3/r);end%% Solve for theta(2)%% V114 is defined in equation 43, p.39.% r is defined in equation 47, p.39.% Psi is defined in equation 49, p.40.% theta(2) uses equations 50 and 51, p.40, based on the configuration% parameter n2%if sol(2) == 1n2 = -1;elsen2 = 1;endif sol(1) == 2n2 = -n2;endV114 = Px*cos(theta(1)) + Py*sin(theta(1));r = sqrt(V114^2 + Pz^2);Psi = acos((a2^2-d4^2-a3^2+V114^2+Pz^2)/(2.0*a2*r));if ~isreal(Psi)theta = [];elsetheta(2) = atan2(Pz,V114) + n2*Psi;%% Solve for theta(3)%% theta(3) uses equation 57, p. 40.%num = cos(theta(2))*V114+sin(theta(2))*Pz-a2;den = cos(theta(2))*Pz - sin(theta(2))*V114;theta(3) = atan2(a3,d4) - atan2(num, den);endcase 'nooffset'a2 = L(2).a;a3 = L(3).a;d1 = L(1).d;px = T(1,4); py = T(2,4); pz = T(3,4);%%% autogenerated codeif L(1).alpha < 0if sol(1) == 1q(1) = angle(-px-py*1i);elseq(1) = angle(px+py*1i);endS1 = sin(q(1));C1 = cos(q(1));if sol(2) == 1q(2) = -angle(a2*d1*-2.0+a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i-sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);elseq(2) = -angle(a2*d1*-2.0+a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i+sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);endS2 = sin(q(2));C2 = cos(q(2));if sol(3) == 1q(3) = -angle(a2*-1i+C2*d1-C2*pz+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i-sqrt((-a2+S2*d1-S2*pz+C1*C2*px+C2*S1*py)^2+(-C2*d1+C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));elseq(3) = -angle(a2*-1i+C2*d1-C2*pz+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt((-a2+S2*d1-S2*pz+C1*C2*px+C2*S1*py)^2+(-C2*d1+C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));endelseif sol(1) == 1q(1) = angle(px+py*1i);elseq(1) = angle(-px-py*1i);endS1 = sin(q(1));C1 = cos(q(1));if sol(2) == 1q(2) = -angle(a2*d1*2.0-a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i-sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);elseq(2) = -angle(a2*d1*2.0-a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i+sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);endS2 = sin(q(2));C2 = cos(q(2));if sol(3) == 1q(3) = -angle(a2*-1i-C2*d1+C2*pz-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i-sqrt((-a2-S2*d1+S2*pz+C1*C2*px+C2*S1*py)^2+(C2*d1-C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));elseq(3) = -angle(a2*-1i-C2*d1+C2*pz-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt((-a2-S2*d1+S2*pz+C1*C2*px+C2*S1*py)^2+(C2*d1-C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));endendtheta(1:3) = q;case'offset'% general case with 6 length parametersa1 = L(1).a;a2 = L(2).a;a3 = L(3).a;d1 = L(1).d;d2 = L(2).d;d3 = L(3).d;px = T(1,4); py = T(2,4); pz = T(3,4);%%% autogenerated codeif L(1).alpha < 0if sol(1) == 1q(1) = -angle(-px+py*1i)+angle(d2*1i+d3*1i-sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2));elseq(1) = angle(d2*1i+d3*1i+sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2))-angle(-px+py*1i);endS1 = sin(q(1));C1 = cos(q(1));if sol(2) == 1q(2) = angle(d1*pz*2.0i-sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i+d1-pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));elseq(2) = angle(d1*pz*2.0i+sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i+d1-pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));endS2 = sin(q(2));C2 = cos(q(2));if sol(3) == 1q(3) = angle(a3*1i-sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0-S2*a2*d1*2.0-S1*a1*py*2.0+S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0))-angle(a2*-1i-C2*a1*1i+C2*d1-C2*pz+S2*a1+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py);elseq(3) = -angle(a2*-1i-C2*a1*1i+C2*d1-C2*pz+S2*a1+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0-S2*a2*d1*2.0-S1*a1*py*2.0+S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0));endelseif sol(1) == 1q(1) = -angle(px-py*1i)+angle(d2*1i+d3*1i-sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2));elseq(1) = -angle(px-py*1i)+angle(d2*1i+d3*1i+sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2));endS1 = sin(q(1));C1 = cos(q(1));if sol(2) == 1q(2) = angle(d1*pz*2.0i-sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i-d1+pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));elseq(2) = angle(d1*pz*2.0i+sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i-d1+pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));endS2 = sin(q(2));C2 = cos(q(2));if sol(3) == 1q(3) = angle(a3*1i-sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0+S2*a2*d1*2.0-S1*a1*py*2.0-S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0))-angle(a2*-1i-C2*a1*1i-C2*d1+C2*pz+S2*a1-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py);elseq(3) = -angle(a2*-1i-C2*a1*1i-C2*d1+C2*pz+S2*a1-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0+S2*a2*d1*2.0-S1*a1*py*2.0-S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0));endendtheta(1:3) = q;case 'rrp'% RRP (Stanford arm like)px = T(1,4); py = T(2,4); pz = T(3,4);d1 = L(1).d;d2 = L(2).d;%%% autogenerated codeif L(1).alpha < 0if sol(1) == 1q(1) = -angle(-px+py*1i)+angle(d2*1i-sqrt(-d2^2+px^2+py^2));elseq(1) = angle(d2*1i+sqrt(-d2^2+px^2+py^2))-angle(-px+py*1i);endS1 = sin(q(1));C1 = cos(q(1));if sol(2) == 1q(2) = angle(d1-pz-C1*px*1i-S1*py*1i);elseq(2) = angle(-d1+pz+C1*px*1i+S1*py*1i);endS2 = sin(q(2));C2 = cos(q(2));q(3) = -C2*d1+C2*pz+C1*S2*px+S1*S2*py;elseif sol(1) == 1q(1) = -angle(px-py*1i)+angle(d2*1i-sqrt(-d2^2+px^2+py^2));elseq(1) = -angle(px-py*1i)+angle(d2*1i+sqrt(-d2^2+px^2+py^2));endS1 = sin(q(1));C1 = cos(q(1));if sol(2) == 1q(2) = angle(-d1+pz-C1*px*1i-S1*py*1i);elseq(2) = angle(d1-pz+C1*px*1i+S1*py*1i);endS2 = sin(q(2));C2 = cos(q(2));q(3) = -C2*d1+C2*pz-C1*S2*px-S1*S2*py;endtheta(1:3) = q;case 'kr5'%Given function will calculate inverse kinematics for KUKA KR5 robot% Equations are calculated and implemented by% Gautam Sinha% Autobirdz Systems Pvt. Ltd.% SIDBI Office,% Indian Institute of Technology Kanpur, Kanpur, Uttar Pradesh% 208016% India%email- gautam.sinha705@% get the a1, a2 and a3-- link lenghts for link no 1,2,3L = robot.links;a1 = L(1).a;a2 = L(2).a;a3 = L(3).a;% Check wether wrist is spherical or notif ~robot.isspherical()error('wrist is not spherical')end% get d1,d2,d3,d4---- Link offsets for link no 1,2,3,4d1 = L(1).d;d2 = L(2).d;d3 = L(3).d;d4 = L(4).d;% Get the parameters from transformation matrixOx = 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);% Set the parameters n1, n2 and n3 to get required configuration from% solutionn1 = -1; % 'l'n2 = -1; % 'u'n4 = -1; % 'n'if ~isempty(strfind(configuration, 'l'))n1 = -1;endif ~isempty(strfind(configuration, 'r'))n1 = 1;endif ~isempty(strfind(configuration, 'u'))if n1 == 1n2 = 1;elsen2 = -1;endendif ~isempty(strfind(configuration, 'd'))if n1 == 1n2 = -1;elsen2 = 1;endendif ~isempty(strfind(configuration, 'n'))n4 = 1;endif ~isempty(strfind(configuration, 'f'))n4 = -1;end% Calculation for theta(1)r=sqrt(Px^2+Py^2);if (n1 == 1)theta(1)= atan2(Py,Px) + asin((d2-d3)/r);elsetheta(1)= atan2(Py,Px)+ pi - asin((d2-d3)/r);end% Calculation for theta(2)X= Px*cos(theta(1)) + Py*sin(theta(1)) - a1;r=sqrt(X^2 + (Pz-d1)^2);Psi = acos((a2^2-d4^2-a3^2+X^2+(Pz-d1)^2)/(2.0*a2*r));if ~isreal(Psi)warning('RTB:ikine6s:notreachable', 'point not reachable');theta = [NaN NaN NaN NaN NaN NaN];returnendtheta(2) = atan2((Pz-d1),X) + n2*Psi;% Calculation for theta(3)Nu = cos(theta(2))*X + sin(theta(2))*(Pz-d1) - a2;Du = sin(theta(2))*X - cos(theta(2))*(Pz-d1);theta(3) = atan2(a3,d4) - atan2(Nu, Du);% Calculation for theta(4)Y = cos(theta(1))*Ax + sin(theta(1))*Ay;M2 = sin(theta(1))*Ax - cos(theta(1))*Ay ;M1 = ( cos(theta(2)-theta(3)) )*Y + ( sin(theta(2)-theta(3)) )*Az;theta(4) = atan2(n4*M2,n4*M1);% Calculation for theta(5)Nu = -cos(theta(4))*M1 - M2*sin(theta(4));M3 = -Az*( cos(theta(2)-theta(3)) ) + Y*( sin(theta(2)-theta(3)) );theta(5) = atan2(Nu,M3);% Calculation for theta(6)Z = cos(theta(1))*Ox + sin(theta(1))*Oy;L2 = sin(theta(1))*Ox - cos(theta(1))*Oy;L1 = Z*( cos(theta(2)-theta(3) )) + Oz*( sin(theta(2)-theta(3)));L3 = Z*( sin(theta(2)-theta(3) )) - Oz*( cos(theta(2)-theta(3)));A1 = L1*cos(theta(4)) + L2*sin(theta(4));A3 = L1*sin(theta(4)) - L2*cos(theta(4));Nu = -A1*cos(theta(5)) - L3*sin(theta(5));Du = -A3;theta(6) = atan2(Nu,Du);otherwiseerror('RTB:ikine6s:badarg', 'Unknown solution type [%s]', robot.ikineType);endif ~isempty(theta)% Solve for the wrist rotation% we need to account for some random translations between the first and last 3% joints (d4) and also d6,a6,alpha6 in the final frame.T13 = robot.A(1:3, theta(1:3)); % transform of first 3 joints% T = T13 * Tz(d4) * R * Tz(d6) Tx(a5)Td4 = SE3(0, 0, L(4).d);% Tz(d4)Tt = SE3(L(6).a, 0, L(6).d) * SE3.Rx(L(6).alpha); % Tz(d6) Tx(a5) Rx(alpha6)R = inv(Td4) * inv(T13) * SE3(T) * inv(Tt);% the spherical wrist implements Euler anglesif sol(3) == 1theta(4:6) = tr2eul(R, 'flip');elsetheta(4:6) = tr2eul(R);endif L(4).alpha > 0theta(5) = -theta(5);end% remove the link offset anglesfor j=1:robot.n %#ok<*AGROW>theta(j) = theta(j) - L(j).offset;end% stack the rowsthetavec(k,:) = theta;elsewarning('RTB:ikine6s:notreachable', 'point not reachable');thetavec(k,:) = [NaN NaN NaN NaN NaN NaN];endendend% predicates to determine which kinematic solution to usefunction s = is_simple(L)alpha = [-pi/2 0 pi/2];s =all([L(2:3).d] == 0) && ...(all([L(1:3).alpha] == alpha) || all([L(1:3).alpha] == -alpha)) && ...all([L(1:3).isrevolute] == 1) && ...(L(1).a == 0);endfunction s = is_offset(L)alpha = [-pi/2 0 pi/2];s =(all([L(1:3).alpha] == alpha) || all([L(1:3).alpha] == -alpha)) && ...all([L(1:3).isrevolute] == 1);endfunction s = is_rrp(L)alpha = [-pi/2 pi/2 0];s =all([L(2:3).a] == 0) && ...(all([L(1:3).alpha] == alpha) || all([L(1:3).alpha] == -alpha)) && ...all([L(1:3).isrevolute] == [1 1 0]);endfunction s = is_puma(L)alpha = [pi/2 0 -pi/2];s =(L(2).d == 0) && (L(1).a == 0) && ...(L(3).d ~= 0) && (L(3).a ~= 0) && ...all([L(1:3).alpha] == alpha) && ...all([L(1:3).isrevolute] == 1);end

2.3 其他