imu-pose-estimation-guide/simple.html at master · Go-deep-c/imu-pose-estimation-guide · GitHub

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
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
<!DOCTYPE html>
<html lang="zh-CN">
<head>
    <meta charset="UTF-8">
    <meta name="viewport" content="width=device-width, initial-scale=1.0">
    <title>IMU位姿推算</title>
    <link rel="preconnect" href="https://fonts.googleapis.com">
    <link rel="preconnect" href="https://fonts.gstatic.com" crossorigin>
    <link href="https://fonts.googleapis.com/css2?family=Noto+Serif+SC:wght@400;600&family=JetBrains+Mono:wght@400&display=swap" rel="stylesheet">
    <link rel="stylesheet" href="https://cdn.jsdelivr.net/npm/katex@0.16.9/dist/katex.min.css">
    <style>
        :root {
            /* 米白色调 - 使用OKLCH色彩空间，带暖色调 */
            --bg: oklch(0.985 0.008 75);           /* 米白背景，带微暖色调 */
            --bg-card: oklch(0.995 0.004 75);      /* 卡片背景 */
            --bg-code: oklch(0.970 0.010 75);      /* 代码背景 */
            --bg-formula: oklch(0.980 0.012 60);   /* 公式背景，带琥珀色调 */

            --text: oklch(0.220 0.015 25);         /* 深褐文字 */
            --text-secondary: oklch(0.400 0.012 25); /* 次要文字 */
            --text-muted: oklch(0.550 0.010 25);   /* 弱化文字 */

            --border: oklch(0.880 0.015 75);       /* 边框 */
            --border-light: oklch(0.920 0.010 75); /* 浅边框 */

            --accent: oklch(0.550 0.140 35);       /* 深褐强调色 */
            --accent-light: oklch(0.700 0.080 35); /* 浅强调色 */

            /* 间距系统 - 4pt基准 */
            --space-xs: 0.25rem;
            --space-sm: 0.5rem;
            --space-md: 1rem;
            --space-lg: 1.5rem;
            --space-xl: 2rem;
            --space-2xl: 3rem;
        }

        * { margin: 0; padding: 0; box-sizing: border-box; }

        body {
            font-family: 'Noto Serif SC', Georgia, serif;
            background: var(--bg);
            color: var(--text);
            line-height: 1.75;
            padding: var(--space-2xl) var(--space-lg);
            max-width: 860px;
            margin: 0 auto;
            -webkit-font-smoothing: antialiased;
        }

        h1 {
            font-size: 1.75rem;
            font-weight: 600;
            margin-bottom: var(--space-xl);
            color: var(--text);
            padding-bottom: var(--space-sm);
            border-bottom: 1px solid var(--border);
        }

        h2 {
            font-size: 1.25rem;
            font-weight: 600;
            margin: var(--space-2xl) 0 var(--space-lg);
            color: var(--text);
        }

        h3 {
            font-size: 1.05rem;
            font-weight: 600;
            margin: var(--space-xl) 0 var(--space-md);
            color: var(--text);
        }

        p {
            margin-bottom: var(--space-md);
            text-align: justify;
        }

        .box {
            background: var(--bg-card);
            border: 1px solid var(--border-light);
            border-radius: 6px;
            padding: var(--space-lg);
            margin: var(--space-lg) 0;
        }

        .flow {
            font-family: 'JetBrains Mono', monospace;
            font-size: 0.78rem;
            line-height: 1.6;
            background: var(--bg-code);
            padding: var(--space-lg);
            border-radius: 4px;
            overflow-x: auto;
            white-space: pre;
            margin: var(--space-md) 0;
            color: var(--text-secondary);
        }

        table {
            width: 100%;
            border-collapse: collapse;
            margin: var(--space-md) 0;
            font-size: 0.88rem;
        }

        th, td {
            padding: var(--space-sm) var(--space-md);
            text-align: left;
            border-bottom: 1px solid var(--border-light);
        }

        th {
            background: var(--bg-code);
            font-weight: 600;
            color: var(--text);
        }

        tr:hover td {
            background: oklch(0.980 0.006 75);
        }

        /* 公式块 - 使用背景色而非side-stripe */
        .formula {
            background: var(--bg-formula);
            padding: var(--space-md) var(--space-lg);
            margin: var(--space-md) 0;
            border-radius: 4px;
        }

        .formula p {
            margin-bottom: var(--space-xs);
            color: var(--text-secondary);
            font-size: 0.85rem;
        }

        /* 注释块 - 使用背景色而非side-stripe */
        .note {
            background: oklch(0.975 0.010 220);
            padding: var(--space-sm) var(--space-md);
            margin: var(--space-md) 0;
            border-radius: 4px;
            font-size: 0.88rem;
            color: var(--text-secondary);
        }

        code {
            font-family: 'JetBrains Mono', monospace;
            background: var(--bg-code);
            padding: 2px 6px;
            border-radius: 3px;
            font-size: 0.82em;
        }

        hr {
            border: none;
            height: 1px;
            background: var(--border-light);
            margin: var(--space-xl) 0;
        }

        /* KaTeX公式样式优化 */
        .katex {
            font-size: 1.02em !important;
        }

        /* 响应式 */
        @media (max-width: 600px) {
            body {
                padding: var(--space-lg) var(--space-md);
            }
            h1 { font-size: 1.5rem; }
            h2 { font-size: 1.15rem; }
        }
    </style>
</head>
<body>

<h1>IMU位姿推算原理</h1>

