6自由度机械手Elbow的正逆运动

抛弃丑陋的D-H方法，采用更符合几何空间的screw方法。

screw方法可以同时计算出位置与姿态，而且可以避免奇异解，计算速度更快。

正运动公式

function data = ForwardKin(obj,theta_list)
l0=obj.l0;
l1 = obj.l1;
l2 = obj.l2;

[m,n] = size(theta_list);

if obj.joint_count ~= m or 1 != n
    error('theta_list size is not good for ForwardKin');
end

th1=theta_list(1);
th2=theta_list(2);
th3=theta_list(3);
th4=theta_list(4);
th5=theta_list(5);
th6=theta_list(6);

data = [cos(th1).*(cos(th4).*cos(th6)+sin(th4).*sin(th5).*sin(th6))+sin( ...
  th1).*(cos(th5).*sin(th2+th3).*sin(th6)+cos(th2+th3).*((-1).*cos( ...
  th6).*sin(th4)+cos(th4).*sin(th5).*sin(th6))),(-1).*cos(th2+th3).* ...
  cos(th4).*cos(th5).*sin(th1)+(-1).*cos(th1).*cos(th5).*sin(th4)+ ...
  sin(th1).*sin(th2+th3).*sin(th5),(-1).*cos(th5).*cos(th6).*sin( ...
  th1).*sin(th2+th3)+cos(th1).*((-1).*cos(th6).*sin(th4).*sin(th5)+ ...
  cos(th4).*sin(th6))+(-1).*cos(th2+th3).*sin(th1).*(cos(th4).*cos( ...
  th6).*sin(th5)+sin(th4).*sin(th6)),(-1).*(l1.*cos(th2)+l2.*cos( ...
  th2+th3)).*sin(th1);cos(th6).*(cos(th4).*sin(th1)+cos(th1).*cos( ...
  th2+th3).*sin(th4))+(sin(th1).*sin(th4).*sin(th5)+(-1).*cos(th1).* ...
  (cos(th5).*sin(th2+th3)+cos(th2+th3).*cos(th4).*sin(th5))).*sin( ...
  th6),(-1).*cos(th5).*sin(th1).*sin(th4)+cos(th1).*(cos(th2+th3).* ...
  cos(th4).*cos(th5)+(-1).*sin(th2+th3).*sin(th5)),sin(th1).*((-1).* ...
  cos(th6).*sin(th4).*sin(th5)+cos(th4).*sin(th6))+cos(th1).*(cos( ...
  th5).*cos(th6).*sin(th2+th3)+cos(th2+th3).*(cos(th4).*cos(th6).* ...
  sin(th5)+sin(th4).*sin(th6))),cos(th1).*(l1.*cos(th2)+l2.*cos(th2+ ...
  th3));(-1).*cos(th6).*sin(th2+th3).*sin(th4)+((-1).*cos(th2+th3).* ...
  cos(th5)+cos(th4).*sin(th2+th3).*sin(th5)).*sin(th6),(-1).*cos( ...
  th4).*cos(th5).*sin(th2+th3)+(-1).*cos(th2+th3).*sin(th5),cos(th2+ ...
  th3).*cos(th5).*cos(th6)+(-1).*sin(th2+th3).*(cos(th4).*cos(th6).* ...
  sin(th5)+sin(th4).*sin(th6)),l0+(-1).*l1.*sin(th2)+(-1).*l2.*sin( ...
  th2+th3);0,0,0,1];

end

逆运动接口：

classdef ElbowManipulator < IManipulator
    %% Elbow manipulator机械手
    
    properties
       l0 = 1;
       l1 = 1;
       l2 = 1;
       gst0 = [1,0,0,1;0,1,0,1;0,0,1,1;0,0,0,1];
       gst_target = [];
 
       joint_count = 6;
    end
    
    methods
        function obj =  ElbowManipulator(name,l0,l1,l2,gst0)
           obj = obj@IManipulator(name); 
           obj.l0 = l0;
           obj.l1 = l1;
           obj.l2 = l2;
           obj.gst0 = gst0;
           
           if 0>= obj.l0 || 0>=obj.l1 || 0>= obj.l2
              error('ElbowManipulator link length sould greater than zero'); 
           end
        end
    end
    
    
     methods
        %% 抽象接口

        %%正运动
        %
        %theta_list:关节角度，需要是列向量,6x1
        data = ForwardKin(obj,theta_list);
        %%逆运动
        %
        %parameters:
        %data:姿态数据
        %
        %return:
        % theta_list:关节角度，这个可能有多个解,cell类型
        % succeed:true =成功找到逆解，false=未找到逆解
        [theta_list,succeed] = InverseKin(obj,data);


        %% 获取初始位置
        function gst0 = GetGst0(obj)
            gst0 = obj.gst0;
        end
    end

    methods
      [theta_list,succeed] = StepOneCalc(obj,tehta3);
      [theta2,theta3,sol,succeed] = StepTwoCalc(obj,tehta3);
      [theta5,theta4,sol,succeed] = StepThreeCalc(obj,theta_list);
      [theta6,sol,succeed] = StepFourCalc(obj,theta_list);
    end
    
end

正逆运动互相验证：

l0 = 1;
l1 = 1;
l2 = 1;

gst0=[1, 0, 0, 0;
    0, 1, 0, l1 + l2;
    0, 0, 1, l0;
    0, 0, 0, 1];
 
 


elbow_manipulator = ElbowManipulator('Elbow',l0,l1,l2,gst0);

forward_theta = [0.671;0.672;0.673;0.674;0.675;0.676];

gst = elbow_manipulator.ForwardKin(forward_theta);

[theta_list,succeed] = elbow_manipulator.InverseKin(gst);

if false == succeed
	fprintf('no soluation');
	return;
end
theta_list

输出是：

   -2.4706    2.4696    5.6102   -1.7428   -0.2491   -0.5704
   -2.4706    2.4696    5.6102    1.3988   -2.8925    2.5712
    0.6710    1.3450    5.6102    0.3703    0.6383    0.1689
    0.6710    1.3450    5.6102   -2.7713    2.5033   -2.9727
   -2.4706    1.7966    0.6730   -1.3932   -1.0529   -0.3155
   -2.4706    1.7966    0.6730    1.7484   -2.0887    2.8261
    0.6710    0.6720    0.6730    0.6740    0.6750    0.6760
    0.6710    0.6720    0.6730   -2.4676    2.4666   -2.4656

可见其中一组正是我们正运动的预设角度。

每一个位置，都可能有多组解，需要业务成进行筛选。

欢迎沟通讨论。