[Title]: Notes for ``A micro Lie theory for state estimation in robotics’’ - 公式详细推导 (IV)
“A micro Lie theory for state estimation in robotics” 1 这篇论文中比较难的是几个Jacobian矩阵的推导,也是这个笔记的主要内容.
在下面推导笔记中,形如 = ( 31 ) \overset{(31)}= =(31) 或 ⇒ ( 31 ) \overset{(31)}\Rightarrow ⇒(31) 意味着该步推导依据的是论文1 中 eq(31).
— Let’s continue —
XI About eq(180) in “Apendix D - the 3D rigid motion group S E ( 3 ) SE(3) SE(3)”
参考"State Estimation for Robotics"2 ,给出了
S
E
(
3
)
SE(3)
SE(3) 上的 Jacobian
J
l
(
ξ
)
=
∑
n
=
0
∞
1
(
n
+
1
)
!
(
ξ
⋏
)
n
{\bf J}_l(\boldsymbol \xi) = \sum_{n=0}^{\infty}\frac{1}{(n+1)!}({\boldsymbol \xi} ^{\curlywedge}) ^n
Jl(ξ)=n=0∑∞(n+1)!1(ξ⋏)n
其中
ξ
=
[
ρ
θ
]
∈
R
6
,
ξ
⋏
=
[
θ
∧
ρ
∧
0
θ
∧
]
∈
R
6
×
6
{\boldsymbol \xi} = \begin{bmatrix} \boldsymbol \rho \\ \boldsymbol \theta \end{bmatrix} \in \mathbb{R}^{6},\qquad {\boldsymbol \xi} ^{\curlywedge} = \begin{bmatrix} \boldsymbol \theta^{\wedge} & \boldsymbol \rho ^{\wedge}\\ 0 &\boldsymbol \theta^{\wedge} \end{bmatrix} \in \mathbb{R}^{6\times 6}
ξ=[ρθ]∈R6,ξ⋏=[θ∧0ρ∧θ∧]∈R6×6
上式子写成
J
l
(
ρ
,
θ
)
=
∑
n
=
0
∞
1
(
n
+
1
)
!
[
θ
∧
ρ
∧
0
θ
∧
]
n
=
[
J
l
(
θ
)
Q
(
ρ
,
θ
)
0
J
l
(
θ
)
]
(XI-1)
{\bf J}_l({\boldsymbol\rho}, {\boldsymbol \theta}) = \sum_{n=0}^{\infty}\frac{1}{(n+1)!}\begin{bmatrix} \boldsymbol \theta^{\wedge} & \boldsymbol \rho ^{\wedge}\\ 0 &\boldsymbol \theta^{\wedge} \end{bmatrix}^n = \begin{bmatrix} {\bf J}_l(\boldsymbol \theta) &\bf Q({\boldsymbol\rho}, {\boldsymbol \theta})\\ 0 &{\bf J}_l(\boldsymbol \theta)\end{bmatrix}\tag{XI-1}
Jl(ρ,θ)=n=0∑∞(n+1)!1[θ∧0ρ∧θ∧]n=[Jl(θ)0Q(ρ,θ)Jl(θ)](XI-1)
为了表达清晰,本记录中
x
∨
{\boldsymbol x}^{\vee}
x∨ 记为
x
^
\hat{\boldsymbol x}
x^. 展开计算 (XI-1)
[
θ
^
ρ
^
0
θ
^
]
0
=
I
6
×
6
=
[
I
3
×
3
0
0
I
3
×
3
]
[
θ
^
ρ
^
0
θ
^
]
1
=
[
θ
^
ρ
^
0
θ
^
]
[
θ
^
ρ
^
0
θ
^
]
2
=
[
θ
^
ρ
^
0
θ
^
]
[
θ
^
ρ
^
0
θ
^
]
=
[
θ
^
2
θ
^
ρ
^
+
ρ
^
θ
^
0
θ
^
2
]
[
θ
^
ρ
^
0
θ
^
]
3
=
[
θ
^
2
θ
^
ρ
^
+
ρ
^
θ
^
0
θ
^
2
]
[
θ
^
ρ
^
0
θ
^
]
=
[
θ
^
3
θ
^
2
ρ
^
+
θ
^
ρ
^
θ
^
+
ρ
^
θ
^
2
0
θ
^
3
]
[
θ
^
ρ
^
0
θ
^
]
3
=
[
θ
^
3
θ
^
2
ρ
^
+
θ
^
ρ
^
θ
^
+
ρ
^
θ
^
2
0
θ
^
3
]
[
θ
^
ρ
^
0
θ
^
]
=
[
θ
^
4
θ
^
3
ρ
^
+
θ
^
2
ρ
^
θ
^
+
θ
^
ρ
^
θ
^
2
+
ρ
^
θ
^
3
0
θ
^
4
]
⋮
(XI-2)
\begin{aligned} \begin{bmatrix} \hat{\boldsymbol \theta} & \hat{\boldsymbol \rho} \\ 0 &\hat{\boldsymbol \theta} \end{bmatrix} ^{0} &= {\bf I}_{6\times 6} =\begin{bmatrix} I_{3\times 3}&0\\0&I_{3\times3}\end{bmatrix}\\ \begin{bmatrix} \hat{\boldsymbol \theta} & \hat{\boldsymbol \rho} \\ 0 &\hat{\boldsymbol \theta} \end{bmatrix} ^{1} &= \begin{bmatrix} \hat{\boldsymbol \theta} & \hat{\boldsymbol \rho} \\ 0 &\hat{\boldsymbol \theta} \end{bmatrix}\\ \begin{bmatrix} \hat{\boldsymbol \theta} & \hat{\boldsymbol \rho} \\ 0 &\hat{\boldsymbol \theta} \end{bmatrix} ^{2} &= \begin{bmatrix} \hat{\boldsymbol \theta} & \hat{\boldsymbol \rho} \\ 0 &\hat{\boldsymbol \theta} \end{bmatrix} \begin{bmatrix} \hat{\boldsymbol \theta} & \hat{\boldsymbol \rho} \\ 0 &\hat{\boldsymbol \theta} \end{bmatrix} = \begin{bmatrix} {\hat{\boldsymbol \theta}}^2 & \hat{\boldsymbol \theta} \hat{\boldsymbol \rho} + \hat{\boldsymbol \rho} \hat{\boldsymbol \theta} \\ 0 &{\hat{\boldsymbol \theta}}^2 \end{bmatrix}\\ \begin{bmatrix} \hat{\boldsymbol \theta} & \hat{\boldsymbol \rho} \\ 0 &\hat{\boldsymbol \theta} \end{bmatrix} ^{3} &= \begin{bmatrix} {\hat{\boldsymbol \theta}}^2 & \hat{\boldsymbol \theta} \hat{\boldsymbol \rho} + \hat{\boldsymbol \rho} \hat{\boldsymbol \theta} \\ 0 &{\hat{\boldsymbol \theta}}^2 \end{bmatrix} \begin{bmatrix} \hat{\boldsymbol \theta} & \hat{\boldsymbol \rho} \\ 0 &\hat{\boldsymbol \theta} \end{bmatrix} = \begin{bmatrix} {\hat{\boldsymbol \theta}}^3 & \hat{\boldsymbol \theta}^{2} \hat{\boldsymbol \rho}+ \hat{\boldsymbol \theta} \hat{\boldsymbol \rho} \hat{\boldsymbol \theta} + \hat{\boldsymbol \rho} \hat{\boldsymbol \theta}^2 \\ 0 &{\hat{\boldsymbol \theta}}^3 \end{bmatrix}\\ \begin{bmatrix} \hat{\boldsymbol \theta} & \hat{\boldsymbol \rho} \\ 0 &\hat{\boldsymbol \theta} \end{bmatrix} ^{3} &= \begin{bmatrix} {\hat{\boldsymbol \theta}}^3 & \hat{\boldsymbol \theta}^{2} \hat{\boldsymbol \rho}+ \hat{\boldsymbol \theta} \hat{\boldsymbol \rho} \hat{\boldsymbol \theta} + \hat{\boldsymbol \rho} \hat{\boldsymbol \theta}^2 \\ 0 &{\hat{\boldsymbol \theta}}^3 \end{bmatrix} \begin{bmatrix} \hat{\boldsymbol \theta} & \hat{\boldsymbol \rho} \\ 0 &\hat{\boldsymbol \theta} \end{bmatrix} = \begin{bmatrix} {\hat{\boldsymbol \theta}}^4 & \hat{\boldsymbol \theta}^{3} \hat{\boldsymbol \rho}+ \hat{\boldsymbol \theta}^{2} \hat{\boldsymbol \rho} \hat{\boldsymbol \theta} + \hat{\boldsymbol \theta} \hat{\boldsymbol \rho} \hat{\boldsymbol \theta}^2 + \hat{\boldsymbol \rho} \hat{\boldsymbol \theta}^3 \\ 0 &{\hat{\boldsymbol \theta}}^4 \end{bmatrix} \\ &\vdots \end{aligned} \tag{XI-2}
[θ^0ρ^θ^]0[θ^0ρ^θ^]1[θ^0ρ^θ^]2[θ^0ρ^θ^]3[θ^0ρ^θ^]3=I6×6=[I3×300I3×3]=[θ^0ρ^θ^]=[θ^0ρ^θ^][θ^0ρ^θ^]=[θ^20θ^ρ^+ρ^θ^θ^2]=[θ^20θ^ρ^+ρ^θ^θ^2][θ^0ρ^θ^]=[θ^30θ^2ρ^+θ^ρ^θ^+ρ^θ^2θ^3]=[θ^30θ^2ρ^+θ^ρ^θ^+ρ^θ^2θ^3][θ^0ρ^θ^]=[θ^40θ^3ρ^+θ^2ρ^θ^+θ^ρ^θ^2+ρ^θ^3θ^4]⋮(XI-2)
显然
J l ( θ ) = ∑ n = 0 ∞ 1 ( n + 1 ) ! θ ^ n {\bf J}_l (\boldsymbol \theta) = \sum_{n=0}^{\infty} \frac{1}{(n+1)!} \hat {\boldsymbol \theta}^{n} Jl(θ)=n=0∑∞(n+1)!1θ^n
就是本 notes 中 VIII 部分的内容. 下面重点是 Q ( ρ , θ ) {\bf Q}( {\boldsymbol \rho}, {\boldsymbol \theta}) Q(ρ,θ) 的计算. 由 (XI-2) 归纳
Q ( ρ , θ ) = 1 2 ! ρ ^ + 1 3 ! ( θ ^ ρ ^ + ρ ^ θ ^ ) + 1 4 ! ( θ ^ 2 ρ ^ + θ ^ ρ ^ θ ^ + ρ ^ θ ^ 2 ) + 1 5 ! ( θ ^ 3 ρ ^ + θ ^ 2 ρ ^ θ ^ + θ ^ ρ ^ θ ^ 2 + ρ ^ θ ^ 3 ) + … = ∑ i + j = 0 i ⩾ 0 , j ⩾ 0 1 ( i + j + 2 ) ! θ ^ i ρ ^ θ ^ j + ∑ i + j = 1 i ⩾ 0 , j ⩾ 0 1 ( i + j + 2 ) ! θ ^ i ρ ^ θ ^ j + ∑ i + j = 2 i ⩾ 0 , j ⩾ 0 1 ( i + j + 2 ) ! θ ^ i ρ ^ θ ^ j + … = ∑ n = 0 ∞ ∑ m = 0 ∞ 1 ( m + n + 2 ) ! θ ^ n ρ ^ θ ^ m (XI-3) \begin{aligned} {\bf Q}({\boldsymbol \rho}, {\boldsymbol \theta}) &= \frac{1}{2!} \hat{\boldsymbol \rho} + \frac{1}{3!} ( \hat{\boldsymbol \theta} \hat{\boldsymbol \rho} + \hat{\boldsymbol \rho} \hat{\boldsymbol \theta}) + \frac{1}{4!}( \hat{\boldsymbol \theta}^{2} \hat{\boldsymbol \rho}+ \hat{\boldsymbol \theta} \hat{\boldsymbol \rho} \hat{\boldsymbol \theta} + \hat{\boldsymbol \rho} \hat{\boldsymbol \theta}^2 ) + \frac{1}{5!}(\hat{\boldsymbol \theta}^{3} \hat{\boldsymbol \rho}+ \hat{\boldsymbol \theta}^{2} \hat{\boldsymbol \rho} \hat{\boldsymbol \theta} + \hat{\boldsymbol \theta} \hat{\boldsymbol \rho} \hat{\boldsymbol \theta}^2 + \hat{\boldsymbol \rho} \hat{\boldsymbol \theta}^3)+ \ldots \\ &= \sum_{\begin{aligned}i+j =0\\ i \geqslant 0,\, j\geqslant 0\end{aligned}}^{} \frac{1}{(i+j+2)!} \hat{\boldsymbol \theta} ^{i} \hat{\boldsymbol \rho} \hat{\boldsymbol \theta}^{j} + \sum_{\begin{aligned}i+j =1\\ i \geqslant 0,\, j\geqslant 0\end{aligned}}^{} \frac{1}{(i+j+2)!} \hat{\boldsymbol \theta} ^{i} \hat{\boldsymbol \rho} \hat{\boldsymbol \theta}^{j}+ \sum_{\begin{aligned}i+j =2\\ i \geqslant 0,\, j\geqslant 0\end{aligned}}^{} \frac{1}{(i+j+2)!} \hat{\boldsymbol \theta} ^{i} \hat{\boldsymbol \rho} \hat{\boldsymbol \theta}^{j} + \ldots\\ &= \sum_{n =0}^{\infty} \sum_{m = 0}^{\infty} \frac{1}{(m+n+2)!} \hat{\boldsymbol \theta} ^{n} \hat{\boldsymbol \rho} \hat{\boldsymbol \theta}^{m} \\ \end{aligned} \tag{XI-3} Q(ρ,θ)=2!1ρ^+3!1(θ^ρ^+ρ^θ^)+4!1(θ^2ρ^+θ^ρ^θ^+ρ^θ^2)+5!1(θ^3ρ^+θ^2ρ^θ^+θ^ρ^θ^2+ρ^θ^3)+…=i+j=0i⩾0,j⩾0∑(i+j+2)!1θ^iρ^θ^j+i+j=1i⩾0,j⩾0∑(i+j+2)!1θ^iρ^θ^j+i+j=2i⩾0,j⩾0∑(i+j+2)!1θ^iρ^θ^j+…=n=0∑∞m=0∑∞(m+n+2)!1θ^nρ^θ^m(XI-3)
再继续归纳前,先总结以下简单规律 (都可以计算获得,复杂的可以借助符号计算软件,诸如 maxima).
θ ^ = θ [ u ] × (XI-4) \hat{\boldsymbol \theta} = \theta [\bf u]_\times \tag{XI-4} θ^=θ[u]×(XI-4)
[ u ] × 0 = I 3 × 3 [ u ] × 3 = − [ u ] × [ u ] × 4 = − [ u ] × 2 [ u ] × 5 = [ u ] × [ u ] × 6 = [ u ] × 2 [ u ] × 7 = − [ u ] × [ u ] × 8 = − [ u ] × 2 [ u ] × 9 = [ u ] × (XI-5) \begin{aligned} \left[{ \bf u}\right]_\times^{0}&={ \bf I}_{3\times 3}\qquad &[\bf u]_\times^{3}&=-[\bf u]_\times\\ [\bf u]_\times^{4}&=-[\bf u]_\times^{2}\qquad &[\bf u]_\times^{5}&=[\bf u]_\times\\ [\bf u]_\times^{6}&=[\bf u]_\times^{2}\qquad &[\bf u]_\times^{7}&=-[\bf u]_\times\\ [\bf u]_\times^{8}&=-[\bf u]_\times^{2}\qquad &[\bf u]_\times^{9}&=[\bf u]_\times\\ \end{aligned} \tag{XI-5} [u]×0[u]×4[u]×6[u]×8=I3×3=−[u]×2=[u]×2=−[u]×2[u]×3[u]×5[u]×7[u]×9=−[u]×=[u]×=−[u]×=[u]×(XI-5)
[ u ] × 2 ρ ^ [ u ] × = [ u ] × ρ ^ [ u ] × 2 [ u ] × 2 ρ ^ [ u ] × 2 = − [ u ] × ρ ^ [ u ] × (XI-6) \begin{aligned} \left[{\bf u}\right]_\times^{2} \, \hat{\boldsymbol \rho}\, [\bf u]_\times & = [{\bf u}]_\times\, \hat{\boldsymbol \rho}\, [{\bf u}]_\times^{2}\\ [{\bf u}]_\times^{2} \, \hat{\boldsymbol\rho}\, [{\bf u}]_\times^{2} & = -[{\bf u}]_\times\, \hat{\boldsymbol \rho}\, [{\bf u}]_\times \end{aligned} \tag{XI-6} [u]×2ρ^[u]×[u]×2ρ^[u]×2=[u]×ρ^[u]×2=−[u]×ρ^[u]×(XI-6)
对 (XI-3) 继续展开,为了看清规律,我们展开足够多项
Q ( ρ , θ ) = ∑ n = 0 ∞ ∑ m = 0 ∞ 1 ( m + n + 2 ) ! θ ^ n ρ ^ θ ^ m = 1 2 ! ρ ^ + 1 3 ! ( θ ^ ρ ^ + ρ ^ θ ^ ) + 1 4 ! ( θ ^ 2 ρ ^ + θ ^ ρ ^ θ ^ + ρ ^ θ ^ 2 ) + 1 5 ! ( θ ^ 3 ρ ^ + θ ^ 2 ρ ^ θ ^ + θ ^ ρ ^ θ ^ 2 + ρ ^ θ ^ 3 ) + 1 6 ! ( θ ^ 4 ρ ^ + θ ^ 3 ρ ^ θ ^ + θ ^ 2 ρ ^ θ ^ 2 + θ ^ ρ ^ θ ^ 3 + ρ ^ θ ^ 4 ) + 1 7 ! ( θ ^ 5 ρ ^ + θ ^ 4 ρ ^ θ ^ + θ ^ 3 ρ ^ θ ^ 2 + θ ^ 2 ρ ^ θ ^ 3 + θ ^ ρ ^ θ ^ 4 + ρ ^ θ ^ 5 ) + 1 8 ! ( θ ^ 6 ρ ^ + θ ^ 5 ρ ^ θ ^ + θ ^ 4 ρ ^ θ ^ 2 + θ ^ 3 ρ ^ θ ^ 3 + θ ^ 2 ρ ^ θ ^ 4 + θ ^ ρ ^ θ ^ 5 + ρ ^ θ ^ 6 ) + 1 9 ! ( θ ^ 7 ρ ^ + θ ^ 6 ρ ^ θ ^ + θ ^ 5 ρ ^ θ ^ 2 + θ ^ 4 ρ ^ θ ^ 3 + θ ^ 3 ρ ^ θ ^ 4 + θ ^ 2 ρ ^ θ ^ 5 + θ ^ ρ ^ θ ^ 6 + ρ ^ θ ^ 7 ) ⋮ (XI-7) \begin{aligned} {\bf Q}({\boldsymbol\rho}, {\boldsymbol \theta}) =& \sum_{n =0}^{\infty} \sum_{m = 0}^{\infty} \frac{1}{(m+n+2)!} \hat{\boldsymbol \theta} ^{n} \hat{\boldsymbol \rho} \hat{\boldsymbol \theta}^{m} \\ \\ = &\quad\ \frac{1}{2!}\hat{\boldsymbol\rho} \\ &+ \frac{1}{3!}(\hat{\boldsymbol \theta} \hat{\boldsymbol\rho} + \hat{\boldsymbol\rho} \hat{\boldsymbol \theta} ) \\ &+ \frac{1}{4!}(\hat{\boldsymbol \theta}^2 \hat{\boldsymbol\rho} + \hat{\boldsymbol \theta} \hat{\boldsymbol\rho} \hat{\boldsymbol \theta} + \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^{2} )\\ &+ \frac{1}{5!}(\hat{\boldsymbol \theta}^3 \hat{\boldsymbol\rho} + \hat{\boldsymbol \theta}^2 \hat{\boldsymbol\rho} \hat{\boldsymbol \theta} + \hat{\boldsymbol \theta} \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^2 + \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^{3} )\\ &+ \frac{1}{6!}(\hat{\boldsymbol \theta}^4 \hat{\boldsymbol\rho} + \hat{\boldsymbol \theta}^3 \hat{\boldsymbol\rho}\hat{\boldsymbol \theta} + \hat{\boldsymbol \theta}^2 \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^2 + \hat{\boldsymbol \theta} \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^3 + \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^{4} )\\ &+ \frac{1}{7!}(\hat{\boldsymbol \theta}^5 \hat{\boldsymbol\rho} + \hat{\boldsymbol \theta}^4 \hat{\boldsymbol\rho}\hat{\boldsymbol \theta} + \hat{\boldsymbol \theta}^3 \hat{\boldsymbol\rho}\hat{\boldsymbol \theta}^2 + \hat{\boldsymbol \theta}^2 \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^3 + \hat{\boldsymbol \theta} \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^4 + \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^{5} )\\ &+ \frac{1}{8!}(\hat{\boldsymbol \theta}^6 \hat{\boldsymbol\rho} + \hat{\boldsymbol \theta}^5 \hat{\boldsymbol\rho} \hat{\boldsymbol \theta} + \hat{\boldsymbol \theta}^4 \hat{\boldsymbol\rho}\hat{\boldsymbol \theta} ^2 + \hat{\boldsymbol \theta}^3 \hat{\boldsymbol\rho}\hat{\boldsymbol \theta}^3 + \hat{\boldsymbol \theta}^2 \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^4 + \hat{\boldsymbol \theta} \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^5 + \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^{6} )\\ &+ \frac{1}{9!}(\hat{\boldsymbol \theta}^7 \hat{\boldsymbol\rho} + \hat{\boldsymbol \theta}^6 \hat{\boldsymbol\rho} \hat{\boldsymbol \theta} + \hat{\boldsymbol \theta}^5 \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^2 + \hat{\boldsymbol \theta}^4 \hat{\boldsymbol\rho}\hat{\boldsymbol \theta} ^3 + \hat{\boldsymbol \theta}^3 \hat{\boldsymbol\rho}\hat{\boldsymbol \theta}^4 + \hat{\boldsymbol \theta}^2 \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^5 + \hat{\boldsymbol \theta} \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^6 + \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^{7} )\\ &\ \, \vdots\\ \end{aligned} \tag{XI-7} Q(ρ,θ)==n=0∑∞m=0∑∞(m+n+2)!1θ^nρ^θ^m 2!1ρ^+3!1(θ^ρ^+ρ^θ^)+4!1(θ^2ρ^+θ^ρ^θ^+ρ^θ^2)+5!1(θ^3ρ^+θ^2ρ^θ^+θ^ρ^θ^2+ρ^θ^3)+6!1(θ^4ρ^+θ^3ρ^θ^+θ^2ρ^θ^2+θ^ρ^θ^3+ρ^θ^4)+7!1(θ^5ρ^+θ^4ρ^θ^+θ^3ρ^θ^2+θ^2ρ^θ^3+θ^ρ^θ^4+ρ^θ^5)+8!1(θ^6ρ^+θ^5ρ^θ^+θ^4ρ^θ^2+θ^3ρ^θ^3+θ^2ρ^θ^4+θ^ρ^θ^5+ρ^θ^6)+9!1(θ^7ρ^+θ^6ρ^θ^+θ^5ρ^θ^2+θ^4ρ^θ^3+θ^3ρ^θ^4+θ^2ρ^θ^5+θ^ρ^θ^6+ρ^θ^7) ⋮(XI-7)
利用 (XI-4) 代换 (XI-7) 中各项
Q ( ρ , θ ) = 1 2 ρ ^ + 1 3 ! θ ( [ u ] × ρ ^ + ρ ^ [ u ] × ) + 1 4 ! θ 2 ( [ u ] × 2 ρ ^ + [ u ] × ρ ^ [ u ] × + ρ ^ [ u ] × 2 ) + 1 5 ! θ 3 ( [ u ] × 3 ρ ^ + [ u ] × 2 ρ ^ [ u ] × + [ u ] × ρ ^ [ u ] × 2 + ρ ^ [ u ] × 3 ) + 1 6 ! θ 4 ( [ u ] × 4 ρ ^ + [ u ] × 3 ρ ^ [ u ] × + [ u ] × 2 ρ ^ [ u ] × 2 + [ u ] × ρ ^ [ u ] × 3 + ρ ^ [ u ] × 4 ) + 1 7 ! θ 5 ( [ u ] × 5 ρ ^ + [ u ] × 4 ρ ^ [ u ] × + [ u ] × 3 ρ ^ [ u ] × 2 + [ u ] × 2 ρ ^ [ u ] × 3 + [ u ] × ρ ^ [ u ] × 4 + ρ ^ [ u ] × 5 ) + 1 8 ! θ 6 ( [ u ] × 6 ρ ^ + [ u ] × 5 ρ ^ [ u ] × + [ u ] × 4 ρ ^ [ u ] × 2 + [ u ] × 3 ρ ^ [ u ] × 3 + [ u ] × 2 ρ ^ [ u ] × 4 + [ u ] × ρ ^ [ u ] × 5 + ρ ^ [ u ] × 6 ) + 1 9 ! θ 7 ( [ u ] × 7 ρ ^ + [ u ] × 6 ρ ^ [ u ] × + [ u ] × 5 ρ ^ [ u ] × 2 + [ u ] × 4 ρ ^ [ u ] × 3 + [ u ] × 3 ρ ^ [ u ] × 4 + [ u ] × 2 ρ ^ [ u ] × 5 + [ u ] × ρ ^ [ u ] × 6 + ρ ^ [ u ] × 7 ) ⋮ \begin{aligned} {\bf Q}({\boldsymbol\rho}, {\boldsymbol \theta}) = &\quad \frac{1}{2}\hat{\boldsymbol\rho} \\ & + \frac{1}{3!}\theta ([{\bf u}]_\times \hat{\boldsymbol\rho} + \hat{\boldsymbol\rho} [{\bf u}]_\times ) \\ & + \frac{1}{4!}\theta ^{2} ([{\bf u}]_\times ^2 \hat{\boldsymbol\rho} + [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times + \hat{\boldsymbol\rho} [{\bf u}]_\times ^{2} )\\ &+ \frac{1}{5!}\theta^{3} ([{\bf u}]_\times ^3 \hat{\boldsymbol\rho} + [{\bf u}]_\times ^2 \hat{\boldsymbol\rho} [{\bf u}]_\times + [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times ^2 + \hat{\boldsymbol\rho} [{\bf u}]_\times ^{3} )\\ &+ \frac{1}{6!}\theta^{4} ([{\bf u}]_\times ^4 \hat{\boldsymbol\rho} +[{\bf u}]_\times ^3 \hat{\boldsymbol\rho}[{\bf u}]_\times + [{\bf u}]_\times ^2 \hat{\boldsymbol\rho}[{\bf u}]_\times ^2 + [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times ^3 + \hat{\boldsymbol\rho}[{\bf u}]_\times ^{4} )\\ &+ \frac{1}{7!}\theta^{5} ([{\bf u}]_\times ^5 \hat{\boldsymbol\rho} + [{\bf u}]_\times ^4 \hat{\boldsymbol\rho}[{\bf u}]_\times + [{\bf u}]_\times ^3 \hat{\boldsymbol\rho}[{\bf u}]_\times ^2 + [{\bf u}]_\times ^2 \hat{\boldsymbol\rho}[{\bf u}]_\times ^3 + [{\bf u}]_\times \hat{\boldsymbol\rho}[{\bf u}]_\times ^4 + \hat{\boldsymbol\rho}[{\bf u}]_\times ^{5} )\\ &+ \frac{1}{8!}\theta^{6}([{\bf u}]_\times ^6 \hat{\boldsymbol\rho} + [{\bf u}]_\times ^5 \hat{\boldsymbol\rho} [{\bf u}]_\times + [{\bf u}]_\times ^4 \hat{\boldsymbol\rho}[{\bf u}]_\times ^2 + [{\bf u}]_\times ^3 \hat{\boldsymbol\rho}[{\bf u}]_\times ^3 + [{\bf u}]_\times ^2 \hat{\boldsymbol\rho} [{\bf u}]_\times ^4 + [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times ^5 + \hat{\boldsymbol\rho} [{\bf u}]_\times ^{6} )\\ &+ \frac{1}{9!}\theta^{7}([{\bf u}]_\times^7 \hat{\boldsymbol\rho} + [{\bf u}]_\times^6 \hat{\boldsymbol\rho} [{\bf u}]_\times + [{\bf u}]_\times^5 \hat{\boldsymbol\rho} [{\bf u}]_\times^2 + [{\bf u}]_\times^4 \hat{\boldsymbol\rho}[{\bf u}]_\times^3 + [{\bf u}]_\times^3 \hat{\boldsymbol\rho}[{\bf u}]_\times^4 + [{\bf u}]_\times^2 \hat{\boldsymbol\rho} [{\bf u}]_\times^5 + [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times^6 + \hat{\boldsymbol\rho} [{\bf u}]_\times^{7} )\\ &\ \, \vdots \end{aligned} Q(ρ,θ)=21ρ^+3!1θ([u]×ρ^+ρ^[u]×)+4!1θ2([u]×2ρ^+[u]×ρ^[u]×+ρ^[u]×2)+5!1θ3([u]×3ρ^+[u]×2ρ^[u]×+[u]×ρ^[u]×2+ρ^[u]×3)+6!1θ4([u]×4ρ^+[u]×3ρ^[u]×+[u]×2ρ^[u]×2+[u]×ρ^[u]×3+ρ^[u]×4)+7!1θ5([u]×5ρ^+[u]×4ρ^[u]×+[u]×3ρ^[u]×2+[u]×2ρ^[u]×3+[u]×ρ^[u]×4+ρ^[u]×5)+8!1θ6([u]×6ρ^+[u]×5ρ^[u]×+[u]×4ρ^[u]×2+[u]×3ρ^[u]×3+[u]×2ρ^[u]×4+[u]×ρ^[u]×5+ρ^[u]×6)+9!1θ7([u]×7ρ^+[u]×6ρ^[u]×+[u]×5ρ^[u]×2+[u]×4ρ^[u]×3+[u]×3ρ^[u]×4+[u]×2ρ^[u]×5+[u]×ρ^[u]×6+ρ^[u]×7) ⋮
利用 (XI-5) 和 (XI-6) 继续代换
Q ( ρ , θ ) = 1 2 ρ ^ + 1 3 ! θ ( [ u ] × ρ ^ + ρ ^ [ u ] × ) + 1 4 ! θ 2 ( [ u ] × 2 ρ ^ + [ u ] × ρ ^ [ u ] × + ρ ^ [ u ] × 2 ) + 1 5 ! θ 3 ( − [ u ] × ρ ^ + [ u ] × 2 ρ ^ [ u ] × + [ u ] × ρ ^ [ u ] × 2 − ρ ^ [ u ] × ) + 1 6 ! θ 4 ( − [ u ] × 2 ρ ^ − [ u ] × ρ ^ [ u ] × − [ u ] × ρ ^ [ u ] × − [ u ] × ρ ^ [ u ] × − ρ ^ [ u ] × 2 ) + 1 7 ! θ 5 ( [ u ] × ρ ^ − [ u ] × 2 ρ ^ [ u ] × − [ u ] × ρ ^ [ u ] × 2 − [ u ] × 2 ρ ^ [ u ] × − [ u ] × ρ ^ [ u ] × 2 + ρ ^ [ u ] × ) + 1 8 ! θ 6 ( [ u ] × 2 ρ ^ + [ u ] × ρ ^ [ u ] × + [ u ] × ρ ^ [ u ] × + [ u ] × ρ ^ [ u ] × + [ u ] × ρ ^ [ u ] × + [ u ] × ρ ^ [ u ] × + ρ ^ [ u ] × 2 ) + 1 9 ! θ 7 ( − [ u ] × ρ ^ + [ u ] × 2 ρ ^ [ u ] × + [ u ] × ρ ^ [ u ] × 2 + [ u ] × 2 ρ ^ [ u ] × + [ u ] × ρ ^ [ u ] × 2 + [ u ] × 2 ρ ^ [ u ] × + [ u ] × ρ ^ [ u ] × 2 − ρ ^ [ u ] × ) ⋮ \begin{aligned} {\bf Q}({\boldsymbol\rho}, {\boldsymbol \theta}) = & \quad\ \frac{1}{2}\hat{\boldsymbol\rho} \\ &+ \frac{1}{3!}\theta ([{\bf u}]_\times \hat{\boldsymbol\rho} + \hat{\boldsymbol\rho} [{\bf u}]_\times ) \\ &+ \frac{1}{4!}\theta ^{2} ([{\bf u}]_\times ^2 \hat{\boldsymbol\rho} + [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times + \hat{\boldsymbol\rho} [{\bf u}]_\times ^{2} )\\ &+ \frac{1}{5!}\theta^{3} (-[{\bf u}]_\times \hat{\boldsymbol\rho} + [{\bf u}]_\times ^2 \hat{\boldsymbol\rho} [{\bf u}]_\times + [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times ^2 - \hat{\boldsymbol\rho} [{\bf u}]_\times )\\ &+ \frac{1}{6!}\theta^{4} (-[{\bf u}]_\times ^2 \hat{\boldsymbol\rho} - [{\bf u}]_\times \hat{\boldsymbol\rho}[{\bf u}]_\times - [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times - [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times - \hat{\boldsymbol\rho}[{\bf u}]_\times ^{2} )\\ &+ \frac{1}{7!}\theta^{5} ([{\bf u}]_\times \hat{\boldsymbol\rho} - [{\bf u}]_\times ^2 \hat{\boldsymbol\rho}[{\bf u}]_\times - [{\bf u}]_\times \hat{\boldsymbol\rho}[{\bf u}]_\times ^2 - [{\bf u}]_\times ^2 \hat{\boldsymbol\rho}[{\bf u}]_\times - [{\bf u}]_\times \hat{\boldsymbol\rho}[{\bf u}]_\times ^2 + \hat{\boldsymbol\rho}[{\bf u}]_\times )\\ &+ \frac{1}{8!}\theta^{6}([{\bf u}]_\times ^2 \hat{\boldsymbol\rho} + [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times + [{\bf u}]_\times \hat{\boldsymbol\rho}[{\bf u}]_\times + [{\bf u}]_\times \hat{\boldsymbol\rho}[{\bf u}]_\times + [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times + [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times + \hat{\boldsymbol\rho} [{\bf u}]_\times ^{2} )\\ &+ \frac{1}{9!}\theta^{7}( - [{\bf u}]_\times \hat{\boldsymbol\rho} + [{\bf u}]_\times^2 \hat{\boldsymbol\rho} [{\bf u}]_\times + [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times^2 + [{\bf u}]_\times^2 \hat{\boldsymbol\rho}[{\bf u}]_\times + [{\bf u}]_\times \hat{\boldsymbol\rho}[{\bf u}]_\times^2 + [{\bf u}]_\times^2 \hat{\boldsymbol\rho} [{\bf u}]_\times + [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times^2 - \hat{\boldsymbol\rho} [{\bf u}]_\times )\\ &\vdots\\ \end{aligned} Q(ρ,θ)= 21ρ^+3!1θ([u]×ρ^+ρ^[u]×)+4!1θ2([u]×2ρ^+[u]×ρ^[u]×+ρ^[u]×2)+5!1θ3(−[u]×ρ^+[u]×2ρ^[u]×+[u]×ρ^[u]×2−ρ^[u]×)+6!1θ4(−[u]×2ρ^−[u]×ρ^[u]×−[u]×ρ^[u]×−[u]×ρ^[u]×−ρ^[u]×2)+7!1θ5([u]×ρ^−[u]×2ρ^[u]×−[u]×ρ^[u]×2−[u]×2ρ^[u]×−[u]×ρ^[u]×2+ρ^[u]×)+8!1θ6([u]×2ρ^+[u]×ρ^[u]×+[u]×ρ^[u]×+[u]×ρ^[u]×+[u]×ρ^[u]×+[u]×ρ^[u]×+ρ^[u]×2)+9!1θ7(−[u]×ρ^+[u]×2ρ^[u]×+[u]×ρ^[u]×2+[u]×2ρ^[u]×+[u]×ρ^[u]×2+[u]×2ρ^[u]×+[u]×ρ^[u]×2−ρ^[u]×)⋮
简单合并同类项
Q
(
ρ
,
θ
)
=
1
2
ρ
^
+
1
3
!
θ
(
[
u
]
×
ρ
^
+
ρ
^
[
u
]
×
)
+
1
4
!
θ
2
(
[
u
]
×
2
ρ
^
+
[
u
]
×
ρ
^
[
u
]
×
+
ρ
^
[
u
]
×
2
)
+
1
5
!
θ
3
(
−
[
u
]
×
ρ
^
+
[
u
]
×
2
ρ
^
[
u
]
×
+
[
u
]
×
ρ
^
[
u
]
×
2
−
ρ
^
[
u
]
×
)
+
1
6
!
θ
4
(
−
[
u
]
×
2
ρ
^
−
3
[
u
]
×
ρ
^
[
u
]
×
−
ρ
^
[
u
]
×
2
)
+
1
7
!
θ
5
(
[
u
]
×
ρ
^
−
2
[
u
]
×
2
ρ
^
[
u
]
×
−
2
[
u
]
×
ρ
^
[
u
]
×
2
+
ρ
^
[
u
]
×
)
+
1
8
!
θ
6
(
[
u
]
×
2
ρ
^
+
5
[
u
]
×
ρ
^
[
u
]
×
+
ρ
^
[
u
]
×
2
)
+
1
9
!
θ
7
(
−
[
u
]
×
ρ
^
+
3
[
u
]
×
2
ρ
^
[
u
]
×
+
3
[
u
]
×
ρ
^
[
u
]
×
2
−
ρ
^
[
u
]
×
)
⋮
(XI-8)
\begin{aligned} {\bf Q}({\boldsymbol\rho}, {\boldsymbol \theta}) = &\quad\ \frac{1}{2}\hat{\boldsymbol\rho} \\ &+ \frac{1}{3!}\theta ([{\bf u}]_\times \hat{\boldsymbol\rho} + \hat{\boldsymbol\rho} [{\bf u}]_\times ) \\ &+ \frac{1}{4!}\theta ^{2} ([{\bf u}]_\times ^2 \hat{\boldsymbol\rho} + [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times + \hat{\boldsymbol\rho} [{\bf u}]_\times ^{2} )\\ &+ \frac{1}{5!}\theta^{3} (-[{\bf u}]_\times \hat{\boldsymbol\rho} + [{\bf u}]_\times ^2 \hat{\boldsymbol\rho} [{\bf u}]_\times + [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times ^2 - \hat{\boldsymbol\rho} [{\bf u}]_\times )\\ &+ \frac{1}{6!}\theta^{4} (-[{\bf u}]_\times ^2 \hat{\boldsymbol\rho} - 3[{\bf u}]_\times \hat{\boldsymbol\rho}[{\bf u}]_\times - \hat{\boldsymbol\rho}[{\bf u}]_\times ^{2} )\\ &+ \frac{1}{7!}\theta^{5} ([{\bf u}]_\times \hat{\boldsymbol\rho} - 2[{\bf u}]_\times ^2 \hat{\boldsymbol\rho}[{\bf u}]_\times - 2 [{\bf u}]_\times \hat{\boldsymbol\rho}[{\bf u}]_\times ^2 + \hat{\boldsymbol\rho}[{\bf u}]_\times )\\ &+ \frac{1}{8!}\theta^{6}([{\bf u}]_\times ^2 \hat{\boldsymbol\rho} + 5[{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times + \hat{\boldsymbol\rho} [{\bf u}]_\times ^{2} )\\ &+ \frac{1}{9!}\theta^{7}( - [{\bf u}]_\times \hat{\boldsymbol\rho} + 3 [{\bf u}]_\times^2 \hat{\boldsymbol\rho} [{\bf u}]_\times + 3 [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times^2 - \hat{\boldsymbol\rho} [{\bf u}]_\times )\\ &\vdots\\ \tag{XI-8} \end{aligned}
Q(ρ,θ)= 21ρ^+3!1θ([u]×ρ^+ρ^[u]×)+4!1θ2([u]×2ρ^+[u]×ρ^[u]×+ρ^[u]×2)+5!1θ3(−[u]×ρ^+[u]×2ρ^[u]×+[u]×ρ^[u]×2−ρ^[u]×)+6!1θ4(−[u]×2ρ^−3[u]×ρ^[u]×−ρ^[u]×2)+7!1θ5([u]×ρ^−2[u]×2ρ^[u]×−2[u]×ρ^[u]×2+ρ^[u]×)+8!1θ6([u]×2ρ^+5[u]×ρ^[u]×+ρ^[u]×2)+9!1θ7(−[u]×ρ^+3[u]×2ρ^[u]×+3[u]×ρ^[u]×2−ρ^[u]×)⋮(XI-8)
经过这一通计算整理,可以发现 (XI-8) 只剩下为数不多的不同项,所以可以对上式合并同类项
Q
(
ρ
,
θ
)
=
1
2
ρ
^
+
(
1
3
!
θ
−
1
5
!
θ
3
+
1
7
!
θ
5
−
1
9
!
θ
7
+
…
)
(
[
u
]
×
ρ
^
+
ρ
^
[
u
]
×
)
+
(
1
4
!
θ
2
−
1
6
!
θ
4
+
1
8
!
θ
6
−
…
)
(
[
u
]
×
2
ρ
^
+
ρ
^
[
u
]
×
2
)
+
(
1
4
!
θ
2
−
3
6
!
θ
4
+
5
8
!
θ
6
−
…
)
(
[
u
]
×
ρ
^
[
u
]
×
)
+
(
1
5
!
θ
3
−
2
7
!
θ
5
+
3
9
!
θ
7
−
…
)
(
[
u
]
×
2
ρ
^
[
u
]
×
+
[
u
]
×
ρ
^
[
u
]
×
2
)
=
(
X
I
−
4
)
1
2
ρ
^
+
(
1
3
!
−
1
5
!
θ
2
+
1
7
!
θ
4
−
1
9
!
θ
6
+
…
)
(
θ
^
ρ
^
+
ρ
^
θ
^
)
+
(
1
4
!
−
1
6
!
θ
2
+
1
8
!
θ
4
−
…
)
(
θ
^
2
ρ
^
+
ρ
^
θ
^
2
)
+
(
1
4
!
−
3
6
!
θ
2
+
5
8
!
θ
4
−
…
)
(
θ
^
ρ
^
θ
^
)
+
(
1
5
!
−
2
7
!
θ
2
+
3
9
!
θ
4
−
…
)
(
θ
^
2
ρ
^
θ
^
+
θ
^
ρ
^
θ
^
2
)
(XI-9)
\begin{aligned} {\bf Q}({\boldsymbol\rho}, {\boldsymbol \theta}) = & \quad\ \frac{1}{2}\hat{\boldsymbol\rho} \\ &+(\frac{1}{3!}\theta - \frac{1}{5!}\theta^{3} + \frac{1}{7!}\theta^{5} - \frac{1}{9!}\theta^{7} +\, \ldots )([{\bf u}]_\times \hat{\boldsymbol\rho} + \hat{\boldsymbol\rho} [{\bf u}]_\times )\\ &+(\frac{1}{4!}\theta ^{2} -\frac{1}{6!}\theta^{4} + \frac{1}{8!}\theta^{6} -\, \ldots) ([{\bf u}]_\times ^2 \hat{\boldsymbol\rho} + \hat{\boldsymbol\rho} [{\bf u}]_\times ^{2})\\ &+(\frac{1}{4!}\theta ^{2} -\frac{3}{6!}\theta^{4} + \frac{5}{8!}\theta^{6} -\, \ldots)([{\bf u}]_\times \hat{\boldsymbol\rho}[{\bf u}]_\times) \\ &+ (\frac{1}{5!}\theta^{3} - \frac{2}{7!}\theta^{5} + \frac{3}{9!}\theta^{7} - \, \ldots ) ([{\bf u}]_\times ^2 \hat{\boldsymbol\rho} [{\bf u}]_\times + [{\bf u}]_\times \hat{\boldsymbol\rho} [{\bf u}]_\times ^2)\\ \overset{(\small \rm XI-4)}= & \quad\ \frac{1}{2}\hat{\boldsymbol\rho} \\ &+ (\frac{1}{3!} - \frac{1}{5!}\theta^{2} + \frac{1}{7!}\theta^{4} - \frac{1}{9!}\theta^{6} +\, \ldots )(\hat{\boldsymbol \theta} \hat{\boldsymbol\rho} + \hat{\boldsymbol\rho}\hat{\boldsymbol \theta} )\\ &+(\frac{1}{4!}-\frac{1}{6!}\theta^{2} + \frac{1}{8!}\theta^{4} -\, \ldots) (\hat{\boldsymbol \theta} ^2 \hat{\boldsymbol\rho} + \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^2)\\ &+ (\frac{1}{4!} -\frac{3}{6!}\theta^{2} + \frac{5}{8!}\theta^{4} -\, \ldots)(\hat{\boldsymbol \theta} \hat{\boldsymbol\rho}\hat{\boldsymbol \theta}) \\ & + (\frac{1}{5!} - \frac{2}{7!}\theta^{2} + \frac{3}{9!}\theta^{4} - \, \ldots ) (\hat{\boldsymbol \theta} ^2 \hat{\boldsymbol\rho} \hat{\boldsymbol \theta} + \hat{\boldsymbol \theta} \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^2)\\ \end{aligned} \tag{XI-9}
Q(ρ,θ)==(XI−4) 21ρ^+(3!1θ−5!1θ3+7!1θ5−9!1θ7+…)([u]×ρ^+ρ^[u]×)+(4!1θ2−6!1θ4+8!1θ6−…)([u]×2ρ^+ρ^[u]×2)+(4!1θ2−6!3θ4+8!5θ6−…)([u]×ρ^[u]×)+(5!1θ3−7!2θ5+9!3θ7−…)([u]×2ρ^[u]×+[u]×ρ^[u]×2) 21ρ^+(3!1−5!1θ2+7!1θ4−9!1θ6+…)(θ^ρ^+ρ^θ^)+(4!1−6!1θ2+8!1θ4−…)(θ^2ρ^+ρ^θ^2)+(4!1−6!3θ2+8!5θ4−…)(θ^ρ^θ^)+(5!1−7!2θ2+9!3θ4−…)(θ^2ρ^θ^+θ^ρ^θ^2)(XI-9)
另外已知 Talyor Expansion
s
i
n
θ
=
θ
−
1
3
!
θ
3
+
1
5
!
θ
5
−
1
7
!
θ
7
+
1
9
!
θ
9
−
…
{\rm sin} \theta = \theta - \frac{1}{3!}\theta^{3} + \frac{1}{5!}\theta^{5} -\frac{1}{7!}\theta^{7} + \frac{1}{9!}\theta^{9}-\, \ldots
sinθ=θ−3!1θ3+5!1θ5−7!1θ7+9!1θ9−…
c o s θ = 1 − 1 2 ! θ 2 + 1 4 ! θ 4 − 1 6 ! θ 6 + 1 8 ! θ 8 − … {\rm cos} \theta = 1 -\frac{1}{2!}\theta^{2} + \frac{1}{4!}\theta^{4} - \frac{1}{6!}\theta^{6} + \frac{1}{8!}\theta^{8} -\, \ldots cosθ=1−2!1θ2+4!1θ4−6!1θ6+8!1θ8−…
反过来对待验证的 eq(180) 进行展开
θ
−
s
i
n
θ
θ
3
=
θ
−
(
θ
−
1
3
!
θ
3
+
1
5
!
θ
5
−
1
7
!
θ
7
+
1
9
!
θ
9
−
…
)
θ
3
=
1
3
!
θ
3
−
1
5
!
θ
5
+
1
7
!
θ
7
−
1
9
!
θ
9
+
…
θ
3
=
1
3
!
−
1
5
!
θ
2
+
1
7
!
θ
4
−
1
9
!
θ
6
+
…
(XI-10)
\begin{aligned} \frac{\theta-{\rm sin}\theta}{\theta^3} &= \frac{\theta -(\theta - \frac{1}{3!}\theta^{3} + \frac{1}{5!}\theta^{5} -\frac{1}{7!}\theta^{7} + \frac{1}{9!}\theta^{9}- \, \ldots)} {\theta^3} \\ &= \frac{\frac{1}{3!}\theta^{3} - \frac{1}{5!}\theta^{5} + \frac{1}{7!}\theta^{7} - \frac{1}{9!}\theta^{9}+ \, \ldots}{\theta^3} \\ & = \frac{1}{3!} - \frac{1}{5!}\theta^{2} + \frac{1}{7!}\theta^{4} - \frac{1}{9!}\theta^{6}+ \, \ldots \end{aligned} \tag{XI-10}
θ3θ−sinθ=θ3θ−(θ−3!1θ3+5!1θ5−7!1θ7+9!1θ9−…)=θ33!1θ3−5!1θ5+7!1θ7−9!1θ9+…=3!1−5!1θ2+7!1θ4−9!1θ6+…(XI-10)
− 1 − θ 2 2 − c o s θ θ 4 = − 1 − 1 2 θ 2 − ( 1 − 1 2 ! θ 2 + 1 4 ! θ 4 − 1 6 ! θ 6 + 1 8 ! θ 8 − … ) θ 4 = 1 4 ! θ 4 − 1 6 ! θ 6 + 1 8 ! θ 8 − … θ 4 = 1 4 ! − 1 6 ! θ 2 + 1 8 ! θ 4 − … (XI-11) \begin{aligned} -\frac{1-\frac{\theta^2}{2} - {\rm cos}\theta}{\theta^4} & =-\frac{1- \frac{1}{2} \theta^2 - (1 -\frac{1}{2!}\theta^{2} + \frac{1}{4!}\theta^{4} - \frac{1}{6!}\theta^{6} + \frac{1}{8!}\theta^{8} -\, \ldots)}{\theta^4}\\ &=\frac{ \frac{1}{4!}\theta^{4} - \frac{1}{6!}\theta^{6} + \frac{1}{8!}\theta^{8} - \, \ldots }{\theta^4}\\ &=\frac{1}{4!} - \frac{1}{6!}\theta^{2} + \frac{1}{8!}\theta^{4} - \, \ldots \\ \end{aligned} \tag{XI-11} −θ41−2θ2−cosθ=−θ41−21θ2−(1−2!1θ2+4!1θ4−6!1θ6+8!1θ8−…)=θ44!1θ4−6!1θ6+8!1θ8−…=4!1−6!1θ2+8!1θ4−…(XI-11)
θ − s i n θ θ 3 ⋅ 1 − 1 − θ 2 2 − c o s θ θ 4 ⋅ ( − 3 ) = 3 − 1 2 θ 2 − θ s i n θ − 3 c o s θ θ 4 = 3 − 1 2 θ 2 − θ ( θ − 1 3 ! θ 3 + 1 5 ! θ 5 − 1 7 ! θ 7 + … ) − 3 ( 1 − 1 2 ! θ 2 + 1 4 ! θ 4 − 1 6 ! θ 6 + 1 8 ! θ 8 − … ) θ 4 = 3 − 1 2 θ 2 − θ 2 + 1 3 ! θ 4 − 1 5 ! θ 6 + 1 7 ! θ 8 − … − 3 + 3 2 ! θ 2 − 3 4 ! θ 4 + 3 6 ! θ 6 − 3 8 ! θ 8 + … ) θ 4 = 1 4 ! θ 4 − 3 6 ! θ 6 + 5 8 ! θ 8 − … θ 4 = 1 4 ! − 3 6 ! θ 2 + 5 8 ! θ 4 − … (XI-12) \begin{aligned} \frac{\theta-{\rm sin}\theta}{\theta^3} \cdot 1 - \frac{1-\frac{\theta^2}{2} - {\rm cos}\theta}{\theta^4}\cdot (-3) & = \frac{3-\frac{1}{2}\theta^2 -\theta {\rm sin}\theta -3 {\rm cos}\theta}{\theta^4} \\ &=\frac{3-\frac{1}{2}\theta^2 -\theta (\theta - \frac{1}{3!}\theta^{3} + \frac{1}{5!}\theta^{5} -\frac{1}{7!}\theta^{7} + \, \ldots) -3 (1 -\frac{1}{2!}\theta^{2} + \frac{1}{4!}\theta^{4} - \frac{1}{6!}\theta^{6} + \frac{1}{8!}\theta^{8} -\, \ldots)}{\theta^4}\\ &=\frac{3-\frac{1}{2}\theta^2 -\theta^2 + \frac{1}{3!}\theta^{4} - \frac{1}{5!}\theta^{6} + \frac{1}{7!}\theta^{8} - \, \ldots - 3 +\frac{3}{2!}\theta^{2} - \frac{3}{4!}\theta^{4} + \frac{3}{6!}\theta^{6} - \frac{3}{8!}\theta^{8}+\, \ldots)}{\theta^4}\\ &=\frac{ \frac{1}{4!}\theta^{4} - \frac{3}{6!}\theta^{6} + \frac{5}{8!}\theta^{8} - \, \ldots }{\theta^4}\\ &=\frac{1}{4!} - \frac{3}{6!}\theta^{2} + \frac{5}{8!}\theta^{4} - \, \ldots \\ \end{aligned} \tag{XI-12} θ3θ−sinθ⋅1−θ41−2θ2−cosθ⋅(−3)=θ43−21θ2−θsinθ−3cosθ=θ43−21θ2−θ(θ−3!1θ3+5!1θ5−7!1θ7+…)−3(1−2!1θ2+4!1θ4−6!1θ6+8!1θ8−…)=θ43−21θ2−θ2+3!1θ4−5!1θ6+7!1θ8−…−3+2!3θ2−4!3θ4+6!3θ6−8!3θ8+…)=θ44!1θ4−6!3θ6+8!5θ8−…=4!1−6!3θ2+8!5θ4−…(XI-12)
− 1 2 ( 1 − θ 2 2 − c o s θ θ 4 − 3 θ − s i n θ − θ 3 6 θ 5 ) = − 1 2 ( − 1 4 ! + 1 6 ! θ 2 − 1 8 ! θ 4 + … − 3 − 1 5 ! θ 5 + 1 7 ! θ 7 − 1 9 ! θ 9 + … θ 5 ) = − 1 2 ( − 1 4 ! + 1 6 ! θ 2 − 1 8 ! θ 4 + … + 3 5 ! − 3 7 ! θ 2 + 3 9 ! θ 4 − … ) = − 1 2 ( − 2 5 ! + 4 7 ! θ 2 − 6 9 ! θ 4 + … ) = 1 5 ! − 2 7 ! θ 2 + 3 9 ! θ 4 − … (XI-13) \begin{aligned} -\frac{1}{2}\left( \frac{1-\frac{\theta^2}{2} -{\rm cos}\theta}{\theta^4} - 3 \frac{\theta -{\rm sin} \theta -\frac{\theta^3}{6}}{\theta^{5}}\right) &= -\frac{1}{2}\left( -\frac{1}{4!} + \frac{1}{6!}\theta^{2} - \frac{1}{8!}\theta^{4} + \, \ldots - 3 \frac{ - \frac{1}{5!}\theta^{5} + \frac{1}{7!}\theta^{7} - \frac{1}{9!}\theta^{9}+ \, \ldots }{\theta^{5}}\right) \\ &= -\frac{1}{2}\left( -\frac{1}{4!} + \frac{1}{6!}\theta^{2} - \frac{1}{8!}\theta^{4} + \, \ldots + \frac{3}{5!} - \frac{3}{7!}\theta^{2} + \frac{3}{9!}\theta^{4} - \, \ldots \right) \\ &= -\frac{1}{2}\left( -\frac{2}{5!} + \frac{4}{7!}\theta^{2} - \frac{6}{9!}\theta^{4} + \, \ldots \right) \\ &= \frac{1}{5!} - \frac{2}{7!}\theta^{2} + \frac{3}{9!}\theta^{4} - \, \ldots \end{aligned} \tag{XI-13} −21(θ41−2θ2−cosθ−3θ5θ−sinθ−6θ3)=−21(−4!1+6!1θ2−8!1θ4+…−3θ5−5!1θ5+7!1θ7−9!1θ9+…)=−21(−4!1+6!1θ2−8!1θ4+…+5!3−7!3θ2+9!3θ4−…)=−21(−5!2+7!4θ2−9!6θ4+…)=5!1−7!2θ2+9!3θ4−…(XI-13)
最后,将 (XI-10) ~ (XI-13) 分别代入 (XI-9) 得到
Q
(
ρ
,
θ
)
=
1
2
ρ
^
+
(
1
3
!
−
1
5
!
θ
2
+
1
7
!
θ
4
−
1
9
!
θ
6
+
…
)
(
θ
^
ρ
^
+
ρ
^
θ
^
)
+
(
1
4
!
−
1
6
!
θ
2
+
1
8
!
θ
4
−
…
)
(
θ
^
2
ρ
^
+
ρ
^
θ
^
2
)
+
(
1
4
!
−
3
6
!
θ
2
+
5
8
!
θ
4
−
…
)
(
θ
^
ρ
^
θ
^
)
+
(
1
5
!
−
2
7
!
θ
2
+
3
9
!
θ
4
−
…
)
(
θ
^
2
ρ
^
θ
^
+
θ
^
ρ
^
θ
^
2
)
=
1
2
ρ
^
+
θ
−
s
i
n
θ
θ
3
(
θ
^
ρ
^
+
ρ
^
θ
^
)
−
1
−
θ
2
2
−
c
o
s
θ
θ
4
(
θ
^
2
ρ
^
+
ρ
^
θ
^
2
)
+
[
θ
−
s
i
n
θ
θ
3
⋅
1
−
1
−
θ
2
2
−
c
o
s
θ
θ
4
⋅
(
−
3
)
]
(
θ
^
ρ
^
θ
^
)
−
1
2
(
1
−
θ
2
2
−
c
o
s
θ
θ
4
−
3
θ
−
s
i
n
θ
−
θ
3
6
θ
5
)
(
θ
^
2
ρ
^
θ
^
+
θ
^
ρ
^
θ
^
2
)
=
1
2
ρ
^
+
θ
−
s
i
n
θ
θ
3
(
θ
^
ρ
^
+
ρ
^
θ
^
+
θ
^
ρ
^
θ
^
)
−
1
−
θ
2
2
−
c
o
s
θ
θ
4
(
θ
^
2
ρ
^
+
ρ
^
θ
^
2
−
3
θ
^
ρ
^
θ
^
)
−
1
2
(
1
−
θ
2
2
−
c
o
s
θ
θ
4
−
3
θ
−
s
i
n
θ
−
θ
3
6
θ
5
)
(
θ
^
2
ρ
^
θ
^
+
θ
^
ρ
^
θ
^
2
)
\begin{aligned} {\bf Q}({\boldsymbol\rho}, {\boldsymbol \theta}) = & \quad\ \frac{1}{2}\hat{\boldsymbol\rho} \\ &+ (\frac{1}{3!} - \frac{1}{5!}\theta^{2} + \frac{1}{7!}\theta^{4} - \frac{1}{9!}\theta^{6} +\, \ldots )(\hat{\boldsymbol \theta} \hat{\boldsymbol\rho} + \hat{\boldsymbol\rho}\hat{\boldsymbol \theta} )\\ &+(\frac{1}{4!}-\frac{1}{6!}\theta^{2} + \frac{1}{8!}\theta^{4} -\, \ldots) (\hat{\boldsymbol \theta} ^2 \hat{\boldsymbol\rho} + \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^2)\\ &+ (\frac{1}{4!} -\frac{3}{6!}\theta^{2} + \frac{5}{8!}\theta^{4} -\, \ldots)(\hat{\boldsymbol \theta} \hat{\boldsymbol\rho}\hat{\boldsymbol \theta}) \\ & + (\frac{1}{5!} - \frac{2}{7!}\theta^{2} + \frac{3}{9!}\theta^{4} - \, \ldots ) (\hat{\boldsymbol \theta} ^2 \hat{\boldsymbol\rho} \hat{\boldsymbol \theta} + \hat{\boldsymbol \theta} \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^2)\\ = & \quad\ \frac{1}{2}\hat{\boldsymbol\rho} \\ &+ \frac{\theta-{\rm sin}\theta}{\theta^3}\,(\hat{\boldsymbol \theta} \hat{\boldsymbol\rho} + \hat{\boldsymbol\rho}\hat{\boldsymbol \theta} )\\ &-\frac{1-\frac{\theta^2}{2} - {\rm cos}\theta}{\theta^4}\, (\hat{\boldsymbol \theta} ^2 \hat{\boldsymbol\rho} + \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^2)\\ &+ \left[\frac{\theta-{\rm sin}\theta}{\theta^3} \cdot 1 - \frac{1-\frac{\theta^2}{2} - {\rm cos}\theta}{\theta^4}\cdot (-3)\right]\ (\hat{\boldsymbol \theta} \hat{\boldsymbol\rho}\hat{\boldsymbol \theta}) \\ & -\frac{1}{2}\left( \frac{1-\frac{\theta^2}{2} -{\rm cos}\theta}{\theta^4} - 3 \frac{\theta -{\rm sin} \theta -\frac{\theta^3}{6}}{\theta^{5}}\right) (\hat{\boldsymbol \theta} ^2 \hat{\boldsymbol\rho} \hat{\boldsymbol \theta} + \hat{\boldsymbol \theta} \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^2)\\ = & \quad\ \frac{1}{2}\hat{\boldsymbol\rho} \\ &+ \frac{\theta-{\rm sin}\theta}{\theta^3}\,(\hat{\boldsymbol \theta} \hat{\boldsymbol\rho} + \hat{\boldsymbol\rho}\hat{\boldsymbol \theta} + \hat{\boldsymbol \theta} \hat{\boldsymbol\rho}\hat{\boldsymbol \theta} )\\ &-\frac{1-\frac{\theta^2}{2} - {\rm cos}\theta}{\theta^4}\, (\hat{\boldsymbol \theta} ^2 \hat{\boldsymbol\rho} + \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^2 -3\hat{\boldsymbol \theta} \hat{\boldsymbol\rho}\hat{\boldsymbol \theta} )\\ & -\frac{1}{2}\left( \frac{1-\frac{\theta^2}{2} -{\rm cos}\theta}{\theta^4} - 3 \frac{\theta -{\rm sin} \theta -\frac{\theta^3}{6}}{\theta^{5}}\right) (\hat{\boldsymbol \theta} ^2 \hat{\boldsymbol\rho} \hat{\boldsymbol \theta} + \hat{\boldsymbol \theta} \hat{\boldsymbol\rho} \hat{\boldsymbol \theta}^2)\\ \end{aligned}
Q(ρ,θ)=== 21ρ^+(3!1−5!1θ2+7!1θ4−9!1θ6+…)(θ^ρ^+ρ^θ^)+(4!1−6!1θ2+8!1θ4−…)(θ^2ρ^+ρ^θ^2)+(4!1−6!3θ2+8!5θ4−…)(θ^ρ^θ^)+(5!1−7!2θ2+9!3θ4−…)(θ^2ρ^θ^+θ^ρ^θ^2) 21ρ^+θ3θ−sinθ(θ^ρ^+ρ^θ^)−θ41−2θ2−cosθ(θ^2ρ^+ρ^θ^2)+[θ3θ−sinθ⋅1−θ41−2θ2−cosθ⋅(−3)] (θ^ρ^θ^)−21(θ41−2θ2−cosθ−3θ5θ−sinθ−6θ3)(θ^2ρ^θ^+θ^ρ^θ^2) 21ρ^+θ3θ−sinθ(θ^ρ^+ρ^θ^+θ^ρ^θ^)−θ41−2θ2−cosθ(θ^2ρ^+ρ^θ^2−3θ^ρ^θ^)−21(θ41−2θ2−cosθ−3θ5θ−sinθ−6θ3)(θ^2ρ^θ^+θ^ρ^θ^2)
最终完成 eq(180) 闭形式的计算.
— End —
<div id="ref-1"></div>
[1] Joan Solà, Jeremie Deray, Dinesh Atchuthan, "A micro Lie theory for state estimation in robotics", arXiv:1812.01537 [cs.RO]
<div id="ref-2"></div>
[2] Timothy D. Barfoot, "State Estimation for Robotics", Cambridge University Press, 2017
<div id="ref-3"></div>
[3] Bernoulli Number, https://mathworld.wolfram.com/BernoulliNumber.html
<div id="ref-4"></div>
[4] 双曲三角函数与三角函数泰勒展开式, https://zhuanlan.zhihu.com/p/166135510
<div id="ref-5"></div>
[5] 三角函数半角公式, https://zhidao.baidu.com/question/1245914655285565579.html
<div id="ref-6"></div>
[6] Gregory S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups, Volume 2", Birkhäuser Boston, MA, 2011