使用四元数代替滚动、俯仰和偏航来跟踪设备运动

Question

请容忍我的冗长问题，我尽可能清楚明了地表达。

我的目标是，在使用相机拍照时获取姿态（横滚、俯仰和偏航），然后将姿态值保存到nsuserdefaults中。保存后，手机方向发生改变，然后通过不断比较姿态值（保存的和当前的）来尝试将手机带回拍照时的相同姿态。

为了实现界面功能，用户界面上有3个点（分别对应每个姿态参数），指导正确的照片取向。当达到正确的姿态时，屏幕上会显示匹配标志。

我一直在寻找横滚、俯仰和偏航值：

CMQuaternion quat = self.motionManager.deviceMotion.attitude.quaternion;
myRoll = radiansToDegrees(atan2(2*(quat.y*quat.w - quat.x*quat.z), 1 - 2*quat.y*quat.y - 2*quat.z*quat.z)) ;
myPitch = radiansToDegrees(atan2(2*(quat.x*quat.w + quat.y*quat.z), 1 - 2*quat.x*quat.x - 2*quat.z*quat.z));
myYaw = radiansToDegrees(2*(quat.x*quat.y + quat.w*quat.z));

当我发现偏航值存在差异时，我在这里找到了相关信息：链接。引用如下：

如果你使用四元数来计算偏航、俯仰和滚转角度，你将面临与仅使用偏航、俯仰和滚转角度相同的问题。你必须在代码中处处使用四元数，并忘记偏航、俯仰和滚转角度。

所以现在我想我需要重新编写所有代码...请问您能否提供一个使用四元数的示例代码？

以下是我正在处理的代码：

在ViewController.m文件中，image:didFinishSavingWithError:函数内部：

