有效四元数角速度

Question

我有一个用四元数表示的方向和一个用四元数或数字表示的角速度（原始方向周围的每秒弧度）。我了解如何使用转换为轴角来做到这一点，但这种方法在计算上相当昂贵，而且不是一个现实的选择。在给定时间间隔（以秒为单位）的情况下，我将如何修改方向四元数？我需要两种情况（四元数和数字）的解决方案。但是，将一种情况转换为另一种情况是可以接受的，并且可能更可取，具体取决于转换所需的各种算法/公式的计算复杂度。

Answer 1

为了更新方向，您需要将当前方向乘以增量旋转。这是与轴角转换相当的昂贵操作。

表示角速度的常用方法是“指数图”，即与旋转轴平行的 3d 矢量和旋转速度的大小（弧度/秒）。转换为 delta 旋转四元数看起来像

Quaternion deltaRotation(const Vector3& em, double deltaTime)
{
   Vector3 ha = em * deltaTime * 0.5; // vector of half angle
   double l = ha.norm(); // magnitude
   if (l > 0) {
      ha *= sin(l) / l;
      return Quaternion(cos(l), ha.x(), ha.y(), ha.z());
   } else {
      return Quaternion(1.0, ha.x(), ha.y(), ha.z());
   }
}

如果您的 deltaTime 较小​​且旋转速度较小，您可以使用 1st Taylor 级数乘数的近似值。但是您应该对结果四元数进行归一化，以避免更频繁地出现数值不稳定。

Quaternion deltaRotationAppx1(const Vector3& em, double deltaTime)
{
   Vector3 ha = em * deltaTime * 0.5; // vector of half angle
   return Quaternion(1.0, ha.x(), ha.y(), ha.z());
}

Answer 2

这是与 Minorlogic 的答案中给出的相同的一阶泰勒级数方法（使用 JOML API）：

public static Quaterniond integrateAngularVelocity(
        double avx, double avy, double avz, double dt) {
    double len = Math.sqrt(avx * avx + avy * avy + avz * avz);
    double theta = len * dt * 0.5;
    if (len > 1.0e-12) {
        double w = Math.cos(theta);
        double s = Math.sin(theta) / len;
        return new Quaterniond(avx * s, avy * s, avz * s, w);
    } else {
        return new Quaterniond(0, 0, 0, 1);
    }
}

这个方法的起源和意义在赵法成和Berend van Wachem（2013）的A novel Quaternion integration approach for describing the behaviour of non-spherical particles的背景部分中进行了解释，并在第4.5节（时间导数）和4.6节（旋转的时间积分）中进行了详细说明率）的优秀Quaternion kinematics for the error-state Kalman filter 由 Joan Solà（2017 年）。

这只是角速度时间积分的一阶泰勒级数展开。 Solà 的 Eq 227 给出了二阶校正：

double avgAVX = 0.5 * (avPrev.x + avCurr.x);
double avgAVY = 0.5 * (avPrev.y + avCurr.y);
double avgAVZ = 0.5 * (avPrev.z + avCurr.z);
Quaterniond integral = IMUIntegration.integrateAngularVelocity(
        avgAVX, avgAVY, avgAVZ, dt);
Vector3d correction = avPrev.cross(avCurr, new Vector3d()).mul(dt * dt / 24.0);
Quaterniond qCorrection = new Quaterniond(
        correction.x, correction.y, correction.z, 0.0);
Quaterniond deltaOrientation = integral.add(qCorrection).normalize();

然而，最后一步中的四元数加法没有有意义的几何解释，并且增加了数值误差超过了离散时间步积分引入的误差。因此，最好使用仅乘法，作为预测器-校正器引入 Zhao 和 Wachem 中的直接乘法 (PCDM) 方法。

L. J. H. Seelen、J. T. Padding 和 J. A. M. Kuipers (2016) 在 Improved quaternion-based integration scheme for rigid body motion 中改进了此方法。如果我读的数学正确，如果你通过假设没有外部扭矩或力来简化，如果你抛弃线速度分量只关注角速度和旋转，如果你只在局部坐标而不是而不是在局部坐标和世界坐标之间来回切换（作者这样做是为了添加外部扭矩、速度和力的贡献），那么您最终会得到一种简单地整合前一个和当前 IMU 之间的角速度的方法读数，然后在当前和下一个 IMU 读数之间，沿这两个插值以任何所需的分辨率进行采样，并应用上面显示的 integrateAngularVelocity 例程，并使用积分采样间隔引起的较小的 dt 值：

private static final int NUM_INTEGRATION_STEPS = 6; // Should be even

public static List<IMUIntegrated> integrateAngularVelocities(List<IMURaw> raw,
            double dt) {
    List<IMUIntegrated> integrated = new ArrayList<>();
    for (int i = 0; i < raw.size(); i++) {
        // Find average between curr and prev/next angular velocities
        IMURaw prevRawAV = i == 0 ? null : raw.get(i - 1);
        IMURaw currRawAV = raw.get(i);
        IMURaw nextRawAV = i == raw.size() - 1 ? null 
                : raw.get(i + 1);
        Vector3d prevAV = prevRawAV == null ? new Vector3d()
                : prevRawAV.getAngularVelocity();
        Vector3d currAV = currRawAV.getAngularVelocity();
        Vector3d nextAV = nextRawAV == null ? new Vector3d()
                : nextRawAV.getAngularVelocity();
        Vector3d prevAvgAV = prevAV.add(currAV, new Vector3d()).mul(0.5);
        Vector3d nextAvgAV = currAV.add(nextAV, new Vector3d()).mul(0.5);

        // Find integration interval
        double integrationDt = dt / NUM_INTEGRATION_STEPS;

        // Linearly interpolate and integrate angular velocities
        Quaterniond deltaOrientation = new Quaterniond();
        for (int j = 0; j <= NUM_INTEGRATION_STEPS; j++) {
            double frac;
            Vector3d startAV, endAV;
            if (j < NUM_INTEGRATION_STEPS / 2) {
                frac = (j / (double) (NUM_INTEGRATION_STEPS / 2));
                startAV = prevAvgAV;
                endAV = currAV;
            } else {
                frac = ((j - NUM_INTEGRATION_STEPS / 2)
                        / (double) (NUM_INTEGRATION_STEPS / 2));
                startAV = currAV;
                endAV = nextAvgAV;
            }
            // Linearly interpolate angular velocities
            Vector3d interpolatedAV = startAV.mul(1.0 - frac, new Vector3d())
                    .add(endAV.mul(frac, new Vector3d()));
            // Integrate to produce a quaternion
            deltaOrientation = integrateAngularVelocity(
                    interpolatedAV.x, interpolatedAV.y, interpolatedAV.z,
                    integrationDt)
                // Concatenate onto cumulative transformation
                .mul(deltaOrientation);
        }
        integrated.add(new IMUIntegrated(currRawAV.timestamp,
                deltaOrientation.normalize()));
    }
    return integrated;
}