【机器人学】7-2.六自由度机器人自干涉检测-计算圆柱体的上下圆心坐标【附MATLAB代码】

目录

前言

机械臂几何参数

机器等效圆柱体坐标确定

MATLAB代码

---

前言

        上一章介绍了机器人自干涉检测的总体算法，提出了算法的三个核心：

                一  根据机械臂的几何数据以及DH参数，确定机械臂等效的圆柱体的上下圆心坐标。

                二  将一个圆柱体旋转到与坐标Z轴对齐，另一个圆柱体圆转到，上下圆在XoY平面的

                     投影形成的椭圆在y方向上长轴为2r，这一个旋转流程的数学表达。

                三  原点与椭圆的关系，求原点是否在椭圆内部，原点到椭圆的最短距离，线段与

                     线段的最短距离。

               【机器人学】7-1.六自由度机器人自干涉检测-总体算法介绍-CSDN博客https://blog.csdn.net/y12345655/article/details/141167918?spm=1001.2014.3001.5502

        这一章博客将解决第一个问题，根据机械臂的几何数据以及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代码】