[self.motionManager startDeviceMotionUpdatesToQueue:[NSOperationQueue mainQueue] withHandler:^(CMDeviceMotion *motion, NSError *error) {

        CMQuaternion quat = self.motionManager.deviceMotion.attitude.quaternion;
        double tempYaw = radiansToDegrees(asin(2*(quat.x*quat.y + quat.w*quat.z)));
        double tempRoll = radiansToDegrees(atan2(2*(quat.y*quat.w - quat.x*quat.z), 1 - 2*quat.y*quat.y - 2*quat.z*quat.z)) ;
        double tempPitch = radiansToDegrees(atan2(2*(quat.x*quat.w + quat.y*quat.z), 1 - 2*quat.x*quat.x - 2*quat.z*quat.z));

        if (savingGyroOrientation == YES) {

            NSLog(@"Roll = %f degrees",tempRoll);
            NSLog(@"Pitch = %f degrees",tempPitch);
            NSLog(@"Yaw = %f degrees",tempYaw);

            [self.deviceStatus setDouble:tempRoll forKey:@"DeviceRoll"];
            [self.deviceStatus setDouble:tempPitch forKey:@"DevicePitch"];
            [self.deviceStatus setDouble:tempYaw forKey:@"DeviceYaw"];
            [self.deviceStatus synchronize];

            savingGyroOrientation = NO;
            checkingGyroOrientation = YES;
            self.savingLabel.hidden = YES;
            self.startTimerButton.hidden = NO;

        }
        savingGyroOrientation = NO;
        checkingGyroOrientation = YES;
        self.savingLabel.hidden = YES;
        self.startTimerButton.hidden = NO;
    }

     if (timerRunning == YES) {
         if (checkingGyroOrientation == YES) {

             self.takePicButton.hidden = YES;

             int xRoll, yPitch, xYaw, yYaw;
             // Roll Checking
             if (tempRoll >= [self.deviceStatus doubleForKey:@"DeviceRoll"]-1 && tempRoll <= [self.deviceStatus doubleForKey:@"DeviceRoll"]+1 ) {

                 [self.rollDot setFrame:CGRectMake(150, 195, 20, 20)];
                 self.rollToR.hidden = YES;
                 self.rollToL.hidden = YES;
                 self.rollDot.hidden = NO;
                 rollOk = YES;
             }else{
                 rollOk = NO;
                 self.rollDot.hidden = YES;
                 self.rollToR.hidden = NO;
                 self.rollToL.hidden = NO;

                 if (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"] < 0) {
                     xRoll = 150 + (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"]);
                     self.rollToR.hidden = YES;
                     if (xRoll <= 0) {
                         [self.rollToL setFrame:CGRectMake(0, 195, 20, 20)];
                     }else if (xRoll>= 300){
                         [self.rollToL setFrame:CGRectMake(300, 195, 20, 20)];
                     }else{
                         [self.rollToL setFrame:CGRectMake(xRoll, 195, 20, 20)];
                     }
                 }
                 if (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"] > 0){
                     xRoll = 150 + (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"]);
                     self.rollToL.hidden = YES;
                     if (xRoll <= 0) {
                         [self.rollToR setFrame:CGRectMake(0, 195, 20, 20)];
                     }else if (xRoll>=300){
                         [self.rollToR setFrame:CGRectMake(300, 195, 20, 20)];
                     }else{
                         [self.rollToR setFrame:CGRectMake(xRoll, 195, 20, 20)];
                     }
                 }
                 if (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"] > 180){
                     xRoll = 150 + (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"]-360);
                     self.rollToR.hidden = YES;
                     if (xRoll <= 0) {
                         [self.rollToL setFrame:CGRectMake(0, 195, 20, 20)];
                     }else if (xRoll>=300){
                         [self.rollToL setFrame:CGRectMake(300, 195, 20, 20)];
                     }else{
                         [self.rollToL setFrame:CGRectMake(xRoll, 195, 20, 20)];
                     }
                 }
                 if (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"] < -180){
                     xRoll = 150 + (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"]+360);
                     self.rollToL.hidden = YES;
                     if (xRoll <= 0) {
                         [self.rollToR setFrame:CGRectMake(0, 195, 20, 20)];
                     }else if (xRoll >= 300){
                         [self.rollToR setFrame:CGRectMake(300, 195, 20, 20)];
                     }else{
                         [self.rollToR setFrame:CGRectMake(xRoll, 195, 20, 20)];
                     }
                 }
             }
             //Pitch Checking
             if (tempPitch >= [self.deviceStatus doubleForKey:@"DevicePitch"]-1 && tempPitch <= [self.deviceStatus doubleForKey:@"DevicePitch"]+1) {
                 [self.pitchDot setFrame:CGRectMake(150, 195, 20, 20)];
                 self.pitchDot.hidden = NO;
                 self.pitchToDown.hidden = YES;
                 self.pitchToUp.hidden = YES;
                 pitchOk = YES;
             }else{
                 pitchOk = NO;
                 self.pitchDot.hidden = YES;
                 self.pitchToDown.hidden = NO;
                 self.pitchToUp.hidden = NO;
                 if (tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"] < 0) {
                     yPitch = 195+(tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"]);
                     //                         NSLog(@"tempPitch is %0.02f Difference is %0.02f, yPitch is %0.02f",tempPitch, tempPitch-[self.deviceStatus doubleForKey:@"DevicePitch"],195+(tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"]));
                     self.pitchToDown.hidden = YES;
                     if (yPitch <= 0) {
                         [self.pitchToUp setFrame:CGRectMake(150, 0, 20, 20)];
                     }else if (yPitch >= 390) {
                         [self.pitchToUp setFrame:CGRectMake(150, 390, 20, 20)];
                     }else{
                         [self.pitchToUp setFrame:CGRectMake(150, yPitch, 20, 20)];
                     }
                 }
                 if (tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"] > 0){
                     yPitch = 195+(tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"]);
                     //                         NSLog(@"tempPitch is %0.02f Difference is %0.02f, yPitch is %0.02f",tempPitch, tempPitch-[self.deviceStatus doubleForKey:@"DevicePitch"],195+(tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"]));
                     self.pitchToUp.hidden = YES;
                     if (yPitch <= 0) {
                         [self.pitchToDown setFrame:CGRectMake(150, 0, 20, 20)];
                     }else if (yPitch >= 390) {
                         [self.pitchToDown setFrame:CGRectMake(150, 390, 20, 20)];
                     }else{
                         [self.pitchToDown setFrame:CGRectMake(150, yPitch, 20, 20)];
                     }
                 }
                 if (tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"] < -180){
                     yPitch = 195+tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"] + 360;
                     //                         NSLog(@"tempPitch is %0.02f Difference is %0.02f, yPitch is %0.02f",tempPitch, tempPitch-[self.deviceStatus doubleForKey:@"DevicePitch"],195+(tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"]));
                     //                         NSLog(@"*yPitch is %d",yPitch);
                     self.pitchToUp.hidden = YES;
                     self.pitchToDown.hidden = NO;
                     if (yPitch <= 0 ) {
                         [self.pitchToDown setFrame:CGRectMake(150, 0, 20, 20)];
                     }else if (yPitch >= 390) {
                         [self.pitchToDown setFrame:CGRectMake(150, 390, 20, 20)];
                     }else{
                         [self.pitchToDown setFrame:CGRectMake(150, yPitch, 20, 20)];
                     }
                 }
             }
             if (tempYaw >= [self.deviceStatus doubleForKey:@"DeviceYaw"]-2 && tempYaw <= [self.deviceStatus doubleForKey:@"DeviceYaw"]+2) {

                 [self.yawDot setFrame:CGRectMake(150, 195, 20, 20)];
                 self.yawDot.hidden = NO;
                 self.rotateRight.hidden = YES;
                 self.rotateLeft.hidden = YES;
                 yawOk = YES;
             }else{
                 yawOk = NO;
                 self.yawDot.hidden = YES;
                 self.rotateRight.hidden = NO;
                 self.rotateLeft.hidden = NO;

                 if (tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"] < 0 ) {
                     xYaw = 150+(tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     yYaw = 195-1.3*(tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     NSLog(@"current yaw is %0.02f Difference is %0.02f",tempYaw, tempYaw-[self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     NSLog(@"saved Yaw is %0.02f",[self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     NSLog(@"xYaw is %d, yYaw is %d",xYaw,yYaw);
                     self.rotateRight.hidden = YES;
                     if (xYaw <=0 && yYaw >=390) {
                         [self.rotateLeft setFrame:CGRectMake(0, 390, 20, 20)];
                     }else{
                         [self.rotateLeft setFrame:CGRectMake(xYaw, yYaw, 20, 20)];
                     }

                 }if (tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"] > 0){
                     xYaw = 150+(tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     yYaw = 195-1.3*(tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     NSLog(@"current yaw is %0.02f Difference is %0.02f",tempYaw, tempYaw-[self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     NSLog(@"saved Yaw is %0.02f",[self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     NSLog(@"*xYaw is %d, yYaw is %d",xYaw,yYaw);
                     self.rotateLeft.hidden = YES;
                     if (xYaw >=300 && yYaw <=0) {
                         [self.rotateRight setFrame:CGRectMake(300, 0, 20, 20)];
                     }else{
                         [self.rotateRight setFrame:CGRectMake(xYaw, yYaw, 20, 20)];
                     }
                 }
             }

             if (rollOk == YES && pitchOk == YES && yawOk ==YES) {
                 self.orientationOkay.hidden = NO;
                 self.centerCircle.hidden = YES;
                 self.rollDot.hidden = YES;
                 self.pitchDot .hidden =YES;
                 self.yawDot.hidden = YES;
                 [self.clickTimer invalidate];
                 self.clickTimer = nil;
                 self.takePicButton.hidden = NO;
                 timerRunning = NO;
                 [self.motionManager stopDeviceMotionUpdates];
                 [self.deviceStatus removeObjectForKey:@"DeviceRoll"];
                 [self.deviceStatus removeObjectForKey:@"DevicePitch"];
                 [self.deviceStatus removeObjectForKey:@"DeviceYaw"];
                 [self.deviceStatus removeObjectForKey:@"DeviceAngle"];

             }else{

                 self.orientationOkay.hidden = YES;
                 if (flagger == YES) {
                     self.centerCircle.hidden = NO;
                 }
                 else{
                     self.centerCircle.hidden = YES;
                 }
             }
         }
     }else{
         self.rotateRight.hidden = YES;
         self.rotateLeft.hidden = YES;
         self.rollToL.hidden = YES;
         self.rollToR.hidden = YES;
         self.pitchToDown.hidden = YES;
         self.pitchToUp.hidden = YES;
         self.rollDot.hidden = NO;
         self.pitchDot .hidden =NO;
         self.yawDot.hidden = NO;
         [self.yawDot setFrame:CGRectMake(0, 390, 20, 20)];
         [self.rollDot setFrame:CGRectMake(0, 195, 20, 20)];
         [self.pitchDot setFrame:CGRectMake(150, 0, 20, 20)];

     }
     }];

如果需要进一步的细节，请告诉我。

欢迎提出任何建议或意见，:) 我是一个编程和ios的新手。

谢谢！

- iSeeker

---

只是想让您知道，在您的四元数定义中，Roll 和 Yaw 被交换了。Roll 的公式是为 Yaw 设计的，而 Yaw 的公式是为 Roll 设计的。 - inorganik

Answer 1

我认为以下事项对于任务管理是必要的：

1. 首先，你需要对四元数有很好的理解（如果你已经掌握了它们，可以跳过这一点）。我推荐阅读 OpenGL:Tutorials:Using Quaternions to represent rotation 或者 The Matrix and Quaternions FAQ。请记住，(x, y, z) 表示旋转的轴（未标准化），w = cos (alpha/2)，大致表示旋转量。

2. 由于 CMQuaternion 只是一个结构体，所以进行所有计算很困难。最好使用全功能的四元数类，例如 cocoamath（您至少需要从 trunk 下载 Quaternion.h、.m 和 QuaternionOperation.m）。

3. 现在考虑基本问题：

   1. 定义两个四元数之间的差异（或者有时称为除法）作为从一个方向到另一个方向的角位移可能是正确的方法。它被定义为
      d = a^-1 * b
      因此，这表达了当前位置到目标位置的差异。

   2. 有了这个差异，您需要定义达到目标方向的条件。我的第一个想法是使用差异角度。可以通过上述计算出的 d 四元数的 w 分量轻松获取：
      alpha = 2 * arccos (w)
      arccos 的定义域受限制，但在这种情况下不应该是问题，因为我们特别关注小值。

也许值得强调的是，每个三维旋转都有两个单位四元数表示，q 和 -q。这可能会令人困惑，但并不重要。

---

更新: 因此，伪代码可能如下所示：

CMQuaternion cmQ = attitude.quaternion;
// Get an instance of cocoamath's Quaternion for our currently reported quaternion
Quaternion current = [Quaternion initWithRe:(double)cmQ.w i:(double)cmQ.x j:(double)cmQ.y k:(double)cmQ.z];
// the complex conjugate and normalised to be on the safe side
Quaternion inverse = [current inverse];
// build the delta, assuming you have your stored direction as class member targetQuaternion
Quaternion diff = [inverse multiply:targetQuaternion];
float alpha = 2 * acos (diff.Re);
// maxDeltaAngle is your class member variable defining the angle from which you assume the position as restored (radians)
if (fabs (alpha) < maxDeltaAngle) {
    // do my fancy camera things
}

开始时确实不那么容易。正如Ali在他的答案中所述，可视化也是一个重要问题。我的解决方案只解决了原始数学部分。

- Kay

---

非常感谢您的回答 :) 如果您能指向一个比较四元数的示例代码，我将不胜感激。我浏览了链接，它们对我来说是有意义的，但我发现很难完全理解四元数概念并将其可视化。在第三点中找到delta部分也类似，我不清楚如何实际实现它。 - iSeeker

看到我的更新。不要沮丧，我花了很长时间才深入研究四元数，为了让每个人都能理解。 - Kay

@Ali和Kay，我认为这些是我需要获取不同轴上设备运动的值...我只是在瞎猜，因为看nslogs似乎很可靠。float xdiff = asin(delta.i); float ydiff = asin(delta.j); float zdiff = asin(delta.k);。我已经在这里更新了代码：链接。我仍在寻找可靠的线索... - iSeeker

@Ali和Kay，成功了... :)。我花了很长时间才让它工作，现在我有很多问题...不得不使用xdiff = asin(delta.j)而不是上面给出的那个。上面的那个以一种交换的方式给出了横滚和俯仰，不知道为什么...也许这是一个问题吧...无论如何，非常感谢你们..我很感激。 - iSeeker

Answer 2

我并不是说以下内容是解决您问题的方案，我不知道这个方案是否用户友好，但我认为值得一试。我看到两个问题：如何存储态度和如何可视化它们。

存储。我会将态度保存为四元数或旋转矩阵；CMAttitude提供了两者。（个人而言，我更喜欢旋转矩阵而不是四元数，因为在我看来旋转矩阵更容易理解。这只是个人偏好。）

可视化。您正在使用3个标记将手机带到与保存的态度相同的位置。这3个标记来自偏航、俯仰和滚动，有效地破坏了您的应用程序的稳定性。我建议不要使用这3个标记，而是可视化保存的态度和当前态度之间的旋转。一种方法是将旋转后的3个基向量投影到手机屏幕上。旋转矩阵的好处是它可以在没有任何额外计算的情况下给你准确的结果。例如，要可视化

| 1 0 0 |
| 0 1 0 |
| 0 0 1 |

这个旋转矩阵代表的是旋转角度，我只需要从[0, 0, 0]到(i)[1, 0, 0]，(ii)[0, 1, 0]和(iii)[0, 0, 1]绘制3个箭头。也就是说，我只需要绘制矩阵的行或列（取决于您想要哪个更好，是行还是列）。

要获取保存的旋转矩阵S和当前旋转矩阵C之间的旋转，您需要计算C^TS（即C转置乘以S）。如果您的旋转矩阵如上所示，则已将手机与保存的姿态对齐。

有关旋转矩阵的优秀教程是Direction Cosine Matrix IMU: Theory手稿。

另一种可视化保存状态和当前状态之间旋转的方式是将其转换为角轴形式。然后，我会将轴投影到手机屏幕上。用户首先将手机与轴对齐，然后需要围绕轴旋转以匹配保存的姿态。四元数基本上表示轴角形式，尽管以一种非平凡的方式。

---

你需要付出大量的努力，根据自己的需求修改这个想法；这绝对不是一个完整的答案。尽管如此，我希望这有一点帮助。

- Ali

---

非常感谢您的回答。这对我来说似乎很难想象，但我一定会尝试的。 :) - iSeeker

我可以使用四元数找到一个解决方案，但我一定会尝试这种方法并让你知道。非常感谢您的支持……从未想过我能做到这一点……我真的很感激。 - iSeeker