二.路径规划---二维路径规划实车实现---gmapping+amcl+map_server+move

专栏系列文章如下：

一.路径规划---二维路径规划仿真实现-gmapping+amcl+map_server+move_base_goldqiu的博客-CSDN博客

本次实验是利用gmapping只使用二维点云进行建图，利用move_base进行规划。

后期需要写一个三维点云实时转换栅格地图包（包可以实现选择性转换输出三维栅格和2.5-D高程栅格、二维栅格地图），进行gmapping的功能替换就行，里程计就不用改，3D-SLAM的里程计直接可以拿来用，接口一样的。

1.先实现gmapping建图：

a.开启速腾聚创 rs_lidar_16雷达驱动（跟之前一样）

b.使用pointcloud_to_laserscan包实现三维转二维（编写pointcloudtoscan.launch）

<?xml version="1.0"?>

<launch>
<!-- run pointcloud_to_laserscan node --><include file="$(find rslidar_sdk)/launch/start.launch" /><node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
<remap from="cloud_in" to="/rslidar_points"/><rosparam># target_frame: rslidar # Leave disabled to output scan in pointcloud frametransform_tolerance: 0.01min_height: -0.4max_height: 1.0
angle_min: -3.1415926 # -M_PIangle_max: 3.1415926 # M_PIangle_increment: 0.003 # 0.17degreescan_time: 0.1range_min: 0.2range_max: 100use_inf: trueinf_epsilon: 1.0
# Concurrency level, affects number of pointclouds queued for processing and number of threads used# 0 : Detect number of cores# 1 : Single threaded# 2->inf : Parallelism levelconcurrency_level: 1</rosparam>
</node>

</launch>

c.源码下载gmapping包，这里不要用apt去下载，因为要修改源码（gmapping源码不支持2048以上的点，需要修改下）

git clone https://github.com/ros-perception/openslam_gmapping.git
git clone https://github.com/ros-perception/slam_gmapping.git

在文件目录openslam_gmapping/include/gmapping/scanmatcher/scanmatcher.h文件中

#define LASER_MAXBEAMS 2048 设置为20480（比较大就行）

d.调用gmapping包和laser_scan_matcher包，并用tf包的static_transform_publisher进行gmapping需要的tf信息提供，又因为gmapping必须要里程计信息，而laser_scan_matcher包实现的是二维雷达前端里程计odom输出。编写demo_gmapping.launch：

<!--
Example launch file: uses laser_scan_matcher together with
slam_gmapping
--><launch>#### set up data playback from bag #############################<param name="/use_sim_time" value="true"/>#### publish an example base_link -> laser transform ###########<include file="$(find mapping)/launch/pointcloudtoscan.launch" />
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /rslidar 40" />#### start rviz ################################################<node pkg="rviz" type="rviz" name="rviz" args="-d $(find laser_scan_matcher)/demo/demo_gmapping.rviz"/>#### start the laser scan_matcher ##############################<node pkg="laser_scan_matcher" type="laser_scan_matcher_node" name="laser_scan_matcher_node" output="screen"><param name="fixed_frame" value = "odom"/><param name="max_iterations" value="10"/></node>#### start gmapping ############################################<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen"><param name="map_udpate_interval" value="1.0"/><param name="maxUrange" value="5.0"/><param name="sigma" value="0.1"/><param name="kernelSize" value="1"/><param name="lstep" value="0.15"/><param name="astep" value="0.15"/><param name="iterations" value="1"/><param name="lsigma" value="0.1"/><param name="ogain" value="3.0"/><param name="lskip" value="1"/><param name="srr" value="0.1"/><param name="srt" value="0.2"/><param name="str" value="0.1"/><param name="stt" value="0.2"/><param name="linearUpdate" value="1.0"/><param name="angularUpdate" value="0.5"/><param name="temporalUpdate" value="0.4"/><param name="resampleThreshold" value="0.5"/><param name="particles" value="10"/><param name="xmin" value="-5.0"/><param name="ymin" value="-5.0"/><param name="xmax" value="5.0"/><param name="ymax" value="5.0"/><param name="delta" value="0.02"/><param name="llsamplerange" value="0.01"/><param name="llsamplestep" value="0.05"/><param name="lasamplerange" value="0.05"/><param name="lasamplestep" value="0.05"/></node></launch>

