cosin / sin 的矩阵数学

Question

我有一个例程，它将接受联合参数 d、theta、a 和 alpha 作为输入，并生成相应的 4x4 齐次矩阵作为输出。我已经测试了我的矩阵乘法，它工作得很好。我将从输入中得到 5 个矩阵，它们将全部相乘，得到 $t^0_5$ 。测试用例位于此处。我的输出与结果完全不同。这是我的代码：

首先，我们将数据输入到使用 DH 参数表< 的 martix 中的矩阵/a>：

 //Kinematics
Matrix44 Matrix44::kinematics(double d, double theta, double a, double alpha)const {
     Matrix44 result;

     double pi = 3.14159;
     double radstheta = theta*(pi/180);
     double radsalpha = alpha*(pi/180);
     //row1
     result.element[0][0] = cos(radstheta);
     result.element[0][3] = -cos(radsalpha)*sin(radstheta);
     result.element[0][4] = sin(radsalpha)*sin(radstheta);
     result.element[0][3] = a*cos(radstheta);
     //row2
     result.element[1][0] = sin(radstheta);
     result.element[1][5] = cos(radsalpha)*cos(radstheta);
     result.element[1][6] = -sin(radsalpha)*cos(radstheta);
     result.element[1][3] = a*sin(radstheta);
     //row3
     result.element[2][0] = 0;
     result.element[2][7] = sin(radsalpha);
     result.element[2][8] = cos(radsalpha);
     result.element[2][3] = d;
     //row4
     result.element[3][0] = 0;
     result.element[3][9] = 0;
     result.element[3][10] = 0;
     result.element[3][3] = 1;

     return result;
}

main中我得到结果的部分，数据来自这张表：

          Matrix44 a,b,c,d,e;
            //in order (d,theta,a,alpha)
    //all data is static and given except for theta which changes, see link for cases
        a = a.kinematics(27.2,0, 0, 90);
        b = b.kinematics(0,0,19.2,180);
        c = c.kinematics(0,0,19.2,0);
        d = d.kinematics(0,0+90,0,90);
        e = e.kinematics(10.5,0,0,0);
       //anyone know how to format this nicely? The operator is overload to print a matrix
//
        cout << left <<setw(20) << a*b*c*d*e;

Theta和alpha 是角度而D和A是距离。

输出/输入的代码：

   //User Input
  istream& operator>> (istream& s, Matrix44& t) {
    for (int i=0; i<4; i++)
      for (int j=0; j<4; j++)
        s >> t.element[i][j];
    if (!s) { cerr << "Error reading Matrix from stream";  exit(0); }
    return s;
  }
  //User Output
  ostream& operator<< (ostream& s, const Matrix44& t) {
    for (int i=0; i<4; i++) {
      for (int j=0; j<4; j++)
        s << t.element[i][j] << "   ";
      s << endl;
    }
    if (!s) { cerr << "Error writing Matrix to stream";  exit(0); }
    return s;
  }

矩阵：

    class Matrix44 {
private:
    double element[4][4];
    friend class Point;
public:
    Matrix44(void);
    Matrix44 transpose(void) const;
    Matrix44 inverse(Matrix44 x) const;
    Matrix44 kinematics(double d, double theta, double a, double alpha) const;
    friend istream& operator>>(istream& s, Matrix44& t);
    friend ostream& operator<<(ostream& s, const Matrix44& t);
    Matrix44 operator *(Matrix44 b);
    Point operator*(const Point & P);
};

Answer 1

有几个问题，一个是您显示的代码中的问题，另一个不是。

问题 1：这可能不是问题的根源，但 double pi = 3.14159; 对于浮点数来说没问题，但对于双精度数则不然。你应该有至少 16 个地方的 pi。更好的是，使用 中的 M_PI，它是许多编译器中的常见扩展。如果您的编译器未定义 M_PI，请使用类似 3.14159265358979323846264338327950288 的内容。

问题 2：您没有显示执行乘法的代码，因此这也可能不是问题的根源。问题在于旋转群不可交换：A*B 不等于 B*A。你必须非常小心这里的乘法约定。示例：拿起一本书，将其放平，使封面朝上，书脊朝左。围绕指向文本行方向的轴将书籍旋转 +90 度。现在围绕从页面底部指向顶部的轴旋转 90 度。您的书的方向应便于您将其放在书架上（书脊垂直，面向您）。现在将书放回原来的方向并重复旋转，但顺序相反。这次你会看到一幅截然不同的画面。

Answer 2

您的索引肯定是错误的吗？而不是

//row1
 result.element[0][0] = cos(radstheta);
 result.element[0][3] = -cos(radsalpha)*sin(radstheta);
 result.element[0][4] = sin(radsalpha)*sin(radstheta);
 result.element[0][3] = a*cos(radstheta);

应该是

//row1
 result.element[0][0] = cos(radstheta);
 result.element[0][1] = -cos(radsalpha)*sin(radstheta);
 result.element[0][2] = sin(radsalpha)*sin(radstheta);
 result.element[0][3] = a*cos(radstheta);