基于MATLAB&SIMULINK开发自动驾驶系统第八讲用于自动驾驶的旋转、方向和四元数
四元数是一个四部分的超复数，用于描述三维旋转和方向。四元数在许多领域都有应用，包括航空航天、计算机图形学和虚拟现实。在自动驾驶中，惯性测量单元（IMU）等传感器以四元数的形式报告方向读数。要将这些数据用于定位，可以使用四元组对象捕获它，对其进行数学运算，或将其转换为其他旋转格式，如欧拉角和旋转矩阵。
可以使用四元数来执行三维点和框架旋转：
- 使用点旋转，可以在一个静态参考框架中旋转点。
- 使用框架旋转，可以围绕静态点旋转参考框架，将框架转换为相对于点的坐标系。
可以通过使用旋转轴和关于该轴的旋转角度来定义这些旋转。四元数封装了旋转轴和旋转角，并有一个代数来操作这些旋转。四元数对象使用 "右手规则 "约定来定义旋转。也就是说，从原点看，正旋转是绕旋转轴顺时针旋转。
8.1 四元数格式
四元数用这种形式表示：
a、b、c、d是实数。这些系数被称为四元数的部分。
i、j和k是四元数的复数元素。这些元素满足以下公式
四元数部分a、b、c、d指定了旋转的轴和角度。对于以单位向量[x，y，z]为代表的围绕旋转轴旋转的ɑ弧度，描述旋转的四元数由这个公式给出。
8.2 四元数创建
可以通过多种方式创建四边形。例如，通过指定其部分来创建一个四元数。
q = quaternion(1,2,3,4)
q =
quaternion
1 + 2i + 3j + 4k
可以用同样的方法创建四元数组。例如，通过指定四个 2 乘 2 矩阵来创建一个 2 乘 2 的四元数组。
qArray = quaternion([1 10; -1 1], [220; -2 2], [3 30; -3 3], [4 40; -4 4])
qArray =
2x2 quaternion array
1+ 2i+ 3j+ 4k 10 + 20i + 30j +40k
-1- 2i- 3j- 4k 1+ 2i+ 3j+ 4k
也可以使用四列数组来构建四元数组，其中每列代表一个四元数部分。例如，创建一个代表随机旋转的四元数组。
qRandom =randrot(4,1)
qRandom=
4x1quaternion array
0.17446 + 0.59506i - 0.73295j + 0.27976k
0.21908 - 0.89875i - 0.298j + 0.23548k
0.6375 + 0.49338i - 0.24049j + 0.54068k
0.69704 - 0.060589i + 0.68679j - 0.19695k
像其他数组一样对四元数组进行索引和操作。例如，从qRandom四元数组中索引一个四元数。
qRandom(3)
ans = quaternion
0.6375 + 0.49338i - 0.24049j + 0.54068k
重塑四元数组：
reshape(qRandom,2,2)
ans =
2x2 quaternion array
0.17446 + 0.59506i - 0.73295j + 0.27976k 0.6375 + 0.49338i - 0.24049j + 0.54068k
0.21908 - 0.89875i - 0.298j + 0.23548k 0.69704 - 0.060589i + 0.68679j - 0.19695k
将四元数组与你创建的第一个四元数组连在一起。
[qRandom; q]
ans=
5x1 quaternion array
0.17446 + 0.59506i - 0.73295j + 0.27976k
0.21908 - 0.89875i - 0.298j + 0.23548k
0.6375 + 0.49338i - 0.24049j + 0.54068k
0.69704 - 0.060589i + 0.68679j - 0.19695k
1 + 2i + 3j + 4k
8.3 四元数
四元数有定义明确的算术运算，要应用这些运算，首先要定义两个四元数，指定它们的实数部分。要应用这些运算，首先通过指定实数部分来定义两个四元数。
q1 =quaternion(1,2,3,4)
q1 = quaternion
1 + 2i + 3j + 4k
q2 =quaternion(-5,6,-7,8)
q2 = quaternion
-5 + 6i - 7j + 8k
四元数的加法与复数类似，各部分独立相加。
q1 + q2
ans= quaternion
-4+ 8i- 4j +12k
四元数的减法与四元数的加法相似。
q1 - q2
ans= quaternion
6- 4i +10j- 4k
因为二次元的复数元素必须满足以下公式
四元数的乘法比加减法更复杂。鉴于这一要求，四元数的乘法不具有换算性。也就是说，在乘四元数时，把四元数的顺序颠倒过来，就会改变其乘积的结果。
q1 * q2
ans= quaternion
-28+ 48i - 14j - 44k
q2 * q1
ans= quaternion
-28- 56i - 30j + 20k
但是，每个四元数都有一个乘法反，所以可以对四元数进行除法。q1的右除数除以q2等于
。
q1 ./ q2
ans= quaternion
0.10345- 0.3908i - 0.091954j +0.022989k
q1被q2左除，相当于
。
q1 .\ q2
ans= quaternion
0.6 -1.2i+ 0j+ 2k
四元数的共轭是由各复数部分的负数构成的，类似于复数的共轭。
conj(q1)
ans= quaternion
1- 2i - 3j - 4k
要用一个四元数来描述旋转，四元数必须是一个单位四元数。单位四元数的法线为1，其中法线定义为
规范化为四元数
qNormalized=normalize(q1)
qNormalized= quaternion
0.18257 + 0.36515i +0.54772j+ 0.7303k
验证这个归一化单位四元组的法线为1。
norm(qNormalized)
ans = 1.0000
归一化二次元的共轭的旋转矩阵等于该归一化二次元的旋转矩阵的倒数。
rotmat(conj(qNormalized),'point')
ans =3×3
-0.6667 0.6667 0.3333
0.1333 -0.3333 0.9333
0.7333 0.6667 0.1333
inv(rotmat(qNormalized,'point'))
ans =3×3
-0.6667 0.6667 0.3333
0.1333 -0.3333 0.9333
0.7333 0.6667 0.1333
8.4 从变换矩阵中提取四元数
如果有一个使用rigid3d（图像处理工具箱）或affine3d（图像处理工具箱）等函数创建的三维变换矩阵，可以从中提取旋转矩阵，并以四元数的形式表示。然而，在进行这种转换之前， 必须首先将旋转矩阵从后乘格式转换为四元数所期望的前乘格式。
8.4.1 后置乘法格式
要使用变换矩阵的旋转矩阵部分进行旋转，请将一个(x，y，z)点乘以这个旋转矩阵。
- 在点旋转中，这个点在一个参考框架内旋转。
- 在框架旋转中，参考框架围绕这个点旋转。
由rigid3d或affine3d对象表示的变换矩阵使用后乘格式。在这种格式中，点依次与旋转矩阵相乘。为了满足矩阵乘法，点及其对应的变换向量必须是行向量。
这个公式显示了旋转矩阵R和转换向量t的后乘格式。
这种格式也适用于R和t组合成同质变换矩阵时。在这个矩阵中，1是用来满足矩阵乘法的，可以忽略。
8.4.2预倍增格式
在预乘格式中，旋转矩阵与点相乘，依次进行。为了满足矩阵乘法，点及其对应的翻译向量必须是列向量。
这个公式显示的是预乘格式，其中R是旋转矩阵，t是翻译向量。
与后乘情况一样，这种格式也适用于R和t组合成同质变换矩阵时。
8.4.3 将后置乘法转换为前置乘法格式
要将旋转矩阵转换为四元数所期望的预乘格式，请取旋转矩阵的转置。
从旋转矩阵和平移向量创建一个三维刚性几何变换对象。旋转角度θ的单位是度。
theta = 30;
rot = [ cosd(theta) sind(theta) 0; ...
-sind(theta) cosd(theta) 0; ...
0 0 1];
trans = [2 3 4];
tform = rigid3d(rot,trans)
tform =
rigid3d with properties:
Rotation: [3x3 double]
Translation: [2 3 4]
旋转矩阵的元素采用后乘格式对旋转点进行排序。通过取其转置将矩阵转换为预乘格式。注意，第一行和第一列的第二个元素互换位置。
rotPost = tform.Rotation
rotPost = 3×3
0.8660 0.5000 0
-0.5000 0.8660 0
0 0 1.0000
rotPre = rotPost.'
rotPre = 3×3
0.8660 -0.5000 0
0.5000 0.8660 0
0 0 1.0000
从旋转矩阵的预乘版本创建一个四元数。指定旋转矩阵的配置为点旋转。
q=quaternion(rotPre,'rotmat','point')
q= quaternion
0.96593+ 0i+ 0j+0.25882k
为了验证预乘四元数和后乘旋转矩阵产生的结果是否相同，用两种方法旋转一个样本点。
point = [1 2 3];
rotatedPointQuaternion =rotatepoint(q,point)
rotatedPointQuaternion = 1×3
-0.1340 2.2321 3.0000
rotatedPointRotationMatrix = point *rotPost
rotatedPointRotationMatrix = 1×3
-0.1340 2.2321 3.0000
要转换回原始的旋转矩阵，请从四元数中提取一个旋转矩阵。然后，使用该旋转矩阵的转置创建一个刚性三维对象。
R =rotmat(q,'point');
T =rigid3d(R',trans);
T.Rotation
ans =3×3
0.8660 0.5000 0
-0.5000 0.8660 0
0 0 1.0000