问题描述
设置是一种测量设备,可为我提供四元数和欧拉角。测量设备安装在磁盘上,磁盘本身安装在臂上。手臂固定在一个吊舱上,因此可以在一端固定约240度的同时上下旋转。光盘可以顺时针和逆时针旋转,在两个方向上都可以无限旋转。
左图为侧视图,右图为俯视图。我要测量的可能旋转角度绘制为箭头。
目标正在显示两个值,一个为-手臂角度,另一个为光盘旋转角度,均在-180和180之间度。
测量设备数据的旋转顺序为ZYX(横摇,俯仰,偏航),四元数为WXYZ。 X代表东方,Y代表北方,Z代表顶端。测量设备本身的旋转顺序不固定(臂和磁盘的运动可以混合)。
我了解到,欧拉角是相对于物体的,因此,当物体旋转时,欧拉角会发生变化。此外,我了解到轮换顺序很重要。我读到反转旋转顺序(到XYZ)会给我外在的旋转(=用于基于世界的不变轴而不是对象的固有轴来获得对象的相同末端方向的旋转),但是我有不知道是否/如何将其用于解决我的问题。
我没有找到实现我的目标的任何python函数,并且不了解执行该路线所需的数学知识。我是否忽略了解决该问题的简单方法?如果没有,您将如何处理?
EDIT1 :找到了一种用于计算手臂角度的解决方案:
- 定义基本矢量(0,1),它是位于中间位置的z轴(表示手臂平行于地球)
- 通过将设备四元数定义的旋转应用于基本矢量来获取当前的z轴
- 使用 arccos(numpy.clip(dot(rotatedVectorZ,baseVectorZ),-1.0,1.0))计算原始z轴与旋转轴之间的角度,该角度等于手臂角度。
计算出的角度永远不会为负,因此每个值都有两个可能的位置,但这足以满足我的情况。
要计算旋转角度,我的想法是将x向量投影到xy平面中,方法是将z设置为0,然后对向量进行归一化,然后计算(1,0 ,0)轴和投影向量,但这不起作用,因为手臂角度在90度附近有些偏移。
现在我正在考虑旋转设备的x,y,z坐标(这是通过将四元数定义的旋转应用于每个矢量(1,0),(0,1,0)和(0,1))在z轴对齐的球体中,这使我能够计算原始x轴(1,0)与旋转的x轴之间的差,但我不是确定这是否按预期工作。
编辑2:找到了一种确定磁盘旋转角度的有效方法,该方法可以确定0至180度之间的磁盘旋转角度:
- 定义基本矢量(1,0),它是x轴位于中间位置(表示磁盘未旋转)
- 通过将设备四元数定义的旋转应用于基本矢量来获取当前的x轴
- 通过计算垂直于原始z轴(即0,1)和当前z轴(等于0,1)的向量以及应用z的应用来计算手臂围绕其旋转的轴。设备的当前四元数值)
- 对该轴矢量进行归一化,并计算将当前z轴旋转到原始z轴(0,1)的四元数
- 使用该四元数旋转当前的x轴(请参阅步骤2)。这模拟了手臂运动到平行于地球的位置。
- 计算新的x轴与基本x轴之间的角度(1,0)
现在我们有了磁盘在180度空间中的旋转角度。
编辑3:找到了最终解决方案,以获取0到360度之间的旋转角度。代替使用点积的arccos,我们可以使用arctan2(行列式,dot(current_x-vector,base_x轴)),因为两个矢量现在都在同一平面上(z值为0)。
解决方法
手臂角度:计算基础z轴(0,1)与设备当前z轴之间的角度(这是设备四元数在基础z轴上的应用(0 ,0,1)。
磁盘旋转角度:通过计算两个归一化向量之间的叉积,计算出与设备的基本z轴和当前z轴垂直的轴。然后使用设备的四元数旋转基本x轴(0,1),然后将其沿刚计算出的垂直轴旋转一个角度,其中a是我们刚开始时计算出的手臂角度。现在,我们可以计算两次旋转的x轴与原始x轴(0,1)之间的角度。
有关更多详细信息,请参阅顶部的原始问题帖。