机器人控制系列教程之Delta机器人运动学分析(2)

基于MATLAB的Delta机器人正向运动学模型求解

我们在上一篇推文中，推导了Delta机器人的正向运动学，简单来说，就是我们可以通过机器人的末端位姿求解出对应的关节空间的角度（位置）。

最终我们分析该机器人的空间位置结构方程最终取得一个解为：

${t_i} = {{ - {N_i} + \sqrt {N_i^2 - 4{M_i}{Q_i}} } \over {2{M_i}}}$

该机器人的驱动角为： ${\theta _i} = 2\arctan ({t_i})$

% 定义Delta机器人的正向运动学求解函数
function theta=sA(x,y,z)

%% Delta机器人结构参数,此处字母表示可参考上图。
Rd=0.120;
rd=0.045;
r=Rd-rd;
La=0.260;
Lb=0.640;

x=-x;
m=x^2+y^2+z^2+r^2+La^2-Lb^2;

% 支链1计算
M1=((m-2*x*r)/(2*La))-(r-x);
N1=2*z;
Q1=((m-2*x*r)/(2*La))+(r-x);
% 支链2计算
M2=((m+(x-sqrt(3)*y)*r)/La)-2*r-(x-sqrt(3)*y);
N2=4*z;
Q2=((m+(x-sqrt(3)*y)*r)/La)+2*r+(x-sqrt(3)*y);
% 支链3计算
M3=((m+(x+sqrt(3)*y)*r)/La)-2*r-(x+sqrt(3)*y);
N3=4*z;
Q3=((m+(x-sqrt(3)*y)*r)/La)+2*r+(x+sqrt(3)*y);

t12=(-N1+sqrt(N1^2-4*M1*Q1))/(2*M1);
t22=(-N2+sqrt(N2^2-4*M2*Q2))/(2*M2);
t32=(-N3+sqrt(N3^2-4*M3*Q3))/(2*M3);

theta1=2*atan(t12);
theta2=2*atan(t22);
theta3=2*atan(t32);

theta=-[theta1 theta2 theta3]*180/pi;
end

函数调用：

a=sA(0,0,0.54532)

当我们输入该机器人的初始位置时，可得出该机器人的关节空间的变量基本为0.因此也验证了我们建立的正向运动学的准确性。

并联机器工作空间求解方法

现有的基于位置反解法求解工作空间方法主要有三种：

第一种方法的主要思路是：先得到工作空间内部点的筛选条件，即通过计算可以得到实数解的点就是工作空间内部的点，将大于工作空间的长方体中的点随机取出并使用筛选条件进行筛选，得到足够的点后将通过筛选的所有点进行三维建模。
第二种方法的主要思路是：先求出工作空间上下边界的最值位置，将该范围内的点以一定的间隔带入位置反解方程进行验证，将确定是工作空间上下边界的点进行插值拟合后即可在 Matlab 中得到工作空间模型。
第三种方法的主要思路是：通过求解单个支链的的子空间边界曲面，通过空间曲面融合技术和结构限制条件，将所有支链的子空间进行融合处理，即可到整体工作空间。

基于位置正解的求解方法：
现有的基于位置正解法求解工作空间方法主要有二种：

使用蒙特卡洛法通过随机输入主动杆输入角对工作空间进行仿真，该方法与串联机器人的工作空间求解一样。
将工作空间的求解转化为对基于位置正解法得到的非线性方程组的求解，由于计算量过大所以得到的点数较少，结果并不是很好。
求解步骤：
预估机器人工作空间的大小，确定机器人三个自由度的变化范围；
带入位置逆解，检查是否符合工作空间中的一点；
重复第2步，生成多个工作空间中的点；
通过matlab等软件绘制三维散点图。

具体代码如下：

clear all
clc;

n=10000;
pj=[];
for i=1:n  
    x=0.001*(-500+1000*rand(1,1));
    y=0.001*(-500+1000*rand(1,1));
    z=0.001*(380+(900-380)*rand(1,1));
    
    rd=0.045;
    Rd=0.120;
    r=Rd-rd;
    La=0.260;
    Lb=0.640;    
    m=x^2+y^2+z^2+r^2+La^2-Lb^2;

    M1=((m-2*x*r)/(2*La))-(r-x);
    N1=2*z;
    Q1=((m-2*x*r)/(2*La))+(r-x);
    M2=((m+(x-sqrt(3)*y)*r)/La)-2*r-(x-sqrt(3)*y);
    N2=4*z;
    Q2=((m+(x-sqrt(3)*y)*r)/La)+2*r+(x-sqrt(3)*y);
    M3=((m+(x+sqrt(3)*y)*r)/La)-2*r-(x+sqrt(3)*y);
    N3=4*z;
    Q3=((m+(x-sqrt(3)*y)*r)/La)+2*r+(x+sqrt(3)*y);

    if N1^2-4*M1*Q1>=0
        if N2^2-4*M2*Q2>=0
            if N3^2-4*M3*Q3>=0
               p=[x,y,z];
               st=sA(x,y,z);
               if st(1,1)<60 && st(1,1)>-60 && st(1,2)<60 && st(1,2)>-60 && st(1,3)<60 && st(1,3)>-60
                   ps=[st,p];
                   pj=[pj;ps];
                end
            end
        end
    end
    
end        

aa=pj(:,4)*1000;
bb=pj(:,5)*1000;
cc=-pj(:,6)*1000;
scatter3(aa,bb,cc,'.')

求解结果如下：