自动驾驶的旋转，方向和四元数- MATLAB和Simulink金宝app - 金宝app,下载188bet金宝搏,金宝搏官方网站

自动驾驶的旋转、方向和四元数

一个四元数是一个四部分的超复数，用来描述三维旋转和方向。四元数在许多领域都有应用，包括航空航天、计算机图形学和虚拟现实。在自动驾驶中，惯性测量单元(IMUs)等传感器将方向读数报告为四元数。若要将此数据用于本地化，可以使用四元数对象，对其进行数学运算，或将其转换为其他旋转格式，如欧拉角和旋转矩阵。

您可以使用四元数来执行3d点和帧旋转。

与点旋转，在静态参照系中旋转点。
与框架旋转，你旋转的参照系周围静点到帧转换成坐标系统相对于点。

可以通过使用旋转轴和旋转的绕该轴线的角度定义这些旋转。四元封装轴和旋转的角度，并且具有用于操作这些旋转的代数。该四元数对象使用“右手规则”约定来定义旋转。也就是说，当从原点观察时，正旋转是围绕旋转轴顺时针旋转的。

四元数格式

四元数以这种形式表示:

$一个 + b 我 + c j + d k$

一个,b,c,d是实数。这些系数被称为部分的四元数。

i、j和k是四元数的复元素。这些元素满足方程我²j =²= k²= ijk = - 1。

