如何通过给定的一组位置矢量获得绕2轴的旋转速度

Question

我有一组很大的标准化位置向量。矢量集由特殊测量设备记录，而设备围绕两个轴旋转。每个位置矢量也是在定义的时间X，Y和Z的重力值的组合。我的任务是获得两个轴的旋转速度。

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

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

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

如果机器只围绕Z轴旋转，我可以很容易地获得一次旋转的周期。受时间影响的Y值和X值的图是正弦或余弦。所以我可以通过搜索零点，最大值或最小值获得周期。

围绕z轴旋转

但这个解决方案只适合1轴问题。如果机器围绕X轴另外旋转，则测量的X，Y和Z值是两个旋转的组合。我不知道如何解决我的问题。

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

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

围绕1轴旋转

Dataset rotation around 1 axis

围绕2轴旋转

Dataset rotation around 2 axes

我该如何开始或继续？

我试图用这张照片可视化旋转过程。

Answer 1

我试着将其作为评论，但有一个长度限制。所以，一些澄清问题/中间结论：

谢谢你的数字！所以从上面的第四幅图中可以看出，显示2轴xyz正弦波的图，从你的机器图中看，就像你有三个坐标系：第一个是地球框架，称之为x1，y1，z1，as你在＆＃34;机器图片＆＃34;图。称为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万向节角度拼接成为特别重要的，其中z2 / z3角度暂时不可观察，因为x和y加速度计是与重力矢量正交，只会显示噪音。它还将帮助我们确定x1 / x2万向节是否达到pi / 2并再次降低，或者保持向同一方向转向＆gt; π/ 2，因为z加速度计上的两个运动看起来都相同，并且z2 / z3角度在那里是不可观察的。

Answer 2

简单回答：

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.

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

Answer 3

您需要的关键可能是：在估算机器x轴的螺距之前，必须将x和y加速度从测量坐标转换为机器坐标。这需要您首先知道滚动角度（关于机器z轴）。在matlab sytax中，

[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_machine分量的反正弦值除以它们之间的时间（在您的情况下为1/8秒） ）。 相同的方法适用于估算滚动和滚动速率（关于机器z轴）：

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.

这是一个鸡与蛋的问题，你需要知道俯仰和滚动来估计俯仰和滚动及其速率。所以你需要从正确的俯仰和滚动角度开始猜测（在15度左右应该没问题）。 所有的测量结果都很嘈杂，所以估算也是如此。费率估计尤为如此。因此，您需要及时过滤估算值。使用滤波后的角速率估算值及时传播您的俯仰和侧倾角估计值。 此外，由于音高接近+/- pi / 2，您的侧倾角和速率估计值将变为纯噪声，因此您应该通过类似cos ^ 2（音高）的方式减小侧倾滤波器的输入。