旋转的表达

从二维开始

从高中学圆锥曲线时，老师提过爱心曲线的表达为：

即两个分别旋转了45°和-45°的椭圆 $x^2+3y^2=1$ 的叠加。

那时我们就知道，在坐标系上的一个点，绕原点逆时针旋转 $\theta$ 角后，坐标变换为：

在学习了线性代数后，我们知道，这个变换可以表示为矩阵乘法：

类似地，在之后学习到复数后，我们知道，复数的乘法可以表示为：

那么，我们可以将旋转看作是复数的乘法，比如对于某个复数z，其旋转 $\theta$ 角后，可以表示为：

当然，有了复平面的表达方式，我们可以推导出在二维平面上向量 $\mathbf v$ ，绕点 $\mathbf k=(x_k,y_k)$ 旋转 $\theta$ 角后的向量：

  • 先把 $\mathbf v$ 平移到原点，然后绕原点旋转 $\theta$ 角，最后再平移回 $\mathbf k$ 即可得到：

到三维

各个坐标轴的旋转矩阵（欧拉角）

我们先考虑只绕一个旋转轴旋转的情况，我们先求绕 $z$ 轴旋转：

绕z轴旋转

如图所示，对于向量 $\vec{a}=(x,y,z)$，绕 $z$ 轴旋转 $\theta$ 角后，z轴的坐标不变，那么不妨做向量在xOy平面的投影，如下图：

绕z轴旋转在xOy平面的投影

不难看出，实际上即是在二维平面xOy上，将向量 $\vec{a}$ 绕原点旋转 $\theta$ 角，即其旋转矩阵T为：

那么，对于三维空间中的向量 $\vec{a}$，其旋转矩阵R为：

相似地，对于绕 $y$ 和 $x$ 轴旋转，其旋转矩阵T为：

以上角都按照右手螺旋法则的旋转方向取正。

如果分别用 $\alpha \ , \ \beta \ , \ \gamma $ 取代 $\theta_x \ , \ \theta_y \ , \ \theta_z$ 即为欧拉角，按照右手螺旋定则有下图：

alt text

那么按照 $R_z(\gamma)R_y(\beta)R_x(\alpha)$ 的xyz顺序旋转，即得到欧拉角旋转矩阵：

该旋转是按照zyx顺序旋转的，即先绕x轴旋转 $\alpha$ 角，再绕y轴旋转 $\beta$ 角，最后绕z轴旋转 $\gamma$ 角。

还可以按照zxz’的顺序旋转，即先绕z轴旋转 $\gamma$ 角，再绕x轴旋转 $\alpha$ 角，最后绕z轴旋转 $\gamma’$ 角，那么旋转矩阵为：

alt text

如上图所示，这个就是zxz’顺序旋转的变换结果。

以下是按照不同的旋转顺序旋转后的结果：

Proper Euler angles Tait–Bryan angles

一定要注意，旋转的变换矩阵是右乘的，即 $\mathbf v’=\mathbf R \mathbf v$ （即左乘），多次按照 $\mathbf R_1 \mathbf R_2 \cdots \mathbf R_n$ 的顺序旋转后，坐标变换为：

旋转矩阵的变换描述的是是旋转的结果，并不是旋转的过程。

并且正交旋转矩阵满足：

某个定轴的旋转矩阵（轴-角）

以下推理内容参考至Rodrigues’ rotation formula

我们设向量 $\mathbf v$ 绕某一定轴 $\mathbf k$ 逆时针旋转 $\theta$ 角（其中 $\mathbf k$ 为单位向量），下面来推导旋转后向量 $\mathbf v’$ 的表达式。

alt text

如上图我们可以知道，向量 $\mathbf v$ 可以沿着 $\mathbf k$ 的平行方向和其平面方向分解为两个向量 和 ：

类似的对于旋转后的向量 $\mathbf v’$ 由平行方向的分量不变，我们可以得到：

那么，我们只需要求出 $\mathbf v’_\perp$ 即可，如下图，由向量旋转有：

alt text

那么，我们可以得到：

如果在直角坐标系中，$\mathbf k=(k_x,k_y,k_z)$，$\mathbf v=(v_x,v_y,v_z)$，那么 $\mathbf k \times \mathbf v$ 用矩阵可以表示为：

又令叉乘矩阵，

于是有：

于是，我们可以得到：

其中，旋转矩阵 $\mathbf R = \mathbf E+\sin \theta \mathbf K+(1-\cos \theta)\mathbf K^2$ 为：