四元数的部分一个,b,c,d指定轴和旋转角度。旋转ɑ绕旋转轴线弧度由单位矢量表示[x,y,z，描述旋转的四元数为:

$因为 (\frac{α}{2}) + 罪 (\frac{α}{2}) (x 我 + y j + z k)$

四元数的创建

打开脚本

您可以通过多种方式创建四元数。例如，通过指定其部分来创建四元数。

q =四元数(1、2、3、4)

q =四元数1 + 2i + 3j + 4k

您可以用相同的方法创建四元数数组。例如，通过指定四个2×2矩阵来创建一个2×2四元数数组。

qArray =四元数([1 10;-1]， [2 20;-2]， [3 30;-3]， [4 40;4 4])

qArray = 2x2四元数数组1 + 2i + 3j + 4k 10 + 20i + 30j + 40k -1 - 2i - 3j - 4k 1 + 2i + 3j + 4k

您还可以使用四列数组来构造四元数，其中每一列表示一个四元数部分。例如，创建一个表示随机旋转的四元数数组。

qRandom = randrot (4,1)

qRandom = 4x1四元数阵列0.14515 + 0.086053i + 0.96601j + 0.19583k 0.51365 - 0.36627i + 0.77569j - 0.017661k -0.81811 - 0.15704i - 0.4889j + 0.25887k 0.27111 + 0.10774i + 0.95433j - 0.064451k

索引和操作四元数就像其他数组一样。例如，从qRandom四元数的数组。

qRandom（3）

ans =四元数-0.81811 - 0.15704i - 0.4889j + 0.25887k

重新定义四元数数组。

重塑（qRandom，2,2）

ans = 2x2四元数阵列0.14515 + 0.086053i + 0.96601j + 0.19583k -0.81811 - 0.15704i - 0.4889j + 0.25887k 0.51365 - 0.36627i + 0.77569j - 0.017661k 0.27111 + 0.10774i + 0.95433j - 0.064451k

将四元数数组与您创建的第一个四元数连接起来。

[qRandom;问)

ANS = 5X1阵列的四元数+ 0.14515 + 0.086053i + 0.96601j 0.19583k 0.51365  -  0.36627i + 0.77569j  -  0.017661k -0.81811  -  0.15704i  -  0.4889j + 0.25887k 0.27111 + 0.10774i + 0.95433j  -  0.064451k 1 + 2I +3J + 4K

四元数的数学

打开生活的脚本

四元数有定义良好的算术运算。要应用这些操作，首先通过指定它们的实数部分来定义两个四元数。

q1 =四元数(1、2、3、4)

q1 =四元数1 + 2i + 3j + 4k

q2 =四元数(5、6、7、8)

q2 =四元数-5 + 6i - 7j + 8k

四元数的添加类似于复数，其中各个部分是独立添加的。

q1 +

ans =四元数-4 + 8i - 4j + 12k

四元数的减法与四元数的加法工作原理相似。

Q1  -  Q2

ans =四元数6 - 4i + 10j - 4k

因为四元数的复数元素必须满足这个方程

$我^{2} = j^{2} = k^{2} = ijk = - 1$ ,

四元数的乘法比加减法复杂得多。鉴于这一要求，四元数的乘法是不可交换的。也就是说，乘以四元的时候，扭转了四元数的顺序改变了他们的产品的结果。

q1 *

ans =四元数-28 + 48i - 14j - 44k

q2 * q1

ans =四元数-28 - 56i - 30j + 20k

但是，每个四元数都有一个乘法逆，所以可以对四元数进行除法。正确的分工Q1通过Q2相当于 $问 1 (问 2^{- 1})$ 。

q1。/ q2

ans =四元数0.10345 - 0.3908i - 0.091954j + 0.022989k

左部Q1通过Q2相当于 $(问 2^{- 1}) 问 1$ 。

q1。\ q2

ans =四元数0.6 - 1.2i + 0j + 2k

四元数的共轭是通过对每一个复数部分求负而得到的，类似于复数的共轭。

连词(q1)

ans =四元数1 - 2i - 3j - 4k

要使用四元数描述旋转，四元数必须是a单位四元数。单位四元数的范数为1，其中范数定义为

$规范 (问) = \sqrt{{一个}^{2} + b^{2} + c^{2} + d^{2}}$ 。

一个四元数的正常化。

qNormalized =正常化(q1)

qNormalized =四元数0.18257 + 0.36515i + 0.54772j + 0.7303k

验证此标准化单元四元数的范数为1。

规范(qNormalized)

ans = 1.0000

一个归一化四元数的共轭的旋转矩阵等于那个归一化四元数的旋转矩阵的倒数。

rotmat(连词(qNormalized),“点”)

ans =3×3-0.6667 0.6667 0.3333 0.1333 -0.3333 0.9333 0.7333 0.6667 0.1333

发票(rotmat (qNormalized,“点”))

ans =3×3-0.6667 0.6667 0.3333 0.1333 -0.3333 0.9333 0.7333 0.6667 0.1333

从变换矩阵中提取四元数

这个示例使用:

打开生活的脚本

如果您有一个使用诸如此类的函数创建的3d转换矩阵rigid3d要么affine3d，你可以从中提取旋转矩阵并以四元数的形式表示出来。然而，在执行此转换之前，您必须首先将旋转矩阵从后乘格式转换为四元数所期望的预乘格式。

自右乘格式

若要使用变换矩阵的旋转矩阵部分进行旋转，将一个(x,y,z)点通过这个旋转矩阵。

在点旋转，该点的参考框架内旋转。
在坐标系旋转中，参照系绕这一点旋转。

表示的变换矩阵rigid3d要么affine3d对象的使用自右乘格式。在这种格式中，点乘以旋转矩阵，按这个顺序。为了满足矩阵乘法，点及其对应的平移向量必须是行向量。

这个方程显示了一个旋转矩阵的后乘格式，R,和一个平移向量，t。

$(\begin{array}{c} x' & y^{'} & z^{'} \end{array}] = (\begin{array}{c} x & y & z \end{array}] (\begin{array}{c} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \end{array}] + (\begin{array}{c} t_{x} & t_{y} & t_{z} \end{array}]$

此格式也适用于何时R和t被合并成一个齐次变换矩阵。在这个矩阵中，1是用来满足矩阵乘法的，可以忽略。

$(\begin{array}{c} x^{'} & y^{'} & z^{'} & 1 \end{array}] = (\begin{array}{c} x & y & z & 1 \end{array}] (\begin{array}{c} R_{11} & R_{12} & R_{13} & 0 \\ R_{21} & R_{22} & R_{23} & 0 \\ R_{31} & R_{32} & R_{33} & 0 \\ t_{x} & t_{y} & t_{z} & 1 \end{array}]$

自左乘格式

在自左乘格式中，旋转矩阵是由点相乘，以该顺序。为了满足矩阵乘法，点和它的对应的平移向量必须是列向量。

这个方程显示了预乘格式，其中R是旋转矩阵吗t为平移向量。

$(\begin{array}{c} x' \\ y^{'} \\ z^{'} \end{array}] = (\begin{array}{c} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \end{array}] (\begin{array}{c} x \\ y \\ z \end{array}] + (\begin{array}{c} t_{x} \\ t_{y} \\ t_{z} \end{array}]$

与post正片叠底的情况一样，这种格式也适用于whenR和t被合并成一个齐次变换矩阵。

$(\begin{array}{c} x^{'} \\ y^{'} \\ z^{'} \\ 1 \end{array}] = (\begin{array}{c} R_{11} & R_{12} & R_{13} & t_{x} \\ R_{21} & R_{22} & R_{23} & t_{y} \\ R_{31} & R_{32} & R_{33} & t_{z} \\ 0 & 0 & 0 & 1 \end{array}] (\begin{array}{c} x \\ y \\ z \\ 1 \end{array}]$

从后乘格式转换为前乘格式

若要将旋转矩阵转换成四元数所期望的预乘格式，请对旋转矩阵进行转置。

从旋转矩阵和平移向量创建一个三维刚性几何变换对象。旋转的角度， $θ$ 为度。

THETA = 30;腐= [COSD（THETA）信德（THETA）0;...信德(θ)cosd(θ)0;...0 0 1];反式= [1 2 3 4];TForm的= rigid3d（腐烂，反式）

tform = rigid3d with properties: Rotation: [3x3 double] Translation: [2 3 4]

旋转矩阵的元素使用后乘格式对旋转点进行排序。通过求其转置将矩阵转换成预乘格式。注意，第一行和第一列的第二个元素交换位置。

rotPost = tform.Rotation

rotPost =3×30.8660 0.5000 0 -0.5000 0.8660 000 1.0000

rotPre = rotPost“。

rotPre =3×30.8660 -0.5000 0 0.5000 0.8660 000 1.0000

从旋转矩阵的预乘版本创建一个四元数。指定为点旋转配置旋转矩阵。

Q =四元数（rotPre，“rotmat”,“点”)

q =四元数0.96593 + 0i + 0j + 0.25882k

为了验证前乘四元数和后乘旋转矩阵产生相同的结果，使用两种方法旋转一个采样点。

点= [1 2 3];rotatedPointQuaternion = rotatepoint (q点)

rotatedPointQuaternion =1×3-0.1340 2.2321 3.0000

rotatedPointRotationMatrix = point * rotPost

rotatedPointRotationMatrix =1×3-0.1340 2.2321 3.0000

若要转换回原始旋转矩阵，请从四元数中提取旋转矩阵。然后,创建一个rigid3d利用这个旋转矩阵的转置。

R = rotmat (q,“点”);T = rigid3d (R ',反式);T.Rotation

ans =3×30.8660 0.5000 0 -0.5000 0.8660 000 1.0000

另请参阅

affine3d|四元数|rigid3d|rotateframe|rotatepoint