这里要注意：

  <param name="/use_sim_time" value="true"/>

跑bag这个标志为true，实物跑这个标志为false

实车效果：

2.再实现move_base路径规划

move_base的框图如下：

拷贝gmapping建图功能下面的功能包，包括雷达驱动、gmapping功能包到src。

github下载scout2小车驱动包：https://github.com/agilexrobotics/scout_ros.git

https://github.com/agilexrobotics/ugv_sdk.git

安装小车驱动依赖：

sudo apt install -y libasio-dev
sudo apt install -y ros-$ROS_DISTRO-teleop-twist-keyboard

设置CAN-To-USB模块

sudo modprobe gs_usb

第一次用小车驱动包则执行：

rosrun scout_bringup setup_can2usb.bash

不是第一次则执行：

rosrun scout_bringup bringup_can2usb.bash

键盘测试下小车运动，没问题则进行下一步：

github下载move_base包（集成在navigation）：

https://github.com/ros-planning/navigation.git

将之前仿真的robot_sim包拷贝到src。

因为用laser_scan_matcher功能包生成的雷达odom没有数据（不知原因），所以用了小车自带的里程计。

更改demo_mapping.launch如下：

<!--
Example launch file: uses laser_scan_matcher together with
slam_gmapping
--><launch>#### set up data playback from bag #############################<param name="/use_sim_time" value="flase"/>
<include file="$(find mapping)/launch/pointcloudtoscan.launch" />
#### publish an example base_link -> laser transform ###########<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.0 -1.57 0.0 0.0 /base_link /rslidar 40" />#### start rviz ################################################<node pkg="rviz" type="rviz" name="rviz" args="-d $(find mapping)/config/demo_gmapping.rviz"/><include file="$(find scout_bringup)/launch/scout_mini_robot_base.launch" />
#### start gmapping ############################################<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen"><param name="base_frame" value="/base_link"/> <param name="odom_frame" value="/odom" /> <param name="map_frame" value="/map" /><param name="map_udpate_interval" value="1.0"/><param name="maxUrange" value="5.0"/><param name="sigma" value="0.1"/><param name="kernelSize" value="1"/><param name="lstep" value="0.15"/><param name="astep" value="0.15"/><param name="iterations" value="1"/><param name="lsigma" value="0.1"/><param name="ogain" value="3.0"/><param name="lskip" value="1"/><param name="srr" value="0.1"/><param name="srt" value="0.2"/><param name="str" value="0.1"/><param name="stt" value="0.2"/><param name="linearUpdate" value="1.0"/><param name="angularUpdate" value="0.5"/><param name="temporalUpdate" value="0.4"/><param name="resampleThreshold" value="0.5"/><param name="particles" value="10"/><param name="xmin" value="-5.0"/><param name="ymin" value="-5.0"/><param name="xmax" value="5.0"/><param name="ymax" value="5.0"/><param name="delta" value="0.02"/><param name="llsamplerange" value="0.01"/><param name="llsamplestep" value="0.05"/><param name="lasamplerange" value="0.05"/><param name="lasamplestep" value="0.05"/></node></launch>

这里要注意：

<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.0 -1.57 0.0 0.0 /base_link /rslidar 40" />

需要调整base_link到rslidar的旋转变换（小车坐标系和雷达坐标系），由于这边是Z轴转了90度，所有要更改参数为 args="0.0 0.0 0.0 -1.57 0.0 0.0 /base_link /rslidar 40"

调好的现象是odom的箭头跟小车运动方向一致。

先实现局部路径规划（局部路径规划调用了gmapping和move_base）：

编写path_planning.launch

<launch>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true"><rosparam file="$(find robot_sim)/config/costmap_common_params.yaml" command="load" ns="global_costmap" /><rosparam file="$(find robot_sim)/config/costmap_common_params.yaml" command="load" ns="local_costmap" /><rosparam file="$(find robot_sim)/config/local_costmap_params.yaml" command="load" /><rosparam file="$(find robot_sim)/config/global_costmap_params.yaml" command="load" /><rosparam file="$(find robot_sim)/config/base_local_planner_params.yaml" command="load" /></node>

</launch>

更改costmap_common_params.yaml