实际上用欧拉角和这个定轴旋转的方法是等效的，我们取 $\mathbf R(\alpha,\beta,\gamma)$ 最左下角的元素 $-\sin \beta$，在 $\mathbf R(\mathbf k,\theta)$ 中也取一样的元素 ：

就可以在给定 $\mathbf k$ 和 $\theta$ 的情况下求出 $\beta$ ，然后就可以求出 $\mathbf R(\alpha,\beta,\gamma)$ 。反过来，我们也可以在给定 $\mathbf R(\alpha,\beta,\gamma)$ 的情况下求出 $\mathbf k$ 和 $\theta$ 。

从欧拉角表示到轴角表示

给定zyx顺序的欧拉角 $\mathbf R(\alpha,\beta,\gamma)$ ，我们可以求出旋转轴 $\mathbf k = (k_z,k_y,k_x)^T$ 和旋转角 $\theta$ ：

我们知道zyx顺序的欧拉角：

先计算zyx旋转矩阵的迹：

轴-角旋转矩阵的迹：

于是，我们可以得到旋转角 $\theta$ ：

于是旋转轴 $\mathbf k$ 可以由下面的公式计算：

四元数

四元数是一种复数，其形式为 $q = w + xi + yj + zk$ ，其中 $w,x,y,z$ 为实数，$i,j,k$ 为虚数单位，满足 $i^2 = j^2 = k^2 = ijk = -1$ 。

四元数为什么可以表示三维的旋转呢？为什么不是三元呢？

这里引入一个Frobenius 定理（1878），在实数域上，有限维可除代数（即乘法、可逆、结合律）有且仅有三种：

  • 实数域
  • 复数域
  • 四元数域

表示的旋转的3个自由度是单位四元数的3个自由度，单位四元数的极坐标[1,(α,β,γ)]，也只有3个自由度Δ(α,β,γ)，每个点[r,(α,β,γ)]在具有3维表面的光滑四维球面做无穷小旋转d(α,β,γ)的时候，近似于沿着4维空间中相切的3维超平面移动了rd(α,β,γ)，有3个自由度(dα,dβ,dγ)。或者设想在无穷远处，四维球近似变成3维超平面。

或者说，平时说的三维旋转并不是真正的三维旋转，是多个二维平面的旋转叠加在之前的### 各个坐标轴的旋转矩阵（欧拉角）这一节就有体现。

在使用四元数进行三维旋转时，其实部即a，并不会改变，而虚部即i，j，k才会改变，而i，j，k正是三维空间中的旋转轴，所以四元数可以表示三维空间中的旋转。

如果要强行使用 $a+bi+cj$ 来表示三维的旋转在做乘法时，请问ij等于多少？如果直接定义为0，那么ij就失去了旋转的意义，如果ij不等于0，那么ij就变成了一个三维向量，那么这个三维向量又代表了什么？所以，三维的旋转必须使用四元数 $a+bi+cj+dk$ 来表示。

四元数的定轴旋转

给定旋转轴 $\mathbf k = (k_x,k_y,k_z)^T$ 和旋转角 $\theta$ ，我们可以得到旋转四元数 $q = \cos \frac{\theta}{2} + \sin \frac{\theta}{2} \mathbf k$ 。

给定旋转四元数 $q = w + xi + yj + zk$ ，我们可以得到旋转轴 $\mathbf k = (k_x,k_y,k_z)^T$ 和旋转角 $\theta$ ：

四元数与旋转矩阵的转换

给定旋转四元数 $q = w + xi + yj + zk$ ，我们可以得到旋转矩阵 $\mathbf R$ ：

给定旋转矩阵 $\mathbf R$ ，我们可以得到旋转四元数 $q = w + xi + yj + zk$ ：

四元数与欧拉角的转换

给定旋转四元数 $q = w + xi + yj + zk$ ，我们可以得到欧拉角 $\mathbf R(\alpha,\beta,\gamma)$ ：

给定欧拉角 $\mathbf R(\alpha,\beta,\gamma)$ ，我们可以得到旋转四元数 $q = w + xi + yj + zk$ ：

实际上四元数这种，方便解决问题，但不好直观描述的数学结构。我反正是更喜欢用矩阵😾

写到最后

由于本人对旋转群和李群的理解还不够深入，暂时对于四元数的推导和证明就不再深入了，以后有机会再补充。

而且，写这个博客主要是为了学习处理传感器数据时的坐标变换，所以旋转矩阵和欧拉角就足够了。

以下是我用于接收传感器数据的代码：

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
#!/home/META_Pi/LoRAcom/env/bin/python
import serial
import time
import pandas as pd
import math
import threading
import numpy as np
from functools import lru_cache


