當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

2020-10-13 四元数用法（不讲原理，只讲计算规则）

發布時間：2025/3/15 编程问答 29 豆豆

生活随笔收集整理的這篇文章主要介紹了 2020-10-13 四元数用法（不讲原理，只讲计算规则）小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

本文不講四元數的定義，直接講操作實例和代碼。

對象：剛體姿態

坐標系定義：

本體坐標系

B

，

參考坐標系

R

，

慣性坐標系

I

涉及四元數定義：
$q=[q0qv]\boldsymbol q = \left[\begin{array}{l} q_0\\ \boldsymbol q_v \end{array}\right]$ 表示 $\rightarrow B$ 的四元數（也就是 $B$ 相對于 $R$ ），
$q′=[q0′qv′]\boldsymbol q'= \left[\begin{array}{l} q'_0\\ \boldsymbol q'_v \end{array}\right]$ 表示 $\rightarrow R$ 的四元數（也就是 $R$ 相對于 $I$ ），
$q′′=[q0′′qv′′]\boldsymbol q''= \left[\begin{array}{l} q''_0\\ \boldsymbol q''_v \end{array}\right]$ 表示 $\rightarrow B$ 的四元數（也就是 $B$ 相對于 $I$ ）。

定義：
$q\boldsymbol q$ 的逆表示為 $qˉ=[q0?qv]\bar \boldsymbol q = \left[\begin{array}{r} q_0\\ -\boldsymbol q_v \end{array}\right]$

四元數乘法（從左向右）：

【 $\rightarrow B$ 的四元數】 = 【 $\rightarrow R$ 的四元數】 $?\otimes$ 【 $\rightarrow B$ 的四元數】
$q′′=q′?q=[q0′?q0?qv′?qvq0′?qv+q0?qv′+qv′×qv]\boldsymbol q''=\boldsymbol q' \otimes \boldsymbol q = \left[\begin{array}{l} q'_0*q_0- \boldsymbol q'_v \cdot \boldsymbol q_v\\ q'_0*\boldsymbol q_v+q_0*\boldsymbol q'_v+\boldsymbol q'_v \times \boldsymbol q_v \end{array}\right]$

【 $\rightarrow B$ 的四元數】 = 【 $\rightarrow I$ 的四元數】 $?\otimes$ 【 $\rightarrow B$ 的四元數】
$q=qˉ′?q′′=[q0′?q0′′+qv′?qv′′q0′?qv′′?q0′′?qv′?qv′×qv′′]\boldsymbol q=\bar \boldsymbol q' \otimes \boldsymbol q'' = \left[\begin{array}{l} q'_0*q''_0+ \boldsymbol q'_v \cdot \boldsymbol q''_v\\ q'_0*\boldsymbol q''_v - q''_0*\boldsymbol q'_v-\boldsymbol q'_v \times \boldsymbol q''_v \end{array}\right]$

相應四元數旋轉矩陣（從右向左）：

