机器人如何从各轴角度算出当前XYZ
转载请先后台留言，大家一起支持原创，推动机器人使用和发展
本公众号对各类ABB机器人应用，仿真，毕业设计提供技术支持，详细后台留言
本公众号诚挚希望与各机器人培训机构，机器人使用单元合作，提供技术支持，详细后台留言
1. 机器人只是知道各个轴的角度，机器人是如何知道当前法兰盘的XYZ笛卡尔系坐标呢？
2. 问题一其实就是机器人运动学正解问题，即通过a1-a6角度值，计算对应的末端工具（此处举例法兰盘）坐标值（XYZ ABC）
3. 对于空间一个位姿（位置XYZ，姿态ABC）可以通过位姿矩阵表示，具体参考昨天发送文章。
4. 之前介绍过，机器人运动学模型可以通过多种方法构建，比如最常见的DH参数建模法，见 机器人DH模型
5. 之前介绍过如何获取ABB机器人DH 参数，见如何获取ABB机器人DH参数
6. 此处举例IRB120机器人。
7. 通常机器人建模如上图，建立Base坐标系（X0Y0Z0），之后依次对应1轴到6轴6个坐标系（x1y1z1….x6y6z6），通过DH参数来表示这6个动坐标系相对于前一个坐标系的关系（T1-T6）。
8. 对于DH参数，旋转和平移顺序如下：即先绕当前x轴旋转α度，再沿新的x轴平移a，再绕新的Z轴旋转θ度，再沿新的Z轴平移d
9. 故按照上式定义计算后整理，可以得到基于DH参数的位姿矩阵如下:
10. 注意到IRB120参数中（如下图），首先出现一个KinematicBaseFrames，值不为0，且坐标系绕大地坐标系的Z轴旋转180度，而KinematicBaseFrames和1轴之间数据为0，故此处我们直接用KinematicBaseFrames的值表示坐标系0到坐标系1的变换T01
11. 对后续各轴DH参数整理如下，其中的θ为Base中的θ值
|
αi (twist) |
ai(mm) length |
Θi Rotation |
di(mm) offset |
|
|
1 |
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 |
根据上表，可以获取如下坐标系旋转关系，下图中也标明各Link之间的DH参数关系
上表使用方法：先绕当前x轴旋转α度，再沿新的x轴平移a，再绕新的Z轴旋转θ度，再沿新的Z轴平移d
12. 利用上述关系，进行机器人运动正解计算，代码先获取当前机器人各关节位置，并计算，计算后结果与机器人自身显示相同，证明计算过程正确。
代码如下：
VAR num alpha{6}:=[0,90,0,-90,90,90]; VAR num a{6}:=[0,0,270,70,0,0]; VAR num theta{6}:=[0,90,0,0,-180,0]; VAR num d{6}:=[290,0,0,302,0,72]; !120机器人DH参数 PROC set_matrix(num i,inout num p_mat{*,*}) p_mat:=[[ (cos(theta{i})),- (sin(theta{i})),0, (a{i})], [ (sin(theta{i})*cos(alpha{i})), (cos(theta{i})*cos(alpha{i})),- (sin(alpha{i})),- (sin(alpha{i})*d{i})], [ (sin(theta{i})*sin(alpha{i})), (cos(theta{i})*sin(alpha{i})), (cos(alpha{i})), (cos(alpha{i})*d{i})], [0,0,0,1]]; !将DH参数转化为位姿矩阵 ENDPROC PROC cal_forward2() VAR pose pose1{6}; VAR pose pose2:=[[0,0,0],[1,0,0,0]]; VAR jointtarget jtmp:=[[0,0,0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]]; jtmp:=CJointT(); 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; !获取当前各轴角度 theta:=[180,90,0,0,-180,0]; !dh parameter theta initial value FOR i FROM 1 TO 6 DO theta{i}:=theta{i}+curr_angle{i}; ENDFOR set_matrix 1,no_mat_1; set_matrix 2,no_mat_2; set_matrix 3,no_mat_3; set_matrix 4,no_mat_4; set_matrix 5,no_mat_5; set_matrix 6,no_mat_6; !设定位姿矩阵，将当前角度θ传入 pose1{1}:=MatrixToPose(no_mat_1); pose1{2}:=MatrixToPose(no_mat_2); pose1{3}:=MatrixToPose(no_mat_3); pose1{4}:=MatrixToPose(no_mat_4); pose1{5}:=MatrixToPose(no_mat_5); pose1{6}:=MatrixToPose(no_mat_6); !将位姿矩阵专为Pose数据类型，也可直接对矩阵进行右乘运算 FOR i FROM 1 TO 6 DO pose2:=PoseMult(pose2,pose1{i}); ENDFOR TPWrite ValToStr(pose2); ENDPROC
********************************
如何获取更多经典文章？