<div class="box">
<p><strong>问题</strong>：使用三轴加速度计和三轴陀螺仪进行位姿推算。</p>
<p><strong>核心思路</strong>：陀螺仪测量角速度 → 推算姿态；加速度计测量线加速度 → 推算位置。</p>
</div>

<h2>一、姿态推算</h2>

<h3>1. 流程框图</h3>
<div class="flow">陀螺仪(ω) → 误差补偿 → 四元数更新 → 姿态输出(q)
                ↓
         加速度计辅助修正(可选)</div>

<h3>2. 核心公式</h3>

<div class="formula">
<p>四元数微分方程</p>
<div id="f1"></div>
</div>

<div class="formula">
<p>展开形式</p>
<div id="f2"></div>
</div>

<div class="formula">
<p>离散更新</p>
<div id="f3"></div>
</div>

<div class="formula">
<p>归一化</p>
<div id="f4"></div>
</div>

<h3>3. 符号说明</h3>
<table>
<tr><th>符号</th><th>含义</th></tr>
<tr><td><code>q = [q₀, q₁, q₂, q₃]ᵀ</code></td><td>单位四元数，q₀为标量部分</td></tr>
<tr><td><code>ω = [ωₓ, ωᵧ, ωᵤ]</code></td><td>三轴角速度 (rad/s)</td></tr>
<tr><td><code>⊗</code></td><td>四元数乘法</td></tr>
<tr><td><code>Δt</code></td><td>采样周期</td></tr>
</table>

<div class="note">四元数避免欧拉角的万向节死锁问题，计算效率高。</div>

<hr>

<h2>二、位置推算</h2>

<h3>1. 流程框图</h3>
<div class="flow">加速度计(a) → 去除重力 → 坐标转换 → 速度积分 → 位置积分
                              ↑
                    需要姿态矩阵 Cᵇₙ</div>

<h3>2. 核心公式</h3>

<div class="formula">
<p>Step 1: 去除重力</p>
<div id="f5"></div>
</div>

<div class="formula">
<p>Step 2: 坐标转换</p>
<div id="f6"></div>
</div>

<div class="formula">
<p>Step 3: 速度更新</p>
<div id="f7"></div>
</div>

<div class="formula">
<p>Step 4: 位置更新</p>
<div id="f8"></div>
</div>

<h3>3. 步骤说明</h3>
<table>
<tr><th>步骤</th><th>说明</th></tr>
<tr><td>重力补偿</td><td>加速度计测量值包含重力分量，必须去除</td></tr>
<tr><td>坐标转换</td><td>将载体系加速度转换到导航系</td></tr>
<tr><td>二次积分</td><td>加速度 → 速度 → 位置</td></tr>
<tr><td>误差累积</td><td>位置误差随 t² 增长（IMU固有缺陷）</td></tr>
</table>

<hr>

<h2>三、完整系统框图</h2>
<div class="flow">  陀螺仪 ──ω──→ 姿态推算 ──→ q, Cᵇₙ
                                    │
                                    ↓
  加速度计 ──a──→ 重力补偿 ──→ 坐标转换 ──→ 位置推算 ──→ p, v</div>

<hr>

<h2>四、关键问题</h2>
<table>
<tr><th>问题</th><th>影响</th><th>改进方法</th></tr>
<tr><td>陀螺零偏</td><td>姿态漂移</td><td>零偏估计补偿</td></tr>
<tr><td>加速度零偏</td><td>位置二次漂移</td><td>零速修正(ZUPT)</td></tr>
<tr><td>误差累积</td><td>长时间发散</td><td>融合GPS/视觉</td></tr>
</table>

<hr>

<h2>五、参考文献</h2>
<p>1. Titterton D, Weston J. <em>Strapdown Inertial Navigation Technology</em>. AIAA, 2004.</p>
<p>2. 秦永元. <em>惯性导航</em>. 科学出版社, 2014.</p>

<script src="https://cdn.jsdelivr.net/npm/katex@0.16.9/dist/katex.min.js"></script>
<script>
const formulas = {
    f1: '\\dot{q} = \\frac{1}{2} q \\otimes \\omega',
    f2: '\\begin{bmatrix} \\dot{q}_0 \\\\ \\dot{q}_1 \\\\ \\dot{q}_2 \\\\ \\dot{q}_3 \\end{bmatrix} = \\frac{1}{2} \\begin{bmatrix} 0 & -\\omega_x & -\\omega_y & -\\omega_z \\\\ \\omega_x & 0 & \\omega_z & -\\omega_y \\\\ \\omega_y & -\\omega_z & 0 & \\omega_x \\\\ \\omega_z & \\omega_y & -\\omega_x & 0 \\end{bmatrix} \\begin{bmatrix} q_0 \\\\ q_1 \\\\ q_2 \\\\ q_3 \\end{bmatrix}',
    f3: 'q_{k+1} = q_k + \\dot{q}_k \\cdot \\Delta t',
    f4: 'q = \\frac{q}{\\|q\\|} = \\frac{q}{\\sqrt{q_0^2 + q_1^2 + q_2^2 + q_3^2}}',
    f5: 'a_{body} = a_{measured} - C_n^b \\cdot g_{nav}, \\quad g_{nav} = [0, 0, g]^T',
    f6: 'a_{nav} = C_b^n \\cdot a_{body}',
    f7: 'v_{k+1} = v_k + a_{nav} \\cdot \\Delta t',
    f8: 'p_{k+1} = p_k + v_k \\cdot \\Delta t + \\frac{1}{2} a_{nav} \\cdot \\Delta t^2'
};
for (let id in formulas) {
    katex.render(formulas[id], document.getElementById(id), { displayMode: true, throwOnError: false });
}
</script>
</body>
</html>