如何通过roll、pitch获取地平面与传感器法向量以计算倾翻夹角

前提约定 #

先明确通用的坐标系与旋转规则（和绝大多数MPU6050开源姿态解算逻辑匹配，若你的解算规则不同可对应调整）：

• 世界大地坐标系：z轴垂直地平面向上，你需要的绿色地平面法向量为该坐标系下的固定单位向量 g = (0, 0, 1)
• 传感器本体坐标系：x、y轴位于传感器芯片平面，z轴垂直芯片平面向上（即你要的蓝色传感器法向量的本体坐标）
• 旋转定义：roll(φ)为绕传感器x轴的旋转角，pitch(θ)为绕传感器y轴的旋转角，旋转顺序为先roll后pitch

计算传感器法向量的世界坐标 #

传感器的蓝色法向量在自身本体坐标系下的坐标为 s_local = (0, 0, 1)，通过旋转矩阵将其转换到世界坐标系下，即可得到两个向量的同坐标系坐标。
代入旋转矩阵运算后可直接得到简化后的世界坐标系下的蓝色法向量：
s_world = (sinθ, -sinφ * cosθ, cosφ * cosθ)

计算倾翻判定角 #

两个单位向量的夹角余弦值等于二者的点积，代入两个向量的坐标计算后可进一步简化，无需单独生成两个向量再做运算：

α = arccos( cosφ * cosθ )

其中α就是你需要的两个法向量的夹角，数值范围为0~90°，数值越大代表箱体倾翻程度越高，你可以直接设定阈值（比如25°）来触发倾翻判定。

补充说明 #

• 运算时注意roll(φ)和pitch(θ)的单位要和反余弦函数的要求匹配，若函数要求弧度输入就先把角度值转成弧度
• 如果你的姿态解算采用的是先pitch后roll的旋转顺序，调整旋转矩阵乘法顺序后重新推导简化公式即可，核心逻辑一致

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