欠驱动机械臂运动学仿真

三轴机械臂建模及运动学仿真

  在开始具体的机械结构及驱动结构设计之前，有必要对机械臂进行运动学仿真来得到其运动学正解、运动学逆解和工作空间情况。

  刚刚结束的小学期里我们对一台货真价实的六轴机械臂进行了拆解和运动学仿真，然后水完了项目报告，本以为这次的仿真在暑期的代码上稍加更改就可以了，然而——终究是我错付了。

各仿真项目的个人理解

  开始之前我需要从应用的角度来谈谈对仿真的几大块内容：正逆解求解、工作空间分析、轨迹规划的理解。首先要明确机械臂设计的总目标，即设计出一款可以按指定方案到达指定位姿的装置。
上述理解中指定方案和指位姿这两个要求对设计和轨迹规划的过程提出了要求，运动学建模和仿真也要考虑这些要求。

• 运动学正逆解：正逆解的求解在两部分会使用到，分别是最初确定DH阵列表相关参数时和轨迹规划求解时。确定DH参数的过程就是一遍遍更改参数验求解正逆解并判断当前参数能否满足可以到达指定位姿这一要求；在轨迹规划这一过程中正逆解的使用主要是逆解，即求解终点和中间点的关节角来达到不同的轨迹规划效果。
• 轨迹规划：轨迹规划的过程即实现指定运动方案的过程，作用不言自明故不在赘诉。
• 工作空间：工作空间的分析是我比较不理解，之前学习过程中以为他只是单纯画出了机器可达与不可达的分界线，用以划分工作禁入区以保护工人。直到我开始通过正逆解求解调整臂长。
  我发现了可以通过筛选工作空间中

三轴机械臂（欠驱动）分析难点

  三维空间具有六个自由度，分别是三个移动自由度和三个旋转自由度。六自由度以下的机械臂被称为欠驱动机械臂（我没查，但应该是）
  如果机器人具有较少的自由度，则不能够随意指定任何位置和姿态，只能移动到期望的位置和较少关节所限定的姿态。即欠驱动机器人的工作空间中任意可达点较少，尤其是三自由度机器人，很多工作点个位姿只有一个，即一个位置的姿态只有一个。
  而MATLAB所具有的RTB工具箱的逆解求解又需要输入目标点的T矩阵（同时包含位置和姿态），而我们实际需要中并不对姿态做约束也无法做到，故RTB的逆解求解程序不太可行。为找到合适的逆解求解方法我做了如下尝试：

更改RTB中逆解求解源码

小学期的分析本来应该是分析一下逆解求解代码的，但因为种种原因主要是懒，并没有进行，最后兜兜转转还是回到了原点，总之先上源码：

• 第一段中mask变量为一个一行六列的矩阵，可根据实际机械臂自由度来在求解前定义前几个数字为1，其余的数字为0；

   assert(numel(opt.mask) == 6, 'RTB:ikine:badarg', 'Mask matrix should have 6 elements');
   assert(n >= numel(find(opt.mask)), 'RTB:ikine:badarg', 'Number of robot DOF must be >= the number of 1s in the mask matrix');
   W = diag(opt.mask);

• 第二段代码定义了求解时时如何把当前值与目标值比较来接近目标值的，核心代码是第一行的tr2delta函数。该函数也是RTB工具箱的一部分用以求解当前值到目标值的微分运动矩阵，并通过计算该矩阵的范数来衡量误差大小。当小于定义的结束误差opt.tol时结束计算；

  e = tr2delta(robot.fkine(q), T);        
  % are we there yet
      if norm(W*e) < opt.tol
      break;

• 第三段代码是迭代计算的过程，是真正的核心。代码采用了课本上讲过的雅克比矩阵迭代的方法，相关理解有人已经讲过了，具体见此处：
  【机器人学】机器人开源项目KDL源码学习：（2）牛顿拉普森迭代法求机器人的数值解

