Tom*_*Vos 8 python 3d rotation quaternions cartesian
将矢量（X，Y，Z）从 ENU 旋转到 NED 的统一方法，反之亦然
\n将四元数从 ENU 旋转到 NED 的统一方法，反之亦然
\n如果可以给出解决方案，我们将不胜感激python，但对我来说更多的是专业知识。
如果我在 ENU 中有这个向量
\nv_xyz_ENU = [5,2,1] \n翻译为
\nv_xyz_NED = [2,5,-1]\n\n\nENU <-> NED
\n
\nX, Y, Z <-> Y, X,-Z
如果我在包含 RPY 的向量上使用这种技术（如果这是正确的表达方式），我会感到困惑。
v_rpy_ENU = [0,0,90] \n翻译为
\nv_rpy_NED = [0,0,-90]\n当我写下计算时，我更喜欢使用以度为单位的角度
这个旋转似乎不正确，因为我知道结果应该是 0\xc2\xb0
\nENU Yaw 0\xc2\xb0 应该是 NED Yaw -90\xc2\xb0
\nENU Yaw 90\xc2\xb0 应该是 NED Yaw 0\xc2 \xb0
\nENU 偏航 180\xc2\xb0 应为 NED 偏航 -90\xc2\xb0/270\xc2\xb0
\nENU 偏航 -90\xc2\xb0 应为 NED 偏航 180\xc2\xb0
我确实尝试过了解四元数。我在网上其他地方找到的有关使用四元数旋转的解释和示例似乎很简单，但我似乎无法得出我理解的结果。
\n现在，我忘记了 RPY 并开始使用四元数。\n我在 ENU 中有这个四元数，代表关于 Z 的相同 90\xc2\xb0 旋转
Q_ENU = pq.Quaternion(axis=[0,0,1], degrees=90)\n有人声称旋转可以与矢量旋转类似，但保持 w 分量不变。所以我尝试：
Q_NED = pq.Quaternion(Q_ENU.q[0], Q_ENU.q[2], Q_ENU.q[1], -Q_ENU.q[3])\n\n\nQuaternion.q 的数据顺序 = [w,x,y,z]
\n
现在，我验证将 Q_NED 转换为 RPY 角度后，NED 中的偏航是否为 0\xc2\xb0，但结果似乎是：
Q_ENU: 0.707 +0.000i +0.000j +0.707k\nQ_NED: 0.707 +0.000i +0.000j -0.707k\nrpy_NED: [0.0, 0.0, -89.99999999999999]\nI expect rpy_NED [0.0, 0.0, 0.0]\n现在，在我开始写我尝试过的所有其他可能性之前，我认为不这样做可以节省我自己和你很多时间。我真的很感谢您对如何做到这一点的一些解释，或者至少朝着正确的方向推动。
\n-T
\nSto*_*ght 11
欢迎来到四元数的神奇世界！我理解您在尝试了解四元数如何工作时的挫败感；请放心，您并不孤单。不幸的是，四元数存在多种“风格”，从如何格式化/解释它们，到四元数乘法如何工作，以及四元数乘法代表什么——不必要地使本已复杂的主题变得复杂。
\n\xe2\x80\x9c四元数是在汉密尔顿完成了他真正出色的工作之后提出的，虽然非常巧妙，但对于那些以任何方式接触过它们的人来说却是一种纯粹的邪恶。\xe2\x80\x9d - 开尔文勋爵
\n许多定义分散在互联网上，但从本质上讲，四元数是 4 维空间中的向量。一个常见的类比是将复数域扩展为具有 1 个“实数”分量和 3 个“虚数”分量。下面是一个 4x1 的四元数向量示例，其中第一个元素是“实”分量 ，r最后三个元素是分别沿x、y和虚轴的“虚”分量、和[1,2]。zijk
hamilton_quaternion = np.array([\n [r],\n [x], # i\n [y], # j\n [z] # k\n])\n这种类比在 3-D 空间中旋转矢量的背景下或跨两个不同的参考系非常有用，并且是工程界的主要类比。然而，出于稍后解释的原因，纯数学家有时会厌恶这种用法。
\n上面提供的示例四元数称为汉密尔顿约定，但还存在第二种约定：JPL约定 [1]。使用 JPL 约定，前三个元素分别是沿、和虚轴的“虚”分量x、y和，最后一个元素是“实”分量。zijkr
jpl_quaternion = np.array([\n [x], # i\n [y], # j\n [z], # k\n [r]\n])\n注意：此后使用的所有四元数都将使用汉密尔顿约定。
\n四元数乘法（在本说明中表示为 \xe2\x8a\x97）遵循一组严格的规则。人们并不像在线性代数中那样简单地将两个四元数交叉相乘。每个分量的虚轴对于确定两个四元数的真实乘积至关重要。
\n右手乘法将虚轴上的乘法定义为：
\ni**2 = j**2 = k**2 = i*j*k = -1\ni*j = k\nj*k = i\nk*i = j\nj*i = -k\nk*j = -i\ni*k = -j\n而左手乘法将虚轴上的乘法定义为：
\ni**2 = j**2 = k**2 = i*j*k = -1\ni*j = -k\nj*k = -i\nk*i = -j\nj*i = k\nk*j = i\ni*k = j\n有关更多详细信息和解释，请参阅 [1] 的第 1.2.2 节。
\n在一般工程和计算机科学的背景下，四元数用于表示向量从一个参考系到另一个参考系的旋转。然而，这种旋转可以表示向量的旋转......
\np\'从表示两个参考系 之间的旋转的四元数q和原始向量获取旋转向量 的“正式”方法p如下：
| 方程 | （参考） |
|---|---|
p = [0; p_x; p_y; p_z] | - |
q = [q_r; q_x; q_y; q_z] | (1) |
q* = [q_r; -q_x; -q_y; -q_z] | (2) |
p\' = q \xe2\x8a\x97 p \xe2\x8a\x97 q* | (3) |
这里q*称为 的四元数共轭q。四元数共轭很简单q，但其中“虚数” x、y和z分量为负，但“实数”分量不变，如方程 1 和 2 所示。
要直观地了解其实际工作原理，请参阅 [2]。
\n!!! 假设！！！
\np是一个“纯”四元数，其中“实”分量为 0p大小（L2 范数）为 1q大小（L2 范数）为 1sqrt(q_r^2 + q_x^2 + q_y^2 + q_z^2)如果这些假设中的任何一个是错误的，都不会p\'给出您期望的旋转向量。可以想象，上述假设严格限制了四元数代数的能力。
可能会注意到，结果p\'是一个四元数 - 但由于上面所做的假设，它应该是一个“纯”四元数，这意味着旋转的 3D 向量可以提取为沿虚轴x、y和轴的、、 和分量。这里这将是, 或的最后 3 个元素。zijkp\'p\'[1:]
到目前为止，在数学家圈子里，没有什么是（应该是？）亵渎的……直到我向您展示四元数乘法的“工程”等效表示法，其中四元数被分解为“真实”分量（| 的左侧）和“向量”组件（| 右侧）。有关详细信息，请参阅第 1.2.2 节 [1]：
\na \xe2\x8a\x97 b = [a0*b0 - np.dot(a, b) | a0*b + b0*a + np.cross(a,b)]\n真的，所有的繁重工作。事实证明，由于三角奇点，使用角度来表示 3D 空间中的旋转是一个可怕的想法。从物理上讲，这些奇点将自身表达为万向节锁： https: //en.wikipedia.org/wiki/Gimbal_lock。
\n可以构建旋转矩阵，以便我们将方程 4 定义为方程 3 [1] 的等效数学表示。一个小小的好处是，旋转矩阵可以写为 3x3，并且 和p都不p\'需要转换为四元数，并且可以保留为表示x、y和z值的旧式 3x1 向量。
| 方程 | （参考） |
|---|---|
p\' = q \xe2\x8a\x97 p \xe2\x8a\x97 q* = R x p | (4) |
def build_rotation_matrix(rotation_quaternion: np.ndarray):\n """Construct a 3x3 rotation matrix given a rotation quaternion"""\n q0 = rotation_quaternion[0][0]\n q1 = rotation_quaternion[1][0]\n q2 = rotation_quaternion[2][0]\n q3 = rotation_quaternion[3][0]\n return np.array([\n [2*q0**2 + 2*q1**2 - 1, 2*(q1*q2 - q3*q0), 2*(q1*q3 + q2*q0)],\n [2*(q1*q2 + q3*q0), 2*q0**2 + 2*q2**2 - 1, 2*(q2*q3 - q1*q0)],\n [2*(q1*q3 - q2*q0), 2*(q2*q3 + q1*q0), 2*q0**2 + 2*q3**2 - 1]\n ])\n其中q0 = q_r、q1 = q_x、q2 = q_y、 和q3 = q_z如前所述。
r = [r_x; r_y; r_z]矢量、旋转角度和旋转四元数之间的关系theta有助于直观地理解四元数的作用。因为，并且仅仅因为，在之前所做的假设中，可以发现以下内容[1]：
| 方程 | （参考） |
|---|---|
q=[cos(theta/2); sin(theta/2)*r_x; sin(theta/2)*r_y; sin(theta/2)*r_z] | (5) |
tan(theta/2) = L2norm([q1; q2; q3])/q0 | (6) |
这是迄今为止这个答案中数学上最不严格的部分，我还没有找到数学证明来帮助我找到两个 FoR 之间的旋转四元数，尽管我还没有看得太仔细。我只是使用 [2] https://eater.net/quaternions/video/rotation并通过单独拖动四元数的i、j和k分量来旋转球体，直到获得球体的 ENU 轴（i、j和k）旋转至 NED（i、j、 和k）。
enu2ned_quaternion = np.array([\n [0],\n [sqrt(2)/2],\n [sqrt(2)/2],\n [0]\n])\nenu2ned_rotation_matrix = build_rotation_matrix(enu2ned_quaternion)\n直观地说，从局部到全局FoR的旋转只是沿相反方向旋转，但沿着相同的旋转平面/矢量。在theta=180中enu2ned_quaternion，我们现在设置theta=-180，通过公式 5 可以看出：
ned2enu_quaternion = np.array([\n [0],\n [-sqrt(2)/2],\n [-sqrt(2)/2],\n [0]\n])\nned2enu_rotation_matrix = build_rotation_matrix(ned2enu_quaternion)\n有趣的是，这也是第一个旋转四元数的共轭，或者ned2enu_quaternion = enu2ned_quaternion*
现在为我们的测试构建数据以及一些方便的函数：
\n# Global FoR : ENU\nglobal_for = {\n \'East\': np.array([[1], [0], [0]]),\n \'West\': np.array([[-1], [0], [0]]),\n \'North\': np.array([[0], [1], [0]]),\n \'South\': np.array([[0], [-1], [0]]),\n \'Up\': np.array([[0], [0], [1]]),\n \'Down\': np.array([[0], [0], [-1]])\n}\n\n# Local FoR : NED\nlocal_for = {\n \'North\': np.array([[1], [0], [0]]),\n \'South\': np.array([[-1], [0], [0]]),\n \'East\': np.array([[0], [1], [0]]),\n \'West\': np.array([[0], [-1], [0]]),\n \'Up\': np.array([[0], [0], [-1]]),\n \'Down\': np.array([[0], [0], [1]]),\n}\n\n\ndef l2norm(vec: np.ndarray):\n """Calculate the L2 norm, Euclidean distance, of a nx1 vector"""\n return sqrt(sum([_**2 for _ in vec]))\n\n\ndef str_vec(vec: np.ndarray):\n return f\'[{round(vec[0][0],2)},\' \\\n f\' {round(vec[1][0],2)},\' \\\n f\' {round(vec[2][0],2)}]\'\n我们现在可以测试我们的转换给我们带来了什么（暂时使用一些丑陋的代码 - 也许我会在不是凌晨 1 点时重新编写它，使其看起来更好）：
\nif __name__ == \'__main__\':\n # Verifying ENU -> NED\n for direction_enu, vector_enu in global_for.items():\n vector_ned = np.matmul(enu2ned_rotation_matrix, vector_enu)\n direction_ned = [_ for _ in local_for.keys()\n if l2norm(vector_ned - local_for[_]) < 0.001]\n if len(direction_ned) > 1:\n print(\'ERROR: more than one solution found? IMPOSSIBLE!!!\')\n break\n direction_ned = direction_ned[0] # cause it\'s a list\n print(f\'ENU {direction_enu} {str_vec(vector_enu)} => \'\n f\'NED {direction_ned} {str_vec(vector_ned)}\')\n\n # Verifying NED -> ENU\n for direction_ned, vector_ned in local_for.items():\n vector_enu = np.matmul(ned2enu_rotation_matrix, vector_ned)\n direction_enu = [_ for _ in global_for.keys()\n if l2norm(vector_enu - global_for[_]) < 0.001]\n if len(direction_enu) > 1:\n print(\'ERROR: more than one solution found? IMPOSSIBLE!!!\')\n break\n direction_enu = direction_enu[0] # cause it\'s a list\n print(f\'NED {direction_ned} {str_vec(vector_ned)} => \'\n f\'ENU {direction_enu} {str_vec(vector_enu)}\')\n结果...
\nENU East [1, 0, 0] => NED East [0.0, 1.0, 0.0]\nENU West [-1, 0, 0] => NED West [-0.0, -1.0, 0.0]\nENU North [0, 1, 0] => NED North [1.0, 0.0, 0.0]\nENU South [0, -1, 0] => NED South [-1.0, -0.0, 0.0]\nENU Up [0, 0, 1] => NED Up [0.0, 0.0, -1.0]\nENU Down [0, 0, -1] => NED Down [0.0, 0.0, 1.0]\nNED North [1, 0, 0] => ENU North [0.0, 1.0, 0.0]\nNED South [-1, 0, 0] => ENU South [-0.0, -1.0, 0.0]\nNED East [0, 1, 0] => ENU East [1.0, 0.0, 0.0]\nNED West [0, -1, 0] => ENU West [-1.0, -0.0, 0.0]\nNED Up [0, 0, -1] => ENU Up [0.0, 0.0, 1.0]\nNED Down [0, 0, 1] => ENU Down [0.0, 0.0, -1.0]\n\n旋转四元数既不q是一个参考系相对于另一个参考系的方向也不是姿态。相反，它表示垂直于旋转平面的“中点”矢量，围绕该矢量从一个参考系旋转到另一个参考系。这就是为什么定义四元数从全局框架旋转到局部框架（或反之亦然）非常重要。ENU 到 NED 旋转就是一个完美的例子，其中旋转四元数是，两个横坐标 (X) 轴（在全局和局部参考系中）之间的“中点”。如果您执行“右手定则”，用三个手指指向 ENU 方向，并从 NED 方向快速来回切换，您会发现两个 FoR 的旋转只是关于 [ 1; 1; 0] 在全球论坛中。[0; sqrt(2)/2; sqrt(2)/2; 0]
我极力推荐以下开源参考：
\n[1] “错误状态卡尔曼滤波器的四元数运动学”作者：Joan Sol\xc3\xa0。https://hal.archives-ouvertes.fr/hal-01122406v5
\n对于一个“游乐场”进行实验，并获得对四元数的“亲身”理解：
\n[2] 可视化四元数，一个可探索的视频系列。格兰特·桑德森的课程。Ben Eater 的技术https://eater.net/quaternions
\n| 归档时间： |
|
| 查看次数： |
7913 次 |
| 最近记录： |