P0 = 101325.0  # Sea level pressure in Pa
T0 = 288.15    # Sea level temperature in K
L = 0.0065
g = 9.81       # Gravity acceleration in m/s²
M = 0.029
R = 8.314
exponent = g * M / (R * L)


ser = serial.Serial('/dev/ttyAMA0', 115200, timeout=1)
data_storage = []
exit_flag = False


prev_time = time.time()
prev_velocity = {'x': 0, 'y': 0, 'z': 0}
prev_angle_velocity = {'x': 0, 'y': 0, 'z': 0}
prev_angle = {'x': 0, 'y': 0, 'z': 0}
prev_position = {'x': 0, 'y': 0, 'z': 0}

# Calibration values for accelerometer bias (should be determined at startup)
accel_bias = {'x': 0, 'y': 0, 'z': 0}
is_calibrated = False
calibration_samples = []
CALIBRATION_SAMPLES_NEEDED = 50
ACCEL_STATIONARY_THRESHOLD = 0.1  # m/s²

# Cache rotation matrices for faster computation
@lru_cache(maxsize=128)
def rotation_matrix(angle_x, angle_y, angle_z):
    """
    Create rotation matrix from Euler angles (in degrees)
    Using the ZYX rotation order (yaw-pitch-roll)
    """
    # Convert angles from degrees to radians
    angle_x_rad = math.radians(angle_x)
    angle_y_rad = math.radians(angle_y)
    angle_z_rad = math.radians(angle_z)

    # Pre-compute sine and cosine values
    cos_x, sin_x = math.cos(angle_x_rad), math.sin(angle_x_rad)
    cos_y, sin_y = math.cos(angle_y_rad), math.sin(angle_y_rad)
    cos_z, sin_z = math.cos(angle_z_rad), math.sin(angle_z_rad)

    # Direct computation of combined rotation matrix
    # This is more efficient than creating 3 matrices and multiplying them
    r00 = cos_y * cos_z
    r01 = sin_x * sin_y * cos_z - cos_x * sin_z
    r02 = cos_x * sin_y * cos_z + sin_x * sin_z

    r10 = cos_y * sin_z
    r11 = sin_x * sin_y * sin_z + cos_x * cos_z
    r12 = cos_x * sin_y * sin_z - sin_x * cos_z

    r20 = -sin_y
    r21 = sin_x * cos_y
    r22 = cos_x * cos_y

    return np.array([
        [r00, r01, r02],
        [r10, r11, r12],
        [r20, r21, r22]
    ])

def transform_acceleration(accel_x, accel_y, accel_z, angle_x, angle_y, angle_z):
    """
    Transform acceleration from sensor's local coordinate system to global coordinate system
    and remove gravity component
    """
    # Use high precision for angles to ensure accurate transformation
    angle_x_rounded = round(angle_x, 5)
    angle_y_rounded = round(angle_y, 5)
    angle_z_rounded = round(angle_z, 5)

    R = rotation_matrix(angle_x_rounded, angle_y_rounded, angle_z_rounded)

    # Transform sensor accelerations to global frame
    accel_global_x = R[0, 0] * accel_x + R[0, 1] * accel_y + R[0, 2] * accel_z
    accel_global_y = R[1, 0] * accel_x + R[1, 1] * accel_y + R[1, 2] * accel_z
    accel_global_z = R[2, 0] * accel_x + R[2, 1] * accel_y + R[2, 2] * accel_z

    # Remove gravity component (gravity points in -z direction in global frame)
    accel_global_z += g

    # Remove calibrated bias
    if is_calibrated:
        accel_global_x -= accel_bias['x']
        accel_global_y -= accel_bias['y']
        accel_global_z -= accel_bias['z']

    return accel_global_x, accel_global_y, accel_global_z

def is_stationary(accel_x, accel_y, accel_z):
    """
    Detect if the sensor is stationary based on acceleration magnitude
    """
    accel_magnitude = math.sqrt(accel_x**2 + accel_y**2 + accel_z**2)
    return accel_magnitude < ACCEL_STATIONARY_THRESHOLD

def calibrate_accelerometer():
    """
    Calibrate the accelerometer by averaging readings when stationary
    """
    global accel_bias, is_calibrated, calibration_samples

    if len(calibration_samples) >= CALIBRATION_SAMPLES_NEEDED:
        # Calculate average acceleration in global frame
        avg_x = sum(sample['x'] for sample in calibration_samples) / len(calibration_samples)
        avg_y = sum(sample['y'] for sample in calibration_samples) / len(calibration_samples)
        avg_z = sum(sample['z'] for sample in calibration_samples) / len(calibration_samples)

        # Set bias values
        accel_bias = {'x': avg_x, 'y': avg_y, 'z': avg_z}
        is_calibrated = True
        print(f"Accelerometer calibrated. Bias: x={avg_x:.6f}, y={avg_y:.6f}, z={avg_z:.6f}")

