福州做网站的,合肥网站建设开发,建设的网站后台会自动退出是正常的,深圳网站建设怎样李群视角下的运动学
连续时间下的运动学模型
物体在连续时间下运动的位姿由 R ( t ) R(t) R(t)和 t ( t ) t(t) t(t)表示#xff0c;根据 R R R的正交性质有#xff1a; R ( t ) T R ( t ) I R(t)^{T}R(t) I R(t)TR(t)I 公式两边对时间 t t t求导有#xff1a; R ( t …李群视角下的运动学
连续时间下的运动学模型
物体在连续时间下运动的位姿由 R ( t ) R(t) R(t)和 t ( t ) t(t) t(t)表示根据 R R R的正交性质有 R ( t ) T R ( t ) I R(t)^{T}R(t) I R(t)TR(t)I 公式两边对时间 t t t求导有 R ( t ) ˙ T R ( t ) R ( t ) T R ( t ) ˙ 0 \dot{R(t)}^{T}R(t) R(t)^{T}\dot{R(t)} 0 R(t)˙TR(t)R(t)TR(t)˙0 R ( t ) T R ( t ) ˙ − ( R ( t ) T R ( t ) ˙ ) T R(t)^{T}\dot{R(t)} -(R(t)^{T}\dot{R(t)})^{T} R(t)TR(t)˙−(R(t)TR(t)˙)T 可以发现 R ( t ) T R ( t ) ˙ R(t)^{T}\dot{R(t)} R(t)TR(t)˙是反对称矩阵不妨取 ω ( t ) ∧ R ( t ) T R ( t ) ˙ \omega(t)^{\wedge} R(t)^{T}\dot{R(t)} ω(t)∧R(t)TR(t)˙后面推导会发现 ω ( t ) \omega(t) ω(t)的意义其实就是瞬时角速度此时有 R ( t ) ˙ R ( t ) ω ( t ) ∧ \dot{R(t)} R(t)\omega(t)^{\wedge} R(t)˙R(t)ω(t)∧ 这里本人认为明确标出 ( t ) (t) (t)可以更容易理解变量之间的关系否则很容易产生误解或疑惑。
上面的方程称为泊松方程Poinsson Formula这是一个微分方程但是不要对它直接求解我们假设 ω \omega ω是固定的值并给定初值 t 0 t_{0} t0时刻的旋转矩阵为 R ( t 0 ) R(t_{0}) R(t0)此时微分方程有解 R ( t ) R ( t 0 ) e x p ( ω ∧ ( t − t 0 ) ) R(t) R(t_{0}) exp(\omega^{\wedge}(t - t_{0})) R(t)R(t0)exp(ω∧(t−t0)) 此时会发现 ω \omega ω刚好对应为角速度而这个解描述的就是角速度不变的情况下的运动方程如果让 t t t无限接近 t 0 t_{0} t0也就是利用极限的思想 ω \omega ω就是瞬时角速度。
离散时间下的运动学模型
离散时间下认为瞬时角速度在短时间内不变也就是 t 0 t_{0} t0到 t t t时刻内不变于是有 R ( t ) R ( t 0 ) e x p ( ω ∧ ( t − t 0 ) ) R ( t 0 ) e x p ( ω ∧ △ t ) R(t) R(t_{0}) exp(\omega^{\wedge}(t - t_{0})) R(t_{0}) exp(\omega^{\wedge}\triangle t) R(t)R(t0)exp(ω∧(t−t0))R(t0)exp(ω∧△t)
线性近似形式
对 R ( t ) R(t) R(t)在 t 0 t_{0} t0时刻做一阶泰勒展开 R ( t 0 △ t ) ≈ R ( t 0 ) R ( t 0 ) ˙ △ t R ( t 0 ) R ( t 0 ) ω ∧ △ t R ( t 0 ) ( I ω ∧ △ t ) R(t_{0} \triangle t) \approx R(t_{0}) \dot{R(t_{0})}\triangle t R(t_{0}) R(t_{0})\omega^{\wedge} \triangle t R(t_{0})(I \omega^{\wedge}\triangle t) R(t0△t)≈R(t0)R(t0)˙△tR(t0)R(t0)ω∧△tR(t0)(Iω∧△t)
离散形式和线性近似形式是更常用的处理形式。
四元数视角下的运动学
描述旋转的四元数具有单位性约束也就是说必须是单位四元数所以满足 q q ∗ q ∗ q 1 qq^{\ast} q^{\ast}q 1 qq∗q∗q1。 q ∗ q 1 q^{\ast}q 1 q∗q1两侧对时间求导有 q ∗ ˙ q q ∗ q ˙ 0 \dot{q^{\ast}}q q^{\ast}\dot{q} 0 q∗˙qq∗q˙0 q ∗ q ˙ − q ∗ ˙ q − ( q ∗ q ˙ ) ∗ q^{\ast}\dot{q} -\dot{q^{\ast}}q -(q^{\ast}\dot{q})^{\ast} q∗q˙−q∗˙q−(q∗q˙)∗ 可以发现 q ∗ q ˙ q^{\ast}\dot{q} q∗q˙是一个纯虚四元数不妨令其为 ϖ \varpi ϖ后面会发现 ϖ \varpi ϖ和李群下的 ω \omega ω有关系准确的说虚部为 1 2 ω \frac{1}{2} \omega 21ω也体现了瞬时角速度的意义。
于是有 q ∗ q ˙ ϖ q^{\ast}\dot{q} \varpi q∗q˙ϖ q ˙ q ϖ \dot{q} q\varpi q˙qϖ
同样假设 ϖ \varpi ϖ为固定值上面的微分方程的解为 q ( t ) q ( t 0 ) e x p ( ϖ △ t ) q(t) q(t_{0})exp(\varpi \triangle t) q(t)q(t0)exp(ϖ△t)。
这里要说明的是这里的指数映射为纯虚四元数的指数映射结果为单位四元数具体的定义为 e x p ( ϖ ) e x p ( θ u ) c o s θ u s i n θ exp(\varpi) exp(\theta \mathbf{u}) cos\theta \mathbf{u}sin \theta exp(ϖ)exp(θu)cosθusinθ
不妨把纯虚四元数 ϖ \varpi ϖ看成四元数形式的李代数。
四元数视角下的运动学与李群视角下运动学的关系
下面要介绍的是上面两中运动学表达之间的关系我们的关注点在于如何把两种表达的物理意义统一在一个描述下。
假设有一个旋转矩阵 R R R和对应的旋转向量 ϕ θ n \phi \theta\mathbf{n} ϕθn对应的四元数为 q e x p ( ϖ ) qexp(\varpi) qexp(ϖ)。
根据四元数与旋转向量之间的转换关系可知 q [ c o s θ 2 , n T s i n θ 2 ] T q [cos\frac{\theta}{2}, \mathbf{n}^{T}sin\frac{\theta}{2}]^{T} q[cos2θ,nTsin2θ]T所以有 ϖ [ 0 , 1 2 ϕ T ] T \varpi [0, \frac{1}{2}\phi^{T}]^{T} ϖ[0,21ϕT]T。
所以四元数视角下的“角速度” ϖ \varpi ϖ其实是瞬时角速度的一半。
为了统一物理意义我们让 ω \omega ω仍然代表瞬时角速度此时四元数视角下的运动学方程为 q ˙ 1 2 q [ 0 , ω T ] T \dot{q} \frac{1}{2}q[0, \omega^{T}]^{T} q˙21q[0,ωT]T
假设某一时刻 t 0 t_{0} t0的瞬时角速度为 ω ϕ θ n \omega \phi \theta \mathbf{n} ωϕθn由上一部分的四元数微分方程的解 q ( t ) q ( t 0 ) e x p ( ϖ △ t ) q(t) q(t_{0})exp(\varpi \triangle t) q(t)q(t0)exp(ϖ△t)可以看出离散形式下的四元数更新方式 q ( t ) q ( t 0 ) e x p ( ϖ △ t ) q ( t 0 ) e x p ( [ 0 , 1 2 ϕ T ] T △ t ) q(t) q(t_{0})exp(\varpi \triangle t) q(t_{0})exp([0, \frac{1}{2}\phi^{T}]^{T} \triangle t) q(t)q(t0)exp(ϖ△t)q(t0)exp([0,21ϕT]T△t) 因为 e x p ( [ 0 , 1 2 ϕ T ] T ) [ c o s θ 2 , n T s i n θ 2 ] T exp([0, \frac{1}{2}\phi^{T}]^{T}) [cos\frac{\theta}{2}, \mathbf{n}^{T}sin\frac{\theta}{2}]^{T} exp([0,21ϕT]T)[cos2θ,nTsin2θ]T当 ω \omega ω很小时就可以近似于是四元数的更新近似为 q ( t ) q ( t 0 ) [ 1 , 1 2 ω T ] T q(t) q(t_{0})[1, \frac{1}{2}\omega^{T}]^{T} q(t)q(t0)[1,21ωT]T
要注意的是如果使用这种近似形式更新后的四元数不再是单位四元数所以长时间更新后要对四元数重新归一化。
参考资料《自动驾驶与机器人中的SLAM技术》 高翔著
