1 SmallRobotArm简介

开源机器人SmallRobotArm是一个开源的6轴机械臂，都由步进电机驱动，github地址：https://github.com/SkyentificGit/SmallRobotArm

 机器人长这个样子

2 欧拉角及姿态变换

由欧拉角求姿态矩阵

源码中用的欧拉角是ZYZ顺组的欧拉角。

已知世界坐标的坐标(x,y,z)和欧拉角(α,β,γ)，求出对应的姿态矩阵：

源码中实现这一功能的是pos2tran(float* Xpt, float* Tpt)函数。

由姿态矩阵求欧拉角

已知姿态矩阵，求对应的(x,y,z)和欧拉角(α,β,γ)为：

 

源码中实现这一功能的是函数void tran2pos(float* Ttp, float* Xtp)。

3 机器人D-H参数及正运动学

由源码可以看到机械臂用的是标准DH参数，参数表：

i	αi	ai	di	θi
1	-90	r1	d1	t1
2	0	r2	0	t2
3	-90	r3	d3	t3
4	90	0	d4	t4
5	-90	0	0	t5
6	0	0	d6	t6

对于标准DH参数为(α,a,d,θ)的关节，其变换矩阵为:

SmallRobotArm机器人6个关节的DH参数变换矩阵分别为：

所以T06=T01*T12*T23*T34*T45*T56

4 逆运动学求解

SmallRobotArm源码中求逆解的函数为InverseK。它根据给出的机械臂中断的(x,y,z)坐标及欧拉角计算了6个关节角度。

设T06为：

 欲求反求6个关节角t1,t2,t3,t4,t5,t6

关节角t1解算

根据等式T16=T12*T23*T34*T45*T56=inv(T01)*T06，建立等式

 取T16的元素（2,2）和（2,3）构成等式：

可以求出： 

 ​​​

 关节角t2和t3的解算

 t1计算出来后，现在T16等式的右边矩阵都是已知数。这里用aij代指右边矩阵的对应元素。

 取T16的（0，0）元素和(1,0)元素构成等式：

 上式乘以s23,下式乘以c23,有：

 ​​​​​​​

 下式减去上式有：

 取T16的（0,1）元素和(1,1)元素构成等式有：

变换后可得：

 

取T16的(0,2)元素和(1,2)元素构成等式有：

 ​​​​​​​

 变换后可得：

 取T16的（0,3）元素和(1,3)元素构成等式有：

变换后得：

将（2-1）、（2-2）和(2-3)三式相加有：

 

 由此式子可以解出s23/c23,进而解出t2+t3

再代入到（2-4）式中，接出t3，则t2也可求出。

关节角t4、t5和t6的解算

 先求出T03的逆矩阵

由inv(T03)*T06

 由T36的元素(0,2)与(1,2)，可以求出t4。

t4=atan2(-T36[1][2],-T36[0][2])

由T36的元素(0,2)与(1,2)元素相加取平方和，再开根号，可以求出s5，再结合元素(2,2)，可求出t5:

由T36的元素(2,1)与元素(2,0)，可求出t6:

t6=atan2(-T36[2][1], T36[2][0])

5 机器人运动控制

如何走一条直线

最基本的运动是如何从一个点走直线到另外一个点，源码中实现该功能的函数是

void goStrightLine(float* xfi, float* xff, float vel0, float acc0, float velini, float velfin)。其中xfi是出发点关节角度数组，xff是目的点关节角度数组，vel0是巡航速度，acc0是加速度，velini是初速度，velfin是结束速度。

void goStrightLine(float* xfi, float* xff, float vel0, float acc0, float velini, float velfin){
  //
  float lmax = max(abs(xff[0]-xfi[0]),abs(xff[1]-xfi[1]));
  lmax = max(lmax,abs(xff[2]-xfi[2]));
  lmax = max(lmax,abs(xff[3]-xfi[3]));
  lmax = max(lmax,abs(xff[4]-xfi[4]));
  lmax = max(lmax,abs(xff[5]-xfi[5]));
  unsigned long preMil = micros();
  double l = 0.0;
  vel0 = min(vel0,sqrt(lmax*acc0+0.5*velini*velini+0.5*velfin*velfin));
  unsigned long curMil = micros();
  unsigned long t = 0;
  double tap = vel0/acc0-velini/acc0;
  double lap = velini*tap+acc0*tap*tap/2.0;
  double lcsp = lmax-(vel0*vel0/2.0/acc0-velfin*velfin/2.0/acc0);
  double tcsp = (lcsp-lap)/vel0+tap;
  double tfin = vel0/acc0-velfin/acc0+tcsp;
  while (curMil-preMil<=tfin){
    t = curMil-preMil;
    //acceleration phase
    if (t<=tap) {
      l = velini*t+acc0*t*t/2.0;
    }
    //contant maximum speed phase
    if (t>tap && t<=tcsp) {
      l = lap+vel0*(t-tap);
    }
    //deceleration phase
    if (t>tcsp) {
      l = lcsp+vel0*(t-tcsp)-acc0*(t-tcsp)*(t-tcsp)/2.0;
    }
  
    //trajectory x and y as a function of l
    float Xx[6];
    Xx[0]=xfi[0]+(xff[0]-xfi[0])/lmax*l;
    Xx[1]=xfi[1]+(xff[1]-xfi[1])/lmax*l;
    Xx[2]=xfi[2]+(xff[2]-xfi[2])/lmax*l;
    Xx[3]=xfi[3]+(xff[3]-xfi[3])/lmax*l;
    Xx[4]=xfi[4]+(xff[4]-xfi[4])/lmax*l;
    Xx[5]=xfi[5]+(xff[5]-xfi[5])/lmax*l;
    
    goTrajectory(Xx);
    curMil = micros();
  }
}

 速度规划

 goStrightLine函数中用的是梯形速度规划算法。如下图左图所示，对于初速度为V0,为Ve，巡航速度为Vm，运动距离为S的直线运动

 则加速阶段走过的距离Sa和减速段走过的距离Sd为：

 ​​​​​​​

 此时Vm为：

若有匀速段，则Vm<=Vmm;若没有匀速段，则最大速度只能到Vmm，所以goStrightLine中取最大速度为Vmm和Vel0的较小值，将其重新赋给vel0。

已知最,高速度vel0，可以求出加速时间：

double tap = vel0/acc0-velini/acc0;

进而求出加速段总距离 

double lap = velini*tap+acc0*tap*tap/2.0;

进而求出匀速段距离

double lcsp = lmax-(vel0*vel0/2.0/acc0-velfin*velfin/2.0/acc0);

从而求出匀速段时间

double tcsp = (lcsp-lap)/vel0+tap;

 再求出减速度时间

double tcsp = (lcsp-lap)/vel0+tap;
  double tfin = vel0/acc0-velfin/acc0+tcsp;

 对于当前的时间t,通过判断t所在的区间，可以求出当前的距离，进而求出每个关节角度xx，再调用

电机执行函数goTrajectory(Xx)函数走到目的姿态去。

电机执行

电机执行函数执行函数很简单，就是根据当前电机的角度，若当前角度小于目标角度电机就发一个脉冲正走一步，若大于当前角度电机就发一个脉冲反走一步。其中dl1~dl6参数就是电机走一步对应的角度。