PS: 非常详细。 一、雷达介绍: 如果使用Arduino读取数据,则将激光雷达的RX和与Arudino TX、TX和Arudino RX相接,上电即可读取数据, 二、数据格式 串口通讯: 速率 115200 8N1 雷达每完整旋转一周会发送90个数据包 每个数据包中包含4个测量点的信息 每个数据包长度固定是22个字节 这样总共360°旋转一周共 1980个字节,相当于每度会有一个距离数据. 其中雷达的数据包格式如下 <start> <index> <speed_l> <speed_h> [Data 0] [Data 1] [Data 2] [Data 3] <checksum_l> <checksum_h> <start> :0xFA,是固定格式表明数据包开始,可用来从数据流中分割数据包。 <index> :数据包的索引号,范围从0xA0 到 0xF9 (总共89个包,每个包4个数据)。 <speed> :有两个speedL和speedH ,它们各一个字节,共同组成转速信息,大概是低6bit表示小数部分。( 实际这里我不知道怎么翻译了,直接抄的雷达大叔的。) <data> :[ Data 0] 到 [Data 3] 是四组测量数据,其中每组测量数据分别由4个字节组成,如下: byte 0 : <distance 7:0=""> # 距离信息的0-7位 byte 1 : <“invalid data” flag> <“strength warning” flag> <distance 13:8=""> # 错误信息标志位 , 警告位, 距离信息13-8位byte 2 : <signal strength="" 7:0=""> # 信号强度 0-7位 byte 3 : <signal strength="" 15:8=""> # 讯号强度 8-15位 距离信息的单位是mm ,整个激光雷达的测量范围大概是15cm 到6m, 只需要把距离信息的两个字节组装成一个整数即可(注意判断无效数据位为0,如果为1意味是无效的数据,需丢弃)。 <checksum> :由两个字节组成的校验码,用来校验整个数据包是否正确,代码如下: def checksum(data): """Compute and return the checksum as an int.""" # group the data by word, little-endian data_list = [] for t in range(10): data_list.append( data[2*t] + (data[2*t+1]<<8) ) # compute the checksum on 32 bits chk32 = 0 for d in data_list: chk32 = (chk32 << 1) + d # return a value wrapped around on 15bits, and truncated to still fit into 15 bits checksum = (chk32 & 0x7FFF) + ( chk32 >> 15 ) # wrap around to fit into 15 bits checksum = checksum & 0x7FFF # truncate to 15 bits return int( checksum ) 三、ROS中测试使用XV-11 环境:ubuntu14.04+indigo(完整版) 准备:雷达+转接板+调速板 第一步建立工作空间 mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src catkin_init_workspace 第二步编译雷达驱动 cd ~/catkin_ws/src git clone cd ~/catkin_ws catkin_make 这个编译一般没什么问题,编译完成后,USB插上连接电脑即可,可以在dev下查看一下端口号 ls /dev/ 一般都是ttyUSB0 第三步雷达测试 打开一个终端 source ~/catkin_ws/devel/setup.bash roscore 打开一个终端(注意使用版本2,具体原因是雷达几个版本的数据格式不一样,目前用的是2) source ~/catkin_ws/devel/setup.bash rosrun xv_11_laser_driver neato_laser_publisher _port:=/dev/ttyUSB0 _firmware_version:=2 打开一个终端查看雷达的信息 rosrun rviz rviz 修改Global Options的Fixed Frame为/neato_laser(手打) 点击Add,添加LaserScan 然后点云就可以出来了, 如果没出数据点云或者出现随机的红色点可能是雷达速度太快,可以调节调速板 速度跳低,则可看到数据。(图片太小不清楚可右键新窗口打开) 四、ROS中使用XV11进行hector_slam ROS中常用的2d slam算法主要有gmapping和hector_slam,以下是这两种方法的介绍,大家觉得长可以直接卡维纳大白话版翻译。 hector_slam 需要高更新频率小测量噪声的激光扫描仪,不需要里程计,使空中无人机与地面小车在不平坦区域运行存在运 用的可能性,利用已经获得的地图对激光束点阵进行优化,估计激光点在地图的表示,和占据网格的概率,其中扫描匹配利用的是高斯牛顿的方法进行求解. 找到激光点集映射到已有地图的刚体转换(x,y,theta). ( 接触的匹配的方法还有最近邻匹配的方法(ICP) ,gmapping代码中的scanmatcher部分有两种方法选择. )为避免局部最小而非全局最优的(类似于多峰值模型的,局部梯度最小了,但非全局最优)出现,地图采用多分辨率的形式。导航中的状态估计可以加入惯性测 量,进行EKF滤波. 【 大白话翻译:hector_slam是个性能非常好的算法,通过最小二乘法匹配扫描点,且依赖高精度的激光雷达数据,但是不需要里程计。因此扫描角很小且噪声较大的kinect是不行的,匹配的时候会陷入局部点,地图非常混乱。】 gmapping 目前激光2Dslam用得最广的方法,gmapping采用的是RBPF的方法。必须得了解粒子滤波 (利用统计特性描述物理表达式下的结果)的方法,粒子滤波的方法一般需要大量的粒子来获取好的结果,但这必会引入计算的复杂度;粒子是一个依据过程的观测 逐渐更新权重与收敛的过程,这种重采样的过程必然会代入粒子耗散问题(depletion problem),大权重粒子显著,小权重粒子会消失(有可能正确的粒子模拟可能在中间的阶段表现权重小而消失)。自适应重采样技术引入减少了粒子耗散问 题 , 计算粒子分布的时候不单单仅依靠机器人的运动(里程计),同时将当前观测考虑进去,减少了机器人位置在粒子滤波步骤中的不确定性,(FAST- SLAM2.0 的思想,可以适当减少粒子数)。 【 大白话翻译:gmapping 是一个比较早的算法,核心思想是粒子滤波并且需要里程计,但并不要求很高性能的传感器,初学者一般都是先来玩这个,不过请注意一点,gmapping只是 mapping,定位还需要amcl这个蒙特卡洛算法包配合使用,最后才能接入navigation stack。】 gmapping需要订阅两个话题。 tf是ros中必用的部分,说简单一些,tf类中定义了两个刚体之间的旋转与平移矩阵,并且重载了乘法运算符,这样我们就可以通过相乘两个tf来沿着tf树的方向求末段执行器相对世界坐标的位置与方向。 scan是激光雷达数据,如果你有雷达可以直接使用,如果没有雷达那么可以用depthimage_to_laserscan包,这个包可以 将kinect发布出来的深度图转换成激光雷达扫描数 据。gmapping需要的里程计odom便是通过我们自己发布tf树的形式告诉gmapping,而我们该如何获得这个里程计,这就需要我们自己完成这一部分了,通常做法是在移动平台上安装电机编码器与电子罗盘,在移动平台上的嵌入式单片机(此处与ros无关,我这里用的树莓派)内完成里程计的制作与PID调试,然后,再用树莓派连接单片机串口,单片机将里程计与航向角发给树莓派,树莓派上需要自己写一个node(关于里程计,高级方法是视觉里程计一类的,不过与其折腾那些不如直接上激光雷达),大致就这个流程。 接下来要说怎么在ros中使用XV-11进行hector_slam。 第一步:首先按照第三步下载XV-11驱动并测试成功 第二步:hector_slam 1、首先下载hector_slam包到你工作空间的src下 命令: cd ~/catkin/src git clone cd .. catkin_make 2、然后在添加neato.launch文件 路径:~/catkin_ws/src/hector_slam/hector_slam_launch/launch/ 在此目录下新建neato.launch文件并添加如下代码, <?xml version="1.0"?> <launch> <!--<node pkg="xv_11_laser_driver" type="neato_laser_publisher" name="xv_11_node"> <param name="port" value="/dev/ttyUSB0" /> <param name="firmware_version" value="2" /> <param name="frame_id" value="laser" /> </node>--> <node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0.0 0.0 0.0 0 0 0.0 /odom /base_link 10" /> <node pkg="tf" type="static_transform_publisher" name="base_frame_laser" args="0 0 0 0 0 0 /base_link /laser 10" /> <!--<node pkg="rviz" type="rviz" name="rviz" args="-d $(find hector_slam_launch)/rviz_cfg/mapping_demo.rviz"/>--> <include file="$(find hector_mapping)/launch/mapping_default.launch" /> <node pkg="rviz" type="rviz" name="rviz" args="-d rviz_cfg.rviz" /> <include file="$(find hector_geotiff)/launch/geotiff_mapper.launch" /> </launch> 3、然后在添加新的mapping_default.launch文件(记得备份原mapping_default.launch文件) 路径:~/catkin_ws/src/hector_slam/hector_mapping/launch 其中的mapping_default_old.launch为备份文件。 新mapping_default.launch代码如下 <?xml version="1.0"?> <launch> <arg name="tf_map_scanmatch_transform_frame_name" default="/scanmatcher_frame" /> <arg name="base_frame" default="base_link" /> <arg name="odom_frame" default="base_link" /> <arg name="pub_map_odom_transform" default="true" /> <arg name="scan_subscriber_queue_size" default="5" /> <arg name="scan_topic" default="scan" /> <arg name="map_size" default="2048" /> <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen"> <!-- Frame names --> <param name="map_frame" value="map" /> <param name="base_frame" value="$(arg base_frame)" /> <param name="odom_frame" value="$(arg base_frame)" /> <!-- Tf use --> <param name="use_tf_scan_transformation" value="true" /> <param name="use_tf_pose_start_estimate" value="false" /> <param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)" /> <!-- Map size / start point --> <param name="map_resolution" value="0.050" /> <param name="map_size" value="$(arg map_size)" /> <param name="map_start_x" value="0.5" /> <param name="map_start_y" value="0.5" /> <param name="map_multi_res_levels" value="2" /> <!-- Map update parameters --> <param name="update_factor_free" value="0.4" /> <param name="update_factor_occupied" value="0.7" /> <param name="map_update_distance_thresh" value="0.2" /> <param name="map_update_angle_thresh" value="0.9" /> <param name="laser_z_min_value" value = "-1.0" /> <param name="laser_z_max_value" value = "1.0" /> <!-- Advertising config --> <param name="advertise_map_service" value="true" /> <param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)" /> <param name="scan_topic" value="$(arg scan_topic)" /> <!-- Debug parameters --> <!-- <param name="output_timing" value="false" /> <param name="pub_drawings" value="true" /> <param name="pub_debug_output" value="true" /> --> <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" /> </node> <!--<node pkg="tf" type="static_transform_publisher" name="map_nav_broadcaster" args="0 0 0 0 0 0 map nav 100" />--> </launch> 4、最后修改xv11驱动文件的neato_laser_publisher.cpp文件 路径:~/catkin_ws/src/xv_11_laser_driver/src 修改neato_laser_publisher.cpp文件 找到 priv_nh.param("frame_id", frame_id, std::string("neato_laser")); 修改为 priv_nh.param(“frame_id“, frame_id, std::string(“laser“)); 5、最后编译文件 cd ~/catkin catkin_make 6、运行测试 roscore rosrun xv_11_laser_driver neato_laser_publisher _port:=/dev/ttyUSB0 _firmware_version:=2 roslaunch hector_slam_launch neato.launch
def checksum(data): """Compute and return the checksum as an int.""" # group the data by word, little-endian data_list = [] for t in range(10): data_list.append( data[2*t] + (data[2*t+1]<<8) ) # compute the checksum on 32 bits chk32 = 0 for d in data_list: chk32 = (chk32 << 1) + d # return a value wrapped around on 15bits, and truncated to still fit into 15 bits checksum = (chk32 & 0x7FFF) + ( chk32 >> 15 ) # wrap around to fit into 15 bits checksum = checksum & 0x7FFF # truncate to 15 bits return int( checksum ) def checksum(data): """Compute and return the checksum as an int.""" # group the data by word, little-endian data_list = [] for t in range(10): data_list.append( data[2*t] + (data[2*t+1]<<8) ) # compute the checksum on 32 bits chk32 = 0 for d in data_list: chk32 = (chk32 << 1) + d # return a value wrapped around on 15bits, and truncated to still fit into 15 bits checksum = (chk32 & 0x7FFF) + ( chk32 >> 15 ) # wrap around to fit into 15 bits checksum = checksum & 0x7FFF # truncate to 15 bits return int( checksum ) def checksum(data): """Compute and return the checksum as an int.""" # group the data by word, little-endian data_list = [] for t in range(10): data_list.append( data[2*t] + (data[2*t+1]<<8) ) # compute the checksum on 32 bits chk32 = 0 for d in data_list: chk32 = (chk32 << 1) + d # return a value wrapped around on 15bits, and truncated to still fit into 15 bits checksum = (chk32 & 0x7FFF) + ( chk32 >> 15 ) # wrap around to fit into 15 bits checksum = checksum & 0x7FFF # truncate to 15 bits return int( checksum ) def checksum(data): """Compute and return the checksum as an int.""" # group the data by word, little-endian data_list = [] for t in range(10): data_list.append( data[2*t] + (data[2*t+1]<<8) ) # compute the checksum on 32 bits chk32 = 0 for d in data_list: chk32 = (chk32 << 1) + d # return a value wrapped around on 15bits, and truncated to still fit into 15 bits checksum = (chk32 & 0x7FFF) + ( chk32 >> 15 ) # wrap around to fit into 15 bits checksum = checksum & 0x7FFF # truncate to 15 bits return int( checksum ) def checksum(data): """Compute and return the checksum as an int.""" # group the data by word, little-endian data_list = [] for t in range(10): data_list.append( data[2*t] + (data[2*t+1]<<8) ) # compute the checksum on 32 bits chk32 = 0 for d in data_list: chk32 = (chk32 << 1) + d # return a value wrapped around on 15bits, and truncated to still fit into 15 bits checksum = (chk32 & 0x7FFF) + ( chk32 >> 15 ) # wrap around to fit into 15 bits checksum = checksum & 0x7FFF # truncate to 15 bits return int( checksum ) def checksum(data): """Compute and return the checksum as an int.""" # group the data by word, little-endian data_list = [] for t in range(10): data_list.append( data[2*t] + (data[2*t+1]<<8) ) # compute the checksum on 32 bits chk32 = 0 for d in data_list: chk32 = (chk32 << 1) + d # return a value wrapped around on 15bits, and truncated to still fit into 15 bits checksum = (chk32 & 0x7FFF) + ( chk32 >> 15 ) # wrap around to fit into 15 bits checksum = checksum & 0x7FFF # truncate to 15 bits return int( checksum ) def checksum(data): """Compute and return the checksum as an int.""" # group the data by word, little-endian data_list = [] for t in range(10): data_list.append( data[2*t] + (data[2*t+1]<<8) ) # compute the checksum on 32 bits chk32 = 0 for d in data_list: chk32 = (chk32 << 1) + d # return a value wrapped around on 15bits, and truncated to still fit into 15 bits checksum = (chk32 & 0x7FFF) + ( chk32 >> 15 ) # wrap around to fit into 15 bits checksum = checksum & 0x7FFF # truncate to 15 bits return int( checksum ) def checksum(data): """Compute and return the checksum as an int.""" # group the data by word, little-endian data_list = [] for t in range(10): data_list.append( data[2*t] + (data[2*t+1]<<8) ) # compute the checksum on 32 bits chk32 = 0 for d in data_list: chk32 = (chk32 << 1) + d # return a value wrapped around on 15bits, and truncated to still fit into 15 bits checksum = (chk32 & 0x7FFF) + ( chk32 >> 15 ) # wrap around to fit into 15 bits checksum = checksum & 0x7FFF # truncate to 15 bits return int( checksum ) def checksum(data): """Compute and return the checksum as an int.""" # group the data by word, little-endian data_list = [] for t in range(10): data_list.append( data[2*t] + (data[2*t+1]<<8) ) # compute the checksum on 32 bits chk32 = 0 for d in data_list: chk32 = (chk32 << 1) + d # return a value wrapped around on 15bits, and truncated to still fit into 15 bits checksum = (chk32 & 0x7FFF) + ( chk32 >> 15 ) # wrap around to fit into 15 bits checksum = checksum & 0x7FFF # truncate to 15 bits return int( checksum )
0