VR控制器项目：SteamVR旋转类型转换适配问题求解

我近期在探索超出自身知识储备的技术方案，尝试组合三款无公开对接先例的程序制作低成本VR控制器，过程中遇到了姿态旋转转换类问题。

整体数据链路 #

位姿数据完整传输流程如下：

• 使用PSMoveServiceEX获取PlayStation Move控制器的位置与姿态数据
• 通过PSMoveFreePIEBridge将位姿数据传输至FreePIE
• 在FreePIE中完成按键、轴映射与姿态数据调整后，将数据输入Razer Hydra插件
• 通过SteamVR Razer Hydra驱动将数据传入SteamVR

当前核心异常 #

SteamVR中显示的控制器俯仰角固定存在*+45度偏移*。
经实际观测得到以下现象：

• PSMoveServiceEX输出的姿态参数为altitude、heading、bank，分别对应俯仰（Pitch）、偏航（Yaw）、滚转（Roll）
• 滚转角输出逻辑异常：控制器顺时针旋转0-90度时，滚转值从0下降至-90，继续旋转90-180度时，值反而回升至0
• 偏航、俯仰角可正常在±180度范围内变化，PSMoveServiceEX配置工具中的3D控制器模型显示完全正常
• 现有FreePIE脚本在滚转角接近±90度时会出现姿态错乱

已尝试的排查与实现 #

经资料查证，判断PSMoveServiceEX输出的是航空领域常用的Tait-Bryan角，推测SteamVR期望输入为Euler角，因此需要完成Tait-Bryan角到Euler角的转换。可选转换路径有两种：

• Tait-Bryan角→Quaternion（四元数）→Euler角
• Tait-Bryan角→Rotation Matrix（旋转矩阵）→Euler角

因未找到易移植的Tait-Bryan转Quaternion示例，选择第二种转换路径，参考公开资料中ZXY顺序Tait-Bryan角转Rotation Matrix、ZXZ顺序Rotation Matrix转Euler角的公式，在FreePIE的Python脚本环境中编写了如下转换代码：

def tait_to_rmatrix(head,alt,bank):
    c1 = math.cos(head)
    c2 = math.cos(alt)
    c3 = math.cos(bank)
    s1 = math.sin(head)
    s2 = math.sin(alt)
    s3 = math.sin(bank)
    R = [(c1*c3)-(s1*s2*s3) , -(c2*s1) , (c1*s3)+(c3*s1*s2) , (c3*s1)+(c1*s2*s3) , c1*c2 , (s1*s3)-(c1*c3*s2) , -(c2*s3) , s2 , c2*c3]
    return R

def rmatrix_to_euler(R):
    yaw = math.atan(R[2]/-R[5])
    pitch = math.atan(math.sqrt(1-(R[8]*R[8]))/R[8])
    roll = math.atan(R[6]/R[7])
    return [yaw,pitch,roll]

RmatrixR = tait_to_rmatrix(-freePieIO[idR].yaw,(freePieIO[idR].pitch+45),freePieIO[idR].roll) 
eulerRout = rmatrix_to_euler(RmatrixR)
diagnostics.watch(math.degrees(eulerRout[0]))
diagnostics.watch(math.degrees(eulerRout[1]))
diagnostics.watch(math.degrees(eulerRout[2]))
hydra[indexR].x = (freePieIO[idR].x * 10)
hydra[indexR].y = (freePieIO[idR].y * 10)
hydra[indexR].z = (freePieIO[idR].z * 10)
hydra[indexR].yaw = eulerRout[0]
hydra[indexR].pitch = eulerRout[1]
hydra[indexR].roll = eulerRout[2]

后续排查定位到滚转异常的诱因在PSMoveFreePIEBridge的源码中，其姿态计算代码如下：

freepie_io_6dof_data poseData;
PSMQuatf normalizedQuat = PSM_QuatfNormalizeWithDefault(&controllerPose.Orientation, k_psm_quaternion_identity);
//glm::quat glmOrientation = glm::quat(normalizedQuat.w, normalizedQuat.x, normalizedQuat.y, normalizedQuat.z);

poseData.x = controllerPose.Position.x;
poseData.y = controllerPose.Position.y;
poseData.z = controllerPose.Position.z;

//data.pitch = glm::pitch(glmOrientation);
//data.yaw = glm::yaw(glmOrientation);
//data.roll = glm::roll(glmOrientation);

//Calcuate rotation here, glm library doesn't work for yaw
//Both glm and this seem to work fine when each axis is independent, but issues when combined. 
poseData.yaw = std::atan2(2 * normalizedQuat.y * normalizedQuat.w - 2 * normalizedQuat.x * normalizedQuat.z, 1 - 2 * normalizedQuat.y * normalizedQuat.y - 2 * normalizedQuat.z * normalizedQuat.z);
poseData.roll = std::asin(2 * normalizedQuat.x * normalizedQuat.y + 2 * normalizedQuat.z * normalizedQuat.w);
poseData.pitch = std::atan2(2 * normalizedQuat.x * normalizedQuat.w - 2 * normalizedQuat.y * normalizedQuat.z, 1 - 2 * normalizedQuat.x * normalizedQuat.x - 2 * normalizedQuat.z * normalizedQuat.z);

WriteToFreepie(poseData, trackedFreepieIndicies[i]);

可见该代码中滚转角计算使用asin()函数，而俯仰、偏航角使用atan2()函数，是滚转值异常的根源。但自行fork代码修改存在较高门槛：项目开发环境搭建流程复杂，学习成本极高。

现存待解决难点 #

即使修正了角度计算逻辑，Euler角本身存在*gimbal lock（万向节锁）*问题，若要彻底规避需要传输Quaternion数据，但FreePIE的Razer Hydra插件仅支持从真实Razer Hydra控制器读取Quaternion，无法向SteamVR写入Quaternion数据，目前仅能考虑通过脚本过滤接近gimbal lock时的极值做临时兼容，整体方案距离可用仍有明显差距，寻求可行的解决思路。

内容的提问来源于stack exchange，提问作者TheGeek007