{"id":1562,"date":"2026-09-01T14:44:52","date_gmt":"2026-09-01T06:44:52","guid":{"rendered":"https:\/\/www.kakosci.com\/?p=1562"},"modified":"2026-09-01T14:44:54","modified_gmt":"2026-09-01T06:44:54","slug":"ros2%e4%b8%ad%e9%9d%99%e6%80%81tf%e5%9d%90%e6%a0%87%e7%b3%bb%e8%bd%ac%e6%8d%a2%e3%80%81%e6%ac%a7%e6%8b%89%e8%a7%92%e3%80%81%e6%b1%82%e9%80%86%e7%ae%80%e6%9e%90%ef%bc%8c%e7%94%a8%e4%ba%8e%e4%bc%a0","status":"publish","type":"post","link":"https:\/\/www.kakosci.com\/index.php\/2026\/09\/01\/ros2%e4%b8%ad%e9%9d%99%e6%80%81tf%e5%9d%90%e6%a0%87%e7%b3%bb%e8%bd%ac%e6%8d%a2%e3%80%81%e6%ac%a7%e6%8b%89%e8%a7%92%e3%80%81%e6%b1%82%e9%80%86%e7%ae%80%e6%9e%90%ef%bc%8c%e7%94%a8%e4%ba%8e%e4%bc%a0\/","title":{"rendered":"ROS2\u4e2d\u9759\u6001TF\u5750\u6807\u7cfb\u8f6c\u6362\u3001\u6b27\u62c9\u89d2\u3001\u6c42\u9006\u7b80\u6790\uff0c\u7528\u4e8e\u4f20\u611f\u5668\uff08\u6fc0\u5149\u96f7\u8fbe\/\u76f8\u673a\uff09\u4e0e\u98de\u63a7\u7b49"},"content":{"rendered":"\n

ROS2\u5750\u6807\u7cfb\u548cTF\u53c2\u6570<\/h2>\n\n\n
\n
\"\"<\/figure>\n<\/div>\n\n\n

ROS2\u9759\u6001\u5750\u6807\u8f6c\u6362\uff08static_transform_publisher\uff09\u7684\u65cb\u8f6c\u53c2\u6570\uff0c\u662f\u57fa\u4e8e\u5185\u65cb<\/strong>Yaw-Pitch-Roll\uff08\u65cb\u8f6c\u4f53\u4f9d\u6b21\u7ed5\u81ea\u8eab\u53d8\u5316\u7684z\u3001y’\u3001x”\u8f74\u65cb\u8f6c<\/span>\uff09\uff0c\u6216\u5916\u65cb<\/strong>Roll-Pitch-Yaw\uff08\u4f9d\u6b21\u7ed5\u56fa\u5b9a\u7684x\u3001y\u3001z\u8f74<\/span>\uff09\u7684\u987a\u5e8f\uff0c\u4e24\u8005\u7b49\u4ef7<\/strong>\uff0c\u9075\u5faa\u53f3\u624b<\/strong>\u65cb\u8f6c\u3002\u5199\u53c2\u6570\u5efa\u8bae\u5e26\u201c–yaw\u201d\u8fd9\u4e9b\u6807\u7b7e\u907f\u514d\u6b67\u4e49\uff1a<\/p>\n\n\n\n

ros2 run tf2_ros static_transform_publisher --x 1.0 --y 0.5 --z 0.0 --yaw 0.2 --pitch 0.1 --roll 0.3 --frame-id parent_frame --child-frame-id child_frame<\/code><\/pre>\n\n\n\n
\n
\n
\"\"<\/figure>\n<\/div><\/div>\n\n\n\n
\n
\"\"<\/figure>\n<\/div><\/div>\n<\/div>\n\n\n\n
\u4ee5\u4e0a\u9762\u7684\u4e24\u4e2a\u4f20\u611f\u5668\u4e3a\u4f8b\uff08mid-360s\u548c\u4e00\u90e8\u6df1\u5ea6\u76f8\u673a\uff09\uff0c\u76f8\u673a\u548c\u96f7\u8fbe\u5728ros2\u4e2d\u7684\u90fd\u9075\u5faa\u53f3\u624b\u5750\u6807\u7cfb\uff08x\u671d\u524d\uff0cy\u671d\u5de6\uff0cz\u671d\u4e0a\uff09\uff0c\u96f7\u8fbe\u7684\u539f\u70b9\u5728\u534a\u7403\u4f53\u4e2d\u5fc3\u5e95\u90e8\u3002\u76f8\u673a\u7684\u539f\u70b9\u6bcf\u4e2a\u5382\u5bb6\u53ef\u80fd\u4e0d\u540c\uff0c\u6211\u8fd9\u8fb9\u7684\u867d\u7136\u662f\u7b2c\u4e09\u65b9\u5382\u5bb6\uff0c\u4f46\u9075\u5faaorbbec\u7684\u5e95\u5c42\uff0c\u539f\u70b9\u5728\u5de6\u7ea2\u5916\u76f8\u673a<\/strong>\u5904\uff08\u6307\u76f8\u673a\u6b63\u89c6\u4f60\u7684\u524d\u65b9\u65f6\u7684\u5de6\u8fb9\uff0c\u6216\u8005\u4f60\u770b\u7740\u76f8\u673a\u6b63\u9762\u65f6\u53f3\u8fb9\u7684\u90a3\u4e2aIR\u6444\u50cf\u5934\uff09\u3002<\/p>\n\n\n\n
\n
\n
\"\"<\/figure>\n<\/div><\/div>\n\n\n\n
\n
\"\"<\/figure>\n<\/div>\n<\/div>\n\n\n\n
\u81f3\u4e8e\u5b83\u4eec\u7684\u7684\u5750\u6807\u8f6c\u6362\u53c2\u6570\uff0c\u5176\u5b9e\u5e94\u8be5\u505a\u4e0b\u5916\u53c2\u6807\u5b9a\u7684\u4f46\u662f\u8fd8\u6ca1\u641e\uff08\u6b38\u563f\uff09\u3002\u6240\u4ee5\u5148\u7528\u4e86\u4e2a\u7b80\u5355\u7684\u65b9\u6cd5\uff0c\u6bd5\u7adf\u662f\u56fa\u5b9a\u7740\u7684\uff0c\u8ba9\u5356\u5bb6\u5e2e\u6211\u5728\u6a21\u578b\u4e0a\u6d4b\u4e86\u4e00\u4e0b\u4e24\u4e2a\u4e2d\u5fc3\u70b9\u95f4\u7684\u0394x\u3001y\u3001z\uff08\u56fe\u4e0a\u7ed9\u6df1\u5ea6\u76f8\u673a\u4e2d\u5fc3\u70b9\u5708\u53cd\u4e86\uff0c\u4e0d\u8fc7\u53cd\u6b63\u662f\u5bf9\u79f0\u7684\uff09\u3002\u8fd9\u4e2a\u65f6\u5019\u5f97\u5230\u7684\u53c2\u6570\u8fd8\u4e0d\u80fd\u76f4\u63a5\u7528\uff0c\u56e0\u4e3a\u662f\u5728\u8f6f\u4ef6\u5750\u6807\u7cfb\u4e0b\u7684\u3002\u6b64\u65f6\u8bb0\u76f8\u673a\u4e3a\u7236\u5750\u6807\u7cfb<\/strong>\uff0c\u96f7\u8fbe\u4e3a\u5b50\u5750\u6807\u7cfb<\/strong>\uff0c\u5f97\u5230\u5e73\u79fb\u53d8\u6362\u53c2\u6570\u4e3a\u0394x\uff0c\u0394y\uff0c\u0394z = -10.9056\uff0c-19.2047\uff0c65.2601\uff08mm\uff09\uff0c\u65cb\u8f6c\u53d8\u6362\u53c2\u6570\u4e3ayaw = -90\u00b0\uff0c pitch = 0\u00b0\uff0c roll = -50\u00b0\uff0c\u6ce8\u610f\u9700\u8981\u8f6c\u6362\u6210\u5f27\u5ea6\uff0cyaw = -1.57079633\uff0c roll = -0.87266463\u3002<\/p>\n\n\n\n
\u9759\u6001\u53d8\u6362\u53c2\u6570\u6c42\u9006\u3001\u4e07\u5411\u9501<\/h2>\n\n\n\n
\u7136\u800c\uff0c\u4e8b\u60c5\u6ca1\u6709\u8fd9\u4e48\u7b80\u5355\u3002\u56e0\u4e3a\u4f7f\u7528fast-lio2\u65f6\uff0ctf\u6811\u91ccodom\u540e\u9762\u63a5\u7684\u5c31\u662f\u6fc0\u5149\u96f7\u8fbe\u7684\u5750\u6807\u7cfb\uff0c\u6ca1\u6cd5\u8ba9\u6df1\u5ea6\u76f8\u673a\u7684\u5750\u6807\u7cfb\u518d\u6210\u4e3a\u96f7\u8fbe\u7684\u7b2c\u4e8c\u4e2a\u7238\u7238\uff08\uff09\uff0c\u6240\u4ee5\u53ea\u80fd\u8ba9\u96f7\u8fbe\u6210\u4e3a\u6df1\u5ea6\u76f8\u673a\u7684\u7236\u5750\u6807\u7cfb\uff0c\u5373\u5bf9\u4e0a\u9762\u7684\u53c2\u6570\u8fdb\u884c\u6c42\u9006\u3002\u57fa\u672c\u516c\u5f0f\u5982\u4e0b\uff0cR\u662f\u65cb\u8f6c\u77e9\u9635\uff0ct\u662fxyz\u5e73\u79fb\u5411\u91cf\u3002\u56e0\u4e3a\u65cb\u8f6c\u77e9\u9635\u6b63\u4ea4\uff0c\u5176\u8f6c\u7f6e\u5c31\u662f\u5b83\u7684\u9006<\/strong>\u3002<\/p>\n\n\n\n
R<\/mi>B<\/mi>A<\/mi><\/mrow><\/msub>=<\/mo>R<\/mi>A<\/mi>B<\/mi><\/mrow>T<\/mi><\/msubsup><\/mrow>R_{BA}=R^{T}_{AB}<\/annotation><\/semantics><\/math><\/div>\n\n\n\n
<\/p>\n\n\n\n
t<\/mi>B<\/mi>A<\/mi><\/mrow><\/msub>=<\/mo>\u2212<\/mo>R<\/mi>B<\/mi>A<\/mi><\/mrow><\/msub>t<\/mi>A<\/mi>B<\/mi><\/mrow><\/msub><\/mrow>t_{BA}=-R_{BA}t_{AB}<\/annotation><\/semantics><\/math><\/div>\n\n\n\n
<\/p>\n\n\n\n
\u5b9e\u9645\u4e0aros2\u63d0\u4f9b\u4e86\u4e00\u4e9b\u65b9\u4fbf\u7684\u5de5\u5177\u53ef\u4ee5\u7b97\u8fd9\u4e2a\uff0c\u5982\u5148\u53d1\u5e03\u4e00\u4e2a\u5df2\u77e5\u7684\u9759\u6001\u53d8\u6362\uff0c\u518d\u53cd\u5411\u67e5\u8be2\u53d8\u6362\uff1a<\/p>\n\n\n\n
ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll A B\n\n# \u518d\u5f00\u4e00\u4e2aterminal\nros2 run tf2_ros tf2_echo B A<\/code><\/pre>\n\n\n\n
\u4e5f\u53ef\u4ee5\u5199\u4e2a\u7b80\u5355\u7684python\u4ee3\u7801\uff0c\u4f7f\u7528invert_transform(x, y, z, roll, pitch, yaw)\u6c42\u89e3\u4e00\u7ec4\u53c2\u6570\u7684\u9006\uff0c\u5148\u628a\u4e09\u4e2a\u65cb\u8f6c\u5f27\u5ea6\u8f6c\u6210\u77e9\u9635\u5f62\u5f0f\uff0c\u518d\u8ba1\u7b97\uff1a<\/p>\n\n\n\n
import numpy as np\nimport math\n\ndef euler_to_rot(roll, pitch, yaw):\n    \"\"\"\n    \u5c06 Z-Y-X \u5185\u65cb\u6b27\u62c9\u89d2\uff08\u5f27\u5ea6\uff09\u8f6c\u6362\u4e3a\u65cb\u8f6c\u77e9\u9635\u3002\n    \u65cb\u8f6c\u987a\u5e8f\uff1a\u5148\u7ed5 Z \u8f74\u8f6c yaw\uff0c\u518d\u7ed5 Y \u8f74\u8f6c pitch\uff0c\u6700\u540e\u7ed5 X \u8f74\u8f6c roll\u3002\n    \u5373 R = Rz(yaw) * Ry(pitch) * Rx(roll)\n    \"\"\"\n    cr = math.cos(roll); sr = math.sin(roll)\n    cp = math.cos(pitch); sp = math.sin(pitch)\n    cy = math.cos(yaw); sy = math.sin(yaw)\n\n    R = np.array([\n        [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],\n        [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],\n        [-sp,   cp*sr,            cp*cr]\n    ])\n    return R\n\ndef rot_to_euler(R):\n    \"\"\"\n    \u4ece\u65cb\u8f6c\u77e9\u9635\u63d0\u53d6 Z-Y-X \u5185\u65cb\u6b27\u62c9\u89d2 (roll, pitch, yaw)\u3002\n    \u5904\u7406\u4e07\u5411\u9501\u60c5\u51b5\uff08pitch \u2248 \u00b1\u03c0\/2\uff09\u3002\n    \"\"\"\n    # \u8ba1\u7b97 pitch\uff0c\u4f7f\u7528 atan2(-R[2,0], sqrt(R[0,0]^2 + R[1,0]^2))\n    pitch = math.atan2(-R[2,0], math.sqrt(R[0,0]**2 + R[1,0]**2))\n\n    if abs(pitch - math.pi\/2) < 1e-6:\n        # pitch = +90\u00b0\uff0c\u5b58\u5728\u4e07\u5411\u9501\uff0croll \u56fa\u5b9a\u4e3a 0\n        roll = 0.0\n        yaw = math.atan2(R[0,1], R[0,2])\n    elif abs(pitch + math.pi\/2) < 1e-6:\n        # pitch = -90\u00b0\uff0c\u4e07\u5411\u9501\n        roll = 0.0\n        yaw = -math.atan2(R[0,1], R[0,2])\n    else:\n        roll = math.atan2(R[2,1], R[2,2])\n        yaw = math.atan2(R[1,0], R[0,0])\n\n    return roll, pitch, yaw\n\ndef invert_transform(x, y, z, roll, pitch, yaw):\n    \"\"\"\n    \u8f93\u5165 A\u2192B \u7684\u53d8\u6362\u53c2\u6570\uff1a\n        x, y, z        \uff1a\u5e73\u79fb\u91cf\n        roll, pitch, yaw\uff1aZ-Y-X \u5185\u65cb\u6b27\u62c9\u89d2\uff08\u5f27\u5ea6\uff09\n    \u8fd4\u56de B\u2192A \u7684\u53d8\u6362\u53c2\u6570\uff08\u540c\u6837\u683c\u5f0f\uff09\u3002\n    \"\"\"\n    # \u6784\u9020 A\u2192B \u7684\u65cb\u8f6c\u77e9\u9635\u548c\u5e73\u79fb\u5411\u91cf\n    R_AB = euler_to_rot(roll, pitch, yaw)\n    t_AB = np.array([x, y, z])\n\n    # \u53cd\u5411\u65cb\u8f6c\u77e9\u9635\n    R_BA = R_AB.T\n    # \u53cd\u5411\u5e73\u79fb\uff1at_BA = -R_BA * t_AB\n    t_BA = -R_BA @ t_AB\n\n    # \u4ece R_BA \u63d0\u53d6\u6b27\u62c9\u89d2\n    roll_BA, pitch_BA, yaw_BA = rot_to_euler(R_BA)\n\n    return t_BA[0], t_BA[1], t_BA[2], roll_BA, pitch_BA, yaw_BA\n\n# ==================== \u6d4b\u8bd5 ====================\nif __name__ == \"__main__\":\n\n    print(invert_transform(-10.9056, -19.2047, 65.2601, -0.87266463, 0, -1.57079633))\n\n    # \u5f97\u5230\u8f93\u51fa\uff1a(np.float64(-19.204700034953575), np.float64(57.00212161349289), np.float64(-33.59420922700847), -3.819693587816054e-09, -0.87266463, 1.5707963317811524)<\/code><\/pre>\n\n\n\n
\u4ee3\u7801\u4e2d\u5bf9\u4e8e\u4e07\u5411\u9501<\/strong>\u7684\u89e3\u51b3\u65b9\u5f0f\u662f\u56fa\u5b9aroll\u4e3a0\u3002\u7b80\u5355\u89e3\u91ca\u4e07\u5411\u9501\uff0c\u5373\u5f53\u4e2d\u95f4\u90a3\u4e2a\u65cb\u8f6c\u89d2\uff08\u6bd4\u5982 Z-Y-X \u987a\u5e8f\u4e2d\u7684 pitch\uff09\u8fbe\u5230\u00b190\u00b0\u65f6\uff0c\u7b2c\u4e00\u4e2a\u65cb\u8f6c\u8f74\u548c\u7b2c\u4e09\u4e2a\u65cb\u8f6c\u8f74\u5728\u7a7a\u95f4\u4e0a\u91cd\u5408<\/strong>\uff0c\u5bfc\u81f4\u7cfb\u7edf\u4e27\u5931\u4e00\u4e2a\u65cb\u8f6c\u81ea\u7531\u5ea6\u7684\u73b0\u8c61\u3002\u6982\u5ff5\u6bd4\u8f83\u62bd\u8c61\uff0c\u6570\u5b66\u4e0a\u5c31\u662f\uff1a<\/p>\n\n\n\n
\u4ee5\u5185\u65cbz-y-x\u987a\u5e8f\u4e3a\u4f8b\uff0c\u5f53pitch = 90\u00b0\u65f6\uff0c\u65cb\u8f6c\u77e9\u9635\uff1aR<\/mi>=<\/mo>R<\/mi>z<\/mi><\/msub>(<\/mo>\u03c8<\/mi>)<\/mo>R<\/mi>y<\/mi><\/msub>(<\/mo>2<\/mn>\u03c0<\/mi>)<\/mo>R<\/mi>x<\/mi><\/msub>(<\/mo>\u03d5<\/mi>)<\/mo><\/mrow>R=R_z(\u03c8)R_y(2\u03c0)R_x(\u03d5)<\/annotation><\/semantics><\/math>\uff0c\u5f97\uff1a<\/p>\n\n\n\n
R<\/mi>=<\/mo>[<\/mo>0<\/mn><\/mtd>s<\/mi>i<\/mi>n<\/mi>(<\/mo>\u03c8<\/mi>\u2212<\/mo>\u03d5<\/mi>)<\/mo><\/mrow><\/mtd>c<\/mi>o<\/mi>s<\/mi>(<\/mo>\u03c8<\/mi>\u2212<\/mo>\u03d5<\/mi>)<\/mo><\/mrow><\/mtd><\/mtr>0<\/mn><\/mtd>c<\/mi>o<\/mi>s<\/mi>(<\/mo>\u03c8<\/mi>\u2212<\/mo>\u03d5<\/mi>)<\/mo><\/mrow><\/mtd>\u2212<\/mo>s<\/mi>i<\/mi>n<\/mi>(<\/mo>\u03c8<\/mi>\u2212<\/mo>\u03d5<\/mi>)<\/mo><\/mrow><\/mtd><\/mtr>\u2212<\/mo>1<\/mn><\/mrow><\/mtd>0<\/mn><\/mtd>0<\/mn><\/mtd><\/mtr><\/mtable>]<\/mo><\/mrow><\/mrow>R=\\begin{bmatrix}\n 0 & sin(\u03c8\u2212\u03d5) & cos(\u03c8\u2212\u03d5)\\\\\n 0 & cos(\u03c8\u2212\u03d5) & \u2212sin(\u03c8\u2212\u03d5)\\\\\n -1 & 0 &0\n\\end{bmatrix}<\/annotation><\/semantics><\/math><\/div>\n\n\n\n
<\/p>\n\n\n\n
\u6b64\u65f6\u77e9\u9635\u4e2d\u53ea\u6709yaw – roll\u7684\u7ec4\u5408\uff0c\u6ca1\u6709\u5b83\u4eec\u5404\u81ea\u5355\u72ec\u7684\u9879\uff0c\u4e5f\u5c31\u662f\u8bf4\u6700\u7ec8\u65cb\u8f6c\u51fa\u6765\u7684\u72b6\u6001\u53ea\u8ddf\u5b83\u4fe9\u7684\u5dee\u503c<\/strong>\u6709\u5173\u4e86\uff0c\u5982\u8fd9\u79cd\u60c5\u51b5\u4e0byaw = 0\u00b0\uff0croll = 30\u00b0\u548cyaw = 30\u00b0\uff0croll = 60\u00b0\u5c06\u4f1a\u7b49\u4ef7\uff0c\u5bfc\u81f4\u5bf9\u4e8e\u4e00\u4e2a\u65cb\u8f6c\u77e9\u9635\uff0c\u65e0\u6cd5\u552f\u4e00<\/strong>\u53cd\u89e3\u51fa\u4e00\u7ec4\u6b27\u62c9\u65cb\u8f6c\u53c2\u6570\u3002\u4f46\u6211\u7684\u8fd9\u4e2a\u4f8b\u5b50\u4e2d\u6700\u540e\u5f97\u5230\u7684\u9006\u53d8\u6362\u4e2dpitch\u662f-50\u00b0\u6240\u4ee5\u6ca1\u89e6\u53d1\u8fd9\u4e2a\u4e1c\u897f\u3002<\/p>\n\n\n\n
\u60f3\u907f\u514d\u4e07\u5411\u9501\u7684\u95ee\u9898\u5c31\u8981\u7528\u5230\u56db\u5143\u6570\u2014\u2014\u2014\u2014\u2014\u2014\u2014\u2014\u2014\u6211\u8fd8\u6ca1\u7528\u5230\u6240\u4ee5\u4e0d\u8bb2\u4e86\uff08\u4e0d\u4f1a\u5c31\u76f4\u8bf4\uff09\uff0c\u563b\u563b>\u03c9<<\/p>\n","protected":false},"excerpt":{"rendered":"
ROS2\u5750\u6807\u7cfb\u548cTF\u53c2\u6570 ROS2\u9759\u6001\u5750\u6807\u8f6c\u6362\uff08static_transform...<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[133],"tags":[91,140,139,137,138],"class_list":["post-1562","post","type-post","status-publish","format-standard","hentry","category-ros2-drone-sensor","tag-ros2","tag-tf","tag-139","tag-137","tag-138"],"_links":{"self":[{"href":"https:\/\/www.kakosci.com\/index.php\/wp-json\/wp\/v2\/posts\/1562","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/www.kakosci.com\/index.php\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/www.kakosci.com\/index.php\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/www.kakosci.com\/index.php\/wp-json\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/www.kakosci.com\/index.php\/wp-json\/wp\/v2\/comments?post=1562"}],"version-history":[{"count":16,"href":"https:\/\/www.kakosci.com\/index.php\/wp-json\/wp\/v2\/posts\/1562\/revisions"}],"predecessor-version":[{"id":1579,"href":"https:\/\/www.kakosci.com\/index.php\/wp-json\/wp\/v2\/posts\/1562\/revisions\/1579"}],"wp:attachment":[{"href":"https:\/\/www.kakosci.com\/index.php\/wp-json\/wp\/v2\/media?parent=1562"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/www.kakosci.com\/index.php\/wp-json\/wp\/v2\/categories?post=1562"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/www.kakosci.com\/index.php\/wp-json\/wp\/v2\/tags?post=1562"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}