目录
前言
上一章介绍了机器人自干涉检测的总体算法,提出了算法的三个核心:
一 根据机械臂的几何数据以及DH参数,确定机械臂等效的圆柱体的上下圆心坐标。
二 将一个圆柱体旋转到与坐标Z轴对齐,另一个圆柱体圆转到,上下圆在XoY平面的
投影形成的椭圆在y方向上长轴为2r,这一个旋转流程的数学表达。
三 原点与椭圆的关系,求原点是否在椭圆内部,原点到椭圆的最短距离,线段与
线段的最短距离。
这一章博客将解决第一个问题,根据机械臂的几何数据以及DH参数,确定机械臂等效的圆柱体的上下圆心坐标。
机械臂几何参数
我们需要机器人的几何结构数据,才能求得等效圆柱体的上下坐标。其中DH参数可以表示绝大多数几何特征。刚好昨天在公园散步的时候,无意中捡到了一张机器人图纸,且包含了机器人DH参数,就以它为例吧。
DH参数为:
关节1 | 关节2 | 关节3 | 关节4 | 关节5 | 关节6 | |
0 | 90 | 0 | 0 | -90 | 90 | |
a | 0 | 0 | 425 | 393 | 0 | 0 |
d | 160.7 | 0 | 0 | 113.3 | 99 | 93.6 |
0 | 90 | 0 | -90 | 0 | 0 | |
0 | 0 | 0 | 0 | 0 | 0 |
改进型的DH参数定义为:
机器人结构图纸为:
我们把这个机器人分为9个圆柱体,标号1~9如上图所示。
机器等效圆柱体坐标确定
将机器人的基座坐标定义为坐标系原点。那么第一个圆柱的上下坐标直接就可以求出来:
机器人可以加高基座,因此预留一个length参数,当没有加高基座时,length为0。
从第二个圆柱开始,就无法直接看出来了,因为圆柱上下底面圆心的坐标,会随机器人关节转角不同而变化。因此结合DH参数的概念辅助求解会好得多。
由于关节越往后数据量越大,并且方法都是一样,因此后面将只提供DH参数的数据,以及方法,不在详细计算。
至此该机械臂等效的9个圆柱体的底面圆心坐标以全部得到,涉及到三角函数的代换,表达式会比较长,可以用MATLAB的simplify函数化简,比如:
此时输出的位置信息要简化得多,如果你的机器人包含夹爪,按照同样的方法进行模块叠加。
MATLAB代码
解析表达式计算圆心坐标
function lineModelP = RobotLineModel(dhPara,exDhPara,jointPose)
a2=dhPara(3,2);
a3=dhPara(4,2);
d1=dhPara(1,3);
d4=dhPara(4,3);
d5=dhPara(5,3);
d6=dhPara(6,3);
d2 = exDhPara(1);
d3 = exDhPara(2);
len6 = exDhPara(3);
e1 = exDhPara(4);
e2 = exDhPara(5);
e3 = exDhPara(6);
for i=1:6
jointPose(i)=jointPose(i)+dhPara(i,4);
end
q1 = jointPose(1);
q2 = jointPose(2);
q3 = jointPose(3);
q4 = jointPose(4);
q5 = jointPose(5);
c1=cos(q1);
s1=sin(q1);
c2=cos(q2);
s2=sin(q2);
c5=cos(q5);
s5=sin(q5);
s23=sin(q2+q3);
c23=cos(q2+q3);
s234=sin(q2+q3+q4);
c234=cos(q2+q3+q4);
s1c2=s1*c2;
c1c2=c1*c2;
s1c5=s1*c5;
c1c5=c1*c5;
s1c23=s1*c23;
c1c23=c1*c23;
s1s234=s1*s234;
s1c234=s1*c234;
c1s234=c1*s234;
c1c234=c1*c234;
s234s5=s234*s5;
s1c234s5=s1c234*s5;
c1c234s5=c1c234*s5;
d2s1=d2*s1;
d2c1=d2*c1;
d2_d3=d2-d3;
d2_d3s1=(d2-d3)*s1;
d2_d3c1=(d2-d3)*c1;
a2s2=a2*s2;
a2s1c2=a2*s1c2;
a2c1c2=a2*c1c2;
a3s1c23=a3*s1c23;
a3c1c23=a3*c1c23;
a3s23=a3*s23;
d4s1=d4*s1;
d4c1=d4*c1;
d5c234=d5*c234;
d5s1s234=d5*s1s234;
d5c1s234=d5*c1s234;
d6s1c5=d6*s1c5;
d6c1c5=d6*c1c5;
d6s234s5=d6*s234s5;
d6s1c234s5=d6*s1c234s5;
d6c1c234s5=d6*c1c234s5;
d6__l = d6 + len6;
lineModelP(1,1:5) = zeros(1,5);
lineModelP(1,6) = 1.5*d1;
lineModelP(2,1) = d2s1;
lineModelP(2,2) = -d2c1;
lineModelP(2,3) = d1;
lineModelP(2,4) = a2c1c2 + d2s1;
lineModelP(2,5) = a2s1c2 - d2c1;
lineModelP(2,6) = a2s2 + d1;
lineModelP(3,1) = a2c1c2 + d2_d3s1;
lineModelP(3,2) = a2s1c2 - d2_d3c1;
lineModelP(3,3) = a2s2 + d1;
lineModelP(3,4) = a2c1c2 + d2_d3s1 + a3c1c23;
lineModelP(3,5) = a2s1c2 - d2_d3c1 + a3s1c23;
lineModelP(3,6) = a2s2 + a3s23 + d1;
lineModelP(4,1) = e3*c1s234 + a3c1c23 + d4s1 + a2c1c2;
lineModelP(4,2) = e3*s1s234 + a3s1c23 - d4c1 + a2s1c2;
lineModelP(4,3) = -e3*c234 + a3s23 + a2s2 + d1;
lineModelP(4,4) = -e3*c1s234 + a3c1c23 + d4s1 + a2c1c2;
lineModelP(4,5) = -e3*s1s234 + a3s1c23 - d4c1 + a2s1c2;
lineModelP(4,6) = e3*c234 + a3s23 + a2s2 + d1;
lineModelP(5,1) = -d5c1s234 + d4s1 - e3*c1c234s5 + a2c1c2 - e3*s1c5 + a3c1c23;
lineModelP(5,2) = -d5s1s234 - d4c1 - e3*s1c234s5 + a2s1c2 + e3*c1c5 + a3s1c23;
lineModelP(5,3) = -e3*s234s5 + a3s23 + a2s2 + d5c234 + d1;
lineModelP(5,4) = -d5c1s234 + d4s1 + d6c1c234s5 + a2c1c2 + d6s1c5 + a3c1c23;
lineModelP(5,5) = -d5s1s234 - d4c1 + d6s1c234s5 + a2s1c2 - d6c1c5 + a3s1c23;
lineModelP(5,6) = d6s234s5 + a3s23 + a2s2 + d5c234 + d1;
lineModelP(6,1) = lineModelP(5,4);
lineModelP(6,2) = lineModelP(5,5);
lineModelP(6,3) = lineModelP(5,6);
lineModelP(6,4) = -d5c1s234 + d4s1 + d6__l*c1c234s5 + a2c1c2 + d6__l*s1c5 + a3c1c23;
lineModelP(6,5) = -d5s1s234 - d4c1 + d6__l*s1c234s5 + a2s1c2 - d6__l*c1c5 + a3s1c23;
lineModelP(6,6) = d6__l*s234s5 + a3s23 + a2s2 + d5c234 + d1;
lineModelP(7,1) = a2c1c2 + (d2+e2)*s1;
lineModelP(7,2) = a2s1c2 - (d2+e2)*c1;
lineModelP(7,3) = a2s2 + d1;
lineModelP(7,4) = lineModelP(2,4);
lineModelP(7,5) = lineModelP(2,5);
lineModelP(7,6) = lineModelP(2,6);
lineModelP(8,1) = a2c1c2 + (d2_d3-e3)*s1 + a3c1c23;
lineModelP(8,2) = a2s1c2 - (d2_d3-e3)*c1 + a3s1c23;
lineModelP(8,3) = a2s2 + a3s23 + d1;
lineModelP(8,4) = lineModelP(3,4);
lineModelP(8,5) = lineModelP(3,5);
lineModelP(8,6) = lineModelP(3,6);
lineModelP(9,1) = (d2+e1)*s1;
lineModelP(9,2) = -(d2+e1)*c1;
lineModelP(9,3) = d1;
lineModelP(9,4) = 0;
lineModelP(9,5) = 0;
lineModelP(9,6) = d1;
end
测试代码
clc;
clear;
DH_Para = [0, 90, 0, 0, -90, 90
0, 0, 425, 393, 0, 0
160.7, 0, 0, 113.3, 99, 93.6
0, 90, 0, -90, 0, 0
0, 0, 0, 0, 0, 0]';
DH_Para(:,2:3)=DH_Para(:,2:3)/1000;
DH_Para(:,1)=DH_Para(:,1)/180*pi;
DH_Para(:,4)=DH_Para(:,4)/180*pi;
exDhPara = [138,123.7,0,75,75,55]'/10000;
jointPose = [-140,0.99,78.65,0.36,-90,0]/180*pi;
lineModelP = RobotLineModel(DH_Para,exDhPara,jointPose)
测试结果
如上图所示,九个圆柱体的上下底面圆心坐标以全部求得。
下一章: 【机器人学】7-3.六自由度机器人自干涉检测-圆柱体的旋转变换【附MATLAB代码】