【机器人】四元数与旋转矩阵的转换关系

导语:最近在搭建电机与机械臂的联合仿真,闲暇之余顺便看了一下姿态轨迹规划的文章,对姿态的插补有点感兴趣,发现用四元数表示姿态然后来进行姿态的插补非常方便,且不会出现奇异。这里对四元数与常规的旋转矩阵的转换做一下总结。

四元数概念

  四元数由实部和虚部组成,设 q q q 为四元数,则可表述为 q = s + x i + y j + z k q=s+xi+yj+zk q=s+xi+yj+zk ,其中 s s s 为实部, i j k ijk ijk 为虚单位,满足 i 2 = j 2 = k 2 = i j k = − 1 i^2=j^2=k^2=ijk=-1 i2=j2=k2=ijk=1 。四元数的模计算公式 ∣ q ∣ = w 2 + x 2 + y 2 + z 2 \left| q \right|=\sqrt{w^2+x^2+y^2+z^2} q=w2+x2+y2+z2 模等于1时,则为单位四元数,四元数也可写成向量形式 q = [ s , u ⃗ ] q=\left[ s,\vec{u} \right] q=[s,u ] ,其中 s s s 为实部, u ⃗ = [ x , y , z ] \vec{u}=\left[ x,y,z \right] u =[x,y,z] 为三个实数构成的虚部向量。单位四元数又可表示为
q = [ cos ⁡ ( θ / 2 ) , sin ⁡ ( θ / 2 ) ∗ v ] q=\left[ \cos \left( \theta /2 \right) ,\sin \left( \theta /2 \right) *v \right] q=[cos(θ/2),sin(θ/2)v]它表示任意向量 r r r 绕轴 v v v 旋转角度 θ \theta θ 得到的四元数。

四元数的运算

共轭:设 q = [ s , u ⃗ ] q=\left[ s,\vec{u} \right] q=[s,u ] 为四元数,则其共轭四元数为 q ∗ = [ s , − u ⃗ ] q^*=\left[ s,-\vec{u} \right] q=[s,u ]
:单位四元数的逆 q − 1 = q ∗ q^{-1}=q^* q1=q
乘法:对应四元数 q = [ s 1 , u 1 ⃗ ] q=\left[ s_1,\vec{u_1} \right] q=[s1,u1 ] q = [ s 2 , u 2 ⃗ ] q=\left[ s_2,\vec{u_2} \right] q=[s2,u2 ],则有 q 1 q 2 = [ s 1 s 2 − u ⃗ 1 ∗ u ⃗ 2 , u ⃗ 1 × u ⃗ 2 + s 1 u ⃗ 2 + s 2 u ⃗ 1 ] q_1q_2=\left[ s_1s_2-\vec{u}_1*\vec{u}_2,\vec{u}_1\times \vec{u}_2+s_1\vec{u}_2+s_2\vec{u}_1 \right] q1q2=[s1s2u 1u 2,u 1×u 2+s1u 2+s2u 1] ,其中*和×分别表示三维向量的内积和外积。并且四元数的乘法是不可交换的。
数乘:对于 q q q 为四元数, r r r 为实数,则数乘定义为 r q = [ r , 0 ] q = [ r s , r v ⃗ ] rq=\left[ r,0 \right] q=\left[ rs,r\vec{v} \right] rq=[r,0]q=[rs,rv ]

四元数转换为旋转矩阵

  一个绕轴 v v v 旋转角度 θ θ θ 的操作可以用单位四元数 q = [ q 1 , q 2 , q 3 , q 4 ] = [ s , x , y , z ] = [ cos ⁡ ( θ / 2 ) , sin ⁡ ( θ / 2 ) ∗ ν ] q=\left[ q_1, q_2, q_3, q_4 \right] =\left[ s,x,y,z \right] =\left[ \cos \left( \theta /2 \right) ,\sin \left( \theta /2 \right) *\nu \right] q=[q1,q2,q3,q4]=[s,x,y,z]=[cos(θ/2),sin(θ/2)ν] 进行表示,其中 θ ∈ [ 0 , π ] \theta \in \left[ 0,\pi \right] θ[0,π]。对应旋转矩阵为 R ( q ) = [ 2 ( q 1 2 + q 2 2 ) − 1 2 ( q 2 q 3 − q 1 q 4 ) 2 ( q 2 q 4 + q 1 q 3 ) 2 ( q 2 q 3 + q 1 q 4 ) 2 ( q 1 2 + q 3 2 ) − 1 2 ( q 3 q 4 − q 1 q 2 ) 2 ( q 2 q 4 − q 1 q 3 ) 2 ( q 3 q 4 + q 1 q 2 ) 2 ( q 1 2 + q 4 2 ) − 1 ] R\left( q \right) =\left[ \begin{matrix} 2\left( q_{1}^{2}+q_{2}^{2} \right) -1& 2\left( q_2q_3-q_1q_4 \right)& 2\left( q_2q_4+q_1q_3 \right)\\ 2\left( q_2q_3+q_1q_4 \right)& 2\left( q_{1}^{2}+q_{3}^{2} \right) -1& 2\left( q_3q_4-q_1q_2 \right)\\ 2\left( q_2q_4-q_1q_3 \right)& 2\left( q_3q_4+q_1q_2 \right)& 2\left( q_{1}^{2}+q_{4}^{2} \right) -1\\ \end{matrix} \right] R(q)= 2(q12+q22)12(q2q3+q1q4)2(q2q4q1q3)2(q2q3q1q4)2(q12+q32)12(q3q4+q1q2)2(q2q4+q1q3)2(q3q4q1q2)2(q12+q42)1

