C# 用重力计计算重力

如何将测斜仪（俯仰、偏航和滚转）转换为系统在

[X、Y、Z]

中预期的重力

处于一定俯仰角、偏航角和横滚角的静止系统应以一定的角度牵引至地面，比如说，这是为了模拟目的。我想做一个函数，输入是俯仰、偏航和滚动，输出是向下力矩的向量3（X，Y，Z）

这意味着一个物体在静止时，其背部向下会从加速计和

[0，-1,0]

输出类似

[0，-1,0]

，其中

[0，-1,0]

减去

[0，-1,0]

导致

[0,0,0]

。或者，如果我们以1g的速度向左拉，我们会看到一个加速计显示新值

[1，-1,0]

在系统运行的情况下[pitch，yaw，roll]->[0，-1,0]功能是我所追求的

Vector3 OriToXYZ(float pitch, float yaw, float roll){
    Vector3 XYZ = Vector.Zero;
    //Simulate what the XYZ should be on a object in this orientation
    //oriented against gravity
    ...
    return XYZ;
}

是的，我知道，正如下面的解释所示，我无法检测系统是否颠倒或不基于滚动，因为滚动仅给出（-90到90），但这是另一个问题）

这就是方向的布局方式。

有关使用此信息的原因、内容和方式的更多信息，请继续阅读。

该计划是通过模拟/计算定向（俯仰、偏航、横摇）时的预期重力值，将惯性计用作陀螺仪的替代品，以去除加速度计数据中的重力分量

由于加速计（XYZ）是重力（XYZ）和运动（XYZ）两个分量的组合，我假设

gravity（XYZ）-calc_g（XYZ）=0，

允许我执行

accelerator（XYZ）-calc_g（XYZ）=运动（XYZ）

来说明为什么我认为这是可能的。当我将手机上的值绘制成图形，并以某种摆锤式的方式用力向侧面移动手机时，看起来像正弦/余弦运动的线是测斜仪，其他线是XYZ加速计：

• 红色=（俯仰和加速L-X）
• 绿色=（偏航和加速L-Y）
• 蓝色=（滚动和加速L-Z）

加速度值在（-2到2）范围内乘以90，以便在图纸中的范围为-180到180，俯仰偏航和横摇范围如上面的说明所示。图像的中间为Y=0，左侧为X=0（X=时间）

已解决 Romasz的解决方案

结果

*这些图表不是来自同一个测量值。

（在注释后（完全）编辑）

如果要计算倾斜方向上的重力分量，则只需俯仰和侧倾（在WP惯例中）-围绕Z旋转（偏航）对加速度计没有影响。公式应如下所示：

VectorX = Cos(Pitch)*Sin(Roll);
VectorY = -Sin(Pitch);
VectorZ = -Cos(Pitch)*Cos(Roll);

（例如，您可以找到类似的公式或）

由于多种原因，精度可能存在一些问题：

• 倾角为单精度，加速度为双精度
• 测斜仪可能需要一段时间才能稳定
• 从测斜仪和加速计读取数据时的时间不同（因为这些传感器具有不同的最小报告间隔，所以要注意）
• 加速计根据其位置具有不同的精度

另外要小心，因为加速度计可能过载（其范围为+-2g）-例如，如果你扣下手机

---

为了测试它，我编写了一个简单的应用程序（）——比较加速度计指示的值和通过倾角计算的值。因为加速度计的值是相对于重力的，所以它的前倾方向：
在XAML中-几个文本块：

<Grid x:Name="LayoutRoot" Background="Transparent" Margin="20">
        <Grid.RowDefinitions>
            <RowDefinition Height="1*"/>
            <RowDefinition Height="1*"/>
            <RowDefinition Height="1*"/>
        </Grid.RowDefinitions>
        <StackPanel Grid.Row="0" VerticalAlignment="Center" Orientation="Horizontal">
            <TextBlock Text="Incliation:" FontSize="16"/>
            <TextBlock Name="incXTB" Margin="10"/>
            <TextBlock Name="incYTB" Margin="10"/>
        </StackPanel>
        <StackPanel Grid.Row="1" VerticalAlignment="Center" Orientation="Horizontal">
            <TextBlock Text="Accelerometers:" FontSize="16"/>
            <TextBlock Name="accXTB" Margin="10"/>
            <TextBlock Name="accYTB" Margin="10"/>
            <TextBlock Name="accZTB" Margin="10"/>
        </StackPanel>
        <StackPanel Grid.Row="2" VerticalAlignment="Center" Orientation="Horizontal">
            <TextBlock Text="Through Inc:" FontSize="16"/>
            <TextBlock Name="accincXTB" Margin="10"/>
            <TextBlock Name="accincYTB" Margin="10"/>
            <TextBlock Name="accincZTB" Margin="10"/>
        </StackPanel>
</Grid>

