【matlab与机器人学一些联动】（编程以及一些机器人工具箱的使用）

0.0.食用方法

• （机器人工具箱的文档中查找）构造器：搜索 Construct ，所有有关的就是构造函数，比如

  `r1 = SO3.angvec(pi/6, [1,0,0]');`

  or

  `r2 = SO3.SO3([1 0 0;0 1 0;0 0 1])`

• 现成的dh转t矩阵的函数

  function tf = dh2tf(a,alpha,d,theta)
  %用dh参数求矩阵
      tf = zeros(4);
      tf(1,1) = cos(theta);
      tf(1,2) = -sin(theta);
      tf(1,4) = a;
      tf(2,1) = sin(theta)*cos(alpha);
      tf(2,2) = cos(theta)*cos(alpha);
      tf(2,3) = -sin(alpha);
      tf(2,4) = -d*sin(alpha);
      tf(3,1) = sin(theta)*sin(alpha);
      tf(3,2) = cos(theta)*sin(alpha);
      tf(3,3) = cos(alpha);
      tf(3,4) = d*cos(alpha);
      tf(4,4) = 1;
  end

• 递推法求雅克比

  注意的点：有一个很好用的操作符： \ 表示求逆左乘，在这个方法中很好用（因为一般得到的是i 2 i+1）

  % 参考第11-13次作业
  syms th1 th2 th3 l1 l2 l3 d1 d2 d3
  v = str2sym('[0 0 0]');
  v = v.';
  w = str2sym('[0 0 0]');
  w = w.';
  z = str2sym('[0 0 1]');
  z = z.';
  l = str2sym('[0 l1 l2 l3]');
  theta = str2sym('[th1 th2 th3 0]');
  dtheta = str2sym('[d1 d2 d3 0]');
  R = eye(3);
  ​
  for i = 1:4
      [r,p] = get_tf(l(i),theta(i));
      R = R*r;
      tmp = w;
      w = r\(w+dtheta(i)*z); %\表示求逆左乘
      v = r\(v+cross(tmp,p));
  end
  ​
  v = R*v
  w = R*w

• 拉格朗日法

  注意的点：由于其中会涉及（对时间）求导或者求偏微分，如果直接用符号变量来参与求导，会导致diff后这个变量变为1，所以需要使用带有自变量的符号函数，以及替换函数不会直接把替换作用于表达式，所以需要变量来存！

  syms t  th1(t)  d1(t)
  %如上，使用diff后就变成下面的，其实相当于没有计算，保留了这个表达
  diff(th1(t),t)
  %最后使用一下变量替换，第一个参数是要操作的表达式，第二个是被替换量，第三个是替换后的表达
  res = subs(expression ,diff(th1(t),t) ,d1(t));
  ​
  %补充表达批量替换
  res = subs(expression , {exp1, exp2, exp3},  {exp1_new, exp2_new, exp3_new})

  最后想求M/V/G矩阵时，就同样用变量替换，把一些不需要的项用0替换就行

• 使用符号变量

  量	表达
  单位矩阵	sym(eye(n))
  零矩阵	sym(zeros(n))
  表达式	str2sm(' exp ')

0.1.常用数据类型

数据类型	类	构造方法	可转类型
R（旋转矩阵）	SO3	- 直接用矩阵convert - 轴角表达angvec	- 欧拉角/rpy - 轴角 - 单位四元数
T（齐次矩阵）	SE3	- 直接实例化SE3(R矩阵,P向量)

• 向量操作要加 .，不然就是对矩阵的操作，不太对

0.2.模型

构建与正逆解

%这里是构建 3r 模型，直接利用这个进行正逆解
L(1) = Link([th1 0 0 0 ],'modified');
L(2) = Link([th2 0 l1 0 ],'modified');
L(3) = Link([th3 0 l2 0 ],'modified');
R3 = SerialLink(L,'name','3r-robot');
%用工具箱的正解
var = [th1,th2,th3];
ans = R3.fkine(var);
R3.plot(var);
​
​
%逆解
ns = R3.ikine(tf,'mask',[1 1 1 0 0 0]);
R3.plot(ans,'jaxes','xyz');

tips：似乎使用‘mask’可以改变需要的自由度，而防止报错

0.3.matlab函数

一个模块：针对分段函数的积分求解表达式

• function的格式用正常的if-else书写（那么使用时得加@表示句柄函数）

• 积分就无法直接int，为了保证精度，只能一段段积分，求出积分后一个个离散点的函数值（类似于使用了自控的那种step函数）

  %这个是用theta（分段）的导求积分得theta的离散函数值
  ths = 0:0.001:3;
  n = 0;
  for i = 0:0.001:3
      n = n+1;
      ths(n) = integral(@dtheta,0,i,'ArrayValued' ,true);
  end

• 另一种积分，上限积分（也就是上面的i视为积分后函数的变量，不着急给具体值）

  theta = @(t)integral(@dtheta,0,t,'ArrayValued' ,true);
  %这个求出的就是theta关于t的一个函数，要求任何时刻的theta，直接之后使用
  theta(time);
  %就可以得到返回值

• 绘图使用fplot

  fplot(@dtheta,[0,3])

• 目前没有找到一个很好的让分段函数积分两次求表达的函数，matlab似乎不太擅长做这个（当时为这个debug了很久几乎自闭）