Learning resources for DQ Robotics in MATLAB
Lesson 1
代码
% Step 2: Define the real numbers a1 and a2
a1 = 123;
a2 = 321;
% Step 3: Calculate and display a3 = a1 + a2
a3 = a1 + a2;
disp(['a3 (a1 + a2) = ', num2str(a3)])
% Step 4: Calculate and display a3 = a1 * a2
a3 = a1 * a2;
disp(['a3 (a1 * a2) = ', num2str(a3)])
% Step 5: Calculate and display a3 = a1 / a2
a3 = a1 / a2;
disp(['a3 (a1 / a2) = ', num2str(a3)])
% Step 6: Define the real numbers theta1 and theta2
theta1 = pi / 12;
theta2 = 3 * pi;
% Step 7: Calculate and display a3 = cos(theta1 + theta2)
a3 = cos(theta1 + theta2);
disp(['a3 (cos(theta1 + theta2)) = ', num2str(a3)])
% Step 8: Calculate and display a3 = cos^2(theta1 + theta2) + sin^2(theta1 + theta2)
a3 = cos(theta1 + theta2)^2 + sin(theta1 + theta2)^2;
disp(['a3 (cos^2(theta1 + theta2) + sin^2(theta1 + theta2)) = ', num2str(a3)])
% Step 9: Calculate and display a3 = exp(a1) + log(a2)
a3 = exp(a1) + log(a2);
disp(['a3 (exp(a1) + log(a2)) = ', num2str(a3)])
% Step 10: Display the base of the natural logarithm, e, with 15 significant digits
format long
e_value = exp(1);
disp(['The base of the natural logarithm, e = ', num2str(e_value, 15)])
结果
a3 (a1 + a2) = 444
a3 (a1 * a2) = 39483
a3 (a1 / a2) = 0.38318
a3 (cos(theta1 + theta2)) = -0.96593
a3 (cos^2(theta1 + theta2) + sin^2(theta1 + theta2)) = 1
a3 (exp(a1) + log(a2)) = 2.619517318749063e+53
The base of the natural logarithm, e = 2.71828182845905
Lesson 2
代码
% quaternion_basics_homework.m
clear all;
include_namespace_dq
% 1. Display message
disp('Starting quaternion basics homework');
% 2. Store r1 as a rotation of π/8 about the x-axis
phi1 = pi / 8;
r1 = cos(phi1 / 2) + i_*sin(phi1 / 2)
% 3. Store r2 as a rotation of π/16 about the y-axis
phi2 = pi / 16;
r2 = cos(phi2 / 2) + j_*sin(phi2 / 2)
% 4. Store r3 as a rotation of π/32 about the z-axis
phi3 = pi / 32;
r3 = cos(phi3 / 2) + k_*sin(phi3 / 2)
% 5. Calculate the sequential rotation and store in r4
r4 = r1 * r2 * r3
% Plot r4
figure;
plot(r4);
title('Sequential Rotation Result (r4)');
% 6. Find the reverse rotation of r4 and store in r5
r5 = conj(r4)
% Plot r5
figure;
plot(r5);
title('Reverse Rotation (r5)');
% 7. Rotate r5 by 360 degrees about the x-axis and store in r6
phi4 = 2 * pi; % 360 degrees in radians
r6 = cos(phi4 / 2) + i_*sin(phi4 / 2);
disp('Rotation of r5 by 360 degrees about the x-axis (r6):');
r6 = r5 * r6
% Plot r5 and r6 to confirm they represent the same rotation
figure;
subplot(1, 2, 1);
plot(r5);
title('Rotation r5');
subplot(1, 2, 2);
plot(r6);
title('Rotation r6');
结果
Starting quaternion basics homework
r1 =
0.98079 + 0.19509i
r2 =
0.99518 + 0.098017j
r3 =
0.9988 + 0.049068k
r4 =
0.97395 + 0.19863i + 0.086491j + 0.066992k
r5 =
0.97395 - 0.19863i - 0.086491j - 0.066992kRotation of r5 by 360 degrees about the x-axis (r6):
r6 =
- 0.97395 + 0.19863i + 0.086491j + 0.066992k
手写作业
题目 1
问题:四元数乘法的通用形式是什么?将 h 1 = a 1 + b 1 i ^ + c 1 j ^ + d 1 k ^ h_1 = a_1 + b_1\hat{i} + c_1\hat{j} + d_1\hat{k} h1=a1+b1i^+c1j^+d1k^和 h 2 = a 2 + b 2 i ^ + c 2 j ^ + d 2 k ^ h_2 = a_2 + b_2\hat{i} + c_2\hat{j} + d_2\hat{k} h2=a2+b2i^+c2j^+d2k^相乘,找到结果 h 3 = a 3 + b 3 i ^ + c 3 j ^ + d 3 k ^ h_3 = a_3 + b_3\hat{i} + c_3\hat{j} + d_3\hat{k} h3=a3+b3i^+c3j^+d3k^的形式。
解答:
四元数乘法的通用公式是基于以下规则:
- i ^ 2 = j ^ 2 = k ^ 2 = − 1 \hat{i}^2 = \hat{j}^2 = \hat{k}^2 = -1 i^2=j^2=k^2=−1
- i ^ j ^ = k ^ \hat{i}\hat{j} = \hat{k} i^j^=k^, j ^ k ^ = i ^ \hat{j}\hat{k} = \hat{i} j^k^=i^, k ^ i ^ = j ^ \hat{k}\hat{i} = \hat{j} k^i^=j^
- j ^ i ^ = − k ^ \hat{j}\hat{i} = -\hat{k} j^i^=−k^, k ^ j ^ = − i ^ \hat{k}\hat{j} = -\hat{i} k^j^=−i^, i ^ k ^ = − j ^ \hat{i}\hat{k} = -\hat{j} i^k^=−j^
将 h 1 h_1 h1和 h 2 h_2 h2相乘得到 h 3 = h 1 ⋅ h 2 h_3 = h_1 \cdot h_2 h3=h1⋅h2,结果可以通过展开每一项并应用以上乘法规则计算。
h 3 = ( a 1 a 2 − b 1 b 2 − c 1 c 2 − d 1 d 2 ) + ( a 1 b 2 + b 1 a 2 + c 1 d 2 − d 1 c 2 ) i ^ + ( a 1 c 2 − b 1 d 2 + c 1 a 2 + d 1 b 2 ) j ^ + ( a 1 d 2 + b 1 c 2 − c 1 b 2 + d 1 a 2 ) k ^ h_3 = (a_1a_2 - b_1b_2 - c_1c_2 - d_1d_2) + (a_1b_2 + b_1a_2 + c_1d_2 - d_1c_2)\hat{i} + (a_1c_2 - b_1d_2 + c_1a_2 + d_1b_2)\hat{j} + (a_1d_2 + b_1c_2 - c_1b_2 + d_1a_2)\hat{k} h3=(a1a2−b1b2−c1c2−d1d2)+(a1b2+b1a2+c1d2−d1c2)i^+(a1c2−b1d2+c1a2+d1b2)j^+(a1d2+b1c2−c1b2+d1a2)k^
题目 2
问题:四元数范数的通用形式是什么?简化 h 1 h 1 ∗ \sqrt{h_1 h_1^*} h1h1∗。
解答:
四元数
h
1
=
a
1
+
b
1
i
^
+
c
1
j
^
+
d
1
k
^
h_1 = a_1 + b_1\hat{i} + c_1\hat{j} + d_1\hat{k}
h1=a1+b1i^+c1j^+d1k^的范数定义为:
∥ h 1 ∥ = h 1 h 1 ∗ \| h_1 \| = \sqrt{h_1 h_1^*} ∥h1∥=h1h1∗
其中 h 1 ∗ = a 1 − b 1 i ^ − c 1 j ^ − d 1 k ^ h_1^* = a_1 - b_1\hat{i} - c_1\hat{j} - d_1\hat{k} h1∗=a1−b1i^−c1j^−d1k^是 h 1 h_1 h1的共轭。
计算 h 1 h 1 ∗ h_1 h_1^* h1h1∗:
h 1 h 1 ∗ = ( a 1 + b 1 i ^ + c 1 j ^ + d 1 k ^ ) ( a 1 − b 1 i ^ − c 1 j ^ − d 1 k ^ ) = a 1 2 + b 1 2 + c 1 2 + d 1 2 h_1 h_1^* = (a_1 + b_1\hat{i} + c_1\hat{j} + d_1\hat{k})(a_1 - b_1\hat{i} - c_1\hat{j} - d_1\hat{k}) = a_1^2 + b_1^2 + c_1^2 + d_1^2 h1h1∗=(a1+b1i^+c1j^+d1k^)(a1−b1i^−c1j^−d1k^)=a12+b12+c12+d12
因此,四元数的范数为:
∥
h
1
∥
=
a
1
2
+
b
1
2
+
c
1
2
+
d
1
2
\| h_1 \| = \sqrt{a_1^2 + b_1^2 + c_1^2 + d_1^2}
∥h1∥=a12+b12+c12+d12
题目 3
问题:证明每个单位四元数 r = cos ( φ 2 ) + v sin ( φ 2 ) r = \cos\left(\frac{\varphi}{2}\right) + \mathbf{v}\sin\left(\frac{\varphi}{2}\right) r=cos(2φ)+vsin(2φ)都具有单位范数。
解答:
单位四元数的定义是具有单位范数的四元数。设
r
=
cos
(
φ
2
)
+
v
sin
(
φ
2
)
r = \cos\left(\frac{\varphi}{2}\right) + \mathbf{v}\sin\left(\frac{\varphi}{2}\right)
r=cos(2φ)+vsin(2φ),其中
v
\mathbf{v}
v是一个单位向量(即
∥
v
∥
=
1
\| \mathbf{v} \| = 1
∥v∥=1)。
四元数 r r r的范数是其实部和虚部平方和的平方根。在这里,我们需要将 r r r写成实部和虚部的形式,然后计算它们的平方和。
对于四元数 r = cos ( φ 2 ) + v sin ( φ 2 ) r = \cos\left(\frac{\varphi}{2}\right) + \mathbf{v} \sin\left(\frac{\varphi}{2}\right) r=cos(2φ)+vsin(2φ),其实部为 cos ( φ 2 ) \cos\left(\frac{\varphi}{2}\right) cos(2φ),虚部为 v sin ( φ 2 ) \mathbf{v} \sin\left(\frac{\varphi}{2}\right) vsin(2φ)。
为了计算四元数的范数 ∥ r ∥ \|r\| ∥r∥,我们取实部和虚部的平方和的平方根:
∥ r ∥ = ( cos ( φ 2 ) ) 2 + ( sin ( φ 2 ) ∥ v ∥ ) 2 \|r\| = \sqrt{\left(\cos\left(\frac{\varphi}{2}\right)\right)^2 + \left(\sin\left(\frac{\varphi}{2}\right) \|\mathbf{v}\|\right)^2} ∥r∥=(cos(2φ))2+(sin(2φ)∥v∥)2
其中:
- cos ( φ 2 ) \cos\left(\frac{\varphi}{2}\right) cos(2φ)是实部。
- sin ( φ 2 ) ∥ v ∥ \sin\left(\frac{\varphi}{2}\right) \|\mathbf{v}\| sin(2φ)∥v∥是虚部的大小。
因为 v \mathbf{v} v是单位向量,意味着 ∥ v ∥ = 1 \|\mathbf{v}\| = 1 ∥v∥=1,代入上式可得:
∥ r ∥ = cos 2 ( φ 2 ) + sin 2 ( φ 2 ) \|r\| = \sqrt{\cos^2\left(\frac{\varphi}{2}\right) + \sin^2\left(\frac{\varphi}{2}\right)} ∥r∥=cos2(2φ)+sin2(2φ)
利用三角恒等式 cos 2 x + sin 2 x = 1 \cos^2 x + \sin^2 x = 1 cos2x+sin2x=1,进一步简化得到:
∥ r ∥ = 1 = 1 \|r\| = \sqrt{1} = 1 ∥r∥=1=1
因此,每个单位四元数确实具有单位范数。
Lesson 3
代码
% dual_quaternion_basics_homework_part1.m
clear all;
close all;
include_namespace_dq
% 1. Store x_1 representing a translation of 1 meter along the x-axis,
% followed by a rotation of π/8 about the z-axis.
t1 = i_; % Translation of 1 meter along the x-axis
r1 = cos(pi/16) + k_*sin(pi/16); % Rotation of π/8 about the z-axis
x_1 = r1 + 0.5 * E_ * t1 * r1
% 2. Store x_2 representing a translation of -1 meter along the z-axis,
% followed by a rotation of -π/2 about the x-axis.
t2 = -k_; % Translation of -1 meter along the z-axis
r2 = cos(-pi/4) + i_*sin(-pi/4); % Rotation of -π/2 about the x-axis
x_2 = r2 + 0.5 * E_ * t2 * r2
% 3. Calculate the sequential rotation of the neutral reference frame,
% first applying x_0 = 1, then x_1, followed by x_2.
x_0 = DQ(1); % Neutral reference frame
disp('Sequential rotation result (x_3):');
x_3 = x_0 * x_1 * x_2 % Sequential rotation
% Plot x_3
figure;
plot(x_3);
title('Sequential rotation result x_3');
% 4. Obtain the rotation part of x_3 without using rotation()
disp('Rotation part of x_3 (without using rotation()):');
rotation_part_x3 = x_3 - E_ * D(x_3)
rotation_part_x3_2 = P(x_3)
% 5. Obtain the translation part of x_3 without using translation()
disp('Translation part of x_3 (without using translation()):');
translation_part_x3 = 2 * D(x_3) * conj(rotation_part_x3)
% 6. Obtain the pose transformation of a rotation of π/8 about the z-axis,
% followed by a translation of 1 meter along the x-axis, and store it in x_4
t41 = 0;
r41 = cos(pi/16) + k_*sin(pi/16); % Rotation of π/8 about the z-axis
x41 = r41 + E_ * 0.5 * t41 * r41;
t42 = i_; % Translation of 1 meter along the x-axis
r42 = 1;
x42 = r42 + E_ * 0.5 * t42 * r42;
x_4 = x41 * x42
% Check if x_4 is equal to x_1
% If different, explain the reason
if ~(x_4 == x_1)
disp('x_4 and x_1 are different because the order of rotation and translation affects the final pose.');
end
结果
x_1 =
(0.98079 + 0.19509k) + E*(0.49039i - 0.097545j)
x_2 =
(0.70711 - 0.70711i) + E*(0.35355j - 0.35355k)
Sequential rotation result (x_3):
x_3 =
(0.69352 - 0.69352i - 0.13795j + 0.13795k) + E*(0.41573 + 0.27779i + 0.27779j - 0.41573k)
Rotation part of x_3 (without using rotation()):
rotation_part_x3 =
0.69352 - 0.69352i - 0.13795j + 0.13795k
rotation_part_x3_2 =
0.69352 - 0.69352i - 0.13795j + 0.13795k
Translation part of x_3 (without using translation()):
translation_part_x3 =
1i - 1k
x_4 =
(0.98079 + 0.19509k) + E*(0.49039i + 0.097545j)
x_4 and x_1 are different because the order of rotation and translation affects the final pose.
x ‾ 4 \underline{x}_4 x4与 x ‾ 1 \underline{x}_1 x1不同的原因在于旋转和平移的顺序影响了最终的姿态变换结果。
在三维空间中,旋转和平移是非交换的操作,也就是说,先进行旋转然后平移,与先平移然后旋转,通常会得到不同的结果。因为在三维空间中,旋转会改变坐标系的方向,进而影响随后的平移方向。
只有在特定情况下会得到相同的结果,包括:
旋转角度为零:
- 如果旋转角度为零,则没有实际的旋转效果,平移的方向不会受到影响。因此,不论平移和旋转的顺序,最终的结果都是相同的。
旋转轴与平移方向平行:
- 如果旋转是绕某个轴(例如 x x x轴)进行,而平移也是沿该轴的方向(即 x x x轴方向),那么旋转不会改变平移的方向。因此,无论先平移再旋转还是先旋转再平移,结果都会相同。
平移向量为零:
- 如果平移量为零,则没有实际的平移效果。这种情况下,无论先旋转还是先平移,最终位置都相同。
单位对偶四元数的公式是先平移后旋转