1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242

import open3d as o3d
import numpy as np
import copy
import math
def create_coordinate_system(size=1.0, origin=[0, 0, 0]):
    """创建坐标系"""
    coord = o3d.geometry.TriangleMesh.create_coordinate_frame(size=size, origin=origin)
    return coord
def create_colored_cube(width=1.0, height=0.5, depth=0.3, color=[0.5, 0.5, 0.5]):
    """创建带颜色的立方体"""
    cube = o3d.geometry.TriangleMesh.create_box(width=width, height=height, depth=depth)
    cube.compute_vertex_normals()
    cube.paint_uniform_color(color)
    return cube
def get_rotation_matrix_x(angle):
    """绕X轴旋转矩阵"""
    return np.array([
        [1, 0, 0, 0],
        [0, math.cos(angle), -math.sin(angle), 0],
        [0, math.sin(angle), math.cos(angle), 0],
        [0, 0, 0, 1]
    ])
def get_rotation_matrix_y(angle):
    """绕Y轴旋转矩阵"""
    return np.array([
        [math.cos(angle), 0, math.sin(angle), 0],
        [0, 1, 0, 0],
        [-math.sin(angle), 0, math.cos(angle), 0],
        [0, 0, 0, 1]
    ])
def get_rotation_matrix_z(angle):
    """绕Z轴旋转矩阵"""
    return np.array([
        [math.cos(angle), -math.sin(angle), 0, 0],
        [math.sin(angle), math.cos(angle), 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
    ])
def get_translation_matrix(tx, ty, tz):
    """平移矩阵"""
    return np.array([
        [1, 0, 0, tx],
        [0, 1, 0, ty],
        [0, 0, 1, tz],
        [0, 0, 0, 1]
    ])
def build_euler_transform_matrix(euler_angles, translation, order='xyz'):
    """
    使用欧拉角构建变换矩阵
    
    参数:
        euler_angles: [rx, ry, rz] 欧拉角(弧度)
        translation: [tx, ty, tz] 平移向量
        order: 旋转顺序 ('xyz', 'zyx'等)
    
    返回:
        4x4变换矩阵
    """
    rx, ry, rz = euler_angles
    tx, ty, tz = translation
        # 根据旋转顺序构建旋转矩阵
    if order == 'xyz':
        R_x = get_rotation_matrix_x(rx)
        R_y = get_rotation_matrix_y(ry)
        R_z = get_rotation_matrix_z(rz)
        R = R_z @ R_y @ R_x  # XYZ顺序: 先X, 再Y, 最后Z
    elif order == 'zyx':
        R_x = get_rotation_matrix_x(rx)
        R_y = get_rotation_matrix_y(ry)
        R_z = get_rotation_matrix_z(rz)
        R = R_x @ R_y @ R_z  # ZYX顺序: 先Z, 再Y, 最后X
    else:
        # 默认使用XYZ顺序
        R_x = get_rotation_matrix_x(rx)
        R_y = get_rotation_matrix_y(ry)
        R_z = get_rotation_matrix_z(rz)
        R = R_z @ R_y @ R_x
        # 构建平移矩阵
    T = get_translation_matrix(tx, ty, tz)
        # 组合变换矩阵 (先旋转后平移)
    transform_matrix = T @ R
        return transform_matrix
def demonstrate_euler_transform():
    """
    演示欧拉角变换矩阵与分步变换的对比
    """
    print("=== 欧拉角变换矩阵与分步变换对比 ===")
        # 创建原始物体和坐标系
    original_cube = create_colored_cube(color=[0.8, 0.8, 0.8])
    world_coord = create_coordinate_system(size=1.0)
        # 定义变换参数
    rotation_angle = math.pi / 2  # 90度
    translation_distance = 2.0
        # 分步变换 (你的代码)
    cube_1 = copy.deepcopy(original_cube)
    R_z = get_rotation_matrix_z(rotation_angle)  # 绕世界Z轴旋转
    R_x = get_rotation_matrix_x(rotation_angle)  # 绕世界X轴旋转
    R_y = get_rotation_matrix_y(rotation_angle)  # 绕世界Y轴旋转
    cube_1.transform(R_x)
    cube_1.paint_uniform_color([0.5, 0, 0])  # 暗红色：X旋转后
    cube_2 = copy.deepcopy(cube_1)
    cube_2.transform(R_y)
    cube_2.paint_uniform_color([0, 0.5, 0])  # 暗绿色：Y旋转后
    cube_3 = copy.deepcopy(cube_2)
    cube_3.transform(R_z)
    cube_3.paint_uniform_color([0, 0, 0.5])  # 暗蓝色：Z旋转后
    cube_4 = copy.deepcopy(cube_3)
    T_x = get_translation_matrix(translation_distance, 0, 0)  # 沿世界X轴平移
    cube_4.transform(T_x)
    cube_4.paint_uniform_color([0.5, 0.5, 0.5])  # 灰色：X平移后
    cube_5 = copy.deepcopy(cube_4)
    T_y = get_translation_matrix(0, translation_distance, 0)  # 沿世界Y轴平移
    cube_5.transform(T_y)
    cube_5.paint_uniform_color([0.5, 0.2, 0.9])  # 紫色：最终结果
        # 使用欧拉角构建变换矩阵
    euler_angles = [rotation_angle, rotation_angle, rotation_angle]  # [rx, ry, rz]
    translation = [translation_distance, translation_distance, 0]    # [tx, ty, tz]
        # 构建变换矩阵 (注意：你的分步顺序是 X旋转 -> Y旋转 -> Z旋转 -> X平移 -> Y平移)
    # 这对应于欧拉角顺序 XYZ 和 平移 [tx, ty, 0]
    transform_matrix = build_euler_transform_matrix(euler_angles, translation, order='xyz')
        # 应用变换矩阵
    cube_matrix = copy.deepcopy(original_cube)
    cube_matrix.transform(transform_matrix)
    cube_matrix.paint_uniform_color([1, 1, 0])  # 黄色：矩阵变换结果
        # 计算分步变换的等效矩阵
    # 注意：你的分步顺序是 R_x -> R_y -> R_z -> T_x -> T_y
    # 在左乘系统中，这对应于：T_y @ T_x @ R_z @ R_y @ R_x
    step_by_step_matrix = T_y @ T_x @ R_z @ R_y @ R_x
        # 应用分步等效矩阵
    cube_step_matrix = copy.deepcopy(original_cube)
    cube_step_matrix.transform(step_by_step_matrix)
    cube_step_matrix.paint_uniform_color([0, 1, 1])  # 青色：分步等效矩阵结果
        # 可视化
    geometries = [
        world_coord, 
        original_cube, 
        cube_1, cube_2, cube_3, cube_4, cube_5,  # 分步变换
        cube_matrix,  # 欧拉角矩阵变换
        cube_step_matrix  # 分步等效矩阵
    ]
        # 打印矩阵信息
    print(f"欧拉角: {[math.degrees(a) for a in euler_angles]} 度")
    print(f"平移: {translation}")
    print(f"欧拉角变换矩阵:\n{transform_matrix}")
    print(f"分步等效矩阵:\n{step_by_step_matrix}")
    print(f"矩阵是否相等: {np.allclose(transform_matrix, step_by_step_matrix)}")
        # 可视化
    o3d.visualization.draw_geometries(
        geometries, 
        window_name="欧拉角变换对比\n"
                   "灰色=原始, 暗红=X旋转, 暗绿=Y旋转, 暗蓝=Z旋转\n"
                   "灰色=X平移, 紫色=Y平移(最终), 黄色=欧拉角矩阵, 青色=分步等效矩阵",
        width=1000, height=800
    )
        return transform_matrix, step_by_step_matrix
def demonstrate_different_euler_orders():
    """
    演示不同欧拉角顺序的影响
    """
    print("\n=== 不同欧拉角顺序对比 ===")
        # 创建原始物体和坐标系
    original_cube = create_colored_cube(color=[0.8, 0.8, 0.8])
    world_coord = create_coordinate_system(size=1.0)
        # 定义变换参数
    euler_angles = [math.pi/4, math.pi/6, math.pi/3]  # 45°, 30°, 60°
    translation = [1.5, 1.0, 0]
        # 不同欧拉角顺序的变换矩阵
    T_xyz = build_euler_transform_matrix(euler_angles, translation, order='xyz')
    T_zyx = build_euler_transform_matrix(euler_angles, translation, order='zyx')
    T_xzy = build_euler_transform_matrix(euler_angles, translation, order='xzy')
        # 应用变换
    cube_xyz = copy.deepcopy(original_cube)
    cube_xyz.transform(T_xyz)
    cube_xyz.paint_uniform_color([1, 0, 0])  # 红色
        cube_zyx = copy.deepcopy(original_cube)
    cube_zyx.transform(T_zyx)
    cube_zyx.paint_uniform_color([0, 1, 0])  # 绿色
        cube_xzy = copy.deepcopy(original_cube)
    cube_xzy.transform(T_xzy)
    cube_xzy.paint_uniform_color([0, 0, 1])  # 蓝色
        # 可视化
    geometries = [world_coord, original_cube, cube_xyz, cube_zyx, cube_xzy]
        print("不同欧拉角顺序对比:")
    print(f"欧拉角: {[math.degrees(a) for a in euler_angles]} 度")
    print(f"平移: {translation}")
    print("红色=XYZ顺序, 绿色=ZYX顺序, 蓝色=XZY顺序")
        o3d.visualization.draw_geometries(
        geometries, 
        window_name="不同欧拉角顺序对比",
        width=800, height=600
    )
if __name__ == "__main__":
    # 演示欧拉角变换矩阵与分步变换对比
    transform_matrix, step_by_step_matrix = demonstrate_euler_transform()
            print("\n" + "="*50)
    print("关键要点总结:")
    print("1. 欧拉角变换矩阵可以一次性表示复杂的旋转和平移")
    print("2. 旋转顺序对最终结果有重要影响")
    print("3. 分步变换的等效矩阵可以通过矩阵乘法得到")
    print("4. 使用变换矩阵比手动分步更高效且不易出错")

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122

import open3d as o3d
import numpy as np
from scipy.spatial.transform import Rotation as R
import copy
def verify_euler_transform(original_matrix, order='xyz', degrees=False):
    """
    验证齐次矩阵与欧拉角之间的正反转换
    
    参数:
        original_matrix: 4x4齐次变换矩阵
        order: 欧拉角顺序，如'xyz', 'zyx'等
        degrees: 是否使用角度制，False表示弧度制
    """
    print("=" * 60)
    print(f"欧拉角顺序: {order}, 使用{'角度' if degrees else '弧度'}制")
    print("=" * 60)
        # 提取旋转部分 (3x3)
    original_rotation = original_matrix[0:3, 0:3]
        # 步骤1: 旋转矩阵转欧拉角
    # 使用scipy的Rotation类进行转换 (更稳定)
    rotation_obj = R.from_matrix(original_rotation)
    euler_angles = rotation_obj.as_euler(order, degrees=degrees)
        print(f"原始旋转矩阵:\n{original_rotation}")
    print(f"提取的欧拉角: {euler_angles}")
        # 步骤2: 欧拉角转回旋转矩阵
    reconstructed_rotation_obj = R.from_euler(order, euler_angles, degrees=degrees)
    reconstructed_rotation = reconstructed_rotation_obj.as_matrix()
        print(f"重建的旋转矩阵:\n{reconstructed_rotation}")
        # 步骤3: 构建重建的齐次矩阵
    reconstructed_matrix = np.eye(4)
    reconstructed_matrix[0:3, 0:3] = reconstructed_rotation
    reconstructed_matrix[0:3, 3] = original_matrix[0:3, 3]  # 保持原平移
        print(f"原始齐次矩阵:\n{original_matrix}")
    print(f"重建齐次矩阵:\n{reconstructed_matrix}")
        # 验证精度
    rotation_error = np.max(np.abs(original_rotation - reconstructed_rotation))
    matrix_error = np.max(np.abs(original_matrix - reconstructed_matrix))
        print(f"旋转矩阵最大误差: {rotation_error:.10f}")
    print(f"齐次矩阵最大误差: {matrix_error:.10f}")
    print(f"转换是否精确: {np.allclose(original_matrix, reconstructed_matrix, atol=1e-10)}")
        return euler_angles, reconstructed_matrix, rotation_error
def demonstrate_with_different_orders():
    """使用不同的欧拉角顺序进行演示"""
        # 创建一个有意义的初始齐次矩阵 (包含旋转和平移)
    original_matrix = np.eye(4)
        # 设置一个包含三个轴旋转的变换
    # 绕X轴旋转30度，Y轴旋转45度，Z轴旋转60度
    angles_deg = [30, 45, 60]
    angles_rad = [np.radians(a) for a in angles_deg]
        # 分别创建三个轴的旋转矩阵
    Rx = np.array([
        [1, 0, 0, 0],
        [0, np.cos(angles_rad[0]), -np.sin(angles_rad[0]), 0],
        [0, np.sin(angles_rad[0]), np.cos(angles_rad[0]), 0],
        [0, 0, 0, 1]
    ])
        Ry = np.array([
        [np.cos(angles_rad[1]), 0, np.sin(angles_rad[1]), 0],
        [0, 1, 0, 0],
        [-np.sin(angles_rad[1]), 0, np.cos(angles_rad[1]), 0],
        [0, 0, 0, 1]
    ])
        Rz = np.array([
        [np.cos(angles_rad[2]), -np.sin(angles_rad[2]), 0, 0],
        [np.sin(angles_rad[2]), np.cos(angles_rad[2]), 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
    ])
        # 组合旋转 (ZYX顺序: 先Z, 再Y, 最后X)
    original_matrix = Rx @ Ry @ Rz
    original_matrix[0:3, 3] = [1.0, 2.0, 3.0]  # 设置平移
        print("创建测试齐次矩阵:")
    print(f"预设欧拉角: X={angles_deg[0]}°, Y={angles_deg[1]}°, Z={angles_deg[2]}°")
    print(f"平移向量: [1.0, 2.0, 3.0]")
    print()
        # 测试不同的欧拉角顺序
    orders = ['xyz', 'zyx', 'yxz', 'zxy']
        results = {}
    for order in orders:
        euler_angles, reconstructed_matrix, error = verify_euler_transform(
            original_matrix, order=order, degrees=False
        )
        results[order] = {
            'euler_angles': euler_angles,
            'error': error,
            'success': error < 1e-10
        }
        print()
        # 总结结果
    print("不同欧拉角顺序的转换结果总结:")
    for order, result in results.items():
        status = "✓ 成功" if result['success'] else "✗ 失败"
        angles_deg = np.degrees(result['euler_angles'])
        print(f"顺序 {order}: {status}, 欧拉角: [{angles_deg[0]:.2f}°, {angles_deg[1]:.2f}°, {angles_deg[2]:.2f}°]")
if __name__ == "__main__":
    #  不同欧拉角顺序的转换
    demonstrate_with_different_orders()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210

import numpy as np
from scipy.spatial.transform import Rotation as R
import open3d as o3d
import copy
def demonstrate_gimbal_lock():
    """演示万向节锁现象"""
        print("=== 万向节锁演示 ===")
        # 情况1：正常情况 (Pitch = 30°)
    print("\n1. 正常情况 (Pitch = 30°):")
    euler_normal = [30, 30, 30]  # [Roll, Pitch, Yaw] in degrees
    R_normal = R.from_euler('zyx', euler_normal, degrees=True)
    print(f"欧拉角: {euler_normal}")
    print(f"旋转矩阵:\n{R_normal.as_matrix()}")
        # 反向转换验证
    euler_back = R_normal.as_euler('zyx', degrees=True)
    print(f"反向转换的欧拉角: {euler_back}")
    print(f"转换是否精确: {np.allclose(euler_normal, euler_back)}")
        # 情况2：万向节锁情况 (Pitch = 90°)
    print("\n2. 万向节锁情况 (Pitch = 90°):")
    euler_gimbal = [30, 90, 30]  # Pitch = 90° 导致万向节锁
    R_gimbal = R.from_euler('zyx', euler_gimbal, degrees=True)
    print(f"原始欧拉角: {euler_gimbal}")
    print(f"旋转矩阵:\n{R_gimbal.as_matrix()}")
        # 反向转换 - 会出现问题！
    euler_back_gimbal = R_gimbal.as_euler('zyx', degrees=True)
    print(f"反向转换的欧拉角: {euler_back_gimbal}")
    print(f"转换是否精确: {np.allclose(euler_gimbal, euler_back_gimbal)}")
        # 情况3：不同的欧拉角产生相同的旋转矩阵
    print("\n3. 不同欧拉角产生相同旋转矩阵:")
    euler_alternative = [60, 90, 0]  # 不同的欧拉角
    R_alternative = R.from_euler('zyx', euler_alternative, degrees=True)
    print(f"替代欧拉角: {euler_alternative}")
    print(f"旋转矩阵:\n{R_alternative.as_matrix()}")
    print(f"矩阵是否相同: {np.allclose(R_gimbal.as_matrix(), R_alternative.as_matrix())}")
def visualize_gimbal_lock():
    """可视化万向节锁效果"""
        # 创建坐标系
    coord_original = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0)
    coord_original.paint_uniform_color([0.5, 0.5, 0.5])  # 灰色-原始
        # 情况1：正常旋转
    coord_normal = copy.deepcopy(coord_original)
    euler_normal = [30, 30, 30]  # Roll=30°, Pitch=30°, Yaw=30°
    R_normal = R.from_euler('zyx', euler_normal, degrees=True)
    T_normal = np.eye(4)
    T_normal[0:3, 0:3] = R_normal.as_matrix()
    T_normal[0:3, 3] = [2, 0, 0]  # 平移
    coord_normal.transform(T_normal)
    coord_normal.paint_uniform_color([0, 1, 0])  # 绿色-正常
        # 情况2：万向节锁情况
    coord_gimbal1 = copy.deepcopy(coord_original)
    euler_gimbal1 = [30, 90, 30]  # Pitch=90° 导致万向节锁
    R_gimbal1 = R.from_euler('zyx', euler_gimbal1, degrees=True)
    T_gimbal1 = np.eye(4)
    T_gimbal1[0:3, 0:3] = R_gimbal1.as_matrix()
    T_gimbal1[0:3, 3] = [0, 2, 0]  # 平移
    coord_gimbal1.transform(T_gimbal1)
    coord_gimbal1.paint_uniform_color([1, 0, 0])  # 红色-万向节锁1
        # 情况3：不同的欧拉角，相同的最终方向
    coord_gimbal2 = copy.deepcopy(coord_original)
    euler_gimbal2 = [60, 90, 0]  # 不同的欧拉角，相同的旋转！
    R_gimbal2 = R.from_euler('zyx', euler_gimbal2, degrees=True)
    T_gimbal2 = np.eye(4)
    T_gimbal2[0:3, 0:3] = R_gimbal2.as_matrix()
    T_gimbal2[0:3, 3] = [2, 2, 0]  # 平移
    coord_gimbal2.transform(T_gimbal2)
    coord_gimbal2.paint_uniform_color([1, 1, 0])  # 黄色-万向节锁2
        print("可视化说明:")
    print("- 灰色: 原始坐标系")
    print("- 绿色: 正常旋转 [30, 30, 30]")
    print("- 红色: 万向节锁情况 [30, 90, 30]") 
    print("- 黄色: 不同欧拉角 [60, 90, 0] - 但与红色方向相同！")
        # 验证红色和黄色是否真的相同
    print(f"红色和黄色的旋转矩阵是否相同: {np.allclose(R_gimbal1.as_matrix(), R_gimbal2.as_matrix())}")
        o3d.visualization.draw_geometries(
        [coord_original, coord_normal, coord_gimbal1, coord_gimbal2],
        window_name="万向节锁可视化",
        width=800, height=600
    )
def analyze_gimbal_lock_math():
    """分析万向节锁的数学原理"""
        print("=== 万向节锁数学分析 ===")
        # 当Pitch = 90°时的旋转矩阵计算
    pitch = np.pi / 2  # 90度
        def rotation_matrix_zyx(yaw, pitch, roll):
        """计算ZYX欧拉角的旋转矩阵"""
        # 绕Z轴旋转
        Rz = np.array([
            [np.cos(yaw), -np.sin(yaw), 0],
            [np.sin(yaw),  np.cos(yaw), 0],
            [0,           0,           1]
        ])
                # 绕Y轴旋转
        Ry = np.array([
            [np.cos(pitch),  0, np.sin(pitch)],
            [0,              1, 0],
            [-np.sin(pitch), 0, np.cos(pitch)]
        ])
                # 绕X轴旋转
        Rx = np.array([
            [1, 0,           0],
            [0, np.cos(roll), -np.sin(roll)],
            [0, np.sin(roll),  np.cos(roll)]
        ])
                return Rz @ Ry @ Rx
        # 计算不同欧拉角但相同旋转矩阵的情况
    print("\n情况1: [yaw=30°, pitch=90°, roll=30°]")
    R1 = rotation_matrix_zyx(np.radians(30), np.radians(90), np.radians(30))
    print(f"旋转矩阵:\n{R1}")
        print("\n情况2: [yaw=60°, pitch=90°, roll=0°]")
    R2 = rotation_matrix_zyx(np.radians(60), np.radians(90), np.radians(0))
    print(f"旋转矩阵:\n{R2}")
        print(f"\n两个矩阵是否相同: {np.allclose(R1, R2)}")
        # 分析矩阵结构
    print("\n矩阵分析:")
    print("当pitch=90°时，旋转矩阵具有特殊结构:")
    print("第一行: [0, sin(yaw-roll), cos(yaw-roll)]")
    print("第二行: [0, cos(yaw-roll), -sin(yaw-roll)]") 
    print("第三行: [-1, 0, 0]")
    print("可见矩阵只依赖于(yaw-roll)的差值!")
def practical_implications():
    """万向节锁的实际影响"""
        print("=== 万向节锁的实际影响 ===")
        # 模拟动画或控制系统中的问题
    print("1. 动画系统中的问题:")
    print("   - 当角色抬头90度时，无法正确进行头部倾斜")
    print("   - 相机看向正上方时，无法平滑旋转")
        print("\n2. 控制系统中的问题:")
    print("   - 飞行器俯仰90度时，偏航和滚转控制混淆")
    print("   - 机械臂特定姿态下失去一个自由度")
        print("\n3. 解决方案:")
    print("   - 使用四元数(Quaternion)替代欧拉角")
    print("   - 使用旋转矩阵直接操作")
    print("   - 限制俯仰角范围(如±89度)")
    print("   - 在接近万向节锁时切换表示方法")
def demonstrate_quaternion_solution():
    """演示四元数作为解决方案"""
        print("=== 四元数解决方案 ===")
        # 万向节锁情况的欧拉角
    euler_gimbal = [30, 90, 30]
        # 使用欧拉角
    R_euler = R.from_euler('zyx', euler_gimbal, degrees=True)
    euler_back = R_euler.as_euler('zyx', degrees=True)
        # 使用四元数
    quat = R.from_euler('zyx', euler_gimbal, degrees=True).as_quat()
    R_quat = R.from_quat(quat)
    euler_from_quat = R_quat.as_euler('zyx', degrees=True)
        print(f"原始欧拉角: {euler_gimbal}")
    print(f"欧拉角反向转换: {euler_back}")
    print(f"四元数: {quat}")
    print(f"四元数转欧拉角: {euler_from_quat}")
        # 四元数的优势
    print("\n四元数优势:")
    print("1. 无万向节锁问题")
    print("2. 插值平滑 (SLERP)")
    print("3. 计算效率高")
    print("4. 数值稳定性好")
if __name__ == "__main__":
    # 演示基本概念
    demonstrate_gimbal_lock()
        # 可视化效果
    visualize_gimbal_lock()
        # 数学分析
    analyze_gimbal_lock_math()
        # 实际影响
    practical_implications()
        # 解决方案
    demonstrate_quaternion_solution()