#机器人几何参数，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint

# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状

obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物

footprint: [[-0.350, -0.465], [-0.350, 0.465], [0.350, 0.465], [0.350, -0.465]]


#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2
#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0

map_type: costmap
observation_sources: scan
scan: {sensor_frame: rslidar, data_type: LaserScan, topic: scan, marking: true, clearing: true}

主要要更改无人车的尺寸、膨胀半径、话题消息名称、类型、frame等。

更改global_costmap_params.yaml：

global_costmap:global_frame: maprobot_base_frame: base_link
update_frequency: 10.0publish_frequency: 10.0transform_tolerance: 0.5
static_map: true

主要更改frame

更改local_costmap_params.yaml：

local_costmap:global_frame: odomrobot_base_frame: base_link
update_frequency: 10.0publish_frequency: 10.0transform_tolerance: 0.5
static_map: false  rolling_window: truewidth: 3height: 3resolution: 0.05

主要更改frame

编写local_navigation.launch

<launch><include file="$(find mapping)/launch/demo_mapping.launch" /><!-- 运行move_base节点 --><include file="$(find robot_sim)/launch/path_planning.launch" />
</launch>

测试局部路径规划，在局部代价地图中，点击目标点，小车会运动过去，同时局部代价地图中进行障碍的感知，小车会绕过突然出现的障碍。

然后实现全局路径规划（需要读取录制好的地图，开启amcl、move_base）：

编写amcl.launch

<launch><node pkg="amcl" type="amcl" name="amcl" output="screen"><!-- Publish scans from best pose at a max of 10 Hz --><param name="odom_model_type" value="diff"/><!-- 里程计模式为差分 --><param name="odom_alpha5" value="0.1"/><param name="transform_tolerance" value="0.2" /><param name="gui_publish_rate" value="10.0"/><param name="laser_max_beams" value="30"/><param name="min_particles" value="500"/><param name="max_particles" value="5000"/><param name="kld_err" value="0.05"/><param name="kld_z" value="0.99"/><param name="odom_alpha1" value="0.2"/><param name="odom_alpha2" value="0.2"/><!-- translation std dev, m --><param name="odom_alpha3" value="0.8"/><param name="odom_alpha4" value="0.2"/><param name="laser_z_hit" value="0.5"/><param name="laser_z_short" value="0.05"/><param name="laser_z_max" value="0.05"/><param name="laser_z_rand" value="0.5"/><param name="laser_sigma_hit" value="0.2"/><param name="laser_lambda_short" value="0.1"/><param name="laser_lambda_short" value="0.1"/><param name="laser_model_type" value="likelihood_field"/><!-- <param name="laser_model_type" value="beam"/> --><param name="laser_likelihood_max_dist" value="2.0"/><param name="update_min_d" value="0.2"/><param name="update_min_a" value="0.5"/><param name="odom_frame_id" value="odom"/><!-- 里程计坐标系 --><param name="base_frame_id" value="base_link"/><!-- 添加机器人基坐标系 --><param name="global_frame_id" value="map"/><!-- 添加地图坐标系 --><param name="resample_interval" value="1"/><param name="transform_tolerance" value="0.1"/><param name="recovery_alpha_slow" value="0.0"/><param name="recovery_alpha_fast" value="0.0"/></node></launch>

编写map_save.launch

<launch><arg name="filename" value="$(find robot_sim)/map/nav" /><node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>

先运行建图，建好图后运行map_save.launch，进行地图保存。

编写global_navigation.launch

<launch><param name="/use_sim_time" value="flase"/>
<include file="$(find mapping)/launch/pointcloudtoscan.launch" />
#### publish an example base_link -> laser transform ###########<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.0 -1.573 0.0 0.0 /base_link /rslidar 40" />#### start rviz ################################################<node pkg="rviz" type="rviz" name="rviz" args="-d $(find mapping)/config/demo_gmapping.rviz"/><!-- 设置地图的配置文件 --><arg name="map" default="nav.yaml" /><!-- 运行地图服务器，并且加载设置的地图--><node name="map_server" pkg="map_server" type="map_server" args="$(find robot_sim)/map/$(arg map)"/><!-- 启动AMCL节点 -->
<include file="$(find robot_sim)/launch/amcl.launch" />
<!-- 运行move_base节点 --><include file="$(find robot_sim)/launch/path_planning.launch" /><!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find robot_sim)/config/robot_mapping.rviz" /> --><include file="$(find scout_bringup)/launch/scout_mini_robot_base.launch" />

