|
机器人如何从各轴角度算出当前XYZ 一文介绍过ABB机器人获取DH参数和运动学正解方法，注意在使用如下DH参数时，平移和旋转顺序为
，即先绕当前x轴旋转α度，再沿新的x轴平移a，再绕新的Z轴旋转θ度，再沿新的Z轴平移d
以下距离IRB120机器人DH参数：
αi (twist)
|
ai(mm) length
|
Θi Rotation
|
di(mm) offset
|
1
|
0
|
0
|
0
|
290
|
2
|
-π/2
|
0
|
-π/2
|
0
|
3
|
0
|
270
|
0
|
0
|
4
|
-π/2
|
70
|
0
|
302
|
5
|
π/2
|
0
|
-π
|
0
|
6
|
π/2
|
0
|
0
|
72
|
故按照上式定义计算后整理，可以用得到基于DH参数的位姿矩阵如下:
上式称为位姿矩阵。其中左上角为旋转矩阵。对于旋转矩阵，可以采用四元数的简化表达方式。所以上述位姿矩阵可以转化为ABB机器人使用的POSE数据类型[[x,y,z],[q1,q2,q3,q4]]
对于
，则可以将每一次平移和旋转作为一个Pose数据的右乘。
ABB机器人的点位信息robtarget，除了有Pose数据，还有轴配置信息。
Confdata包括cf1，cf4，cf6和cfx。对于cf1，若当前1轴角度为0-90，则cf1为0,1轴角度90-180，cf1为1，1轴角度为-90~0°，则cf1为-1，其他以此类推。Cf4和cf6同理。具体可以参考轴配置confdata解释 一文
对于cfx，介绍如下。
注：
1. “相对于轴1 的腕中心”：意义为当前5轴坐标系在1轴坐标系下的关系。轴1坐标系会随着1轴转动而转动，轴5坐标系也会随着其他轴转动而转动。“在前面”的意思是轴5坐标系在轴1坐标系下的前方，即在轴1坐标系下的轴5坐标系位置的x为正
2. “相对于下臂的腕中心”：意义为当前5轴坐标系在2轴坐标系下的关系。轴2坐标系会随着2轴转动而转动，轴5坐标系也会随着其他轴转动而转动。“在前面”的意思是轴5坐标系在轴2坐标系下的前方，即在轴2坐标系下的轴5坐标系位置的y为正（轴2坐标系如图，前方为Y方向）
代码如下：
LOCALVAR num alpha{6}:=[0,-90,0,-90,90,90]; LOCALVAR num a{6}:=[0,0,270,70,0,0]; LOCALVAR num theta{6}:=[0,-90,0,0,-180,0]; LOCALVAR num d{6}:=[290,0,0,302,0,72];!以上为IRB120机器人 DH参数
funcnum n_cal_cfg(jointtarget j1) VAR num n_cfx; VAR robtarget ptmp; VAR pose pose1{6}; VAR pose pose10{6}; VAR pose pose3_5:=[[0,0,0],[1,0,0,0]]; VAR pose pose2_5:=[[0,0,0],[1,0,0,0]]; VAR pose pose6:=[[0,0,0],[1,0,0,0]]; VAR jointtarget jtmp:=[[0,0,0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]]; jtmp:=j1; curr_angle{1}:=jtmp.robax.rax_1; curr_angle{2}:=jtmp.robax.rax_2; curr_angle{3}:=jtmp.robax.rax_3; curr_angle{4}:=jtmp.robax.rax_4; curr_angle{5}:=jtmp.robax.rax_5; curr_angle{6}:=jtmp.robax.rax_6; FOR i FROM 1 TO 6 DO pose1{i}:=f_dh2pose(alpha{i},a{i},theta{i}+curr_angle{i},d{i}); !获取各轴坐标系的位姿矩阵 ENDFOR pose3_5:=[[0,0,0],[1,0,0,0]]; FOR i FROM 3 TO 5 DO pose3_5:=PoseMult(pose3_5,pose1{i}); ENDFOR !计算轴3坐标系到轴5坐标系的变化，即轴5坐标系在2轴坐标系的位姿 pose2_5:=[[0,0,0],[1,0,0,0]]; FOR i FROM 2 TO 5 DO pose2_5:=PoseMult(pose2_5,pose1{i}); ENDFOR !计算轴2坐标系到轴5坐标系的变化，即轴5坐标系在1轴坐标系的位姿
IF pose2_5.trans.x>0 THEN !判断腕坐标系在轴1坐标系的前后
IF pose3_5.trans.y>0 THEN !判断腕坐标系在轴2坐标系的前后，轴2坐标系前方为y方向 IF jtmp.robax.rax_5>0 THEN !判断5轴角度 n_cfx:=0; ELSE n_cfx:=1; ENDIF ELSE IF jtmp.robax.rax_5>0 THEN n_cfx:=2; ELSE n_cfx:=3; ENDIF ENDIF ELSE IF pose3_5.trans.y>0 THEN IF jtmp.robax.rax_5>0 THEN n_cfx:=4; ELSE n_cfx:=5; ENDIF ELSE IF jtmp.robax.rax_5>0 THEN n_cfx:=6; ELSE n_cfx:=7; ENDIF ENDIF ENDIF return n_cfx;ENDFUNC
FUNCpose f_dh2pose(num alpha,num a,num theta,num d) VAR pose pose1:=[[0,0,0],[1,0,0,0]]; !计算各轴位姿矩阵并转化为Pose !顺序为先绕当前x轴旋转α度，再沿新的x轴平移a，再绕新的Z轴旋转θ度，再沿新的Z轴平移dpose1:=PoseMult(pose1,[[0,0,0],orientzyx(0,0,alpha)]); pose1:=PoseMult(pose1,[[a,0,0],orientzyx(0,0,0)]); pose1:=PoseMult(pose1,[[0,0,0],orientzyx(theta,0,0)]); pose1:=PoseMult(pose1,[[0,0,d],orientzyx(0,0,0)]); RETURN pose1; ENDFUNC
|
