![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/gnss-ndt-localization-re.png\")
<\/a><\/p>\n\u4e8c\u3001\u5b9a\u4f4d\u539f\u7406<\/h1>\n
\u7531\u4e8e\u5c0f\u8f66\u5e95\u76d8\u8001\u8dd1\u504f\uff0codom\u8bba\u901f\u8ba1\u4e0d\u51c6\uff0c\u672c\u6587\u4e2d\u7684\u5b9a\u4f4d\u4e3b\u8981\u4f7f\u7528RTK+IMU+Lidar\u3002<\/p>\n
<\/p>\n fix2tfpose\u5305\u901a\u8fc7\u8ba2\u9605fix\uff0c\u53d1\u5e03gps\u5b9a\u4f4dtopic\u4e3a\/gnss_pose\u3002\u6211\u4eec\u542f\u7528GNSS\u65f6\uff0c\u9664\u4e86use_gnss\u8bbe\u7f6e\u4e3a1\u4ee5\u5916\uff0c\u8fd8\u9700\u8981\u4fee\u6539gps fix\u7684topic\u540d\u79f0\uff1a<\/p>\n \u542f\u52a8\u64ad\u5305\u548c\u5b9a\u4f4d\u53ef\u4ee5\u770b\u5230fix2pose\u5df2\u7ecf\u751f\u6548\uff0c\u4f46\u662f\u8f93\u51fa\u7684\/gnss_pose\u4e0emap frame\u6709\u4e00\u4e2a\u8ddd\u79bb\uff1a<\/p>\n <\/p>\n \u521a\u624d\u6211\u4eec\u5df2\u7ecf\u5c1d\u8bd5\u8ba9Autoware\u7684GNSS\u5b9a\u4f4d\u751f\u6548\u4e86\uff0c\u4f46\u662f\u5b83\u7684\u4f4d\u7f6e\u4e0emap\u4f4d\u7f6e\u65e0\u6cd5\u5339\u914d\uff0c\u56e0\u6b64\u6211\u4eec\u8fd8\u9700\u8981\u8ba1\u7b97GNSS\u5b9a\u4f4d\u4e0eNDT\u5b9a\u4f4d\u4f4d\u59ff\u7684\u65cb\u8f6c\u77e9\u9635\uff0c\u8ba9GNSS\u548cNDT Matching\u7684\u5b9a\u4f4d\u80fd\u5339\u914d\u4e0a\u3002<\/p>\n geo_pos_conv.cpp \u4e3b\u8981\u5b9e\u73b0\u5730\u7406\u4f4d\u7f6e\u8f6c\u6362\u5668\u7684\u529f\u80fd\uff0c\u5c06\u7eac\u5ea6\u3001\u7ecf\u5ea6\u3001\u9ad8\u5ea6\u5750\u6807(LLH) \u8f6c\u5316\u4e3a\u7b1b\u5361\u5c14\u5750\u6807 (XYZ) \u3002\u4f7f\u7528\u201cset_plane\u201d\u51fd\u6570\u6839\u636e runtime \u8bbe\u7f6e\u7684\u53c2\u6570\u8f93\u5165\u5bf9\u5e94\u7eac\u5ea6\u548c\u7ecf\u5ea6\u8bbe\u7f6e\u5e73\u9762\uff0c\u83b7\u5f97\u7684 XYZ \u5750\u6807\u3002 \u4f8b\u5982\u6211\u6240\u5728\u5730\u533a\u7684wgs84\u7ecf\u7eac\u5ea6\u4e3alongitude,latitude<\/span>:<\/span> 116.260100\u00b0,40.048550\u00b0\uff0c\u5c06<\/span>\u7ecf\u7eac\u5ea6\u8f6c\u6362\u5ea6\u5206\u79d2\u7684\u65b9\u5f0f\uff1a<\/p>\n \u53ea\u4fee\u6539 geo_pos_conv.cpp \u6587\u4ef6\u5730\u56fe\u548c pcd \u70b9\u4e91\u8fd8\u662f\u65e0\u6cd5\u5bf9\u9f50\uff0c\u9700\u8981\u52a0\u5165\u65cb\u8f6c\u77e9\u9635\uff0c\u5c06\u6e90\u5750\u6807\u7cfb\uff08map\uff09\u4e2d\u7684\u70b9\u53d8\u6362\u5230\u76ee\u6807\u5750\u6807\u7cfb\u4e2d\u3002<\/p>\n \u6bcf\u4e2a\u5730\u7406\u4f4d\u7f6e\u7684\u65cb\u8f6c\u77e9\u9635\u53c2\u6570\u90fd\u662f\u4e0d\u540c\u7684\u3002\u8981\u83b7\u53d6\u65cb\u8f6c\u77e9\u9635\u7684\u6b65\u9aa4\uff0c\u9700\u8981\u5148\u6253\u5f00\u5730\u56fe\u5b9a\u4f4d\u529f\u80fd\uff0c\u53ef\u4ee5\u4f7f\u7528\u6570\u636e\u5305\u5b9a\u4f4d\uff0c\u4e5f\u53ef\u4ee5\u4f7f\u7528\u5c0f\u8f66\u5b8c\u6210\u5b9e\u65f6\u5b9a\u4f4d\u3002\u4e0b\u9762\u4ee5 rosbag \u4e3a\u4f8b\uff0c\u7b80\u8ff0\u64cd\u4f5c\u6b65\u9aa4\u3002<\/p>\n \u5982\u679cndt\u4e0d\u80fd\u81ea\u884c\u5b9a\u4f4d\uff0c\u5148\u624b\u52a82D pOSE Estimate\u8bbe\u7f6e\u4e0b\u521d\u59cb\u4f4d\u59ff\uff0c\u786e\u4fdd\u70b9\u4e91\u5730\u56fe<\/span>\u5904\u4e8e\u5df2\u5339\u914d<\/span>\u72b6\u6001\u540e\uff0c\u6309\u7a7a\u683c\u6682\u505c\u64ad\u5305\uff1a<\/p>\n *\u6ce8\uff1a\u8fd9\u91cc\u521d\u59cb\u4f4d\u7f6e\u662f\u76f8\u5bf9\u70b9\u4e91\u5730\u56fe\u7684\u4f4d\u7f6e\uff0c\u5e76\u4e14\u5468\u56f4\u70b9\u4e91\u8981\u80fd\u5339\u914d\u4e0a\uff01\u4e0d\u662f\u76f8\u5bf9\u4e8e\u77e2\u91cf\u56fe\uff0c\u4e00\u5b9a\u6ce8\u610f\uff01<\/p>\n \u542f\u52a8\u8ba1\u7b97\u65cb\u8f6c\u77e9\u9635\u8282\u70b9\uff08\u6b64\u65f6\u6570\u636e\u5305\u8fd8\u5904\u4e8e\u6682\u505c\u64ad\u653e\u72b6\u6001\uff09\uff1a<\/p>\n \u8fd9\u4e2a\u8282\u70b9\u7684\u529f\u80fd\u662f\u7b49\u5f85\u63a5\u6536\u5230\u8db3\u591f\u6570\u91cf\u7684\u6837\u672c\uff08\/ndt_pose \u548c\/gnss_pose \u7684\u8bdd\u9898\u6570\u636e\uff0c\u6240\u4f7f\u7528\u7684\u6570\u636e\u5305\u9700\u8981\u5f55\u5236\u5927\u6982\u4e09\u5206\u949f\uff0c\u5f55\u5236\u7684\u6570\u636e\u6700\u597d\u662f\u80fd\u5728\u8d77\u70b9\u4f4d\u7f6e\u9759\u6b62\uff09\uff0c\u6267\u884c\u77e9\u9635\u8ba1\u7b97\uff08\u521a\u6027\u53d8\u6362\uff09\u5e76\u6253\u5370\u7ed3\u679c\u3002<\/p>\n \u5f00\u59cb\u64ad\u653e\u6570\u636e\uff0c\u7b49\u5f853\u5206\u949f\u5de6\u53f3\uff0cgnss_matrix.py\u4f1a\u8f93\u51fa\u65cb\u8f6c\u77e9\u9635\uff1a<\/p>\n \u6211\u4eec\u9700\u8981\u7684\u662f\u6700\u4e0b\u8fb9\u7684r martrix\u3001t matrix\uff0c\u5b83\u662f\uff1a<\/p>\n *\u6ce8\uff1a\u5982\u679c\u51fa\u73b0 nan \u65e0\u6548\u503c\uff0c\u53ea\u9700\u8981\u518d\u6267\u884c\u4e00\u6b21\u4ee5\u4e0a\u6240\u6709\u6b65\u9aa4\u91cd\u65b0\u8ba1\u7b97\u5373\u53ef\u3002<\/p>\n \u6839\u636e gnss_matrix \u8282\u70b9\u7b97\u51fa\u7684\u65cb\u8f6c\u77e9\u9635\uff0c\u586b\u5165 geo_pos_conv.cpp \u6587\u4ef6\u4e2d\uff0c\u4fee\u6539\u672b\u5c3e\u5904\u7684 R \u548c T \u53c2\u6570\u3002<\/p>\n \u4fee\u6539\u5b8c\u6210\u540e\uff0c\u5355\u72ec\u7f16\u8bd1\u8be5\u529f\u80fd\u5305\uff1a<\/p>\n \u53ef\u4ee5\u770b\u5230\u867d\u7136\u4e0d\u50cf\u4e4b\u524d\u504f\u79bb\u90a3\u4e48\u8fdc\u4e86\uff0c\u4f46\u662f\u4f9d\u7136\u662f\u6709\u504f\u5dee\u7684\uff0c\u53ef\u80fd\u662f\u521d\u59cb\u5316\u7684\u65f6\u5019gnss\u5b9a\u4f4d\u4e0d\u51c6\uff0c\u6211\u8fd9\u91cc\u7b49\u64ad\u5305\u4e00\u5b9agnss\u7a33\u5b9a\u540e\u518d\u8fdb\u884c\u77e9\u9635\u8ba1\u7b97\uff1a<\/p>\n \u6ce8\uff1a\u91cd\u8dd1\u65f6\uff0c\u4e00\u5b9a\u8981\u5148\u6062\u590dR\/\u539f\u59cb\u503c\uff0c\u7f16\u8bd1\u540e\u518d\u8dd1gnss_matrix\uff08\u5426\u5219\u8dd1\u51fa\u6765\u4f1a\u662f\u76f8\u5bf9\u4e4b\u524d\u65cb\u8f6c\u7684\u57fa\u7840\u4e0a\u518d\u65cb\u8f6c\u7684\u503c\uff09<\/p>\n \u5f97\u5230\u4e00\u7ec4\u65b0\u7684\u77e9\u9635\uff1a<\/p>\n \u8bbe\u7f6e\u5e76\u7f16\u8bd1gnss\u540e\u786e\u8ba4\u6548\u679c\uff1a<\/p>\n *\u6ce8\uff1agps\u7684\u5b9a\u4f4d\uff0c\u4e0d\u662f\u5f88\u7a33\u5b9a\uff0c\u6709\u65f6\u53ef\u4ee5\u8ddfndt\u5b8c\u5168\u5339\u914d\uff0c\u6709\u65f6\u4f1a\u504f\u79fb\u5230\u4e24\u4fa7\uff0c\u8ddd\u79bb\u57285-20cm\u5de6\u53f3\u6ce2\u52a8\uff0c\u50cf\u662fRTK\u5b9a\u4f4d\u7cbe\u5ea6\u95ee\u9898\uff0c\u8fd9\u91cc\u6ce8\u610f\u5f55\u5236\u6570\u636e\u524d\u4e00\u5b9a\u68c0\u67e5\u4e0bRTK\u662f\u5426\u5df2\u751f\u6548\uff08\/gps\/fix\u6570\u636e\u7684status.status\u4e3a0\u8868\u793a\u666e\u901agps\uff0c1\/2\u8868\u793a\u7ecf\u8fc7\u661f\/\u5730rtk\u4fee\u6b63\uff09\u3002<\/p>\n \u8fd9\u91cc\u5982\u679c\u76f8\u5bf9\u5173\u7cfb\u5f88\u7a33\u5b9a\u7684\u8bdd\uff0c\u4e5f\u53ef\u4ee5\u5728\u4e0a\u4e00\u6b65\u9aa4\u7684\u57fa\u7840\u4e0a\uff0c\u624b\u52a8\u5fae\u8c03\u5e73\u79fb\u5411\u91cfT\u6765\u89e3\u51b3\uff1a<\/p>\n \u6700\u7ec8\uff0c\u6211\u8fd8\u662f\u81ea\u5df1\u624b\u52a8\u6807\u4e86\u4e00\u4e2a\u77e9\u9635\uff0c\u7f16\u8bd1\u540e\uff0c\u521d\u59cb\u5316\u5b9a\u4f4d\u9700\u8981\u5148\u8ba9\u8f66\u8d70\u4e0a\u51e0\u7c73\uff0c\u4ed6\u5373\u53ef\u81ea\u5df1\u521d\u59cb\u5316\u5b9a\u4f4d\u6210\u529f\uff1b\u884c\u9a76\u9014\u4e2d\u6545\u610f\u624b\u52a8\u628a\u5f53\u524dPose\u4f4d\u7f6e\u6539\u79f0\u6210\u9519\u7684\uff0c\u57fa\u672c5s\u4e4b\u5185\u53ef\u4ee5\u6839\u636eGNSS\u5750\u6807\u81ea\u52a8\u4fee\u6b63\u56de\u6765\uff1a<\/p>\n *\u6ce8\uff1a\u9700\u89815s\u7684\u539f\u56e0\u662f\u6211\u7684GNSS+IMU\u822a\u5411\u89d2\u6570\u636e\u7279\u522b\u5dee\uff0c\u627e\u4e0d\u7740\u5317\uff0c\u53ea\u80fd\u6839\u636e\u5927\u698210-20\u5398\u7c73\u8bef\u5dee\u7684\u65b9\u4f4d\u8f9b\u82e6ndt\u53bbmatching\u4e86\u3002<\/p>\n \u5b9e\u9645\u4e0a\u8f66\u53d1\u73b0\uff0c\u8fd9\u4e2a\u65b9\u6cd5\u6709\u6539\u5584\u4f46\u662f\u4e0d\u591a\uff0c\u8d70\u5b8c\u5168\u7a0b\u7684\u5b9a\u4f4d\u7a33\u5b9a\u6027\u76f8\u8f83\u4e0d\u4f7f\u7528gnss\u66f4\u5dee\u4e86\u3002\u5176\u4ed6\u5730\u56fe\u6709\u5761\u5ea6\u6216\u9ad8\u5ea6\u5dee\u7684\u5730\u65b9\uff0c\u8fd9\u6837\u7684\u65b9\u6cd5\u4f9d\u7136\u6bd4\u8f83\u96bematch\u4e0a\u3002\u7ecf\u8fc7\u7814\u7a76\uff0c\u5728\u8fd9\u4e00\u6b65\u7684\u57fa\u7840\u4e0a\u518d\u505a\u4e00\u4e9b\u4fee\u6539\u5373\u53ef\u8fbe\u5230\u6bd4\u8f83\u597d\u7684\u6548\u679c\u3002<\/p>\n <\/p>\n \u4e0a\u8fb9\u7684\u65b9\u6cd5\uff0c\u5728pc\u4e0a\u4eff\u771f\u6548\u679c\u8fd8\u53ef\u4ee5\uff0c\u4f46\u662f\u5b9e\u9645\u4f7f\u7528\u65f6\uff0c\u5f53\u51fa\u73b0ndt\u5b9a\u4f4d\u4e0d\u6210\u529f\uff0c\u4f1a\u9891\u7e41\u7684ndt\u548cgnss\u7684\u5b9a\u4f4d\u6765\u56de\u5207\u65e0\u6cd5\u6210\u529f\u4fee\u6b63\uff0crviz\u753b\u9762\u95ea\u778e\u6211\u7684\u72d7\u773c\uff0c\u60f3\u624b\u52a8\u70b9\u4e0b\u90fd\u4e0d\u884c\uff0c\u7a81\u53d1\u5947\u60f3\uff0c\u4e3a\u5565\u5b50\u4e0d\u8ba9\u5b83\u81ea\u52a8\u6267\u884c\u4e00\u4e2a\u8fd9\u4e2a\u64cd\u4f5c\u5462\uff1f\u6765\u52a8\u624b\u8bd5\u4e00\u8bd5\uff1a<\/p>\n \u91cd\u65b0\u8bd5\u4e0b\uff0c\u4ece\u4e4b\u524dndt pose\u4e0egnss pose\u53cd\u590d\u62c9\u626f\u7684\u72b6\u6001\uff0c\u5230\u504f\u79fb\u540e\u5927\u6982\u7387\u53ef\u4ee5\u91cd\u5b9a\u4f4d\u6210\u529f\uff08gnss pose\u8ddd\u79bb\u771f\u5b9e\u4f4d\u7f6e\u504f\u5dee2\u7c73\u8303\u56f4\u5185\uff0c\u4e14\u5468\u56f4\u6709\u8db3\u591f\u7684\u7279\u5f81\u65f6\uff09\uff0c\u901f\u5ea6\u4e5f\u6bd4\u4e4b\u95f4\u5feb\u4e86\u5f88\u591a\u3002<\/p>\n \u6700\u7ec8\u7684\u8282\u70b9\u5173\u7cfb\u56fe\uff1a<\/p>\n \u4eca\u5929\u8fd8\u662f\u5f88\u6709\u6536\u83b7\u7684\uff0cGNSS\u5b9a\u4f4d\u7684\u4ecb\u5165\u5bf9NDT match\u7279\u5f81\u4e0d\u8db3\u65f6\u5b9a\u4f4d\u98d8\u95ee\u9898\u6709\u6539\u5584\uff0cGNSS\u4fee\u6b63\u5b9a\u4f4d\u7684\u4e3b\u8981\u5f71\u54cd\u56e0\u7d20\u5982\u4e0b\uff1a<\/p>\n \u4eca\u5929\u53ef\u4ee5\u5feb\u4e50\u7684\u7761\u89c9\u4e86\u3002<\/p>\n <\/p>\n yan 24.12.8 2:18<\/p>\n <\/p>\n \u53c2\u8003\uff1a<\/p>\n Autoware \u7b14\u8bb0 No. 5\u2014\u2014\u57fa\u4e8eGNSS\u7684\u5b9a\u4f4d<\/a><\/p>\n<\/a><\/p>\n\u4e09\u3001\u542f\u7528GNSS\u5b9a\u4f4d<\/h1>\n
1. \u4fee\u6539tix2tfpose\u914d\u7f6e<\/h4>\n
# \u4fee\u6539fix2tfpose\u7684gps topic\u4e3a\u5916\u90e8\u53c2\u6570\u4f20\u5165\r\nvim Autoware\/src\/autoware\/core_perception\/gnss_localizer\/nodes\/fix2tfpose\/fix2tfpose.cpp\r\n private_nh.getParam(\"gps_topic\", _gps_topic<\/span>);\r\n pose_publisher = nh.advertise<geometry_msgs::PoseStamped>(\"gnss_pose<\/span>\", 1000); \r\n stat_publisher = nh.advertise<std_msgs::Bool>(\"\/gnss_stat\", 1000);\r\n ros::Subscriber gnss_pose_subscriber = nh.subscribe(_gps_topic<\/span>, 100, GNSSCallback);\r\n\r\nvim Autoware\/src\/autoware\/core_perception\/gnss_localizer\/launch\/fix2tfpose.launch\r\n <arg name=\"plane\" default=\"7\"\/>\r\n <arg name=\"gps_topic\" default=\"gps\/fix<\/span>\"\/>\r\n <node pkg=\"gnss_localizer\" type=\"fix2tfpose\" name=\"fix2tfpose\" output=\"log\">\r\n <param name=\"plane\" value=\"$(arg plane)\"\/>\r\n <param name=\"gps_topic\" value=\"$(arg gps_topic)\"\/>\r\n <\/node>\r\n\r\n# \u7f16\u8bd1\r\ncd ~\/Autoware\r\ncolcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=gnss_localizer\r\n\r\n# \u8fd0\u884c\u6d4b\u8bd5\u786e\u8ba4\u8282\u70b9\u8ba2\u9605\u5173\u7cfb\u6b63\u5e38\r\nroslaunch gnss_localizer fix2tfpose.launch plane:=7 gps_topic:=\/gps\/fix\r\nrosrun rqt_graph rqt_graph\r\n\r\n# \u68c0\u67e5\/gnss_pose\u662f\u5426\u6b63\u5e38\u8f93\u51fa\r\nrostopic echo \/gnss_pose<\/code><\/pre>\n<\/a><\/p>\n2. \u9a8c\u8bc1GNSS\u5b9a\u4f4d\u662f\u5426\u751f\u6548<\/h4>\n
# \u542f\u52a8\u64ad\u5305\u3001gnss\u5b9a\u4f4d\r\nroslaunch autolaunch run.launch use_gnss:=1 rviz:=true map_name:=jingheyuan bag_file:=\/home\/autoware\/shared_dir\/bag\/map-8-jhy-gps\/map-8-jhy-gps-241203-2301.bag\r\n\r\n# \u67e5\u770b\/gnss_pose\u4e0e\/ndt_pose\u53d1\u5e03\u7684\u6570\u636e\u662f\u5426\u57fa\u672c\u4e00\u81f4\uff0c\u5982\u679c\u57fa\u672c\u4e00\u81f4\uff0c\u90a3\u4e48\u6210\u529f\uff08\u6211\u4eec\u8fd9\u91cc\u8fd8\u5bf9\u4e0d\u4e0a\uff0c\u522b\u7740\u6025)\r\nrostopic echo \/gnss_pose\r\nrostopic echo \/ndt_pose<\/code><\/pre>\n<\/a><\/p>\n\u56db\u3001GNSS\u5b9a\u4f4d\u4e0eNDT\u5b9a\u4f4d\u4f4d\u59ff\u7684\u65cb\u8f6c\u77e9\u9635<\/h1>\n
1. \u4fee\u6539 geo_pos_conv.cpp \u521d\u59cb\u5316\u5ea6\u5206\u4f4d\u7f6e<\/h4>\n
\n\u4fee\u6539 void geo_pos_conv::set_plane(int num)\u51fd\u6570\uff0c\u0300set_plane` \u51fd\u6570\u91cd\u8f7d\u91c7\u7528\u7eac\u5ea6 (`lat`) \u548c\u7ecf\u5ea6 (`lon`) \u503c\uff0c\u5e76\u76f8\u5e94\u5730\u8bbe\u7f6e\u6210\u5458\u53d8\u91cf `m_PLato` \u548c `m_PLo`\u3002\u6839\u636e\u6574\u6570 num \u7684\u503c\u8f93\u51fa\u4e0d\u540c\u5e73\u9762\u5bf9\u5e94\u7684\u9884\u5b9a\u4e49\u503c\u8bbe\u7f6e\u7eac\u5ea6\u548c\u7ecf\u5ea6\u3002\u6309\u7167\u4ee5\u4e0b\u65b9\u5f0f\u4fee\u6539 num=7 \u65f6\u7684\u4ee3\u7801\u5757\uff1a<\/p>\nvim Autoware\/src\/autoware\/common\/gnss\/src\/geo_pos_conv.cpp\r\n\r\nvoid geo_pos_conv::set_plane(int num) {\r\n ...\r\n else if (num == 7) {\r\n \/*lon_deg = 36;\r\n lon_min = 0;\r\n lat_deg = 137;\r\n lat_min = 10;*\/\r\n \/\/ \u8fd9\u91cc\u4fee\u6539\u4e3a\u4f60\u81ea\u5df1\u6240\u5728\u5730\u7684\u7ecf\u7eac\u5ea6\u5206\r\n lon_deg = 40<\/span>;\r\n lon_min = 2;\r\n lat_deg = 116;\r\n lat_min = 15; \r\n ...\r\n\r\n }\r\n\r\n# \u4fee\u6539\u5b8c\u9700\u8981\u5148\u7f16\u8bd1\u4ee5\u4e0b\uff0c\u7b49\u4e0b\u8ba1\u7b97\u65cb\u8f6c\u77e9\u9635\u8981\u7528\u5230 \r\ncd ~\/Autoware\r\ncolcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=gnss<\/code><\/pre>\n\n
\n
\n
2. \u8ba1\u7b97GNSS\u4e0e\u5730\u56fe\u7684\u65cb\u8f6c\u77e9\u9635<\/h4>\n
2.1 \u786e\u4fddndt matching\u5904\u4e8e\u5b9a\u4f4d\u6210\u529f\u72b6\u6001<\/h6>\n
# (\u4e34\u65f6)\u786e\u4fdd\u52a0\u8f7d\u7684\u662f\u5168\u5c40\u5730\u56fe\uff08\u5730\u56fe\u5927\u7684\u8bdd\u7b491-2\u5206\u949f\u624d\u80fd\u52a0\u8f7d\u5b8c\uff09\uff0c\u800c\u4e0d\u662f\u5206\u5272\u52a8\u6001\u52a0\u8f7d\u7684\uff08\u6ca1\u6709\u914d\u597d\u65cb\u8f6c\u77e9\u9635\u524d\uff0c\u52a8\u6001\u52a0\u8f7d\u4f4d\u7f6e\u662f\u9519\u7684\uff09\r\nvim ~\/Autoware\/src\/autolaunch\/launch\/run.launch\r\n <!-- algo -->\r\n <include file=\"$(find autolaunch)\/launch\/localization.launch\"> \r\n ...\r\n <arg name=\"points_map\" default=\"$(arg map_path)\/$(arg map_name)\/points-map\/GlobalMap.pcd<\/span>\" \/>\r\n <!-- <arg name=\"points_map\" default=\"$(arg map_path)\/$(arg map_name)\/points-map\/grid-area\/\" \/> -->\r\nvim ~\/Autoware\/src\/autolaunch\/launch\/localization.launch\r\n <node pkg=\"map_file\" type=\"points_map_loader\" name=\"points_map_loader\" args=\"noupdate $(arg points_map)\"\/>\r\n <!-- <node pkg=\"map_file\" type=\"points_map_loader\" name=\"points_map_loader\" >\r\n <rosparam subst_value=\"true\">\r\n area: $(arg points_area)\r\n arealist_path: $(arg points_area_list)\r\n pcd_paths: [ $(arg points_map) ]\r\n update_rate: 30000\r\n <\/rosparam>\r\n <\/node> -->\r\n\r\n# \u542f\u52a8\u64ad\u5305\u3001\u5b9a\u4f4d\u6a21\u5757\uff08\u6ce8\u610f\u6b64\u65f6\u9700\u8981\u5148\u5173\u95edGNSS\u5b9a\u4f4d\u529f\u80fd\uff09\r\nroslaunch autolaunch run.launch map_name:=jingheyuan rviz:=true use_gnss:=0<\/span> bag_file:=\/home\/autoware\/shared_dir\/bag\/map-8-jhy-gps\/map-8-jhy-gps-241203-2301.bag\r\n\r\n# \u68c0\u67e5use_gnss\u53c2\u6570\u72b6\u6001\r\nrosparam get \/ndt_matching\/use_gnss\r\n 0\r\n<\/span>\r\n# \u786e\u4fddrtk\u662f\u751f\u6548\u7684\uff08status >= 1\uff09\r\nrostopic echo \/gps\/fix<\/code><\/pre>\n<\/a><\/p>\n2.2 \u8ba1\u7b97\u65cb\u8f6c\u77e9\u9635<\/h6>\n
rosrun turn_on_wheeltec_robot gnss_matrix.py<\/code><\/pre>\n2.3. \u7ee7\u7eed\u64ad\u653ebag\uff0c\u8f93\u51fa\u65cb\u8f6c\u77e9\u9635<\/h6>\n
<\/a><\/p>\nr matrix:\r\n[[ 0.99708656 -0.07450855 -0.016336 ]\r\n [ 0.06207712 0.91707773 -0.39384626]\r\n [ 0.0443263 0.39168472 0.91903115]]\r\nt matrix:\r\n[ -729.95410472 -1585.00749103 -732.72186379]<\/code><\/pre>\n2.4 \u8bbe\u7f6e\u65cb\u8f6c\u77e9\u9635<\/h6>\n
vim Autoware\/src\/autoware\/common\/gnss\/src\/geo_pos_conv.cpp\r\n\r\n #if 1\r\n \/\/ debug gnss localtor\r\n Eigen::Matrix3f R;\r\n Eigen::RowVector3f T;\r\n Eigen::RowVector3f m_3;\r\n Eigen::RowVector3f m_4;\r\n \r\n \/\/ \u539f\u59cb\u7b1b\u5361\u5c14\u5750\u6807\uff08 m_y, m_x, m_z\uff09\u5206\u914d\u7ed9m_3\u5411\u91cf\uff0c\u8d4b\u503c\u987a\u5e8f\u4e3ay,x,z\r\n m_3 << m_y, m_x, m_z;\r\n\r\n \/*\r\n R << 1, 0, 0, \r\n 0, 1, 0,\r\n 0, 0,1;\r\n \r\n T << 0, 0, 0; \r\n *\/\r\n \/\/ \u65cb\u8f6c\u77e9\u9635R\u662f\u4e00\u4e2a3x3\u77e9\u9635\uff0c\u6bcf\u884c\u4ee3\u8868\u65cb\u8f6c\u540e\u53d8\u6362\u8f74\u7684\u65b9\u5411\r\n R << 0.99708656, -0.07450855, -0.016336,\r\n 0.06207712, 0.91707773, -0.39384626,\r\n 0.0443263, 0.39168472, 0.91903115;<\/span>\r\n \/\/ \u5e73\u79fb\u52a8\u5411\u91cfT\u8868\u793a\u65cb\u8f6c\u540e\u5e94\u7528\u4e8e\u5750\u6807\u7684\u4f4d\u79fb\r\n T << -729.95410472, -1585.00749103, -732.72186379;<\/span>\r\n\r\n \/\/ \u53d8\u6362\u540e\u7684\u5750\u6807m_4\u662f\u901a\u8fc7\u5c06\u539f\u59cb\u5750\u6807m_3\u4e58\u4ee5\u559c\u6b22\u8f6c\u77e9\u9635R.transpose()\u7684\u8f6c\u7f6e\uff0c\u7136\u540e\u52a0\u4e0a\u5e73\u79fb\u5411\u91cf\u5f97\u5230\u7684T\r\n m_4 = m_3 * R.transpose() + T;\r\n\r\n \/\/ \u5c06\u8f6c\u6362\u540e\u7684\u5750\u6807m_4\u5206\u914d\u56dem_x\uff0cm_y,m_z\u53d8\u91cf\uff0c\u8d4b\u503c\u987a\u5e8f\u4e3ay,x,z\r\n m_y = m_4[0];\r\n m_x = m_4[1];\r\n m_z = m_4[2];\r\n #endif<\/code><\/pre>\n2.5 \u7f16\u8bd1<\/h6>\n
cd ~\/Autoware\r\ncolcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=gnss<\/code><\/pre>\n3. NDT+GNSS\u8054\u5408\u5b9a\u4f4d\u9a8c\u8bc1<\/h4>\n
3.1 GNSS\u5b9a\u4f4d\u9a8c\u8bc1<\/h6>\n
# \u4e34\u65f6\u5173\u95edGNSS\u4fee\u6b63pose\u7684\u81ea\u52a8\u65cb\u8f6c\u7b56\u7565\r\nvim ~\/Autoware\/src\/autolaunch\/launch\/localization.launch\r\n <!-- lidar ndt matching -->\r\n <include file=\"$(find lidar_localizer)\/launch\/ndt_matching.launch\">\r\n ...\r\n <arg name=\"use_gnss_fix_pose\" default=\"false<\/span>\" \/> <!-- ndt matching\u5b9a\u4f4d\u8d28\u91cf\u592a\u5dee\u65f6\uff0c\u4f7f\u7528gnss\u4f4d\u7f6e\u8fdb\u884c\u91cd\u5b9a\u4f4d -->\r\n\r\n\r\n# \u542f\u52a8\u64ad\u5305\u3001gnss\u5b9a\u4f4d\r\nroslaunch autolaunch run.launch use_gnss:=1 rviz:=true map_name:=jingheyuan bag_file:=\/home\/autoware\/shared_dir\/bag\/map-8-jhy-gps\/map-8-jhy-gps-241203-2301.bag<\/code><\/pre>\n<\/a><\/p>\nR << 1, 0, 0,\r\n 0, 1, 0,\r\n 0, 0,1;\r\n \r\n T << 0, 0, 0;<\/code><\/pre>\nr matrix:\r\n[[ 0.99961893 0.01157622 -0.02505958]\r\n [-0.00929102 0.99594709 0.08945991]\r\n [ 0.02599362 -0.08919299 0.99567512]]\r\n\r\nt matrix:\r\n[ -877.05005961 -1675.08312445 91.9169504 ]\r\n<\/code><\/pre>\n \/\/ \u65cb\u8f6c\u77e9\u9635R\u662f\u4e00\u4e2a3x3\u77e9\u9635\uff0c\u6bcf\u884c\u4ee3\u8868\u65cb\u8f6c\u540e\u53d8\u6362\u8f74\u7684\u65b9\u5411\r\n R << 0.99961893, 0.01157622, -0.02505958,\r\n -0.00929102, 0.99594709, 0.08945991,\r\n 0.02599362, -0.08919299, 0.99567512;\r\n \/\/ \u5e73\u79fb\u52a8\u5411\u91cfT\u8868\u793a\u65cb\u8f6c\u540e\u5e94\u7528\u4e8e\u5750\u6807\u7684\u4f4d\u79fb\r\n T << -877.05005961, -1675.08312445, 91.9169504;<\/code><\/pre>\n<\/a><\/p>\n3.2 \u624b\u52a8\u4fee\u6b63\u77e9\u9635<\/h6>\n
\n
\n
\n
\n
<\/a><\/p>\n R << 1, 0, 0,\r\n 0, 1, 0,\r\n 0, 0,1;\r\n T << -858.55005961, -1687.08312445, -36.5;\r\n\r\ncd ~\/Autoware\r\n.\/build.sh gnss<\/code><\/pre>\n\u4e94\u3001\u6062\u590d\u4e34\u65f6\u914d\u7f6e<\/h1>\n
# \u6062\u590d\u4e3a\u52a0\u8f7d\u5206\u5272\u5730\u56fe\r\nvim ~\/Autoware\/src\/autolaunch\/launch\/run.launch\r\n <!-- algo -->\r\n <include file=\"$(find autolaunch)\/launch\/localization.launch\"> \r\n ...\r\n <!-- <arg name=\"points_map\" default=\"$(arg map_path)\/$(arg map_name)\/points-map\/GlobalMap.pcd\" \/> -->\r\n <arg name=\"points_map\" default=\"$(arg map_path)\/$(arg map_name)\/points-map\/grid-area\/<\/span>\" \/>\r\nvim ~\/Autoware\/src\/autolaunch\/launch\/localization.launch\r\n <!-- <node pkg=\"map_file\" type=\"points_map_loader\" name=\"points_map_loader\" args=\"noupdate $(arg points_map)\"\/> -->\r\n <node pkg=\"map_file\" type=\"points_map_loader\" name=\"points_map_loader\" >\r\n <rosparam subst_value=\"true\">\r\n area<\/span>: $(arg points_area)\r\n arealist_path<\/span>: $(arg points_area_list)\r\n pcd_paths: [ $(arg points_map) ]\r\n update_rate: 30000\r\n <\/rosparam>\r\n <\/node>\r\n\r\n# \u6062\u590dGNSS\u4fee\u6b63pose\u7684\u81ea\u52a8\u65cb\u8f6c\u7b56\u7565\r\nvim ~\/Autoware\/src\/autolaunch\/launch\/localization.launch\r\n <!-- lidar ndt matching -->\r\n <include file=\"$(find lidar_localizer)\/launch\/ndt_matching.launch\">\r\n ...\r\n <arg name=\"use_gnss_fix_pose\" default=\"true<\/span>\" \/> <!-- ndt matching\u5b9a\u4f4d\u8d28\u91cf\u592a\u5dee\u65f6\uff0c\u4f7f\u7528gnss\u4f4d\u7f6e\u8fdb\u884c\u91cd\u5b9a\u4f4d -->\r\n<\/code><\/pre>\n\u516d\u3001\u6539\u5584GNSS\u5b9a\u4f4d\u4fee\u6b63\u65b9\u6cd5<\/h1>\n
vim Autoware\/src\/autoware\/core_perception\/lidar_localizer\/nodes\/ndt_matching\/ndt_matching.cpp\r\n\r\n static double _gnss_reinit_fitness = 1.0<\/span>; \r\n static void gnss_callback(const geometry_msgs::PoseStamped::ConstPtr& input)\r\n {\r\n ...\r\n \/\/ \u542f\u7528GNSS\u4f46\u5c1a\u672a\u521d\u59cb\u5316\u4f4d\u59ff\uff0c\u6216ndt\u5b9a\u4f4d\u8d28\u91cf\u592a\u5dee\uff08\/ndt_stat.score>500\uff0c\u5206\u6570\u8d8a\u5927matching\u8d28\u91cf\u8d8a\u5dee\uff09\u65f6\r\n if ((_use_gnss == 1 && init_pos_set == 0) || fitness_score >= _gnss_reinit_fitness)\r\n {\r\n \/\/ \u76f4\u63a5\u4f7f\u7528gnss\u5f53\u524d\u4f4d\u59ff\u6001\u91cd\u7f6e\u521d\u59cb\u4f4d\u59ff\uff0c\u4ee5\u4fee\u6b63\u5b9a\u4f4d\u98d8\u95ee\u9898\uff08\u53d1\u9001topic\u800c\u4e0d\u662f\u76f4\u63a5\u8c03\u7528initialpose_callback\u7684\u539f\u56e0\u662f\u8981\u540c\u6b65\u901a\u77e5\u7ed9op_global_planner\uff09\r\n geometry_msgs::PoseWithCovarianceStamped initialpose_msg;\r\n initialpose_msg.header = input->header;\r\n initialpose_msg.pose.pose = input->pose;\r\n initialpose_msg.pose.covariance[0] = 0.25;\r\n initialpose_msg.pose.covariance[6 * 1 + 1] = 0.25;\r\n initialpose_msg.pose.covariance[6 * 5 + 5] = 0.06853892326654787;\r\n initialpose_pub.publish(initialpose_msg);<\/span>\r\n init_pos_set = 1;\r\n \/\/ !!!\u6ce8\u91ca\u6389\u8fd9\u4e2aif\u91cc\u7684\u5176\u4ed6\u4ee3\u7801\uff0c\u589e\u52a0\u4e0a\u8fb9\u8fd9\u90e8\u5206\uff0c\u5982\u679c\u533a\u5206\u521d\u59cb\u5316\/\u884c\u9a76\u8fc7\u7a0b\u603b\u7684\u573a\u666f\u533a\u522b\u5904\u7406\u4e0b\u6548\u679c\u4f1a\u66f4\u597d\r\n }\r\n ...\r\n int main(){\r\n private_nh.param<double>(\"gnss_reinit_fitness\", _gnss_reinit_fitness, 1.0<\/span>);\r\n }\r\n }\r\n\r\nvim Autoware\/src\/autolaunch\/launch\/localization.launch\r\n <include file=\"$(find lidar_localizer)\/launch\/ndt_matching.launch\">\r\n ...\r\n <arg name=\"gnss_reinit_fitness\" default=\"1.0<\/span>\" \/> <!-- ndt_matching\u5206\u6570\u5927\u4e8e1.0\u65f6\uff0c\u57fa\u672c\u5c31\u5df2\u7ecf\u98d8\u4e86\uff0c\u8fd9\u65f6\u5e94\u8be5\u7528gnss_pose\u91cd\u7f6e\u4e00\u4e0b -->\r\n <\/include>\r\n# \u7f16\u8bd1\r\ncolcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=lidar_localizer\r\n\r\n# \u542f\u52a8\u64ad\u5305\u3001ndt+gnss\u5b9a\u4f4d\r\nroslaunch autolaunch run.launch use_gnss:=1 rviz:=true map_name:=jingheyuan bag_file:=\/home\/autoware\/shared_dir\/bag\/map-8-jhy-gps\/map-8-jhy-gps-241203-2301.bag\r\n\r\n# \u67e5\u770bndt+gnss pose\u3001\u5b9a\u4f4d\u8d28\u91cf\r\nrostopic echo \/ndt_pose\r\nrostopic echo \/gnss_pose\r\nrostopic echo \/ndt_stat # \u770bscore\uff0c>1.0\u57fa\u672c\u90fd\u662f\u5e73\u98d8\u4e86<\/code><\/pre>\n<\/a><\/p>\n\u4e03\u3001\u603b\u7ed3<\/h1>\n
\n