</launch>

运行后效果如下：

全局路径规划下，小车起始位置为红色，终点为蓝色，经过10分钟左右能到达终点。

暴露出的问题点：

由于使用的是小车自带里程计，所以建出的图会漂，后续要迭代的功能是用多传感器融合SLAM下的三维点云地图转换成二维栅格地图，且多传感器融合后能给出高精度里程计，保证图不漂。
全局路径规划下小车运动缓慢且前进时不断左右转甚至转圈，运动不够平滑，需要根据小车运动学优化规划算法。（或者还可能是小车运动控制的问题）
目前使用的ros包比较多且杂，传感器底层驱动和小车底层驱动包无须更改，SLAM、感知和规划都是用的开源包，但后续需要将SLAM、感知和规划部分重新写框架代码并整合成不同的三个功能包，接口之间方便互相调用，能够迭代升级不同算法，不断迭代升级小车的智能化水平。

二.路径规划---二维路径规划实车实现---gmapping+amcl+map_server+move_base相关推荐

一.路径规划---二维路径规划仿真实现-gmapping+amcl+map_server+move_base
参考http://www.autolabor.com.cn/book/ROSTutorials/chapter1.html 1.首先实现仿真平台在rviz和gazebo的实现: 编写rviz环境下的x ...
【A_star二维路径规划】基于matlab A_star算法无人机二维路径规划（起终点障碍物可设置）【含Matlab源码 1321期】
⛄一.获取代码方式获取代码方式1: 通过订阅紫极神光博客付费专栏,凭支付凭证,私信博主,可获得此代码. 获取代码方式2: 完整代码已上传我的资源:[A_star二维路径规划]基于matlab A_s ...
《MATLAB智能算法30个案例》：第23章基于蚁群算法的二维路径规划算法
<MATLAB智能算法30个案例>:第23章基于蚁群算法的二维路径规划算法 1. 前言 2. MATLAB 仿真示例 3. 小结 1. 前言 <MATLAB智能算法30个案例分析& ...
Autoware实车测试记录（四）--全局及局部路径规划相关功能以及发送控制指令至底盘
经过前面的一系列工作,现在小车在地图中任意位置已经可以实现实时定位以及检测到障碍物以及追踪这些障碍物.下面的内容主要是继续完成下一步-路径规划,这其中包括了全局宏观上的路径规划(全局路径规划)以及在行 ...
左上角到右下角的路径 oj_【机器人路径规划】快速扩展随机树(RRT)算法
内容无关:最近的课题内容和机器人运动规划方法有关,我把学习的内容整理成为工具箱上传到了我的github仓库,稍后将会发一篇说明介绍使用方法. XM522706601/robotics_tutorial ...
路径规划: 浅谈路径规划算法
原文地址:http://theory.stanford.edu/~amitp/GameProgramming/ 1 导言 1.1 算法 1.2 Dijkstra算法与最佳优先搜索 1.3 A*算法 2 ...
大数据部门各个职位的职业生涯规划和发展路径
从职业发展路径来看,一般可以分两个路线来走,一个是专业技术路线,也叫T序列,另一个是管理路线,也叫M序列,每个序列都分很多级别.T序列一般职位从低到高是工程师.资深工程师.架构师/专家.高级架构师/高 ...
autoware使用rosbag数据生成路径点并进行路径规划（七）
autoware使用rosbag数据生成路径点并进行路径规划(七) 第一步启动autoware和播放数据包 $ cd ~/autoware.ai $ source install/setup.bash ...
二查看oracle归档日志路径
一,检查是否开启归档,开启的方法数据的存档模式的操作必须要在mount中进行,且数据库要处于非open状态.所有关闭存档模式前,我们需要查看当前数据库的状态,并且关闭数据库,启动到mount,才可进 ...

二.路径规划---二维路径规划实车实现---gmapping+amcl+map_server+move_base

二.路径规划---二维路径规划实车实现---gmapping+amcl+map_server+move_base相关推荐

最新文章

热门文章