剛體轉動牛 頓尤拉方程的 詳細討 論與實際應用

剛體轉動積分器 與

姿態估測

大綱• 從牛頓尤拉方程認識貼體角速度的角色。並

且推導出兩種積分剛體轉動的方式，並與Lagrange 法做比較。我們會以模擬陀螺運動的章動進動為比較這三種方法的平台。

• 第一種方法 : 剛體轉動積分器，遞推牛頓尤拉方程與貼角方向餘弦法

• 第二種方法 : 尤拉角 Lagrange 法 ( 學過 )• 第三種方法 : 姿態估測法，貼角方向餘弦積

分法

觀念釐清

1. 牛頓尤拉方程中的 xyz 下標是沿著 body 還是 space 座標取的分量 ? 所以 ωxyz是物體轉動角速度在 body座標觀察到的分量。 Body座標系是隨著物體而轉動的。第一個問題是 :既然 body座標隨著物體轉動，為什麼觀察到的物體角速度 ωxyz不是零 ?

2. 牛頓尤拉方程怎麼來 ? 從向量在 space (s) 與轉動中的 body (b) 座標線性變化而來。所以力矩 τs是沿著 space frame觀察到或取分量的力矩向量嘛。那我們代入 L=Iω，就會得到牛頓尤拉方程嘛。第二個問題，我們上一頁才說牛頓尤拉方程 xyz下標是沿著 body座標，怎麼在公式的源頭 τs變成沿著 space座標 ?

3. 向量變換公式怎麼來 ? 其中的 ω是甚麼 ?是轉動中的 non-inertial座標 b (body)相對於靜止 inertial座標 s (space)的轉動角速度嘛。相對於 space，所以是 ωs嘛。所以對應於此外積項的牛頓尤拉方程的最後一項，應該要有一個 ωs是對應 space座標，怎麼兩個都是對應到 body座標 ??

觀念問題• 若我觀測者隨著 body frame 轉，則我觀察到的

body 的自身角速度是不是零 ? 向量沿著某一座標取分量，是否代表著在該座標觀察到的測量值是那向量的三個分量 ?

• 角速度感測器都隨著物體而轉 (strap-down angular rate sensor ，如手機裡的陀螺儀 ) ，那角速度感測器量到的角速度是甚麼 ? 是待測物的轉動角速度沿著 body frame 的分量 ? 是不是零 ??! 它量到的角速度是甚麼挖歌 ? 是貼體角速度嗎 ??

整理一下• 牛頓尤拉方程的力矩項是沿著 body 取分量，

但是公式的源頭的力矩項是沿著 space 取分量。怎麼回事 ?

• 貼體角速度 ωb到底是甚麼東西 ?

• 貼體角速度 ωb要怎麼量測 ?要怎麼理解 ?有沒有辦法應用 ?

• 貼體角速度 ωb跟向量變化公式中的 ωs是否一樣 ?

• 角速度感測器量到的角速度是甚麼東西 ?

貼體角速度的剖析

𝑟 ′=𝐴 (𝑑 Ω⃗ )×𝑟

A 即為 Rodrigues rotation formula 的矩陣型式，我們稱 Cayley-Klein parameters

• 證明了當 body frame 從 space frame 轉動一微小角度的時候， ωs是 body frame相對space frame的轉動角速度沿著 space frame的分量。

• 而且我們可以用同一個ωs來求得的從space轉到 body的轉換矩陣

• 設定 xyz 軸為剛體特徵軸，這樣 L 中的 I才是 diagonal 的。我們將向量變換公式應用上一段微小的轉動 ti -> ti+1

• 在 ti 時間， bz 與 Sz 重和，在 ti+1 時間， bz 轉動一微小角度• 這樣子 Body xyz 軸的瞬時轉動角速度 = 剛體瞬時轉動角

速度• 這樣設定之後，向量變換公式 中的 ω就是剛體瞬時角速度

ti

ti+1

• 下一時間再把 space及 body 移到下一個ti+1 -> ti+2 ，所以 space frame 會持續改變。

• 假設我們知道初始貼體角速度及剛體特徵軸位置ωb(t0)， xyz(t0)，

=CK(ωb(t0))

ti+1

ti+2

• 接著在新的位置 Sz (ti+1)數值解牛頓尤拉方程得到ωb(t1)

ti+1

ti+2

• 再用之前相同的公式求得 bz(t1)，疊代之後就會得到

小結• 一般認為貼體角速度無太大用處，事實上

貼角可用來積分剛體轉動運動。 (ps. 剛體轉動積分器原理。 )

• 不須帶入尤拉角解 Lagrange 方程也可模擬剛體運動。

• 準不準呢 ? 我們就把公式寫成 python 程式，然後跟 Lagrange 法比較看看 !

DEMO-1

• Run file Gyroscope-TeachDemo-1.py• 程式 EulerDCMiter() ，就是第一種方法• 完全不須用到尤拉角轉換或解 ODE ，比

Lagrange 法簡單且快速• 應用 ? 不需要一台 PC 做運算，一小塊 IC 就

可以。可攜式裝置 Portable device 。• Demo Space cone Body cone 。• 觀察 A 方法產生的四種陀螺章動進動。

DEMO-2

• AB 方法的比較，貼角積分法 vs Lagrange 法• Run file Gyroscope-TeachDemo-3.py ，說明

DrawOption• 看其他陀螺章進動的特殊情況，說明

numerical drift ，偏移，校準，演算融合fusion 。

姿態估測法 – 貼體角速度的直接堆疊

DEMO-3

• BC 方法， Lagrange 法 vs 姿態估測法• Run file Gyroscope-TeachDemo-4.py ，介紹

directDCMiter()• 用 Lagrange 法得到的貼角來積分• 此三行就是姿態感測器中的解算步驟• 說明誤差• 加噪音• 加濾波，北京飛創智能公司介紹

北京恒泰飞创智能科技• 北京北航天汇科技孵化器有限公司在孵企业 – 創業於2014/1• 北京航空航天大学技轉• 其鎮店之寶為 –

串口6 軸加速度計 / 陀螺儀 MPU6050 模塊

• 模塊內部集成了姿態解算器，配合動態卡爾曼濾波算法，能夠在動態環境下准確輸出模塊的當前姿態，姿態測量精度 0.01 度，穩定性極高，性能甚至優於某些專業的傾角儀！

• 趙文 :毕业于北京航空航天大学宇航学院飞行器设计专业（本科博士）。在组合导航算法、嵌入式控制系统、软硬件设计领域有丰富的经验与成果。主要成果有多项组合导航算法产品与专项技术。

• http://blog.sina.com.cn/zhaowenwin

應用舉例• 四軸飛行器• 相機手震穩定器• 機械手臂• 導航

三種方法的適用場合與優缺點

問題的解答• 貼體角速度，一般並不等於轉動坐標系中觀察到

的物體角速度，角速度沿 body 的分量不等於body 中的觀測值。

• 但是當我們使用牛頓尤拉方程 (使用了向量變化式中 t 時間 s 與 b frame 的重合假設 ) ，那麼貼體角速度就是 body 軸的瞬時角速度。

• 而角度感測器測量的就是軸的瞬時角速度，因此貼體角速度會等於轉動坐標系中的感測器觀測到的角速度。因此貼體角速度 = 角速度感測器的觀察值。

總結• 貼體角速度就是向量變化公式中的 ω• 貼體角速度就是角速度感測器量到的物理量

• 當 body xyz軸取在物體特徵軸上，貼體角速度就是 body xyz軸的瞬時角速度，並且可以直接拿來做角速度的積分來模擬剛體轉動。