四元数的球面线性插值(slerp)

四元数的球面线性插值

Reference:

  1. 四元数插值与均值(姿态平滑)
  2. MathWorks:slerp
  3. 四元数的球面线性插值(slerp)
  4. Quaternion kinematics for the error-state KF

1. 概念

对于一般线性插值来说: r = P 0 + ( P 1 − P 0 ) t \mathbf{r}=\mathbf{P}_0+\left(\mathbf{P}_1-\mathbf{P}_0\right) t r=P0+(P1P0)t,其中 t ∈ [ 0 , 1 ] \mathrm{t} \in[0,1] t[0,1],代表了插值矢量 r \mathbf{r} r 末端在弦 P 0 − P 1 \mathbf{P}_0-\mathbf{P}_1 P0P1 上的位置。假如 t t t 取值为 1 / 4 1 / 4 1/4 2 / 4 2 / 4 2/4 3 / 4 3 / 4 3/4 即将 P 0 P 1 \mathbf{P}_0 \mathbf{P}_1 P0P1 弦长均分为 4 4 4 等份, 可以看出其对应的弧长并不相等。靠近中间位置的弧长较长,而靠近两段处的弧长较短,这就意味着当 t t t 匀速变化时,代表姿态矢量的角速度变化并不均匀:
在这里插入图片描述
球面线性插值(Spherical linear interpolation, 通常简称Slerp)是四元数的一种线性插值运算,主要用于在两个表示旋转的四元数之间平滑差值

通常Slerp会用在SLAM轨迹插值中,设 p \mathbf{p} p 对应的时刻为 t 1 t_1 t1 q \mathbf{q} q 对应的时刻为 t 2 t_2 t2 t ′ t^{\prime} t 时刻对应的四元数为 r \mathbf{r} r,即为所求,于是比例系数 t t t 满足:
t = t ′ − t 1 t 2 − t 1 t=\frac{t^{\prime}-t_1}{t_2-t_1} t=t2t1tt1注意单位四元数 p \mathbf{p} p q \mathbf{q} q 之间的夹角为 θ \theta θ p \mathbf{p} p r \mathbf{r} r 之间的夹角为 t θ t \theta q \mathbf{q} q r \mathbf{r} r 之间的夹角为 ( 1 − t ) θ (1-t) \theta (1t)θ。按一般的插值公式写出 r \mathbf{r} r 的表达式如下,即要找到合适的 a ( t ) a(t) a(t) b ( t ) b(t) b(t)
r = a ( t ) p + b ( t ) q \mathbf{r}=a(t) \mathbf{p}+b(t) \mathbf{q} r=a(t)p+b(t)q同时有:
cos ⁡ θ = p ⋅ q , cos ⁡ ( t θ ) = p ⋅ r , cos ⁡ [ ( 1 − t ) θ ] = q ⋅ r \cos \theta=\mathbf{p} \cdot \mathbf{q}, \cos (t \theta)=\mathbf{p} \cdot \mathbf{r}, \cos [(1-t) \theta]=\mathbf{q} \cdot \mathbf{r} cosθ=pq,cos()=pr,cos[(1t)θ]=qr p \mathbf{p} p 同时点乘 r = a ( t ) p + b ( t ) q \mathbf{r}=a(t) \mathbf{p}+b(t) \mathbf{q} r=a(t)p+b(t)q 两边可以得到:
p ⋅ r = p ⋅ [ a ( t ) p + b ( t ) q ] cos ⁡ ( t θ ) = a ( t ) + b ( t ) cos ⁡ ( θ ) \begin{array}{l} \mathbf{p} \cdot \mathbf{r}=\mathbf{p} \cdot[a(t) \mathbf{p}+b(t) \mathbf{q}] \\ \cos (t \theta)=a(t)+b(t) \cos (\theta) \end{array} pr=p[a(t)p+b(t)q]cos()=a(t)+b(t)cos(θ) q \mathbf{q} q 同时点乘 r = a ( t ) p + b ( t ) q \mathbf{r}=a(t) \mathbf{p}+b(t) \mathbf{q} r=a(t)p+b(t)q 两边可以得到:
q ⋅ r = q ⋅ [ a ( t ) p + b ( t ) q ] cos ⁡ [ ( 1 − t ) θ ] = a ( t ) cos ⁡ ( θ ) + b ( t ) \begin{array}{c} \mathbf{q} \cdot \mathbf{r}=\mathbf{q} \cdot[a(t) \mathbf{p}+b(t) \mathbf{q}] \\ \cos [(1-t) \theta]=a(t) \cos (\theta)+b(t) \end{array} qr=q[a(t)p+b(t)q]cos[(1t)θ]=a(t)cos(θ)+b(t)联立上面两个公式可得:
a ( t ) = sin ⁡ [ ( 1 − t ) θ ] sin ⁡ θ b ( t ) = sin ⁡ ( t θ ) sin ⁡ θ \begin{array}{c} a(t)=\frac{\sin [(1-t) \theta]}{\sin \theta} \\ b(t)=\frac{\sin (t \theta)}{\sin \theta} \end{array} a(t)=sinθsin[(1t)θ]b(t)=sinθsin()于是有:
Slerp ⁡ ( p , q , t ) = sin ⁡ [ ( 1 − t ) θ ] ⋅ p + sin ⁡ ( t θ ) ⋅ q sin ⁡ θ \operatorname{Slerp}(\mathbf{p}, \mathbf{q}, t)=\frac{\sin [(1-t) \theta] \cdot \mathbf{p}+\sin (t \theta) \cdot \mathbf{q}}{\sin \theta} Slerp(p,q,t)=sinθsin[(1t)θ]p+sin()q

