目录
前言
机械臂几何参数
机器等效圆柱体坐标确定
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)测试结果
如上图所示，九个圆柱体的上下底面圆心坐标以全部求得。
获取上下圆心坐标用的工具代码：
不可直接照抄，一定要明白每一个圆心坐标是如何求出来的。
clc;
clear;
%format long g; %输出结果为小数
syms d1 d2 d3 a2 a3 d4 a4 d5 d6_2 a7 th1 th2 th3 th4 th5 th6 d7 th7 a7 e3 e2
syms alp7 th8 d8 a8 alp8 th10 d10 a10 alp10 d6_2 a9 d6 d9
th(10) = th10; d(10) = d10; a(10) = a10; alp(10) = alp10;
th(1) = th1; d(1) = d1; a(1) = 0; alp(1) = 0;
th(2) = th2; d(2) = d2; a(2) = 0; alp(2) = pi/2; %th2=th2+pi/2
th(3) = th3; d(3) = 0; a(3) = a3; alp(3) = 0;
th(4) = th4; d(4) = 0; a(4) = a4; alp(4) = 0; %th4=th4-pi/2
th(5) = th5; d(5) = d5; a(5) = 0; alp(5) = -pi/2;
th(6) = th6; d(6) = d6+d7; a(6) = 0; alp(6) = pi/2; %d6_2为法兰的延长 在加上工端
th(7) = 0; d(7) = 0; a(7) = 0; alp(7) = 17.5/100; %alp7 = 19.2/110 弧长除以半径
th(8) = 0; d(8) = d8+d9; a(8) = 0; alp(8) = 0;
T01 = connectingRodTransfer([alp(1), a(1), d(1), th(1)],0);
T12 = connectingRodTransfer([alp(2), a(2), d(2), th(2)],0);
T23 = connectingRodTransfer([alp(3), a(3), d(3), th(3)],0);
T34 = connectingRodTransfer([alp(4), a(4), d(4), th(4)],0);
T45 = connectingRodTransfer([alp(5), a(5), d(5), th(5)],0);
T56 = connectingRodTransfer([alp(6), a(6), d(6), th(6)],0);%
T67 = connectingRodTransfer([alp(7), a(7), d(7), th(7)],0);%
T78 = connectingRodTransfer([alp(8), a(8), d(8), th(8)],0);%
T=T01*T12*T23*T34*T45*T56*T67*T78;
simplify(T(1,4))
simplify(T(2,4))
simplify(T(3,4))
return
function TArr = connectingRodTransfer(dh,theta,inv)
if nargin == 2
inv=0;
end
ct = cos(dh(4)+theta);
st = sin(dh(4)+theta);
ca = cos(dh(1));
sa = sin(dh(1));
a = dh(2);
d = dh(3);
TArr = [ ct -st 0 a;
st*ca ct*ca -sa -sa*d;
st*sa ct*sa ca ca*d;
0 0 0 1 ];
if inv
TArr=[TArr(1:3,1:3)' -TArr(1:3,1:3)'*TArr(1:3,4);0 0 0 1];
end下一章： 【机器人学】7-3.六自由度机器人自干涉检测-圆柱体的旋转变换【附MATLAB代码】
![[CSCCTF 2019 Qual]FlaskLight1](https://i-blog.csdnimg.cn/direct/206e3f2e1f9e4b78a07809597944a647.png)