【 $\rightarrow B$ 的旋轉矩陣】 = 【 $\rightarrow B$ 的旋轉矩陣】 $×\times$ 【 $\rightarrow R$ 的旋轉矩陣】
$A(q′′)=A(q)A(q′)\boldsymbol{A}(\boldsymbol q'')=\boldsymbol{A}(\boldsymbol q)\boldsymbol{A}(\boldsymbol q')$

其中
$A(q)=(q02?qvTqv)I+2qvqvT?2q0[qv×]=[2(q02+q12)?12(q1q2+q0q3)2(q1q3?q0q2)2(q1q2?q0q3)2(q02+q22)?12(q2q3+q0q1)2(q1q3+q0q2)2(q2q3?q0q1)2(q02+q32)?1]\begin{array}{l} \boldsymbol{A}(\boldsymbol{q})=\left(q_{0}^{2}-\boldsymbol{q}_{v}^{\mathrm{T}}\boldsymbol{q}_{v} \right) \boldsymbol{I}+2 \boldsymbol{q}_{v} \boldsymbol{q}_{v}^{\mathrm{T}}-2 q_{0}\left[\boldsymbol{q}_{v} \times\right] \\ ={\left[\begin{array}{lll} 2\left(q_{0}^{2}+q_{1}^{2}\right)-1 & 2\left(q_{1} q_{2}+q_{0} q_{3}\right) & 2\left(q_{1} q_{3}-q_{0} q_{2}\right) \\ 2\left(q_{1} q_{2}-q_{0} q_{3}\right) & 2\left(q_{0}^{2}+q_{2}^{2}\right)-1 & 2\left(q_{2} q_{3}+q_{0} q_{1}\right) \\ 2\left(q_{1} q_{3}+q_{0} q_{2}\right) & 2\left(q_{2} q_{3}-q_{0} q_{1}\right) & 2\left(q_{0}^{2}+q_{3}^{2}\right)-1 \end{array}\right]} \end{array}$
或者
$A(q)=I?2q0[qv×]+2[qv×][qv×]\boldsymbol{A}(\boldsymbol{q})=\boldsymbol{I}-2q_{0}\left[\boldsymbol{q}_{v} \times\right] +2\left[\boldsymbol{q}_{v} \times\right] \left[\boldsymbol{q}_{v} \times\right]$
和
$[qv×]=[0?q3q2q30?q1?q2q10]\begin{array}{r} {\left[\boldsymbol{q}_{v} \times\right]=\left[\begin{array}{ccc} 0 & -q_{3} & q_{2} \\ q_{3} & 0 & -q_{1} \\ -q_{2} & q_{1} & 0 \end{array}\right]} \end{array}$

Matlab代碼

function L = TransMatrix(quat) quat_0=quat(1);quat_1=quat(2);quat_2=quat(3);quat_3=quat(4); L = [2*(quat_0^2+quat_1^2)-1 2*(quat_1*quat_2+quat_0*quat_3) 2*(quat_1*quat_3-quat_0*quat_2); ...2*(quat_1*quat_2-quat_0*quat_3) 2*(quat_0^2+quat_2^2)-1 2*(quat_2*quat_3+quat_0*quat_1); ...2*(quat_1*quat_3+quat_0*quat_2) 2*(quat_2*quat_3-quat_0*quat_1) 2*(quat_0^2+quat_3^2)-1]; end

四元數的一些性質：

$qvqvT?qvTqvI=[qv×][qv×]\boldsymbol{q}_{v} \boldsymbol{q}_{v}^{\mathrm{T}}- \boldsymbol{q}_{v}^{\mathrm{T}}\boldsymbol{q}_{v}\boldsymbol{I}= \left[\boldsymbol{q}_{v} \times\right]\left[\boldsymbol{q}_{v} \times\right]$

旋轉矩陣 $A(q)\boldsymbol{A}(\boldsymbol{q})$ 的特征值是： $q_0^2-1-2q_0\left(q_0^2-1\right)^{\frac{1}{2}}$ ， $q_0^2-1+2q_0\left(q_0^2-1\right)^{\frac{1}{2}}$ ，和 $1$ .

對于 $q=[q0qv]\boldsymbol q = \left[\begin{array}{l} q_0\\ \boldsymbol q_v \end{array}\right]$ ，當 $q_0=0$ 時，則有 $qvTqv=1\boldsymbol q^T_v\boldsymbol q_v=1$ ，也可以推出 $qvqvT?qv×qv×=I\boldsymbol q_v\boldsymbol q^T_v-\boldsymbol q^{\times}_v\boldsymbol q^{\times}_v=\boldsymbol{I}$

$[q0qv]\left[\begin{array}{r} q_0\\ \boldsymbol q_v \end{array}\right]$ 和 $[?q0?qv]\left[\begin{array}{r} -q_0\\ -\boldsymbol q_v \end{array}\right]$ 表示同樣的姿態， $[?q0qv]\left[\begin{array}{r} -q_0\\ \boldsymbol q_v \end{array}\right]$ 和 $[q0?qv]\left[\begin{array}{r} q_0\\ -\boldsymbol q_v \end{array}\right]$ 表示同樣的姿態。

四元數的代數性質： $qaT(qb?qc)=qcT(qb??qa)=qbT(qa?qc?)\boldsymbol q^T_a(\boldsymbol q_b \otimes \boldsymbol q_c)=\boldsymbol q^T_c(\boldsymbol q^*_b \otimes \boldsymbol q_a)=\boldsymbol q^T_b(\boldsymbol q_a \otimes \boldsymbol q^*_c)$

$Vec(b??a)=?Vec(a??b)\text{Vec}(\mathbf{b}^{*} \otimes \mathbf{a})=-\text{Vec}(\mathbf{a}^{*} \otimes \mathbf{b})$

$for?any?x∈R3,A∈R3×3,?and?R∈SO(3)?\text { for any } \boldsymbol{x} \in \mathbf{R}^{3}, \boldsymbol{A} \in \mathbf{R}^{3 \times 3} \text {, and } \boldsymbol{R} \in \mathrm{SO} \text { (3) }$
$tr?(A(x)∧)=12tr?[(x)∧(A?AT)]=?xT(A?AT)∨(x)∧A+AT(x)∧=[(tr?(A)I3?A)x]∧R(x)∧RT=(Rx)∧\begin{aligned} &\operatorname{tr}\left(\boldsymbol{A}(\boldsymbol{x})^{\wedge}\right)=\frac{1}{2} \operatorname{tr}\left[(\boldsymbol{x})^{\wedge}\left(\boldsymbol{A}-\boldsymbol{A}^{\mathrm{T}}\right)\right]=-\boldsymbol{x}^{\mathrm{T}}\left(\boldsymbol{A}-\boldsymbol{A}^{\mathrm{T}}\right)^{\vee} \\ &(\boldsymbol{x})^{\wedge} \boldsymbol{A}+\boldsymbol{A}^{\mathrm{T}}(\boldsymbol{x})^{\wedge}=\left[\left(\operatorname{tr}(\boldsymbol{A}) \boldsymbol{I}_{3}-\boldsymbol{A}\right) \boldsymbol{x}\right]^{\wedge} \\ &\boldsymbol{R}(\boldsymbol{x})^{\wedge} \boldsymbol{R}^{\mathrm{T}}=(\boldsymbol{R} \boldsymbol{x})^{\wedge} \end{aligned}$

$∧^{\wedge}$ 也就是上面的叉乘算子， $∨^{\vee}$ 是其逆算子

與50位技術專家面對面20年技術見證，附贈技術全景圖

總結

以上是生活随笔為你收集整理的2020-10-13 四元数用法（不讲原理，只讲计算规则）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。

上一篇： 2020-10-13 多智能体基本图论
下一篇： 2020-10-19 Keil安装及使用