使用D-H矩阵计算正向运动学

时间:2017-10-30 09:30:52

标签: robot kinematics

我有一个6自由度机器人手臂模型:

robot arm structure

我想计算正向运动学,所以我使用D-H矩阵。 D-H参数是:

static const std::vector<float> theta = {
0,0,90.0f,0,-90.0f,0};

// d
static const std::vector<float> d = {
380.948f,0,0,-560.18f,0,0};

// a
static const std::vector<float> a = {
-220.0f,522.331f,80.0f,0,0,94.77f};

// alpha
static const std::vector<float> alpha = {
90.0f,0,90.0f,-90.0f,-90.0f,0};

和计算:

glm::mat4 Robothand::armForKinematics() noexcept
{
    glm::mat4 pose(1.0f);
    float cos_theta, sin_theta, cos_alpha, sin_alpha;

    for (auto i = 0; i < 6;i++)
    {
        cos_theta = cosf(glm::radians(theta[i]));
        sin_theta = sinf(glm::radians(theta[i]));
        cos_alpha = cosf(glm::radians(alpha[i]));
        sin_alpha = sinf(glm::radians(alpha[i]));

        glm::mat4 Ai = {
cos_theta, -sin_theta * cos_alpha,sin_theta * sin_alpha, a[i] * cos_theta,      
sin_theta, cos_theta * cos_alpha, -cos_theta * sin_alpha,a[i] * sin_theta,
0,         sin_alpha,             cos_alpha,             d[i],
0,         0,                     0,                     1 };

    pose = pose * Ai;
    }

return pose;
}

我遇到的问题是,我无法得到正确的结果,例如,我想计算从第一关节到第四关节的转换矩阵,我将改变for循环i&lt; 3,然后我可以得到姿势矩阵,我可以通过姿势*(0,0,0,1)在第4坐标系中进行原点坐标。但结果(380.948,382.331,0)似乎不正确,因为它应该是沿x轴移动而不是y轴。我已经阅读了很多关于D-H矩阵的书籍和材料,但我无法弄清楚它有什么问题。

1 个答案:

答案 0 :(得分:0)

我自己已经弄明白了,背后的真正问题是glm :: mat,glm :: mat是col-type,这意味着列将在行之前初始化,我更改了代码并获得了正确的结果:

for (int i = 0; i < joint_num; ++i)
{
    pose = glm::rotate(pose, glm::radians(degrees[i]), glm::vec3(0, 0, 1));
    pose = glm::translate(pose,glm::vec3(0,0,d[i]));
    pose = glm::translate(pose, glm::vec3(a[i], 0, 0));
    pose = glm::rotate(pose,glm::radians(alpha[i]),glm::vec3(1,0,0));
}
然后我可以通过以下方式得到这个位置:

auto pos = pose * glm::vec4(x,y,z,1);