while true
    % 计算误差
    Tq = robot.fkine(q');
    e(1:3) = transl(T - Tq);
    Rq = t2r(Tq);
    [th,n] = tr2angvec(Rq'*t2r(T));
    e(4:6) = th*n;
    
    J = jacob0(robot, q);   % 计算雅克比
    % 根据末端误差求取关节变化
    if opt.pinv  % 雅克比伪逆法
        dq = opt.alpha * pinv( J(m,:) ) * e(m);
    else         % 雅克比转置法
        dq = J(m,:)' * e(m);
        dq = opt.alpha * dq;
    end

    % 更新关节值
    q = q + dq';

    % 判断误差是否小于容许误差tolerance
    nm = norm(e(m));
    if nm <= opt.tol
        break
    end

  分析完代码后开始着手改代码，雅克比矩阵迭代部分我也只能说是一知半解，故更改的部分可以在误差比对环节。我们最后要的是空间点的位置，对其姿态并不做要求，因而想尽量减少姿态对结果的影响，故作如下修改：

   aaa=robot.fkine(q);
    e=T-aaa;
    e=e(:,4);
    e(4:6,:)=0;
    %e = tr2delta(robot.fkine(q), T);

  将误差改为当前点与最终点位置差的范数，即只通过位置来决定误差，但经过实验后发现结果与源代码并无太大差异。改到这里其实我是挺绝望的，毕竟查资料看代码花了很长时间，但项目还得继续做下去，我也只能安慰自己：最起码看懂代码了
另外，关于RTB的逆解求解还有一个参数列表，这里一并分享

% set defaultparameters for solution 默认参数设置
opt.ilimit = 500; %默认迭代次数
opt.rlimit = 100; %最大连续拒绝次数
opt.slimit = 100; %最大尝试次数
opt.tol = 1e-10; %默认迭代误差
opt.lambda = 0.1; %默认步长
opt.lambdamin = 0; %默认步长最小值
opt.search = false; %默认关闭搜索初值
opt.quiet = false; %保持安静，减少输出
opt.verbose = false; %输出的信息详细
opt.mask = [1 1 1 1 1 1]; %默认六自由度

求解析解

  三自由度机器人的解析解表达式较为简单，可不使用矩阵变换，直接根据几何知识得出，具体如下图：

  但是，以我贫乏的数学知识，列出几页演算稿是解不出来这个三元非线性方程组。故此方法胎死腹中。

数值求解

  虽然上一个方法并没有让我们求出解析解，但是最后还是得到了三个关节变量的非线性方程组。MATLAB在数值求解非线性方程\组上也有一套相关算法，于是本着暴力求解的态度，我在MATLAB社区看了一些相关帖子。最后功夫不负有心人，找到了一篇二自由度机械臂拟牛顿法求其解析解的教程，链接如下：
MATLAB二自由度解析解
借鉴其思想和代码，带入上图三自由度解析解方程可以达到数值法求解解析解的代码， 包括以下两部分：

数值求解：

//数值求解
function [x,fval,exitflag]=Robot_Inverse_Solution(a,b,c)
x=a;
y=b;
z=c;
l1=0.18;
l2=0.15;
l3=0.03;
options=optimset('display','off','MaxFunEvals',1000000,'TolFun',1e-3);
[x,fval,exitflag]=fsolve('threeJoint',[0 0.1 0.1],options,l1,l2,l3,x,y,z);

end

非线性方程定义：

//非线性方程定义
function F=threeJoint(theta,l1,l2,l3,x,y,z)

theta1=theta(1);
theta2=theta(2);
theta3=theta(3);

% F1=x-l2*cos(theta2)*cos(theta1)-l3*cos(theta2+theta3)*cos(theta1);
% F2=y-l2*cos(theta2)*sin(theta1)-l3*cos(theta2+theta3)*sin(theta1);
% F3=z-l1-l2*sin(theta2)*-l3*sin(theta2+theta3);

F=[F1,F2,F3];
end

  该方法具有初值敏感性，故在求解时输入关节变量theta的兴趣区间对求解结果的准确性有很大影响，而大多数情况下我们并不能给出一个很恰当的估计值，故此方法在理论上并不太适合实际应用。
在具体的求解过程中也确实体现了这一点，我尝试了一组比较好求的解：

theta=[0，pi/3，0]；

尽管如此，结果依然很不理想，这仍然是很失败的尝试。

工作空间筛选

  面对接二连三的失败，我决定暂时放弃逆解，先进行工作空间分析。工作空间的分析采用蒙特卡罗法，机械臂的每个关节关节变量能够产生出N个伪随机值，即将关节变量的值按由小到大的顺序代入机械手的正运动学方程，得到末端执行器在参考坐标中的位置向量，将所得到的机械手位置向量绘制出来即为机械臂的工作空间。
  在分析过程中我突然想到既然蒙特卡罗法生成了一万甚至十万个工作点，那能否通过筛选来找到合适的工作点呢？

    q1_rand = q1_s*angle + unifrnd(0,1,[num,1])*(q1_end - q1_s)*angle;
    q2_rand = q2_s*angle + unifrnd(0,1,[num,1])*(q2_end - q2_s)*angle;
    q3_rand = q3_s*angle + unifrnd(0,1,[num,1])*(q3_end - q3_s)*angle;
    q4_rand = rand(num,1)*0;
    q = [q1_rand q2_rand q3_rand q4_rand];
    fk=modrobot.fkine(q).t;%正向运动学仿真函数index=zeros(1,5000)
		for i=1:5000
   			 t=fk(1,i);
   			 t=transl(t)	%从SE3矩阵改为可以直接诶操作的矩阵
   			 %%具体赛选过程
   			 if (abs(t(1,2)-0.01)<0.001)&(abs(t(1,1)-0.01)<0.001）
   			 end
        	 if(abs(t(1,1)-0.01)<0.001&(abs(t(1,1)-0.01)<0.001）
        	 end
             if(abs(t(1,3)-0.01)<0.001&(abs(t(1,3)-0.01)<0.001）
               index(1,i)=1;
   	         end
end

  该程序可以实现这一功能，并返回满足条件的一组解的index。也就是说工作空间可以提供安全区域划分也可以用于筛选合适的运动学逆解。但这一方法计算量很大，只适合最初确定机器人构型时使用，在轨迹规划时并不实用，

实用的解析解法

  本来到此已经顺利的完成了最开始的任务，但blog的撰写把战线拖得很长，从八月多一直拖到了今天。也就是在今天，在张爱军老师的课上，Saeed B.Niku教授的机器人学我带来了了新的希望——解析解是可解的！

  本来还想手撸逆解来着 ，但结果如图所示，在解逆解的路上死在了正解求解的途中，这时不妨让我们打开手中的MATLAB：

syms th1 th2 th3  l1 l2 l3 ;
syms nx ny nz ax ay az ox oy oz px py pz;
TT=[nx,ox,ax,px;
    ny,oy,ay,py;
    nz,oz,az,pz;
    0,0,0,1];


%坐标系绕Z轴旋转的转换矩阵Tz
Tz1=[ cos(th1) sin(th1) 0 0;
    -sin(th1) cos(th1) 0 0;
    0         0        1 0;
    0 0 0 1]; 

Tz2=[ cos(th2) sin(th2) 0 0;
    -sin(th2) cos(th2) 0 0;
    0         0        1 0;
    0 0 0 1]; 

Tz3=[ cos(th3) sin(th3) 0 0;
    -sin(th3) cos(th3) 0 0;
    0         0        1 0;
    0 0 0 1]; 
%正解
T1=trotx(0)*transl([0 0 0])*Tz1*transl([0 0 l1]);
T2=trotx(90)*transl([0 0 0])*Tz2*transl([0 0 0]);
T3=trotx(0)*transl([l2 0 0])*Tz3*transl([0 0 0]);
T4=transl([l3 0 0]);

%Paul反变换
T=T1*T2*T3*T4
t1=T_ink(T1)*TT
t11= T2*T3*T4
    

图为第一次Paul反变换后两侧p向量，带入p2计算后结果刚好与书中相同.

  其实写到这里上文所说三自由度机械臂的难点已经不构成了，因为逆解的求解使用Paul反变换法可以较为简单的求解，甚至写到这里四轴五周等欠驱动机械臂也有了大概的想法，即除了关注两侧的p向量可以再多关注一个方向的绕轴旋转，但这都是后话了。

总结

很简单的想法，很幼稚的尝试，也尝试了很多时间；搭建一个机械臂的想法从八月下旬开始到现在已经做好了第一步的仿真、调试了顶层的MoveIt代码、写了直流电机的PWM驱动和串口通信…写完这篇博客后就是组装和调试了，又是一大批的活计。
保研已经是没啥可能了，考研又还太早，反正也是闲着，干点有趣且有意义的事情吧。

大三也该去收获了