也就是MATLAB上写的:

  • Quaternion spherical linear interpolation (SLERP) is an extension of linear interpolation along a plane to spherical interpolation in three dimensions. Given two quaternions, q 1 q_1 q1 and q 2 q_2 q2, SLERP interpolates a new quaternion, q 0 q_0 q0, along the great circle that connects q 1 q_1 q1 and q 2 q_2 q2. The interpolation coefficient, T T T, determines how close the output quaternion is to either q 1 q_1 q1 and q 2 q_2 q2.

    The SLERP algorithm can be described in terms of sinusoids:
    q 0 = s i n ( ( 1 − T ) θ ) s i n ( θ ) q 1 + s i n ( T θ ) s i n ( θ ) q 2 q_0=\frac{sin((1−T)θ)}{sin(θ)}q1+\frac{sin(Tθ)}{sin(θ)}q2 q0=sin(θ)sin((1T)θ)q1+sin(θ)sin()q2where q 1 q_1 q1 and q 2 q_2 q2 are normalized quaternions, and θ θ θ is half the angular distance between q 1 q_1 q1 and q 2 q_2 q2.

2. 问题

注意上面公式存在以下问题,需要在实现过程中考虑:

  • 如果四元数点积的结果是负值(夹角大于90°),那么后面的插值就会在4D球面上绕远路。为了解决这个问题,先测试点积的结果,当结果是负值时,将两个四元数的其中一个取反(并不会改变它代表的朝向)。而经过这一步操作,可以保证这个旋转走的是最短路径。
  • p \mathbf{p} p q \mathbf{q} q 的夹角 θ \theta θ 差非常小时会导致 sin ⁡ θ → 0 \sin \theta \rightarrow 0 sinθ0,这时除法可能会出现问题。为了避免这样的问题,当 θ \theta θ 非常小时可以使用简单的线性插值代替 ( θ → 0 (\theta \rightarrow 0 (θ0 时, sin ⁡ θ ≈ θ ) \sin \theta \approx \theta) sinθθ),因此方程退化为线性方程: slerp ⁡ ( p , q , t ) = ( 1 − t ) p + t q \operatorname{slerp}(\mathbf{p}, \mathbf{q}, t)=(1-t) \mathbf{p}+t \mathbf{q} slerp(p,q,t)=(1t)p+tq
  • 7
    点赞
  • 14
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值