机械臂运动学正解验证

将自己最近所了解的机械臂运动学正解验证知识总结，供大家参考。

以遨博I5机械臂为例，使用改进的Dh参数，在matlab机器人工具箱中进行验证，最后将变换矩阵T转为位置和姿态(欧拉角)。

验证环境：

matlab2020b(ubuntu18.04)

Eigen库将矩阵转为欧拉角

1、auboI5改进Dh参数

2、matlab验证程序及结果

clc


d1 = 0.0985
d2 = 0.1215
a2 = 0.408
a3 = 0.376
d5 = 0.1025
d6 = 0.094




L(1)=Link([0,d1,0,0,0,pi],       'modified')
L(2)=Link([0,d2,0,-pi/2,0,-pi/2],  'modified')
L(3)=Link([0,0,a2,pi],           'modified')
L(4)=Link([0,0,a3,pi,0,-pi/2],     'modified')
L(5)=Link([0,d5,0,-pi/2],        'modified')
L(6)=Link([0,d6,0,pi/2],         'modified')




robot =SerialLink(L,'name','aubo_i5')
title("T")


T = robot.fkine([0 0 0 0 0 0])
robot.plot([0 0 0 0 0 0])

左上角3*3矩阵表示姿态，红框3*1依次是X Y Z。

3、aubo机械臂位姿

对上了~

4、使用Eigen将姿态矩阵转为欧拉角

程序如下：

const double ARC_TO_DEG = 57.29577951308238;
const double DEG_TO_ARC = 0.0174532925199433;


bool isRotationMatirx(Eigen::Matrix3d R)
{
    double err=1e-6;
    Eigen::Matrix3d shouldIdenity;
    shouldIdenity=R*R.transpose();
    Eigen::Matrix3d I=Eigen::Matrix3d::Identity();
    return (shouldIdenity - I).norm() < err;
}




Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R)
{
    assert(isRotationMatirx(R));
    double sy = sqrt(R(0,0) * R(0,0) + R(1,0) * R(1,0));
    bool singular = sy < 1e-6;
    double x, y, z;
    if (!singular)
    {
        x = atan2( R(2,1), R(2,2));
        y = atan2(-R(2,0), sy);
        z = atan2( R(1,0), R(0,0));
    }
    else
    {
        x = atan2(-R(1,2), R(1,1));
        y = atan2(-R(2,0), sy);
        z = 0;
    }
    return {x, y, z};
}


int main(int argc, char *argv[])
{
    // 设定欧拉角(角度)，绕固定轴
    double roll_deg = 90.0f;    // 绕X轴
    double pitch_deg = 0.0f;     // 绕Y轴
    double yaw_deg = 0.0f;     // 绕Z轴


    //        double roll_deg = 178.340073;      // 绕X轴
    //        double pitch_deg = 1.419545;     // 绕Y轴
    //        double yaw_deg = 133.484192;     // 绕Z轴


    // 转化为弧度
    double roll_arc = roll_deg * DEG_TO_ARC;    // 绕X轴
    double pitch_arc = pitch_deg * DEG_TO_ARC;  // 绕Y轴
    double yaw_arc = yaw_deg * DEG_TO_ARC;      // 绕Z轴


    cout << endl;
    cout << "roll_arc = " << roll_arc << endl;
    cout << "pitch_arc = " << pitch_arc << endl;
    cout << "yaw_arc = " << yaw_arc << endl;


    // 初始化欧拉角（rpy）,对应绕x轴，绕y轴，绕z轴的旋转角度
    Eigen::Vector3d euler_angle(roll_arc, pitch_arc, yaw_arc);


    // 使用Eigen库将欧拉角转换为旋转矩阵
    Eigen::Matrix3d rotation_matrix1;


    rotation_matrix1 = 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 << "\nrotation matrix1 =\n" << rotation_matrix1 << endl << endl;




    // 使用自定义函数将旋转矩阵转换为欧拉角
    Eigen::Vector3d eulerAngle2 = rotationMatrixToEulerAngles(rotation_matrix1); // roll,pitch,yaw
    cout << "roll_2 pitch_2 yaw_2 = " << eulerAngle2[0] << " " << eulerAngle2[1]
         << " " << eulerAngle2[2] << endl << endl;
}

结果如下：

最后：参考文章

Ubuntu下安装matlab: https://zhuanlan.zhihu.com/p/429084566

机械臂改进型DH参数：https://blog.csdn.net/ACE_YOUNG/article/details/125595485

Matlab验证：https://blog.csdn.net/qq_31253399/article/details/106188637

Matlab内部有直接姿态矩阵转欧拉角的，奈何ubuntu虚拟机空间不够，开始时没装航空工具箱。。。

原理推导等有机会再详细总结。

欢迎关注公众号：

如需完整程序可后台留言"机械臂运动学正解验证"。