Question

我有两个旋转表示为偏航，俯仰，滚动（Tait-Brian内在的右手）。构建一个等效于它们的单个旋转的推荐方法是什么？编辑：如果我从答案中理解正确，我必须首先将偏航，俯仰，滚动转换为矩阵或四元数，组合它们然后将结果转换回偏航，俯仰，滚动表示。此外，我的首要任务是简单性，然后是数值稳定性和效率。谢谢:)）

Answer 1

作为一般答案，如果为两个旋转中的每个旋转制作旋转矩阵，则可以制作单个矩阵，这是两者的乘积（顺序很重要！），以表示应用两个旋转的效果。

可以设想“万向节锁定”可能使某些角度（通常涉及非常接近90度的角度）在数值上不稳定的情况。

使用四元数更快更稳定。你可以在http://www.genesis3d.com/~kdtop/Quaternions-UsingToRepresentRotation.htm看到一个很好的处理 - 总之，每个旋转都可以用四元数表示，多个旋转只用四元数的乘积来表示。它们往往具有更好的稳定性。

可以在http://en.m.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

找到执行此操作的公式

UPDATE 使用http://planning.cs.uiuc.edu/node102.html提供的公式，您可以调整以下代码来执行一系列旋转。虽然代码是用C ++编写的（并编译成），但我没有利用某些内置的C ++类型和方法来使这段代码更加优雅 - 在这里展示我的C根。重点是展示旋转方程如何工作，以及如何连接多个旋转。

两个关键函数是calcRot，它计算给定偏航，俯仰和滚转的旋转矩阵;和mMult将两个矩阵相乘。当你有两个连续的旋转时，它们的旋转矩阵的乘积是“复合”旋转 - 你必须注意你做事的顺序。我使用的例子显示了这一点。首先，我通过两个单独的旋转来旋转矢量;然后我计算一个结合两个旋转并得到相同结果的单个矩阵;最后我颠倒了旋转的顺序，得到了不同的结果。所有这些都可以帮助您解决问题。

确保我使用的约定对你有意义。

#include <iostream>
#include <cmath>

#define PI (2.0*acos(0.0))
//#define DEBUG

void calcRot(double ypr[3], double M[3][3]) {
// extrinsic rotations: using the world frame of reference
// ypr: yaw, pitch, roll in radians
  double cy, sy, cp, sp, cr, sr;
// compute sin and cos of each just once:
  cy = cos(ypr[0]);
  sy = sin(ypr[0]);
  cp = cos(ypr[1]);
  sp = sin(ypr[1]);
  cr = cos(ypr[2]);
  sr = sin(ypr[2]);

// compute this rotation matrix:
// source: http://planning.cs.uiuc.edu/node102.html
  M[0][0] =  cy*cp;
  M[0][1] =  cy*sp*sr - sy*cr;
  M[0][2] =  cy*sp*cr + sy*sr;
  M[1][0] =  sy*cp;
  M[1][1] =  sy*sp*sr + cy*cr;
  M[1][2] =  sy*sp*sr - cy*sr;
  M[2][0] = -sp;
  M[2][1] =  cp*sr;
  M[2][2] =  cp*cr;
}

void mMult(double M[3][3], double R[3][3]) {
// multiply M * R, returning result in M
  double T[3][3] = {0};
  for(int ii = 0; ii < 3; ii++) {
    for(int jj = 0; jj < 3; jj++) {
      for(int kk = 0; kk < 3; kk++ ) {
        T[ii][jj] += M[ii][kk] * R[kk][jj];
      }
    }
  }
  // copy the result:
  for(int ii = 0; ii < 3; ii++) {
    for(int jj = 0; jj < 3; jj++ ) {
      M[ii][jj] = T[ii][jj];
    }
  }
}

void printRotMat(double M[3][3]) {
// print 3x3 matrix - for debug purposes
#ifdef DEBUG
  std::cout << "rotation matrix is: " << std::endl;
  for(int ii = 0; ii < 3; ii++) {
    for(int jj = 0; jj < 3; jj++ ) {
      std::cout << M[ii][jj] << " ";
    }
    std::cout << std::endl;
  }
  std::cout << std::endl;
#endif
}

void applyRot(double before[3], double after[3], double M[3][3]) {
// apply rotation matrix M to vector 'before'
// returning result in vector 'after'
  double sumBefore = 0, sumAfter = 0;
  std::cout << "Result of rotation:" << std::endl;
  for(int ii = 0; ii < 3; ii++) {
    std::cout << before[ii] << " -> ";
    sumBefore += before[ii] * before[ii];
    after[ii] = 0;
    for( int jj = 0; jj < 3; jj++) {
      after[ii] += M[ii][jj]*before[jj];
    }
    sumAfter += after[ii] * after[ii];
    std::cout << after[ii] << std::endl;
  }
  std::cout << std::endl;
  #ifdef DEBUG
    std::cout << "length before: " << sqrt(sumBefore) << "; after: " << sqrt(sumAfter) << std::endl;
  #endif
}

int main(void) {
  double r1[3] = {0, 0, PI/2}; // order: yaw, pitch, roll
  double r2[3] = {0, PI/2, 0};

  double initPoint[3] = {3,4,5}; // initial point before rotation
  double rotPoint[3], rotPoint2[3];

  // initialize rotation matrix to I
  double R[3][3];
  double R2[3][3];

  // compute first rotation matrix in-place:
  calcRot(r1, R);
  printRotMat(R);
  applyRot(initPoint, rotPoint, R);
  // apply second rotation on top of first:
  calcRot(r2, R2);
  std::cout << std::endl << "second rotation matrix: " << std::endl;
  printRotMat(R2);
  // applying second matrix to result of first rotation:
  std::cout << std::endl << "applying just the second matrix to result of first: " << std::endl;
  applyRot(rotPoint, rotPoint2, R2);
  mMult(R2, R);
  std::cout << "after multiplication: " << std::endl;
  printRotMat(R2);
  std::cout << "Applying the combined matrix to the intial vector: " << std::endl;
  applyRot(initPoint, rotPoint2, R2);

  // now in the opposite order:
  double S[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
  calcRot(r2, S);
  printRotMat(S);
  calcRot(r1, R2);
  mMult(R2, S);
  std::cout << "applying rotation in the opposite order: " << std::endl;
  printRotMat(R2);
  applyRot(initPoint, rotPoint, R2);

}

输出（#DEBUG未定义 - 已注释掉）：

Result of rotation:
3 -> 3
4 -> -5
5 -> 4


second rotation matrix: 

applying just the second matrix to result of first: 
Result of rotation:
3 -> 4
-5 -> -5
4 -> -3

after multiplication: 
Applying the combined matrix to the intial vector: 
Result of rotation:
3 -> 4
4 -> -5
5 -> -3

请注意，最后两个给出相同的结果，表明您可以组合旋转矩阵。

applying rotation in the opposite order: 
Result of rotation:
3 -> 5
4 -> 3
5 -> 4

现在结果不同 - 顺序很重要。

Answer 2

如果您熟悉矩阵运算，可以尝试Rodrigues' rotation formula。如果您熟悉四元数，则可以尝试P'= q * P * q'方法。

Quaterion数学有点复杂，但代码更简单，更快。

推荐的旋转组合方式

2 个答案: