Qt 如何通过一组给定的位置向量获得绕两个轴的转速

我有一大堆标准化的位置向量。矢量集由专用测量装置记录，同时该装置绕两个轴旋转。每个位置向量也是X、Y和Z在指定时间的重力值的组合。我的任务是得到两轴的转速

测量装置的坐标系相对于机器坐标系绕z轴旋转约45°

测量装置的z轴平行于机器的z轴。

我试着把卡瑟斯坐标转换成球坐标。为此，我使用了Qt框架和MATLAB。结果我得到了两个角和一个半径。在我看来，半径并不重要。但是这两个角度不适合我的问题，因为我需要机器绕Z轴和X轴的旋转速度。在这一点上，重要的是要知道旋转速度是如此之慢，以至于重力矢量始终指向地面1g in。测量装置的X、Y和Z值表示受重力矢量影响的方向。例如，如果Z轴指向地面，则该值接近1。如果轴与地面平行（也与重力向量正交），则该值几乎为零

如果机器只绕Z轴旋转，我可以很容易地得到一次旋转的周期。Y值和X值随时间变化的曲线图为正弦或余弦曲线。所以我可以通过搜索零点，最大值或最小值得到周期

绕z轴旋转

但该解决方案仅适用于1轴问题。如果机器绕X轴额外旋转，则测量的X、Y和Z值是两种旋转的组合。我不知道怎样才能解决我的问题

绕机器z轴和x轴旋转：55秒后开始旋转！

另一个想法是反向运动学，但为此，我需要机器的尺寸和测量设备安装的确切位置

绕1轴旋转

绕2轴旋转

我如何开始或继续

---

我试着用这张图片想象旋转过程。

---

我试着把这个作为一个评论，但是有一个长度限制。因此，一些澄清问题/中间结论：

谢谢你的数字！从上面的第四幅图来看，这幅图显示了2轴x-y-z正弦波，从机器图来看，好像有三个坐标系：第一个是地球坐标系，称它为x1，y1，z1，正如你在“机器图”中所示。第二帧称为x2、y2、z2，绕x1和x2轴旋转（它们保持平行）。第三帧x3、y3、z3是绕z2（=z3）轴旋转的帧。您的加速计固定在x3、y3、z3坐标系中

单轴数据集的z3=z2与地球z1对齐，并围绕z旋转，因此x3和y3围绕正交正弦波中的采样重力旋转

在第二个数据集中，外部框架x1=x2以恒定速率旋转，在z加速度计上产生完美的正弦波，而内部z3=z2框架也可能以恒定速率旋转，但现在x3和y3上的加速度计的振幅通过乘以x1/x2旋转角的余弦进行调制

这一切听起来对吗

在根据位置测量值估计速度时，我们需要知道的另一件事是系统如何随时间变化的某种模型或概念：我们能假设一些最大角加速度吗？或者我们可以假设一旦旋转达到速度，它们是恒定的吗？当x1/x2角度通过+/-pi/2弧度时，这在尝试缝合z2/z3万向节角度时变得尤为重要，因为x和y加速计与重力矢量正交，只会显示噪声，因此z2/z3角度变得暂时不可观察。这也将有助于我们确定x1/x2万向节是升到pi/2再降回来，还是继续朝同一方向旋转到>pi/2，因为在z加速度计上两个运动看起来是一样的，并且z2/z3角度在那里是不可观察的。

简单回答：

Use two-argument arctangent.
The roll angle is atan2(ay, ax).
The pitch angle is atan2(az, sqrt(ax*ax + ay*ay)).
Then time-difference these to get angle rates.

---

这种过于简单的解决方案有很多问题，但这是一个很好的起点。

可能您需要的关键是：在估计机器x轴的节距之前，必须将x和y加速度从测量坐标转换为机器坐标。这要求您首先知道滚动角度（关于机器z轴）。在税收方面,

[x_machine; y_machine] = [cos(roll) -sin(roll); sin(roll) cos(roll)] * [x_meas; y_meas].
z_machine = z_meas, always.

给定机器坐标中的x、y、z，可以直接估计机器x轴的俯仰角和速率：

pitch = atan2(z_machine, -y_machine)  (right hand rule about the machine x axis; positive acceleration pointing down); 
pitch_rate = -asin((xyz_i cross xyz_i-1)_x) / dt_i, 

其中，在英语中，速率是根据最新机器坐标加速度向量与前一个机器坐标加速度向量的叉积的x_机器分量的反正弦除以它们之间的时间（在您的情况下为1/8秒）计算得出的。 同样的方法适用于估算横滚和横滚率（关于机器z轴）：

这是一个鸡和蛋的问题，你需要知道投球和滚球来估计投球和滚球的速度。因此，您需要从正确的俯仰角和侧倾角（在15度左右应该可以）的良好猜测开始。 所有的测量值都是有噪声的，因此估计值也会很低。利率估计尤其如此。因此，您需要及时过滤估计值。使用过滤后的角度速率估计值及时传播俯仰和滚动角度估计值。

---

此外，当俯仰接近+/-pi/2时，您的横滚角度和速率估计值会变成纯噪声，因此您可能应该通过类似cos^2（俯仰）的方式对横滚过滤器的输入进行加权。

您需要定义轴。你有吗

roll = atan2(-x_meas, -y_meas) * cos(pitch) / abs(cos(pitch));  
roll_rate = -asin((xyz_meas_i cross xyz_meas_i-1)_z) * cos(pitch) / abs(cos(pitch)) / dt_i.