SCARA机械臂逆运动推导

正运动学公式，后面要用到：

该公式产生的实际计算公式是：

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

对于SCARA机械臂来说，其最终的姿态是：

我们对比正运动学的公式和最终姿态，会得到一组等式：

于是立刻可以得到逆解的theta4 = z - l0

设g1为：

然后找轴3上的点p,以及轴1上的点q,则有：

g1作用于p再减去q，然后根据模长不变性质可得：

 上式是K-P子问题3，可以求解theta2

找一个轴3上的点p'，g1作用于p',有

因为theta2已知，上式可以运动K-P子问题1求解theta1 

改变一下g1的形式：

然后找一个非轴3上的点p,联合上式，立刻可以解得theta3 

classdef ScaraManipulator <  IManipulator
	%Scara机械手
    
    properties
        l0;
        l1;
        l2;

        w1;
        w2;
        w3;
        w4;

        % 第4个关节是移动关节，不需要q
        q1;
        q2;
        q3;

        gst0;
        gst_target;
    end

	 methods
        function obj =  ScaraManipulator(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('ScaraManipulator 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
        [theta4,succeed] = CalcTheta4(obj,gst);
        [theta2,sol,succeed] = CalcTheta2(obj,theta4,gst_target);
        [theta1,sol,succeed] = CalcTheta1(obj,theta_list,gst_target);
        [theta3,sol,succeed] = CalcTheta3(obj,theta_list,gst_target);
    end


end

测试代码：

addpath('./');

l0 = 0.3;
l1 = 0.15;
l2 = 0.05;

gst0=[1, 0, 0, 0;
    0, 1, 0, l1 + l2;
    0, 0, 1, l0;
    0, 0, 0, 1];
 
scara_manipulator = ScaraManipulator('Scara',l0,l1,l2,gst0);

%forward_theta = [0.671;0.672;0.673;0.674];
forward_theta = [0;0;0;0];

forward_theta = [0.5876;0.3234;0.75673;0.21234];

gst = scara_manipulator.ForwardKin(forward_theta);

gst

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

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

输出：

theta_list =

    0.7482    5.9598    1.2429    0.2123
    0.5876    0.3234    0.7567    0.2123

验证通过