\u73af\u5883\u4fe1\u606f\uff1a<\/p>\n
- \n
- Ubuntu 22.04<\/li>\n
- ROS 2 Humble<\/li>\n<\/ul>\n
SLAM\u7b97\u6cd5\uff1a<\/p>\n
- \n
- LIO_SAM<\/li>\n<\/ul>\n
2. LIO-SAM\u7b80\u4ecb<\/h4>\n
LIO-SAM\uff08Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping\uff09\u662f\u4e00\u4e2a\u5229\u7528GT-SAM\u901a\u8fc7\u5e73\u6ed1\u548c\u6620\u5c04\u5b9e\u73b0\u7d27\u8026\u5408\u7684\u6fc0\u5149\u96f7\u8fbe\u60ef\u6027\u91cc\u7a0b\u8ba1\uff0c\u5b9e\u73b0\u4e86\u9ad8\u7cbe\u5ea6\u3001\u5b9e\u65f6\u7684\u79fb\u52a8\u673a\u5668\u4eba\u7684\u8f68\u8ff9\u4f30\u8ba1\u548c\u5efa\u56fe\u3002<\/p>\n
![](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/10\/lio-sam-demo.gif\")
<\/p>\n\u67b6\u6784\u8bbe\u8ba1\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/10\/lio-sam-arch-1024x548.png\")
<\/p>\n\u5de5\u4f5c\u539f\u7406\uff1a\u63a5\u65363D\u6fc0\u5149\u96f7\u8fbe\u3001IMU\u4ee5\u53caGPS\u4f5c\u4e3a\u8f93\u5165\uff0c\u5f15\u5165\u56db\u79cd\u56e0\u5b50\u53bb\u6784\u5efa\u56e0\u5b50\u56fe\u3002<\/p>\n
- \n
- IMU\u9884\u79ef\u5206\u56e0\u5b50<\/li>\n
- \u6fc0\u5149\u91cc\u7a0b\u8ba1\u56e0\u5b50<\/li>\n
- GPS\u56e0\u5b50<\/li>\n
- \u95ed\u73af\u56e0\u5b50<\/li>\n<\/ul>\n
![\"\"](\"http:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/lio-sam-graph-scaled.jpg\")
<\/a><\/p>\n\u8bba\u6587\uff1a<\/p>\n
https:\/\/github.com\/TixiaoShan\/LIO-SAM\/blob\/master\/config\/doc\/paper.pdf<\/a><\/p>\n
<\/p>\n
\u4e8c\u3001\u4f20\u611f\u5668\u51c6\u5907\u5de5\u4f5c<\/h1>\n
1. Hesai 40P<\/h4>\n
1.1 ROS2\u70b9\u4e91\u8c03\u901a<\/h6>\n
\u53c2\u8003\uff1a\u6fc0\u5149\u96f7\u8fbe\u2014\u79be\u8d5bPandar40P<\/a><\/p>\n
1.2 \u65f6\u949f\u540c\u6b65<\/h6>\n
\u7531\u4e8e\u6682\u65f6\u6ca1\u6709\u589e\u52a0\u65f6\u949f\u540c\u6b65\u529f\u80fd\uff0c\u76ee\u524dLidar\u9ed8\u8ba4\u8f93\u51fa\u7684\u662f1970-07-14\u7684\u65f6\u95f4\uff0c\u6211\u4eec\u9700\u8981\u4f7f\u7528PTP\u5bf9\u96f7\u8fbe\u7684\u65f6\u95f4\u8fdb\u884c\u540c\u6b65\uff0c\u65b9\u6cd5\u8be6\u89c1Hesai Lidar PTP<\/a>\u3002<\/p>\n
\u542f\u52a8\u5e76\u68c0\u67e5\u4f20\u611f\u5668\u65f6\u949f\u662f\u5426\u6b63\u5e38\uff1a<\/p>\n
# check hesai timestamp\r\nsudo ptp4l -m -4<\/span> -i eth0\r\ncd ~\/ros2_src<\/code> ros2 launch hesai_ros_driver start.py\r\n# topic<\/span>\r\nros2 topic echo \/sensors\/lidar\/points<\/code><\/code><\/pre>\n2. H30 IMU<\/h4>\n
2.1 ROS2\u6570\u636e\u8f93\u51fa<\/h6>\n
\u53c2\u8003\uff1aIMU\u2014Yesense\u4e5d\u8f74H30<\/a><\/p>\n
2.2 \u65f6\u949f\u540c\u6b65<\/h6>\n
\u7531\u4e8eH30\u6a21\u5757\u4e0d\u5177\u5907\u65f6\u949f\u540c\u6b65\u529f\u80fd\uff0c\u6211\u4eec\u6682\u65f6\u91c7\u7528sensor\u7cbe\u632f\u548c\u7cfb\u7edf\u65f6\u95f4\u7684gap\u6765\u4fee\u6b63IMU\u6570\u636e\u65f6\u95f4\uff0c\u65b9\u6cd5\u8be6\u89c1\uff1aH30 IMU\u65f6\u95f4\u6821\u51c6<\/a>\u3002<\/p>\n
\u542f\u52a8\u5e76\u68c0\u67e5\u4f20\u611f\u5668\u65f6\u949f\u662f\u5426\u6b63\u5e38\uff1a<\/p>\n
# check H30 timestamp<\/span>\r\ncd ~\/ros2_src\r\nros2 launch yesense_std_ros2 yesense_node.launch.py\r\n\r\n# topic<\/span>\r\nros2 topicecho<\/code> \/sensors\/imu\/data<\/code><\/pre>\n3. \u5b89\u88c5\u7ed3\u6784<\/h4>\n
\u4e3a\u4fbf\u4e8e\u6d4b\u8bd5\uff0c\u6211\u4eec\u53ef\u4ee5\u628aIMU\u5b89\u88c5\u5728\u6fc0\u5149\u96f7\u8fbe\u7684\u5e95\u90e8\u6216\u9876\u90e8\uff0c\u8ba9\u5b83\u4eec\u7684 x \u8f74\u6307\u5411\u540c\u4e00\u65b9\u5411\u3002<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/lidar-imu-install.png\")
<\/a><\/h1>\n\u6ce8\u610fPandar40P\u7684X\u65b9\u5411\uff0cX\u65b9\u5411\u5e76\u4e0d\u662f\u7ebf\u675f\u7684\u6b63\u524d\u65b9\uff0c\u800c\u662f90\u5ea6\u4f4d\u7f6e\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/pandar40p-xyz-1024x491.png\")
<\/a><\/p>\nIMU\u7684X\u8f74\u9700\u8981\u4e0eLidar\u7684X\u8f74\u5bf9\u9f50\uff0c\u4e00\u8d77\u671d\u524d\u5b89\u88c5\u3002<\/p>\n
<\/p>\n
\u4e09\u3001LIO_SAM<\/h1>\n
1. \u7f16\u8bd1\u6e90\u7801<\/h4>\n
1.1 ROS2<\/h6>\n
# download code\r\ncd ~\/ros2_ws\/src\r\ngit clone -b ros2 --recursive git@github.com:yanjingang\/LIO-SAM.git\r\n\r\n# depend\r\nsudo apt install -y ros-humble-perception-pcl ros-humble-pcl-msgs ros-humble-vision-opencv ros-humble-xacro \r\n# Add GTSAM-PPA\r\nsudo add-apt-repository ppa:borglab\/gtsam-release-4.1\r\nsudo apt install libgtsam-dev libgtsam-unstable-dev\r\n\r\n# build\r\ncolcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release<\/code><\/pre>\n1.2 ROS1<\/h6>\n
# noetic depend\r\nsudo apt-get install -y ros-noetic-navigation ros-noetic-robot-localization ros-noetic-robot-state-publisher\r\n\r\n# gtsam\r\nsudo add-apt-repository ppa:borglab\/gtsam-release-4.0\r\nsudo apt install libgtsam-dev libgtsam-unstable-dev\r\n\r\n# build\r\ncd ~\/catkin_liosam\/src\r\ngit clone https:\/\/github.com\/TixiaoShan\/LIO-SAM.git\r\nvim LIO-SAM\/CMakeLists.txt\r\n set(CMAKE_CXX_FLAGS \"-std=c++14\")\r\nvim LIO-SAM\/include\/utility.h\r\n #include <opencv2\/opencv.hpp>\r\nvim LIO-SAM\/config\/params.yaml\r\n gpsTopic: \"odometry\/gps\"\r\n Horizon_SCAN: 2000\r\ncd ..\r\ncatkin_make\r\n\r\n# run \r\nsource devel\/setup.bash\r\nroslaunch lio_sam run.launch<\/code><\/pre>\n2. \u914d\u7f6e<\/h4>\n
\u6839\u636e\u4f60\u7684\u96f7\u8fbe\u5e78\u597d\uff0c\u914d\u7f6e\u5bf9\u5e94\u7684\u53c2\u6570\u6211\u8fd9\u91cc\u7528\u7684\u662fPardar40P\uff0c\u6839\u636e\u5b98\u65b9\u4ea7\u54c1\u624b\u518c\u8fdb\u884c\u914d\u7f6e\uff1a<\/p>\n
vim src\/LIO-SAM\/config\/params.yaml\r\n # Topics\r\n pointCloudTopic: \"\/sensors\/lidar\/points<\/span>\" # Point cloud data\r\n imuTopic: \"\/sensors\/imu\/data<\/span>\" # IMU data\r\n\r\n # Sensor Settings\r\n sensor: hesai<\/span> # lidar sensor type, either 'velodyne', 'ouster' or 'livox'\r\n N_SCAN: 40<\/span> # Pardar40P\u670940\u4e2a\u901a\u9053 number of lidar channels (i.e., Velodyne\/Ouster: 16, 32, 64, 128, Livox Horizon: 6)\r\n Horizon_SCAN: 1800<\/span> # Pardar40P\u6c34\u5e73\u5206\u8fa8\u7387 = \u6c34\u5e73\u89c6\u573a\u89d2 \/ \u6c34\u5e73\u89d2\u5206\u8fa8\u7387 = 360 \/ 0.2(10hz\u65f6) = 1800 lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)<\/code><\/pre>\n![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/pandar40p-params.png\")
<\/a><\/p>\n3. Hesai-Veloyne\u70b9\u4e91\u683c\u5f0f\u8f6c\u6362<\/h4>\n
LIO-SAM\u3001FAST-LIO2\u7b97\u6cd5\uff0c\u5bf9\u673a\u68b0\u96f7\u8fbe\u53ea\u9002\u914d\u4e86Veloyne\u96f7\u8fbe\uff0c\u5bf9\u56fd\u5185\u7684\u96f7\u8fbe\u578b\u53f7\u5747\u4e0d\u9002\u914d\u3002\u56fd\u5185\u7684\u901f\u817e\u3001\u79be\u8d5b\u7b49\u96f7\u8fbe\u9700\u8981\u5c06\u70b9\u4e91\u6570\u636e\u7ed3\u6784\u8f6c\u5316\u4e3aVeloyne\u7ed3\u6784\uff0c\u8fdb\u800c\u8fd0\u884cLIO-SAM\u3001FAST-LIO2\u7b49\u5efa\u56fe\u7b97\u6cd5\u3002<\/p>\n
3.1 \u6dfb\u52a0Hesai Lidar\u7c7b\u578b<\/h6>\n
vim src\/LIO-SAM\/include\/lio_sam\/utility.hpp\r\n enum class SensorType { VELODYNE, OUSTER, LIVOX , HESAI<\/span>};\r\n \r\n ParamServer(std::string node_name, const rclcpp::NodeOptions & options) : Node(node_name, options){\r\n ...\r\n else if (sensorStr == \"hesai\")\r\n {\r\n sensor = SensorType::HESAI<\/span>;\r\n }\r\n ...<\/code><\/pre>\n3.2 \u70b9\u4e91\u683c\u5f0f\u8f6c\u6362<\/h6>\n
\u6fc0\u5149\u96f7\u8fbe\u5750\u6807\u7cfb\u8981\u7b26\u5408REP105\u6807\u51c6\uff0c\u5373XYZ\u4ee3\u8868\u524d\/\u5de6\/\u4e0a<\/span>\u65b9\u5411\u3002\u4e0d\u540c\u96f7\u8fbe\u7684\u53ef\u80fd\u4e0d\u4e00\u6837\uff0c\u6ce8\u610f\u70b9\u4e91\u7684\u7ed3\u6784\u8ddf\u4f60\u7684\u6fc0\u5149\u96f7\u8fbe\u9a71\u52a8\u6709\u5173\u3002\u79be\u8d5b\u6fc0\u5149\u96f7\u8fbe\u7684\u5750\u6807\u7cfb\u4e0e\u4f20\u7edf\u7684REP105\u4e0d\u540c\uff0c\u5176XYZ\u5206\u522b\u4ee3\u8868\u5de6\/\u540e\/\u4e0a<\/span>\u65b9\u5411\u3002\u6211\u4eec\u5728cachePointCloud()\u91cc\u8f6c\u6362\u4e3aVelodyne\u6570\u636e\u683c\u5f0f\u7684\u540c\u65f6\uff0c\u4fee\u6b63\u8fd9\u4e2a\u5750\u6807\u7cfb\u504f\u5dee\u3002<\/p>\n
vim src\/LIO-SAM\/src\/imageProjection.cpp\r\n \/\/ Hesai DataType\r\n struct PandarPointXYZIRT<\/span> {\r\n PCL_ADD_POINT4D;\r\n float intensity;\r\n double timestamp;\r\n uint16_t ring; \/\/\/< laser ring number\r\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW \/\/ make sure our new allocators are aligned\r\n } EIGEN_ALIGN16;\r\n POINT_CLOUD_REGISTER_POINT_STRUCT(PandarPointXYZIRT,\r\n (float, x, x)\r\n (float, y, y)\r\n (float, z, z)\r\n (float, intensity, intensity)\r\n (double, timestamp, timestamp)\r\n (uint16_t, ring, ring)\r\n )\r\n ...\r\n class ImageProjection : public ParamServer{\r\n private:\r\n pcl::PointCloud<PandarPointXYZIRT>::Ptr tmpPandarCloudIn<\/span>;\r\n ...\r\n public:\r\n void allocateMemory(){\r\n tmpPandarCloudIn<\/span>.reset(new pcl::PointCloud<PandarPointXYZIRT>());\r\n ...\r\n bool cachePointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr& laserCloudMsg){\r\n else if (sensor == SensorType::HESAI<\/span>) {\r\n \/\/ Convert to Velodyne format\r\n pcl::moveFromROSMsg(currentCloudMsg, *tmpPandarCloudIn);\r\n laserCloudIn->points.resize(tmpPandarCloudIn->size());\r\n laserCloudIn->is_dense = tmpPandarCloudIn->is_dense;\r\n double time_begin = tmpPandarCloudIn->points[0].timestamp;\r\n for (size_t i = 0; i < tmpPandarCloudIn->size(); i++) {\r\n auto &src = tmpPandarCloudIn->points[i];\r\n auto &dst = laserCloudIn->points[i];\r\n dst.x = src.y * -1; \/\/ \u540e->\u524d<\/span>\r\n \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0dst.y = src.x; \/\/ \u5de6<\/span>\r\n \/\/dst.x = src.x;\r\n \/\/dst.y = src.y;\r\n dst.z = src.z;\r\n dst.intensity = src.intensity;\r\n dst.ring = src.ring;\r\n \/\/dst.tiSme = src.t * 1e-9f;\r\n dst.time = src.timestamp - time_begin; \/\/ s\r\n }\r\n }\r\n ...\r\n void projectPointCloud()\r\n if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER || sensor == SensorType::HESAI<\/span>)\r\n ...<\/code><\/pre>\n\u8c03\u6574\u540e\uff0cHesai Pandar\u6fc0\u5149\u96f7\u8fbe\u7684base_link\u53d8\u4e3aREP105\u6807\u51c6\u7684\uff0c\u4f60\u53ea\u9700\u8981\u628a\u5b83\u5f53\u4f5cVelodyne\u6fc0\u5149\u96f7\u8fbe\u6765\u8c03\u6574IMU\u4e4b\u95f4\u7684\u5916\u90e8\u53c2\u6570\u3002<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/lidar-base-link-1024x931.jpg\")
<\/a><\/p>\n4. IMU-Lidar\u6807\u5b9a<\/h4>\n
\u5728\u4f7f\u7528LIO-SAM\u8fdb\u884cLidar+IMU\u5efa\u56fe\u4e4b\u524d\uff0c\u9700\u8981\u505a\u4e24\u4e2a\u6807\u5b9a\u5de5\u4f5c\uff0cIMU\u5185\u53c2\u6807\u5b9a\u548cLidar-IMU\u5916\u53c2\u6807\u5b9a\u3002IMU\u5185\u53c2\u6807\u5b9a\uff0c\u53ef\u4ee5\u89e3\u51b3\u5176\u56fa\u6709\u7684\u6d4b\u91cf\u8bef\u5dee\u95ee\u9898\uff1bLidar\u548cIMU\u7684\u5916\u53c2\u6807\u5b9a(\u4ee5\u4e0b\u7b80\u79f0\u201c\u5916\u53c2\u6807\u5b9a\u201d)\u3002\u5916\u53c2\u6807\u5b9a\u7684\u76ee\u7684\u662f\u83b7\u5f97\u6fc0\u5149\u96f7\u8fbe\u548cIMU\u4e4b\u95f4\u7684\u4f4d\u7f6e\u8f6c\u6362\u5173\u7cfb\uff0c\u5176\u4e2d\u5305\u62ec\u5e73\u79fb\u5173\u7cfb\u548c\u65cb\u8f6c\u5173\u7cfb\uff0c\u5206\u522b\u5bf9\u5e94\u6700\u7ec8\u8f93\u51fa\u7ed3\u679c\u4e2d\u7684\u5e73\u79fb\u5411\u91cf\u4e0e\u65cb\u8f6c\u77e9\u9635\u3002\u9700\u8981\u4f7f\u7528Ubuntu20.04 + ROS1\u73af\u5883\u8fdb\u884c\u6807\u5b9a\u3002<\/p>\n
4.1 \u5750\u6807\u7cfb<\/h6>\n
LIO-SAM \u5c06 IMU \u539f\u59cb\u6570\u636e\u4ece IMU \u6846\u67b6\u8f6c\u6362\u4e3a\u6fc0\u5149\u96f7\u8fbe\u6846\u67b6\uff0c\u6240\u4ee5IMU\u540c\u6837\u9700\u8981\u9075\u5faa ROS REP-105 \u7ea6\u5b9a\u7684\u5750\u6807\u7cfb\uff08x – \u5411\u524d\uff0cy – \u5411\u5de6\uff0cz – \u5411\u4e0a\uff09\u3002\u7531\u4e8eH30 IMU\u7684XYZ\u540c\u6837\u4e3a\u524d\u5de6\u4e0a\uff0c\u56e0\u6b64\u4e0d\u9700\u8981\u8c03\u6574\u3002<\/p>\n
Lidar\u8ddfIMU\u4e00\u8d77\u90fd\u7ed1\u5b9a\u5230\u5e95\u76d8link\u4e0a\uff1a<\/p>\n
vim src\/LIO-SAM\/config\/robot.urdf.xacro\r\n <link name=\"lidar_link\"> <\/link>\r\n <parent link=\"chassis_link<\/span>\" \/><\/code><\/pre>\n4.2 IMU\u5185\u53c2<\/h6>\n
\u5185\u53c2\u7684\u83b7\u53d6\u65b9\u6cd5\u8be6\u89c1\uff1aH30 IMU + Pandar40P Lidar\u8054\u5408\u6807\u5b9a<\/a><\/p>\n
IMU\u5185\u53c2\u914d\u7f6e\uff1a<\/p>\n
vim src\/LIO-SAM\/config\/params.yaml\r\n # IMU Settings (IMU\u5185\u53c2)\r\n # imuAccNoise: 3.9939570888238808e-03\r\n # imuGyrNoise: 1.5636343949698187e-03\r\n # imuAccBiasN: 6.4356659353532566e-05\r\n # imuGyrBiasN: 3.5640318696367613e-05\r\n imuAccNoise: 7.5656614363139437e-03 # acc_n\r\n imuGyrNoise: 1.2050330151032497e-03 # gyr_n\r\n imuAccBiasN: 2.9325075138204079e-04 # acc_w\r\n imuGyrBiasN: 1.4177677047615486e-05 # gyr_w\r\n\r\n imuGravity: 9.80511\r\n imuRPYWeight: 0.01<\/code><\/pre>\n4.3 IMU-Lidar\u5916\u53c2<\/h6>\n
\u4e3a\u4e86\u4f7f\u7cfb\u7edf\u6b63\u5e38\u8fd0\u884c\uff0c\u9700\u8981\u5728\u201cparams.yaml\u201d\u6587\u4ef6\u4e2d\u63d0\u4f9b\u6b63\u786e\u7684\u5916\u90e8\u8f6c\u6362\uff0c\u6211\u4eec\u9700\u8981\u81ea\u5df1\u6839\u636eLidar-IMU\u6807\u5b9a\u60c5\u51b5\u4fee\u6539\u8fd9\u91cc\u7684\u6807\u5b9a\u5916\u53c2\u3002\uff08\u914d\u7f6e\u91cc\u6709\u4e24\u4e2a\u5916\u90e8\u53c2\u6570\u7684\u539f\u56e0\u662f\u4f5c\u8005\u7528\u7684 IMU Microstrain 3DM-GX5-25\u52a0\u901f\u5ea6\u548c\u59ff\u6001\u5177\u6709\u4e0d\u540c\u7684\u5750\u6807\uff0c\u7528\u4e8e\u5206\u522b\u8bbe\u7f6e\uff09<\/p>\n
- \n
- extrinsicTrans\uff1aLidar\u5750\u6807\u7cfb\u7684\u539f\u70b9\u5728imu\u5750\u6807\u7cfb\u4e0b\u7684\u5750\u6807\u503c\u3002<\/li>\n
- extrinsicRot\uff1a\u8868\u793aimu->lidar\u7684\u5750\u6807\u53d8\u6362, \u7528\u4e8e\u65cb\u8f6cimu\u5750\u6807\u7cfb\u4e0b\u7684\u52a0\u901f\u5ea6\u548c\u89d2\u901f\u5ea6\u5230lidar\u5750\u6807\u7cfb\u4e0b<\/li>\n
- extrinsicRPY\uff1a\u65cb\u8f6cimu\u5750\u6807\u7cfb\u4e0b\u7684\u6b27\u62c9\u89d2\u5230lidar\u5750\u6807\u4e0b<\/li>\n<\/ul>\n
\u5916\u53c2\u7684\u6807\u5b9a\u65b9\u6cd5\u8be6\u89c1\uff1aH30 IMU + Pandar40P Lidar\u8054\u5408\u6807\u5b9a<\/a><\/p>\n
\u5916\u53c2\u914d\u7f6e\uff1a<\/p>\n
vim src\/LIO-SAM\/config\/params.yaml\r\n # Extrinsics (lidar -> IMU \u5916\u53c2)\r\n # extrinsicTrans: [ 0.0, 0.0, 0.0 ]\r\n # extrinsicRot: [-1.0, 0.0, 0.0,\r\n # 0.0, 1.0, 0.0,\r\n # 0.0, 0.0, -1.0 ]\r\n # extrinsicRPY: [ 0.0, 1.0, 0.0,\r\n # -1.0, 0.0, 0.0,\r\n # 0.0, 0.0, 1.0 ]\r\n extrinsicTrans: [0.0169827, -0.0285108, 0.097318]\r\n extrinsicRot: [-0.03966705, -0.99917971, -0.00815956,\r\n 0.99920914, -0.0396428, -0.00311358,\r\n 0.00278756, -0.00827661, 0.99996186]\r\n extrinsicRPY: [-0.03966705, -0.99917971, -0.00815956,\r\n 0.99920914, -0.0396428, -0.00311358,\r\n 0.00278756, -0.00827661, 0.99996186]\r\n<\/code><\/pre>\n5. \u91cd\u65b0\u7f16\u8bd1<\/h4>\n
# build\r\ncd ~\/ros2_ws\r\ncolcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=lio_sam<\/code><\/pre>\n<\/h4>\n
\u4e09\u3001SLAM\u5efa\u56fe<\/h1>\n
1. \u91c7\u96c6\u6570\u636e\u5efa\u56fe<\/h4>\n
# \u542f\u52a8\u4f20\u611f\u5668\r\nros2 launch hesai_ros_driver start.py\r\nros2 launch yesense_std_ros2 yesense_node.launch.py\r\n\r\n# \u542f\u52a8PTP\u65f6\u949f\u540c\u6b65\uff0c\u89c2\u5bdfLidar\/IMU\u65f6\u95f4\u662f\u5426\u4e00\u81f4\r\nsudo ptp4l -m -4<\/span> -i eth0\r\n\r\n# \u542f\u52a8SLAM\u5efa\u56fe\r\nros2 launch lio_sam run.launch.py\r\n\r\n# \u4fdd\u5b58\u5730\u56fe\r\nros2 service call \/lio_sam\/save_map lio_sam\/srv\/SaveMap<\/code><\/pre>\n2. \u5e38\u89c1\u95ee\u9898<\/h4>\n
2.1 \u6298\u7ebf\u6216\u87ba\u65cb<\/h6>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/lio-sam-test-1024x583.png\")
<\/a> ![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/lio-sam-test2-1024x583.png\")
<\/a><\/p>\n\u5982\u679c\u6fc0\u5149\u96f7\u8fbe\u548c IMU \u6570\u636e\u683c\u5f0f\u7b26\u5408 LIO-SAM \u7684REP-105\u683c\u5f0f\u8981\u6c42\uff0c\u90a3\u4e48\u8fd9\u4e2a\u95ee\u9898\u53ef\u80fd\u662f\u7531\u6fc0\u5149\u96f7\u8fbe\u548c IMU \u6570\u636e\u7684\u65f6\u95f4\u6233\u4e0d\u540c\u6b65\u6216\u540c\u6b65\u7cbe\u5ea6\u4e0d\u8db3\u5f15\u8d77\u7684\u3002<\/p>\n
\u89e3\u51b3\u65b9\u6cd5\uff1a<\/p>\n
- \n
- \u786e\u8ba4\u6fc0\u5149\u96f7\u8fbe\u548cIMU\u7684X\u8f74\u65b9\u5411\u4fdd\u6301\u4e00\u81f4<\/li>\n
- \u8c03\u6574\u6fc0\u5149\u96f7\u8fbe\u70b9\u4e91\u5750\u6807\u683c\u5f0f\u4e3aREP-105\u683c\u5f0f<\/li>\n
- \u4fdd\u6301\u5f00\u7740ptp\u65f6\u949f\u540c\u6b65\uff0c\u540c\u65f6\u964d\u4f4eIMU\u4e0e\u7cfb\u7edf\u65f6\u949f\u540c\u6b65\u7684\u8bef\u5dee\u52305-10ms<\/li>\n<\/ul>\n
\u5b9e\u6d4b\u5b8c\u6210\u4ee5\u4e0a\u4e09\u6b65\u540e\uff0c\u6298\u7ebf\u548c\u87ba\u65cb\u65cb\u8f6c\u73b0\u8c61\u6d88\u5931\uff0c\u5f00\u59cb\u51fa\u73b0\u521d\u59cb\u5316\u8df3\u52a8\u95ee\u9898\u3002<\/span><\/p>\n
2.2 \u4e0a\u4e0b\u8df3\u52a8<\/h6>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/lia-sam-3floor-1024x583.png\")
<\/a><\/p>\n\u5982\u679c\u521a\u5f00\u59cbbase_link \u7acb\u5373\u5f00\u59cb\u4e0a\u4e0b\u8df3\u52a8\uff0c\u90a3\u4e48IMU extrinsics \u5f88\u53ef\u80fd\u662f\u9519\u8bef\u7684\u3002\u4f8b\u5982\uff0c\u91cd\u529b\u52a0\u901f\u5ea6\u4e3a\u8d1f\u503c\u3002<\/p>\n
\u89e3\u51b3\u65b9\u6cd5\uff1a<\/p>\n
- \n
- IMU frame\u4f7f\u7528base_link\u3002<\/li>\n
- IMU\u5185\u53c2\u6807\u5b9a\uff1a\u8bbe\u7f6eparams.yaml\u4e2d\u7684imuAccNoise\u3001imuGyrNoise\u3001imuAccBiasN\u3001imuGyrBiasN\u3002<\/li>\n
- IMU-Lidar\u5916\u53c2\u6807\u5b9a\uff1a\u8bbe\u7f6eparams.yaml\u4e2d\u7684extrinsicTrans\u3001extrinsicRot\u3001extrinsicRPY\u77e9\u9635(\u4e00\u5b9a\u8981\u8bbe\u7f6e\u6b63\u786e\uff0c\u5426\u5219\u5404\u79cd\u8dd1\u98de\u548c\u6296\u52a8\u73b0\u8c61)\u3002<\/li>\n<\/ul>\n
2.3 \u79fb\u52a8\u8ddd\u79bb\u4e0d\u51c6<\/h6>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/lio-sam-1024x585.png\")
<\/a><\/p>\n\u5b9e\u9645\u79fb\u52a8\u4e0e\u5730\u56fe\u4e0a\u7684\u8ddd\u79bb\u4e0d\u5bf9\u5e94,\u5730\u56fe\u4e0a\u8d70\u7684\u66f4\u6162,\u8d70\u7740\u8d70\u7740\u5c31\u4f1a\u91cd\u53e0\u3002extrinsicRot\u548cextrinsicRPY\u77e9\u9635\u5982\u679c\u8bbe\u7f6e\u4e0d\u6b63\u786e\u5219\u5efa\u51fa\u7684\u5730\u56fe\u6216\u51fa\u73b0\u91cd\u53e0\u3002<\/p>\n
\u89e3\u51b3\u65b9\u6cd5:<\/p>\n
- \n
- IMU\u5185\u53c2\u6807\u5b9a<\/li>\n
- IMU-Lidar\u5916\u53c2\u6807\u5b9a<\/li>\n<\/ul>\n
2.4 \u4fdd\u5b58\u5730\u56fe\u8fc7\u7a0b\u4e2dtopic\u6d88\u5931<\/h6>\n
\u5730\u56fe\u8fc7\u5927\uff0c\u4fdd\u5b58\u5730\u56fe\u6587\u4ef6\u65f6\u8017\u65f6\u8f83\u957f\uff0c\u4f46\u662f\u5efa\u56fe\u7a0b\u5e8f\u7684map topic\u8f93\u51fa\u4e0d\u4f1a\u7b49\u90a3\u4e48\u5c31\u5c31\u9000\u51fa\u4e86\uff0c\u9700\u8981\u624b\u52a8\u8c03\u4e0bros\u7684\u9ed8\u8ba4\u9000\u51fa\u65f6\u95f4\uff1a<\/p>\n
sudo vim \/opt\/ros\/humble\/lib\/python3.10\/site-packages\/launch\/actions\/execute_local.py\r\n sigterm_timeout: SomeSubstitutionsType = LaunchConfiguration(\r\n 'sigterm_timeout', default=30<\/span>),\r\n sigkill_timeout: SomeSubstitutionsType = LaunchConfiguration(\r\n 'sigkill_timeout', default=30<\/span>),\r\n<\/code><\/pre>\n3. \u89e3\u51b3\u65b9\u6cd5<\/h4>\n
\u6211\u9047\u5230\u5efa\u56fe\u5efa\u82b1\u540e\uff0c\u91c7\u53d6\u4e86\u4ee5\u4e0b\u51e0\u4e2a\u63aa\u65bd\uff0c\u6700\u7ec8\u5efa\u56fe\u6210\u529f\uff1a<\/p>\n
- \n
- Lidar\u3001IMU\u90fd\u5148\u5bf9\u9f50\u5230\u64cd\u4f5c\u7cfb\u7edf\u65f6\u95f4<\/li>\n
- \u8c03\u9ad8\u6fc0\u5149\u96f7\u8fbe\u4f4d\u7f6e\uff0c\u8ba9\u5b83\u53ef\u4ee5\u626b\u63cf360\u5ea6\uff0c\u540c\u65f6\u53bb\u6389\u89d2\u5ea6\u5c4f\u853d\u914d\u7f6e<\/li>\n
- \u6d4b\u91cf\u5e76\u8c03\u6574\u6fc0\u5149\u96f7\u8fbe\u4e0e\u8f66\u8f86\u540e\u8f74\u4e2d\u5fc3\u7684tf\u5173\u7cfb\uff0c\u4e4b\u524d\u914d\u7f6e\u7684z\u9ad8\u5ea6\u503c\u662f\u9519\u7684<\/li>\n
- \u589e\u52a0gps\u7684\u4ecb\u5165<\/li>\n<\/ol>\n
\u589e\u52a0GPS\u540e\u7684rqt_graph\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/liosam-gps-1024x557.png\")
<\/a><\/p>\n\u5efa\u56fe\u6548\u679c\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/lio_sam_6axis-jhy-1024x629.png\")
<\/a><\/p>\nPCD\u5730\u56fe\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/map-liosam-jingheyuan-1024x604.png\")
<\/a><\/p>\n<\/p>\n
\u56db\u3001\u603b\u7ed3<\/h1>\n
\u603b\u4f53\u6765\u8bf4LIO-SAM\u7b97\u6cd5\u5bf9\u7cbe\u5ea6\u975e\u5e38\u654f\u611f\uff0c\u8981\u60f3\u6b63\u5e38\u5efa\u56fe\uff0c\u9700\u8981\u6ce8\u610f\u4ee5\u4e0b\u4e8b\u9879\uff1a<\/span><\/p>\n
- \n
- Lidar\u548cIMU\u65f6\u95f4\u5bf9\u9f50<\/li>\n
- Lidar\u3001IMU\u6570\u636e\u683c\u5f0f\u7b26\u5408 LIO-SAM \u7684REP-105\u683c\u5f0f\u8981\u6c42\uff0c\u5c24\u5176\u6ce8\u610f\u5750\u6807\u7cfb\u65b9\u5411<\/li>\n
- IMU\u5185\u53c2\u3001Lidar->IMU\u5916\u53c2(\u6c34\u5e73\u65b9\u5411) \u9700\u8981\u7cbe\u51c6<\/li>\n
- \u5c40\u90e8\u6709\u8db3\u591f\u7684\u7279\u5f81\u70b9\n
- \n
- \u9053\u8def\u5bbd\u5ea6\u5927\u4e8e\u4e24\u4e2a\u8f66\u4f53\u5bbd\u5ea6\uff0c\u559c\u6b22\u5efa\u7b51\u7269\u7b49\u7279\u5f81\u660e\u663e\u7684\u73af\u5883<\/li>\n
- \u5982\u679c\u516c\u56ed\u90a3\u79cd\u6ca1\u6709\u5efa\u7b51\uff0c\u53ea\u6709\u9053\u8def\u548c\u6811\u6728\u7684\uff0c\u7279\u5f81\u4e0d\u8db3\uff0c\u65e0\u6cd5\u5efa\u56fe<\/li>\n
- \u7279\u5f81\u4e0d\u8db3\u65f6\uff0c\u91c7\u96c6\u8fc7\u7a0b\u4e2d\u8f66\u8f86\u7684\u6643\u52a8\u6216\u5176\u4ed6\u8f66\u8f86\u6216\u884c\u4eba\u7684\u8fdb\u5165\u5f88\u5bb9\u6613\u5bfc\u81f4\u5730\u56fe\u5efa\u82b1<\/li>\n<\/ul>\n<\/li>\n
- \u66f4\u591a\u6fc0\u5149\u96f7\u8fbe\u7684\u7ebf\u6570\u3001360\u5ea6\u7684\u96f7\u8fbe\u626b\u63cf\u89c6\u89d2\u3001\u66f4\u9ad8\u7684IMU\u5e27\u7387\u3001\u589e\u52a0GPS\u6570\u636e\uff0c\u5bf9\u5efa\u56fe\u6548\u679c\u90fd\u6709\u6539\u5584\u4f5c\u7528<\/li>\n<\/ol>\n
<\/p>\n
\u53c2\u8003\uff1a<\/p>\n
https:\/\/github.com\/TixiaoShan\/LIO-SAM\/tree\/ros2<\/a><\/p>\n
LIO_SAM 6AXIS hesai adaption<\/a><\/p>\n
LIO-SAM\uff1a\u914d\u7f6e\u73af\u5883\u3001\u5b89\u88c5\u6d4b\u8bd5\u3001\u9002\u914d\u81ea\u5df1\u91c7\u96c6\u6570\u636e\u96c6<\/a><\/p>\n
IMU\u5185\u53c2\u6807\u5b9a\/Lidar-IMU\u5916\u53c2\u6807\u5b9a<\/a><\/p>\n
\u6fc0\u5149\u96f7\u8fbe\u548cIMU\u8054\u5408\u6807\u5b9a\u5e76\u8fd0\u884cLIOSAM<\/a><\/p>\n
\u5341\u56db.\u6fc0\u5149\u548c\u60ef\u5bfcLIO-SLAM\u6846\u67b6\u5b66\u4e60\u4e4b\u60ef\u5bfc\u5185\u53c2\u6807\u5b9a<\/a><\/p>\n
\u5341\u4e94.\u6fc0\u5149\u548c\u60ef\u5bfcLIO-SLAM\u6846\u67b6\u5b66\u4e60\u4e4b\u60ef\u5bfc\u4e0e\u96f7\u8fbe\u5916\u53c2\u6807\u5b9a\uff081\uff09<\/a><\/p>\n
3D \u6fc0\u5149\u96f7\u8fbe\u548c IMU \u5916\u90e8\u53c2\u6570\u7684\u81ea\u52a8\u6821\u51c6<\/a><\/p>\n
\u901f\u817e16\u7ebf\u6fc0\u5149+IMU\u5916\u53c2\u6807\u5b9a<\/a><\/p>\n
IMU+\u6fc0\u5149\u96f7\u8fbe\u878d\u5408\u4f7f\u7528LIO-SAM\u5efa\u56fe\u5b66\u4e60\u7b14\u8bb0\u2014\u2014\u8be6\u7ec6\u3001\u957f\u6587\u3001\u591a\u56fe\u3001\u5168\u6d41\u7a0b<\/a><\/p>\n
\u79be\u8d5b\u3001\u901f\u817e\u4e09\u7ef4\u96f7\u8fbe\u9002\u914dlio-sam\u3001fast-lio2\u5efa\u56fe\u7b97\u6cd5<\/a><\/p>\n
LIO-SAM\u8fd0\u884c\u81ea\u5df1\u6570\u636e\u5305\u9047\u5230\u7684\u95ee\u9898\u89e3\u51b3–SLAM\u4e0d\u5b66\u65e0\u6570\u672f\u5c0f\u95ee\u9898<\/a><\/p>\n
LIO-SAM Hesai PandarXT32\u96f7\u8fbe imu\uff1aN100 \u6234\u4e16\u7ec4\u5408\u60ef\u5bfcGPS<\/a><\/p>\n
- LIO_SAM<\/li>\n<\/ul>\n