{"id":83533,"date":"2026-07-26T01:17:24","date_gmt":"2026-07-25T17:17:24","guid":{"rendered":"https:\/\/www.wsisp.com\/helps\/83533.html"},"modified":"2026-07-26T01:17:24","modified_gmt":"2026-07-25T17:17:24","slug":"%e3%80%9010%e5%a4%a9%e9%80%9f%e9%80%9aros2-px4%e6%97%a0%e4%ba%ba%e6%9c%ba%e3%80%91%e4%ba%8c-fast-lio2-%e4%bb%bf%e7%9c%9f%e9%83%a8%e7%bd%b2%ef%bc%8c200hz-imu%e5%89%8d%e6%8e%a8%e7%9a%84%e6%97%a0","status":"publish","type":"post","link":"https:\/\/www.wsisp.com\/helps\/83533.html","title":{"rendered":"\u301010\u5929\u901f\u901aROS2-PX4\u65e0\u4eba\u673a\u3011(\u4e8c) FAST-LIO2 \u4eff\u771f\u90e8\u7f72\uff0c200HZ IMU\u524d\u63a8\u7684\u65e0\u9650\u9b45\u529b"},"content":{"rendered":"<h3>\u524d\u8a00<\/h3>\n<ul>\n<li>\u4e0a\u4e00\u671f\u6211\u4eec\u82b1\u4e86\u5f88\u5927\u7bc7\u5e45&#xff0c;\u4ece\u96f6\u642d\u5efa\u4e86\u4e00\u5957\u5b8c\u6574\u7684 ROS2 Humble &#043; PX4 SITL &#043; Gazebo Classic \u4eff\u771f\u73af\u5883&#xff1a;\n<ul>\n<li>\u301010\u5929\u901f\u901aROS2-PX4\u65e0\u4eba\u673a\u3011(\u4e00) \u4ece\u96f6\u642d\u5efa\u4eff\u771f\u73af\u5883&#xff0c;\u5988\u5988\u518d\u4e5f\u4e0d\u7528\u62c5\u5fc3\u6211\u70b8\u673a\u4e86&#xff01;<\/li>\n<\/ul>\n<\/li>\n<li>\u73b0\u5728\u4eff\u771f\u6709\u4e86\u3001VLP-16 3D \u6fc0\u5149\u96f7\u8fbe\u6709\u4e86\u3001IMU \u6709\u4e86\u3001\u7f51\u7b3c\u6d4b\u8bd5\u573a\u6709\u4e86&#xff0c;\u4f46\u98de\u673a\u8fd8\u4e0d\u77e5\u9053\u81ea\u5df1\u5728\u54ea\u91cc\u2014\u2014PX4 \u81ea\u5e26\u7684 EKF \u4f9d\u8d56 GPS&#xff0c;\u5728\u5ba4\u5185\/\u7f51\u7b3c\u8fd9\u79cd GPS \u62d2\u6b62\u73af\u5883\u4e0b\u76f4\u63a5\u6293\u778e<\/li>\n<li>\u524d\u51e0\u671f\u6211\u4eec\u521a\u597d\u4e5f\u5b8c\u6574\u63a8\u5bfc\u4e86 FAST-LIO2 \u548c Point-LIO \u7684\u7b97\u6cd5\u539f\u7406\u4e0e\u6e90\u7801&#xff1a;\n<ul>\n<li>\u3010FAST-LIO2 \u6e90\u7801\u89e3\u8bfb\u3011\u4ece Mid-360 \u5230 ESEKF \u2014 \u516c\u5f0f\u4e0e\u4ee3\u7801\u9010\u884c\u5bf9\u7167<\/li>\n<li>\u3010Point-LIO \u6e90\u7801\u89e3\u8bfb\u3011\u9010\u70b9\u66f4\u65b0\u53d6\u4ee3\u9010\u5e27\u66f4\u65b0 \u2014 \u516c\u5f0f\u4e0e\u4ee3\u7801\u9010\u884c\u5bf9\u7167<\/li>\n<\/ul>\n<\/li>\n<li>\u672c\u671f\u6211\u4eec\u5c31\u628a FAST-LIO2 \u90e8\u7f72\u5230\u4eff\u771f\u73af\u5883\u4e2d&#xff0c;\u8ba9\u98de\u673a\u5728\u7f51\u7b3c\u91cc\u9760 LiDAR &#043; IMU \u5b9e\u65f6\u5efa\u56fe\u5b9a\u4f4d&#xff0c;\u6253\u901a&#034;\u771f\u673a\u90e8\u7f72\u524d\u7684\u6700\u540e\u4e00\u516c\u91cc&#034;<\/li>\n<li>\u5728\u8fdb\u9636\u90e8\u5206&#xff0c;\u6211\u4eec\u4f1a\u53c2\u8003\u4e1c\u5317\u5927\u5b66 REAL_DRONE_400 \u5f00\u6e90\u9879\u76ee&#xff0c;\u7ed9 FAST-LIO2 \u52a0\u4e0a 200Hz IMU \u524d\u63a8\u2014\u2014\u7528\u4e2d\u503c\u79ef\u5206\u5728\u6bcf\u4e24\u6761 IMU \u6d88\u606f\u4e4b\u95f4\u9884\u6d4b\u4e00\u6b21\u4f4d\u59ff&#xff0c;\u628a\u91cc\u7a0b\u8ba1\u8f93\u51fa\u9891\u7387\u4ece LiDAR \u5e27\u7387&#xff08;10Hz&#xff09;\u62c9\u5230 IMU \u5e27\u7387&#xff08;200Hz&#xff09;\u3002\u8fd9\u5bf9 PX4 \u7684 Offboard \u63a7\u5236\u81f3\u5173\u91cd\u8981\u2014\u2014EKF2 \u9700\u8981\u9ad8\u9891\u91cc\u7a0b\u8ba1\u8f93\u5165\u624d\u80fd\u7a33\u5b9a\u98de\u884c<\/li>\n<li>\u6838\u5fc3\u94fe\u8def&#xff1a;hku-mars\/FAST_LIO ROS2 \u5206\u652f &#043; \u6761\u4ef6\u7f16\u8bd1\u53bb\u6389 livox_ros_driver2 &#043; VLP-16 \u4eff\u771f\u914d\u7f6e &#043; camera_init TF \u6811 &#043; 200Hz IMU \u524d\u63a8 &#043; RViz2 \u53ef\u89c6\u5316<\/li>\n<\/ul>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260725171713-6a64ef99d91b1.png\" alt=\"\u5728\u8fd9\u91cc\u63d2\u5165\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<hr \/>\n<h4>\u6587\u7ae0\u76ee\u5f55<\/h4>\n<ul>\n<li>\n<ul>\n<li>\u524d\u8a00<\/li>\n<li>1 FAST-LIO2 \u5feb\u901f\u56de\u987e<\/li>\n<li>\n<ul>\n<li>\n<ul>\n<li>1-1 \u6838\u5fc3\u601d\u8def<\/li>\n<li>1-2 \u4e3a\u4ec0\u4e48\u4e0d\u7528\u7279\u5f81\u63d0\u53d6<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<\/li>\n<li>2 \u7f16\u8bd1 FAST-LIO2<\/li>\n<li>\n<ul>\n<li>\n<ul>\n<li>2-1 \u62c9\u53d6\u4ed3\u5e93<\/li>\n<li>2-2 \u4fee\u6539\u6e90\u7801 \u2014 \u53bb\u6389 livox_ros_driver2 \u786c\u4f9d\u8d56<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<\/li>\n<li>3 \u542f\u52a8\u914d\u7f6e<\/li>\n<li>\n<ul>\n<li>\n<ul>\n<li>3-1 TF \u6811\u8bbe\u8ba1<\/li>\n<li>3-2 \u542f\u52a8\u811a\u672c<\/li>\n<li>3-3 \u8bdd\u9898\u8bf4\u660e<\/li>\n<li>3-4 \u53c2\u6570\u8bf4\u660e<\/li>\n<li>3-5 RViz2 \u53ef\u89c6\u5316<\/li>\n<li>3-6 \u5e38\u89c1\u8b66\u544a&#xff1a;&#096;No point, skip this scan!&#096;<\/li>\n<li>\n<ul>\n<li>3-6-1 IMU \u53bb\u7578\u53d8\u540e\u6ca1\u6709\u6709\u6548\u70b9&#xff08;\u7b2c 1069 \u884c&#xff09;<\/li>\n<li>3-6-2 \u964d\u91c7\u6837\u540e\u6709\u6548\u70b9\u592a\u5c11&#xff08;\u7b2c 1107 \u884c&#xff09;<\/li>\n<\/ul>\n<\/li>\n<li>3-7 \u5b9e\u673a\u90e8\u7f72 \u2014 livox_ros_driver2 \u7684\u56db\u4e2a\u5751<\/li>\n<li>\n<ul>\n<li>3-7-1 PTP \u65f6\u949f\u540c\u6b65\u4e0d\u5b8c\u6574<\/li>\n<li>3-7-2 CustomMsg vs PointCloud2<\/li>\n<li>3-7-3 FAST-LIO2 \u914d\u7f6e lidar_type \u7684\u6620\u5c04\u9677\u9631<\/li>\n<li>3-7-4 livox_ros_driver2 \u7248\u672c\u5dee\u5f02<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<\/li>\n<li>4 \u8fdb\u9636\u73a9\u6cd5 \u2014 200Hz IMU \u524d\u63a8<\/li>\n<li>\n<ul>\n<li>\n<ul>\n<li>4-1 \u4e3a\u4ec0\u4e48\u9700\u8981\u9ad8\u9891\u91cc\u7a0b\u8ba1<\/li>\n<li>4-2 \u6838\u5fc3\u601d\u8def<\/li>\n<li>4-3 \u4e2d\u503c\u79ef\u5206\u63a8\u5bfc<\/li>\n<li>4-4 \u4ee3\u7801\u5b9e\u73b0<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<\/li>\n<li>5 \u540e\u7eed\u6269\u5c55\u65b9\u5411<\/li>\n<li>\u603b\u7ed3<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<hr \/>\n<h3>1 FAST-LIO2 \u5feb\u901f\u56de\u987e<\/h3>\n<h5>1-1 \u6838\u5fc3\u601d\u8def<\/h5>\n<ul>\n<li>\u6211\u4eec\u5728 \u3010FAST-LIO2 \u6e90\u7801\u89e3\u8bfb\u3011\u4ece Mid-360 \u5230 ESEKF \u2014 \u516c\u5f0f\u4e0e\u4ee3\u7801\u9010\u884c\u5bf9\u7167 \u4e2d\u5df2\u7ecf\u5b8c\u6574\u63a8\u5bfc\u8fc7&#xff0c;\u8fd9\u91cc\u53ea\u505a\u6700\u5c0f\u56de\u987e<\/li>\n<li>FAST-LIO2 \u505a\u4e00\u4ef6\u4e8b&#xff1a;\u628a LiDAR \u70b9\u4e91\u548c IMU \u6570\u636e\u878d\u5408&#xff0c;\u8f93\u51fa\u5b9e\u65f6 6-DOF \u4f4d\u59ff &#043; 3D \u5730\u56fe<\/li>\n<li>\u6838\u5fc3\u516c\u5f0f\u662f\u8fed\u4ee3\u8bef\u5dee\u72b6\u6001\u5361\u5c14\u66fc\u6ee4\u6ce2&#xff08;ESEKF&#xff09;&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>           x<\/p>\n<p>           ^<\/p>\n<p>          k<\/p>\n<p>           \u03ba<\/p>\n<p>           &#043;<\/p>\n<p>           1<\/p>\n<p>         &#061;<\/p>\n<p>           x<\/p>\n<p>           ^<\/p>\n<p>          k<\/p>\n<p>          \u03ba<\/p>\n<p>         &#043;<\/p>\n<p>          K<\/p>\n<p>          \u03ba<\/p>\n<p>          (<\/p>\n<p>           z<\/p>\n<p>           k<\/p>\n<p>          \u2212<\/p>\n<p>          h<\/p>\n<p>          (<\/p>\n<p>            x<\/p>\n<p>            ^<\/p>\n<p>           k<\/p>\n<p>           \u03ba<\/p>\n<p>          )<\/p>\n<p>          \u2212<\/p>\n<p>           H<\/p>\n<p>           \u03ba<\/p>\n<p>          (<\/p>\n<p>            x<\/p>\n<p>            ^<\/p>\n<p>           k<\/p>\n<p>           \u03ba<\/p>\n<p>          \u2212<\/p>\n<p>            x<\/p>\n<p>            ^<\/p>\n<p>           k<\/p>\n<p>          )<\/p>\n<p>          )<\/p>\n<p>         \\\\hat{x}_{k}^{\\\\kappa&#043;1} &#061; \\\\hat{x}_{k}^{\\\\kappa} &#043; K^{\\\\kappa}\\\\left( z_k &#8211; h(\\\\hat{x}_k^\\\\kappa) &#8211; H^\\\\kappa(\\\\hat{x}_k^\\\\kappa &#8211; \\\\hat{x}_k) \\\\right) <\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1.1555em;vertical-align: -0.2914em\"><\/span><span class=\"mord\"><span class=\"mord accent\"><span class=\"vlist-t\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.6944em\"><span class=\"\" style=\"top: -3em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord mathnormal\">x<\/span><\/span><span class=\"\" style=\"top: -3em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"accent-body\" style=\"left: -0.2222em\"><span class=\"mord\">^<\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.8641em\"><span class=\"\" style=\"top: -2.4086em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\">\u03ba<\/span><span class=\"mbin mtight\">&#043;<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2914em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.9614em;vertical-align: -0.247em\"><\/span><span class=\"mord\"><span class=\"mord accent\"><span class=\"vlist-t\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.6944em\"><span class=\"\" style=\"top: -3em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord mathnormal\">x<\/span><\/span><span class=\"\" style=\"top: -3em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"accent-body\" style=\"left: -0.2222em\"><span class=\"mord\">^<\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\">\u03ba<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.247em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">&#043;<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0715em\">K<\/span><span class=\"msupsub\"><span class=\"vlist-t\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\">\u03ba<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"minner\"><span class=\"mopen delimcenter\" style=\"top: 0em\">(<\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.044em\">z<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.044em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u2212<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mord mathnormal\">h<\/span><span class=\"mopen\">(<\/span><span class=\"mord\"><span class=\"mord accent\"><span class=\"vlist-t\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.6944em\"><span class=\"\" style=\"top: -3em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord mathnormal\">x<\/span><\/span><span class=\"\" style=\"top: -3em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"accent-body\" style=\"left: -0.2222em\"><span class=\"mord\">^<\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">\u03ba<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.247em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u2212<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0813em\">H<\/span><span class=\"msupsub\"><span class=\"vlist-t\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">\u03ba<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mopen\">(<\/span><span class=\"mord\"><span class=\"mord accent\"><span class=\"vlist-t\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.6944em\"><span class=\"\" style=\"top: -3em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord mathnormal\">x<\/span><\/span><span class=\"\" style=\"top: -3em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"accent-body\" style=\"left: -0.2222em\"><span class=\"mord\">^<\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">\u03ba<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.247em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u2212<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mord\"><span class=\"mord accent\"><span class=\"vlist-t\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.6944em\"><span class=\"\" style=\"top: -3em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord mathnormal\">x<\/span><\/span><span class=\"\" style=\"top: -3em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"accent-body\" style=\"left: -0.2222em\"><span class=\"mord\">^<\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mclose\">)<\/span><span class=\"mclose delimcenter\" style=\"top: 0em\">)<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<ul>\n<li>\u5176\u4e2d K \u662f\u5361\u5c14\u66fc\u589e\u76ca&#xff0c;H \u662f\u89c2\u6d4b\u96c5\u53ef\u6bd4&#xff0c;z &#8211; h(x) \u662f\u70b9\u5230\u5e73\u9762\u7684\u6b8b\u5dee<\/li>\n<li>\u8bf4\u4eba\u8bdd\u5c31\u662f&#xff1a;\u7528 IMU \u9884\u6d4b\u4f4d\u59ff \u2192 \u628a\u65b0\u6765\u7684 LiDAR \u70b9\u6295\u5f71\u5230\u5730\u56fe\u4e0a \u2192 \u7b97\u70b9\u5230\u6700\u8fd1\u5e73\u9762\u7684\u8ddd\u79bb \u2192 \u7528\u8fd9\u4e2a\u8ddd\u79bb\u53cd\u590d\u4fee\u6b63\u4f4d\u59ff \u2192 \u6536\u655b\u540e\u628a\u70b9\u52a0\u5165\u5730\u56fe<\/li>\n<\/ul>\n<p>FAST-LIO2 \u7684\u672c\u8d28&#xff1a;IMU \u505a\u5148\u9a8c\u3001LiDAR \u70b9\u5bf9\u9762\u505a\u89c2\u6d4b\u3001\u8fed\u4ee3\u5361\u5c14\u66fc\u505a\u878d\u5408\u3002\u4e0d\u50cf LOAM \u90a3\u6837\u5148\u63d0\u7279\u5f81&#xff08;\u8fb9\u7f18\u70b9\/\u5e73\u9762\u70b9&#xff09;&#xff0c;\u800c\u662f\u76f4\u63a5\u5728\u539f\u59cb\u70b9\u4e0a\u7b97\u6b8b\u5dee<\/p>\n<h5>1-2 \u4e3a\u4ec0\u4e48\u4e0d\u7528\u7279\u5f81\u63d0\u53d6<\/h5>\n<ul>\n<li>\u4f20\u7edf LOAM&#xff08;Lidar Odometry and Mapping&#xff09;\u9700\u8981\u4ece\u6bcf\u5e27\u70b9\u4e91\u4e2d\u63d0\u53d6&#034;\u8fb9\u7f18\u70b9&#034;\u548c&#034;\u5e73\u9762\u70b9&#034;&#xff0c;\u8fd9\u4e2a\u8fc7\u7a0b\u8ba1\u7b97\u91cf\u5927\u4e14\u5bf9\u573a\u666f\u654f\u611f\u2014\u2014\u7a7a\u65f7\u73af\u5883\u627e\u4e0d\u5230\u8db3\u591f\u5e73\u9762\u70b9\u65f6&#xff0c;\u7279\u5f81\u9000\u5316\u4f1a\u5bfc\u81f4\u91cc\u7a0b\u8ba1\u53d1\u6563<\/li>\n<li>FAST-LIO2 \u8df3\u8fc7\u4e86\u7279\u5f81\u63d0\u53d6&#xff0c;\u76f4\u63a5\u7528 ikd-Tree \u5728\u5168\u5c40\u5730\u56fe\u4e2d\u641c\u7d22\u6bcf\u4e2a LiDAR \u70b9\u7684\u6700\u8fd1\u90bb&#xff0c;\u62df\u5408\u5c40\u90e8\u5e73\u9762&#xff0c;\u8ba1\u7b97\u70b9\u5230\u5e73\u9762\u7684\u8ddd\u79bb\u4f5c\u4e3a\u6b8b\u5dee<\/li>\n<li>\u8bf4\u4eba\u8bdd\u5c31\u662f&#xff1a;LOAM \u662f&#034;\u5148\u628a\u70b9\u4e91\u5206\u7c7b\u518d\u5339\u914d&#034;&#xff0c;FAST-LIO2 \u662f&#034;\u76f4\u63a5\u62ff\u539f\u59cb\u70b9\u53bb\u5730\u56fe\u91cc\u627e\u6700\u8fd1\u7684\u90bb\u5c45&#034;&#xff0c;\u7701\u4e86\u4e00\u6b65\u3001\u4e5f\u5c11\u4e86\u4e00\u4e2a\u9000\u5316\u6765\u6e90<\/li>\n<\/ul>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260725171715-6a64ef9b7df4c.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<hr \/>\n<h3>2 \u7f16\u8bd1 FAST-LIO2<\/h3>\n<h5>2-1 \u62c9\u53d6\u4ed3\u5e93<\/h5>\n<ul>\n<li>FAST-LIO2 \u7684\u5b98\u65b9 ROS2 \u7248\u672c\u76f4\u63a5\u7ef4\u62a4\u5728 hku-mars\/FAST_LIO \u4ed3\u5e93\u7684 ROS2 \u5206\u652f\u4e0a&#xff08;\u4e0d\u662f\u5355\u72ec\u7684 repo&#xff09;&#xff0c;\u7531\u793e\u533a\u8d21\u732e\u8005 Ericsii \u63d0\u4ea4\u7684 PR \u5408\u5e76\u800c\u6765<\/li>\n<li>\u6ce8\u610f\u5206\u652f&#xff1a;\u5fc5\u987b clone ROS2 \u5206\u652f&#xff0c;\u9ed8\u8ba4\u7684 main \u5206\u652f\u53ea\u652f\u6301 ROS1<\/li>\n<\/ul>\n<p><span class=\"token builtin class-name\">cd<\/span> ~\/postgraduate0\/px4_ws\/src<br \/>\n<span class=\"token function\">git<\/span> clone https:\/\/github.com\/hku-mars\/FAST_LIO.git <span class=\"token parameter variable\">&#8211;branch<\/span> ROS2 <span class=\"token parameter variable\">&#8211;recursive<\/span><\/p>\n<ul>\n<li>&#8211;recursive \u786e\u4fdd\u628a\u5b50\u6a21\u5757 ikd-Tree&#xff08;\u589e\u91cf KD \u6811&#xff0c;FAST-LIO2 \u7684\u5730\u56fe\u6570\u636e\u7ed3\u6784\u6838\u5fc3&#xff09;\u4e00\u8d77\u62c9\u4e0b\u6765<\/li>\n<li>clone \u5b8c\u6210\u540e\u76ee\u5f55\u7ed3\u6784&#xff1a;<\/li>\n<\/ul>\n<p>src\/FAST_LIO\/<br \/>\n\u251c\u2500\u2500 include\/ikd-Tree\/    # ikd-Tree \u5b50\u6a21\u5757&#xff08;\u589e\u91cf KD \u6811\u5b9e\u73b0&#xff09;<br \/>\n\u251c\u2500\u2500 src\/<br \/>\n\u2502   \u251c\u2500\u2500 laserMapping.cpp  # \u4e3b\u8282\u70b9&#xff1a;ESEKF \u8fed\u4ee3 &#043; \u5730\u56fe\u7ef4\u62a4<br \/>\n\u2502   \u251c\u2500\u2500 preprocess.cpp    # \u70b9\u4e91\u9884\u5904\u7406&#xff08;\u53bb\u7578\u53d8\/\u964d\u91c7\u6837&#xff09;<br \/>\n\u2502   \u2514\u2500\u2500 IMU_Processing.hpp # IMU \u524d\u5411\u4f20\u64ad\u4e0e\u53cd\u5411\u8865\u507f<br \/>\n\u251c\u2500\u2500 config\/               # LiDAR \u53c2\u6570\u914d\u7f6e&#xff08;avia\/mid360\/velodyne\/ouster&#xff09;<br \/>\n\u251c\u2500\u2500 launch\/               # ROS2 launch \u6587\u4ef6<br \/>\n\u2514\u2500\u2500 msg\/                  # \u81ea\u5b9a\u4e49\u6d88\u606f&#xff08;Pose6D&#xff09;<\/p>\n<h5>2-2 \u4fee\u6539\u6e90\u7801 \u2014 \u53bb\u6389 livox_ros_driver2 \u786c\u4f9d\u8d56<\/h5>\n<ul>\n<li>\n<p>\u5b98\u65b9\u4ee3\u7801\u786c\u4f9d\u8d56 livox_ros_driver2&#xff08;\u89c8\u6c83\u6fc0\u5149\u96f7\u8fbe\u7684 ROS2 \u9a71\u52a8\u5305&#xff09;&#xff0c;\u4f46\u6211\u4eec\u7684\u4eff\u771f\u7528\u7684\u662f Velodyne VLP-16\u2014\u2014\u5b83\u53d1\u5e03\u7684\u662f\u6807\u51c6 sensor_msgs\/PointCloud2&#xff0c;\u6839\u672c\u4e0d\u9700\u8981 CustomMsg<\/p>\n<\/li>\n<li>\n<p>livox_ros_driver2 \u4e0d\u5728 apt \u6e90\u91cc&#xff0c;\u9700\u8981\u4ece GitHub \u5355\u72ec\u5b89\u88c5\u6574\u4e2a Livox-SDK2 \u5de5\u5177\u94fe&#xff0c;\u53ea\u4e3a\u4e86\u4e00\u7ec4\u6d88\u606f\u5b9a\u4e49\u62c9\u4e00\u6574\u5957\u9a71\u52a8\u592a\u4e0d\u5212\u7b97<\/p>\n<\/li>\n<li>\n<p>\u6240\u4ee5\u6211\u4eec\u5bf9\u6e90\u7801\u505a\u4e86\u6761\u4ef6\u7f16\u8bd1\u6539\u9020&#xff1a;\u8ba9 livox_ros_driver2 \u53d8\u6210\u53ef\u9009\u7684\u2014\u2014\u5982\u679c CMake \u68c0\u6d4b\u5230\u5c31\u7f16\u8bd1 Livox \u652f\u6301&#xff0c;\u68c0\u6d4b\u4e0d\u5230\u5c31\u7528\u6807\u51c6 PointCloud2 \u8def\u5f84\u7f16\u8bd1<\/p>\n<\/li>\n<li>\n<p>\u6539\u52a8\u7684\u6838\u5fc3\u903b\u8f91&#xff1a;<\/p>\n<\/li>\n<\/ul>\n<p>CMake \u7528 find_package(livox_ros_driver2 QUIET) \u5c1d\u8bd5\u67e5\u627e&#xff0c;\u627e\u5230\u5219\u5b9a\u4e49 USE_LIVOX \u5b8f\u3002\u6e90\u7801\u4e2d\u6240\u6709 CustomMsg \u76f8\u5173\u7684 include\u3001\u51fd\u6570\u3001\u8ba2\u9605\u5668\u5168\u90e8\u7528 #ifdef USE_LIVOX &#8230; #endif \u5305\u88f9\u3002\u4eff\u771f\u573a\u666f\u4e0b CMake \u627e\u4e0d\u5230 livox_ros_driver2 \u2192 \u5b8f\u4e0d\u5b9a\u4e49 \u2192 \u4ee3\u7801\u7f16\u8bd1\u8df3\u8fc7 Livox \u8def\u5f84 \u2192 \u53ea\u8d70 PointCloud2<\/p>\n<ul>\n<li>\u6d89\u53ca\u7684\u6587\u4ef6\u548c\u5177\u4f53\u6539\u52a8&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>\u6587\u4ef6\u6539\u52a8<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">CMakeLists.txt<\/td>\n<td align=\"left\">find_package(livox_ros_driver2 QUIET)&#xff0c;\u627e\u5230\u5219 add_definitions(-DUSE_LIVOX) \u5e76\u8ffd\u52a0\u5230\u4f9d\u8d56\u5217\u8868<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">package.xml<\/td>\n<td align=\"left\">\u5220\u9664 &lt;depend&gt;livox_ros_driver2&lt;\/depend&gt;<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">src\/preprocess.h<\/td>\n<td align=\"left\">#include &lt;CustomMsg&gt;\u3001process(CustomMsg)\u3001avia_handler(CustomMsg) \u5168\u90e8\u52a0 #ifdef USE_LIVOX<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">src\/preprocess.cpp<\/td>\n<td align=\"left\">process(CustomMsg) \u91cd\u8f7d &#043; avia_handler() \u51fd\u6570\u4f53\u5168\u90e8\u52a0 #ifdef USE_LIVOX<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">src\/laserMapping.cpp<\/td>\n<td align=\"left\">#include &lt;CustomMsg&gt;\u3001livox_pcl_cbk() \u56de\u8c03\u3001AVIA \u8ba2\u9605\u5206\u652f\u3001sub_pcl_livox_ \u6210\u5458\u53d8\u91cf\u5168\u90e8\u52a0 #ifdef USE_LIVOX<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<ul>\n<li>\u5173\u952e\u4ee3\u7801\u7247\u6bb5&#xff08;CMakeLists.txt&#xff09;&#xff1a;<\/li>\n<\/ul>\n<p>find_package(livox_ros_driver2 QUIET)<br \/>\nif(livox_ros_driver2_FOUND)<br \/>\n  add_definitions(-DUSE_LIVOX)<br \/>\n  message(STATUS &#034;livox_ros_driver2 found, enabling Livox support&#034;)<br \/>\nelse()<br \/>\n  message(STATUS &#034;livox_ros_driver2 NOT found, building without Livox support&#034;)<br \/>\nendif()<\/p>\n<ul>\n<li>\u5173\u952e\u4ee3\u7801\u7247\u6bb5&#xff08;laserMapping.cpp \u4e2d\u7684\u8ba2\u9605\u903b\u8f91&#xff09;&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>p_pre<span class=\"token operator\">-&gt;<\/span>lidar_type <span class=\"token operator\">&#061;&#061;<\/span> AVIA<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">ifdef<\/span> <span class=\"token expression\">USE_LIVOX<\/span><\/span><br \/>\n    sub_pcl_livox_ <span class=\"token operator\">&#061;<\/span> <span class=\"token keyword\">this<\/span><span class=\"token operator\">-&gt;<\/span><span class=\"token generic-function\"><span class=\"token function\">create_subscription<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>livox_ros_driver2<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>CustomMsg<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><br \/>\n        lid_topic<span class=\"token punctuation\">,<\/span> <span class=\"token number\">20<\/span><span class=\"token punctuation\">,<\/span> livox_pcl_cbk<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">else<\/span><\/span><br \/>\n    <span class=\"token function\">RCLCPP_ERROR<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">this<\/span><span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_logger<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        <span class=\"token string\">&#034;AVIA selected but livox_ros_driver2 not available!&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    rclcpp<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">shutdown<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">return<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token macro property\"><span class=\"token directive-hash\">#<\/span><span class=\"token directive keyword\">endif<\/span><\/span><br \/>\n<span class=\"token punctuation\">}<\/span><br \/>\n<span class=\"token keyword\">else<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n    sub_pcl_pc_ <span class=\"token operator\">&#061;<\/span> <span class=\"token keyword\">this<\/span><span class=\"token operator\">-&gt;<\/span><span class=\"token generic-function\"><span class=\"token function\">create_subscription<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>sensor_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>PointCloud2<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><br \/>\n        lid_topic<span class=\"token punctuation\">,<\/span> rclcpp<span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">SensorDataQoS<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> standard_pcl_cbk<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<ul>\n<li>\n<p>\u8bf4\u4eba\u8bdd\u5c31\u662f&#xff1a;\u4f60\u9009 lidar_type: 2&#xff08;Velodyne&#xff09;\u2192 \u8d70 standard_pcl_cbk&#xff08;\u6807\u51c6 PointCloud2 \u56de\u8c03&#xff09;\u2192 \u4e0d\u9700\u8981 livox_ros_driver2\u3002\u4f60\u9009 lidar_type: 1&#xff08;AVIA&#xff09;\u4f46\u6ca1\u88c5 livox_ros_driver2 \u2192 \u76f4\u63a5\u62a5\u9519\u9000\u51fa&#xff0c;\u4e0d\u4f1a\u51fa\u73b0\u83ab\u540d\u5176\u5999\u7684\u94fe\u63a5\u5931\u8d25<\/p>\n<\/li>\n<li>\n<p>\u4fee\u6539\u5b8c\u6210\u540e\u7f16\u8bd1&#xff1a;<\/p>\n<\/li>\n<\/ul>\n<p><span class=\"token builtin class-name\">cd<\/span> ~\/postgraduate0\/px4_ws<br \/>\n<span class=\"token builtin class-name\">source<\/span> \/opt\/ros\/humble\/setup.bash<br \/>\ncolcon build &#8211;symlink-install &#8211;packages-select fast_lio<\/p>\n<ul>\n<li>\u7f16\u8bd1\u8f93\u51fa\u4f1a\u6709 livox_ros_driver2 NOT found, building without Livox support (simulation mode) \u63d0\u793a&#xff0c;\u8fd9\u662f\u9884\u671f\u884c\u4e3a<\/li>\n<li>\u6ce8\u610f&#xff1a;\u4ee5\u540e\u5982\u679c\u8981\u63a5\u5b9e\u7269 Livox Mid-360 \/ Avia&#xff0c;\u53ea\u9700\u5b89\u88c5 livox_ros_driver2&#xff0c;\u91cd\u65b0 colcon build \u5373\u53ef\u81ea\u52a8\u542f\u7528 Livox \u652f\u6301&#xff0c;\u6e90\u7801\u4e0d\u9700\u8981\u518d\u6539<\/li>\n<\/ul>\n<hr \/>\n<h3>3 \u542f\u52a8\u914d\u7f6e<\/h3>\n<h5>3-1 TF \u6811\u8bbe\u8ba1<\/h5>\n<ul>\n<li>\n<p>\u8fd9\u662f\u6700\u5bb9\u6613\u8e29\u5751\u7684\u5730\u65b9\u3002FAST-LIO2 \u5185\u90e8\u6709\u4e00\u5957\u81ea\u5df1\u7684\u5750\u6807\u7cfb\u4f53\u7cfb&#xff0c;\u548c\u6211\u4eec\u7b2c\u4e00\u671f\u642d\u5efa\u7684 PX4 \u4f20\u611f\u5668 TF \u6811\u5fc5\u987b\u5bf9\u9f50&#xff0c;\u5426\u5219 RViz2 \u91cc\u5168\u662f \u201cNo transform from [xxx] to [yyy]\u201d \u7684\u62a5\u9519<\/p>\n<\/li>\n<li>\n<p>FAST-LIO2 \u7684\u5750\u6807\u7cfb\u5b9a\u4e49&#xff1a;<\/p>\n<ul>\n<li>camera_init&#xff1a;\u4e16\u754c\u5750\u6807\u7cfb&#xff08;\u539f\u70b9 &#061; FAST-LIO2 \u521d\u59cb\u5316\u7684\u7b2c\u4e00\u5e27 LiDAR \u4f4d\u7f6e&#xff09;<\/li>\n<li>body&#xff1a;IMU \u673a\u4f53\u5750\u6807\u7cfb&#xff08;FAST-LIO2 \u901a\u8fc7 Odometry \u8bdd\u9898\u52a8\u6001\u53d1\u5e03 camera_init \u2192 body \u7684 TF&#xff09;<\/li>\n<li>\u6240\u6709\u8f93\u51fa\u8bdd\u9898&#xff08;\/cloud_registered\u3001\/Odometry\u3001\/path&#xff09;\u90fd\u5728 camera_init \u7cfb\u4e0b<\/li>\n<\/ul>\n<\/li>\n<li>\n<p>PX4 \u7684\u5750\u6807\u7cfb\u5b9a\u4e49&#xff1a;<\/p>\n<ul>\n<li>base_link&#xff1a;\u98de\u673a\u673a\u4f53\u5750\u6807\u7cfb&#xff08;\u6839\u8282\u70b9&#xff09;<\/li>\n<li>velodyne_link\u3001imu_link\u3001camera_front_link\u3001camera_down_link&#xff1a;\u4f20\u611f\u5668\u5b50\u5750\u6807\u7cfb<\/li>\n<\/ul>\n<\/li>\n<li>\n<p>\u95ee\u9898&#xff1a;FAST-LIO2 \u53ea\u6709 camera_init \u2192 body&#xff0c;PX4 \u53ea\u6709 base_link \u2192 \u5404\u4f20\u611f\u5668&#xff0c;\u4e24\u6761\u94fe\u4e4b\u95f4\u6ca1\u6709\u8fde\u63a5\u2014\u2014RViz2 \u8bbe camera_init \u4e3a Fixed Frame \u65f6\u770b\u4e0d\u5230 PX4 \u7684 TF&#xff0c;\u8bbe base_link \u65f6\u770b\u4e0d\u5230 FAST-LIO2 \u7684\u6570\u636e<\/p>\n<\/li>\n<li>\n<p>\u89e3\u51b3\u65b9\u6848&#xff1a;\u52a0\u4e00\u6761 body \u2192 base_link \u7684\u9759\u6001 TF&#xff08;identity&#xff09;<\/p>\n<\/li>\n<\/ul>\n<p>camera_init              \u2190 FAST-LIO2 \u4e16\u754c\u7cfb (Fixed Frame)<br \/>\n  \u2514\u2500\u2500 body                \u2190 IMU \u7cfb (Odometry TF \u52a8\u6001\u53d1\u5e03)<br \/>\n       \u2514\u2500\u2500 base_link       \u2190 PX4 \u673a\u4f53\u7cfb (static identity)<br \/>\n            \u251c\u2500\u2500 velodyne_link<br \/>\n            \u251c\u2500\u2500 imu_link<br \/>\n            \u251c\u2500\u2500 camera_front_link<br \/>\n            \u2514\u2500\u2500 camera_down_link<\/p>\n<ul>\n<li>\u8bf4\u4eba\u8bdd\u5c31\u662f&#xff1a;\u5728 FAST-LIO2 \u7684 IMU \u5750\u6807\u7cfb&#xff08;body&#xff09;\u548c PX4 \u7684\u673a\u8eab\u5750\u6807\u7cfb&#xff08;base_link&#xff09;\u4e4b\u95f4\u52a0\u4e00\u6761&#034;\u5b83\u4eec\u5c31\u662f\u540c\u4e00\u4e2a\u4e1c\u897f&#034;\u7684\u58f0\u660e\u3002\u56e0\u4e3a\u4eff\u771f\u4e2d IMU \u6570\u636e\u548c LiDAR \u6570\u636e\u90fd\u5c5e\u4e8e\u540c\u4e00\u67b6\u98de\u673a&#xff0c;body &#061; base_link&#xff0c;identity \u5c31\u591f\u4e86<\/li>\n<\/ul>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260725171717-6a64ef9d21a1d.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<h5>3-2 \u542f\u52a8\u811a\u672c<\/h5>\n<ul>\n<li>\u7b2c\u4e00\u671f\u6211\u4eec\u7528 3_rviz.sh \u505a\u7eaf\u53ef\u89c6\u5316&#xff0c;\u672c\u671f\u6539\u6210 3_fastlio2.sh&#xff1a;\u5728\u539f\u6709 TF &#043; RViz2 \u7684\u57fa\u7840\u4e0a&#xff0c;\u52a0\u4e0a\u4e86 FAST-LIO2 \u8282\u70b9\u7684\u542f\u52a8<\/li>\n<\/ul>\n<p><span class=\"token shebang important\">#!\/bin\/bash<\/span><br \/>\n<span class=\"token comment\"># FAST-LIO2 &#043; RViz2: \u70b9\u4e91\u5efa\u56fe &#043; \u53ef\u89c6\u5316<\/span><br \/>\n<span class=\"token builtin class-name\">set<\/span> <span class=\"token parameter variable\">-e<\/span><\/p>\n<p><span class=\"token function-name function\">cleanup<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;&gt;&gt;&gt; \u6b63\u5728\u5173\u95ed\u6240\u6709 FAST-LIO2 \/ RViz2 \u8fdb\u7a0b&#8230;&#034;<\/span><br \/>\n    <span class=\"token function\">kill<\/span> <span class=\"token variable\">$FASTLIO_PID<\/span> <span class=\"token operator\"><span class=\"token file-descriptor important\">2<\/span>&gt;<\/span>\/dev\/null<br \/>\n    <span class=\"token function\">kill<\/span> <span class=\"token variable\">$TF1<\/span> <span class=\"token variable\">$TF2<\/span> <span class=\"token variable\">$TF3<\/span> <span class=\"token variable\">$TF4<\/span> <span class=\"token variable\">$TF5<\/span> <span class=\"token operator\"><span class=\"token file-descriptor important\">2<\/span>&gt;<\/span>\/dev\/null<br \/>\n    <span class=\"token function\">pkill<\/span> <span class=\"token parameter variable\">-f<\/span> <span class=\"token string\">&#034;fastlio_mapping&#034;<\/span> <span class=\"token operator\"><span class=\"token file-descriptor important\">2<\/span>&gt;<\/span>\/dev\/null <span class=\"token operator\">||<\/span> <span class=\"token boolean\">true<\/span><br \/>\n    <span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;&gt;&gt;&gt; \u5df2\u5168\u90e8\u5173\u95ed&#034;<\/span><br \/>\n    <span class=\"token builtin class-name\">exit<\/span> <span class=\"token number\">0<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><br \/>\n<span class=\"token builtin class-name\">trap<\/span> cleanup SIGINT SIGTERM<\/p>\n<p><span class=\"token builtin class-name\">source<\/span> \/opt\/ros\/humble\/setup.bash<br \/>\n<span class=\"token builtin class-name\">source<\/span> \/home\/lzh\/postgraduate0\/px4_ws\/install\/setup.bash<\/p>\n<p><span class=\"token comment\"># &#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061; 1. \u4f20\u611f\u5668\u9759\u6001 TF &#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;<\/span><br \/>\n<span class=\"token comment\"># base_link \u2192 velodyne_link (VLP-16 \u5728\u9876\u90e8 0.12m)<\/span><br \/>\nros2 run tf2_ros static_transform_publisher <span class=\"token punctuation\">\\\\<\/span><br \/>\n    <span class=\"token number\">0<\/span> <span class=\"token number\">0<\/span> <span class=\"token number\">0.12<\/span> <span class=\"token number\">0<\/span> <span class=\"token number\">0<\/span> <span class=\"token number\">0<\/span> base_link velodyne_link <span class=\"token operator\">&amp;<\/span><br \/>\n<span class=\"token assign-left variable\">TF1<\/span><span class=\"token operator\">&#061;<\/span><span class=\"token variable\">$!<\/span><\/p>\n<p><span class=\"token comment\"># base_link \u2192 imu_link (IMU \u5728 base_link \u4e0a\u65b9 0.05m)<\/span><br \/>\nros2 run tf2_ros static_transform_publisher <span class=\"token punctuation\">\\\\<\/span><br \/>\n    <span class=\"token number\">0<\/span> <span class=\"token number\">0<\/span> <span class=\"token number\">0.05<\/span> <span class=\"token number\">0<\/span> <span class=\"token number\">0<\/span> <span class=\"token number\">0<\/span> base_link imu_link <span class=\"token operator\">&amp;<\/span><br \/>\n<span class=\"token assign-left variable\">TF2<\/span><span class=\"token operator\">&#061;<\/span><span class=\"token variable\">$!<\/span><\/p>\n<p><span class=\"token comment\"># base_link \u2192 camera_front_link<\/span><br \/>\nros2 run tf2_ros static_transform_publisher <span class=\"token punctuation\">\\\\<\/span><br \/>\n    <span class=\"token number\">0.15<\/span> <span class=\"token number\">0<\/span> <span class=\"token number\">0.08<\/span> <span class=\"token number\">0<\/span> <span class=\"token number\">0<\/span> <span class=\"token number\">0<\/span> base_link camera_front_link <span class=\"token operator\">&amp;<\/span><br \/>\n<span class=\"token assign-left variable\">TF3<\/span><span class=\"token operator\">&#061;<\/span><span class=\"token variable\">$!<\/span><\/p>\n<p><span class=\"token comment\"># base_link \u2192 camera_down_link<\/span><br \/>\nros2 run tf2_ros static_transform_publisher <span class=\"token punctuation\">\\\\<\/span><br \/>\n    <span class=\"token number\">0<\/span> <span class=\"token number\">0<\/span> <span class=\"token parameter variable\">-0.02<\/span> <span class=\"token number\">0<\/span> <span class=\"token number\">1.5708<\/span> <span class=\"token number\">0<\/span> base_link camera_down_link <span class=\"token operator\">&amp;<\/span><br \/>\n<span class=\"token assign-left variable\">TF4<\/span><span class=\"token operator\">&#061;<\/span><span class=\"token variable\">$!<\/span><\/p>\n<p><span class=\"token comment\"># body(FAST-LIO2 IMU frame) \u2192 base_link (PX4 drone frame)<\/span><br \/>\n<span class=\"token comment\"># FAST-LIO2 \u53d1\u5e03 camera_init \u2192 body \u7684 odometry TF<\/span><br \/>\n<span class=\"token comment\"># \u52a0\u4e0a\u8fd9\u4e2a\u9759\u6001 TF \u540e, TF \u6811\u8fde\u901a: camera_init \u2192 body \u2192 base_link \u2192 sensors<\/span><br \/>\nros2 run tf2_ros static_transform_publisher <span class=\"token punctuation\">\\\\<\/span><br \/>\n    <span class=\"token number\">0<\/span> <span class=\"token number\">0<\/span> <span class=\"token number\">0<\/span> <span class=\"token number\">0<\/span> <span class=\"token number\">0<\/span> <span class=\"token number\">0<\/span> body base_link <span class=\"token operator\">&amp;<\/span><br \/>\n<span class=\"token assign-left variable\">TF5<\/span><span class=\"token operator\">&#061;<\/span><span class=\"token variable\">$!<\/span><\/p>\n<p><span class=\"token function\">sleep<\/span> <span class=\"token number\">1<\/span><\/p>\n<p><span class=\"token comment\"># &#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061; 2. \u542f\u52a8 FAST-LIO2 &#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;<\/span><br \/>\n<span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;&gt;&gt;&gt; \u542f\u52a8 FAST-LIO2 (px4_vlp16 \u914d\u7f6e)&#8230;&#034;<\/span><br \/>\nros2 launch fast_lio mapping_px4.launch.py rviz:<span class=\"token operator\">&#061;<\/span>false <span class=\"token operator\">&amp;<\/span><br \/>\n<span class=\"token assign-left variable\">FASTLIO_PID<\/span><span class=\"token operator\">&#061;<\/span><span class=\"token variable\">$!<\/span><br \/>\n<span class=\"token function\">sleep<\/span> <span class=\"token number\">2<\/span><\/p>\n<p><span class=\"token comment\"># &#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061; 3. \u542f\u52a8 RViz2 &#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;&#061;<\/span><br \/>\n<span class=\"token builtin class-name\">echo<\/span> <span class=\"token string\">&#034;&gt;&gt;&gt; \u542f\u52a8 RViz2&#8230;&#034;<\/span><br \/>\nrviz2 <span class=\"token parameter variable\">-d<\/span> \/home\/lzh\/postgraduate0\/px4_ws\/px4_sim.rviz<\/p>\n<p><span class=\"token comment\"># \u6e05\u7406<\/span><br \/>\n<span class=\"token function\">kill<\/span> <span class=\"token variable\">$FASTLIO_PID<\/span> <span class=\"token operator\"><span class=\"token file-descriptor important\">2<\/span>&gt;<\/span>\/dev\/null<br \/>\n<span class=\"token function\">kill<\/span> <span class=\"token variable\">$TF1<\/span> <span class=\"token variable\">$TF2<\/span> <span class=\"token variable\">$TF3<\/span> <span class=\"token variable\">$TF4<\/span> <span class=\"token variable\">$TF5<\/span> <span class=\"token operator\"><span class=\"token file-descriptor important\">2<\/span>&gt;<\/span>\/dev\/null<\/p>\n<ul>\n<li>\u542f\u52a8\u987a\u5e8f&#xff08;\u9700\u8981\u4e09\u4e2a\u7ec8\u7aef&#xff09;&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>\u7ec8\u7aef\u547d\u4ee4\u804c\u8d23<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">\u7ec8\u7aef 1<\/td>\n<td align=\"left\">.\/1_bringup.sh<\/td>\n<td align=\"left\">MicroXRCEAgent &#043; PX4 SITL &#043; Gazebo&#xff08;\u7f51\u7b3c &#043; iris_vlp16&#xff09;<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\u7ec8\u7aef 2<\/td>\n<td align=\"left\">.\/2_keyboard.sh<\/td>\n<td align=\"left\">\u952e\u76d8 Offboard \u63a7\u5236&#xff08;\u7b49 Ready for takeoff! \u540e\u7a7a\u683c\u89e3\u9501\u8d77\u98de&#xff09;<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\u7ec8\u7aef 3<\/td>\n<td align=\"left\">.\/3_fastlio2.sh<\/td>\n<td align=\"left\">FAST-LIO2 \u5efa\u56fe &#043; RViz2 \u53ef\u89c6\u5316<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<ul>\n<li>\u6ce8\u610f&#xff1a;\u7ec8\u7aef 3 \u4e00\u5b9a\u8981\u7b49 Gazebo \u5b8c\u5168\u52a0\u8f7d&#xff08;Ready for takeoff! \u51fa\u73b0&#xff09;\u540e\u518d\u542f\u52a8&#xff0c;\u5426\u5219 \/velodyne\/points \u548c \/imu\/data \u8bdd\u9898\u8fd8\u6ca1\u5f00\u59cb\u53d1\u5e03&#xff0c;FAST-LIO2 \u4f1a\u4e00\u76f4\u7b49\u5f85\u6570\u636e<\/li>\n<\/ul>\n<h5>3-3 \u8bdd\u9898\u8bf4\u660e<\/h5>\n<ul>\n<li>FAST-LIO2 \u542f\u52a8\u540e&#xff0c;ros2 topic list \u53ef\u4ee5\u770b\u5230\u65b0\u589e\u7684\u8bdd\u9898&#xff1a;<\/li>\n<\/ul>\n<p>lzh&#064;lzh:~\/postgraduate0\/px4_ws$ ros2 topic list<br \/>\n\/Laser_map<br \/>\n\/Odometry<br \/>\n\/clock<br \/>\n\/cloud_effected<br \/>\n\/cloud_registered<br \/>\n\/cloud_registered_body<br \/>\n\/fmu\/in\/obstacle_distance<br \/>\n\/fmu\/in\/offboard_control_mode<br \/>\n\/fmu\/in\/onboard_computer_status<br \/>\n\/fmu\/in\/sensor_optical_flow<br \/>\n\/fmu\/in\/telemetry_status<br \/>\n\/fmu\/in\/trajectory_setpoint<br \/>\n\/fmu\/in\/vehicle_attitude_setpoint<br \/>\n\/fmu\/in\/vehicle_command<br \/>\n\/fmu\/in\/vehicle_mocap_odometry<br \/>\n\/fmu\/in\/vehicle_rates_setpoint<br \/>\n\/fmu\/in\/vehicle_trajectory_bezier<br \/>\n\/fmu\/in\/vehicle_trajectory_waypoint<br \/>\n\/fmu\/in\/vehicle_visual_odometry<br \/>\n\/fmu\/out\/failsafe_flags<br \/>\n\/fmu\/out\/position_setpoint_triplet<br \/>\n\/fmu\/out\/sensor_combined<br \/>\n\/fmu\/out\/timesync_status<br \/>\n\/fmu\/out\/vehicle_attitude<br \/>\n\/fmu\/out\/vehicle_control_mode<br \/>\n\/fmu\/out\/vehicle_global_position<br \/>\n\/fmu\/out\/vehicle_gps_position<br \/>\n\/fmu\/out\/vehicle_local_position<br \/>\n\/fmu\/out\/vehicle_odometry<br \/>\n\/fmu\/out\/vehicle_status<br \/>\n\/imu\/data<br \/>\n\/parameter_events<br \/>\n\/path<br \/>\n\/performance_metrics<br \/>\n\/rosout<br \/>\n\/tf<br \/>\n\/tf_static<br \/>\n\/velodyne\/points<\/p>\n<ul>\n<li>FAST-LIO2 \u53d1\u5e03\u7684 6 \u4e2a\u6838\u5fc3\u8bdd\u9898&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>\u8bdd\u9898\u7c7b\u578b\u542b\u4e49<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">\/cloud_registered<\/td>\n<td align=\"left\">sensor_msgs\/PointCloud2<\/td>\n<td align=\"left\">\u6700\u6838\u5fc3&#xff1a;\u914d\u51c6\u540e\u7684\u5168\u5c40\u70b9\u4e91&#xff08;\u4e16\u754c\u7cfb camera_init&#xff09;&#xff0c;\u968f\u65f6\u95f4\u7d2f\u79ef\u5f62\u6210 3D \u5730\u56fe<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\/cloud_registered_body<\/td>\n<td align=\"left\">sensor_msgs\/PointCloud2<\/td>\n<td align=\"left\">\u5f53\u524d\u5e27\u70b9\u4e91\u5728 IMU \u673a\u4f53\u7cfb&#xff08;body&#xff09;\u4e0b\u7684\u8868\u793a<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\/cloud_effected<\/td>\n<td align=\"left\">sensor_msgs\/PointCloud2<\/td>\n<td align=\"left\">\u5f53\u524d\u5e27\u4e2d\u53c2\u4e0e ESKF \u66f4\u65b0\u7684\u6709\u6548\u7279\u5f81\u70b9<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\/Odometry<\/td>\n<td align=\"left\">nav_msgs\/Odometry<\/td>\n<td align=\"left\">6-DOF \u4f4d\u59ff\u4f30\u8ba1&#xff08;camera_init \u2192 body&#xff09;&#xff0c;\u540c\u65f6\u53d1\u5e03\u5bf9\u5e94\u7684 TF<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\/path<\/td>\n<td align=\"left\">nav_msgs\/Path<\/td>\n<td align=\"left\">\u5386\u53f2\u8f68\u8ff9\u7ebf&#xff08;camera_init \u7cfb&#xff09;<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\/Laser_map<\/td>\n<td align=\"left\">sensor_msgs\/PointCloud2<\/td>\n<td align=\"left\">\u964d\u91c7\u6837\u540e\u7684\u5168\u5c40\u5730\u56fe\u70b9\u4e91&#xff08;\u4f4e\u9891\u53d1\u5e03&#xff0c;1Hz&#xff09;<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<ul>\n<li>PX4 \u63d0\u4f9b\u7684\u5173\u952e\u8f93\u5165\u8bdd\u9898&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>\u8bdd\u9898\u7c7b\u578b\u542b\u4e49<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">\/velodyne\/points<\/td>\n<td align=\"left\">sensor_msgs\/PointCloud2<\/td>\n<td align=\"left\">VLP-16 3D \u6fc0\u5149\u96f7\u8fbe\u70b9\u4e91&#xff08;10Hz&#xff0c;velodyne_link \u7cfb&#xff09;<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\/imu\/data<\/td>\n<td align=\"left\">sensor_msgs\/Imu<\/td>\n<td align=\"left\">\u539f\u59cb IMU \u6570\u636e&#xff08;200Hz&#xff0c;imu_link \u7cfb&#xff09;&#xff0c;\u6765\u81ea gazebo_ros_imu \u6a21\u578b<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<p>FAST-LIO2 \u53ea\u9700\u8981\u4e24\u4e2a\u8f93\u5165&#xff1a;\u4e00\u4e2a LiDAR \u8bdd\u9898\u3001\u4e00\u4e2a IMU \u8bdd\u9898\u3002\u5176\u4ed6\u6240\u6709 fmu\/* \u8bdd\u9898&#xff08;\u98de\u63a7\u72b6\u6001\u3001GPS \u7b49&#xff09;\u90fd\u4e0d\u9700\u8981\u2014\u2014\u8fd9\u5c31\u662f\u7eaf LiDAR-\u60ef\u6027\u91cc\u7a0b\u8ba1\u7684\u4f18\u96c5\u4e4b\u5904<\/p>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260725171718-6a64ef9ea9414.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<h5>3-4 \u53c2\u6570\u8bf4\u660e<\/h5>\n<ul>\n<li>FAST-LIO2 \u7684 YAML \u914d\u7f6e\u6587\u4ef6\u63a7\u5236\u9884\u5904\u7406\u3001ESKF\u3001\u53d1\u5e03\u884c\u4e3a\u7b49\u6240\u6709\u53c2\u6570\u3002\u4ee5\u4e0b\u662f\u6211\u4eec\u4e3a iris_vlp16 \u4eff\u771f\u5b9a\u5236\u7684 px4_vlp16.yaml&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token key atrule\">\/**<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    <span class=\"token key atrule\">ros__parameters<\/span><span class=\"token punctuation\">:<\/span><br \/>\n        <span class=\"token key atrule\">feature_extract_enable<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token boolean important\">false<\/span>   <span class=\"token comment\"># \u5173\u95ed\u7279\u5f81\u63d0\u53d6&#xff08;FAST-LIO2 \u7528\u76f4\u63a5\u6cd5&#xff09;<\/span><br \/>\n        <span class=\"token key atrule\">point_filter_num<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">1<\/span>             <span class=\"token comment\"># \u6bcf\u9694 N \u4e2a\u70b9\u91c7\u6837\u4e00\u4e2a&#xff08;1&#061;\u4e0d\u964d\u91c7\u6837&#xff09;<\/span><br \/>\n        <span class=\"token key atrule\">max_iteration<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">3<\/span>                <span class=\"token comment\"># ESKF \u6700\u5927\u8fed\u4ee3\u6b21\u6570<\/span><br \/>\n        <span class=\"token key atrule\">filter_size_surf<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">0.5<\/span>           <span class=\"token comment\"># \u70b9\u4e91\u964d\u91c7\u6837\u4f53\u7d20\u5927\u5c0f [m]<\/span><br \/>\n        <span class=\"token key atrule\">filter_size_map<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">0.5<\/span>            <span class=\"token comment\"># \u5730\u56fe\u964d\u91c7\u6837\u4f53\u7d20\u5927\u5c0f [m]<\/span><br \/>\n        <span class=\"token key atrule\">cube_side_length<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">1000.0<\/span>        <span class=\"token comment\"># \u5730\u56fe\u7acb\u65b9\u4f53\u8fb9\u957f [m]&#xff08;\u8fdc\u5927\u4e8e\u7f51\u7b3c 9m&#xff09;<\/span><\/p>\n<p>        <span class=\"token key atrule\">common<\/span><span class=\"token punctuation\">:<\/span><br \/>\n            <span class=\"token key atrule\">lid_topic<\/span><span class=\"token punctuation\">:<\/span>  <span class=\"token string\">&#034;\/velodyne\/points&#034;<\/span>   <span class=\"token comment\"># \u8f93\u5165\u7684 LiDAR \u8bdd\u9898<\/span><br \/>\n            <span class=\"token key atrule\">imu_topic<\/span><span class=\"token punctuation\">:<\/span>  <span class=\"token string\">&#034;\/imu\/data&#034;<\/span>          <span class=\"token comment\"># \u8f93\u5165\u7684 IMU \u8bdd\u9898<\/span><br \/>\n            <span class=\"token key atrule\">time_sync_en<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token boolean important\">false<\/span>              <span class=\"token comment\"># \u8f6f\u540c\u6b65&#xff08;\u4eff\u771f\u4e0d\u9700\u8981&#xff09;<\/span><br \/>\n            <span class=\"token key atrule\">time_offset_lidar_to_imu<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">0.0<\/span>    <span class=\"token comment\"># LiDAR-IMU \u65f6\u95f4\u504f\u79fb [s]<\/span><\/p>\n<p>        <span class=\"token key atrule\">preprocess<\/span><span class=\"token punctuation\">:<\/span><br \/>\n            <span class=\"token key atrule\">lidar_type<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">2<\/span>               <span class=\"token comment\"># 1&#061;Livox, 2&#061;Velodyne, 3&#061;Ouster, 4&#061;\u901a\u7528<\/span><br \/>\n            <span class=\"token key atrule\">scan_line<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">16<\/span>               <span class=\"token comment\"># VLP-16 \u7684\u7ebf\u6570<\/span><br \/>\n            <span class=\"token key atrule\">scan_rate<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">10<\/span>               <span class=\"token comment\"># VLP-16 \u7684\u626b\u63cf\u9891\u7387 [Hz]<\/span><br \/>\n            <span class=\"token key atrule\">timestamp_unit<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">2<\/span>           <span class=\"token comment\"># \u65f6\u95f4\u6233\u5355\u4f4d&#xff1a;0&#061;s, 1&#061;ms, 2&#061;us, 3&#061;ns<\/span><br \/>\n            <span class=\"token key atrule\">blind<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">0.5<\/span>                  <span class=\"token comment\"># \u76f2\u533a\u8ddd\u79bb [m]&#xff08;&lt;0.5m \u7684\u70b9\u4e22\u5f03&#xff09;<\/span><\/p>\n<p>        <span class=\"token key atrule\">mapping<\/span><span class=\"token punctuation\">:<\/span><br \/>\n            <span class=\"token key atrule\">acc_cov<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">0.1<\/span>                <span class=\"token comment\"># \u52a0\u901f\u5ea6\u6d4b\u91cf\u566a\u58f0\u534f\u65b9\u5dee<\/span><br \/>\n            <span class=\"token key atrule\">gyr_cov<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">0.1<\/span>                <span class=\"token comment\"># \u89d2\u901f\u5ea6\u6d4b\u91cf\u566a\u58f0\u534f\u65b9\u5dee<\/span><br \/>\n            <span class=\"token key atrule\">b_acc_cov<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">0.0001<\/span>           <span class=\"token comment\"># \u52a0\u901f\u5ea6\u8ba1 bias \u968f\u673a\u6e38\u8d70\u534f\u65b9\u5dee<\/span><br \/>\n            <span class=\"token key atrule\">b_gyr_cov<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">0.0001<\/span>           <span class=\"token comment\"># \u9640\u87ba\u4eea bias \u968f\u673a\u6e38\u8d70\u534f\u65b9\u5dee<\/span><br \/>\n            <span class=\"token key atrule\">fov_degree<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">360.0<\/span>           <span class=\"token comment\"># LiDAR \u6c34\u5e73\u89c6\u573a\u89d2 [\u00b0]<\/span><br \/>\n            <span class=\"token key atrule\">det_range<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">100.0<\/span>            <span class=\"token comment\"># \u6709\u6548\u63a2\u6d4b\u8ddd\u79bb [m]<\/span><br \/>\n            <span class=\"token key atrule\">extrinsic_est_en<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token boolean important\">false<\/span>     <span class=\"token comment\"># \u662f\u5426\u5728\u7ebf\u4f30\u8ba1 LiDAR-IMU \u5916\u53c2<\/span><br \/>\n            <span class=\"token key atrule\">extrinsic_T<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token punctuation\">[<\/span> <span class=\"token number\">0.<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.12<\/span><span class=\"token punctuation\">]<\/span>  <span class=\"token comment\"># LiDAR \u2192 IMU \u5e73\u79fb [x,y,z]&#xff08;VLP-16 \u5728\u9876\u90e8 12cm&#xff09;<\/span><br \/>\n            <span class=\"token key atrule\">extrinsic_R<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token punctuation\">[<\/span> <span class=\"token number\">1.<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.<\/span><span class=\"token punctuation\">,<\/span>    <span class=\"token comment\"># LiDAR \u2192 IMU \u65cb\u8f6c&#xff08;\u5355\u4f4d\u77e9\u9635&#061;\u65e0\u65cb\u8f6c&#xff09;<\/span><br \/>\n                        0.<span class=\"token punctuation\">,<\/span> <span class=\"token number\">1.<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                        <span class=\"token number\">0.<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">1.<\/span><span class=\"token punctuation\">]<\/span><\/p>\n<p>        <span class=\"token key atrule\">publish<\/span><span class=\"token punctuation\">:<\/span><br \/>\n            <span class=\"token key atrule\">path_en<\/span><span class=\"token punctuation\">:<\/span>  <span class=\"token boolean important\">true<\/span>              <span class=\"token comment\"># \u53d1\u5e03\u98de\u884c\u8f68\u8ff9<\/span><br \/>\n            <span class=\"token key atrule\">scan_publish_en<\/span><span class=\"token punctuation\">:<\/span>  <span class=\"token boolean important\">true<\/span>      <span class=\"token comment\"># \u53d1\u5e03\u914d\u51c6\u70b9\u4e91<\/span><br \/>\n            <span class=\"token key atrule\">dense_publish_en<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token boolean important\">true<\/span>      <span class=\"token comment\"># \u53d1\u5e03\u7a20\u5bc6\u70b9\u4e91&#xff08;false \u5219\u53ea\u53d1\u964d\u91c7\u6837\u540e\u7684&#xff09;<\/span><br \/>\n            <span class=\"token key atrule\">scan_bodyframe_pub_en<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token boolean important\">false<\/span> <span class=\"token comment\"># \u53d1\u5e03 body \u7cfb\u4e0b\u7684\u5f53\u524d\u5e27\u70b9\u4e91<\/span><\/p>\n<p>        <span class=\"token key atrule\">pcd_save<\/span><span class=\"token punctuation\">:<\/span><br \/>\n            <span class=\"token key atrule\">pcd_save_en<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token boolean important\">false<\/span>          <span class=\"token comment\"># \u662f\u5426\u4fdd\u5b58 PCD \u5730\u56fe\u6587\u4ef6<\/span><br \/>\n            <span class=\"token key atrule\">interval<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">-1<\/span>                <span class=\"token comment\"># -1&#061;\u6240\u6709\u5e27\u5b58\u5230\u4e00\u4e2a\u6587\u4ef6&#xff08;\u53ef\u80fd\u5185\u5b58\u6ea2\u51fa&#xff09;<\/span><\/p>\n<ul>\n<li>\u91cd\u70b9\u53c2\u6570\u89e3\u8bfb&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>\u53c2\u6570\u9ed8\u8ba4\u503c\u542b\u4e49\u4e3a\u4ec0\u4e48\u8981\u6539\/\u4e0d\u6539<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">lidar_type<\/td>\n<td align=\"left\">2<\/td>\n<td align=\"left\">LiDAR \u7c7b\u578b<\/td>\n<td align=\"left\">\u8bbe\u4e3a 2&#xff08;Velodyne&#xff09;&#xff0c;\u8d70 velodyne_handler&#xff0c;\u4e0d\u53bb\u78b0 Livox \u7684 CustomMsg<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">scan_line<\/td>\n<td align=\"left\">16<\/td>\n<td align=\"left\">\u626b\u63cf\u7ebf\u6570<\/td>\n<td align=\"left\">VLP-16 \u5c31\u662f 16 \u7ebf&#xff0c;\u7528\u4e8e\u53bb\u7578\u53d8\u65f6\u7684\u7ebf\u675f\u5206\u914d<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">extrinsic_T<\/td>\n<td align=\"left\">[0,0,0.12]<\/td>\n<td align=\"left\">LiDAR \u2192 IMU \u5916\u53c2\u5e73\u79fb<\/td>\n<td align=\"left\">VLP-16 \u88c5\u5728\u98de\u673a\u9876\u90e8 12cm \u5904&#xff08;\u53c2\u8003\u7b2c\u4e00\u671f\u6a21\u578b\u4e2d\u7684 pose&gt;0 0 0.12&lt;\/pose&gt;&#xff09;<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">blind<\/td>\n<td align=\"left\">0.5<\/td>\n<td align=\"left\">\u76f2\u533a\u8ddd\u79bb<\/td>\n<td align=\"left\">VLP-16 \u6700\u77ed\u6709\u6548\u8ddd\u79bb\u7ea6 0.5m&#xff0c;\u5c0f\u4e8e\u8fd9\u4e2a\u8ddd\u79bb\u7684\u70b9\u566a\u58f0\u5927&#xff0c;\u76f4\u63a5\u4e22\u5f03<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">filter_size_surf<\/td>\n<td align=\"left\">0.5<\/td>\n<td align=\"left\">\u964d\u91c7\u6837\u4f53\u7d20<\/td>\n<td align=\"left\">\u4eff\u771f\u70b9\u4e91\u7a20\u5bc6&#xff08;720\u00d716&#061;11520 pts\/frame&#xff09;&#xff0c;0.5m \u964d\u91c7\u6837\u540e\u7ea6\u51e0\u767e\u4e2a\u70b9&#xff0c;\u591f\u7528\u4e14\u4e0d\u5361<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">acc_cov \/ gyr_cov<\/td>\n<td align=\"left\">0.1<\/td>\n<td align=\"left\">IMU \u566a\u58f0\u534f\u65b9\u5dee<\/td>\n<td align=\"left\">\u4eff\u771f IMU \u566a\u58f0\u5f88\u5c0f&#xff0c;\u8bbe\u592a\u5927\u6ee4\u6ce2\u4f1a\u4e0d\u4fe1\u4efb IMU&#xff0c;\u592a\u5c0f\u4f1a\u8fc7\u62df\u5408<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">extrinsic_est_en<\/td>\n<td align=\"left\">false<\/td>\n<td align=\"left\">\u5728\u7ebf\u4f30\u8ba1\u5916\u53c2<\/td>\n<td align=\"left\">\u4eff\u771f\u4e2d\u5916\u53c2\u662f\u7cbe\u786e\u5df2\u77e5\u7684&#xff08;\u6211\u4eec\u81ea\u5df1\u5728 SDF \u91cc\u8bbe\u7684&#xff09;&#xff0c;\u4e0d\u9700\u8981\u5728\u7ebf\u4f30\u8ba1<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<p>\u53c2\u6570\u8c03\u4f18\u7684\u6838\u5fc3\u539f\u5219&#xff1a;\u4eff\u771f\u91cc\u566a\u58f0\u5c0f\u3001\u5916\u53c2\u7cbe\u786e \u2192 \u534f\u65b9\u5dee\u53ef\u4ee5\u8bbe\u5c0f\u3001\u5916\u53c2\u4f30\u8ba1\u5173\u95ed\u3002\u5b9e\u7269\u91cc IMU \u6709 bias\u3001\u5916\u53c2\u9760\u624b\u5de5\u91cf \u2192 \u534f\u65b9\u5dee\u8981\u653e\u5927\u3001\u5916\u53c2\u4f30\u8ba1\u6253\u5f00<\/p>\n<h5>3-5 RViz2 \u53ef\u89c6\u5316<\/h5>\n<ul>\n<li>\u5728\u539f\u6709\u7684\u7b2c\u4e00\u671f px4_sim.rviz \u57fa\u7840\u4e0a&#xff0c;\u65b0\u589e\u4e86 3 \u4e2a FAST-LIO2 \u4e13\u5c5e\u663e\u793a\u9762\u677f&#xff1a;<\/li>\n<\/ul>\n<table>\n<tr>\u663e\u793a\u8bdd\u9898\u989c\u8272\u8bf4\u660e<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">FAST-LIO2 Map<\/td>\n<td align=\"left\">\/cloud_registered<\/td>\n<td align=\"left\">\u9ec4\u8272<\/td>\n<td align=\"left\">\u914d\u51c6\u540e\u7684\u5168\u5c40\u70b9\u4e91\u5730\u56fe&#xff0c;\u968f\u65f6\u95f4\u7d2f\u79ef<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">FAST-LIO2 Odometry<\/td>\n<td align=\"left\">\/Odometry<\/td>\n<td align=\"left\">\u6a59\u8272\u7bad\u5934<\/td>\n<td align=\"left\">\u5b9e\u65f6\u4f4d\u59ff\u4f30\u8ba1&#xff08;\u4fdd\u7559 500 \u5e27\u5386\u53f2&#xff09;<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">FAST-LIO2 Path<\/td>\n<td align=\"left\">\/path<\/td>\n<td align=\"left\">\u6a59\u7ea2\u8272\u7ebf<\/td>\n<td align=\"left\">\u98de\u884c\u8f68\u8ff9\u7ebf<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<ul>\n<li>\u539f\u6709\u7684 PX4 Velodyne \u70b9\u4e91&#xff08;\u5f69\u8679\u8272&#xff0c;\/velodyne\/points&#xff09;\u548c PX4 Odometry&#xff08;\u7eff\u8272&#xff0c;\/fmu\/out\/vehicle_odometry&#xff09;\u7ee7\u7eed\u4fdd\u7559&#xff0c;\u53ef\u4ee5\u76f4\u89c2\u5bf9\u6bd4 PX4 EKF \u91cc\u7a0b\u8ba1\u548c FAST-LIO2 \u91cc\u7a0b\u8ba1\u7684\u5dee\u5f02<\/li>\n<li>Fixed Frame \u4ece base_link \u6539\u4e3a camera_init\u2014\u2014\u56e0\u4e3a FAST-LIO2 \u7684\u6240\u6709\u8f93\u51fa\u90fd\u5728 camera_init \u7cfb\u4e0b<\/li>\n<li>\u5b8c\u6574 RViz \u914d\u7f6e\u6587\u4ef6\u89c1 px4_sim.rviz&#xff08;\u5df2\u5728\u7b2c\u4e00\u671f\u6587\u7ae0\u4e2d\u7ed9\u51fa&#xff0c;\u672c\u671f\u5728\u539f\u6709\u57fa\u7840\u4e0a\u589e\u52a0\u4e86\u4e09\u6761 FAST-LIO2 \u7684 Display&#xff09;<\/li>\n<\/ul>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260725171718-6a64ef9ee8d2c.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260725171720-6a64efa07db9c.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<p><img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260725171722-6a64efa216e46.png\" alt=\"\u8bf7\u6dfb\u52a0\u56fe\u7247\u63cf\u8ff0\" \/><\/p>\n<h5>3-6 \u5e38\u89c1\u8b66\u544a&#xff1a;No point, skip this scan!<\/h5>\n<ul>\n<li>\u5728\u8c03\u8bd5\u7b97\u6cd5\u65f6&#xff0c;\u4f60\u53ef\u80fd\u4f1a\u5728\u7ec8\u7aef\u9891\u7e41\u770b\u5230\u8fd9\u6761\u9ec4\u8272\u8b66\u544a&#xff1a;<\/li>\n<\/ul>\n<p>[fastlio_mapping-1] [WARN] [laser_mapping]: No point, skip this scan!<\/p>\n<ul>\n<li>\u8fd9\u6761\u8b66\u544a\u5728 src\/laserMapping.cpp \u7684 timer_callback \u4e2d\u6709 \u4e24\u5904\u89e6\u53d1\u70b9<\/li>\n<\/ul>\n<h6>3-6-1 IMU \u53bb\u7578\u53d8\u540e\u6ca1\u6709\u6709\u6548\u70b9&#xff08;\u7b2c 1069 \u884c&#xff09;<\/h6>\n<p>p_imu<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">Process<\/span><span class=\"token punctuation\">(<\/span>Measures<span class=\"token punctuation\">,<\/span> kf<span class=\"token punctuation\">,<\/span> feats_undistort<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\nstate_point <span class=\"token operator\">&#061;<\/span> kf<span class=\"token punctuation\">.<\/span><span class=\"token function\">get_x<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\npos_lid <span class=\"token operator\">&#061;<\/span> state_point<span class=\"token punctuation\">.<\/span>pos <span class=\"token operator\">&#043;<\/span> state_point<span class=\"token punctuation\">.<\/span>rot <span class=\"token operator\">*<\/span> state_point<span class=\"token punctuation\">.<\/span>offset_T_L_I<span class=\"token punctuation\">;<\/span><\/p>\n<p><span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>feats_undistort<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">empty<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">||<\/span> <span class=\"token punctuation\">(<\/span>feats_undistort <span class=\"token operator\">&#061;&#061;<\/span> <span class=\"token constant\">NULL<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token function\">RCLCPP_WARN<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">this<\/span><span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_logger<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;No point, skip this scan!\\\\n&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">return<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<ul>\n<li>p_imu-&gt;Process() \u505a\u4e24\u4ef6\u4e8b&#xff1a;\u7528 IMU \u5bf9 LiDAR \u70b9\u505a\u8fd0\u52a8\u8865\u507f&#xff08;\u53bb\u7578\u53d8&#xff09;&#xff0c;\u7136\u540e\u7528 ESKF \u9884\u6d4b\u4f4d\u59ff<\/li>\n<li>\u5982\u679c\u53bb\u7578\u53d8\u540e feats_undistort \u4e3a\u7a7a&#xff0c;\u8bf4\u660e\u8fd9\u4e00\u5e27 LiDAR \u7684\u6240\u6709\u70b9\u5728\u8865\u507f\u8fc7\u7a0b\u4e2d\u88ab\u5e72\u6389\u4e86\u3002\u6700\u5e38\u89c1\u7684\u539f\u56e0&#xff1a;\n<ul>\n<li>\u76f2\u533a\u8fc7\u6ee4\u592a\u6fc0\u8fdb&#xff1a;blind \u53c2\u6570\u8bbe\u592a\u5927&#xff08;\u6bd4\u5982\u8bbe\u4e86 2m&#xff0c;\u98de\u673a\u8d77\u98de\u65f6\u79bb\u5730\u592a\u8fd1&#xff09;<\/li>\n<li>IMU buffer \u8fd8\u6ca1\u5c31\u7eea&#xff1a;\u7b2c\u4e00\u5e27 LiDAR \u5230\u8fbe\u65f6 IMU \u6570\u636e\u4e0d\u591f\u505a\u8865\u507f&#xff0c;Process \u76f4\u63a5\u8fd4\u56de\u4e86<\/li>\n<li>\u70b9\u4e91\u672c\u6765\u5c31\u5c11&#xff1a;LiDAR \u5c04\u7ebf\u5168\u90e8\u6253\u5728\u76f2\u533a\u5185&#xff08;\u6bd4\u5982\u98de\u673a\u8d34\u7740\u7f51\u7b3c\u5899\u58c1\u8d77\u98de&#xff09;<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<h6>3-6-2 \u964d\u91c7\u6837\u540e\u6709\u6548\u70b9\u592a\u5c11&#xff08;\u7b2c 1107 \u884c&#xff09;<\/h6>\n<p>downSizeFilterSurf<span class=\"token punctuation\">.<\/span><span class=\"token function\">setInputCloud<\/span><span class=\"token punctuation\">(<\/span>feats_undistort<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\ndownSizeFilterSurf<span class=\"token punctuation\">.<\/span><span class=\"token function\">filter<\/span><span class=\"token punctuation\">(<\/span><span class=\"token operator\">*<\/span>feats_down_body<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\nfeats_down_size <span class=\"token operator\">&#061;<\/span> feats_down_body<span class=\"token operator\">-&gt;<\/span>points<span class=\"token punctuation\">.<\/span><span class=\"token function\">size<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p><span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>feats_down_size <span class=\"token operator\">&lt;<\/span> <span class=\"token number\">5<\/span><span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token function\">RCLCPP_WARN<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">this<\/span><span class=\"token operator\">-&gt;<\/span><span class=\"token function\">get_logger<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token string\">&#034;No point, skip this scan!\\\\n&#034;<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token keyword\">return<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<ul>\n<li>\n<p>\u53bb\u7578\u53d8\u540e\u6709\u70b9&#xff0c;\u4f46\u7ecf\u8fc7\u4f53\u7d20\u964d\u91c7\u6837&#xff08;filter_size_surf \u9ed8\u8ba4 0.5m&#xff09;\u540e&#xff0c;\u540c\u4e00\u4e2a\u4f53\u7d20\u683c\u5b50\u91cc\u53ea\u4fdd\u7559\u4e00\u4e2a\u70b9<\/p>\n<\/li>\n<li>\n<p>\u5982\u679c\u5269\u4f59\u70b9\u6570 &lt; 5&#xff0c;ESKF \u89c2\u6d4b\u66f4\u65b0\u76f4\u63a5\u8df3\u8fc7\u2014\u20145 \u4e2a\u70b9\u662f ikd-Tree \u641c\u7d22\u548c\u5e73\u9762\u62df\u5408\u7684\u6700\u4f4e\u95e8\u69db&#xff1a;<\/p>\n<ul>\n<li>ikd-Tree \u5bf9\u6bcf\u4e2a\u70b9\u641c NUM_MATCH_POINTS&#xff08;\u9ed8\u8ba4 5&#xff09;\u4e2a\u6700\u8fd1\u90bb\u6765\u62df\u5408\u5e73\u9762<\/li>\n<li>\u5982\u679c\u6574\u4e2a scan \u7684\u964d\u91c7\u6837\u70b9\u90fd\u4e0d\u591f 5 \u4e2a&#xff0c;\u90a3\u4e00\u4e2a\u5e73\u9762\u90fd\u62df\u5408\u4e0d\u51fa\u6765&#xff0c;ESKF \u89c2\u6d4b\u66f4\u65b0\u65e0\u610f\u4e49<\/li>\n<\/ul>\n<\/li>\n<li>\n<p>\u8bf4\u4eba\u8bdd\u5c31\u662f&#xff1a;\u98de\u673a\u79bb\u5730 0.3m \u8d77\u98de\u3001LiDAR \u76f2\u533a 0.5m \u2192 \u6240\u6709\u70b9\u90fd\u88ab\u76f2\u533a\u8fc7\u6ee4 \u2192 \u6ca1\u70b9\u3002\u6216\u8005\u4f60\u5f00\u7740\u98de\u673a\u9876\u5728\u5929\u82b1\u677f\u4e0a\u2014\u2014LiDAR \u770b\u51fa\u53bb\u53ea\u6709\u5929\u82b1\u677f\u90a3\u4e00\u4e2a\u5e73\u9762&#xff0c;\u964d\u91c7\u6837\u540e\u51d1\u4e0d\u591f 5 \u4e2a\u70b9 \u2192 \u8df3\u8fc7<\/p>\n<\/li>\n<li>\n<p>\u5982\u4f55\u89e3\u51b3&#xff1a;<\/p>\n<ul>\n<li>\u628a blind \u8c03\u5c0f&#xff08;\u4eff\u771f\u91cc\u53ef\u4ee5\u8bbe 0.1m&#xff0c;\u5b9e\u7269 Mid-360 \u76f2\u533a\u7ea6 0.1m&#xff09;<\/li>\n<li>\u8d77\u98de\u524d\u786e\u4fdd\u98de\u673a\u56db\u5468\u6709\u8db3\u591f\u7684\u7ed3\u6784&#xff08;\u7f51\u7b3c\u7684\u7eb5\u6a2a\u6746\u5c31\u662f\u5f88\u597d\u7684\u70b9\u4e91\u6765\u6e90&#xff09;&#xff0c;\u4e0d\u8981\u5728\u5b8c\u5168\u7a7a\u65f7\u7684 Gazebo \u7a7a\u767d\u4e16\u754c\u91cc\u98de<\/li>\n<li>\u5982\u679c\u6301\u7eed\u62a5\u9519&#xff0c;\u68c0\u67e5 \/velodyne\/points \u662f\u5426\u6b63\u5e38\u53d1\u5e03&#xff1a;ros2 topic hz \/velodyne\/points<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<h5>3-7 \u5b9e\u673a\u90e8\u7f72 \u2014 livox_ros_driver2 \u7684\u56db\u4e2a\u5751<\/h5>\n<ul>\n<li>\u5982\u679c\u4f60\u5728\u5b9e\u7269\u4e0a\u63a5 Livox Mid-360&#xff08;\u89c8\u6c83\u56fa\u6001\u6fc0\u5149\u96f7\u8fbe&#xff09;&#xff0c;\u4f1a\u76f4\u63a5\u9047\u5230 livox_ros_driver2\u3002\u4eff\u771f\u73af\u8282\u6211\u4eec\u523b\u610f\u7ed5\u5f00\u4e86\u5b83&#xff0c;\u4f46\u5b9e\u7269\u7ed5\u4e0d\u8fc7\u53bb<\/li>\n<li>\u4ee5\u4e0b\u662f\u5b9e\u673a\u8e29\u5751\u5b9e\u5f55\u2014\u2014\u95ee\u9898\u8868\u73b0\u4e3a\u70b9\u4e91\u5ef6\u8fdf 2000-3300ms\u3001FAST-LIO2 \u5b8c\u5168\u4e0d\u53d1 odom<\/li>\n<\/ul>\n<h6>3-7-1 PTP \u65f6\u949f\u540c\u6b65\u4e0d\u5b8c\u6574<\/h6>\n<ul>\n<li>Livox Mid-360 \u4f7f\u7528 PTP&#xff08;Precision Time Protocol&#xff0c;\u7cbe\u5bc6\u65f6\u95f4\u534f\u8bae&#xff09;\u505a\u65f6\u949f\u540c\u6b65&#xff0c;master \u7aef&#xff08;\u673a\u8f7d\u7535\u8111&#xff09;\u901a\u8fc7 ptp4l \u4e0e\u96f7\u8fbe slave \u7aef\u4fdd\u6301\u7eb3\u79d2\u7ea7\u4e00\u81f4<\/li>\n<li>\u51fa\u5382\u9ed8\u8ba4\u7684 ptp4l.service \u53ea\u6709 ptp4l -i eth0 -S -m \u57fa\u7840\u53c2\u6570&#xff0c;\u7f3a\u5c11 -f automotive-master.cfg \u914d\u7f6e\u6587\u4ef6<\/li>\n<li>\u6ca1\u6709\u6b63\u786e\u914d\u7f6e\u7684\u7ed3\u679c&#xff1a;\u4ea4\u6362\u673a\u4e0a PTP \u62a5\u6587\u5728\u8d70&#xff0c;\u4f46\u6570\u636e\u65f6\u949f \u672a\u9501\u5b9a&#xff0c;\u96f7\u8fbe\u4ee5\u81ea\u7531\u8fd0\u884c\u6a21\u5f0f\u53d1\u6570\u636e\u2014\u2014\u65f6\u95f4\u6233\u4ee5 ~31ms\/s \u7684\u901f\u7387\u6301\u7eed\u6f02\u79fb<\/li>\n<li>\u8bf4\u4eba\u8bdd\u5c31\u662f&#xff1a;\u96f7\u8fbe\u548c\u7535\u8111\u7684&#034;\u624b\u8868&#034;\u5bf9\u4e0d\u4e0a&#xff0c;\u6bcf\u8fc7\u4e00\u79d2\u5dee 31ms&#xff0c;\u51e0\u5206\u949f\u540e\u5dee\u597d\u51e0\u79d2&#xff0c;\u70b9\u4e91\u65f6\u95f4\u6233\u5168\u90e8\u9519\u4e71<\/li>\n<\/ul>\n<p><span class=\"token comment\"># \/etc\/linuxptp\/mid360_master.cfg<\/span><br \/>\n<span class=\"token punctuation\">[<\/span>global<span class=\"token punctuation\">]<\/span><br \/>\n<span class=\"token comment\"># E2E (End-to-End) \u5ef6\u8fdf\u6d4b\u91cf\u6a21\u5f0f<\/span><br \/>\ndelay_mechanism        E2E<br \/>\n<span class=\"token comment\"># 125ms \u5feb\u901f Sync \u95f4\u9694 \u2014 Mid-360 \u652f\u6301<\/span><br \/>\nlogSyncInterval        <span class=\"token number\">1<\/span><br \/>\n<span class=\"token comment\"># \u9ad8\u65f6\u949f\u8d28\u91cf\u58f0\u660e (Class 6, Clock Accuracy 0x20)<\/span><br \/>\nclockClass             <span class=\"token number\">6<\/span><br \/>\nclockAccuracy          0x20<\/p>\n<p># \/etc\/systemd\/system\/ptp4l.service<br \/>\nExecStart&#061;\/usr\/sbin\/ptp4l -f \/etc\/linuxptp\/mid360_master.cfg -i eth0 -S -m<\/p>\n<ul>\n<li>\u4fee\u590d\u540e ptp4l \u5f00\u673a\u81ea\u542f&#xff0c;\u96f7\u8fbe\u65f6\u949f\u9501\u5b9a&#xff0c;\u65f6\u95f4\u504f\u79fb\u4ece 2.2 \u79d2\u964d\u5230 60ms \u4ee5\u5185<\/li>\n<\/ul>\n<h6>3-7-2 CustomMsg vs PointCloud2<\/h6>\n<ul>\n<li>livox_ros_driver2 \u7684 xfer_format \u53c2\u6570\u63a7\u5236\u6570\u636e\u8f93\u51fa\u683c\u5f0f&#xff1a;\n<ul>\n<li>xfer_format &#061; 1&#xff1a;CustomMsg&#xff08;Livox \u79c1\u6709\u683c\u5f0f&#xff0c;\u5305\u542b\u9010\u70b9\u65f6\u95f4\u6233\u3001tag\u3001line \u7b49\u4e30\u5bcc\u4fe1\u606f&#xff09;<\/li>\n<li>xfer_format &#061; 0&#xff1a;PointCloud2&#xff08;\u6807\u51c6 ROS2 \u70b9\u4e91\u683c\u5f0f&#xff0c;pcl::PointXYZI \u7ed3\u6784&#xff09;<\/li>\n<\/ul>\n<\/li>\n<li>\u5b9e\u6d4b CustomMsg \u5ef6\u8fdf ~200ms\u3001PointCloud2 \u5ef6\u8fdf ~60ms<\/li>\n<li>CustomMsg \u6d88\u606f\u4f53\u66f4\u5927\u3001\u5e8f\u5217\u5316\/\u53cd\u5e8f\u5217\u5316\u5f00\u9500\u66f4\u9ad8&#xff0c;\u52a0\u4e0a\u9a71\u52a8\u5185\u90e8\u7684\u961f\u5217\u8c03\u5ea6&#xff0c;\u9ad8\u5e27\u7387\u4e0b\u5bb9\u6613\u5806\u79ef\u2014\u2014\u8fd9\u662f\u70b9\u4e91\u5ef6\u8fdf\u7684\u4e3b\u8981\u6765\u6e90<\/li>\n<li>\u8bf4\u4eba\u8bdd\u5c31\u662f&#xff1a;CustomMsg \u4fe1\u606f\u91cf\u867d\u5927\u4f46\u592a\u91cd&#xff0c;200Hz \u7684 IMU \u8fd8\u6ca1\u5565&#xff0c;200Hz \u7684\u70b9\u4e91\u5c31\u625b\u4e0d\u4f4f\u4e86\u3002\u5b9e\u7269\u80fd\u8dd1 PointCloud2 \u5c31\u522b\u7528 CustomMsg<\/li>\n<\/ul>\n<p>\u5173\u952e\u7ed3\u8bba&#xff1a;\u5b9e\u7269 Mid-360 \u4f18\u5148\u7528 xfer_format&#061;0&#xff08;PointCloud2&#xff09;\u3002CustomMsg \u7684\u9010\u70b9\u65f6\u95f4\u6233\u5728\u5927\u591a\u6570\u573a\u666f\u4e0b\u4e0d\u662f\u521a\u9700\u2014\u2014FAST-LIO2 \u7528 IMU \u53cd\u5411\u8865\u507f\u53bb\u7578\u53d8&#xff0c;\u4e0d\u4f9d\u8d56 Livox \u79c1\u6709\u65f6\u95f4\u6233<\/p>\n<h6>3-7-3 FAST-LIO2 \u914d\u7f6e lidar_type \u7684\u6620\u5c04\u9677\u9631<\/h6>\n<ul>\n<li>\u8bbe xfer_format&#061;0 \u540e\u96f7\u8fbe\u53d1\u7684\u662f\u6807\u51c6 PointCloud2&#xff0c;\u4f46 FAST-LIO2 \u7684 YAML \u914d\u7f6e\u91cc\u6709\u4e00\u4e2a\u5bb9\u6613\u8e29\u7684\u5751&#xff1a;\n<ul>\n<li>lidar_type: 1 \u2192 \u8d70 AVIA \u5206\u652f \u2192 \u8ba2\u9605 CustomMsg \u2192 \u6536\u4e0d\u5230 PointCloud2 \u2192 \u6c38\u8fdc\u6ca1\u6570\u636e \u2192 \u4e0d\u53d1 odom<\/li>\n<li>lidar_type: 4 \u2192 \u8d70 mid360_handler \u2192 \u5199\u6b7b\u4e86 reflectivity \u5b57\u6bb5&#xff0c;\u4f46\u9a71\u52a8 2.0 \u7528\u7684\u662f intensity \u5b57\u6bb5 \u2192 \u70b9\u4e91\u5f3a\u5ea6\u4e3a 0&#xff0c;\u4f46\u8fd8\u80fd\u8dd1<\/li>\n<\/ul>\n<\/li>\n<li>\u6700\u53ef\u9760\u7684\u65b9\u6848&#xff1a;\u6539\u7528 lidar_type: 5&#xff08;\u5982\u679c\u4ee3\u7801\u5df2\u6269\u5c55&#xff09;&#xff0c;\u6216\u5c06\u5176\u6620\u5c04\u5230 default_handler\u2014\u2014\u76f4\u63a5\u7528\u6807\u51c6 pcl::PointXYZI \u89e3\u6790&#xff0c;\u4e0d\u5173\u5fc3 tag\/line\/reflectivity \u7b49\u79c1\u6709\u5b57\u6bb5<\/li>\n<li>FAST-LIO2 \u7684\u672c\u8d28\u662f\u76f4\u63a5\u6cd5\u2014\u2014\u5b83\u4e0d\u4f9d\u8d56\u70b9\u4e91\u7684\u7ebf\u53f7\u6765\u63d0\u53d6\u7279\u5f81&#xff0c;\u6240\u4ee5 default_handler \u5b8c\u5168\u591f\u7528<\/li>\n<\/ul>\n<p><span class=\"token comment\"># mid360.yaml \u2014 \u5173\u952e\u53c2\u6570<\/span><br \/>\n<span class=\"token key atrule\">common<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    <span class=\"token key atrule\">lid_topic<\/span><span class=\"token punctuation\">:<\/span>  <span class=\"token string\">&#034;\/livox\/lidar&#034;<\/span><br \/>\n    <span class=\"token key atrule\">imu_topic<\/span><span class=\"token punctuation\">:<\/span>  <span class=\"token string\">&#034;\/livox\/imu&#034;<\/span><\/p>\n<p><span class=\"token key atrule\">preprocess<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    <span class=\"token key atrule\">lidar_type<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">4<\/span>         <span class=\"token comment\"># \u8d70 mid360_handler&#xff08;\u6216 5 \u8d70 default_handler&#xff09;<\/span><br \/>\n    <span class=\"token key atrule\">scan_line<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">4<\/span>          <span class=\"token comment\"># Mid-360 4 \u7ebf\u975e\u91cd\u590d\u626b\u63cf<\/span><br \/>\n    <span class=\"token key atrule\">blind<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">0.1<\/span>            <span class=\"token comment\"># Mid-360 \u76f2\u533a 0.1m<\/span><br \/>\n    <span class=\"token key atrule\">point_filter_num<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token number\">1<\/span><\/p>\n<p><span class=\"token key atrule\">mapping<\/span><span class=\"token punctuation\">:<\/span><br \/>\n    <span class=\"token key atrule\">extrinsic_T<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token punctuation\">[<\/span><span class=\"token number\">0.0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.0<\/span><span class=\"token punctuation\">]<\/span>   <span class=\"token comment\"># Mid-360 \u5185\u7f6e IMU&#xff0c;\u5916\u53c2\u5e73\u79fb ~ 0<\/span><br \/>\n    <span class=\"token key atrule\">extrinsic_R<\/span><span class=\"token punctuation\">:<\/span> <span class=\"token punctuation\">[<\/span><span class=\"token number\">1.<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                  <span class=\"token number\">0.<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">1.<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.<\/span><span class=\"token punctuation\">,<\/span><br \/>\n                  <span class=\"token number\">0.<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">0.<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">1.<\/span><span class=\"token punctuation\">]<\/span><\/p>\n<h6>3-7-4 livox_ros_driver2 \u7248\u672c\u5dee\u5f02<\/h6>\n<ul>\n<li>\n<p>\u5b98\u65b9 livox_ros_driver2 \u6700\u65b0\u7248\u662f v1.2.6&#xff08;\u652f\u6301 Jazzy &#043; MID360s &#043; \u4fee\u590d\u82e5\u5e72 bug&#xff09;<\/p>\n<\/li>\n<li>\n<p>\u672c\u5730\u53ef\u80fd\u6b8b\u7559\u65e7\u7248 v1.2.4&#xff0c;\u4e14 FAST-LIO2 \u7684 CMake \u548c package.xml \u6df7\u5728 driver \u7684 workspace \u91cc&#xff0c;\u8026\u5408\u5ea6\u6781\u9ad8\u2014\u2014\u5347\u7ea7 driver \u53ef\u80fd\u7834\u574f FAST-LIO2 \u7684\u7f16\u8bd1<\/p>\n<\/li>\n<li>\n<p>\u5efa\u8bae&#xff1a;driver \u548c SLAM \u7b97\u6cd5\u653e\u5728 \u4e0d\u540c workspace&#xff0c;\u7528\u6807\u51c6 ROS2 ament_cmake \u7684 find_package \u89e3\u8026\u3002driver \u4f5c\u4e3a\u72ec\u7acb\u7684 system \u5305\u5b89\u88c5&#xff08;sudo make install \u6216 rosdep&#xff09;&#xff0c;FAST-LIO2 \u53ea\u901a\u8fc7\u6d88\u606f\u7c7b\u578b\u4f9d\u8d56\u5b83<\/p>\n<\/li>\n<li>\n<p>\u4fee\u590d\u7ed3\u679c&#xff1a;<\/p>\n<\/li>\n<\/ul>\n<table>\n<tr>\u6307\u6807\u4fee\u590d\u524d\u4fee\u590d\u540e<\/tr>\n<tbody>\n<tr>\n<td align=\"left\">\u70b9\u4e91\u5ef6\u8fdf<\/td>\n<td align=\"center\">2200ms \u6301\u7eed\u6f02\u79fb<\/td>\n<td align=\"center\">~60ms \u7a33\u5b9a<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">Odometry<\/td>\n<td align=\"center\">\u4e0d\u53d1<\/td>\n<td align=\"center\">20Hz \u6b63\u5e38<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">ptp4l<\/td>\n<td align=\"center\">\u7f3a\u914d\u7f6e<\/td>\n<td align=\"center\">\u5f00\u673a\u81ea\u542f&#xff0c;\u6b63\u786e\u914d\u7f6e<\/td>\n<\/tr>\n<tr>\n<td align=\"left\">\u6570\u636e\u683c\u5f0f<\/td>\n<td align=\"center\">CustomMsg<\/td>\n<td align=\"center\">PointCloud2<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<ul>\n<li>\u9644&#xff1a;\u70b9\u4e91\u5ef6\u8fdf\u68c0\u6d4b\u811a\u672c&#xff08;\u4fdd\u5b58\u4e3a check_lidar_delay.sh&#xff0c;\u653e\u5728\u5b9e\u673a\u4e0a\u8dd1&#xff09;&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token shebang important\">#!\/bin\/bash<\/span><br \/>\n<span class=\"token builtin class-name\">source<\/span> \/opt\/ros\/humble\/setup.bash<br \/>\n<span class=\"token builtin class-name\">source<\/span> \/root\/livox_ros_driver2\/install\/setup.bash<br \/>\n<span class=\"token builtin class-name\">source<\/span> \/root\/drone_ws\/install\/setup.bash <span class=\"token operator\"><span class=\"token file-descriptor important\">2<\/span>&gt;<\/span>\/dev\/null<br \/>\npython3 <span class=\"token parameter variable\">-c<\/span> <span class=\"token string\">&#034;<br \/>\nimport rclpy, time<br \/>\nrclpy.init()<br \/>\nn &#061; rclpy.create_node(&#039;dc&#039;)<\/p>\n<p>topic_type &#061; None<br \/>\nfor name, types in n.get_topic_names_and_types():<br \/>\n    if name &#061;&#061; &#039;\/livox\/lidar&#039;:<br \/>\n        topic_type &#061; types[0]<br \/>\n        break<\/p>\n<p>if &#039;CustomMsg&#039; in topic_type:<br \/>\n    from livox_ros_driver2.msg import CustomMsg<br \/>\n    def cb(msg):<br \/>\n        now &#061; time.time()<br \/>\n        s &#061; msg.header.stamp.sec &#043; msg.header.stamp.nanosec \/ 1e9<br \/>\n        print(f&#039;  delay&#061;{(now-s)*1000:.0f}ms&#039;)<br \/>\n    n.create_subscription(CustomMsg, &#039;\/livox\/lidar&#039;, cb, 1)<br \/>\nelse:<br \/>\n    from sensor_msgs.msg import PointCloud2<br \/>\n    def cb(msg):<br \/>\n        now &#061; time.time()<br \/>\n        s &#061; msg.header.stamp.sec &#043; msg.header.stamp.nanosec \/ 1e9<br \/>\n        print(f&#039;  delay&#061;{(now-s)*1000:.0f}ms&#039;)<br \/>\n    n.create_subscription(PointCloud2, &#039;\/livox\/lidar&#039;, cb, 1)<\/p>\n<p>print(f&#039;Topic: {topic_type} | Ctrl&#043;C to stop&#039;)<br \/>\nrclpy.spin(n)<br \/>\n&#034;<\/span><\/p>\n<ul>\n<li>\u8fd9\u4e2a\u811a\u672c\u81ea\u52a8\u68c0\u6d4b \/livox\/lidar \u7684\u6d88\u606f\u7c7b\u578b&#xff08;CustomMsg \u8fd8\u662f PointCloud2&#xff09;&#xff0c;\u7136\u540e\u6253\u5370\u6bcf\u6761\u70b9\u4e91\u6d88\u606f\u7684\u5ef6\u8fdf delay &#061; now &#8211; stamp<\/li>\n<li>\u6b63\u5e38\u503c\u5e94\u8be5 &lt; 100ms\u3002\u5982\u679c\u6301\u7eed\u98d9\u5347&#xff08;\u6bcf\u8fc7\u4e00\u79d2\u52a0 ~30ms&#xff09;&#xff0c;\u8bf4\u660e PTP \u65f6\u949f\u6ca1\u9501\u2014\u2014\u56de\u5230 3-7-1 \u68c0\u67e5 ptp4l \u914d\u7f6e<\/li>\n<\/ul>\n<hr \/>\n<h3>4 \u8fdb\u9636\u73a9\u6cd5 \u2014 200Hz IMU \u524d\u63a8<\/h3>\n<h5>4-1 \u4e3a\u4ec0\u4e48\u9700\u8981\u9ad8\u9891\u91cc\u7a0b\u8ba1<\/h5>\n<ul>\n<li>\u9ed8\u8ba4\u7684 FAST-LIO2 \u91cc\u7a0b\u8ba1\u8f93\u51fa\u9891\u7387 &#061; LiDAR \u5e27\u7387\u3002\u6211\u4eec\u7684 VLP-16 \u662f 10Hz&#xff0c;\u4e5f\u5c31\u662f\u6bcf\u79d2\u53ea\u6709 10 \u4e2a\u4f4d\u59ff\u4f30\u8ba1<\/li>\n<li>\u5bf9\u4e8e\u7eaf\u5efa\u56fe\u6765\u8bf4\u8fd9\u6ca1\u95ee\u9898\u2014\u2014\u70b9\u4e91\u5730\u56fe\u4e0d\u9700\u8981\u9ad8\u9891\u66f4\u65b0\u3002\u4f46\u5982\u679c\u4f60\u8981\u628a\u91cc\u7a0b\u8ba1\u5582\u7ed9 PX4 \u7684 EKF2 \u505a\u72b6\u6001\u4f30\u8ba1&#xff08;\u66ff\u4ee3 GPS&#xff09;&#xff0c;10Hz \u8fdc\u8fdc\u4e0d\u591f<\/li>\n<li>PX4 EKF2 \u7684 IMU \u66f4\u65b0\u662f 200Hz&#043; \u7684&#xff0c;\u5b83\u671f\u671b\u5728\u6bcf\u6761 IMU \u6d4b\u91cf\u4e4b\u540e\u90fd\u80fd\u62ff\u5230\u4e00\u4e2a\u5bf9\u5e94\u7684\u91cc\u7a0b\u8ba1\u4f4d\u59ff\u6765\u505a\u878d\u5408\u3002\u5982\u679c\u91cc\u7a0b\u8ba1\u53ea\u6709 10Hz&#xff0c;EKF2 \u4f1a\u5728\u4e24\u6b21\u91cc\u7a0b\u8ba1\u4e4b\u95f4&#034;\u88f8\u5954&#034;\u2014\u2014\u7eaf\u9760 IMU \u79ef\u5206&#xff0c;\u53d1\u6563\u5f88\u5feb<\/li>\n<li>\u8bf4\u4eba\u8bdd\u5c31\u662f&#xff1a;\u98de\u673a\u6bcf 0.005 \u79d2&#xff08;200Hz&#xff09;\u95ee\u4e00\u6b21&#034;\u6211\u5728\u54ea&#xff1f;&#034;&#xff0c;\u4f46 FAST-LIO2 \u6bcf 0.1 \u79d2&#xff08;10Hz&#xff09;\u624d\u7b54\u4e00\u6b21\u2014\u2014\u5176\u4ed6 19 \u6b21\u90fd\u53ea\u80fd\u731c<\/li>\n<\/ul>\n<p>\u8fd9\u4e2a\u95ee\u9898\u7684\u672c\u8d28\u662f IMU \u548c LiDAR \u7684\u91c7\u6837\u9891\u7387\u4e0d\u5339\u914d&#xff1a;IMU 200Hz\u3001LiDAR 10Hz\u3002\u89e3\u51b3\u601d\u8def\u4e5f\u5f88\u76f4\u89c2\u2014\u2014IMU \u524d\u63a8&#xff1a;\u5728\u4e24\u6b21 LiDAR scan \u4e4b\u95f4&#xff0c;\u62ff\u6700\u65b0\u7684 ESKF \u72b6\u6001&#xff08;\u4f4d\u7f6e\u3001\u901f\u5ea6\u3001\u59ff\u6001\u3001bias&#xff09;\u505a\u8d77\u70b9&#xff0c;\u7528\u6bcf\u6761 IMU \u6d88\u606f\u505a\u4e00\u6b21\u4e2d\u503c\u79ef\u5206\u9884\u6d4b\u4e00\u4e2a\u4f4d\u59ff<\/p>\n<ul>\n<li>\u672c\u7b97\u6cd5\u53c2\u8003\u4e1c\u5317\u5927\u5b66 REAL_DRONE_400 \u5f00\u6e90\u9879\u76ee\u7684 fastPredictIMU \u5b9e\u73b0<\/li>\n<\/ul>\n<h5>4-2 \u6838\u5fc3\u601d\u8def<\/h5>\n<ul>\n<li>\u6574\u4e2a\u6d41\u7a0b\u5206\u4e3a\u4e24\u6bb5&#xff1a;<\/li>\n<\/ul>\n<p>IMU 200Hz \u2500\u2500\u2192 fastPredictIMU() \u2500\u2500\u2192 \/Odom_high_freq (200Hz)  \u2190 \u6bcf\u6761 IMU \u90fd\u53d1\u4e00\u6b21\u9884\u6d4b\u4f4d\u59ff<br \/>\n                   \u2191 \u62ff latest_* \u505a\u8d77\u70b9<br \/>\nLiDAR 10Hz \u2500\u2500\u2192 ESKF \u8fed\u4ee3 \u2500\u2500\u2192 updateLatestStates() \u7f13\u5b58 P\/Q\/V\/Ba\/Bg  \u2190 \u77eb\u6b63 bias&#xff0c;\u91cd\u7f6e\u8d77\u70b9<\/p>\n<ul>\n<li>\u4f4e\u901f\u73af\u8def&#xff08;10Hz LiDAR&#xff09;&#xff1a;\u6bcf\u6765\u4e00\u5e27\u70b9\u4e91&#xff0c;\u8dd1\u4e00\u6b21\u5b8c\u6574\u7684 ESKF \u8fed\u4ee3\u3002ESKF \u5229\u7528 LiDAR \u70b9\u5bf9\u9762\u7684\u89c2\u6d4b\u6b8b\u5dee&#xff0c;\u540c\u65f6\u4fee\u6b63\u4f4d\u7f6e\u3001\u901f\u5ea6\u3001\u59ff\u6001\u3001\u52a0\u901f\u5ea6 bias \u548c\u9640\u87ba\u4eea bias\u3002\u66f4\u65b0\u5b8c\u6bd5\u540e&#xff0c;\u8c03\u7528 updateLatestStates() \u628a\u6700\u65b0\u72b6\u6001\u5b58\u5230\u5168\u5c40\u7f13\u5b58\u91cc<\/li>\n<li>\u9ad8\u901f\u73af\u8def&#xff08;200Hz IMU&#xff09;&#xff1a;\u5728\u4e24\u6b21 LiDAR scan \u4e4b\u95f4&#xff0c;\u6bcf\u6536\u5230\u4e00\u6761 IMU \u6d88\u606f\u5c31\u8c03\u7528\u4e00\u6b21 fastPredictIMU()\u3002\u8fd9\u6761\u51fd\u6570\u7528 \u4e2d\u503c\u79ef\u5206 \u4ece\u7f13\u5b58\u7684 ESKF \u72b6\u6001\u5411\u524d\u63a8\u4e00\u6b65&#xff0c;\u9884\u6d4b\u5f53\u524d\u7684 PVQ&#xff0c;\u53d1\u5e03\u5230 \/Odom_high_freq<\/li>\n<li>\u8bf4\u4eba\u8bdd\u5c31\u662f&#xff1a;LiDAR \u6bcf 0.1 \u79d2\u6765\u4e00\u6b21&#034;\u6743\u5a01\u77eb\u6b63&#034;&#xff08;\u4fee\u6b63 bias\u3001\u4fee\u6b63\u6f02\u79fb&#xff09;&#xff0c;IMU \u5728\u4e2d\u95f4\u6bcf 0.005 \u79d2\u505a\u4e00\u6b21&#034;\u8f7b\u91cf\u9884\u6d4b&#034;&#xff08;\u7eaf\u79ef\u5206&#xff0c;\u4e0d\u6539 bias&#xff09;<\/li>\n<li>\u76f8\u6bd4\u4e8e\u539f\u672c\u7684 10Hz \u4ec5\u9760 LiDAR \u5e27\u4e4b\u95f4\u7684\u7eaf IMU \u88f8\u63a8&#xff08;ESKF \u5185\u90e8\u7684\u524d\u5411\u4f20\u64ad\u6ca1\u66b4\u9732\u51fa\u6765&#xff09;&#xff0c;\u73b0\u5728\u7684 200Hz \u663e\u5f0f\u524d\u63a8\u628a\u6bcf\u4e00\u6b21\u9884\u6d4b\u90fd\u53d8\u6210\u4e86\u4e00\u5e27\u53ef\u76f4\u63a5\u6d88\u8d39\u7684 \/Odom_high_freq&#xff0c;PX4 EKF2 \u6bcf 5ms \u5c31\u80fd\u62ff\u5230\u4e00\u4e2a\u4f4d\u59ff\u4f30\u8ba1&#xff0c;\u4e0d\u518d\u6709&#034;\u4e24\u6b21\u91cc\u7a0b\u8ba1\u4e4b\u95f4\u88f8\u5954 19 \u6b65&#034;\u7684\u95ee\u9898<\/li>\n<\/ul>\n<h5>4-3 \u4e2d\u503c\u79ef\u5206\u63a8\u5bfc<\/h5>\n<ul>\n<li>\u4ece\u65f6\u95f4 t_k&#xff08;\u4e0a\u4e00\u6b21\u9884\u6d4b\/LiDAR \u66f4\u65b0\u7684\u65f6\u523b&#xff09;\u5230 t_{k&#043;1}&#xff08;\u5f53\u524d IMU \u6d88\u606f\u5230\u8fbe\u65f6\u523b&#xff09;&#xff0c;\u6211\u4eec\u7f13\u5b58\u4e86&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>             P<\/p>\n<p>             k<\/p>\n<p>             \u2208<\/p>\n<p>              R<\/p>\n<p>              3<\/p>\n<p>            \u4e16\u754c\u7cfb\u4e0b\u7684\u4f4d\u7f6e<\/p>\n<p>             V<\/p>\n<p>             k<\/p>\n<p>             \u2208<\/p>\n<p>              R<\/p>\n<p>              3<\/p>\n<p>            \u4e16\u754c\u7cfb\u4e0b\u7684\u901f\u5ea6<\/p>\n<p>             R<\/p>\n<p>             k<\/p>\n<p>              w<\/p>\n<p>              b<\/p>\n<p>             \u2208<\/p>\n<p>             S<\/p>\n<p>             O<\/p>\n<p>             (<\/p>\n<p>             3<\/p>\n<p>             )<\/p>\n<p>            IMU\u00a0body\u00a0\u7cfb\u5230\u00a0world\u00a0\u7cfb\u7684\u65cb\u8f6c\u77e9\u9635<\/p>\n<p>               B<\/p>\n<p>               a<\/p>\n<p>              k<\/p>\n<p>             ,<\/p>\n<p>               B<\/p>\n<p>               g<\/p>\n<p>              k<\/p>\n<p>             \u2208<\/p>\n<p>              R<\/p>\n<p>              3<\/p>\n<p>            \u52a0\u901f\u5ea6\u8ba1\u548c\u9640\u87ba\u4eea\u7684\u00a0bias&#xff08;\u7531\u00a0ESKF\u00a0\u4f30\u8ba1&#xff09;<\/p>\n<p>              a<\/p>\n<p>              k<\/p>\n<p>              m<\/p>\n<p>             ,<\/p>\n<p>              \u03c9<\/p>\n<p>              k<\/p>\n<p>              m<\/p>\n<p>             \u2208<\/p>\n<p>              R<\/p>\n<p>              3<\/p>\n<p>            \u4e0a\u4e00\u5e27\u7684\u00a0IMU\u00a0\u539f\u59cb\u52a0\u901f\u5ea6\u548c\u89d2\u901f\u5ea6\u6d4b\u91cf\u503c<\/p>\n<p>         \\\\begin{aligned} \\\\mathbf{P}_k &amp;\\\\in \\\\mathbb{R}^3 &amp;\\\\text{\u4e16\u754c\u7cfb\u4e0b\u7684\u4f4d\u7f6e} \\\\\\\\ \\\\mathbf{V}_k &amp;\\\\in \\\\mathbb{R}^3 &amp;\\\\text{\u4e16\u754c\u7cfb\u4e0b\u7684\u901f\u5ea6} \\\\\\\\ \\\\mathbf{R}_k^{wb} &amp;\\\\in SO(3) &amp;\\\\text{IMU body \u7cfb\u5230 world \u7cfb\u7684\u65cb\u8f6c\u77e9\u9635} \\\\\\\\ \\\\mathbf{Ba}_k, \\\\mathbf{Bg}_k &amp;\\\\in \\\\mathbb{R}^3 &amp;\\\\text{\u52a0\u901f\u5ea6\u8ba1\u548c\u9640\u87ba\u4eea\u7684 bias&#xff08;\u7531 ESKF \u4f30\u8ba1&#xff09;} \\\\\\\\ \\\\mathbf{a}_k^m, \\\\boldsymbol{\\\\omega}_k^m &amp;\\\\in \\\\mathbb{R}^3 &amp;\\\\text{\u4e0a\u4e00\u5e27\u7684 IMU \u539f\u59cb\u52a0\u901f\u5ea6\u548c\u89d2\u901f\u5ea6\u6d4b\u91cf\u503c} \\\\end{aligned} <\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 7.6555em;vertical-align: -3.5778em\"><\/span><span class=\"mord\"><span class=\"mtable\"><span class=\"col-align-r\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 4.0778em\"><span class=\"\" style=\"top: -6.2137em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\"><span class=\"mord mathbf\">P<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -4.6896em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\"><span class=\"mord mathbf\" style=\"margin-right: 0.016em\">V<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.016em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.1304em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\"><span class=\"mord mathbf\">R<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.8991em\"><span class=\"\" style=\"top: -2.453em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0269em\">w<\/span><span class=\"mord mathnormal mtight\">b<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.247em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -1.6063em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\"><span class=\"mord\"><span class=\"mord mathbf\">Ba<\/span><\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mpunct\">,<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord\"><span class=\"mord\"><span class=\"mord mathbf\" style=\"margin-right: 0.016em\">Bg<\/span><\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.242em\"><span class=\"\" style=\"top: -2.4559em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2441em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -0.0822em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\"><span class=\"mord mathbf\">a<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">m<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.247em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mpunct\">,<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord\"><span class=\"mord\"><span class=\"mord\"><span class=\"mord boldsymbol\" style=\"margin-right: 0.037em\">\u03c9<\/span><\/span><\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">m<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.247em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 3.5778em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"col-align-l\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 4.0778em\"><span class=\"\" style=\"top: -6.2137em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\"><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2208<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mord\"><span class=\"mord mathbb\">R<\/span><span class=\"msupsub\"><span class=\"vlist-t\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.8641em\"><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\">3<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -4.6896em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\"><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2208<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mord\"><span class=\"mord mathbb\">R<\/span><span class=\"msupsub\"><span class=\"vlist-t\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.8641em\"><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\">3<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.1304em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\"><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2208<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0576em\">S<\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0278em\">O<\/span><span class=\"mopen\">(<\/span><span class=\"mord\">3<\/span><span class=\"mclose\">)<\/span><\/span><\/span><span class=\"\" style=\"top: -1.6063em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\"><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2208<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mord\"><span class=\"mord mathbb\">R<\/span><span class=\"msupsub\"><span class=\"vlist-t\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.8641em\"><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\">3<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -0.0822em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\"><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2208<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mord\"><span class=\"mord mathbb\">R<\/span><span class=\"msupsub\"><span class=\"vlist-t\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.8641em\"><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\">3<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 3.5778em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"arraycolsep\" style=\"width: 1em\"><\/span><span class=\"col-align-r\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 4.0778em\"><span class=\"\" style=\"top: -6.2137em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord text\"><span class=\"mord cjk_fallback\">\u4e16\u754c\u7cfb\u4e0b\u7684\u4f4d\u7f6e<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -4.6896em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord text\"><span class=\"mord cjk_fallback\">\u4e16\u754c\u7cfb\u4e0b\u7684\u901f\u5ea6<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.1304em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord text\"><span class=\"mord\">IMU\u00a0body\u00a0<\/span><span class=\"mord cjk_fallback\">\u7cfb\u5230<\/span><span class=\"mord\">\u00a0world\u00a0<\/span><span class=\"mord cjk_fallback\">\u7cfb\u7684\u65cb\u8f6c\u77e9\u9635<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -1.6063em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord text\"><span class=\"mord cjk_fallback\">\u52a0\u901f\u5ea6\u8ba1\u548c\u9640\u87ba\u4eea\u7684<\/span><span class=\"mord\">\u00a0bias<\/span><span class=\"mord cjk_fallback\">&#xff08;\u7531<\/span><span class=\"mord\">\u00a0ESKF\u00a0<\/span><span class=\"mord cjk_fallback\">\u4f30\u8ba1&#xff09;<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -0.0822em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord text\"><span class=\"mord cjk_fallback\">\u4e0a\u4e00\u5e27\u7684<\/span><span class=\"mord\">\u00a0IMU\u00a0<\/span><span class=\"mord cjk_fallback\">\u539f\u59cb\u52a0\u901f\u5ea6\u548c\u89d2\u901f\u5ea6\u6d4b\u91cf\u503c<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 3.5778em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<ul>\n<li>\u4ee5\u9632\u4f60\u5fd8\u8bb0&#xff1a;\u4e2d\u503c\u79ef\u5206\u548c\u6b27\u62c9\u79ef\u5206\u7684\u533a\u522b\n<ul>\n<li>\u6b27\u62c9\u79ef\u5206&#xff08;\u524d\u5411\u6b27\u62c9&#xff09;&#xff1a;\u53ea\u7528\u533a\u95f4\u8d77\u70b9\u7684\u659c\u7387\u6765\u63a8\u7ec8\u70b9&#xff0c;\u4e00\u9636\u7cbe\u5ea6<\/li>\n<\/ul>\n<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>          y<\/p>\n<p>           k<\/p>\n<p>           &#043;<\/p>\n<p>           1<\/p>\n<p>         &#061;<\/p>\n<p>          y<\/p>\n<p>          k<\/p>\n<p>         &#043;<\/p>\n<p>         f<\/p>\n<p>         (<\/p>\n<p>          t<\/p>\n<p>          k<\/p>\n<p>         ,<br \/>\n        \u2009<\/p>\n<p>          y<\/p>\n<p>          k<\/p>\n<p>         )<\/p>\n<p>         \u22c5<\/p>\n<p>         \u0394<\/p>\n<p>         t<\/p>\n<p>         y_{k&#043;1} &#061; y_k &#043; f(t_k, \\\\, y_k) \\\\cdot \\\\Delta t <\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.6389em;vertical-align: -0.2083em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">y<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0359em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><span class=\"mbin mtight\">&#043;<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2083em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.7778em;vertical-align: -0.1944em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">y<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0359em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">&#043;<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1076em\">f<\/span><span class=\"mopen\">(<\/span><span class=\"mord\"><span class=\"mord mathnormal\">t<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mpunct\">,<\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">y<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0359em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u22c5<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.6833em\"><\/span><span class=\"mord\">\u0394<\/span><span class=\"mord mathnormal\">t<\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<p>* \u4e2d\u503c\u79ef\u5206&#xff1a;\u7528\u533a\u95f4\u8d77\u70b9\u548c\u7ec8\u70b9\u659c\u7387\u7684 __\u5e73\u5747\u503c__ \u6765\u63a8&#xff0c;\u4e8c\u9636\u7cbe\u5ea6<\/p>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>          y<\/p>\n<p>           k<\/p>\n<p>           &#043;<\/p>\n<p>           1<\/p>\n<p>         &#061;<\/p>\n<p>          y<\/p>\n<p>          k<\/p>\n<p>         &#043;<\/p>\n<p>         f<br \/>\n        \u2009\u2063<\/p>\n<p>          (<\/p>\n<p>             t<\/p>\n<p>             k<\/p>\n<p>            &#043;<\/p>\n<p>             t<\/p>\n<p>              k<\/p>\n<p>              &#043;<\/p>\n<p>              1<\/p>\n<p>           2<\/p>\n<p>          ,<br \/>\n         \u2005\u200a<\/p>\n<p>             y<\/p>\n<p>             k<\/p>\n<p>            &#043;<\/p>\n<p>             y<\/p>\n<p>              k<\/p>\n<p>              &#043;<\/p>\n<p>              1<\/p>\n<p>             pred<\/p>\n<p>           2<\/p>\n<p>          )<\/p>\n<p>         \u22c5<\/p>\n<p>         \u0394<\/p>\n<p>         t<\/p>\n<p>         y_{k&#043;1} &#061; y_k &#043; f\\\\!\\\\left(\\\\frac{t_k &#043; t_{k&#043;1}}{2}, \\\\; \\\\frac{y_k &#043; y_{k&#043;1}^{\\\\text{pred}}}{2}\\\\right) \\\\cdot \\\\Delta t <\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.6389em;vertical-align: -0.2083em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">y<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0359em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><span class=\"mbin mtight\">&#043;<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2083em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.7778em;vertical-align: -0.1944em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">y<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0359em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">&#043;<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 3em;vertical-align: -1.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.1076em\">f<\/span><span class=\"mspace\" style=\"margin-right: -0.1667em\"><\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"minner\"><span class=\"mopen delimcenter\" style=\"top: 0em\"><span class=\"delimsizing size4\">(<\/span><\/span><span class=\"mord\"><span class=\"mopen nulldelimiter\"><\/span><span class=\"mfrac\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 1.2921em\"><span class=\"\" style=\"top: -2.314em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\">2<\/span><\/span><\/span><span class=\"\" style=\"top: -3.23em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"frac-line\" style=\"border-bottom-width: 0.04em\"><\/span><\/span><span class=\"\" style=\"top: -3.677em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\"><span class=\"mord mathnormal\">t<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">&#043;<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\">t<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><span class=\"mbin mtight\">&#043;<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2083em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.686em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"mclose nulldelimiter\"><\/span><\/span><span class=\"mpunct\">,<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord\"><span class=\"mopen nulldelimiter\"><\/span><span class=\"mfrac\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 1.7167em\"><span class=\"\" style=\"top: -2.314em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\">2<\/span><\/span><\/span><span class=\"\" style=\"top: -3.23em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"frac-line\" style=\"border-bottom-width: 0.04em\"><\/span><\/span><span class=\"\" style=\"top: -3.7496em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">y<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0359em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">&#043;<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">y<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.967em\"><span class=\"\" style=\"top: -2.3987em;margin-left: -0.0359em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><span class=\"mbin mtight\">&#043;<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.1809em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord text mtight\"><span class=\"mord mtight\">pred<\/span><\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3596em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.686em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"mclose nulldelimiter\"><\/span><\/span><span class=\"mclose delimcenter\" style=\"top: 0em\"><span class=\"delimsizing size4\">)<\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u22c5<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.6833em\"><\/span><span class=\"mord\">\u0394<\/span><span class=\"mord mathnormal\">t<\/span><\/span><\/span><\/span><\/span><\/span> * \u8bf4\u4eba\u8bdd\u5c31\u662f&#xff1a;\u6b27\u62c9\u79ef\u5206\u662f\u4e00\u6761\u7b14\u76f4\u5c04\u7ebf&#xff08;\u659c\u7387\u9760\u731c&#xff09;&#xff0c;\u4e2d\u503c\u79ef\u5206\u662f\u4e00\u6761\u6298\u7ebf&#xff08;\u5148\u731c\u540e\u53d6\u5e73\u5747&#xff09;\u3002\u5bf9\u4e8e 200Hz \u7684 IMU&#xff0c;\u4e24\u8005\u5dee\u522b\u4e0d\u5927&#xff1b;\u4f46\u98de\u673a\u5feb\u901f\u504f\u822a\u65f6\u89d2\u901f\u5ea6\u53d8\u5316\u5267\u70c8&#xff0c;\u4e2d\u503c\u6cd5\u7684\u65cb\u8f6c\u79ef\u5206\u66f4\u7a33&#xff0c;\u4e0d\u4f1a\u51fa\u73b0\u6b27\u62c9\u6cd5\u7684&#034;\u65cb\u8f6c\u8fc7\u51b2&#034;<\/p>\n<ul>\n<li>\u6b65\u9aa4 1&#xff1a;\u628a\u4e0a\u4e00\u5e27\u7684\u52a0\u901f\u5ea6\u6d4b\u91cf\u503c\u8f6c\u6362\u5230\u4e16\u754c\u7cfb&#xff08;\u53bb\u6389 bias \u548c\u91cd\u529b&#xff09;&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>          a<\/p>\n<p>          k<\/p>\n<p>          w<\/p>\n<p>         &#061;<\/p>\n<p>          R<\/p>\n<p>          k<\/p>\n<p>         \u22c5<\/p>\n<p>         (<\/p>\n<p>          a<\/p>\n<p>          k<\/p>\n<p>          m<\/p>\n<p>         \u2212<\/p>\n<p>         B<\/p>\n<p>          a<\/p>\n<p>          k<\/p>\n<p>         )<\/p>\n<p>         &#043;<\/p>\n<p>         g<\/p>\n<p>         a_k^w &#061; R_k \\\\cdot (a_k^m &#8211; Ba_k) &#043; g <\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.9614em;vertical-align: -0.247em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\">a<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0269em\">w<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.247em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.8333em;vertical-align: -0.15em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0077em\">R<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0077em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u22c5<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mopen\">(<\/span><span class=\"mord\"><span class=\"mord mathnormal\">a<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">m<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.247em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u2212<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0502em\">B<\/span><span class=\"mord\"><span class=\"mord mathnormal\">a<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">&#043;<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.625em;vertical-align: -0.1944em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">g<\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<ul>\n<li>\n<p>\u5176\u4e2d g &#061; [g_x, g_y, g_z] \u662f\u4e16\u754c\u7cfb\u4e0b\u7684\u91cd\u529b\u5411\u91cf&#xff08;\u7531 ESKF \u5728\u7ebf\u4f30\u8ba1&#xff0c;\u521d\u59cb\u503c\u901a\u5e38\u4e3a [0, 0, -9.81]&#xff09;<\/p>\n<\/li>\n<li>\n<p>\u6b65\u9aa4 2&#xff1a;\u5bf9\u5f53\u524d\u5e27\u89d2\u901f\u5ea6\u505a\u4e2d\u503c&#xff0c;\u66f4\u65b0\u65cb\u8f6c&#xff1a;<\/p>\n<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>          \u03c9<\/p>\n<p>          mid<\/p>\n<p>         &#061;<\/p>\n<p>            \u03c9<\/p>\n<p>            k<\/p>\n<p>            m<\/p>\n<p>           &#043;<\/p>\n<p>            \u03c9<\/p>\n<p>             k<\/p>\n<p>             &#043;<\/p>\n<p>             1<\/p>\n<p>            m<\/p>\n<p>          2<\/p>\n<p>         \u2212<\/p>\n<p>         B<\/p>\n<p>          g<\/p>\n<p>          k<\/p>\n<p>         \\\\omega_{\\\\text{mid}} &#061; \\\\frac{\\\\omega_k^m &#043; \\\\omega_{k&#043;1}^m}{2} &#8211; Bg_k <\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.5806em;vertical-align: -0.15em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">\u03c9<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0359em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord text mtight\"><span class=\"mord mtight\">mid<\/span><\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 2.0818em;vertical-align: -0.686em\"><\/span><span class=\"mord\"><span class=\"mopen nulldelimiter\"><\/span><span class=\"mfrac\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 1.3958em\"><span class=\"\" style=\"top: -2.314em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\">2<\/span><\/span><\/span><span class=\"\" style=\"top: -3.23em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"frac-line\" style=\"border-bottom-width: 0.04em\"><\/span><\/span><span class=\"\" style=\"top: -3.7314em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">\u03c9<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.6644em\"><span class=\"\" style=\"top: -2.4169em;margin-left: -0.0359em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><span class=\"\" style=\"top: -3.063em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">m<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2831em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">&#043;<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">\u03c9<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.6644em\"><span class=\"\" style=\"top: -2.4169em;margin-left: -0.0359em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><span class=\"mbin mtight\">&#043;<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.063em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">m<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3414em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.686em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"mclose nulldelimiter\"><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u2212<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.8778em;vertical-align: -0.1944em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0502em\">B<\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">g<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0359em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>          R<\/p>\n<p>           k<\/p>\n<p>           &#043;<\/p>\n<p>           1<\/p>\n<p>         &#061;<\/p>\n<p>          R<\/p>\n<p>          k<\/p>\n<p>         \u22c5<\/p>\n<p>         exp<\/p>\n<p>         \u2061<\/p>\n<p>         (<\/p>\n<p>          \u03c9<\/p>\n<p>          mid<\/p>\n<p>         \u22c5<\/p>\n<p>         \u0394<\/p>\n<p>         t<\/p>\n<p>         )<\/p>\n<p>         R_{k&#043;1} &#061; R_k \\\\cdot \\\\exp(\\\\omega_{\\\\text{mid}} \\\\cdot \\\\Delta t) <\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.8917em;vertical-align: -0.2083em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0077em\">R<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0077em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><span class=\"mbin mtight\">&#043;<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2083em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.8333em;vertical-align: -0.15em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0077em\">R<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0077em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u22c5<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mop\">exp<\/span><span class=\"mopen\">(<\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">\u03c9<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0359em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord text mtight\"><span class=\"mord mtight\">mid<\/span><\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u22c5<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord\">\u0394<\/span><span class=\"mord mathnormal\">t<\/span><span class=\"mclose\">)<\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<ul>\n<li>\n<p>\u8fd9\u91cc\u7684 exp() \u662f SO(3) \u7684\u6307\u6570\u6620\u5c04&#xff08;\u5c06\u89d2\u901f\u5ea6\u5411\u91cf\u8f6c\u4e3a\u65cb\u8f6c\u77e9\u9635&#xff09;&#xff0c;\u7b49\u4ef7\u4e8e &lt;Exp&gt; \u51fd\u6570<\/p>\n<\/li>\n<li>\n<p>\u6b65\u9aa4 3&#xff1a;\u7528\u65b0\u65cb\u8f6c\u628a\u5f53\u524d\u5e27\u52a0\u901f\u5ea6\u4e5f\u8f6c\u5230\u4e16\u754c\u7cfb\u505a\u4e2d\u503c&#xff1a;<\/p>\n<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>          a<\/p>\n<p>           k<\/p>\n<p>           &#043;<\/p>\n<p>           1<\/p>\n<p>          w<\/p>\n<p>         &#061;<\/p>\n<p>          R<\/p>\n<p>           k<\/p>\n<p>           &#043;<\/p>\n<p>           1<\/p>\n<p>         \u22c5<\/p>\n<p>         (<\/p>\n<p>          a<\/p>\n<p>           k<\/p>\n<p>           &#043;<\/p>\n<p>           1<\/p>\n<p>          m<\/p>\n<p>         \u2212<\/p>\n<p>         B<\/p>\n<p>          a<\/p>\n<p>          k<\/p>\n<p>         )<\/p>\n<p>         &#043;<\/p>\n<p>         g<\/p>\n<p>         a_{k&#043;1}^w &#061; R_{k&#043;1} \\\\cdot (a_{k&#043;1}^m &#8211; Ba_k) &#043; g <\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 1.0197em;vertical-align: -0.3053em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\">a<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><span class=\"mbin mtight\">&#043;<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0269em\">w<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3053em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.8917em;vertical-align: -0.2083em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0077em\">R<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.0077em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><span class=\"mbin mtight\">&#043;<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2083em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u22c5<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1.0553em;vertical-align: -0.3053em\"><\/span><span class=\"mopen\">(<\/span><span class=\"mord\"><span class=\"mord mathnormal\">a<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><span class=\"mbin mtight\">&#043;<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">m<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3053em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u2212<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1em;vertical-align: -0.25em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0502em\">B<\/span><span class=\"mord\"><span class=\"mord mathnormal\">a<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mclose\">)<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">&#043;<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.625em;vertical-align: -0.1944em\"><\/span><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">g<\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>          a<\/p>\n<p>          mid<\/p>\n<p>          w<\/p>\n<p>         &#061;<\/p>\n<p>            a<\/p>\n<p>            k<\/p>\n<p>            w<\/p>\n<p>           &#043;<\/p>\n<p>            a<\/p>\n<p>             k<\/p>\n<p>             &#043;<\/p>\n<p>             1<\/p>\n<p>            w<\/p>\n<p>          2<\/p>\n<p>         a_{\\\\text{mid}}^w &#061; \\\\frac{a_k^w &#043; a_{k&#043;1}^w}{2} <\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.9614em;vertical-align: -0.247em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\">a<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord text mtight\"><span class=\"mord mtight\">mid<\/span><\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0269em\">w<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.247em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 2.0818em;vertical-align: -0.686em\"><\/span><span class=\"mord\"><span class=\"mopen nulldelimiter\"><\/span><span class=\"mfrac\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 1.3958em\"><span class=\"\" style=\"top: -2.314em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\">2<\/span><\/span><\/span><span class=\"\" style=\"top: -3.23em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"frac-line\" style=\"border-bottom-width: 0.04em\"><\/span><\/span><span class=\"\" style=\"top: -3.7314em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\"><span class=\"mord mathnormal\">a<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.6644em\"><span class=\"\" style=\"top: -2.4169em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><span class=\"\" style=\"top: -3.063em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0269em\">w<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2831em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">&#043;<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\">a<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.6644em\"><span class=\"\" style=\"top: -2.4169em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><span class=\"mbin mtight\">&#043;<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.063em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0269em\">w<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3414em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.686em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"mclose nulldelimiter\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<ul>\n<li>\u6b65\u9aa4 4&#xff1a;\u4e2d\u503c\u52a0\u901f\u5ea6\u79ef\u5206&#xff0c;\u66f4\u65b0\u4f4d\u7f6e\u548c\u901f\u5ea6&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>          V<\/p>\n<p>           k<\/p>\n<p>           &#043;<\/p>\n<p>           1<\/p>\n<p>         &#061;<\/p>\n<p>          V<\/p>\n<p>          k<\/p>\n<p>         &#043;<\/p>\n<p>          a<\/p>\n<p>          mid<\/p>\n<p>          w<\/p>\n<p>         \u22c5<\/p>\n<p>         \u0394<\/p>\n<p>         t<\/p>\n<p>         V_{k&#043;1} &#061; V_k &#043; a_{\\\\text{mid}}^w \\\\cdot \\\\Delta t <\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.8917em;vertical-align: -0.2083em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.2222em\">V<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.2222em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><span class=\"mbin mtight\">&#043;<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2083em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.8333em;vertical-align: -0.15em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.2222em\">V<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.2222em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">&#043;<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.9614em;vertical-align: -0.247em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\">a<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord text mtight\"><span class=\"mord mtight\">mid<\/span><\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0269em\">w<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.247em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u22c5<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.6833em\"><\/span><span class=\"mord\">\u0394<\/span><span class=\"mord mathnormal\">t<\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>          P<\/p>\n<p>           k<\/p>\n<p>           &#043;<\/p>\n<p>           1<\/p>\n<p>         &#061;<\/p>\n<p>          P<\/p>\n<p>          k<\/p>\n<p>         &#043;<\/p>\n<p>          V<\/p>\n<p>          k<\/p>\n<p>         \u22c5<\/p>\n<p>         \u0394<\/p>\n<p>         t<\/p>\n<p>         &#043;<\/p>\n<p>          1<\/p>\n<p>          2<\/p>\n<p>          a<\/p>\n<p>          mid<\/p>\n<p>          w<\/p>\n<p>         \u22c5<\/p>\n<p>         (<\/p>\n<p>         \u0394<\/p>\n<p>         t<\/p>\n<p>          )<\/p>\n<p>          2<\/p>\n<p>         P_{k&#043;1} &#061; P_k &#043; V_k \\\\cdot \\\\Delta t &#043; \\\\frac{1}{2} a_{\\\\text{mid}}^w \\\\cdot (\\\\Delta t)^2 <\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.8917em;vertical-align: -0.2083em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.1389em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><span class=\"mbin mtight\">&#043;<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.2083em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">&#061;<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.8333em;vertical-align: -0.15em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.1389em\">P<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.1389em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">&#043;<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.8333em;vertical-align: -0.15em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.2222em\">V<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3361em\"><span class=\"\" style=\"top: -2.55em;margin-left: -0.2222em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.15em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u22c5<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 0.7667em;vertical-align: -0.0833em\"><\/span><span class=\"mord\">\u0394<\/span><span class=\"mord mathnormal\">t<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">&#043;<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 2.0074em;vertical-align: -0.686em\"><\/span><span class=\"mord\"><span class=\"mopen nulldelimiter\"><\/span><span class=\"mfrac\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 1.3214em\"><span class=\"\" style=\"top: -2.314em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\">2<\/span><\/span><\/span><span class=\"\" style=\"top: -3.23em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"frac-line\" style=\"border-bottom-width: 0.04em\"><\/span><\/span><span class=\"\" style=\"top: -3.677em\"><span class=\"pstrut\" style=\"height: 3em\"><\/span><span class=\"mord\"><span class=\"mord\">1<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.686em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><span class=\"mclose nulldelimiter\"><\/span><\/span><span class=\"mord\"><span class=\"mord mathnormal\">a<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord text mtight\"><span class=\"mord mtight\">mid<\/span><\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0269em\">w<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.247em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><span class=\"mbin\">\u22c5<\/span><span class=\"mspace\" style=\"margin-right: 0.2222em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1.1141em;vertical-align: -0.25em\"><\/span><span class=\"mopen\">(<\/span><span class=\"mord\">\u0394<\/span><span class=\"mord mathnormal\">t<\/span><span class=\"mclose\"><span class=\"mclose\">)<\/span><span class=\"msupsub\"><span class=\"vlist-t\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.8641em\"><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\">2<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<ul>\n<li>\u6b65\u9aa4 5&#xff1a;\u7f13\u5b58\u5f53\u524d\u6d4b\u91cf\u503c\u4f9b\u4e0b\u4e00\u5e27\u4f7f\u7528&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"katex--display\"><span class=\"katex-display\"><span class=\"katex\"><span class=\"katex-mathml\"><\/p>\n<p>          a<\/p>\n<p>          k<\/p>\n<p>          m<\/p>\n<p>         \u2190<\/p>\n<p>          a<\/p>\n<p>           k<\/p>\n<p>           &#043;<\/p>\n<p>           1<\/p>\n<p>          m<\/p>\n<p>         ,<\/p>\n<p>          \u03c9<\/p>\n<p>          k<\/p>\n<p>          m<\/p>\n<p>         \u2190<\/p>\n<p>          \u03c9<\/p>\n<p>           k<\/p>\n<p>           &#043;<\/p>\n<p>           1<\/p>\n<p>          m<\/p>\n<p>         a_k^m \\\\leftarrow a_{k&#043;1}^m,\\\\quad \\\\omega_k^m \\\\leftarrow \\\\omega_{k&#043;1}^m <\/p>\n<p>     <\/span><span class=\"katex-html\"><span class=\"base\"><span class=\"strut\" style=\"height: 0.9614em;vertical-align: -0.247em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\">a<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">m<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.247em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2190<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1.0197em;vertical-align: -0.3053em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\">a<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-left: 0em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><span class=\"mbin mtight\">&#043;<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">m<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3053em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mpunct\">,<\/span><span class=\"mspace\" style=\"margin-right: 1em\"><\/span><span class=\"mspace\" style=\"margin-right: 0.1667em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">\u03c9<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-left: -0.0359em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">m<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.247em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><span class=\"mrel\">\u2190<\/span><span class=\"mspace\" style=\"margin-right: 0.2778em\"><\/span><\/span><span class=\"base\"><span class=\"strut\" style=\"height: 1.0197em;vertical-align: -0.3053em\"><\/span><span class=\"mord\"><span class=\"mord mathnormal\" style=\"margin-right: 0.0359em\">\u03c9<\/span><span class=\"msupsub\"><span class=\"vlist-t vlist-t2\"><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.7144em\"><span class=\"\" style=\"top: -2.453em;margin-left: -0.0359em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mtight\"><span class=\"mord mathnormal mtight\" style=\"margin-right: 0.0315em\">k<\/span><span class=\"mbin mtight\">&#043;<\/span><span class=\"mord mtight\">1<\/span><\/span><\/span><\/span><span class=\"\" style=\"top: -3.113em;margin-right: 0.05em\"><span class=\"pstrut\" style=\"height: 2.7em\"><\/span><span class=\"sizing reset-size6 size3 mtight\"><span class=\"mord mathnormal mtight\">m<\/span><\/span><\/span><\/span><span class=\"vlist-s\">\u200b<\/span><\/span><span class=\"vlist-r\"><span class=\"vlist\" style=\"height: 0.3053em\"><span class=\"\"><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\n<ul>\n<li>\u4ee5\u4e0a\u5c31\u662f\u4e00\u6b21\u5b8c\u6574\u7684\u4e2d\u503c\u79ef\u5206\u524d\u63a8\u3002\u5bf9\u6bd4\u6b27\u62c9\u79ef\u5206&#xff08;\u53ea\u7528\u5355\u8fb9\u503c&#xff09;&#xff0c;\u4e2d\u503c\u6cd5\u7528\u8d77\u70b9\u548c\u7ec8\u70b9\u52a0\u901f\u5ea6\/\u89d2\u901f\u5ea6\u7684\u5e73\u5747\u503c&#xff0c;\u4e8c\u9636\u7cbe\u5ea6&#xff0c;\u5bf9 200Hz \u91c7\u6837\u7387\u7684 IMU \u6765\u8bf4\u8bef\u5dee\u8fdc\u5c0f\u4e8e 0.1 \u79d2\u5185 LiDAR \u7684\u7d2f\u79ef\u6f02\u79fb<\/li>\n<\/ul>\n<p>\u4e2d\u503c\u79ef\u5206\u7684\u5173\u952e&#xff1a;\u89d2\u901f\u5ea6\u7528\u4e2d\u503c &#061; \u65cb\u8f6c\u66f4\u51c6&#xff0c;\u52a0\u901f\u5ea6\u7528\u4e2d\u503c &#061; \u4f4d\u7f6e\/\u901f\u5ea6\u66f4\u7a33\u3002200Hz \u4e0b\u6b27\u62c9\u548c\u4e2d\u503c\u5dee\u522b\u4e0d\u5927&#xff0c;\u4f46\u4e2d\u503c\u7684\u6570\u503c\u7a33\u5b9a\u6027\u66f4\u597d\u2014\u2014\u7279\u522b\u662f\u89d2\u901f\u5ea6\u5267\u70c8\u53d8\u5316\u65f6&#xff08;\u98de\u673a\u5feb\u901f\u504f\u822a&#xff09;&#xff0c;\u4e2d\u503c\u6cd5\u4e0d\u4f1a\u51fa\u73b0\u6b27\u62c9\u6cd5\u7684&#034;\u65cb\u8f6c\u8fc7\u51b2&#034;<\/p>\n<h5>4-4 \u4ee3\u7801\u5b9e\u73b0<\/h5>\n<ul>\n<li>\n<p>\u4ee3\u7801\u4fee\u6539\u5168\u90e8\u96c6\u4e2d\u5728 src\/laserMapping.cpp&#xff0c;\u5206\u4e09\u5757&#xff1a;\u72b6\u6001\u7f13\u5b58 &#043; \u524d\u63a8\u51fd\u6570 &#043; \u8c03\u7528\u70b9<\/p>\n<\/li>\n<li>\n<p>\u72b6\u6001\u7f13\u5b58&#xff08;\u5168\u5c40\u53d8\u91cf&#xff0c;\u52a0\u5728 p_pre \u524d\u9762&#xff09;&#xff1a;<\/p>\n<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/*** 200Hz IMU forward propagation: cache latest ESKF state ***\/<\/span><br \/>\n<span class=\"token keyword\">double<\/span> latest_time<span class=\"token punctuation\">;<\/span><br \/>\nV3D <span class=\"token function\">latest_P<\/span><span class=\"token punctuation\">(<\/span>Zero3d<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token function\">latest_V<\/span><span class=\"token punctuation\">(<\/span>Zero3d<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token function\">latest_Ba<\/span><span class=\"token punctuation\">(<\/span>Zero3d<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token function\">latest_Bg<\/span><span class=\"token punctuation\">(<\/span>Zero3d<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\nV3D <span class=\"token function\">latest_acc_0<\/span><span class=\"token punctuation\">(<\/span>Zero3d<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token function\">latest_gyr_0<\/span><span class=\"token punctuation\">(<\/span>Zero3d<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span>  <span class=\"token comment\">\/\/ \u4e0a\u4e00\u5e27 IMU \u6d4b\u91cf\u503c<\/span><br \/>\nM3D <span class=\"token function\">latest_Q<\/span><span class=\"token punctuation\">(<\/span>Eye3d<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span>                               <span class=\"token comment\">\/\/ \u5f53\u524d\u65cb\u8f6c\u77e9\u9635<\/span><br \/>\n<span class=\"token keyword\">bool<\/span> init <span class=\"token operator\">&#061;<\/span> <span class=\"token boolean\">false<\/span><span class=\"token punctuation\">;<\/span>                                 <span class=\"token comment\">\/\/ ESKF \u662f\u5426\u5df2\u521d\u59cb\u5316&#xff08;\u6709\u4e86\u7b2c\u4e00\u7ec4 bias&#xff09;<\/span><br \/>\nrclcpp<span class=\"token double-colon punctuation\">::<\/span>Publisher<span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Odometry<span class=\"token operator\">&gt;<\/span><span class=\"token double-colon punctuation\">::<\/span>SharedPtr pubOdomHighFreq_<span class=\"token punctuation\">;<\/span><\/p>\n<ul>\n<li>updateLatestStates() \u2014 ESKF \u66f4\u65b0\u540e\u7f13\u5b58&#xff08;\u52a0\u5728 SigHandle \u51fd\u6570\u540e\u9762&#xff09;&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/\/ \u6bcf\u6b21 ESKF \u66f4\u65b0\u540e&#xff0c;\u7f13\u5b58\u6700\u65b0\u72b6\u6001<\/span><br \/>\n<span class=\"token keyword\">void<\/span> <span class=\"token function\">updateLatestStates<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n    latest_time <span class=\"token operator\">&#061;<\/span> lidar_end_time<span class=\"token punctuation\">;<\/span><br \/>\n    latest_P <span class=\"token operator\">&#061;<\/span> state_point<span class=\"token punctuation\">.<\/span>pos<span class=\"token punctuation\">;<\/span><br \/>\n    latest_Q <span class=\"token operator\">&#061;<\/span> state_point<span class=\"token punctuation\">.<\/span>rot<span class=\"token punctuation\">;<\/span><br \/>\n    latest_V <span class=\"token operator\">&#061;<\/span> state_point<span class=\"token punctuation\">.<\/span>vel<span class=\"token punctuation\">;<\/span><br \/>\n    latest_Ba <span class=\"token operator\">&#061;<\/span> state_point<span class=\"token punctuation\">.<\/span>ba<span class=\"token punctuation\">;<\/span><br \/>\n    latest_Bg <span class=\"token operator\">&#061;<\/span> state_point<span class=\"token punctuation\">.<\/span>bg<span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<ul>\n<li>\n<p>\u5bf9\u5e94 4-3 \u8282\u516c\u5f0f&#xff0c;\u8fd9\u91cc\u7f13\u5b58\u4e86\u4e2d\u503c\u79ef\u5206\u9700\u8981\u7684\u5168\u90e8\u8d77\u70b9\u72b6\u6001&#xff1a;P_k\u3001V_k\u3001R_k\u3001Ba_k\u3001Bg_k<\/p>\n<\/li>\n<li>\n<p>fastPredictIMU() \u2014 \u6838\u5fc3\u524d\u63a8\u51fd\u6570&#xff1a;<\/p>\n<\/li>\n<\/ul>\n<p><span class=\"token keyword\">void<\/span> <span class=\"token function\">fastPredictIMU<\/span><span class=\"token punctuation\">(<\/span><span class=\"token keyword\">double<\/span> t<span class=\"token punctuation\">,<\/span> V3D acc<span class=\"token punctuation\">,<\/span> V3D gyr<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token keyword\">double<\/span> dt <span class=\"token operator\">&#061;<\/span> t <span class=\"token operator\">&#8211;<\/span> latest_time<span class=\"token punctuation\">;<\/span><br \/>\n    latest_time <span class=\"token operator\">&#061;<\/span> t<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token comment\">\/\/ \u5bf9\u5e94\u516c\u5f0f (1): a_k^w &#061; R_k * (a_k^m &#8211; Ba_k) &#043; g<\/span><br \/>\n    V3D un_acc_0 <span class=\"token operator\">&#061;<\/span> latest_Q <span class=\"token operator\">*<\/span> <span class=\"token punctuation\">(<\/span>latest_acc_0 <span class=\"token operator\">&#8211;<\/span> latest_Ba<span class=\"token punctuation\">)<\/span><br \/>\n                 <span class=\"token operator\">&#043;<\/span> <span class=\"token function\">V3D<\/span><span class=\"token punctuation\">(<\/span>state_point<span class=\"token punctuation\">.<\/span>grav<span class=\"token punctuation\">[<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span> state_point<span class=\"token punctuation\">.<\/span>grav<span class=\"token punctuation\">[<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span> state_point<span class=\"token punctuation\">.<\/span>grav<span class=\"token punctuation\">[<\/span><span class=\"token number\">2<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token comment\">\/\/ \u5bf9\u5e94\u516c\u5f0f (2): \u03c9_mid &#061; (\u03c9_k^m &#043; \u03c9_{k&#043;1}^m)\/2 &#8211; Bg_k<\/span><br \/>\n    V3D un_gyr <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.5<\/span> <span class=\"token operator\">*<\/span> <span class=\"token punctuation\">(<\/span>latest_gyr_0 <span class=\"token operator\">&#043;<\/span> gyr<span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#8211;<\/span> latest_Bg<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token comment\">\/\/ \u5bf9\u5e94\u516c\u5f0f (2): R_{k&#043;1} &#061; R_k * exp(\u03c9_mid * \u0394t)<\/span><br \/>\n    latest_Q <span class=\"token operator\">&#061;<\/span> latest_Q <span class=\"token operator\">*<\/span> <span class=\"token function\">Exp<\/span><span class=\"token punctuation\">(<\/span>un_gyr<span class=\"token punctuation\">,<\/span> dt<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token comment\">\/\/ \u5bf9\u5e94\u516c\u5f0f (3): a_{k&#043;1}^w &#061; R_{k&#043;1} * (a_{k&#043;1}^m &#8211; Ba_k) &#043; g<\/span><br \/>\n    V3D un_acc_1 <span class=\"token operator\">&#061;<\/span> latest_Q <span class=\"token operator\">*<\/span> <span class=\"token punctuation\">(<\/span>acc <span class=\"token operator\">&#8211;<\/span> latest_Ba<span class=\"token punctuation\">)<\/span><br \/>\n                 <span class=\"token operator\">&#043;<\/span> <span class=\"token function\">V3D<\/span><span class=\"token punctuation\">(<\/span>state_point<span class=\"token punctuation\">.<\/span>grav<span class=\"token punctuation\">[<\/span><span class=\"token number\">0<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span> state_point<span class=\"token punctuation\">.<\/span>grav<span class=\"token punctuation\">[<\/span><span class=\"token number\">1<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">,<\/span> state_point<span class=\"token punctuation\">.<\/span>grav<span class=\"token punctuation\">[<\/span><span class=\"token number\">2<\/span><span class=\"token punctuation\">]<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token comment\">\/\/ \u5bf9\u5e94\u516c\u5f0f (3): a_mid^w &#061; (a_k^w &#043; a_{k&#043;1}^w) \/ 2<\/span><br \/>\n    V3D un_acc <span class=\"token operator\">&#061;<\/span> <span class=\"token number\">0.5<\/span> <span class=\"token operator\">*<\/span> <span class=\"token punctuation\">(<\/span>un_acc_0 <span class=\"token operator\">&#043;<\/span> un_acc_1<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token comment\">\/\/ \u5bf9\u5e94\u516c\u5f0f (4): V_{k&#043;1} &#061; V_k &#043; a_mid * \u0394t<\/span><br \/>\n    <span class=\"token comment\">\/\/              P_{k&#043;1} &#061; P_k &#043; V_k * \u0394t &#043; 0.5 * a_mid * \u0394t^2<\/span><br \/>\n    latest_P <span class=\"token operator\">&#061;<\/span> latest_P <span class=\"token operator\">&#043;<\/span> dt <span class=\"token operator\">*<\/span> latest_V <span class=\"token operator\">&#043;<\/span> <span class=\"token number\">0.5<\/span> <span class=\"token operator\">*<\/span> dt <span class=\"token operator\">*<\/span> dt <span class=\"token operator\">*<\/span> un_acc<span class=\"token punctuation\">;<\/span><br \/>\n    latest_V <span class=\"token operator\">&#061;<\/span> latest_V <span class=\"token operator\">&#043;<\/span> dt <span class=\"token operator\">*<\/span> un_acc<span class=\"token punctuation\">;<\/span><br \/>\n    <span class=\"token comment\">\/\/ \u5bf9\u5e94\u516c\u5f0f (5): \u7f13\u5b58\u5f53\u524d\u6d4b\u91cf\u503c<\/span><br \/>\n    latest_acc_0 <span class=\"token operator\">&#061;<\/span> acc<span class=\"token punctuation\">;<\/span><br \/>\n    latest_gyr_0 <span class=\"token operator\">&#061;<\/span> gyr<span class=\"token punctuation\">;<\/span><\/p>\n<p>    <span class=\"token comment\">\/\/ \u53d1\u5e03 200Hz \u91cc\u7a0b\u8ba1<\/span><br \/>\n    nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Odometry odomHigh<span class=\"token punctuation\">;<\/span><br \/>\n    Eigen<span class=\"token double-colon punctuation\">::<\/span>Quaterniond quadrotor_Q <span class=\"token operator\">&#061;<\/span> <span class=\"token class-name\">Eigen<\/span><span class=\"token double-colon punctuation\">::<\/span><span class=\"token function\">Quaterniond<\/span><span class=\"token punctuation\">(<\/span>latest_Q<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>header<span class=\"token punctuation\">.<\/span>stamp <span class=\"token operator\">&#061;<\/span> <span class=\"token function\">get_ros_time<\/span><span class=\"token punctuation\">(<\/span>t<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>header<span class=\"token punctuation\">.<\/span>frame_id <span class=\"token operator\">&#061;<\/span> <span class=\"token string\">&#034;camera_init&#034;<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>child_frame_id <span class=\"token operator\">&#061;<\/span> <span class=\"token string\">&#034;body&#034;<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>pose<span class=\"token punctuation\">.<\/span>pose<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>x <span class=\"token operator\">&#061;<\/span> latest_P<span class=\"token punctuation\">.<\/span><span class=\"token function\">x<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>pose<span class=\"token punctuation\">.<\/span>pose<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>y <span class=\"token operator\">&#061;<\/span> latest_P<span class=\"token punctuation\">.<\/span><span class=\"token function\">y<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>pose<span class=\"token punctuation\">.<\/span>pose<span class=\"token punctuation\">.<\/span>position<span class=\"token punctuation\">.<\/span>z <span class=\"token operator\">&#061;<\/span> latest_P<span class=\"token punctuation\">.<\/span><span class=\"token function\">z<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>pose<span class=\"token punctuation\">.<\/span>pose<span class=\"token punctuation\">.<\/span>orientation<span class=\"token punctuation\">.<\/span>x <span class=\"token operator\">&#061;<\/span> quadrotor_Q<span class=\"token punctuation\">.<\/span><span class=\"token function\">x<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>pose<span class=\"token punctuation\">.<\/span>pose<span class=\"token punctuation\">.<\/span>orientation<span class=\"token punctuation\">.<\/span>y <span class=\"token operator\">&#061;<\/span> quadrotor_Q<span class=\"token punctuation\">.<\/span><span class=\"token function\">y<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>pose<span class=\"token punctuation\">.<\/span>pose<span class=\"token punctuation\">.<\/span>orientation<span class=\"token punctuation\">.<\/span>z <span class=\"token operator\">&#061;<\/span> quadrotor_Q<span class=\"token punctuation\">.<\/span><span class=\"token function\">z<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>pose<span class=\"token punctuation\">.<\/span>pose<span class=\"token punctuation\">.<\/span>orientation<span class=\"token punctuation\">.<\/span>w <span class=\"token operator\">&#061;<\/span> quadrotor_Q<span class=\"token punctuation\">.<\/span><span class=\"token function\">w<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>twist<span class=\"token punctuation\">.<\/span>twist<span class=\"token punctuation\">.<\/span>linear<span class=\"token punctuation\">.<\/span>x <span class=\"token operator\">&#061;<\/span> latest_V<span class=\"token punctuation\">.<\/span><span class=\"token function\">x<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>twist<span class=\"token punctuation\">.<\/span>twist<span class=\"token punctuation\">.<\/span>linear<span class=\"token punctuation\">.<\/span>y <span class=\"token operator\">&#061;<\/span> latest_V<span class=\"token punctuation\">.<\/span><span class=\"token function\">y<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>twist<span class=\"token punctuation\">.<\/span>twist<span class=\"token punctuation\">.<\/span>linear<span class=\"token punctuation\">.<\/span>z <span class=\"token operator\">&#061;<\/span> latest_V<span class=\"token punctuation\">.<\/span><span class=\"token function\">z<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>twist<span class=\"token punctuation\">.<\/span>twist<span class=\"token punctuation\">.<\/span>angular<span class=\"token punctuation\">.<\/span>x <span class=\"token operator\">&#061;<\/span> gyr<span class=\"token punctuation\">.<\/span><span class=\"token function\">x<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#8211;<\/span> state_point<span class=\"token punctuation\">.<\/span>bg<span class=\"token punctuation\">.<\/span><span class=\"token function\">x<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>twist<span class=\"token punctuation\">.<\/span>twist<span class=\"token punctuation\">.<\/span>angular<span class=\"token punctuation\">.<\/span>y <span class=\"token operator\">&#061;<\/span> gyr<span class=\"token punctuation\">.<\/span><span class=\"token function\">y<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#8211;<\/span> state_point<span class=\"token punctuation\">.<\/span>bg<span class=\"token punctuation\">.<\/span><span class=\"token function\">y<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n    odomHigh<span class=\"token punctuation\">.<\/span>twist<span class=\"token punctuation\">.<\/span>twist<span class=\"token punctuation\">.<\/span>angular<span class=\"token punctuation\">.<\/span>z <span class=\"token operator\">&#061;<\/span> gyr<span class=\"token punctuation\">.<\/span><span class=\"token function\">z<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span> <span class=\"token operator\">&#8211;<\/span> state_point<span class=\"token punctuation\">.<\/span>bg<span class=\"token punctuation\">.<\/span><span class=\"token function\">z<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p>    pubOdomHighFreq_<span class=\"token operator\">-&gt;<\/span><span class=\"token function\">publish<\/span><span class=\"token punctuation\">(<\/span>odomHigh<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<ul>\n<li>\n<p>\u6ce8\u610f&#xff1a;state_point.grav \u662f ESKF \u5728\u7ebf\u4f30\u8ba1\u7684\u91cd\u529b\u5411\u91cf&#xff08;\u4e16\u754c\u7cfb&#xff09;\u3002\u521d\u59cb\u5316\u4e3a [0, 0, -9.81]&#xff08;NED \u4e16\u754c\u7cfb&#xff0c;\u5982\u679c\u5b9e\u9645\u7528\u7684\u662f ENU \u7cfb\u5219\u662f [0, 0, 9.81]&#xff09;&#xff0c;\u98de\u884c\u8fc7\u7a0b\u4e2d ESKF \u4f1a\u52a8\u6001\u4fee\u6b63<\/p>\n<\/li>\n<li>\n<p>imu_cbk \u4e2d\u52a0\u8c03\u7528&#xff08;imu_cbk \u51fd\u6570\u5f00\u5934&#xff0c;\u5728\u65f6\u95f4\u540c\u6b65\u903b\u8f91\u4e4b\u524d&#xff09;&#xff1a;<\/p>\n<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/\/ 200Hz forward propagation: ESKF \u521d\u59cb\u5316\u540e, \u6bcf\u6b21 IMU \u6d88\u606f\u9884\u6d4b\u4e00\u6b21\u9ad8\u9891\u91cc\u7a0b\u8ba1<\/span><br \/>\n<span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span>init<span class=\"token punctuation\">)<\/span><br \/>\n<span class=\"token punctuation\">{<\/span><br \/>\n    <span class=\"token function\">fastPredictIMU<\/span><span class=\"token punctuation\">(<\/span><span class=\"token function\">get_time_sec<\/span><span class=\"token punctuation\">(<\/span>msg_in<span class=\"token operator\">-&gt;<\/span>header<span class=\"token punctuation\">.<\/span>stamp<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        <span class=\"token function\">V3D<\/span><span class=\"token punctuation\">(<\/span>msg_in<span class=\"token operator\">-&gt;<\/span>linear_acceleration<span class=\"token punctuation\">.<\/span>x<span class=\"token punctuation\">,<\/span> msg_in<span class=\"token operator\">-&gt;<\/span>linear_acceleration<span class=\"token punctuation\">.<\/span>y<span class=\"token punctuation\">,<\/span> msg_in<span class=\"token operator\">-&gt;<\/span>linear_acceleration<span class=\"token punctuation\">.<\/span>z<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">,<\/span><br \/>\n        <span class=\"token function\">V3D<\/span><span class=\"token punctuation\">(<\/span>msg_in<span class=\"token operator\">-&gt;<\/span>angular_velocity<span class=\"token punctuation\">.<\/span>x<span class=\"token punctuation\">,<\/span> msg_in<span class=\"token operator\">-&gt;<\/span>angular_velocity<span class=\"token punctuation\">.<\/span>y<span class=\"token punctuation\">,<\/span> msg_in<span class=\"token operator\">-&gt;<\/span>angular_velocity<span class=\"token punctuation\">.<\/span>z<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token punctuation\">}<\/span><\/p>\n<ul>\n<li>init \u6807\u8bb0\u7684\u8bbe\u7f6e&#xff08;\u5728 timer_callback \u4e2d&#xff0c;ESKF \u66f4\u65b0\u5b8c\u6bd5\u540e&#xff09;&#xff1a;<\/li>\n<\/ul>\n<p><span class=\"token comment\">\/******* Publish odometry *******\/<\/span><br \/>\n<span class=\"token function\">publish_odometry<\/span><span class=\"token punctuation\">(<\/span>pubOdomAftMapped_<span class=\"token punctuation\">,<\/span> tf_broadcaster_<span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<p><span class=\"token comment\">\/*** Cache latest ESKF state for 200Hz IMU forward propagation ***\/<\/span><br \/>\n<span class=\"token function\">updateLatestStates<\/span><span class=\"token punctuation\">(<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><br \/>\n<span class=\"token keyword\">if<\/span> <span class=\"token punctuation\">(<\/span><span class=\"token operator\">!<\/span>init<span class=\"token punctuation\">)<\/span> init <span class=\"token operator\">&#061;<\/span> <span class=\"token boolean\">true<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<ul>\n<li>\n<p>init \u5728\u7b2c\u4e00\u6761 LiDAR scan \u7684 ESKF \u66f4\u65b0\u5b8c\u6210\u540e\u624d\u8bbe\u4e3a true\u2014\u2014\u786e\u4fdd fastPredictIMU \u4f7f\u7528\u7684 latest_P\/V\/Q\/Ba\/Bg \u5df2\u7ecf\u5305\u542b\u4e86\u7b2c\u4e00\u7ec4 ESKF bias \u4f30\u8ba1&#xff0c;\u800c\u4e0d\u662f\u5168\u96f6<\/p>\n<\/li>\n<li>\n<p>\u8bf4\u4eba\u8bdd\u5c31\u662f&#xff1a;bias \u6ca1\u4f30\u8ba1\u597d\u4e4b\u524d\u4e0d\u8981\u778e\u524d\u63a8&#xff0c;\u7b49 ESKF \u8bf4&#034;\u6211\u51c6\u5907\u597d\u4e86&#034;\u518d\u5f00 IMU \u9ad8\u9891\u53d1\u8868<\/p>\n<\/li>\n<li>\n<p>publisher \u6ce8\u518c&#xff08;\u6784\u9020\u51fd\u6570\u4e2d&#xff09;&#xff1a;<\/p>\n<\/li>\n<\/ul>\n<p>pubOdomHighFreq_ <span class=\"token operator\">&#061;<\/span> <span class=\"token keyword\">this<\/span><span class=\"token operator\">-&gt;<\/span><span class=\"token generic-function\"><span class=\"token function\">create_publisher<\/span><span class=\"token generic class-name\"><span class=\"token operator\">&lt;<\/span>nav_msgs<span class=\"token double-colon punctuation\">::<\/span>msg<span class=\"token double-colon punctuation\">::<\/span>Odometry<span class=\"token operator\">&gt;<\/span><\/span><\/span><span class=\"token punctuation\">(<\/span><span class=\"token string\">&#034;\/Odom_high_freq&#034;<\/span><span class=\"token punctuation\">,<\/span> <span class=\"token number\">20<\/span><span class=\"token punctuation\">)<\/span><span class=\"token punctuation\">;<\/span><\/p>\n<ul>\n<li>\u9a8c\u8bc1&#xff1a;\u542f\u52a8\u4eff\u771f\u540e\u8fd0\u884c&#xff1a;<\/li>\n<\/ul>\n<p>ros2 topic hz \/Odom_high_freq   <span class=\"token comment\"># \u9884\u671f ~200Hz<\/span><br \/>\nros2 topic hz \/Odometry         <span class=\"token comment\"># \u539f\u6765\u7684 10Hz \u4e0d\u53d7\u5f71\u54cd<\/span><\/p>\n<ul>\n<li>\u4e24\u8005\u7684\u4f4d\u59ff\u5728 LiDAR \u5e27\u65f6\u523b&#xff08;init \u524d\u7684\u7d2f\u79ef&#xff09;\u5e94\u8be5\u662f\u5b8c\u5168\u4e00\u81f4\u7684&#xff0c;\u56e0\u4e3a fastPredictIMU \u7684\u79ef\u5206\u8d77\u70b9 latest_* \u5c31\u662f ESKF \u7684\u6700\u65b0\u4f30\u8ba1\u3002\u5dee\u522b\u4ec5\u5728 LiDAR \u5e27\u4e4b\u95f4\u7684&#034;\u88f8\u63a8&#034;\u90e8\u5206\u2014\u2014\u9ad8\u9891\u662f\u7eaf IMU \u79ef\u5206&#xff0c;\u4f4e\u9891\u7b49\u4e0b\u4e00\u5e27 LiDAR \u6765\u77eb\u6b63<\/li>\n<\/ul>\n<hr \/>\n<h3>5 \u540e\u7eed\u6269\u5c55\u65b9\u5411<\/h3>\n<ul>\n<li>FAST-LIO2 \u53ea\u662f SLAM \u7cfb\u7edf\u7684\u91cc\u7a0b\u8ba1\u524d\u7aef&#xff0c;\u4ece\u8fd9\u91cc\u53ef\u4ee5\u5ef6\u4f38\u51fa\u5f88\u591a\u65b9\u5411&#xff1a;\n<ul>\n<li>\u56de\u73af\u68c0\u6d4b &#043; \u5168\u5c40\u4f18\u5316&#xff1a;FAST-LIO2 \u6ca1\u6709\u56de\u73af&#xff0c;\u957f\u65f6\u95f4\u98de\u884c\u4f1a\u6709\u7d2f\u79ef\u6f02\u79fb\u3002\u53ef\u4ee5\u63a5 ScanContext&#xff08;\u57fa\u4e8e\u70b9\u4e91\u7684\u573a\u666f\u8bc6\u522b&#xff09;&#043; GTSAM&#xff08;\u56fe\u4f18\u5316&#xff09;\u505a\u540e\u7aef<\/li>\n<li>3D \u5360\u636e\u5730\u56fe&#xff1a;\/cloud_registered \u662f\u7a20\u5bc6\u70b9\u4e91\u5730\u56fe&#xff0c;\u53ef\u4ee5\u76f4\u63a5\u5582\u7ed9 OctoMap \u6216 Voxblox \u751f\u6210 ESDF&#xff08;\u6b27\u51e0\u91cc\u5f97\u7b26\u53f7\u8ddd\u79bb\u573a&#xff09;&#xff0c;\u7ed9\u8f68\u8ff9\u89c4\u5212\u7528<\/li>\n<li>EGO-Planner \u8f68\u8ff9\u89c4\u5212&#xff1a;\u62ff\u5230 ESDF \u540e\u5c31\u53ef\u4ee5\u5728\u672a\u77e5\u73af\u5883\u4e2d\u5b9e\u65f6\u89c4\u5212\u907f\u969c\u8f68\u8ff9&#xff0c;\u5f62\u6210 \u201cFAST-LIO2 \u2192 ESDF \u2192 EGO-Planner \u2192 PX4 Offboard\u201d \u7684\u5b8c\u6574\u81ea\u4e3b\u98de\u884c\u95ed\u73af<\/li>\n<li>\u5b9e\u7269\u90e8\u7f72&#xff1a;\u6362\u7528 Livox Mid-360&#xff08;\u56fa\u6001\u6fc0\u5149\u96f7\u8fbe&#xff0c;\u4f53\u79ef\u5c0f\u3001\u91cd\u91cf\u8f7b\u3001FoV \u5927&#xff09;&#xff0c;\u5b89\u88c5 livox_ros_driver2 \u540e\u91cd\u65b0\u7f16\u8bd1\u5373\u53ef\u3002px4_vlp16.yaml \u4e2d\u7684\u53c2\u6570\u9700\u8981\u6839\u636e Mid-360 \u7684\u7279\u6027\u91cd\u65b0\u8c03&#xff1a;lidar_type: 1&#xff08;AVIA \u7c7b&#xff09;\u3001scan_line: 4\u3001fov_degree: 360\u3001blind: 0.1<\/li>\n<li>\u591a\u673a\u534f\u540c&#xff1a;\u591a\u67b6\u98de\u673a\u5404\u81ea\u8dd1 FAST-LIO2&#xff0c;\u901a\u8fc7 vehicle_visual_odometry \u628a\u91cc\u7a0b\u8ba1\u53d1\u7ed9 PX4 \u66ff\u4ee3 GPS&#xff0c;\u5b9e\u73b0\u5ba4\u5185\u7f16\u961f<\/li>\n<\/ul>\n<\/li>\n<li>\u8003\u8651\u540e\u9762\u6709\u4e00\u671f\u6211\u4eec\u6765\u8c08\u8c08 OctoMap &#043; EGO-Planner \u7684\u96c6\u6210<\/li>\n<\/ul>\n<hr \/>\n<h3>\u603b\u7ed3<\/h3>\n<ul>\n<li>\u672c\u6587\u5728\u4e0a\u4e00\u671f ROS2-PX4 \u4eff\u771f\u73af\u5883\u7684\u57fa\u7840\u4e0a&#xff0c;\u5b8c\u6574\u90e8\u7f72\u4e86 FAST-LIO2 LiDAR-\u60ef\u6027\u91cc\u7a0b\u8ba1<\/li>\n<li>\u6838\u5fc3\u5de5\u4f5c\u56de\u987e&#xff1a;\n<ul>\n<li>\u62c9\u53d6\u5b98\u65b9 hku-mars\/FAST_LIO \u7684 ROS2 \u5206\u652f<\/li>\n<li>\u901a\u8fc7 #ifdef USE_LIVOX \u6761\u4ef6\u7f16\u8bd1\u53bb\u6389 livox_ros_driver2 \u786c\u4f9d\u8d56&#xff0c;\u4eff\u771f\u7528 VLP-16 \u76f4\u63a5\u8d70\u6807\u51c6 PointCloud2 \u8def\u5f84<\/li>\n<li>\u8bbe\u8ba1 camera_init \u2192 body \u2192 base_link \u2192 sensors \u7684 TF \u6811&#xff0c;\u6253\u901a FAST-LIO2 \u548c PX4 \u4e24\u5957\u5750\u6807\u7cfb<\/li>\n<li>\u5b9a\u5236 px4_vlp16.yaml \u53c2\u6570\u914d\u7f6e&#xff08;VLP-16\u300116 \u7ebf\u300110Hz\u3001\u5916\u53c2 [0,0,0.12]&#xff09;<\/li>\n<li>\u4e00\u952e\u542f\u52a8\u811a\u672c 3_fastlio2.sh&#xff1a;\u9759\u6001 TF &#043; FAST-LIO2 &#043; RViz2 \u53ef\u89c6\u5316<\/li>\n<\/ul>\n<\/li>\n<li>\u6838\u5fc3\u8e29\u5751\u56de\u987e&#xff1a;\n<ul>\n<li>FAST-LIO2 \u5fc5\u987b clone ROS2 \u5206\u652f&#xff0c;main \u5206\u652f\u53ea\u652f\u6301 ROS1<\/li>\n<li>livox_ros_driver2 \u4e0d\u5728 apt \u6e90\u91cc\u2014\u2014\u505a\u4eff\u771f\u4e0d\u8981\u5f3a\u884c\u88c5\u4e00\u6574\u5957 Livox SDK&#xff0c;\u6761\u4ef6\u7f16\u8bd1\u662f\u6700\u4f18\u96c5\u7684\u89e3\u6cd5<\/li>\n<li>camera_init \u548c base_link \u4e4b\u95f4\u6ca1\u6709 TF \u2192 RViz2 \u5168\u90e8\u62a5 \u201cNo transform\u201d \u2192 \u52a0 body \u2192 base_link \u9759\u6001 identity \u4e00\u6b65\u641e\u5b9a<\/li>\n<li>Fixed Frame \u8981\u6539\u6210 camera_init&#xff0c;\u5426\u5219 FAST-LIO2 \u7684\u70b9\u4e91\/\u91cc\u7a0b\u8ba1\/\u8def\u5f84\u5168\u90e8\u4e0d\u663e\u793a<\/li>\n<\/ul>\n<\/li>\n<li>\u5982\u6709\u9519\u8bef&#xff0c;\u6b22\u8fce\u6307\u51fa&#xff01;<\/li>\n<li>\u611f\u8c22\u89c2\u770b&#xff01;<img decoding=\"async\" src=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260725171723-6a64efa39bf62.png\" alt=\"\u5728\u8fd9\u91cc\u63d2\u5165\u56fe\u7247\u63cf\u8ff0\" \/><\/li>\n<\/ul>\n","protected":false},"excerpt":{"rendered":"<p>\u524d\u8a00<br \/>\n\u4e0a\u4e00\u671f\u6211\u4eec\u82b1\u4e86\u5f88\u5927\u7bc7\u5e45&#xff0c;\u4ece\u96f6\u642d\u5efa\u4e86\u4e00\u5957\u5b8c\u6574\u7684 ROS2 Humble  PX4 SITL  Gazebo Classic \u4eff\u771f\u73af\u5883&#xff1a; \u301010\u5929\u901f\u901aROS2-PX4\u65e0\u4eba\u673a\u3011(\u4e00) \u4ece\u96f6\u642d\u5efa\u4eff\u771f\u73af\u5883&#xff0c;\u5988\u5988\u518d\u4e5f\u4e0d\u7528\u62c5\u5fc3\u6211\u70b8\u673a\u4e86&#xff01; \u73b0\u5728\u4eff\u771f\u6709\u4e86\u3001VLP-16 3D \u6fc0\u5149\u96f7\u8fbe\u6709\u4e86\u3001IMU \u6709\u4e86\u3001\u7f51\u7b3c\u6d4b\u8bd5\u573a\u6709\u4e86&#xff0c;\u4f46\u98de\u673a\u8fd8\u4e0d\u77e5\u9053\u81ea\u5df1\u5728\u54ea\u91cc\u2014\u2014PX4 \u81ea\u5e26\u7684 EKF \u4f9d\u8d56 GPS&#xff0c;\u5728\u5ba4\u5185\/\u7f51\u7b3c\u8fd9\u79cd G<\/p>\n","protected":false},"author":2,"featured_media":83525,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[1],"tags":[9355,9356,55,2968,3267],"topic":[],"class_list":["post-83533","post","type-post","status-publish","format-standard","has-post-thumbnail","hentry","category-server","tag-lio","tag-mid360","tag-c","tag-ros2","tag-3267"],"yoast_head":"<!-- This site is optimized with the Yoast SEO plugin v20.3 - https:\/\/yoast.com\/wordpress\/plugins\/seo\/ -->\n<title>\u301010\u5929\u901f\u901aROS2-PX4\u65e0\u4eba\u673a\u3011(\u4e8c) FAST-LIO2 \u4eff\u771f\u90e8\u7f72\uff0c200HZ IMU\u524d\u63a8\u7684\u65e0\u9650\u9b45\u529b - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3<\/title>\n<meta name=\"robots\" content=\"index, follow, max-snippet:-1, max-image-preview:large, max-video-preview:-1\" \/>\n<link rel=\"canonical\" href=\"https:\/\/www.wsisp.com\/helps\/83533.html\" \/>\n<meta property=\"og:locale\" content=\"zh_CN\" \/>\n<meta property=\"og:type\" content=\"article\" \/>\n<meta property=\"og:title\" content=\"\u301010\u5929\u901f\u901aROS2-PX4\u65e0\u4eba\u673a\u3011(\u4e8c) FAST-LIO2 \u4eff\u771f\u90e8\u7f72\uff0c200HZ IMU\u524d\u63a8\u7684\u65e0\u9650\u9b45\u529b - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\" \/>\n<meta property=\"og:description\" content=\"\u524d\u8a00 \u4e0a\u4e00\u671f\u6211\u4eec\u82b1\u4e86\u5f88\u5927\u7bc7\u5e45&#xff0c;\u4ece\u96f6\u642d\u5efa\u4e86\u4e00\u5957\u5b8c\u6574\u7684 ROS2 Humble PX4 SITL Gazebo Classic \u4eff\u771f\u73af\u5883&#xff1a; \u301010\u5929\u901f\u901aROS2-PX4\u65e0\u4eba\u673a\u3011(\u4e00) \u4ece\u96f6\u642d\u5efa\u4eff\u771f\u73af\u5883&#xff0c;\u5988\u5988\u518d\u4e5f\u4e0d\u7528\u62c5\u5fc3\u6211\u70b8\u673a\u4e86&#xff01; \u73b0\u5728\u4eff\u771f\u6709\u4e86\u3001VLP-16 3D \u6fc0\u5149\u96f7\u8fbe\u6709\u4e86\u3001IMU \u6709\u4e86\u3001\u7f51\u7b3c\u6d4b\u8bd5\u573a\u6709\u4e86&#xff0c;\u4f46\u98de\u673a\u8fd8\u4e0d\u77e5\u9053\u81ea\u5df1\u5728\u54ea\u91cc\u2014\u2014PX4 \u81ea\u5e26\u7684 EKF \u4f9d\u8d56 GPS&#xff0c;\u5728\u5ba4\u5185\/\u7f51\u7b3c\u8fd9\u79cd G\" \/>\n<meta property=\"og:url\" content=\"https:\/\/www.wsisp.com\/helps\/83533.html\" \/>\n<meta property=\"og:site_name\" content=\"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\" \/>\n<meta property=\"article:published_time\" content=\"2026-07-25T17:17:24+00:00\" \/>\n<meta property=\"og:image\" content=\"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260725171713-6a64ef99d91b1.png\" \/>\n<meta name=\"author\" content=\"admin\" \/>\n<meta name=\"twitter:card\" content=\"summary_large_image\" \/>\n<meta name=\"twitter:label1\" content=\"\u4f5c\u8005\" \/>\n\t<meta name=\"twitter:data1\" content=\"admin\" \/>\n\t<meta name=\"twitter:label2\" content=\"\u9884\u8ba1\u9605\u8bfb\u65f6\u95f4\" \/>\n\t<meta name=\"twitter:data2\" content=\"20 \u5206\" \/>\n<script type=\"application\/ld+json\" class=\"yoast-schema-graph\">{\"@context\":\"https:\/\/schema.org\",\"@graph\":[{\"@type\":\"WebPage\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/83533.html\",\"url\":\"https:\/\/www.wsisp.com\/helps\/83533.html\",\"name\":\"\u301010\u5929\u901f\u901aROS2-PX4\u65e0\u4eba\u673a\u3011(\u4e8c) FAST-LIO2 \u4eff\u771f\u90e8\u7f72\uff0c200HZ IMU\u524d\u63a8\u7684\u65e0\u9650\u9b45\u529b - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\",\"isPartOf\":{\"@id\":\"https:\/\/www.wsisp.com\/helps\/#website\"},\"datePublished\":\"2026-07-25T17:17:24+00:00\",\"dateModified\":\"2026-07-25T17:17:24+00:00\",\"author\":{\"@id\":\"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41\"},\"breadcrumb\":{\"@id\":\"https:\/\/www.wsisp.com\/helps\/83533.html#breadcrumb\"},\"inLanguage\":\"zh-Hans\",\"potentialAction\":[{\"@type\":\"ReadAction\",\"target\":[\"https:\/\/www.wsisp.com\/helps\/83533.html\"]}]},{\"@type\":\"BreadcrumbList\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/83533.html#breadcrumb\",\"itemListElement\":[{\"@type\":\"ListItem\",\"position\":1,\"name\":\"\u9996\u9875\",\"item\":\"https:\/\/www.wsisp.com\/helps\"},{\"@type\":\"ListItem\",\"position\":2,\"name\":\"\u301010\u5929\u901f\u901aROS2-PX4\u65e0\u4eba\u673a\u3011(\u4e8c) FAST-LIO2 \u4eff\u771f\u90e8\u7f72\uff0c200HZ IMU\u524d\u63a8\u7684\u65e0\u9650\u9b45\u529b\"}]},{\"@type\":\"WebSite\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/#website\",\"url\":\"https:\/\/www.wsisp.com\/helps\/\",\"name\":\"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3\",\"description\":\"\u9999\u6e2f\u670d\u52a1\u5668_\u9999\u6e2f\u4e91\u670d\u52a1\u5668\u8d44\u8baf_\u670d\u52a1\u5668\u5e2e\u52a9\u6587\u6863_\u670d\u52a1\u5668\u6559\u7a0b\",\"potentialAction\":[{\"@type\":\"SearchAction\",\"target\":{\"@type\":\"EntryPoint\",\"urlTemplate\":\"https:\/\/www.wsisp.com\/helps\/?s={search_term_string}\"},\"query-input\":\"required name=search_term_string\"}],\"inLanguage\":\"zh-Hans\"},{\"@type\":\"Person\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41\",\"name\":\"admin\",\"image\":{\"@type\":\"ImageObject\",\"inLanguage\":\"zh-Hans\",\"@id\":\"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/image\/\",\"url\":\"https:\/\/gravatar.wp-china-yes.net\/avatar\/?s=96&d=mystery\",\"contentUrl\":\"https:\/\/gravatar.wp-china-yes.net\/avatar\/?s=96&d=mystery\",\"caption\":\"admin\"},\"sameAs\":[\"http:\/\/wp.wsisp.com\"],\"url\":\"https:\/\/www.wsisp.com\/helps\/author\/admin\"}]}<\/script>\n<!-- \/ Yoast SEO plugin. -->","yoast_head_json":{"title":"\u301010\u5929\u901f\u901aROS2-PX4\u65e0\u4eba\u673a\u3011(\u4e8c) FAST-LIO2 \u4eff\u771f\u90e8\u7f72\uff0c200HZ IMU\u524d\u63a8\u7684\u65e0\u9650\u9b45\u529b - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","robots":{"index":"index","follow":"follow","max-snippet":"max-snippet:-1","max-image-preview":"max-image-preview:large","max-video-preview":"max-video-preview:-1"},"canonical":"https:\/\/www.wsisp.com\/helps\/83533.html","og_locale":"zh_CN","og_type":"article","og_title":"\u301010\u5929\u901f\u901aROS2-PX4\u65e0\u4eba\u673a\u3011(\u4e8c) FAST-LIO2 \u4eff\u771f\u90e8\u7f72\uff0c200HZ IMU\u524d\u63a8\u7684\u65e0\u9650\u9b45\u529b - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","og_description":"\u524d\u8a00 \u4e0a\u4e00\u671f\u6211\u4eec\u82b1\u4e86\u5f88\u5927\u7bc7\u5e45&#xff0c;\u4ece\u96f6\u642d\u5efa\u4e86\u4e00\u5957\u5b8c\u6574\u7684 ROS2 Humble PX4 SITL Gazebo Classic \u4eff\u771f\u73af\u5883&#xff1a; \u301010\u5929\u901f\u901aROS2-PX4\u65e0\u4eba\u673a\u3011(\u4e00) \u4ece\u96f6\u642d\u5efa\u4eff\u771f\u73af\u5883&#xff0c;\u5988\u5988\u518d\u4e5f\u4e0d\u7528\u62c5\u5fc3\u6211\u70b8\u673a\u4e86&#xff01; \u73b0\u5728\u4eff\u771f\u6709\u4e86\u3001VLP-16 3D \u6fc0\u5149\u96f7\u8fbe\u6709\u4e86\u3001IMU \u6709\u4e86\u3001\u7f51\u7b3c\u6d4b\u8bd5\u573a\u6709\u4e86&#xff0c;\u4f46\u98de\u673a\u8fd8\u4e0d\u77e5\u9053\u81ea\u5df1\u5728\u54ea\u91cc\u2014\u2014PX4 \u81ea\u5e26\u7684 EKF \u4f9d\u8d56 GPS&#xff0c;\u5728\u5ba4\u5185\/\u7f51\u7b3c\u8fd9\u79cd G","og_url":"https:\/\/www.wsisp.com\/helps\/83533.html","og_site_name":"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","article_published_time":"2026-07-25T17:17:24+00:00","og_image":[{"url":"https:\/\/www.wsisp.com\/helps\/wp-content\/uploads\/2026\/07\/20260725171713-6a64ef99d91b1.png"}],"author":"admin","twitter_card":"summary_large_image","twitter_misc":{"\u4f5c\u8005":"admin","\u9884\u8ba1\u9605\u8bfb\u65f6\u95f4":"20 \u5206"},"schema":{"@context":"https:\/\/schema.org","@graph":[{"@type":"WebPage","@id":"https:\/\/www.wsisp.com\/helps\/83533.html","url":"https:\/\/www.wsisp.com\/helps\/83533.html","name":"\u301010\u5929\u901f\u901aROS2-PX4\u65e0\u4eba\u673a\u3011(\u4e8c) FAST-LIO2 \u4eff\u771f\u90e8\u7f72\uff0c200HZ IMU\u524d\u63a8\u7684\u65e0\u9650\u9b45\u529b - \u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","isPartOf":{"@id":"https:\/\/www.wsisp.com\/helps\/#website"},"datePublished":"2026-07-25T17:17:24+00:00","dateModified":"2026-07-25T17:17:24+00:00","author":{"@id":"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41"},"breadcrumb":{"@id":"https:\/\/www.wsisp.com\/helps\/83533.html#breadcrumb"},"inLanguage":"zh-Hans","potentialAction":[{"@type":"ReadAction","target":["https:\/\/www.wsisp.com\/helps\/83533.html"]}]},{"@type":"BreadcrumbList","@id":"https:\/\/www.wsisp.com\/helps\/83533.html#breadcrumb","itemListElement":[{"@type":"ListItem","position":1,"name":"\u9996\u9875","item":"https:\/\/www.wsisp.com\/helps"},{"@type":"ListItem","position":2,"name":"\u301010\u5929\u901f\u901aROS2-PX4\u65e0\u4eba\u673a\u3011(\u4e8c) FAST-LIO2 \u4eff\u771f\u90e8\u7f72\uff0c200HZ IMU\u524d\u63a8\u7684\u65e0\u9650\u9b45\u529b"}]},{"@type":"WebSite","@id":"https:\/\/www.wsisp.com\/helps\/#website","url":"https:\/\/www.wsisp.com\/helps\/","name":"\u7f51\u7855\u4e92\u8054\u5e2e\u52a9\u4e2d\u5fc3","description":"\u9999\u6e2f\u670d\u52a1\u5668_\u9999\u6e2f\u4e91\u670d\u52a1\u5668\u8d44\u8baf_\u670d\u52a1\u5668\u5e2e\u52a9\u6587\u6863_\u670d\u52a1\u5668\u6559\u7a0b","potentialAction":[{"@type":"SearchAction","target":{"@type":"EntryPoint","urlTemplate":"https:\/\/www.wsisp.com\/helps\/?s={search_term_string}"},"query-input":"required name=search_term_string"}],"inLanguage":"zh-Hans"},{"@type":"Person","@id":"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/358e386c577a3ab51c4493330a20ad41","name":"admin","image":{"@type":"ImageObject","inLanguage":"zh-Hans","@id":"https:\/\/www.wsisp.com\/helps\/#\/schema\/person\/image\/","url":"https:\/\/gravatar.wp-china-yes.net\/avatar\/?s=96&d=mystery","contentUrl":"https:\/\/gravatar.wp-china-yes.net\/avatar\/?s=96&d=mystery","caption":"admin"},"sameAs":["http:\/\/wp.wsisp.com"],"url":"https:\/\/www.wsisp.com\/helps\/author\/admin"}]}},"_links":{"self":[{"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/posts\/83533","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/users\/2"}],"replies":[{"embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/comments?post=83533"}],"version-history":[{"count":0,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/posts\/83533\/revisions"}],"wp:featuredmedia":[{"embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/media\/83525"}],"wp:attachment":[{"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/media?parent=83533"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/categories?post=83533"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/tags?post=83533"},{"taxonomy":"topic","embeddable":true,"href":"https:\/\/www.wsisp.com\/helps\/wp-json\/wp\/v2\/topic?post=83533"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}