<think>我们已知旋转前和旋转后的三轴倾角（即重力向量在三个轴上的分量），目标是计算绕固定旋转轴旋转的角度。根据之前的推导，我们可以通过旋转前后重力向量的点积和叉积来计算旋转角度。 步骤： 1. 将旋转前和旋转后的倾角转换为重力分量向量（单位化）。 2. 计算两个单位向量的点积，得到夹角的余弦值。 3. 计算两个单位向量的叉积，并求叉积的模，得到夹角的正弦值。 4. 使用反正切函数（atan2）计算旋转角度。 注意： - 旋转轴未知，但不需要显式求出。 - 由于旋转前后重力向量的模都是g，所以我们可以用单位向量计算。 - 旋转角度θ的范围是[0, π]（因为点积是对称的），但实际上旋转角度可以是任意值，但这里我们计算的是两个重力向量之间的最小旋转角（即0到π）。但实际旋转角度可能是这个角度或者其补角？然而，根据旋转的连续性，我们通常可以确定旋转方向（通过叉积的方向），但这里我们计算的是旋转的绝对值吗？注意，我们使用叉积的模（总是正数）和点积，然后通过atan2(y,x)得到[0,2π)的角度，但叉积的模是正数，所以atan2(y,x)会得到[0,π]的角度。实际上，旋转角度θ在0到π之间，因为旋转角度大于π时，我们可以用2π-θ来表示，但旋转的最小角度就是θ（0到π）。但注意，绕固定轴的旋转角度可以是任意值（0到2π），但通过两个向量我们只能确定0到π的角度（因为向量旋转θ和旋转2π-θ后，从向量角度看是一样的，除非我们考虑旋转方向）。因此，我们需要知道旋转方向（叉积的方向）来确定是θ还是-θ（即旋转方向）。但在实际应用中，我们往往只关心最小旋转角度（0到π），或者通过连续测量来推断方向。 然而，在问题中，我们有一个固定的旋转轴，且旋转是连续的，所以我们可以通过前后时刻的重力向量变化，利用叉积的方向（与旋转轴方向一致）来确定旋转方向，从而得到有符号的旋转角度。但根据问题描述，我们只需要旋转角度（通常指绝对值）。如果要求有符号角度，则需要考虑叉积的方向与旋转轴方向的关系（但旋转轴方向未知）。不过，在计算旋转角度时，我们可以通过叉积向量与旋转轴方向平行来确定符号，但旋转轴方向未知，所以通常我们计算的是最小角度（0到π）。 但是，在连续旋转过程中，我们可以通过前后两个重力向量变化，利用叉积的方向（相对于前一个向量）来确定旋转方向。然而，如果我们只关心单次旋转的角度（从状态A到状态B），那么我们可以计算0到π的角度，但无法区分是顺时针还是逆时针（因为旋转轴方向未知，且叉积方向只能告诉我们旋转轴的方向，但旋转轴方向固定，我们不知道其指向）。所以通常我们计算的是旋转的最小角度。 根据问题，我们只需要旋转角度（标量，绝对值），所以我们可以计算0到π的角度。 代码实现： 输入：旋转前的倾角（alpha_x, alpha_y, alpha_z）和旋转后的倾角（alpha_x_prime, alpha_y_prime, alpha_z_prime） 输出：旋转角度theta（弧度） 注意：倾角是轴与重力方向的夹角，所以重力分量： g_x = g * cos(alpha_x) g_y = g * cos(alpha_y) g_z = g * cos(alpha_z) 但是，这里有一个关键：重力分量的符号。通常，如果坐标系是右手系，且Z轴向上，那么重力在Z轴的分量应该是负的（因为重力向下）。所以需要根据实际坐标系确定符号。但倾角通常定义为轴与重力方向（向下）的夹角，所以重力分量在轴上的投影应该是正的（如果轴的正方向与重力方向成锐角），但实际在加速度计中，重力加速度向量在局部坐标系中的分量是：当轴正方向向上时，分量为负；向下时，分量为正。所以这里需要明确。 为了避免混淆，我们假设输入的是重力加速度在三个轴上的分量（带符号），而不是倾角。或者，如果输入是倾角，那么我们需要根据坐标系的定义来转换成分量。 假设用户提供的是三个轴上的重力分量（单位：g，即归一化重力向量），那么我们可以直接使用。 因此，我们假设输入是： vector_before = [gx1, gy1, gz1] # 旋转前重力向量（归一化或未归一化均可，因为后面会归一化） vector_after = [gx2, gy2, gz2] # 旋转后重力向量 步骤： 1. 将两个向量归一化（单位化） 2. 计算点积：dot = gx1*gx2 + gy1*gy2 + gz1*gz2 3. 计算叉积：cross = [ (gy1*gz2 - gz1*gy2), (gz1*gx2 - gx1*gz2), (gx1*gy2 - gy1*gx2) ] 4. 计算叉积的模：norm_cross = sqrt(cross[0]**2 + cross[1]**2 + cross[2]**2) 5. 使用atan2(norm_cross, dot)得到旋转角度theta（弧度） 注意：这个theta在0到π之间。 但是，如果用户提供的是倾角（角度值），那么我们需要先计算重力分量。假设重力加速度大小为g，那么： gx1 = g * cos(alpha_x1) # 这里注意：如果倾角是轴正方向与重力方向（向下）的夹角，那么分量应该是正的（因为重力方向与轴正方向成锐角时，投影为正）？但实际加速度计输出：当轴正方向与重力方向一致时（向下），分量为+g；向上时为负。所以这里需要根据传感器的安装方向来确定。通常，在标准安装中（Z轴向上），重力在Z轴的分量是负的，所以如果倾角是轴与重力方向的夹角，那么对于Z轴（向上），倾角为180度，cos(180°)=-1，所以g_z = g * (-1) = -g。因此，直接使用倾角的余弦乘以g即可。 所以，如果输入是倾角（弧度），那么： gx1 = g * math.cos(alpha_x1) gy1 = g * math.cos(alpha_y1) gz1 = g * math.cos(alpha_z1) 同样，旋转后的分量： gx2 = g * math.cos(alpha_x2) gy2 = g * math.cos(alpha_y2) gz2 = g * math.cos(alpha_z2) 然后，我们使用这些分量向量（不需要归一化，因为点积和叉积计算时，归一化常数g会被约掉？但叉积的模会多一个g^2，所以我们需要归一化？）所以，最好归一化。 因此，我们可以先归一化： norm1 = math.sqrt(gx1**2 + gy1**2 + gz1**2) gx1, gy1, gz1 = gx1/norm1, gy1/norm1, gz1/norm1 norm2 = math.sqrt(gx2**2 + gy2**2 + gz2**2) gx2, gy2, gz2 = gx2/norm2, gy2/norm2, gz2/norm2 然后计算点积和叉积。 但注意：由于重力向量模长固定，所以归一化常数应该是g，但实际测量中可能有噪声，所以归一化可以消除模长变化的影响（比如加速度计有其他加速度干扰，则模长会变化，但这里假设只有重力，即静态或准静态）。 代码实现（Python）： </think> ### 根据三轴倾角变化计算旋转角度的算法实现 以下是基于重力向量变化计算绕固定旋转轴旋转角度的完整Python实现。该算法利用旋转前后重力分量的点积和叉积关系计算旋转角度，无需已知旋转轴方向。 ```python import numpy as np import math def calculate_rotation_angle(angles_before, angles_after): """ 根据旋转前后的三轴倾角计算绕固定旋转轴的旋转角度 参数: angles_before: 旋转前的三轴倾角 (α_x, α_y, α_z) [弧度] angles_after: 旋转后的三轴倾角 (α_x', α_y', α_z') [弧度] 返回: rotation_angle: 绕固定轴的旋转角度 [弧度] """ # 1. 将倾角转换为重力分量向量 (假设重力加速度g=1) g_before = np.array([ math.cos(angles_before[0]), # g_x = cos(α_x) math.cos(angles_before[1]), # g_y = cos(α_y) math.cos(angles_before[2]) # g_z = cos(α_z) ]) g_after = np.array([ math.cos(angles_after[0]), math.cos(angles_after[1]), math.cos(angles_after[2]) ]) # 2. 归一化重力向量 (转换为单位向量) g_before_unit = g_before / np.linalg.norm(g_before) g_after_unit = g_after / np.linalg.norm(g_after) # 3. 计算点积和叉积 dot_product = np.dot(g_before_unit, g_after_unit) cross_product = np.cross(g_before_unit, g_after_unit) cross_magnitude = np.linalg.norm(cross_product) # 4. 计算旋转角度 (使用atan2避免象限问题) rotation_angle = math.atan2(cross_magnitude, dot_product) return rotation_angle # ===== 使用示例 ===== if __name__ == "__main__": # 示例1: 绕X轴旋转45度 angles_initial = (math.pi/2, math.pi/2, math.pi) # 初始姿态: (90°, 90°, 180°) angles_rotated = (math.pi/4, math.pi/2, 3*math.pi/4) # 旋转后: (45°, 90°, 135°) angle_rad = calculate_rotation_angle(angles_initial, angles_rotated) print(f"旋转角度: {math.degrees(angle_rad):.2f}°") # 示例2: 任意旋转 angles_before = (1.2, 0.8, 1.5) # 任意初始倾角 angles_after = (1.0, 1.1, 1.3) # 旋转后倾角 angle_rad = calculate_rotation_angle(angles_before, angles_after) print(f"旋转角度: {math.degrees(angle_rad):.2f}°") ``` #### 算法说明： 1. **输入处理**： - 输入为旋转前后的三轴倾角（弧度制） - 使用 $g_i = \cos(\alpha_i)$ 将倾角转换为重力分量[^1] - 重力向量归一化：$\hat{a} = \frac{\vec{g}_\text{before}}{\|\vec{g}_\text{before}\|}$, $\hat{b} = \frac{\vec{g}_\text{after}}{\|\vec{g}_\text{after}\|}$ 2. **核心计算**： - 点积：$\cos\theta = \hat{a} \cdot \hat{b}$ - 叉积模长：$\sin\theta = \|\hat{a} \times \hat{b}\|$ - 旋转角度：$\theta = \atan2(\sin\theta, \cos\theta)$ 3. **输出**： - 返回绕固定旋转轴的绝对旋转角度（弧度） - 角度范围 $[0, \pi]$，实际旋转方向可通过叉积符号确定 #### 注意事项： 1. **传感器校准**： - 实际应用中需校准传感器，消除零偏误差 - 重力分量需满足 $\sum \cos^2\alpha_i = 1$，否则需归一化处理 2. **动态环境处理**： ```python # 添加低通滤波处理加速度计噪声 def low_pass_filter(raw_value, prev_value, alpha=0.2): return alpha * raw_value + (1 - alpha) * prev_value ``` 3. **多传感器融合**： - 当旋转轴接近重力方向时（$\sin\theta ≈ 0$），需融合陀螺仪数据提高精度[^2] - 完整姿态估计推荐使用Madgwick或Mahony滤波算法 #### 典型应用场景： 1. 机器人关节角度监测 2. 无人机姿态稳定控制 3. 虚拟现实头盔跟踪 4. 工程机械倾角安全监测