Question

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

Answer 1

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

表示角速度的常用方法是指数映射＆＃34;平行于旋转轴的3d矢量和旋转速度的大小（每秒弧度）。转换为delta旋转四元数看起来像

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

   return q;
}

如果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);
    }
}

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

这只是角速度时间积分的一阶泰勒级数展开。 Solà的等式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值：< / p>

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;
}

有效的四元数角速度

2 个答案: