5_机械臂工具位姿计算理论及代码实现验证

1、机械臂工具位姿计算理论

机器人的首要功能之一是能够计算它所持的夹具(或未夹持夹具)相对于规范坐标系的位姿，也就是说需要计算工具坐标系{T}相对于工作台坐标系{S}的变换矩阵。只要通过运动学方程计算出

，就可以应用第二章所述的笛卡尔变换计算{T}相对于{S}的变换矩阵。求解一个简单的变换方程，得出：

方程3-18在某些机器人系统中称为WHERE函数，用它可计算手臂的位置。对于图3-28的情况，WHERE的输出是轴销相对于工作台顶角处的位姿。

方程3-18是广义运动学方程。根据连杆的几何形状，由基座(可看成一个固定连杆)的广义变换矩阵(

)和另一端的执行器坐标变换矩阵(

)可以计算运动学方程。这些附加的变换可以包括工具的偏距和转角，且适用于任意固定坐标系。

2、AUBOPE I5机械臂仅添加工具位置数据

I5机械臂正解计算结果：

3、只有工具位置的计算

以mm为单位。

根据笔记《3_机械臂位姿变换计算过程代码》，如果已知工具点相对于腕部坐标系{W}(法兰中心)的位姿，通过pos_trans可直接计算出工具点相对于基坐标系{B}的位姿，本次不使用该方法。而是根据正解的计算结果和工具点相对于腕部坐标系{W}(法兰中心)的位姿去计算。

以之前i5机械臂正解计算为基础，新添加代码如下：

代码语言：javascript

复制

double j[6] = {-4.670867*DEG_TO_ARC, 33.841753*DEG_TO_ARC, 146.829943*DEG_TO_ARC, 25.152904*DEG_TO_ARC, 90.289842*DEG_TO_ARC, -48.170107*DEG_TO_ARC};  
Eigen::Vector3d euler_angle(10*DEG_TO_ARC, 20*DEG_TO_ARC, 30*DEG_TO_ARC);  
// 使用Eigen库将欧拉角转换为旋转矩阵  
    Eigen::Matrix3d R;  
    R = Eigen::AngleAxisd(euler_angle[2], Eigen::Vector3d::UnitZ()) *  
        Eigen::AngleAxisd(euler_angle[1], Eigen::Vector3d::UnitY()) *    
        Eigen::AngleAxisd(euler_angle[0], Eigen::Vector3d::UnitX());
cout &lt;&lt; &#34;R is :\n&#34; &lt;&lt; R &lt;&lt; std::endl;  

Eigen::MatrixXd tool_pos(3,1);  
tool_pos(0,0)= 10;  
tool_pos(1,0)= 20;  
tool_pos(2,0)= 30;  

Eigen::MatrixXd homogeneous(1,4);  
homogeneous(0,0)=0;  
homogeneous(0,1)=0;  
homogeneous(0,2)=0;  
homogeneous(0,3)=1;  
Eigen::Matrix     &lt;double,            &lt;/double,3, 4&gt; m3x4;    
Eigen::Matrix     &lt;double,            &lt;/double,4, 4&gt; m4x4;    
m3x4 &lt;&lt; R, tool_pos;  
cout &lt;&lt; &#34;m3x4 is :\n&#34; &lt;&lt; m3x4 &lt;&lt; std::endl;  
m4x4 &lt;&lt; m3x4, homogeneous;  
cout &lt;&lt; &#34;m4x4 is: \n&#34; &lt;&lt; m4x4 &lt;&lt; std::endl;  

Eigen::Matrix4d tool_pose;

tool_pose = T06*m4x4;

cout << "tool_pose is: \n" << tool_pose << std::endl;

计算结果如下：

4、工具有位置和姿态变换的计算