第一件事：加速度计不能测量重力。它们测量除重力以外的所有实际力产生的加速度。（这是牛顿的解释。相对论的解释更容易：加速计测量加速度是因为作用在加速计上的所有真实力。在广义相对论中，重力是一种虚构的力。）

加速计无法感知重力的第一条线索是观察静止在地球表面的加速计的输出。它向上的加速度约为1g。作用在加速计上的力为重力，向下约1g，法向力，向上约1g。如果加速度计确实感应到重力，那么静止在地球表面的加速度计将记录到接近零的加速度。他们没有。它们所能感受到的就是向上1g的法向力

另一个线索是：将加速计绑在跳伞者身上。当跳伞者站在飞机上等待飞机到达降落点时。飞机的地板向上推动跳伞者，这种力通过跳伞者的身体传播到加速计。加速计将向上记录约1g。当跳伞者跳跃时，加速计会突然记录到侧向加速度，因为作用在跳伞者身上的唯一力是水平风。注册加速度没有向上或向下的分量。当跳伞者下降并加快垂直速度时，阻力将向上移动，使加速度计的输出从侧面向上移动。当跳伞者拉动撕裂绳时，加速计输出将出现峰值，然后在跳伞者达到稳定速度时下降。加速计的输出发生了巨大的变化，即使重力几乎没有变化

那么如何实现你想要的呢？因为加速计不感知重力，所以在软件中需要某种重力模型。根据需要，该模型的复杂程度可以从

• 游戏控制器中使用的非常简单的模型。那些游戏控制程序员可能甚至不知道他们正在建立一个地球局部重力场的模型。模型不一定是所有的

  <Grid x:Name="LayoutRoot" Background="Transparent" Margin="20">
          <Grid.RowDefinitions>
              <RowDefinition Height="1*"/>
              <RowDefinition Height="1*"/>
              <RowDefinition Height="1*"/>
          </Grid.RowDefinitions>
          <StackPanel Grid.Row="0" VerticalAlignment="Center" Orientation="Horizontal">
              <TextBlock Text="Incliation:" FontSize="16"/>
              <TextBlock Name="incXTB" Margin="10"/>
              <TextBlock Name="incYTB" Margin="10"/>
          </StackPanel>
          <StackPanel Grid.Row="1" VerticalAlignment="Center" Orientation="Horizontal">
              <TextBlock Text="Accelerometers:" FontSize="16"/>
              <TextBlock Name="accXTB" Margin="10"/>
              <TextBlock Name="accYTB" Margin="10"/>
              <TextBlock Name="accZTB" Margin="10"/>
          </StackPanel>
          <StackPanel Grid.Row="2" VerticalAlignment="Center" Orientation="Horizontal">
              <TextBlock Text="Through Inc:" FontSize="16"/>
              <TextBlock Name="accincXTB" Margin="10"/>
              <TextBlock Name="accincYTB" Margin="10"/>
              <TextBlock Name="accincZTB" Margin="10"/>
          </StackPanel>
  </Grid>
  

  public partial class MainPage : PhoneApplicationPage
  {
      private Inclinometer myIncMeter = null;
      private float inclX = 0;
      private float inclY = 0;
  
      private Accelerometer myAccel = null;
      private double accX = 0;
      private double accY = 0;
      private double accZ = 0;
  
      public MainPage()
      {
          InitializeComponent();
          this.DataContext = this;
          myIncMeter = Inclinometer.GetDefault();
          myIncMeter.ReportInterval = myIncMeter.MinimumReportInterval;
          myAccel = Accelerometer.GetDefault();
          myAccel.ReportInterval = myIncMeter.MinimumReportInterval;
  
          CompositionTarget.Rendering += CompositionTarget_Rendering;
      }
  
      private void CompositionTarget_Rendering(object sender, EventArgs e)
      {
          InclinometerReading incRead = myIncMeter.GetCurrentReading();
          AccelerometerReading accRead = myAccel.GetCurrentReading();
  
          accX = accRead.AccelerationX;
          accY = accRead.AccelerationY;
          accZ = accRead.AccelerationZ;
  
          inclX = incRead.RollDegrees;
          inclY = incRead.PitchDegrees;
  
          incXTB.Text = "X: " + inclX.ToString("0.00");
          incYTB.Text = "Y: " + inclY.ToString("0.00");
  
          accXTB.Text = "X: " + accX.ToString("0.00");
          accYTB.Text = "Y: " + accY.ToString("0.00");
          accZTB.Text = "Z: " + accZ.ToString("0.00");
  
          accincXTB.Text = "X: " + ((Math.Cos(inclY * Math.PI / 180) * Math.Sin(inclX * Math.PI / 180))).ToString("0.00");
          accincYTB.Text = "Y: " + (-Math.Sin(inclY * Math.PI / 180)).ToString("0.00");
          accincZTB.Text = "Z: " + (-(Math.Cos(inclX * Math.PI / 180) * Math.Cos(inclY * Math.PI / 180))).ToString("0.00");
      }
  }