《自动驾驶编程原理与应用》课件 第5章车辆定位

1主讲：黄炜自动驾驶编程原理与应用2第五章车辆定位知识点：了解主流车辆定位方法，以及我国北斗卫星导航系统发展历程和定位原理；掌握自动驾驶系统车辆定位功能包相关功能、实现原理和使用方法。重点：重点掌握激光点云定位原理，以及Autoware车辆定位模块中ROS节点ndt_matching和nmea2tfpose等。难点：车辆定位功能模块与示例节点源码解析。自动驾驶编程原理与应用3引言车辆定位主要用于为自动驾驶决策规划提供车辆自身的位置和姿态（简称位姿）等信息。准确可靠的车辆位姿信息是实现自动驾驶功能的重要前提和基础。4引言车辆定位信号定位（Signal-basedPositioning）航迹推算（DeadReckoning）地图匹配（MapMatching）5引言信号定位和地图匹配的定位原理主要是利用信号的空间交汇测量及环境特征匹配进行直接定位。航迹推算则是依据加速度、角速度等信息结合初始值进行积分定位。由于不同定位手段有其各自独特的优势和局限，利用多种组合定位技术实现融合定位可以最大程度上发挥多传感器协同感知优势，不仅增加了系统冗余而且提升了定位的准确性和可靠性。本章节将着重介绍如何基于地图匹配中的激光点云定位技术以及信号定位中的卫星辅助定位技术获取车辆的位姿信息，并解析在编程实现功能过程中涉及的核心算法原理与关键功能包源码。6本章节目录5.1定位方法概述常见定位方法简介Autoware定位基本步骤5.2激光点云定位功能包激光点云定位方法节点ndt_matching与源码解析5.3卫星辅助定位功能包卫星辅助定位方法节点nmea2tfpose与源码解析75.1定位方法概述85.1定位方法概述常见定位方法简介基于信号定位的车辆定位方法通常指通过全球导航卫星系统（GlobalNavigationSatelliteSystem,GNSS）的卫星信号以及无线网络（Wi-Fi）、超带宽（Ultra-WideBand,UWB）等信号方式获取信息对车辆进行定位。信号定位95.1定位方法概述常见定位方法简介基于GNSS定位技术现已被广泛应用于车辆导航，其系统由空间段、地面段和用户段三部分组成，可在全球范围内提供全天候、全天时的导航服务。信号定位105.1定位方法概述常见定位方法简介全球卫星导航系统国际委员会公布的全球卫星导航系统供应商有美国的全球定位系统（GPS）、俄罗斯的格洛纳斯卫星导航系统（GLONASS）、欧盟的伽利略卫星导航系统（GALILEO）以及中国的北斗卫星导航系统（BDS）。GPSGLONASSGALILEOBDS信号定位115.1定位方法概述常见定位方法简介北斗卫星定位（BDS）是我国完全自主建设、独立运行的的全球卫星导航系统。北斗卫星定位系统空间段由35颗卫星组成，其中地球静止轨道卫星5颗、中地轨道卫星27颗、倾斜同步轨道卫星3颗。信号定位125.1定位方法概述常见定位方法简介北斗卫星导航系统具有以下特点：高轨卫星更多，抗遮挡能力强；提供多个频点的导航信号，具备通过多频信号组合使用提高服务精度的条件；深度融合导航与通信，可在全球范围内全天候、全天时为各类用户提供导航、定位、授时、位置报告和短报文通信服务。信号定位135.1定位方法概述常见定位方法简介然而实际使用GNSS过程中卫星信号易受对流层、电离层折射和多路径效应等与传播途径有关因素的影响，同时还存在卫星星历误差、时钟误差等与卫星、接收机有关的系统误差，最终导致GNSS在城市环境中提供的定位性能普遍都在米级水平，难以满足车辆定位对于精度的要求。信号定位145.1定位方法概述常见定位方法简介为了提升车辆定位精度，通常需要结合实时动态载波相位差分技术（Real-TimeKinematic,RTK）对GNSS定位结果进行修正。RTK工作时至少需要两台GNSS接收机，其中一台安装在基准站，另一台安装在基准站附近的移动站（即安装在车辆上）。信号定位155.1定位方法概述常见定位方法简介基准站接收采集卫星数据，并通过数据传输链路将获得的载波相位观测值及站坐标实时传输给在其周围工作的移动站。移动站通过对自身采集的卫星数据和接收来自基准站的数据链进行实时载波相位差分处理，确定移动站相对于基准站的位置，最后根据基准站的坐标得到自身的瞬时绝对位置。在计算得到差分改正数后，可以通过卫星导航增强系统提高导航精度和完好性。卫星导航增强系统通常可以分为地基增强系统和星基增强系统。信号定位165.1定位方法概述常见定位方法简介连续运行参考站系统（ContinuouslyOperatingReferenceStations,CORS）是一种广泛使用的地基增强手段，其将测得的信息通过数据播发系统以及多基准站网络RTK技术建立的连续运行卫星定位服务综合系统进行传输，使用户可以通过网络与基准站联系，获得从数据处理中心计算得到的修正数据。地基增强系统的优点是能够实现实时、动态、厘米级的定位精度，缺点是需要依赖通信链路来播发差分误差信息，覆盖范围有一定限制，无法覆盖定位目标在通信信号范围之外的偏远地区、海洋等区域。信号定位175.1定位方法概述常见定位方法简介星基增强系统一般通过地球静止轨道卫星搭载卫星导航增强信号转发器，向用户播发对流层延迟误差、电离层延迟误差、星历误差、时钟误差等多种修正信息，不仅实现了对于GNSS定位精度的改进，而且可以有效覆盖偏远地区、海洋等区域。星基增强系统通常需要大量分布极广的地面参考站（基准站）对导航卫星进行监测，将获得的原始定位数据并送至主控站进行处理，计算得到卫星的定位修正信息（差分参数和完好性参数），通过上行注入站发给地球静止轨道卫星，最后由地球静止轨道卫星向GNSS接收机播发修正信息，从而获得高精度、广域覆盖的导航定位服务。信号定位185.1定位方法概述常见定位方法简介中高轨卫星星座几何构型变化慢，相邻历元间观测方程之间的相关性太强，因此精密单点定位收敛至厘米级精度所需的时间较长。低轨卫星相对地面运动速度快，移动站接收机接收到低轨导航增强信号后，与现有的卫星导航信号联合定位，可缩短精密单点定位的收敛时间。信号定位195.1定位方法概述常见定位方法简介为了提升我国自主导航系统在境外的导航定位精度、减轻对境外地面系统的依赖，我国科学家李德仁院士提出利用低轨卫星上搭载的GNSS接收机连续观测记录，结合激光测距等手段和现有地基增强系统，提高了北斗卫星导航系统在全球范围的实时定位精度。信号定位20常见定位方法简介除了信号定位，航迹推算也是自动驾驶车辆常用的定位方法之一。基于航迹推算定位原理，惯性导航系统（InertialNavigationSystem,INS）可以通过低成本、高刷新率的惯性测量单元（InertialMeasurementUnit,IMU）获取的车辆加速度和角速度信息，以及上一时刻的位置和姿态信息，推算出当前的位置和姿态。5.1定位方法概述航迹推算21常见定位方法简介优点：INS在工作过程中，既不向外部发送信号，也不接收来自外部的信号，是一种完全自主的导航系统，具有很强的自主性、隐蔽性、保密性和灵活性。缺点：虽然INS在短时间内能够很好地解决GNSS周期跳变和信号丢失后整周模糊度参数的重新计算问题，但是基于航迹推算的定位方法会随着时间推移不断累积定位误差，所以INS不能长时间单独工作，必须对其连续校准以保证定位精度。5.1定位方法概述航迹推算22常见定位方法简介GNSS和INS二者之间具有很强的互补性。在短时间内INS的定位误差比GNSS小，并且INS能在GNSS无法准确定位的情况下（比如在地下停车场、隧道、高层密集建筑、浓密树荫等无法接收到卫星信号、可接收卫星信号数量较少或无线通信网络无法覆盖的场景）短时间内提供高精度的定位。而当长时间使用时，须通过GNSS离散测量值对INS误差进行估计和校正，通过抓取系统漂移量，达到快速估计状态参数与收敛的目的。基于上述良好的互补优势，使用GNSS-INS组合定位导航系统可以有效提高整体导航性能和定位精度。5.1定位方法概述航迹推算23常见定位方法简介目前GNSS-INS组合定位导航系统尚且只能实现大约85%的场景覆盖。基于地图匹配的定位方法通过车载视觉传感器和激光雷达传感器获取道路环境的图像与激光点云数据，对地标观测值与地标在高精度地图上的位置进行特征匹配，再转换到高精地图坐标系上，实现精确的车辆定位。5.1定位方法概述地图匹配24常见定位方法简介在GNSS-INS组合定位导航的基础上实现多源异构传感器融合定位。5.1定位方法概述地图匹配25Autoware定位基本步骤以运行官方示例数据为例，在终端输入以下命令开始车辆定位：$cdautoware.ai$sourceinstall/setup.bash$roslaunchautoware_quickstart_examplesmy_localization.launch5.1定位方法概述26Autoware定位基本步骤为了查看启动文件my_localization.launch的文本内容，在终端输入以下命令，在官方示例autoware_quickstart_examples路径下找到工作目录“./launch/rosbag_demo”，然后在rosbag_demo文件夹中打开启动文件my_localization.launch：$cdautoware.ai/src/autoware/documentation/autoware_quickstart_examples$cdlaunch/rosbag_demo$geditmy_localization.launch5.1定位方法概述<launch><!--settingpathparameter--><argname="get_height"value="true"/><!--Setup--><includefile="$(findruntime_manager)/launch_files/setup_tf.launch"><argname="x"value="1.2"/><argname="y"value="0.0"/><argname="z"value="2.0"/><argname="yaw"value="0.0"/><argname="pitch"value="0.0"/><argname="roll"value="0.0"/><argname="frame_id"value="/base_link"/><argname="child_frame_id"value="/velodyne"/><argname="period_in_ms"value="10"/></include><includefile="$(findvehicle_description)/launch/vehicle_model.launch"/><!--pointsdownsampler--><includefile="$(findpoints_downsampler)/launch/points_downsample.launch"/><!--nmea2tfpose--><includefile="$(findgnss_localizer)/launch/nmea2tfpose.launch"/><!--ndt_matching--><includefile="$(findlidar_localizer)/launch/ndt_matching.launch"><argname="get_height"value="$(argget_height)"/></include></launch>27Autoware定位基本步骤启动文件主要通过导入“nmea2tfpose.launch”、“ndt_matching.launch”等多个启动文件到当前文件实现车辆定位过程中所需卫星定位和点云配准定位等相关功能的调用。my_localization.launch5.1定位方法概述285.2激光点云定位功能包29激光点云定位方法点云匹配算法不仅可用于地图构建，还可用于车辆定位。本章将以NDT点云匹配算法为例，介绍如何通过点云匹配算法匹配激光雷达实时扫描的激光点云数据与先验地图点云数据的方式，实现基于地图匹配的车辆定位。本章将不再赘述NDT点云匹配算法原理。需要注意的是，点云地图构建如果是以离线的方式进行，其对于点云匹配算法的实时性要求相对不高；而车辆定位通常是以在线的方式进行，因此其对于点云匹配算法的实时性有较高的要求。实际在使用点云匹配算法时，可根据点云地图构建和车辆定位各自的特点，选择相应合适的点云匹配算法。5.2激光点云定位功能包30节点ndt_matching与源码解析5.2激光点云定位功能包<launch><!--settingpathparameter--><argname="get_height"value="true"/><!--Setup--><includefile="$(findruntime_manager)/launch_files/setup_tf.launch"><argname="x"value="1.2"/><argname="y"value="0.0"/><argname="z"value="2.0"/><argname="yaw"value="0.0"/><argname="pitch"value="0.0"/><argname="roll"value="0.0"/><argname="frame_id"value="/base_link"/><argname="child_frame_id"value="/velodyne"/><argname="period_in_ms"value="10"/></include><includefile="$(findvehicle_description)/launch/vehicle_model.launch"/><!--pointsdownsampler--><includefile="$(findpoints_downsampler)/launch/points_downsample.launch"/><!--nmea2tfpose--><includefile="$(findgnss_localizer)/launch/nmea2tfpose.launch"/><!--ndt_matching--><includefile="$(findlidar_localizer)/launch/ndt_matching.launch"><argname="get_height"value="$(argget_height)"/></include></launch>my_localization.launch启动文件my_localization.launch调用了功能包“~/autoware.ai/src/autoware/core_perception/lidar_localizer”目录下的启动文件“~/launch/ndt_matching.launch”，用于启动点云配准定位过程中所需的多个节点。<!----><launch><argname="method_type"default="0"/><!--pcl_generic=0,pcl_anh=1,pcl_anh_gpu=2,pcl_openmp=3--><argname="use_gnss"default="1"/><argname="use_odom"default="false"/><argname="use_imu"default="false"/><argname="imu_upside_down"default="false"/><argname="imu_topic"default="/imu_raw"/><argname="queue_size"default="1"/><argname="offset"default="linear"/><argname="get_height"default="false"/><argname="use_local_transform"default="false"/><argname="sync"default="false"/><argname="output_log_data"default="false"/><nodepkg="lidar_localizer"type="ndt_matching"name="ndt_matching"output="screen"><paramname="method_type"value="$(argmethod_type)"/><paramname="use_gnss"value="$(arguse_gnss)"/><paramname="use_odom"value="$(arguse_odom)"/><paramname="use_imu"value="$(arguse_imu)"/><paramname="imu_upside_down"value="$(argimu_upside_down)"/><paramname="imu_topic"value="$(argimu_topic)"/><paramname="queue_size"value="$(argqueue_size)"/><paramname="offset"value="$(argoffset)"/><paramname="get_height"value="$(argget_height)"/><paramname="use_local_transform"value="$(arguse_local_transform)"/><paramname="output_log_data"value="$(argoutput_log_data)"/><remapfrom="/points_raw"to="/sync_drivers/points_raw"if="$(argsync)"/></node></launch>31运行启动文件“ndt_matching.launch”会启动节点ndt_matching。其中，节点ndt_matching用于激光点云配准定位。ndt_matching.launch节点ndt_matching与源码解析5.2激光点云定位功能包……add_executable(ndt_matchingnodes/ndt_matching/ndt_matching.cpp)target_link_libraries(ndt_matching${catkin_LIBRARIES})add_dependencies(ndt_matching${catkin_EXPORTED_TARGETS})……32节点ndt_matching与源码解析双击打开功能包lidar_localizer目录下的CMakeLists.txt文件。节点ndt_matching的源文件是功能包lidar_localizer目录下的“nodes/ndt_matching/ndt_matching.cpp”。CMakeLists.txt5.2激光点云定位功能包33节点ndt_matching与源码解析（1）主函数流程解析在VSCode软件中打开文件夹autoware.ai下的代码空间src，然后在侧边栏中依次点击文件夹autoware下的子文件夹core_perception找到功能包lidar_localizer，选择节点文件夹nodes中的节点ndt_matching，最后双击打开源文件ndt_matching.cpp。源文件ndt_matching.cpp中主函数的流程如图所示。5.2激光点云定位功能包……intmain(intargc,char**argv){ros::init(argc,argv,"ndt_matching");pthread_mutex_init(&mutex,NULL);……//Updatedininitialpose_callbackorgnss_callbackinitial_pose.x=0.0;initial_pose.y=0.0;initial_pose.z=0.0;initial_pose.roll=0.0;initial_pose.pitch=0.0;initial_pose.yaw=0.0;……}34节点ndt_matching与源码解析（1）主函数流程解析ndt_matching.cpp文本中主函数的初始化部分初始化5.2激光点云定位功能包主函数通过ros::init()函数对节点ndt_matching进行初始化，解析并使用节点启动时传入的参数，然后采用pthread_mutex_init()函数初始化互斥锁（参数为NULL，表示使用默认的互斥锁属性，默认属性为快速互斥锁），再对位姿initial_pose进行初始化设置。35节点ndt_matching与源码解析（1）主函数流程解析上述提到的互斥锁是Linux下的一种多线程编程同步机制，当某个线程获取到互斥锁后，其他线程需要等待该线程释放锁才能继续访问共享资源，从而避免了多个线程同时对共享资源进行写操作导致的数据不一致问题。初始化5.2激光点云定位功能包36节点ndt_matching与源码解析（1）主函数流程解析参数获取……intmain(intargc,char**argv){……//Getingparametersintmethod_type_tmp=0;private_nh.getParam("method_type",method_type_tmp);_method_type=static_cast<MethodType>(method_type_tmp);private_nh.getParam("use_gnss",_use_gnss);private_nh.getParam("queue_size",_queue_size);private_nh.getParam("offset",_offset);private_nh.getParam("get_height",_get_height);private_nh.getParam("use_local_transform",_use_local_transform);private_nh.getParam("use_imu",_use_imu);private_nh.getParam("use_odom",_use_odom);private_nh.getParam("imu_upside_down",_imu_upside_down);private_nh.getParam("imu_topic",_imu_topic);……}ndt_matching.cpp文本中主函数的参数获取部分5.2激光点云定位功能包通过私有句柄调用getParam函数从参数服务器上获取method_type_tmp、_use_gnss、_queue_size、_offset、_get_height、_use_local_transform、_use_imu、_use_odom、_imu_upside_down、_imu_topic等参数（与启动文件ndt_matching.launch中所定义的参数一一对应），然后在此基础上检查各参数的配置情况。如果上述参数未在启动文件中进行配置，则自动设置为默认值。37节点ndt_matching与源码解析（1）主函数流程解析车体坐标系与激光雷达坐标系之间的相对位置关系tf_btol可以通过设置平移向量（tl_btol）和旋转向量（rot_x_btol，rot_y_btol，rot_z_btol）计算变换矩阵的方式来表示。坐标变换5.2激光点云定位功能包……intmain(intargc,char**argv){……Eigen::Translation3ftl_btol(_tf_x,_tf_y,_tf_z);//tl:translationEigen::AngleAxisfrot_x_btol(_tf_roll,Eigen::Vector3f::UnitX());//rot:rotationEigen::AngleAxisfrot_y_btol(_tf_pitch,Eigen::Vector3f::UnitY());Eigen::AngleAxisfrot_z_btol(_tf_yaw,Eigen::Vector3f::UnitZ());tf_btol=(tl_btol*rot_z_btol*rot_y_btol*rot_x_btol).matrix();……}ndt_matching.cpp文本中主函数的变换矩阵计算部分……intmain(intargc,char**argv){……predict_pose_pub=nh.advertise<geometry_msgs::PoseStamped>("/predict_pose",10);predict_pose_imu_pub=nh.advertise<geometry_msgs::PoseStamped>("/predict_pose_imu",10);predict_pose_odom_pub=nh.advertise<geometry_msgs::PoseStamped>("/predict_pose_odom",10);predict_pose_imu_odom_pub=nh.advertise<geometry_msgs::PoseStamped>("/predict_pose_imu_odom",10);ndt_pose_pub=nh.advertise<geometry_msgs::PoseStamped>("/ndt_pose",10);……ros::Subscriberparam_sub=nh.subscribe("config/ndt",10,param_callback);ros::Subscribergnss_sub=nh.subscribe("gnss_pose",10,gnss_callback);ros::Subscribermap_sub=nh.subscribe("points_map",1,map_callback);ros::Subscriberinitialpose_sub=nh.subscribe("initialpose",10,initialpose_callback);ros::Subscriberpoints_sub=nh.subscribe("filtered_points",_queue_size,points_callback);……pthread_tthread;pthread_create(&thread,NULL,thread_func,NULL);ros::spin();return0;}38节点ndt_matching与源码解析（1）主函数流程解析ndt_matching.cpp文本中主函数的话题发布和订阅部分发布和订阅5.2激光点云定位功能包通过查找源文件ndt_matching.cpp中主函数发布和订阅的话题，可以快速确认节点ndt_matching的输出和输入信息。39节点ndt_matching与源码解析（1）主函数流程解析基于节点ndt_matching的点云匹配方法的输入信息主要来源于先验信息中的点云地图数据（话题“points_map”）和初始位姿数据（话题“initialpose”），以及基于传感器信息中GNSS测量的车体位姿数据（话题“gnss_pose”）、过滤处理后的激光雷达点云数据（话题“filtered_points”）、ODOM测量数据（话题“/vehicle/odom”）和IMU测量数据（话题“_imu_topic”）等，然后基于NDT点云匹配算法计算出旋转和平移矩阵，最后输出NDT点云配准得到车体相对点云地图的位姿（话题“/ndt_pose”）等信息。发布和订阅5.2激光点云定位功能包40节点ndt_matching与源码解析（1）主函数流程解析订阅话题时，主函数会通过回调函数（如param_callback函数、gnss_callback函数、map_callback函数、points_callback函数等）处理接收到的话题信息。发布和订阅5.2激光点云定位功能包41节点ndt_matching与源码解析（1）主函数流程解析5.2激光点云定位功能包……void*thread_func(void*args){ros::NodeHandlenh_map;ros::CallbackQueuemap_callback_queue;nh_map.setCallbackQueue(&map_callback_queue);ros::Subscribermap_sub=nh_map.subscribe("points_map",10,map_callback);ros::Rateros_rate(10);while(nh_map.ok()){map_callback_queue.callAvailable(ros::WallDuration());ros_rate.sleep();}returnnullptr;}……ndt_matching.cpp文本中thread_func函数发布和订阅主函数使用函数pthread_create()创建新线程，新线程从thread_func函数开始运行时订阅话题“points_map”，其中thread_func函数主要用于开启一个线程检测并更新地图。思考：为什么需要创建新线程？42节点ndt_matching与源码解析（2）关键子函数解析param_callback函数主要用于NDT配准算法的参数设置，具体通过autoware_config_msgs::ConfigNDT.msg文件进行设置。……intmain(intargc,char**argv){……ros::Subscriberparam_sub=nh.subscribe("config/ndt",10,param_callback);ros::Subscribergnss_sub=nh.subscribe("gnss_pose",10,gnss_callback);ros::Subscribermap_sub=nh.subscribe("points_map",1,map_callback);ros::Subscriberinitialpose_sub=nh.subscribe("initialpose",10,initialpose_callback);ros::Subscriberpoints_sub=nh.subscribe("filtered_points",_queue_size,points_callback);……}ndt_mapping.cpp文本中主函数的话题发布和订阅部分param_callback函数5.2激光点云定位功能包43节点ndt_matching与源码解析（2）关键子函数解析Headerheaderint32init_pos_gnssfloat32xfloat32yfloat32zfloat32rollfloat32pitchfloat32yaw#int32lidar_original#int32max#int32min#int32layerint32use_predict_posefloat32error_thresholdfloat32resolutionfloat32step_sizefloat32trans_epsilonint32max_iterations#float32leaf_size#float32angle_error#float32shift_x#float32shift_y#float32shift_zautoware_config_msgs::ConfigNDT.msg文本param_callback函数5.2激光点云定位功能包具体方式：根据主函数中私有句柄调用getParam函数从参数服务器上获取的参数_use_gnss，在param_callback函数中判断是否使用GNSS进行位姿估计。如果使用GNSS进行位姿估计（当参数_use_gnss的值不为0时表示使用GNSS进行位姿估计），则利用GNSS中的位姿信息对车体位姿进行初始化；如果未使用GNSS进行位姿估计，则直接调用NDT配置参数消息autoware_config_msgs::ConfigNDT.msg传入的初始位姿参数对车体位姿进行初始化。44节点ndt_matching与源码解析（2）关键子函数解析通过标志位init_pos_set赋值表示是否已经完成车体位姿初始化。param_callback函数……staticvoidparam_callback(constautoware_config_msgs::ConfigNDT::ConstPtr&input){if(_use_gnss!=input->init_pos_gnss){init_pos_set=0;}elseif(_use_gnss==0&&(initial_pose.x!=input->x||initial_pose.y!=input->y||initial_pose.z!=input->z||initial_pose.roll!=input->roll||initial_pose.pitch!=input->pitch||initial_pose.yaw!=input->yaw)){init_pos_set=0;}_use_gnss=input->init_pos_gnss;……}ndt_mapping.cpp文本中param_callback函数Part1当判断语句_use_gnss!=input->init_pos_gnss为真时，表示利用参数服务器获取的参数_use_gnss与由NDT配置参数消息autoware_config_msgs::ConfigNDT.msg传入的参数init_pos_gnss二者的值不相等，此时将标志位init_pos_set的值设置为0表示未进行位姿初始化。5.2激光点云定位功能包45节点ndt_matching与源码解析（2）关键子函数解析param_callback函数……staticvoidparam_callback(constautoware_config_msgs::ConfigNDT::ConstPtr&input){if(_use_gnss!=input->init_pos_gnss){init_pos_set=0;}elseif(_use_gnss==0&&(initial_pose.x!=input->x||initial_pose.y!=input->y||initial_pose.z!=input->z||initial_pose.roll!=input->roll||initial_pose.pitch!=input->pitch||initial_pose.yaw!=input->yaw)){init_pos_set=0;}_use_gnss=input->init_pos_gnss;……}ndt_mapping.cpp文本中param_callback函数Part1当判断语句为真时，表示在不使用GNSS（_use_gnss的值为0）的前提下，当由NDT配置参数消息autoware_config_msgs::ConfigNDT.msg传入的初始位姿参数（input->x，input->y，input->z，input->roll，input->pitch，input->yaw）与主函数初始化部分设置的initial_pose不相匹配时，标志位init_pos_gnss的值也将被设置为0以表示需要进行位姿初始化。5.2激光点云定位功能包46节点ndt_matching与源码解析（2）关键子函数解析param_callback函数……staticvoidparam_callback(constautoware_config_msgs::ConfigNDT::ConstPtr&input){if(_use_gnss!=input->init_pos_gnss){init_pos_set=0;}elseif(_use_gnss==0&&(initial_pose.x!=input->x||initial_pose.y!=input->y||initial_pose.z!=input->z||initial_pose.roll!=input->roll||initial_pose.pitch!=input->pitch||initial_pose.yaw!=input->yaw)){init_pos_set=0;}_use_gnss=input->init_pos_gnss;……}ndt_mapping.cpp文本中param_callback函数Part1完成对标志位init_pos_gnss赋值后，将由NDT配置参数消息autoware_config_msgs::ConfigNDT.msg传入的参数init_pos_gnss赋值给_use_gnss实现同步。5.2激光点云定位功能包47节点ndt_matching与源码解析（2）关键子函数解析param_callback函数……staticvoidparam_callback(constautoware_config_msgs::ConfigNDT::ConstPtr&input){……//Settingparametersif(input->resolution!=ndt_res){ndt_res=input->resolution;if(_method_type==MethodType::PCL_GENERIC)ndt.setResolution(ndt_res);……}if(input->step_size!=step_size){step_size=input->step_size;if(_method_type==MethodType::PCL_GENERIC)ndt.setStepSize(step_size);……}……ndt_mapping.cpp文本中param_callback函数Part2param_callback函数通过调用NDT配置参数消息autoware_config_msgs::ConfigNDT.msg传入参数的方式对NDT配准算法参数分辨率（ndt_res）、步长（step_size）进行设置。5.2激光点云定位功能包NDT配准算法参数具体需要根据计算机设备情况选择方法类型_method_type进行设置。48节点ndt_matching与源码解析（2）关键子函数解析param_callback函数……staticvoidparam_callback(constautoware_config_msgs::ConfigNDT::ConstPtr&input){……if(input->trans_epsilon!=trans_eps){trans_eps=input->trans_epsilon;if(_method_type==MethodType::PCL_GENERIC)ndt.setTransformationEpsilon(trans_eps);……}if(input->max_iterations!=max_iter){max_iter=input->max_iterations;if(_method_type==MethodType::PCL_GENERIC)ndt.setMaximumIterations(max_iter);……}……ndt_mapping.cpp文本中param_callback函数Part35.2激光点云定位功能包param_callback函数通过调用NDT配置参数消息autoware_config_msgs::ConfigNDT.msg传入参数的方式对NDT配准算法参数连续变化差值（trans_eps）和最大迭代次数（max_iter）进行设置。49节点ndt_matching与源码解析（2）关键子函数解析param_callback函数……staticvoidparam_callback(constautoware_config_msgs::ConfigNDT::ConstPtr&input){……if(_use_gnss==0&&init_pos_set==0){initial_pose.x=input->x;initial_pose.y=input->y;initial_pose.z=input->z;initial_pose.roll=input->roll;initial_pose.pitch=input->pitch;initial_pose.yaw=input->yaw;……

}……}}……ndt_mapping.cpp文本中param_callback函数Part45.2激光点云定位功能包当判断语句_use_gnss==0&&init_pos_set==0为真时，表示未使用GNSS且未进行位姿初始化，此时利用NDT配置参数消息autoware_config_msgs::ConfigNDT.msg传入的初始位姿参数（input->x，input->y，input->z，input->roll，input->pitch，input->yaw）对initial_pose进行赋值完成位姿初始化。50节点ndt_matching与源码解析（2）关键子函数解析param_callback函数……staticvoidparam_callback(constautoware_config_msgs::ConfigNDT::ConstPtr&input){……if(_use_gnss==0&&init_pos_set==0){……if(_use_local_transform==true){tf::Vector3v(input->x,input->y,input->z);tf::Quaternionq;q.setRPY(input->roll,input->pitch,input->yaw);tf::Transformtransform(q,v);initial_pose.x=(local_transform.inverse()*transform).getOrigin().getX();initial_pose.y=(local_transform.inverse()*transform).getOrigin().getY();initial_pose.z=(local_transform.inverse()*transform).getOrigin().getZ();tf::Matrix3x3m(q);m.getRPY(initial_pose.roll,initial_pose.pitch,initial_pose.yaw);……

}……}}……ndt_mapping.cpp文本中param_callback函数Part55.2激光点云定位功能包当判断语句_use_gnss==0&&init_pos_set==0为真，且判断语句_use_local_transform==true为真时，表示使用局部变换，此时需要补偿一个局部变换矩阵local_transform.inverse()，通过局部变换矩阵local_transform.inverse()实现世界坐标系与地图坐标系之间的转换，从而得到车辆在地图坐标系中的初始位姿。51节点ndt_matching与源码解析（2）关键子函数解析gnss_callback函数主要用于实现GNSS重定位功能，具体用于使用GNSS时需要对GNSS位置进行初始化的情况，或者IMU与ODOM累计误差已经影响到NDT配准结果需要提供NDT配准初值的情况。……intmain(intargc,char**argv){……ros::Subscriberparam_sub=nh.subscribe("config/ndt",10,param_callback);ros::Subscribergnss_sub=nh.subscribe("gnss_pose",10,gnss_callback);ros::Subscribermap_sub=nh.subscribe("points_map",1,map_callback);ros::Subscriberinitialpose_sub=nh.subscribe("initialpose",10,initialpose_callback);ros::Subscriberpoints_sub=nh.subscribe("filtered_points",_queue_size,points_callback);……}ndt_mapping.cpp文本中主函数的话题发布和订阅部分5.2激光点云定位功能包gnss_callback函数52节点ndt_matching与源码解析（2）关键子函数解析gnss_callback函数……staticvoidgnss_callback(constgeometry_msgs::PoseStamped::ConstPtr&input){tf::Quaterniongnss_q(input->pose.orientation.x,input->pose.orientation.y,input->pose.orientation.z,input->pose.orientation.w);tf::Matrix3x3gnss_m(gnss_q);posecurrent_gnss_pose;current_gnss_pose.x=input->pose.position.x;current_gnss_pose.y=input->pose.position.y;current_gnss_pose.z=input->pose.position.z;gnss_m.getRPY(current_gnss_pose.roll,current_gnss_pose.pitch,current_gnss_pose.yaw);staticposeprevious_gnss_pose=current_gnss_pose;ros::Timecurrent_gnss_time=input->header.stamp;staticros::Timeprevious_gnss_time=current_gnss_time;……}……}……ndt_matching.cpp文本中gnss_callback函数Part15.2激光点云定位功能包将GNSS的旋转参数保存至TF类型的旋转四元数gnss_q，其次将位置四元数转换为旋转矩阵gnss_m，然后利用函数gnss_m.getRPY()计算得到旋转矩阵的RPY旋转角。53节点ndt_matching与源码解析（2）关键子函数解析gnss_callback函数……staticvoidgnss_callback(constgeometry_msgs::PoseStamped::ConstPtr&input){if((_use_gnss==1&&init_pos_set==0)||fitness_score>=500.0){previous_pose.x=previous_gnss_pose.x;previous_pose.y=previous_gnss_pose.y;previous_pose.z=previous_gnss_pose.z;previous_pose.roll=previous_gnss_pose.roll;previous_pose.pitch=previous_gnss_pose.pitch;previous_pose.yaw=previous_gnss_pose.yaw;current_pose.x=current_gnss_pose.x;current_pose.y=current_gnss_pose.y;current_pose.z=current_gnss_pose.z;current_pose.roll=current_gnss_pose.roll;current_pose.pitch=current_gnss_pose.pitch;current_pose.yaw=current_gnss_pose.yaw;……init_pos_set=1;}……}……ndt_matching.cpp文本中gnss_callback函数Part25.2激光点云定位功能包当判断语句_use_gnss==1&&init_pos_set==0为真，或者当判断语句fitness_score>=500.0为真时，表示如果使用GNSS且标志位init_pos_set的值设置为0，或者如果适应度分数fitness_score大于或等于500（适应度分数fitness_score主要用于评价NDT配准效果，其值越小配准效果越好），则利用GNSS重定位更新先前位姿previous_pose和当前位姿current_pose，将GNSS位姿作为车辆位姿。最后，将标志位init_pos_set的值更新为1结束GNSS重定位。54节点ndt_matching与源码解析（2）关键子函数解析map_callback函数主要用于载入全局点云地图，并将该全局点云地图添加为NDT配准的目标点云。……intmain(intargc,char**argv){……ros::Subscriberparam_sub=nh.subscribe("config/ndt",10,param_callback);ros::Subscribergnss_sub=nh.subscribe("gnss_pose",10,gnss_callback);ros::Subscribermap_sub=nh.subscribe("points_map",1,map_callback);ros::Subscriberinitialpose_sub=nh.subscribe("initialpose",10,initialpose_callback);ros::Subscriberpoints_sub=nh.subscribe("filtered_points",_queue_size,points_callback);……}ndt_mapping.cpp文本中主函数的话题发布和订阅部分5.2激光点云定位功能包map_callback函数55节点ndt_matching与源码解析（2）关键子函数解析……staticunsignedintpoints_map_num=0;……staticvoidmap_callback(constsensor_msgs::PointCloud2::ConstPtr&input){//if(map_loaded==0)if(points_map_num!=input->width){std::cout<<"Updatepoints_map."<<std::endl;points_map_num=input->width;//Convertthedatatype(fromsensor_msgstopcl).pcl::fromROSMsg(*input,map);……}}……ndt_matching.cpp文本中map_callback函数Part15.2激光点云定位功能包首先判断输入的sensor_msgs::PointCloud2类型点云宽度input->width是否与points_map_num相等，当判断语句points_map_num!=input->width为真时，表示全局点云地图未加载，则将input->width的值赋给points_map_num。然后利用pcl::fromROSMsg()函数将输入的sensor_msgs::PointCloud2类型的点云数据*input转化为pcl::PointCloud<pcl::PointXYZI>类型的全局点云地图map。map_callback函数56节点ndt_matching与源码解析（2）关键子函数解析……staticunsignedintpoints_map_num=0;……staticvoidmap_callback(constsensor_msgs::PointCloud2::ConstPtr&input){if(points_map_num!=input->width){……if(_use_local_transform==true){tf::TransformListenerlocal_transform_listener;try{ros::Timenow=ros::Time(0);local_transform_listener.waitForTransform("/map","/world",now,ros::Duration(10.0));local_transform_listener.lookupTransform("/map","world",now,local_transform);}catch(tf::TransformException&ex){ROS_ERROR("%s",ex.what());}pcl_ros::transformPointCloud(map,map,local_transform.inverse());}……}}……ndt_matching.cpp文本中map_callback函数Part25.2激光点云定位功能包当判断语句_use_local_transform==true为真时，表示世界坐标系与地图坐标系之间的局部变换存在，则使用try语句块和catch子句进行异常检测与处理，其中try块中包含可能会抛出异常的语句，一旦有异常抛出就会被后面关联的catch子句捕获并输出ROS错误。map_callback函数57节点ndt_matching与源码解析（2）关键子函数解析……staticunsignedintpoints_map_num=0;……staticvoidmap_callback(constsensor_msgs::PointCloud2::ConstPtr&input){if(points_map_num!=input->width){……if(_use_local_transform==true){tf::TransformListenerlocal_transform_listener;try{ros::Timenow=ros::Time(0);local_transform_listener.waitForTransform("/map","/world",now,ros::Duration(10.0));local_transform_listener.lookupTransform("/map","world",now,local_transform);}catch(tf::TransformException&ex){ROS_ERROR("%s",ex.what());}pcl_ros::transformPointCloud(map,map,local_transform.inverse());}……}}……ndt_matching.cpp文本中map_callback函数Part25.2激光点云定位功能包ROS中的tf坐标变换并不是实时的，利用lookupTransform()函数获取缓冲区里某个时刻的坐标数据，但是前提是缓冲区里有这个时刻的坐标数据，所以代码中为了获得当前时刻ros::Time(0)的坐标数据使用了waitForTransform()函数等待tf坐标变换，如果等待时间超过设定时间