在给定起点、方向四元数和行进距离的情况下查找下一个 3D 点

Question

我需要使用什么公式来找到给定的第二个 3D 点 (P1)：

第一个点P0 = [x0,y0,z0]
方向四元数 Q0 = [q0, q1, q2, q3]
走过的距离S

我猜测行进距离 S 需要拆分为它的组成部分 X、Y 和 Z。有使用四元数的简单方法吗？

Answer 1

方向向量（forward-vector）的分量为：

x = 2 * (q1*q3 + q0*q2)
y = 2 * (q2*q3 - q0*q1)
z = 1 - 2 * (q1*q1 + q2*q2)

这个公式是从四元数到矩阵（如下）乘以 (0,0,1) 向量计算得出的。

归一化D=(x,y,z)如果不是单位，计算P_New.x= P0.x + S * D.x等分量

要获得向上和向左的方向矢量（也许您的方向是指另一个基本框架方向 - OX 或 OY 作为前向），使用下面引用的矩阵的另一列：

Link: 四元数乘法和正交矩阵乘法都可以用来表示旋转。如果四元数由 qw + i qx + j qy + k qz 表示，则表示相同旋转的等效矩阵为：

1 - 2*qy2 - 2*qz2   2*qx*qy - 2*qz*qw   2*qx*qz + 2*qy*qw
2*qx*qy + 2*qz*qw   1 - 2*qx2 - 2*qz2   2*qy*qz - 2*qx*qw
2*qx*qz - 2*qy*qw   2*qy*qz + 2*qx*qw   1 - 2*qx2 - 2*qy2

在给定起点、方向四元数和行进距离的情况下查找下一个 3D 点

Find the next 3D point given a starting point, a orientation quaternion, and a distance travelled

math

robotics

quaternions

slam