def listen_for_input():
    global exit_flag
    while not exit_flag:
        user_input = input()
        if user_input == '':
            exit_flag = True

input_thread = threading.Thread(target=listen_for_input)
input_thread.daemon = True
input_thread.start()

if ser.is_open:
    print("ONline\n")
    print("====== Start loading data ======")
    print("Calibrating accelerometer, please keep the sensor stationary...")

    while not exit_flag:
        if ser.in_waiting > 0:
            data = ser.readline().decode('utf-8', errors='ignore').strip()
            cur_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime()) + f".{int(time.time() * 1000) % 1000}"
            process_start_time = time.time()

            try:
                parts = data.split('/')
                p = float(parts[6])  # Pressure
                h = (T0 / L) * (1 - math.exp(math.log(p / P0) / exponent))

                # Raw sensor data (angles in degrees, acceleration in m/s²)
                angle_x = float(parts[0])
                angle_y = float(parts[1])
                angle_z = float(parts[2])
                accel_x_sensor = float(parts[3])
                accel_y_sensor = float(parts[4])
                accel_z_sensor = float(parts[5])

                # Transform acceleration to global coordinates and remove gravity
                accel_x, accel_y, accel_z = transform_acceleration(
                    accel_x_sensor, accel_y_sensor, accel_z_sensor,
                    angle_x, angle_y, angle_z
                )

                # If not calibrated yet, collect calibration samples
                if not is_calibrated:
                    if len(calibration_samples) < CALIBRATION_SAMPLES_NEEDED:
                        # Add transformed acceleration to calibration samples
                        calibration_samples.append({'x': accel_x, 'y': accel_y, 'z': accel_z})
                        if len(calibration_samples) >= CALIBRATION_SAMPLES_NEEDED:
                            calibrate_accelerometer()
                    continue  # Skip integration during calibration

                cur_time_seconds = time.time()
                delta_t = cur_time_seconds - prev_time
                prev_time = cur_time_seconds

                # Check if sensor is stationary
                if is_stationary(accel_x, accel_y, accel_z):
                    # Reset velocity when stationary to prevent drift
                    velocity_x = 0
                    velocity_y = 0
                    velocity_z = 0
                else:
                    # Integrate acceleration to get velocity (in global coordinates)
                    velocity_x = prev_velocity['x'] + accel_x * delta_t
                    velocity_y = prev_velocity['y'] + accel_y * delta_t
                    velocity_z = prev_velocity['z'] + accel_z * delta_t

                # Integrate velocity to get position (in global coordinates)
                position_x = prev_position['x'] + velocity_x * delta_t
                position_y = prev_position['y'] + velocity_y * delta_t
                position_z = prev_position['z'] + velocity_z * delta_t

                data_storage.append({
                    'cur_time': cur_time,
                    'Accel_x_sensor': accel_x_sensor, 'Accel_y_sensor': accel_y_sensor, 'Accel_z_sensor': accel_z_sensor,
                    'Accel_x': accel_x, 'Accel_y': accel_y, 'Accel_z': accel_z,
                    'Velocity_x': velocity_x, 'Velocity_y': velocity_y, 'Velocity_z': velocity_z,
                    'Position_x': position_x, 'Position_y': position_y, 'Position_z': position_z,
                    'Angle_x': angle_x, 'Angle_y': angle_y, 'Angle_z': angle_z,
                    'Pressure': p,
                    'Height': h,
                    'Is_stationary': is_stationary(accel_x, accel_y, accel_z)
                })

                # Update previous values for the next iteration
                prev_velocity = {'x': velocity_x, 'y': velocity_y, 'z': velocity_z}
                prev_position = {'x': position_x, 'y': position_y, 'z': position_z}

                process_end_time = time.time()
                process_time_ms = (process_end_time - process_start_time) * 1000
                print(f"[{cur_time}] received: {data} (processed in {process_time_ms:.2f}ms)")

                if process_time_ms > 1.0:
                    print(f"WARNING: Processing time exceeded 1ms threshold: {process_time_ms:.2f}ms")

            except Exception as e:
                print(f"ERROR ON {data}, LOG: {e}")

else:
    print("Offline")

# Saving data to Excel
over_time = time.strftime("%Y-%m-%d %H-%M-%S", time.localtime())
df = pd.DataFrame(data_storage)
df.to_excel(f'{over_time}.xlsx', index=False)
print("\n===== Data was stored in Excel =====")