旋转矩阵转换为四元数

有旋转矩阵 i R j = [ s n α ] = [ s x n x α x s y n y α y s z n z α z ] ^iR_j=\left[ \begin{matrix} s& n& \alpha\\ \end{matrix} \right] =\left[ \begin{matrix} s_x& n_x& \alpha _x\\ s_y& n_y& \alpha _y\\ s_z& n_z& \alpha _z\\ \end{matrix} \right] iRj=[snα]= sxsysznxnynzαxαyαz , 转换为单位四元数 q = [ q 1 , q 2 , q 3 , q 4 ] q=\left[ q_1, q_2, q_3, q_4 \right] q=[q1,q2,q3,q4]

q 1 = 1 2 s x + η y + α z + 1 q_1=\frac{1}{2}\sqrt{s_x+\eta _y+\alpha _z+1} q1=21sx+ηy+αz+1

q 2 = 1 2 s i g n ( n z − α y ) s x − η y − α z + 1 q_2=\frac{1}{2}sign\left( n_z-\alpha_y \right) \sqrt{s_x-\eta _y-\alpha _z+1} q2=21sign(nzαy)sxηyαz+1

q 3 = 1 2 s i g n ( α x − s z ) − s x + η y − α z + 1 q_3=\frac{1}{2}sign\left( \alpha_x-s_z \right) \sqrt{-s_x+\eta _y-\alpha _z+1} q3=21sign(αxsz)sx+ηyαz+1

q 4 = 1 2 s i g n ( s y − n x ) − s x − η y + α z + 1 q_4=\frac{1}{2}sign\left( s_y-n_x \right) \sqrt{-s_x-\eta _y+\alpha _z+1} q4=21sign(synx)sxηy+αz+1

  • 5
    点赞
  • 26
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
### 回答1: 旋转矩阵四元数之间的转换公式如下: 旋转矩阵四元数: 给定一个旋转矩阵R,其对应的四元数q为: q = (w, x, y, z) = (cos(θ/2), sin(θ/2) * u_x, sin(θ/2) * u_y, sin(θ/2) * u_z) 其中,θ为旋转角度,(u_x, u_y, u_z)为旋转轴的单位向量。 四元数旋转矩阵: 给定一个四元数q,其对应的旋转矩阵R为: R = [1 - 2*(y^2 + z^2) 2*(x*y - w*z) 2*(x*z + w*y)] [2*(x*y + w*z) 1 - 2*(x^2 + z^2) 2*(y*z - w*x)] [2*(x*z - w*y) 2*(y*z + w*x) 1 - 2*(x^2 + y^2)] 其中,w, x, y, z分别为四元数的实部和虚部。 ### 回答2: 旋转矩阵四元数是表示旋转的两种数学工具。它们之间的转换可以通过以下数学公式实现: 1. 旋转矩阵(Rotation Matrix)到四元数(Quaternion)的转换公式: 给定一个旋转矩阵R,可以通过如下公式将其转换四元数表示: q = (q0, q1, q2, q3) = (0.5 * sqrt(max(0, 1 + R[0][0] + R[1][1] + R[2][2])), 0.5 * sqrt(max(0, 1 + R[0][0] - R[1][1] - R[2][2])), 0.5 * sqrt(max(0, 1 - R[0][0] + R[1][1] - R[2][2])), 0.5 * sqrt(max(0, 1 - R[0][0] - R[1][1] + R[2][2]))) 其中,sqrt()是平方根函数,max()是取最大值函数。R[0][0]、R[1][1]、R[2][2]分别代表旋转矩阵R的第1、2、3行第1、2、3列的元素。 2. 四元数旋转矩阵转换公式: 给定一个四元数q,可以通过如下公式将其转换旋转矩阵表示: R = [[1 - 2*q2^2 - 2*q3^2, 2*q1*q2 - 2*q0*q3, 2*q1*q3 + 2*q0*q2], [2*q1*q2 + 2*q0*q3, 1 - 2*q1^2 - 2*q3^2, 2*q2*q3 - 2*q0*q1], [2*q1*q3 - 2*q0*q2, 2*q2*q3 + 2*q0*q1, 1 - 2*q1^2 - 2*q2^2]] 其中,^表示乘方运算。 这些公式可以方便地在旋转矩阵四元数之间进行转换,并在计算机图形学、机器人学等领域中得到广泛应用。 ### 回答3: 旋转矩阵四元数之间的数学公式是相互转化的关系。 首先,我们来看旋转矩阵四元数转换公式: 给定一个旋转矩阵R,其转换四元数q的公式为: q = (r + 1)/2, 其中,r是一个3x3的旋转矩阵,并且r的迹(即对角线上元素之和)应为3。 接下来,我们来看四元数旋转矩阵转换公式: 给定一个四元数q,其转换旋转矩阵R的公式为: R = [[1 - 2(y^2 + z^2), 2(xy - zw), 2(xz + yw)], [2(xy + zw), 1 - 2(x^2 + z^2), 2(yz - xw)], [2(xz - yw), 2(yz + xw), 1 - 2(x^2 + y^2)]], 其中,(x, y, z, w)是四元数q的四个分量。 总结起来,旋转矩阵四元数转换公式为q = (r + 1)/2,而四元数旋转矩阵转换公式为R = [[1 - 2(y^2 + z^2), 2(xy - zw), 2(xz + yw)], [2(xy + zw), 1 - 2(x^2 + z^2), 2(yz - xw)], [2(xz - yw), 2(yz + xw), 1 - 2(x^2 + y^2)]]。这些公式在计算机图形学、机器人运动控制等领域起着重要的作用。
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值