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)函数走到目的姿态去。文章来源：https://www.toymoban.com/news/detail-592606.html
电机执行
电机执行函数执行函数很简单，就是根据当前电机的角度，若当前角度小于目标角度电机就发一个脉冲正走一步，若大于当前角度电机就发一个脉冲反走一步。其中dl1~dl6参数就是电机走一步对应的角度。文章来源地址https://www.toymoban.com/news/detail-592606.html
到了这里，关于开源机器人SmallRobotArm机器人源码解读的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
