题目: 流形上的复合函数偏导数 - Notes for “Kalman Filter on Differentiable Manifolds” - IKFoM (II)
文章目录
前言
Dongjiao He, Wei Xu, Fu Zhang 的论文 “Kalman Filter on Differentiable Manifolds”[1] 中构建的 IKFoM (“流形上的迭代 Kalman 滤波”) 的系统架构比较复杂. 另外, 为了通用化的缘故, 同时在欧几里得空间、 S O ( 3 ) SO(3) SO(3)、 S 2 \mathbb{S}^{2} S2 上推导, 增加了论文阅读的难度. 尝试在论文原作者简练的推导过程上, 以自己的理解进行详细和啰嗦地推导, 作为自己学习理解的记录.
本文是涉及到论文 “Kalman Filter on Differentiable Manifolds”[1] 的 “Table I. Operation and Differentiation of Importatnt Manifolds in Parctice” 以及 “Appendix B. Important manifolds in partice and their derivatives” 部分的阅读笔记. 主要关于
- S O ( 3 ) SO(3) SO(3) 上的复合函数偏导数 ∂ ( ( ( x ⊞ u ) ⊕ v ) ⊟ y ) ∂ u \frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{u}} ∂u∂(((x⊞u)⊕v)⊟y) 及 ∂ ( ( ( x ⊞ u ) ⊕ v ) ⊟ y ) ∂ v \frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{v}} ∂v∂(((x⊞u)⊕v)⊟y)
- S 2 ( r ) \mathbb{S}^2(r) S2(r) 上的复合函数偏导数 ∂ ( ( ( x ⊞ u ) ⊕ v ) ⊟ y ) ∂ u \frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{u}} ∂u∂(((x⊞u)⊕v)⊟y) 及 ∂ ( ( ( x ⊞ u ) ⊕ v ) ⊟ y ) ∂ v \frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{v}} ∂v∂(((x⊞u)⊕v)⊟y)
本文是相关主题的第二篇, 前一篇请参考
流形上的运动学 - Notes for “Kalman Filter on Differentiable Manifolds“ - IKFoM (I)
I. 预备知识: 求导法则与链式法则
A. 函数积的求导法则
设 u = u ( x ) u=u(x) u=u(x), v = v ( x ) v=v(x) v=v(x) 都可导, 则
( u v ) ′ = u ′ v + u v ′ (uv)^{'} = u^{'}v+uv^{'} (uv)′=u′v+uv′
B. 函数商的求导法则
设 u = u ( x ) u=u(x) u=u(x), v = v ( x ) v=v(x) v=v(x) 都可导, 则
( u v ) ′ = u ′ v − u v ′ v 2 ( v ≠ 0 ) \left(\frac{u}{v}\right)^{'} = \frac{u^{'}v-uv^{'}}{v^{2}}\quad (v\neq 0) (vu)′=v2u′v−uv′(v=0)
C. 向量的求导法则向量之间求导采用分子布局标记 (Numerator-layout notation)
假设变量 x ∈ R n \mathbf{x}\in \mathbb{R}^{n} x∈Rn, B \mathbf{B} B 的转置与 x \mathbf{x} x 的维度兼容, 但 B \mathbf{B} B 与 x \mathbf{x} x 无关, 则
d ( B T x ) d x = B T \frac{d(\mathbf{B}^{\small\rm T} \, \mathbf{x})}{d\mathbf{x}} = \mathbf{B}^{\small\rm T} dxd(BTx)=BT
D. 四象限函数 atan2 的偏导数[5]
四象限函数 a t a n 2 ( y , x ) {\rm{atan2}}(y, x) atan2(y,x) 在除了在原点和 x 轴的负半轴, 在其它定义域都是连续且光滑的, 即存在连续的无穷阶偏导. 一阶偏导为
∂ ( a t a n 2 ( y , x ) ) ∂ x = − y x 2 + y 2 ∂ ( a t a n 2 ( y , x ) ) ∂ y = x x 2 + y 2 \frac{\partial \left({\rm{atan2}}(y,x)\right)}{\partial x} = \frac{-y}{x^2+y^2}\\ \frac{\partial \left({\rm{atan2}}(y,x)\right)}{\partial y} = \frac{x}{x^2+y^2} ∂x∂(atan2(y,x))=x2+y2−y∂y∂(atan2(y,x))=x2+y2x
E. 多元复合函数求导的链式法则
若函数 u = φ ( x ) u=\varphi(x) u=φ(x), v = ψ ( t ) v=\psi(t) v=ψ(t) 在点 t t t 可导, z = f ( u , v ) z=f(u,v) z=f(u,v) 在点 ( u , v ) (u,v) (u,v) 处偏导连续, 则复合函数 z = f ( φ ( t ) , ψ ( t ) ) z=f(\varphi(t), \psi(t)) z=f(φ(t),ψ(t)) 在点 t t t 可到, 且有链式法则
d z d t = ∂ z ∂ u ⋅ d u d t + ∂ z ∂ v ⋅ d v d t \frac{dz}{dt} = \frac{\partial z}{\partial u}\cdot\frac{du}{dt} + \frac{\partial z}{\partial v}\cdot\frac{dv}{dt} dtdz=∂u∂z⋅dtdu+∂v∂z⋅dtdv
整体上, 我们主要应用流形上复合函数求导的链式法则进行推导, 相对于上面实数域中"多元复合函数求导的链式法则"形式上稍复杂, 本质上都是微扰动的传递链.
F. 流形上复合函数求导的链式法则 The chain rule of the derivative of the composition function on manifolds[2]
流形上的链式法则由 R n \mathbb{R}^n Rn 上的复合函数求导的链式法则推广而来.
Let F : N → M F: N \rightarrow M F:N→M and G : M → P G: M\rightarrow P G:M→P are smooth maps of manifolds and p ∈ N p\in N p∈N. The differentials of F F F at p p p and G G G at F ( p ) F(p) F(p) are linear maps
T p N → F ∗ , p T F ( p ) M → G ∗ , F ( p ) T G ( F ( p ) ) P T_{p}N \xrightarrow{F_{{\ast},p}} T_{F(p)}M\xrightarrow{G_{\ast,F(p)}} T_{G(F(p))}P TpNF∗,pTF(p)MG∗,F(p)TG(F(p))P
(The chain rule) Let F : N → M F: N \rightarrow M F:N→M and G : M → P G: M\rightarrow P G:M→P are smooth maps of manifolds and p ∈ N p\in N p∈N, then
( G ∘ F ) ∗ , p = G ∗ , F ( p ) ∘ F ∗ , p (G\circ F)_{\ast,p} = G_{\ast, F(p)}\circ F_{\ast,p} (G∘F)∗,p=G∗,F(p)∘F∗,p
利用上述流形上的链式法则,
(
(
x
⊞
u
)
⊕
v
)
⊟
y
((\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v})\boxminus \mathbf{y}
((x⊞u)⊕v)⊟y 关于
u
\mathbf{u}
u 和
v
\mathbf{v}
v 的偏导数可以写成
∂
(
(
(
x
⊞
u
)
⊕
v
)
⊟
y
)
∂
u
=
∂
(
z
⊟
y
)
∂
z
∣
z
=
(
x
⊞
u
)
⊕
v
⋅
∂
(
z
⊕
v
)
∂
z
∣
z
=
x
⊞
u
⋅
∂
(
x
⊞
u
)
∂
u
(I-1)
\frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{u}}= \left.\frac{\partial(\mathbf{z}\boxminus\mathbf{y})}{\partial\mathbf{z}}\right|_{\mathbf{z}=(\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v}}\cdot \left.\frac{\partial(\mathbf{z}\oplus\mathbf{v})}{\partial\mathbf{z}}\right|_{\mathbf{z}=\mathbf{x}\boxplus\mathbf{u}}\cdot \frac{\partial(\mathbf{x}\boxplus\mathbf{u})}{\partial\mathbf{u}}\tag{I-1}
∂u∂(((x⊞u)⊕v)⊟y)=∂z∂(z⊟y)
z=(x⊞u)⊕v⋅∂z∂(z⊕v)
z=x⊞u⋅∂u∂(x⊞u)(I-1)
∂ ( ( ( x ⊞ u ) ⊕ v ) ⊟ y ) ∂ v = ∂ ( z ⊟ y ) ∂ z ∣ z = ( x ⊞ u ) ⊕ v ⋅ ∂ ( ( x ⊞ u ) ⊕ v ) ∂ v (I-2) \frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{v}} = \left.\frac{\partial(\mathbf{z}\boxminus\mathbf{y})}{\partial\mathbf{z}}\right|_{\mathbf{z}=(\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v}}\cdot \frac{\partial((\mathbf{x}\boxplus\mathbf{u})\oplus\mathbf{v})}{\partial\mathbf{v}}\tag{I-2} ∂v∂(((x⊞u)⊕v)⊟y)=∂z∂(z⊟y) z=(x⊞u)⊕v⋅∂v∂((x⊞u)⊕v)(I-2)
其中 x , y ∈ M \mathbf{x}, \mathbf{y} \in \mathcal{M} x,y∈M, u ∈ R n \mathbf{u}\in \mathbb{R}^n u∈Rn 和 v ∈ R l \mathbf{v}\in \mathbb{R}^l v∈Rl.
下面将流形 M \mathcal{M} M 设定为具体的 S O ( 3 ) SO(3) SO(3) 或者 S 2 ( r ) \mathbb{S}^2(r) S2(r), 推导各个偏导数的具体形式.
II. S O ( 3 ) SO(3) SO(3) 上的复合函数求偏导
关于 S O ( 3 ) SO(3) SO(3) 上的导数与 Jacobian 在 Joan Solà, Jeremie Deray, Dinesh Atchuthan 所著 “A micro Lie theory for state estimation in robotics” [3] 中有比较详细的讨论. S O ( 3 ) SO(3) SO(3) 上的导数与 Jacobian 的详细推导, 也可参见本专栏中博文
- Notes for “A micro Lie theory for state estimation in robotics“ - (I)
- Notes for “A micro Lie theory for state estimation in robotics“ - (II)
- Notes for ``A micro Lie theory for state estimation in robotics‘‘ - (III)
- Notes for ``A micro Lie theory for state estimation in robotics‘‘ - (IV)
言归正传. 先分别对式 (I-1) 右手边三个因子项进行分析.
一. 关于 u \mathbf{u} u 的偏导
1. 第一个因子项
文献 [3] 中式 (149) 中左侧子式
J Q Q ⊖ R = J r − 1 ( Q ⊖ R ) \mathbf{J}_{\mathbf{Q}}^{\mathbf{\mathbf{Q}\ominus\mathbf{R}}} = \mathbf{J}_{r}^{-1}(\mathbf{Q}\ominus\mathbf{R}) JQQ⊖R=Jr−1(Q⊖R)
故式 (I-1) 右手边第一个因子项为
∂
(
z
⊟
y
)
∂
z
∣
z
=
(
x
⊞
u
)
⊕
v
=
J
r
−
1
(
z
⊟
y
)
∣
z
=
(
x
⊞
u
)
⊕
v
(II-1-1)
\left.\frac{\partial(\mathbf{z}\boxminus\mathbf{y})}{\partial\mathbf{z}}\right|_{\mathbf{z}=(\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v}} = \left.\mathbf{J}_{r}^{-1}(\mathbf{z}\boxminus\mathbf{y})\right|_{\mathbf{z}=(\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v}}\tag{II-1-1}
∂z∂(z⊟y)
z=(x⊞u)⊕v=Jr−1(z⊟y)
z=(x⊞u)⊕v(II-1-1)
文献 [3] 中式 (147) 可知
J r − 1 = J l − T \mathbf{J}_{r}^{-1} = \mathbf{J}_{l}^{-\rm{T}} Jr−1=Jl−T
而论文 [1] 中定义的
A
(
u
)
\mathbf{A}(\mathbf{u})
A(u) 就是文献[3] 中定义的
J
l
(
u
)
\mathbf{J}_{l}(\mathbf{u})
Jl(u).
A
(
u
)
=
J
l
(
u
)
=
J
r
(
u
)
T
(II-1-2)
\mathbf{A}(\mathbf{u}) = \mathbf{J}_{l}(\mathbf{u}) = \mathbf{J}_{r}(\mathbf{u})^{\rm T} \tag{II-1-2}
A(u)=Jl(u)=Jr(u)T(II-1-2)
故式 (I-1) 右手边第一个因子项进一步写成
∂
(
z
⊟
y
)
∂
z
∣
z
=
(
x
⊞
u
)
⊕
v
=
J
r
−
1
(
z
⊟
y
)
∣
z
=
(
x
⊞
u
)
⊕
v
=
A
(
(
(
x
⊞
u
)
⊕
v
)
⊟
y
)
−
T
(II-1-3)
\begin{aligned} \left.\frac{\partial(\mathbf{z}\boxminus\mathbf{y})}{\partial\mathbf{z}}\right|_{\mathbf{z}=(\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v}} &= \left.\mathbf{J}_{r}^{-1}(\mathbf{z}\boxminus\mathbf{y})\right|_{\mathbf{z}=(\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v}}\\ &= \mathbf{A}(((\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v})\boxminus\mathbf{y})^{-\rm{T}} \end{aligned} \tag{II-1-3}
∂z∂(z⊟y)
z=(x⊞u)⊕v=Jr−1(z⊟y)
z=(x⊞u)⊕v=A(((x⊞u)⊕v)⊟y)−T(II-1-3)
2. 第二个因子项
文献 [3] 中式 (148) 中左侧子式
J R R ⊕ θ = R ( θ ) T \mathbf{J}_{\mathbf{R}}^{\mathbf{R}\oplus\boldsymbol{\theta}} = \mathbf{R}(\boldsymbol{\theta})^{\rm T} JRR⊕θ=R(θ)T
故式 (I-1) 右手边第二个因子项为
∂
(
z
⊕
v
)
∂
z
∣
z
=
x
⊞
u
=
R
(
v
)
T
(II-1-4)
\left.\frac{\partial(\mathbf{z}\oplus\mathbf{v})}{\partial\mathbf{z}}\right|_{\mathbf{z}=\mathbf{x}\boxplus\mathbf{u}} = \mathbf{R}(\mathbf{v})^{\rm T} \tag{II-1-4}
∂z∂(z⊕v)
z=x⊞u=R(v)T(II-1-4)
指数映射形式的 Rodrigues’ rotation formula 可知
R
(
v
)
T
=
E
x
p
(
−
v
)
(II-1-5)
\mathbf{R}(\mathbf{v})^{\rm T} = {\rm{Exp}}(-\mathbf{v}) \tag{II-1-5}
R(v)T=Exp(−v)(II-1-5)
故式 (I-1) 右手边第二个因子项进一步写成
∂
(
z
⊕
v
)
∂
z
∣
z
=
x
⊞
u
=
E
x
p
(
−
v
)
(II-1-6)
\left.\frac{\partial(\mathbf{z}\oplus\mathbf{v})}{\partial\mathbf{z}}\right|_{\mathbf{z}=\mathbf{x}\boxplus\mathbf{u}} = {\rm{Exp}}(-\mathbf{v}) \tag{II-1-6}
∂z∂(z⊕v)
z=x⊞u=Exp(−v)(II-1-6)
3. 第三个因子项
文献 [3] 中式 (148) 中右侧子式
J θ R ⊕ θ = J r ( θ ) \mathbf{J}_{\boldsymbol{\theta}}^{\mathbf{R}\oplus\boldsymbol{\theta}} = \mathbf{J}_{r}(\boldsymbol{\theta}) JθR⊕θ=Jr(θ)
故式 (I-1) 右手边第三个因子项为
∂
(
x
⊞
u
)
∂
u
=
J
r
(
u
)
(II-1-7)
\frac{\partial(\mathbf{x}\boxplus\mathbf{u})}{\partial\mathbf{u}} = \mathbf{J}_{r}(\mathbf{u}) \tag{II-1-7}
∂u∂(x⊞u)=Jr(u)(II-1-7)
由式 (II-1-2) 式 (I-1) 右手边第三个因子项可以进一步写成
∂
(
x
⊞
u
)
∂
u
=
A
(
u
)
T
(II-1-8)
\frac{\partial(\mathbf{x}\boxplus\mathbf{u})}{\partial\mathbf{u}} = \mathbf{A}(\mathbf{u})^{\rm{T}}\tag{II-1-8}
∂u∂(x⊞u)=A(u)T(II-1-8)
4. 链式法则各项整合
这样
S
O
(
3
)
SO(3)
SO(3) 上关于
u
\mathbf{u}
u 的偏导数式 (I-1) 就可以写成
∂
(
(
(
x
⊞
u
)
⊕
v
)
⊟
y
)
∂
u
=
∂
(
z
⊟
y
)
∂
z
∣
z
=
(
x
⊞
u
)
⊕
v
⋅
∂
(
z
⊕
v
)
∂
z
∣
z
=
x
⊞
u
⋅
∂
(
x
⊞
u
)
∂
u
=
J
r
−
1
(
z
⊟
y
)
∣
z
=
(
x
⊞
u
)
⊕
v
⋅
R
(
v
)
T
⋅
J
r
(
u
)
=
A
(
(
(
x
⊞
u
)
⊕
v
)
⊟
y
)
−
T
⋅
E
x
p
(
−
v
)
⋅
A
(
u
)
T
(II-1-9)
\begin{aligned} \frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{u}}&= \left.\frac{\partial(\mathbf{z}\boxminus\mathbf{y})}{\partial\mathbf{z}}\right|_{\mathbf{z}=(\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v}}\cdot \left.\frac{\partial(\mathbf{z}\oplus\mathbf{v})}{\partial\mathbf{z}}\right|_{\mathbf{z}=\mathbf{x}\boxplus\mathbf{u}}\cdot \frac{\partial(\mathbf{x}\boxplus\mathbf{u})}{\partial\mathbf{u}}\\ &= \left.\mathbf{J}_{r}^{-1}(\mathbf{z}\boxminus\mathbf{y})\right|_{\mathbf{z}=(\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v}}\cdot \mathbf{R}(\mathbf{v})^{\rm T} \cdot \mathbf{J}_{r}(\mathbf{u})\\ &= \mathbf{A}(((\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v})\boxminus\mathbf{y})^{-\rm{T}}\cdot {\rm{Exp}}(-\mathbf{v}) \cdot \mathbf{A}(\mathbf{u})^{\rm{T}} \end{aligned} \tag{II-1-9}
∂u∂(((x⊞u)⊕v)⊟y)=∂z∂(z⊟y)
z=(x⊞u)⊕v⋅∂z∂(z⊕v)
z=x⊞u⋅∂u∂(x⊞u)=Jr−1(z⊟y)
z=(x⊞u)⊕v⋅R(v)T⋅Jr(u)=A(((x⊞u)⊕v)⊟y)−T⋅Exp(−v)⋅A(u)T(II-1-9)
下面, 再分别对式 (I-2) 右手边两个因子项进行分析.
二. 关于 v \mathbf{v} v 的偏导
1. 第一个因子项
式 (I-2) 右手边第一个因子项与式 (I-1) 右手边第一个因子项一致.
2. 第二个因子项
∂ ( ( x ⊞ u ) ⊕ v ) ∂ v = ∂ ( z ⊕ v ) ∂ v ∣ z = x ⊞ u = ( I I − 1 − 8 ) A ( v ) T (II-2-1) \frac{\partial((\mathbf{x}\boxplus\mathbf{u})\oplus\mathbf{v})}{\partial\mathbf{v}} = \left.\frac{\partial(\mathbf{z}\oplus\mathbf{v})}{\partial\mathbf{v}}\right|_{\mathbf{z}=\mathbf{x}\boxplus\mathbf{u}} \overset{\small\rm(II-1-8)}= \mathbf{A}(\mathbf{v})^{\rm{T}}\tag{II-2-1} ∂v∂((x⊞u)⊕v)=∂v∂(z⊕v) z=x⊞u=(II−1−8)A(v)T(II-2-1)
3. 链式法则各项整合
这样
S
O
(
3
)
SO(3)
SO(3) 上关于
v
\mathbf{v}
v 的偏导数式 (I-2) 就可以写成
∂
(
(
(
x
⊞
u
)
⊕
v
)
⊟
y
)
∂
v
=
∂
(
z
⊟
y
)
∂
z
∣
z
=
(
x
⊞
u
)
⊕
v
⋅
∂
(
(
x
⊞
u
)
⊕
v
)
∂
v
=
A
(
(
(
x
⊞
u
)
⊕
v
)
⊟
y
)
−
T
⋅
A
(
v
)
T
(II-2-2)
\begin{aligned} \frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{v}} &= \left.\frac{\partial(\mathbf{z}\boxminus\mathbf{y})}{\partial\mathbf{z}}\right|_{\mathbf{z}=(\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v}}\cdot \frac{\partial((\mathbf{x}\boxplus\mathbf{u})\oplus\mathbf{v})}{\partial\mathbf{v}}\\ & = \mathbf{A}(((\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v})\boxminus\mathbf{y})^{-\rm{T}} \cdot \mathbf{A}(\mathbf{v})^{\rm{T}} \end{aligned}\tag{II-2-2}
∂v∂(((x⊞u)⊕v)⊟y)=∂z∂(z⊟y)
z=(x⊞u)⊕v⋅∂v∂((x⊞u)⊕v)=A(((x⊞u)⊕v)⊟y)−T⋅A(v)T(II-2-2)
三. 小结
以上完成了 S O ( 3 ) SO(3) SO(3) 上的复合函数偏导数 ∂ ( ( ( x ⊞ u ) ⊕ v ) ⊟ y ) ∂ u \frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{u}} ∂u∂(((x⊞u)⊕v)⊟y) 及 ∂ ( ( ( x ⊞ u ) ⊕ v ) ⊟ y ) ∂ v \frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{v}} ∂v∂(((x⊞u)⊕v)⊟y) 的推导.
III. S 2 ( r ) \mathbb{S}^2(r) S2(r) 上的复合函数求偏导
一. S 2 \mathbb{S}^2 S2 上的几何说明
S 2 ( r ) \mathbb{S}^2(r) S2(r) 是 R 3 \mathbb{R}^3 R3 中半径为 r r r 的球面. 关于 S 2 ( r ) \mathbb{S}^2(r) S2(r) 上的复合函数求偏导的具体推导可参考的文献材料不常见, 需要认真计算才可以获得论文 [1] 中的结论. 我们下面抽丝剥茧认真推导一下.
The ⊞ \boxplus ⊞ \ ⊟ \boxminus ⊟ \ ⊕ \oplus ⊕ operations on 2-sphere[1]
S = S 2 ( r ) ≜ { x ∈ R 3 ∣ ∥ x ∥ = r , r > 0 } : \mathcal{S}=\mathbb{S}^{2}(r)\triangleq \{\mathbf{x} \in \mathbb{R}^3 | \|\mathbf{x}\| = r, r> 0\}: S=S2(r)≜{x∈R3∣∥x∥=r,r>0}:x ⊞ u = R ( B ( x ) u ) ⋅ x \mathbf{x}\boxplus \mathbf{u} = \mathbf{R}(\mathbf{B}(\mathbf{x})\mathbf{u})\cdot\mathbf{x} x⊞u=R(B(x)u)⋅x
y ⊟ x = B ( x ) T ( θ [ x ] × y ∥ [ x ] × y ∥ ) , θ = a t a n 2 ( ∥ [ x ] × y ∥ , x T y ) \mathbf{y}\boxminus \mathbf{x} = \mathbf{B}(\mathbf{x})^{\rm T}\left(\theta \frac{[\mathbf{x}]_{\times} \mathbf{y}}{\|[\mathbf{x}]_{\times} \mathbf{y}\|}\right), \quad \theta = \rm{atan2}(\|[\mathbf{x}]_{\times}\mathbf{y}\|, \mathbf{x}^{\rm T}\mathbf{y}) y⊟x=B(x)T(θ∥[x]×y∥[x]×y),θ=atan2(∥[x]×y∥,xTy)
x ⊕ v = R ( v ) ⋅ x \mathbf{x}\oplus \mathbf{v} = \mathbf{R}(\mathbf{v})\cdot \mathbf{x} x⊕v=R(v)⋅x
where
R ( w ) ∈ S O ( 3 ) \mathbf{R}(\mathbf{w})\in SO(3) R(w)∈SO(3) as a rotation about an exis-angle represented
B ( x ) = [ b 1 b 2 ] ∈ R 3 × 2 \mathbf{B}(\mathbf{x}) = \begin{bmatrix}\mathbf{b}_1 &\mathbf{b}_2\end{bmatrix} \in\mathbb{R}^{3\times 2} B(x)=[b1b2]∈R3×2 as two orthonormal bases lying in the tangent plane of x ∈ S 2 ( r ) \mathbf{x}\in \mathbb{S}^2(r) x∈S2(r)
参照和模仿文献 [4], 对以上定义绘制如 Fig.1 所示几何关系图, 以清晰描述各个向量之间的相互关系. 下面照着 Fig. 1 开始说明 2-sphere 上的几何关系.
![2-sphere](https://i-blog.csdnimg.cn/blog_migrate/52773c8fdbd23e408d3a6f94683f12f4.png)
1. 旋转向量
u ∈ R 2 \mathbf{u}\in \mathbb{R}^2 u∈R2 为在 2-sphere 的切平面局部坐标系下的旋转向量 (也可称作扰动向量), 是在切平面上的最少参数表示 (2 维). 而在 2-shpere 表面点 x \mathbf{x} x 上建立的切平面的局部坐标系的两个正交基向量 B ( x ) = [ b 1 b 2 ] \mathbf{B}(\mathbf{x})= \begin{bmatrix}\mathbf{b}_1 &\mathbf{b}_2\end{bmatrix} B(x)=[b1b2] 全局坐标系下的表示. B ( x ) ⋅ u ∈ R 3 \mathbf{B}(\mathbf{x})\cdot \mathbf{u}\in \mathbb{R}^3 B(x)⋅u∈R3 则是切平面局部坐标系下旋转向量 u \mathbf{u} u, 通过利用全局坐标系下的正交基, 转换为在全局坐标系下的表示. 因为毛球定理的缘故, u \mathbf{u} u 不是 2-sphere 上处处连续的切向量场[4].
2. 旋转作用
向量
x
∈
S
2
(
r
)
\mathbf{x}\in \mathbb{S}^2(r)
x∈S2(r) 上作用切空间上的扰动
u
\mathbf{u}
u 得到的新向量
x
⊞
u
∈
S
2
(
r
)
\mathbf{x}\boxplus \mathbf{u}\in \mathbb{S}^2(r)
x⊞u∈S2(r) 仍在 2-sphere 上. 故定义扰动
u
\mathbf{u}
u 产生的作用就是向量
x
\mathbf{x}
x 在全局坐标系下绕着扰动向量
u
\mathbf{u}
u 旋转, 即
y
≜
x
⊞
u
=
R
(
B
(
x
)
u
)
⋅
x
(III-1-1)
\mathbf{y} \triangleq \mathbf{x}\boxplus \mathbf{u} = \mathbf{R}(\mathbf{B}(\mathbf{x})\mathbf{u})\cdot\mathbf{x} \tag{III-1-1}
y≜x⊞u=R(B(x)u)⋅x(III-1-1)
故扰动向量
u
\mathbf{u}
u 就是轴-角 (axis-angle) 表示的旋转向量.
R
(
B
(
x
)
⋅
u
)
=
E
x
p
(
B
(
x
)
⋅
u
)
∈
S
O
(
3
)
\mathbf{R}(\mathbf{B}(\mathbf{x})\cdot \mathbf{u}) = {\rm{Exp}}(\mathbf{B}(\mathbf{x})\cdot \mathbf{u}) \in SO(3)
R(B(x)⋅u)=Exp(B(x)⋅u)∈SO(3) 为全局坐标系下的旋转向量
B
(
x
)
⋅
u
\mathbf{B}(\mathbf{x})\cdot \mathbf{u}
B(x)⋅u 通过指数映射对应的旋转矩阵. 因为是全局坐标系下的旋转, 故呈现为
S
O
(
3
)
SO(3)
SO(3) 上的 left-
⊞
\boxplus
⊞ 形式.
3. 坐标系转换
假设全局坐标系下表示的旋转向量为
G
u
{^{G}\mathbf{u}}
Gu, 即
G
u
=
B
(
x
)
⋅
u
(III-1-2)
{^{G}\mathbf{u}} = \mathbf{B}(\mathbf{x})\cdot \mathbf{u} \tag{III-1-2}
Gu=B(x)⋅u(III-1-2)
两边同时左乘
B
(
u
)
T
\mathbf{B}(\mathbf{u})^{\small\rm T}
B(u)T 得到
B
(
x
)
T
G
u
=
B
(
x
)
T
B
(
x
)
⋅
u
=
u
(III-1-3)
\mathbf{B}(\mathbf{x})^{\small\rm T} \,{^{G}\mathbf{u}} = \mathbf{B}(\mathbf{x})^{\small\rm T}\, \mathbf{B}(\mathbf{x})\cdot \mathbf{u} = \mathbf{u} \tag{III-1-3}
B(x)TGu=B(x)TB(x)⋅u=u(III-1-3)
故
B
(
u
)
T
\mathbf{B}(\mathbf{u})^{\small\rm T}
B(u)T 将全局坐标系下表示的旋转向量
G
u
^{G}\mathbf{u}
Gu 映射回到切平面上的局部坐标系表示下的旋转向量
u
\mathbf{u}
u.
4. 旋转的轴-角求解
2-Sphere 上两个向量
x
\mathbf{x}
x 与
y
\mathbf{y}
y 之间的夹角为
θ
=
a
t
a
n
2
(
∥
[
x
]
×
y
∥
‾
r
2
s
i
n
θ
,
x
T
y
‾
r
2
cos
θ
)
(III-1-4)
\theta = \rm{atan2}(\underset{r^2{\rm sin}\theta}{\underline{\|[\mathbf{x}]_{\times}\mathbf{y}\|}}, \underset{r^2{\rm\cos}\theta}{\underline{\mathbf{x}^{\rm T}\mathbf{y}}}) \tag{III-1-4}
θ=atan2(r2sinθ∥[x]×y∥,r2cosθxTy)(III-1-4)
2-Sphere 上两个向量
x
\mathbf{x}
x 与
y
\mathbf{y}
y 之间的轴-角旋转作用
y
⊟
x
=
B
(
x
)
T
‾
映射回切平面局部坐标系
(
θ
‾
旋转角度
[
x
]
×
y
∥
[
x
]
×
y
∥
‾
单位旋转轴向量
)
(III-1-5)
\mathbf{y}\boxminus \mathbf{x} = \underset{映射回切平面局部坐标系}{\underline{\mathbf{B}(\mathbf{x})^{\rm T}}}\left({\underset{旋转角度}{\underline{\theta}}}\, {\underset{单位旋转轴向量}{\underline{\frac{[\mathbf{x}]_{\times} \mathbf{y}}{\|[\mathbf{x}]_{\times} \mathbf{y}\|}}}}\right) \tag{III-1-5}
y⊟x=映射回切平面局部坐标系B(x)T
旋转角度θ单位旋转轴向量∥[x]×y∥[x]×y
(III-1-5)
由式 (III-1-1) 可知
y
⊟
x
=
(
x
⊞
u
)
⊟
x
=
u
\mathbf{y} \boxminus\mathbf{x} = (\mathbf{x}\boxplus \mathbf{u}) \boxminus\mathbf{x} = \mathbf{u}
y⊟x=(x⊞u)⊟x=u
故式 (III-1-5) 可以写为
u
=
B
(
x
)
T
(
θ
[
x
]
×
y
∥
[
x
]
×
y
∥
)
⇒
B
(
x
)
u
=
θ
[
x
]
×
y
∥
[
x
]
×
y
∥
(III-1-6)
\begin{aligned} \mathbf{u} &= {\mathbf{B}(\mathbf{x})^{\rm T}} \left({\theta}\, {\frac{[\mathbf{x}]_{\times} \mathbf{y}}{\|[\mathbf{x}]_{\times} \mathbf{y}\|}}\right) \\ \Rightarrow\qquad{\mathbf{B}(\mathbf{x})} \mathbf{u} &= {\theta}\, {\frac{[\mathbf{x}]_{\times} \mathbf{y}}{\|[\mathbf{x}]_{\times} \mathbf{y}\|}} \end{aligned} \tag{III-1-6}
u⇒B(x)u=B(x)T(θ∥[x]×y∥[x]×y)=θ∥[x]×y∥[x]×y(III-1-6)
5. 非切平面上旋转作用
对并不处于 2-sphere 上切平面的扰动向量
v
\mathbf{v}
v, 受到扰动后的向量也仍在 2-sphere 上, 仍可将该扰动视作为旋转作用. 故定义
x
⊕
v
=
R
(
v
)
⋅
x
(III-1-7)
\mathbf{x}\oplus \mathbf{v} = \mathbf{R}(\mathbf{v})\cdot \mathbf{x}\tag{III-1-7}
x⊕v=R(v)⋅x(III-1-7)
其中
v
∈
R
3
\mathbf{v}\in\mathbb{R}^3
v∈R3 为全局坐标系下描述.
6. 半径相关
∥ [ x ] × y ∥ 2 + ( x T y ) 2 = ( r 2 s i n θ ) 2 + ( r 2 cos θ ) 2 = r 4 (III-1-8) \|[\mathbf{x}]_{\times}\mathbf{y}\|^2 + (\mathbf{x}^{\rm T}\mathbf{y})^2 = ({r^2{\rm sin}\theta})^2 + ({r^2{\rm\cos}\theta})^2 = r^4 \tag{III-1-8} ∥[x]×y∥2+(xTy)2=(r2sinθ)2+(r2cosθ)2=r4(III-1-8)
其中 2-sphere 半径 r r r 为常数.
下面我们具体推导 S 2 ( r ) \mathbb{S}^2(r) S2(r) 上对应的式 (I-1) 和式 (I-2).
二. 关于 u \mathbf{u} u 的偏导
1. 第一个因子项
式 (I-1) 中右手边第一个因子项为
∂
(
z
⊟
y
)
∂
z
∣
z
=
(
x
⊞
u
)
⊕
v
\left.\frac{\partial(\mathbf{z}\boxminus\mathbf{y})}{\partial\mathbf{z}}\right|_{\mathbf{z}=(\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v}}
∂z∂(z⊟y)
z=(x⊞u)⊕v
两个向量
z
\mathbf{z}
z 与
y
\mathbf{y}
y 之间的轴-角旋转关系可以按照式 (III-1-5) 和式 (III-1-4) 的形式展开为
z
⊟
y
=
B
(
y
)
T
(
θ
[
y
]
×
z
∥
[
y
]
×
z
∥
)
where
θ
=
a
t
a
n
2
(
∥
[
y
]
×
z
∥
,
y
T
z
)
(III-2-1)
\mathbf{z}\boxminus\mathbf{y} = {\mathbf{B}(\mathbf{y})^{\rm T}} \left({\theta}\, {\frac{[\mathbf{y}]_{\times} \mathbf{z}}{\|[\mathbf{y}]_{\times} \mathbf{z}\|}}\right)\\ \text{where}\,\,\, \theta = \rm{atan2}({\|[\mathbf{y}]_{\times}\mathbf{z}\|}, {\mathbf{y}^{\rm T}\mathbf{z}})\tag{III-2-1}
z⊟y=B(y)T(θ∥[y]×z∥[y]×z)whereθ=atan2(∥[y]×z∥,yTz)(III-2-1)
在继续求偏导前, 先预备以下导数性质
我们分别用预备知识中提到的"函数积的求导法则"以及"向量的求导法则", 对
z
⊟
y
\mathbf{z}\boxminus \mathbf{y}
z⊟y 求
z
\mathbf{z}
z 的偏导数
∂
(
z
⊟
y
)
∂
z
=
B
(
y
)
T
∂
∂
z
(
(
[
y
]
×
z
)
⋅
(
θ
∥
[
y
]
×
z
∥
)
)
=
B
(
y
)
T
(
∂
[
y
]
×
z
∂
z
⋅
(
θ
∥
[
y
]
×
z
∥
)
+
(
[
y
]
×
z
)
⋅
∂
∂
z
(
θ
∥
[
y
]
×
z
∥
)
)
=
B
(
y
)
T
(
θ
[
y
]
×
∥
[
y
]
×
z
∥
+
(
[
y
]
×
z
)
⋅
∂
∂
z
(
θ
∥
[
y
]
×
z
∥
)
)
(III-2-2)
\begin{aligned} \frac{\partial (\mathbf{z}\boxminus\mathbf{y})}{\partial \mathbf{z}} &= {\mathbf{B}(\mathbf{y})^{\rm T}} \frac{\partial}{\partial\mathbf{z}} \left( \left( [\mathbf{y}]_{\times} \mathbf{z}\right) \cdot \left({\frac{{\theta}}{\|[\mathbf{y}]_{\times} \mathbf{z}\|}} \right)\right) \\ &= {\mathbf{B}(\mathbf{y})^{\rm T}} \left( \frac{\partial [\mathbf{y}]_{\times} \mathbf{z}}{\partial\mathbf{z}} \cdot \left({\frac{{\theta}}{\|[\mathbf{y}]_{\times} \mathbf{z}\|}} \right) + \left( [\mathbf{y}]_{\times} \mathbf{z}\right) \cdot \frac{\partial}{\partial\mathbf{z}}\left({\frac{{\theta}}{\|[\mathbf{y}]_{\times} \mathbf{z}\|}} \right) \right)\\ &= {\mathbf{B}(\mathbf{y})^{\rm T}} \left( {\frac{{\theta} [\mathbf{y}]_{\times}}{\|[\mathbf{y}]_{\times} \mathbf{z}\|}} + \left( [\mathbf{y}]_{\times} \mathbf{z}\right) \cdot \frac{\partial}{\partial\mathbf{z}}\left({\frac{{\theta}}{\|[\mathbf{y}]_{\times} \mathbf{z}\|}} \right) \right) \end{aligned} \tag{III-2-2}
∂z∂(z⊟y)=B(y)T∂z∂(([y]×z)⋅(∥[y]×z∥θ))=B(y)T(∂z∂[y]×z⋅(∥[y]×z∥θ)+([y]×z)⋅∂z∂(∥[y]×z∥θ))=B(y)T(∥[y]×z∥θ[y]×+([y]×z)⋅∂z∂(∥[y]×z∥θ))(III-2-2)
由式 (III-2-1) 可知,
θ
\theta
θ 也是关于
z
\mathbf{z}
z 的函数. 利用函数商的求导法则, 可以得到
∂
∂
z
(
θ
∥
[
y
]
×
z
∥
)
=
∂
θ
∂
z
⋅
∥
[
y
]
×
z
∥
−
θ
∂
(
∥
[
y
]
×
z
∥
)
∂
z
∥
[
y
]
×
z
∥
2
(III-2-3)
\frac{\partial}{\partial\mathbf{z}}\left({\frac{{\theta}}{\|[\mathbf{y}]_{\times} \mathbf{z}\|}} \right) = \frac{\frac{\partial \theta}{\partial\mathbf{z}}\cdot{\|[\mathbf{y}]_{\times} \mathbf{z}\|} - \theta \frac{\partial({\|[\mathbf{y}]_{\times} \mathbf{z}\|})}{\partial \mathbf{z}} }{{\|[\mathbf{y}]_{\times} \mathbf{z}\|}^2} \tag{III-2-3}
∂z∂(∥[y]×z∥θ)=∥[y]×z∥2∂z∂θ⋅∥[y]×z∥−θ∂z∂(∥[y]×z∥)(III-2-3)
现在, 式 (III-2-2) 中就剩下
∂
θ
∂
z
\frac{\partial\theta}{\partial\mathbf{z}}
∂z∂θ 和
∂
(
∥
[
y
]
×
z
∥
)
∂
z
\frac{\partial({\|[\mathbf{y}]_{\times} \mathbf{z}\|})}{\partial \mathbf{z}}
∂z∂(∥[y]×z∥) 这两项需要推导. 前者看起来比较棘手, 我们先处理简单的.
[
y
]
×
z
=
[
0
−
y
3
y
2
y
3
0
−
y
1
−
y
2
y
1
0
]
[
z
1
z
2
z
3
]
=
[
−
y
3
z
2
+
y
2
z
3
y
3
z
1
−
y
1
z
3
−
y
2
z
1
+
y
1
z
2
]
(III-2-4)
[\mathbf{y}]_{\times}{\mathbf{z}}= \begin{bmatrix} 0 &-y_3 &y_2\\ y_3 & 0& -y_1\\ -y_2 &y_1 &0 \end{bmatrix}\begin{bmatrix} z_1\\ z_2\\z_3 \end{bmatrix} =\begin{bmatrix} -y_3 z_2 + y_2 z_3\\ y_3 z_1 - y_1 z_3\\ -y_2 z_1 + y_1 z_2 \end{bmatrix} \tag{III-2-4}
[y]×z=
0y3−y2−y30y1y2−y10
z1z2z3
=
−y3z2+y2z3y3z1−y1z3−y2z1+y1z2
(III-2-4)
求范数得到
∥
[
y
]
×
z
∥
=
(
−
y
3
z
2
+
y
2
z
3
)
2
+
(
y
3
z
1
−
y
1
z
3
)
2
+
(
−
y
2
z
1
+
y
1
z
2
)
2
(III-2-5)
\|[\mathbf{y}]_{\times}{\mathbf{z}}\| = \sqrt{(-y_3 z_2 + y_2 z_3)^2 + (y_3 z_1 - y_1 z_3)^2 + (-y_2 z_1 + y_1 z_2)^2} \tag{III-2-5}
∥[y]×z∥=(−y3z2+y2z3)2+(y3z1−y1z3)2+(−y2z1+y1z2)2(III-2-5)
对
∥
[
y
]
×
z
∥
\|[\mathbf{y}]_{\times}{\mathbf{z}}\|
∥[y]×z∥ 求
z
\mathbf{z}
z 的偏导 (注意, 偏导结果选用分子布局标记)
∂
(
∥
[
y
]
×
z
∥
)
∂
z
=
[
∂
(
∥
[
y
]
×
z
∥
)
∂
z
1
∂
(
∥
[
y
]
×
z
∥
)
∂
z
2
∂
(
∥
[
y
]
×
z
∥
)
∂
z
3
]
T
(III-2-5)
=
1
∥
[
y
]
×
z
∥
[
y
3
(
y
3
z
1
−
y
1
z
3
)
−
y
2
(
−
y
2
z
1
+
y
1
z
2
)
−
y
3
(
−
y
3
z
2
+
y
2
z
3
)
+
y
1
(
−
y
2
z
1
+
y
1
z
2
)
y
2
(
−
y
3
z
2
+
y
2
z
3
)
−
y
1
(
y
3
z
1
−
y
1
z
3
)
]
T
=
(
1
∥
[
y
]
×
z
∥
[
0
y
3
−
y
2
−
y
3
0
y
1
y
2
−
y
1
0
]
[
−
y
3
z
2
+
y
2
z
3
y
3
z
1
−
y
1
z
3
−
y
2
z
1
+
y
1
z
2
]
)
T
(III-2-4)
=
(
−
[
y
]
×
∥
[
y
]
×
z
∥
[
y
]
×
z
)
T
=
−
z
T
[
y
]
×
2
∥
[
y
]
×
z
∥
(III-2-6)
\begin{aligned} \frac{\partial(\|[\mathbf{y}]_{\times}{\mathbf{z}}\|)}{\partial\mathbf{z}} & = \begin{bmatrix} \frac{\partial(\|[\mathbf{y}]_{\times}{\mathbf{z}}\|)}{\partial{z_1}}\\ \frac{\partial(\|[\mathbf{y}]_{\times}{\mathbf{z}}\|)}{\partial{z_2}}\\ \frac{\partial(\|[\mathbf{y}]_{\times}{\mathbf{z}}\|)}{\partial{z_3}} \end{bmatrix}^{\small\rm T}\\ \small\text{(III-2-5)}\quad &= \frac{1}{\|[\mathbf{y}]_{\times}{\mathbf{z}}\|}\begin{bmatrix} y_3 (y_3 z_1 - y_1 z_3) - y_2 (-y_2 z_1 + y_1 z_2)\\ -y_3 (-y_3 z_2 + y_2 z_3) + y_1 (-y_2 z_1 + y_1 z_2)\\ y_2 (-y_3 z_2 + y_2 z_3) - y_1 (y_3 z_1 - y_1 z_3) \end{bmatrix}^{\small\rm T}\\ &= \left( \frac{1}{\|[\mathbf{y}]_{\times}{\mathbf{z}}\|} \begin{bmatrix}0 &y_3 &-y_2\\ -y_3 &0 &y_1\\ y_2 & -y_1 & 0 \end{bmatrix} \begin{bmatrix}-y_3 z_2 + y_2 z_3\\ y_3 z_1 - y_1 z_3\\ -y_2 z_1 + y_1 z_2\end{bmatrix}\right)^{\small\rm T}\\ \small\text{(III-2-4)}\quad &= \left( \frac{-[\mathbf{y}]_{\times}}{\|[\mathbf{y}]_{\times}{\mathbf{z}}\|} [\mathbf{y}]_{\times}{\mathbf{z}}\right)^{\small\rm T}\\ &= - \frac{ {\mathbf{z}}^{\small\rm T} [\mathbf{y}]_{\times}^{2} }{\|[\mathbf{y}]_{\times}{\mathbf{z}}\|} \end{aligned}\tag{III-2-6}
∂z∂(∥[y]×z∥)(III-2-5)(III-2-4)=
∂z1∂(∥[y]×z∥)∂z2∂(∥[y]×z∥)∂z3∂(∥[y]×z∥)
T=∥[y]×z∥1
y3(y3z1−y1z3)−y2(−y2z1+y1z2)−y3(−y3z2+y2z3)+y1(−y2z1+y1z2)y2(−y3z2+y2z3)−y1(y3z1−y1z3)
T=
∥[y]×z∥1
0−y3y2y30−y1−y2y10
−y3z2+y2z3y3z1−y1z3−y2z1+y1z2
T=(∥[y]×z∥−[y]×[y]×z)T=−∥[y]×z∥zT[y]×2(III-2-6)
下面处理
∂
θ
∂
z
\frac{\partial\theta}{\partial\mathbf{z}}
∂z∂θ, 将式 (III-2-1) 中
θ
\theta
θ 展开式代入进行推导
∂
θ
∂
z
=
∂
(
a
t
a
n
2
(
∥
[
y
]
×
z
∥
,
y
T
z
)
)
∂
z
(多元复合函数求导的链式法则)
=
∂
(
a
t
a
n
2
(
p
,
y
T
z
)
)
∂
p
∣
p
=
∥
[
y
]
×
z
∥
⋅
∂
(
∥
[
y
]
×
z
∥
)
∂
z
+
∂
(
a
t
a
n
2
(
∥
[
y
]
×
z
∥
,
q
)
)
∂
q
∣
q
=
y
T
z
⋅
∂
(
y
T
z
)
∂
z
(四象限函数 atan2 的偏导数)+(III-1-8)
=
y
T
z
r
4
⋅
∂
(
∥
[
y
]
×
z
∥
)
∂
z
−
∥
[
y
]
×
z
∥
r
4
⋅
∂
(
y
T
z
)
∂
z
(III-2-6)+(向量的求导法则)
=
−
y
T
z
r
4
⋅
z
T
[
y
]
×
2
∥
[
y
]
×
z
∥
−
∥
[
y
]
×
z
∥
r
4
⋅
y
T
(III-2-7)
\begin{aligned} \frac{\partial\theta}{\partial\mathbf{z}} &= \frac{\partial\left({\rm{atan2}({\|[\mathbf{y}]_{\times}\mathbf{z}\|}, {\mathbf{y}^{\rm T}\mathbf{z}})}\right)}{\partial\mathbf{z}}\\ \small\text{(多元复合函数求导的链式法则)} \quad &= \left. \frac{\partial\left({\rm{atan2}}(p, {\mathbf{y}^{\rm T}\mathbf{z}}) \right)}{\partial p}\right|_{p={\|[\mathbf{y}]_{\times}\mathbf{z}\|}} \cdot\frac{\partial(\|[\mathbf{y}]_{\times}\mathbf{z}\|)}{\partial\mathbf{z}} \\ &\quad + \left. \frac{\partial\left( {\rm{atan2}} ({\|[\mathbf{y}]_{\times}\mathbf{z}\|}, q) \right)}{\partial q}\right|_{q={\mathbf{y}^{\rm T}\mathbf{z}}} \cdot \frac{\partial({\mathbf{y}^{\rm T}\mathbf{z}})}{\partial\mathbf{z}}\\ \small\text{(四象限函数 atan2 的偏导数)+(III-1-8)}\quad&= \frac{{\mathbf{y}^{\rm T}\mathbf{z}}}{r^4} \cdot \frac{\partial(\|[\mathbf{y}]_{\times}\mathbf{z}\|)}{\partial\mathbf{z}} - \frac{{\|[\mathbf{y}]_{\times}\mathbf{z}\|}}{r^4}\cdot \frac{\partial({\mathbf{y}^{\rm T}\mathbf{z}})}{\partial\mathbf{z}}\\ \small\text{(III-2-6)+(向量的求导法则)} \quad&= -\frac{{\mathbf{y}^{\rm T}\mathbf{z}}}{r^4} \cdot \frac{ {\mathbf{z}}^{\small\rm T} [\mathbf{y}]_{\times}^{2} }{\|[\mathbf{y}]_{\times}{\mathbf{z}}\|} - \frac{{\|[\mathbf{y}]_{\times}\mathbf{z}\|}}{r^4}\cdot {\mathbf{y}^{\rm T}} \end{aligned} \tag{III-2-7}
∂z∂θ(多元复合函数求导的链式法则)(四象限函数 atan2 的偏导数)+(III-1-8)(III-2-6)+(向量的求导法则)=∂z∂(atan2(∥[y]×z∥,yTz))=∂p∂(atan2(p,yTz))
p=∥[y]×z∥⋅∂z∂(∥[y]×z∥)+∂q∂(atan2(∥[y]×z∥,q))
q=yTz⋅∂z∂(yTz)=r4yTz⋅∂z∂(∥[y]×z∥)−r4∥[y]×z∥⋅∂z∂(yTz)=−r4yTz⋅∥[y]×z∥zT[y]×2−r4∥[y]×z∥⋅yT(III-2-7)
将式 (III-2-6) 和式 (III-2-7) 代入式 (III-2-3) 得到
∂
∂
z
(
θ
∥
[
y
]
×
z
∥
)
=
∂
θ
∂
z
∥
[
y
]
×
z
∥
−
θ
∂
(
∥
[
y
]
×
z
∥
)
∂
z
∥
[
y
]
×
z
∥
2
=
1
r
4
y
T
z
[
[
y
]
×
z
]
×
y
∥
[
y
]
×
z
∥
2
−
1
r
4
y
T
−
[
[
y
]
×
z
]
×
y
∥
[
y
]
×
z
∥
3
θ
=
1
r
4
(
−
y
T
z
∥
[
y
]
×
z
∥
+
r
4
θ
∥
[
y
]
×
z
∥
3
z
T
[
y
]
×
2
−
y
T
)
≜
P
(
z
,
y
)
(III-2-8)
\begin{aligned} \frac{\partial}{\partial\mathbf{z}}\left({\frac{{\theta}}{\|[\mathbf{y}]_{\times} \mathbf{z}\|}} \right) &= \frac{\frac{\partial \theta}{\partial\mathbf{z}}{\|[\mathbf{y}]_{\times} \mathbf{z}\|} - \theta \frac{\partial({\|[\mathbf{y}]_{\times} \mathbf{z}\|})}{\partial \mathbf{z}} }{{\|[\mathbf{y}]_{\times} \mathbf{z}\|}^2}\\ &=\frac{1}{r^4} \frac{ {\mathbf{y}^{\rm T}\mathbf{z}} \left[ [\mathbf{y}]_{\times}{\mathbf{z}}\right]_{\times} \mathbf{y} }{\|[\mathbf{y}]_{\times}{\mathbf{z}}\|^2} - \frac{1}{r^4} \mathbf{y}^{\small\rm T} - \frac{ \left[ [\mathbf{y}]_{\times}{\mathbf{z}}\right]_{\times} \mathbf{y} }{\|[\mathbf{y}]_{\times}{\mathbf{z}}\|^3} \theta\\ &= \frac{1}{r^4} \left( \frac{ -{\mathbf{y}^{\rm T}\mathbf{z}}\|[\mathbf{y}]_{\times}{\mathbf{z}}\| + r^4\theta}{\|[\mathbf{y}]_{\times}{\mathbf{z}}\|^3} {\mathbf{z}}^{\small\rm T} [\mathbf{y}]_{\times}^{2} - \mathbf{y}^{\small\rm T}\right)\\ &\triangleq \mathbf{P}(\mathbf{z}, \mathbf{y}) \end{aligned} \tag{III-2-8}
∂z∂(∥[y]×z∥θ)=∥[y]×z∥2∂z∂θ∥[y]×z∥−θ∂z∂(∥[y]×z∥)=r41∥[y]×z∥2yTz[[y]×z]×y−r41yT−∥[y]×z∥3[[y]×z]×yθ=r41(∥[y]×z∥3−yTz∥[y]×z∥+r4θzT[y]×2−yT)≜P(z,y)(III-2-8)
式 (III-2-8) 代入式 (III-2-2) 得到
N
(
z
,
y
)
≜
∂
(
z
⊟
y
)
∂
z
=
B
(
y
)
T
(
θ
[
y
]
×
∥
[
y
]
×
z
∥
+
(
[
y
]
×
z
)
⋅
P
(
z
,
y
)
)
(III-2-9)
\begin{aligned} \mathbf{N}(\mathbf{z},\mathbf{y})\triangleq \frac{\partial (\mathbf{z}\boxminus\mathbf{y})}{\partial \mathbf{z}} &= {\mathbf{B}(\mathbf{y})^{\rm T}} \left( {\frac{{\theta} [\mathbf{y}]_{\times}}{\|[\mathbf{y}]_{\times} \mathbf{z}\|}} + \left( [\mathbf{y}]_{\times} \mathbf{z}\right) \cdot \mathbf{P}(\mathbf{z}, \mathbf{y}) \right) \end{aligned} \tag{III-2-9}
N(z,y)≜∂z∂(z⊟y)=B(y)T(∥[y]×z∥θ[y]×+([y]×z)⋅P(z,y))(III-2-9)
这样就完成了
S
2
(
r
)
\mathbb{S}^2(r)
S2(r) 上式 (I-1) 右手边第一个因子项的推导.
2. 第二个因子项
S
2
(
r
)
\mathbb{S}^2(r)
S2(r) 上式 (I-1) 右手边第二个因子项为
∂
(
z
⊕
v
)
∂
z
=
(III-1-7)
∂
(
R
(
v
)
⋅
z
)
∂
z
=
R
(
v
)
(III-2-10)
\frac{\partial(\mathbf{z}\oplus\mathbf{v})}{\partial\mathbf{z}} \overset{\small\text{(III-1-7)}}= \frac{\partial(\mathbf{R}(\mathbf{v})\cdot \mathbf{z})}{\partial \mathbf{z}} = \mathbf{R}(\mathbf{v}) \tag{III-2-10}
∂z∂(z⊕v)=(III-1-7)∂z∂(R(v)⋅z)=R(v)(III-2-10)
3. 第三个因子项
S 2 ( r ) \mathbb{S}^2(r) S2(r) 上式 (I-1) 右手边第三个因子项为
∂ ( x ⊞ u ) ∂ u = (III-1-1) ∂ { R ( B ( x ) u ) ⋅ x } ∂ u (链式法则) = ∂ { R ( w ) ⋅ x } ∂ { R ( w ) } ∣ w = B ( x ) u ⋅ ∂ R ( w ) ∂ w ∣ w = B ( x ) u ⋅ ∂ ( B ( x ) u ) ∂ u (III-2-11) \begin{aligned} \frac{\partial(\mathbf{x}\boxplus\mathbf{u})}{\partial\mathbf{u}} &\overset{\small\text{(III-1-1)}}= \frac{\partial\left\{\mathbf{R}(\mathbf{B}(\mathbf{x})\,\mathbf{u})\cdot\mathbf{x}\right\}}{\partial\mathbf{u}}\\ \small\text{(链式法则)}\qquad&\;\;\,\, = \left.\frac{\partial\left\{\mathbf{R}(\mathbf{w})\cdot\mathbf{x}\right\}}{\partial \left\{\mathbf{R}(\mathbf{w})\right\}}\right|_{\mathbf{w}=\mathbf{B}(\mathbf{x})\mathbf{u}} \cdot \left.\frac{{\partial \mathbf{R}(\mathbf{w})}}{\partial \mathbf{w}} \right|_{\mathbf{w}=\mathbf{B}(\mathbf{x})\mathbf{u}} \cdot \frac{\partial \left(\mathbf{B}(\mathbf{x})\,\mathbf{u}\right)}{\partial \mathbf{u}} \end{aligned} \tag{III-2-11} ∂u∂(x⊞u)(链式法则)=(III-1-1)∂u∂{R(B(x)u)⋅x}=∂{R(w)}∂{R(w)⋅x} w=B(x)u⋅∂w∂R(w) w=B(x)u⋅∂u∂(B(x)u)(III-2-11)
文献 [3] 中式 (150) “Rotation action”
J R R ⋅ x = − R [ x ] × \mathbf{J}_{\mathbf{R}}^{\mathbf{R}\cdot\mathbf{x}} = -\mathbf{R}[\mathbf{x}]_{\times} JRR⋅x=−R[x]×
故式 (III-2-11) 右手侧第一因子项可以写成
∂
{
R
(
w
)
⋅
x
}
∂
{
R
(
w
)
}
∣
w
=
B
(
x
)
u
=
−
R
(
B
(
x
)
u
)
[
x
]
×
=
−
E
x
p
(
B
(
x
)
u
)
[
x
]
×
(III-2-12)
\left.\frac{\partial\left\{\mathbf{R}(\mathbf{w})\cdot\mathbf{x}\right\}}{\partial \left\{\mathbf{R}(\mathbf{w})\right\}}\right|_{\mathbf{w}=\mathbf{B}(\mathbf{x})\mathbf{u}} = - \mathbf{R}(\mathbf{B}(\mathbf{x})\,\mathbf{u})\,[\mathbf{x}]_{\times} = - {\rm{Exp}}(\mathbf{B}(\mathbf{x})\,\mathbf{u})\,[\mathbf{x}]_{\times} \tag{III-2-12}
∂{R(w)}∂{R(w)⋅x}
w=B(x)u=−R(B(x)u)[x]×=−Exp(B(x)u)[x]×(III-2-12)
文献 [3] 中定义了流形上的 right Jacobian
J r ( τ ) ≜ τ D E x p ( τ ) D τ \mathbf{J}_{r}({\boldsymbol{\tau}}) \triangleq \frac{{^{\boldsymbol{\tau}}D\,{\rm Exp}({\boldsymbol{\tau}})}}{D\,{\boldsymbol{\tau}}} Jr(τ)≜DττDExp(τ)
故式 (III-2-11) 右手侧第二因子项可以写成
∂
R
(
w
)
∂
w
∣
w
=
B
(
x
)
u
=
∂
E
x
p
(
w
)
∂
w
∣
w
=
B
(
x
)
u
(right Jacobian)
=
J
r
(
B
(
x
)
u
)
(II-1-2)
=
A
(
B
(
x
)
u
)
T
(III-2-13)
\begin{aligned} \left.\frac{{\partial \mathbf{R}(\mathbf{w})}}{\partial \mathbf{w}} \right|_{\mathbf{w}=\mathbf{B}(\mathbf{x})\mathbf{u}} &= \left.\frac{{\partial {\rm{Exp}}(\mathbf{w})}}{\partial \mathbf{w}} \right|_{\mathbf{w}=\mathbf{B}(\mathbf{x})\mathbf{u}}\\ \small\text{(right Jacobian)}\quad &= \mathbf{J}_{r}(\mathbf{B}(\mathbf{x})\,\mathbf{u}) \\ \small\text{(II-1-2)}\quad &= \mathbf{A}(\mathbf{B}(\mathbf{x})\,\mathbf{u})^{\small\rm T} \end{aligned}\tag{III-2-13}
∂w∂R(w)
w=B(x)u(right Jacobian)(II-1-2)=∂w∂Exp(w)
w=B(x)u=Jr(B(x)u)=A(B(x)u)T(III-2-13)
由向量的求导法则的分子布局标记, 式 (III-2-11) 右手侧第三因子项可以写成
∂
(
B
(
x
)
u
)
∂
u
=
B
(
x
)
(III-2-14)
\frac{\partial \left(\mathbf{B}(\mathbf{x})\,\mathbf{u}\right)}{\partial \mathbf{u}} = \mathbf{B}(\mathbf{x}) \tag{III-2-14}
∂u∂(B(x)u)=B(x)(III-2-14)
以上分项代回式 (III-2-11), 则
S
2
(
r
)
\mathbb{S}^2(r)
S2(r) 上式 (I-1) 右手边第三个因子项为
M
(
x
,
u
)
≜
∂
(
x
⊞
u
)
∂
u
=
−
E
x
p
(
B
(
x
)
u
)
[
x
]
×
A
(
B
(
x
)
u
)
T
B
(
x
)
(III-2-15)
\mathbf{M}(\mathbf{x},\mathbf{u}) \triangleq \frac{\partial(\mathbf{x}\boxplus\mathbf{u})}{\partial\mathbf{u}} = - {\rm{Exp}}(\mathbf{B}(\mathbf{x})\,\mathbf{u})\,[\mathbf{x}]_{\times}\, \mathbf{A}(\mathbf{B}(\mathbf{x})\,\mathbf{u})^{\small\rm T} \, \mathbf{B}(\mathbf{x}) \tag{III-2-15}
M(x,u)≜∂u∂(x⊞u)=−Exp(B(x)u)[x]×A(B(x)u)TB(x)(III-2-15)
4. 链式法则各项整合
结合式 (III-2-9)、式 (III-2-10) 和式 (III-2-15),
S
2
(
r
)
\mathbb{S}^{2}(r)
S2(r) 上关于
u
\mathbf{u}
u 的偏导数式 (I-1) 就可以写成
∂
(
(
(
x
⊞
u
)
⊕
v
)
⊟
y
)
∂
u
=
∂
(
z
⊟
y
)
∂
z
∣
z
=
(
x
⊞
u
)
⊕
v
⋅
∂
(
z
⊕
v
)
∂
z
∣
z
=
x
⊞
u
⋅
∂
(
x
⊞
u
)
∂
u
=
N
(
(
x
⊞
u
)
⊕
v
,
y
)
R
(
v
)
M
(
x
,
u
)
(III-2-16)
\begin{aligned} \frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{u}}&= \left.\frac{\partial(\mathbf{z}\boxminus\mathbf{y})}{\partial\mathbf{z}}\right|_{\mathbf{z}=(\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v}}\cdot \left.\frac{\partial(\mathbf{z}\oplus\mathbf{v})}{\partial\mathbf{z}}\right|_{\mathbf{z}=\mathbf{x}\boxplus\mathbf{u}}\cdot \frac{\partial(\mathbf{x}\boxplus\mathbf{u})}{\partial\mathbf{u}}\\ &= \mathbf{N}((\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v},\mathbf{y})\, \mathbf{R}(\mathbf{v})\, \mathbf{M}(\mathbf{x},\mathbf{u}) \end{aligned} \tag{III-2-16}
∂u∂(((x⊞u)⊕v)⊟y)=∂z∂(z⊟y)
z=(x⊞u)⊕v⋅∂z∂(z⊕v)
z=x⊞u⋅∂u∂(x⊞u)=N((x⊞u)⊕v,y)R(v)M(x,u)(III-2-16)
下面再分别对 S 2 ( r ) \mathbb{S}^{2}(r) S2(r) 上的式 (I-2) 右手边两个因子项进行分析.
三. 关于 v \mathbf{v} v 的偏导
1. 第一个因子项
S 2 ( r ) \mathbb{S}^2(r) S2(r) 上式 (I-2) 右手边第一个因子项与式 (I-1) 右手边第一个因子项一致, 都可以参考式 (III-2-9).
2. 第二个因子项
∂ ( ( x ⊞ u ) ⊕ v ) ∂ v = ∂ ( z ⊕ v ) ∂ v ∣ z = x ⊞ u (III-1-7) = ∂ ( R ( v ) ⋅ z ) ∂ v ∣ z = x ⊞ u = ∂ ( R ( v ) ⋅ z ) ∂ R ( v ) ∣ z = x ⊞ u ⋅ ∂ R ( v ) ∂ v ‾ (III-2-12)+ (III-2-13) ‾ = − R ( v ) [ x ⊞ u ] × A ( v ) T ‾ (III-3-1) \begin{aligned} \frac{\partial((\mathbf{x}\boxplus\mathbf{u})\oplus\mathbf{v})}{\partial\mathbf{v}} &= \left.\frac{\partial(\mathbf{z}\oplus\mathbf{v})}{\partial\mathbf{v}}\right|_{\mathbf{z}=\mathbf{x}\boxplus\mathbf{u}}\\ {\small\text{(III-1-7)}} \quad &= \left.\frac{\partial \left(\mathbf{R}(\mathbf{v})\cdot \mathbf{z}\right)}{\partial\mathbf{v}} \right|_{\mathbf{z}=\mathbf{x}\boxplus\mathbf{u}}\\ &= \left.\frac{\partial \left(\mathbf{R}(\mathbf{v})\cdot \mathbf{z}\right)}{\partial\mathbf{R}(\mathbf{v})} \right|_{\mathbf{z}=\mathbf{x}\boxplus\mathbf{u}} \cdot \underline{\frac{\partial \,\mathbf{R}(\mathbf{v})}{\partial \, \mathbf{v}}}\\ \small\text{(III-2-12)+}\underline{\small\text{(III-2-13)}}\quad &= {- \mathbf{R}(\mathbf{v})[\mathbf{x}\boxplus\mathbf{u}]_{\times}} \, \underline{\mathbf{A}(\mathbf{v})^{\small\rm T}} \end{aligned} \tag{III-3-1} ∂v∂((x⊞u)⊕v)(III-1-7)(III-2-12)+(III-2-13)=∂v∂(z⊕v) z=x⊞u=∂v∂(R(v)⋅z) z=x⊞u=∂R(v)∂(R(v)⋅z) z=x⊞u⋅∂v∂R(v)=−R(v)[x⊞u]×A(v)T(III-3-1)
3. 链式法则各项整合
这样结合式 (III-2-9) 和式 (III-3-1),
S
2
(
r
)
\mathbb{S}^2(r)
S2(r) 上关于
v
\mathbf{v}
v 的偏导数式 (I-2) 就可以写成
∂
(
(
(
x
⊞
u
)
⊕
v
)
⊟
y
)
∂
v
=
∂
(
z
⊟
y
)
∂
z
∣
z
=
(
x
⊞
u
)
⊕
v
⋅
∂
(
(
x
⊞
u
)
⊕
v
)
∂
v
=
−
N
(
(
x
⊞
u
)
⊕
v
,
y
)
R
(
v
)
[
x
⊞
u
]
×
A
(
v
)
T
(III-3-2)
\begin{aligned} \frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{v}} &= \left.\frac{\partial(\mathbf{z}\boxminus\mathbf{y})}{\partial\mathbf{z}}\right|_{\mathbf{z}=(\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v}}\cdot \frac{\partial((\mathbf{x}\boxplus\mathbf{u})\oplus\mathbf{v})}{\partial\mathbf{v}}\\ & = - \mathbf{N}((\mathbf{x}\boxplus\mathbf{u})\oplus \mathbf{v},\mathbf{y}) \, { \mathbf{R}(\mathbf{v})\,[\mathbf{x}\boxplus\mathbf{u}]_{\times}} \, {\mathbf{A}(\mathbf{v})^{\small\rm T}} \end{aligned}\tag{III-3-2}
∂v∂(((x⊞u)⊕v)⊟y)=∂z∂(z⊟y)
z=(x⊞u)⊕v⋅∂v∂((x⊞u)⊕v)=−N((x⊞u)⊕v,y)R(v)[x⊞u]×A(v)T(III-3-2)
四. 小结
以上完成了 S 2 ( r ) \mathbf{S}^{2}(r) S2(r) 上的复合函数偏导数 ∂ ( ( ( x ⊞ u ) ⊕ v ) ⊟ y ) ∂ u \frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{u}} ∂u∂(((x⊞u)⊕v)⊟y) 及 ∂ ( ( ( x ⊞ u ) ⊕ v ) ⊟ y ) ∂ v \frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{v}} ∂v∂(((x⊞u)⊕v)⊟y) 的推导.
总结
本文主要是阅读论文 “Kalman Filters on Differentiable Manifolds”[1] 过程中, 对 S O ( 3 ) SO(3) SO(3) 和 S 2 ( r ) \mathbf{S}^{2}(r) S2(r) 上的复合函数偏导数 ∂ ( ( ( x ⊞ u ) ⊕ v ) ⊟ y ) ∂ u \frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{u}} ∂u∂(((x⊞u)⊕v)⊟y) 及 ∂ ( ( ( x ⊞ u ) ⊕ v ) ⊟ y ) ∂ v \frac{\partial(((\mathbf{x}\boxplus \mathbf{u})\oplus\mathbf{v})\boxminus\mathbf{y})}{\partial\mathbf{v}} ∂v∂(((x⊞u)⊕v)⊟y) 进行了详细和啰嗦地推导.
参考文献
[1] Dongjiao He, Wei Xu, Fu Zhang, “Kalman Filters on Differentiable Manifolds”, https://arxiv.org/abs/2102.03804, 2021, Version V3
[2] Loring W. Tu, “An Introduction to Manifolds”, Springer New York, NY, 2010
[3] Joan Solà, Jeremie Deray, Dinesh Atchuthan, “A micro Lie theory for state estimation in robotics”, https://arxiv.org/abs/1812.01537, 2021, Version v9
[4] Michael Bloesch, Michael Burri, Sammy Omari, Marco Hutter, Roland Siegwart, Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback, The International Journal of Robotics Research, 2017, Vol. 36(10), 1053–1072
[5] addis, 四象限 Arctan 函数(atan2), https://wuli.wiki/changed/Arctan.html#:~:text=%E4%B8%80%E9%98%B6%E5%81%8F%E5%AF%BC%E4%B8%BA%20%E2%88%82%20%E2%88%82x%20Arctan%28y%2Cx%29%20%3D%20%E2%88%92y%20x2%20%2By2%2C,x%29%20%3D%20x%20x%202%20%2B%20y%202