{"id":28708,"date":"2025-05-20T17:31:22","date_gmt":"2025-05-20T09:31:22","guid":{"rendered":"http:\/\/192.168.10.115\/?p=28708"},"modified":"2025-05-20T17:31:22","modified_gmt":"2025-05-20T09:31:22","slug":"2025-05-20-%e8%87%aa%e5%8a%a8%e9%a9%be%e9%a9%b6%e4%b8%ad%e5%9d%90%e6%a0%87%e5%8f%98%e5%8c%96%e5%9f%ba%e7%a1%80%e7%90%86%e8%ae%ba","status":"publish","type":"post","link":"http:\/\/222.128.65.89:18086\/index.php\/2025\/05\/20\/28708\/","title":{"rendered":"2025-05-20 \u81ea\u52a8\u9a7e\u9a76\u4e2d\u5750\u6807\u53d8\u5316\u57fa\u7840\u7406\u8bba"},"content":{"rendered":"\n<p>\u539f\u6587\u94fe\u63a5\uff1a<a href=\"https:\/\/www.toutiao.com\/article\/7505704799000232460\/?channel=&amp;source=news\">https:\/\/www.toutiao.com\/article\/7505704799000232460\/?channel=&amp;source=news<\/a><\/p>\n\n\n\n<p>\u5728\u81ea\u52a8\u9a7e\u9a76\u4e2d\u5b58\u5728\u591a\u79cd\u5750\u6807\u7cfb\u8868\u793a\u65b9\u5f0f\uff0c\u4e5f\u7ecf\u5e38\u9700\u8981\u7528\u5230\u5750\u6807\u7cfb\u53d8\u5316\uff0c\u672c\u6587\u5bf9\u6b64\u505a\u4e00\u4e2a\u603b\u7ed3\u3002<\/p>\n\n\n\n<h1 class=\"wp-block-heading\">&#8211;1 \u5750\u6807\u7cfb\u5b9a\u4e49\uff1a<\/h1>\n\n\n\n<p>\u81ea\u52a8\u9a7e\u9a76\u4e2d\u6211\u4eec\u4f1a\u7528\u5230\u54ea\u4e9b\u5750\u6807\u7cfb\u5462\uff1f<\/p>\n\n\n\n<p>\uff081\uff09\u4e16\u754c\u5750\u6807\u7cfb<\/p>\n\n\n\n<p>\u5730\u7406\u5750\u6807\u7cfb\uff1aWGS84 \u662f\u5730\u5fc3\u5750\u6807\u7cfb \u5373\u7ecf\u7eac\u5ea6\u3001\u6d77\u62d4 \u5355\u4f4d\u5ea6<\/p>\n\n\n\n<p>\u6295\u5f71\u5750\u6807\u7cfb\uff1aUTM \u901a\u7528\u6a2a\u8f74\u58a8\u5361\u6258\u6295\u5f71\u5750\u6807 \u5355\u4f4d\u662f\u7c73<\/p>\n\n\n\n<p>\uff082\uff09\u8f66\u4f53\u5750\u6807\u7cfb<\/p>\n\n\n\n<p>\u7528\u4e8e\u63cf\u8ff0\u8f66\u8f86\u5468\u56f4\u548c\u672c\u8f66\u76f8\u5bf9\u4f4d\u7f6e\u5173\u7cfb\uff0c\u53f3\u624b\u5b9a\u5219\uff0c\u524d\u5411x \u5de6\u4fa7y \u4e0a\u65b9z\u3002<\/p>\n\n\n\n<figure class=\"wp-block-image\"><img decoding=\"async\" src=\"https:\/\/p3-sign.toutiaoimg.com\/tos-cn-i-axegupay5k\/a9cfee351400446f943f1193988639af~tplv-tt-origin-web:gif.jpeg?_iz=58558&amp;from=article.pc_detail&amp;lk3s=953192f4&amp;x-expires=1748239187&amp;x-signature=DmsmH5lmOwRCFShaw681VOPwya8%3D\" alt=\"\"\/><\/figure>\n\n\n\n<p>\uff083\uff09Frenet\u5750\u6807\u7cfb<\/p>\n\n\n\n<p>\u5728 Frenet \u5750\u6807\u7cfb\u4e2d\uff0cs \u4ee3\u8868\u6cbf\u9053\u8def\u7684\u8ddd\u79bb\u79f0\u4e3a\u7eb5\u5750\u6807\uff0cd\u8868\u793a\u4e0e\u7eb5\u5411\u7ebf\u7684\u4f4d\u79fb\u79f0\u4e3a\u6a2a\u5750\u6807\u3002<\/p>\n\n\n\n<figure class=\"wp-block-image\"><img decoding=\"async\" src=\"https:\/\/p3-sign.toutiaoimg.com\/tos-cn-i-6w9my0ksvp\/638098cc8b4f4fad91b410b282fa3851~tplv-tt-origin-web:gif.jpeg?_iz=58558&amp;from=article.pc_detail&amp;lk3s=953192f4&amp;x-expires=1748239187&amp;x-signature=r3TTXEPM2vfNtSG3t24lHtr1i0s%3D\" alt=\"\"\/><\/figure>\n\n\n\n<p>\uff084\uff09\u76f8\u673a\u76f8\u5173\u5750\u6807\u7cfb<\/p>\n\n\n\n<p>\u76f8\u673a\u5750\u6807\u7cfb\uff1a\u4e00\u4e2a\u4e09\u7ef4\u76f4\u89d2\u5750\u6807\u7cfb\uff0c\u539f\u70b9\u4f4d\u4e8e\u955c\u5934\u7684\u5149\u5fc3\u5904\uff0cx,y\u5206\u522b\u4e0e\u50cf\u9762\u7684\u4e24\u8fb9\u5e73\u884c\uff0cZ\u8f74\u4e3a\u955c\u5934\u5149\u8f74\uff0c\u4e0e\u50cf\u9762\u5782\u76f4\u3002<\/p>\n\n\n\n<p>\u56fe\u50cf\u5750\u6807\u7cfb\uff1a\u4e00\u4e2a\u4e8c\u7ef4\u76f4\u89d2\u5750\u6807\u7cfb\uff0c\u539f\u70b9\u662f\u5149\u8f74\u4e0e\u50cf\u9762\u7684\u4ea4\u70b9\uff08\u53c8\u79f0\u4e3b\u70b9\uff09\uff0c\u5373\u56fe\u50cf\u7684\u4e2d\u5fc3\u70b9\uff0cx,y\u5206\u522b\u4e0e\u50cf\u9762\u7684\u4e24\u8fb9\u5e73\u884c\u3002<\/p>\n\n\n\n<p>\u50cf\u7d20\u5750\u6807\u7cfb\uff1a\u4e00\u4e2a\u4e8c\u7ef4\u76f4\u89d2\u5750\u6807\u7cfb\uff0c\u53cd\u5e94\u4e86\u76f8\u673a\uff08CCD\/CMOS\uff09\u82af\u7247\u4e2d\u50cf\u7d20\u7684\u6392\u5217\u60c5\u51b5\u3002\u539f\u70b9\u4f4d\u4e8e\u56fe\u50cf\u7684\u5de6\u4e0a\u89d2\uff0c\u4e0bx,y\u5206\u522b\u5e73\u884c\u4e0e\u50cf\u9762\u3002\u50cf\u7d20\u5750\u6807\u4e0e\u56fe\u50cf\u5750\u6807\u5b9e\u9645\u662f\u5e73\u79fb\u7684\u5173\u7cfb\uff08\u4e0b\u9762\u4f1a\u8be6\u7ec6\u63cf\u8ff0\uff09\u3002\u50cf\u7d20\u5750\u6807\u4e2d\u5750\u6807\u8f74\u5355\u4f4d\u4e3a\u50cf\u7d20\u3002<\/p>\n\n\n\n<p>\uff085\uff09\u6fc0\u5149\u96f7\u8fbe\u5750\u6807\u7cfb\uff1a\u7403\u5750\u6807\u7cfb\uff0c\u5f84\u5411\u8ddd\u79bb <em>r <\/em>\u4fef\u4ef0\u89d2 <em>\u03b8 <\/em>\u65b9\u4f4d\u89d2 <em>\u03d5 <\/em>\u7ec4\u6210\u3002<\/p>\n\n\n\n<figure class=\"wp-block-image\"><img decoding=\"async\" src=\"https:\/\/p3-sign.toutiaoimg.com\/tos-cn-i-6w9my0ksvp\/75b32fc1c2304065a16ffec0bb592887~tplv-tt-origin-web:gif.jpeg?_iz=58558&amp;from=article.pc_detail&amp;lk3s=953192f4&amp;x-expires=1748239187&amp;x-signature=2E4fjLNofRdcQEAEF6sKJBdiivY%3D\" alt=\"\"\/><\/figure>\n\n\n\n<figure class=\"wp-block-image\"><img decoding=\"async\" src=\"https:\/\/p3-sign.toutiaoimg.com\/tos-cn-i-6w9my0ksvp\/316fd5fbcb3e45a78e88395aa74c378b~tplv-tt-origin-web:gif.jpeg?_iz=58558&amp;from=article.pc_detail&amp;lk3s=953192f4&amp;x-expires=1748239187&amp;x-signature=ONtUsZsEcExZRqwgRgFbbYs%2BF00%3D\" alt=\"\"\/><\/figure>\n\n\n\n<h1 class=\"wp-block-heading\">&#8211;2 \u5750\u6807\u7cfb\u53d8\u5316\u7684\u5e38\u7528\u64cd\u4f5c\uff1a<\/h1>\n\n\n\n<p>\u8fd9\u91cc\u4ecb\u7ecd\u5e38\u89c1\u7684\u4e24\u79cd\uff1a<\/p>\n\n\n\n<p>\uff081\uff09\u70b9\u7684\u65cb\u8f6c\u4e0e\u5e73\u79fb \uff08\u4e3b\u52a8\u53d8\u6362\uff09<\/p>\n\n\n\n<p>\uff082\uff09\u70b9\u7684\u4f4d\u7f6e\u4e0d\u53d8\uff0c\u5176\u76f8\u5bf9\u7684\u5750\u6807\u7cfb\u65cb\u8f6c\u4e0e\u5e73\u79fb \uff08\u88ab\u52a8\u53d8\u6362\uff09<\/p>\n\n\n\n<h1 class=\"wp-block-heading\">&#8211;3 \u70b9\u53d8\u6362\u6570\u5b66\u8868\u8fbe\uff1a<\/h1>\n\n\n\n<p>\uff081\uff09\u57fa\u4e8e\u51fd\u6570\u65b9\u7a0b\u8fdb\u884c\u8868\u8fbe\uff1a<\/p>\n\n\n\n<ul class=\"wp-block-list\">\n<li>\u539f\u59cb\u70b9 <em>P<\/em>=(<em>x<\/em>,<em>y<\/em>)<\/li>\n\n\n\n<li>\u7ed5\u539f\u70b9\u9006\u65f6\u9488\u65cb\u8f6c\u89d2\u5ea6 <em>\u03b8<\/em><\/li>\n\n\n\n<li>\u518d\u5e73\u79fb\u5411\u91cf <em>t<\/em>=(<em>tx<\/em>,<em>ty<\/em>)<\/li>\n\n\n\n<li>\u5f97\u5230\u65b0\u70b9 <em>P<\/em>\u2032=(<em>x<\/em>\u2032,<em>y<\/em>\u2032)<\/li>\n<\/ul>\n\n\n\n<figure class=\"wp-block-image\"><img decoding=\"async\" src=\"https:\/\/p3-sign.toutiaoimg.com\/tos-cn-i-6w9my0ksvp\/6d5d5780459742f1bdd90473009c42ae~tplv-tt-origin-web:gif.jpeg?_iz=58558&amp;from=article.pc_detail&amp;lk3s=953192f4&amp;x-expires=1748239187&amp;x-signature=PjfzijBBPUS3FYrcY85ZMuQCVs4%3D\" alt=\"\"\/><\/figure>\n\n\n\n<p>\uff082\uff09\u57fa\u4e8e\u53d8\u6362\u77e9\u9635\u8fdb\u884c\u8868\u8fbe<\/p>\n\n\n\n<p>\u4f7f\u7528\u9f50\u6b21\u5750\u6807\uff0c\u53ef\u4ee5\u5c06\u65cb\u8f6c\u548c\u5e73\u79fb\u7edf\u4e00\u4e3a\u4e00\u4e2a\u77e9\u9635\u4e58\u6cd5\uff1a<\/p>\n\n\n\n<figure class=\"wp-block-image\"><img decoding=\"async\" src=\"https:\/\/p26-sign.toutiaoimg.com\/tos-cn-i-6w9my0ksvp\/2d929cdaa2214ef789b5371e4b5e24b3~tplv-tt-origin-web:gif.jpeg?_iz=58558&amp;from=article.pc_detail&amp;lk3s=953192f4&amp;x-expires=1748239187&amp;x-signature=KJRkwpSVqDPZ01oP9eT4iVs9jlU%3D\" alt=\"\"\/><\/figure>\n\n\n\n<h1 class=\"wp-block-heading\">&#8211;4 \u5750\u6807\u7cfb\u53d8\u6362\u6570\u5b66\u8868\u8fbe\uff1a<\/h1>\n\n\n\n<p>\uff081\uff09\u57fa\u4e8e\u51fd\u6570\u65b9\u7a0b\u8fdb\u884c\u8868\u8fbe\uff1a<\/p>\n\n\n\n<ul class=\"wp-block-list\">\n<li>\u70b9 <em>P<\/em> \u7684\u5b9e\u9645\u4f4d\u7f6e\u4e0d\u53d8\uff1b<\/li>\n\n\n\n<li>\u65b0\u5750\u6807\u7cfb\u7531\u539f\u5750\u6807\u7cfb\u7ed5\u539f\u70b9\u9006\u65f6\u9488\u65cb\u8f6c <em>\u03b8<\/em>\uff0c\u5e76\u5e73\u79fb\u5230 <em>t<\/em>=(<em>tx<\/em>,<em>ty<\/em>)<\/li>\n\n\n\n<li>\u6c42\u70b9 <em>P<\/em> \u5728\u65b0\u5750\u6807\u7cfb\u4e0b\u7684\u5750\u6807 (<em>x<\/em>\u2032,<em>y<\/em>\u2032)<\/li>\n<\/ul>\n\n\n\n<figure class=\"wp-block-image\"><img decoding=\"async\" src=\"https:\/\/p3-sign.toutiaoimg.com\/tos-cn-i-6w9my0ksvp\/f745a1d1ec9c49d681c74c3044a2bfe3~tplv-tt-origin-web:gif.jpeg?_iz=58558&amp;from=article.pc_detail&amp;lk3s=953192f4&amp;x-expires=1748239187&amp;x-signature=TEqCKS9r8RSkCGh%2FnBGUoZ63cXY%3D\" alt=\"\"\/><\/figure>\n\n\n\n<p>\uff082\uff09\u57fa\u4e8e\u53d8\u6362\u77e9\u9635\u8fdb\u884c\u8868\u8fbe\uff1a<\/p>\n\n\n\n<figure class=\"wp-block-image\"><img decoding=\"async\" src=\"https:\/\/p3-sign.toutiaoimg.com\/tos-cn-i-6w9my0ksvp\/893d35046b3c46888f371f9c4c945352~tplv-tt-origin-web:gif.jpeg?_iz=58558&amp;from=article.pc_detail&amp;lk3s=953192f4&amp;x-expires=1748239187&amp;x-signature=9ygsmofiWhHm36x16IvJJd5JWFY%3D\" alt=\"\"\/><\/figure>\n\n\n\n<h1 class=\"wp-block-heading\">&#8211;5 \u8f66\u4f53\u5750\u6807\u7cfb\u548c\u4e16\u754c\u5750\u6807\u53d8\u6362\uff1a<\/h1>\n\n\n\n<p>\u5750\u6807\u7cfb\u53d8\u6362\u5176\u5b9e\u4e0d\u592a\u597d\u7406\u89e3\u4ec0\u4e48\u65f6\u5019\u5e94\u8be5\u5148\u65cb\u8f6c\u518d\u5e73\u79fb\uff0c\u4ec0\u4e48\u65f6\u5019\u5e94\u8be5\u5148\u5e73\u79fb\u518d\u65cb\u8f6c\uff0c\u8fd9\u91cc\u8ddd\u79bb\u81ea\u52a8\u9a7e\u9a76\u4e2d\u5b9e\u9645\u5e94\u8be5\u7684\u8f66\u4f53\u5750\u6807\u5230\u4e16\u754c\u5750\u6807\u7cfb\u53d8\u6362\u7684\u6848\u4f8b\u6765\u5e2e\u52a9\u7406\u89e3\u3002<\/p>\n\n\n\n<figure class=\"wp-block-image\"><img decoding=\"async\" src=\"https:\/\/p3-sign.toutiaoimg.com\/tos-cn-i-6w9my0ksvp\/0471e3aa581741f2bb9e97d9eaaa2be5~tplv-tt-origin-web:gif.jpeg?_iz=58558&amp;from=article.pc_detail&amp;lk3s=953192f4&amp;x-expires=1748239187&amp;x-signature=B24c7%2BdM6%2F7YXe8EUq1qP3WrnWY%3D\" alt=\"\"\/><\/figure>\n\n\n\n<p>\u4e0b\u8bc9\u4ee3\u7801\u5c55\u793a\u4e86\uff0c\u5c06\u8f66\u4f53\u5750\u6807\u7cfb\u8f6c\u5230\u4e16\u754c\u5750\u6807\u7cfb\u4ee5\u53ca\u5c06\u4e16\u754c\u5750\u6807\u7cfb\u518d\u8f6c\u56de\u8f66\u4f53\u5750\u6807\u7cfb\u7684\u8fc7\u7a0b\u3002<\/p>\n\n\n\n<pre class=\"wp-block-code\"><code>import numpy as np\nimport matplotlib.pyplot as plt\nfrom matplotlib.patches import Rectangle, FancyArrow\n\n# \u5b9a\u4e49\u4e3b\u8f66\u548c\u76ee\u6807\u8f66\u7684\u53c2\u6570\nmain_car_length = 4  # \u4e3b\u8f66\u957f\u5ea6\nmain_car_width = 2  # \u4e3b\u8f66\u5bbd\u5ea6\nmain_car_dx = 1  # \u4e3b\u8f66\u5728\u4e16\u754c\u5750\u6807\u7cfb\u4e2d\u7684x\u4f4d\u7f6e\nmain_car_dy = 1  # \u4e3b\u8f66\u5728\u4e16\u754c\u5750\u6807\u7cfb\u4e2d\u7684y\u4f4d\u7f6e\nmain_car_theta = 30  # \u4e3b\u8f66\u5728\u4e16\u754c\u5750\u6807\u7cfb\u4e2d\u7684\u671d\u5411\u89d2\u5ea6\uff08\u5ea6\uff09\n\ntarget_car_length = 4  # \u76ee\u6807\u8f66\u957f\u5ea6\ntarget_car_width = 2  # \u76ee\u6807\u8f66\u5bbd\u5ea6\ntarget_car_dx = 10  # \u76ee\u6807\u8f66\u5728\u8f66\u4f53\u5750\u6807\u7cfb\u4e2d\u7684x\u4f4d\u7f6e\ntarget_car_dy = 5  # \u76ee\u6807\u8f66\u5728\u8f66\u4f53\u5750\u6807\u7cfb\u4e2d\u7684y\u4f4d\u7f6e\ntarget_car_theta = 30  # \u76ee\u6807\u8f66\u5728\u8f66\u4f53\u5750\u6807\u7cfb\u4e2d\u7684\u671d\u5411\u89d2\u5ea6\uff08\u5ea6\uff09\n\n\n# \u5c06\u76ee\u6807\u8f66\u4ece\u8f66\u4f53\u5750\u6807\u7cfb\u8f6c\u6362\u5230\u4e16\u754c\u5750\u6807\u7cfb\ndef car_body_to_world(dx, dy, theta, main_car_dx, main_car_dy, main_car_theta):\n    # \u5c06\u89d2\u5ea6\u8f6c\u6362\u4e3a\u5f27\u5ea6\n    theta_rad = np.radians(theta)\n    main_car_theta_rad = np.radians(main_car_theta)\n\n    # \u65cb\u8f6c\u77e9\u9635\n    rotation_matrix = np.array(\n        &#91;\n            &#91;np.cos(main_car_theta_rad), -np.sin(main_car_theta_rad)],\n            &#91;np.sin(main_car_theta_rad), np.cos(main_car_theta_rad)],\n        ]\n    )\n\n    # \u76ee\u6807\u8f66\u5728\u8f66\u4f53\u5750\u6807\u7cfb\u4e2d\u7684\u4f4d\u7f6e\n    target_car_position_body = np.array(&#91;dx, dy])\n\n    # \u65cb\u8f6c\n    target_car_position_rel = rotation_matrix @ target_car_position_body\n\n    # \u5e73\u79fb\n    target_car_position_world = target_car_position_rel + np.array(&#91;main_car_dx, main_car_dy])\n\n    # \u76ee\u6807\u8f66\u5728\u4e16\u754c\u5750\u6807\u7cfb\u4e2d\u7684\u671d\u5411\u89d2\u5ea6\n    target_car_theta_world = theta + main_car_theta\n\n    return target_car_position_world, target_car_theta_world\n\n\n# \u5c06\u76ee\u6807\u8f66\u4ece\u4e16\u754c\u5750\u6807\u7cfb\u8f6c\u6362\u5230\u8f66\u4f53\u5750\u6807\u7cfb\ndef car_world_to_body(x_w, y_w, theta_w, main_car_dx, main_car_dy, main_car_theta):\n    # \u5c06\u89d2\u5ea6\u8f6c\u6362\u4e3a\u5f27\u5ea6\n    main_car_theta_rad = np.radians(main_car_theta)\n\n    # \u9006\u65cb\u8f6c\u77e9\u9635\n    inverse_rotation_matrix = np.array(\n        &#91;\n            &#91;np.cos(main_car_theta_rad), np.sin(main_car_theta_rad)],\n            &#91;-np.sin(main_car_theta_rad), np.cos(main_car_theta_rad)],\n        ]\n    )\n\n    # \u76ee\u6807\u8f66\u5728\u4e16\u754c\u5750\u6807\u7cfb\u4e2d\u7684\u4f4d\u7f6e\n    target_car_position_world = np.array(&#91;x_w, y_w])\n\n    # \u5e73\u79fb\n    target_car_position_rel = target_car_position_world - np.array(&#91;main_car_dx, main_car_dy])\n\n    # \u9006\u65cb\u8f6c\n    target_car_position_body = inverse_rotation_matrix @ target_car_position_rel\n\n    # \u76ee\u6807\u8f66\u5728\u8f66\u4f53\u5750\u6807\u7cfb\u4e2d\u7684\u671d\u5411\u89d2\u5ea6\n    target_car_theta_body = theta_w - main_car_theta\n\n    return target_car_position_body, target_car_theta_body\n\n\n# \u7ed8\u5236\u8f66\u8f86\ndef draw_car(ax, position, length, width, theta, color=\"blue\"):\n    # \u5c06\u89d2\u5ea6\u8f6c\u6362\u4e3a\u5f27\u5ea6\n    theta_rad = np.radians(theta)\n\n    # \u8ba1\u7b97\u77e9\u5f62\u7684\u56db\u4e2a\u89d2\n    corners = np.array(\n        &#91;&#91;-length \/ 2, -width \/ 2], &#91;length \/ 2, -width \/ 2], &#91;length \/ 2, width \/ 2], &#91;-length \/ 2, width \/ 2]]\n    )\n\n    # \u65cb\u8f6c\u77e9\u5f62\n    rotation_matrix = np.array(&#91;&#91;np.cos(theta_rad), -np.sin(theta_rad)], &#91;np.sin(theta_rad), np.cos(theta_rad)]])\n    rotated_corners = corners @ rotation_matrix.T\n\n    # \u5e73\u79fb\u77e9\u5f62\n    translated_corners = rotated_corners + position\n\n    # \u7ed8\u5236\u77e9\u5f62\n    rect = Rectangle(position - np.array(&#91;length \/ 2, width \/ 2]), length, width, angle=theta, color=color, fill=False)\n    ax.add_patch(rect)\n\n    # \u7ed8\u5236\u822a\u5411\u7bad\u5934\n    arrow_length = 1.5\n    arrow_dx = arrow_length * np.cos(theta_rad)\n    arrow_dy = arrow_length * np.sin(theta_rad)\n    arrow = FancyArrow(position&#91;0], position&#91;1], arrow_dx, arrow_dy, color=color, width=0.2)\n    ax.add_patch(arrow)\n\n    # \u6807\u6ce8\u8f66\u8f86\u4f4d\u7f6e\u548c\u822a\u5411\n    ax.text(\n        position&#91;0],\n        position&#91;1] - 1,\n        f\"({position&#91;0]:.2f}, {position&#91;1]:.2f})\\n\u03b8={theta:.2f}\u00b0\",\n        fontsize=12,\n        color=color,\n    )\n\n\n# \u521b\u5efa\u56fe\u5f62\u548c\u8f74\nfig, ax = plt.subplots(figsize=(10, 10))\nax.set_xlim(-20, 20)\nax.set_ylim(-20, 20)\nax.set_aspect(\"equal\")\nax.grid(True)\n\n# \u7ed8\u5236\u4e3b\u8f66\nmain_car_position = np.array(&#91;main_car_dx, main_car_dy])\ndraw_car(ax, main_car_position, main_car_length, main_car_width, main_car_theta, color=\"red\")\n\n# \u8f66\u4f53\u5750\u6807\u7cfb\u5230\u4e16\u754c\u5750\u6807\u7cfb\ntarget_car_position_world, target_car_theta_world = car_body_to_world(\n    target_car_dx, target_car_dy, target_car_theta, main_car_dx, main_car_dy, main_car_theta\n)\ndraw_car(ax, target_car_position_world, target_car_length, target_car_width, target_car_theta_world, color=\"blue\")\n\n# \u4e16\u754c\u5750\u6807\u7cfb\u5230\u8f66\u4f53\u5750\u6807\u7cfb\ntarget_car_position_body, target_car_theta_body = car_world_to_body(\n    target_car_position_world&#91;0],\n    target_car_position_world&#91;1],\n    target_car_theta_world,\n    main_car_dx,\n    main_car_dy,\n    main_car_theta,\n)\ndraw_car(ax, target_car_position_body, target_car_length, target_car_width, target_car_theta_body, color=\"gray\")\n\n# \u6253\u5370\u4e3b\u8f66\u548c\u76ee\u6807\u8f66\u7684\u4fe1\u606f\nprint(f\"\u4e3b\u8f66\u7684\u4e16\u754c\u5750\u6807: ({main_car_dx}, {main_car_dy})\")\nprint(f\"\u4e3b\u8f66\u7684\u822a\u5411: {main_car_theta}\u00b0\")\nprint(f\"\u76ee\u6807\u8f66\u5728\u8f66\u4f53\u5750\u6807\u7cfb\u4e0b\u7684\u5750\u6807: ({target_car_dx}, {target_car_dy})\")\nprint(f\"\u76ee\u6807\u8f66\u5728\u8f66\u4f53\u5750\u6807\u7cfb\u4e0b\u7684\u822a\u5411: {target_car_theta}\u00b0\")\nprint(f\"\u76ee\u6807\u8f66\u5728\u4e16\u754c\u5750\u6807\u7cfb\u4e0b\u7684\u5750\u6807: ({target_car_position_world&#91;0]:.2f}, {target_car_position_world&#91;1]:.2f})\")\nprint(f\"\u76ee\u6807\u8f66\u5728\u4e16\u754c\u5750\u6807\u7cfb\u4e0b\u7684\u822a\u5411: {target_car_theta_world:.2f}\u00b0\")\n\n# \u6dfb\u52a0\u56fe\u4f8b\nax.legend(&#91;\"Main Car\", \"Target Car in World\", \"Target Car in Body\"])\n\n# \u663e\u793a\u56fe\u5f62\nplt.show()<\/code><\/pre>\n\n\n\n<p>&#8211;5.1 \u8f66\u4f53\u5750\u6807\u7cfb\u4e0b\u7684\u76ee\u6807\u53d8\u6362\u5230\u4e16\u754c\u5750\u6807\u7cfb\u4e0b\u8868\u8fbe\uff1a\u5148\u65cb\u8f6c\u518d\u5e73\u79fb\uff0c\u56e0\u4e3a\u4f60\u77e5\u9053\u7684\u662f\u4e3b\u8f66\u5728\u4e16\u754c\u5750\u6807\u7cfb\u4e0b\u7684\u5750\u6807\uff0c\u8fd9\u4e2a\u65f6\u5019\u4f60\u5e0c\u671b\u5c06\u76ee\u6807\u8f66\u6240\u5c5e\u7684\u8f66\u4f53\u5750\u6807\u7cfb\u65cb\u8f6c\u53d8\u6362\u5230\u4e16\u754c\u5750\u6807\u7cfb\u4e0b\uff0c\u7ed3\u5408\u4f60\u77e5\u9053\u7684\u4fe1\u606f\uff0c\u4f60\u9700\u8981\u5148\u65cb\u8f6c\u5230\u548c\u4e16\u754c\u5750\u6807\u7cfb\u4e00\u6837\u7684\u671d\u5411\uff0c\u7136\u540e\u518d\u6839\u636e\u4e3b\u8f66\u7684\u8f66\u4f53\u5750\u6807\u8fdb\u884c\u5e73\u79fb\u3002<\/p>\n\n\n\n<p>&#8211;5.2 \u4e16\u754c\u5750\u6807\u7cfb\u4e0b\u7684\u76ee\u6807\u8981\u5728\u8f66\u4f53\u5750\u6807\u7cfb\u4e0b\u8868\u8fbe\uff1a\u5148\u5e73\u79fb\u518d\u65cb\u8f6c\uff0c\u56e0\u4e3a\u4f60\u77e5\u9053\u7684\u662f\u4e3b\u8f66\u5728\u4e16\u754c\u5750\u6807\u7cfb\u4e0b\u7684\u5750\u6807\uff0c\u8fd9\u4e2a\u65f6\u5019\u4f60\u5e0c\u671b\u5c06\u76ee\u6807\u8f66\u6240\u5c5e\u7684\u4e16\u754c\u5750\u6807\u7cfb\u65cb\u8f6c\u53d8\u6362\u5230\u4e3b\u8f66\u4e0b\uff0c\u7ed3\u5408\u4f60\u77e5\u9053\u7684\u4fe1\u606f\uff0c\u5982\u679c\u4f60\u5148\u65cb\u8f6c\uff0c\u90a3\u4f60\u65cb\u8f6c\u4e4b\u540e\u7684\u9700\u8981\u7684\u5e73\u79fb\u503c\u5176\u5b9e\u4f60\u662f\u4e0d\u77e5\u9053\u7684\uff0c\u5b83\u548c\u4f60\u76ee\u524d\u77e5\u9053\u7684\u4e3b\u8f66\u5750\u6807\u4e0d\u662f\u4e00\u4e2a\u6570\uff0c\u56e0\u6b64\u4f60\u53ea\u80fd\u5148\u7528\u73b0\u5728\u77e5\u9053\u7684\u4e3b\u8f66\u5750\u6807\u5148\u5e73\u79fb\u518d\u65cb\u8f6c\u3002<\/p>\n\n\n\n<p>\u7efc\u4e0a\uff1a<\/p>\n\n\n\n<ol start=\"1\" class=\"wp-block-list\">\n<li>\u65cb\u8f6c + \u5e73\u79fb\u90fd\u662f\u5728\u73b0\u6709\u5750\u6807\u7cfb\u4e0b\u8fdb\u884c\u7684<\/li>\n\n\n\n<li>\u65cb\u8f6c\u548c\u5e73\u79fb\u7684\u987a\u5e8f\uff0c\u53d6\u51b3\u4e8e\u4f60\u77e5\u9053\u7684\u5e73\u79fb\u5750\u6807\u662f\u54ea\u4e2a\u5750\u6807\u7cfb\u76f8\u5bf9\u4e8e\u54ea\u4e2a\u5750\u6807\u7cfb\u7684<\/li>\n<\/ol>\n","protected":false},"excerpt":{"rendered":"<p>\u539f\u6587\u94fe\u63a5\uff1ahttps:\/\/www.toutia [&hellip;]<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[27,1],"tags":[],"class_list":["post-28708","post","type-post","status-publish","format-standard","hentry","category-industry-trends","category-home"],"views":0,"_links":{"self":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/28708"}],"collection":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/comments?post=28708"}],"version-history":[{"count":2,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/28708\/revisions"}],"predecessor-version":[{"id":28739,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/28708\/revisions\/28739"}],"wp:attachment":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/media?parent=28708"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/categories?post=28708"},{"taxonomy":"post_tag","embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/tags?post=28708"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}