ROS-基于已知地图的无人机路径规划算法仿真
在上一篇文章中我们实现了基于PX4的无人机SLAM建图(Cartographer)仿真,最终得到了indoor3的地图。
现在,基于这张建立好的地图进行路径规划算法仿真。
一、创建功能包
在工作空间下创建功能包:
cd ~/catkin_ws/src
catkin_create_pkg my_nav geometry_msgs move_base_msgs roscpp rospy tf visualization_msgs
然后在功能包目录下创建几个文件夹,用于区分文件
config——用于存放相关配置文件;
launch——用于存放launch文件;
map——用于存放地图;
rviz——用于存放rviz文件;
script——用于存放脚本文件;
二、文件配置
2.1 地图文件
将之前保存的地图文件(包括一个.pgm和一个.yaml文件)复制到map文件夹下,但要注意修改.yaml文件中的图片路径,
image: /home/xxx/catkin_ws/src/my_nav/map/indoor3_carto.pgm # 注意修改路径
resolution: 0.050000
origin: [-12.806009, -9.646632, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
2.2 代价地图和规划器配置
通用代价地图配置文件:costmap_common_params.yaml
# 设置机器人检测障碍物的最大范围
obstacle_range: 1
# 设置机器人检测自由空间的最大范围
raytrace_range: 2# 设置机器人的外形半径
robot_radius: 0.17
# 设置障碍物的膨胀参数,也就是机器人应该与障碍物保持的最小安全距离
inflation_radius: 1.0
# 设置代价比例系数,越大则代价值越小,占用的计算量也越小
cost_scaling_factor: 2.0
# 设置地图类型为代价地图
map_type: costmap# observation_sources列出了代价地图需要关注的所有传感器信息,每个传感器信息都会在后面列出详细内容(这里只有雷达)。
# sensor_frame表示传感器的参考系名称
# data_type表示激光数据或者点云数据使用的消息类型
# topic表示传感器发布的话题名称
# marking和clearing用来表示是否需要使用传感器的实时信息来添加或清除代价地图中的障碍物信息。
observation_sources: iris_0/scan
iris_0/scan: {sensor_frame: iris_0/laser_2d, data_type: LaserScan, topic: iris_0/scan, marking: true, clearing: true}
全局代价地图配置文件:global_costmap_params.yaml
global_costmap:# 设置全局代价地图运行的参考系global_frame: map# 设置代价地图可以参考的机器人本体的坐标系robot_base_frame: base_link# 设置全局地图信息更新频率,单位是Hzupdate_frequency: 10.0# 设置全局代价地图发布可视化信息的频率,单位是Hzpublish_frequency: 10.0# 坐标系间的转换可以忍受的最大延时transform_tolerance: 0.5# 用来决定代价地图是否需要根据map_server提供的地图信息进行初始化static_map: true
本地代价地图配置文件:local_costmap_params.yaml
local_costmap:global_frame: maprobot_base_frame: base_linkupdate_frequency: 10.0publish_frequency: 10.0transform_tolerance: 0.5 static_map: false# 设置在机器人移动过程中是否需要滚动窗口,以保持机器人处于中心位置rolling_window: true# width、height和resolution用于设置代价地图的长(米)、高(米)和分辨率(米/格)width: 3height: 3resolution: 0.05
move_base配置文件:move_base_params.yaml
# 当move_base在不活动状态时,是否关掉costmap
shutdown_costmaps: false
# 向无人机发送话题命令的频率
controller_frequency: 2.0
# 在空间清理操作执行前,控制器花多长时间等有效控制下发
controller_patience: 15.0
# 全局规划操作的执行频率.
# 如果设置为0.0,则全局规划器仅在接收到新的目标点或者局部规划器报告路径堵塞时才会重新执行规划操作
planner_frequency: 5.0
# 在空间清理操作执行前,留给规划器多长时间来找出一条有效规划
planner_patience: 5.0# 清除机制的参数,决定清除多远处的障碍
conservative_reset_dist: 3.0# 震荡超时,超过则底层进行异常处理
oscillation_timeout: 10.0
# 震荡距离
oscillation_distance: 0.2
# 是否启动旋转的恢复,必须是recovery_behavior_enabled在使能的基础上才能生效
clearing_rotation_allowed: false
全局规划器配置文件:global_planner_params.yaml
GlobalPlanner:# 是否使用D*全局规划算法,默认为true,如果为false,则使用A*算法use_dijkstra: True# 是否沿着网格规划路径,默认为false,false时使用最速下降法计算路径use_grid_path: False# 是否使用二次逼近的趋势,默认为true,否则使用简单的线性方法use_quadratic: True
本地规划器配置文件:dwa_local_planner_params.yaml
DWAPlannerROS:
# Robot Configuration Parametersmax_vel_x: 0.2 # x方向最大线速度绝对值,单位:m/smin_vel_x: -0.2 # x方向最小线速度绝对值,负数代表可后退,单位:m/smax_vel_y: 0.2 # y方向最大线速度绝对值,单位:m/smin_vel_y: -0.2 # y方向最小线速度绝对值,负数代表可后退,单位:m/smax_vel_trans: 0.3 # 机器人最大平移速度的绝对值,单位为:m/smin_vel_trans: 0.01 # 机器人最小平移速度的绝对值,单位为:m/s(不可为零)max_vel_theta: 0.1 # 机器人的最大旋转角速度的绝对值,单位为:rad/s min_vel_theta: 0.01 # 器人的最小旋转角速度的绝对值,单位为:rad/sacc_lim_x: 1 # 机器人在x方向的极限加速度,单位为:m/s^2acc_lim_y: 1 # 机器人在y方向的极限加速度,单位为:m/s^2acc_lim_theta: 1 # 机器人的极限旋转加速度,单位为:rad/s^2# Goal Tolerance Parametesxy_goal_tolerance: 0.05 # 到到目标点时,控制器在x和y方向上的容差(m)yaw_goal_tolerance: 3.14 # 到达目标点时,控制器在偏航/旋转时的弧度容差(rad)latch_xy_goal_tolerance: false # 设置为true时表示:如果到达容错距离内,机器人就会原地旋转# Forward Simulation Parameterssim_time: 2.0 # 前向模拟轨迹的时间,单位为svx_samples: 20 # x方向速度空间的采样点数vy_samples: 20 # y方向速度空间的采样点数vth_samples: 40 # 旋转方向的速度空间采样点数controller_frequency: 2.0# Trajectory Scoring Parameterspath_distance_bias: 32.0 # 控制器与给定路径接近程度的权重goal_distance_bias: 20.0 # 控制器与局部目标点的接近程度的权重,也用于速度控制occdist_scale: 0.02 # 控制器躲避障碍物的程度forward_point_distance: 0.325 # 以机器人为中心,额外放置一个计分点的距离stop_time_buffer: 0.2 # 机器人在碰撞发生前必须拥有的最少时间量scaling_speed: 0.25 # 开始缩放机器人足迹时的速度的绝对值,单位为m/smax_scaling_factor: 0.2 # 最大缩放因子# Oscillation Prevention Parametersoscillation_reset_dist: 0.05 # 机器人必须运动多少米远后才能复位震荡标记
# Debuggingpublish_traj_pc : true # 将规划的轨迹在RVIZ上进行可视化publish_cost_grid_pc: true # 将代价值进行可视化显示
这里偏航角容差yaw_goal_tolerance设成了pi,因为我的无人机进行路径规划导航时只要求位置到达目标点即可,航向没有要求,所以航向角容差比较大。
2.3 创建launch文件
完成所有配置文件创建后,现在开始创建launch文件启动move_base节点,并加载相关配置文件。在launch文件夹目录下,创建名为move_base.launch的文件,文件代码如下
<launch><arg name="cmd_vel_topic" default="/cmd_vel" /><arg name="odom_topic" default="odom" /><arg name="move_forward_only" default="false"/><node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true" ><param name="base_global_planner" value="global_planner/GlobalPlanner" /><param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" /><rosparam file="$(find my_nav)/config/costmap_common_params.yaml" command="load" ns="global_costmap" /><rosparam file="$(find my_nav)/config/costmap_common_params.yaml" command="load" ns="local_costmap" /><rosparam file="$(find my_nav)/config/local_costmap_params.yaml" command="load" /><rosparam file="$(find my_nav)/config/global_costmap_params.yaml" command="load" /><rosparam file="$(find my_nav)/config/move_base_params.yaml" command="load" /><rosparam file="$(find my_nav)/config/dwa_local_planner_params.yaml" command="load" /><rosparam file="$(find my_nav)/config/global_planner_params.yaml" command="load" /><remap from="cmd_vel" to="$(arg cmd_vel_topic)"/><remap from="odom" to="$(arg odom_topic)"/><param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" /><param name="DWAPlannerROS/max_vel_y" value="0.0" if="$(arg move_forward_only)" /><param name="DWAPlannerROS/min_vel_y" value="0.0" if="$(arg move_forward_only)" /></node></launch>
然后创建一个运行所有导航功能节点的顶层launch文件nav_demo_indoor3.launch,
<launch><node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 base_link iris_0/laser_2d 100" /><!-- 设置地图的配置文件 --><arg name="map" default="indoor3.yaml" /><arg name="move_forward_only" default="false"/><!-- 运行地图服务器,并且加载设置的地图 --><node name="map_server" pkg="map_server" type="map_server" args="$(find my_nav)/map/$(arg map)" /><!-- 运行move_base节点 --><include file="$(find my_nav)/launch/move_base.launch" ><arg name="move_forward_only" value="$(arg move_forward_only)"/><arg name="cmd_vel_topic" default="/xtdrone/iris_0/cmd_vel_flu" /><arg name="odom_topic" default="/iris_0/mavros/local_position/odom" /></include><!-- 运行虚拟定位,兼容AMCL输出 --><!-- fake localization包含map到odom的tf坐标转换 --><node pkg="fake_localization" type="fake_localization" name="fake_localization" output="screen" ><remap from="base_pose_ground_truth" to="iris_0/mavros/local_position/odom" /><param name="global_frame_id" value="map" /><param name="base_frame_id" value="base_link" /></node><!-- 运行rviz --><node pkg="rviz" type="rviz" name="rviz" args="-d $(find my_nav)/rviz/2d_motion_planning.rviz" /></launch>
至此,我们基本上已经完成了实现基于已知地图的路径规划算法仿真的文件配置,下面准备开始进行仿真。
三、仿真实现
1、首先打开地图环境,运行
roslaunch my_nav indoor3.launch
2、启动导航功能节点,运行
roslaunch my_nav nav_demo_indoor3.launch
完成启动后可以检查一下自己的tf tree结构是否完整,运行
rosrun rqt_tf_tree rqt_tf_tree
完整的tf tree结构如下
检查无误后,就可以控制无人机起飞了。
3、启动通信脚本(这里我把脚本复制到了my_nav这个功能包下)
cd ~/catkin_ws/src/my_nav/script
python multirotor_communication.py iris 0
4、启动键盘控制脚本,控制无人机起飞
cd ~/catkin_ws/src/my_nav/script
python multirotor_keyboard_control.py iris 1 vel
在键盘控制终端激活情况下,首先按b键,将无人机切换至offboard模式;然后按t键解锁无人机,解锁后持续按i键,至upward vel增大至0.30以上,就可以看到无人机已经开始起飞了。
无人机起飞至合适高度后,按s或k键,使无人机悬停。
完成起飞后,需要关闭键盘控制的终端,否则路径规划导航时的指令会与键盘
5、在rviz界面上点击“2D Nav Goal”,然后在地图中任意位置点击并拖动鼠标确定方向,然后就可以看到地图上会自动显示一条绿色的轨迹,这是全局规划产生的路径,无人机坐标系附近还会有一条红色的轨迹,这是局部规划产生的路径。
至此,基于已知地图的无人机路径规划算法仿真就基本实现了。接下来还会进一步实现多个目标点的自动路径规划与导航实现。
ROS-基于已知地图的无人机路径规划算法仿真相关推荐
- python无人机路径规划算法_RRT算法在Python中的实现,快速,拓展,随机,树
""" <基于智能优化与RRT算法的无人机任务规划方法研究>博士论文 <基于改进人工势场法的路径规划算法研究>硕士论文 ""& ...
- python无人机路径规划算法_快速拓展随机树(RRT)路径规划,python
1 """2 version1.1,2018-05-09 3 <基于智能优化与RRT算法的无人机任务规划方法研究>博士论文4 <基于改进人工势场法的路径 ...
- 常用的地图导航和路径规划算法
作者:李传学 链接:https://www.zhihu.com/question/24870090/answer/73834896 来源:知乎 著作权归作者所有.商业转载请联系作者获得授权,非商业转载 ...
- TurtleBot基于已知地图的自主导航
Note:我们使用的是TurtleBot2. 预设置 TurtleBot端和PC端 都需要安装ROS,安装过程自行查看相关教程. 都需要安装相关功能包,执行以下命令: sudo apt-get ins ...
- python无人机路径规划算法_无人机集群——航迹规划你不知道的各种算法优缺点...
我们都知道无人机(UAV )因具有可探测性低.造价低廉.不惧伤亡.起降简单.操作灵活.系统配置多样化.自动控制智能化等特点,因而在未来一体化联合作战中扮演越来越重要的角色.然而早期的无人机都是按照地面 ...
- 基于强化学习的智能机器人路径规划算法研究(附代码)
目录 一.摘要 二.路径规划技术的研究进展 1.研究现状 2.算法分类 2.1 全局路径规划算法 2.2 局部路径规划算法 三.本文采用的路径规划算法--强化学习 1. 概念 2. 与其他机器学习方式 ...
- 自动驾驶路径规划——基于概率采样的路径规划算法(RRT、RRT*)
目录 1. RRT算法背景 1.1 RRT算法核心思想 1.2 RRT算法优缺点 2. 经典RRT算法 2.1 RRT算法流程 2.2 RRT伪代码 3. 基于目标概率采样 4. RRT*算法 4.1 ...
- ROS常用局部路径规划算法比较
本博文主要讨论ROS导航包中集成的局部路径规划算法,DWA.TEB.MPC等算法在使用过程中的各自的优缺点.以下均为自己在使用过程中总结的经验及查阅资料得来,如有理解不到位的地方,还希望在评论区多多讨 ...
- 路径规划算法学习Day5
路径规划算法学习Day5-A*算法的实现 原理回顾 一.A* 算法评价函数 二.曼哈顿距离(Manhattan Distance) 2.1.名词解释 2.2.其它启发函数 二.A* 算法matlab完 ...
- 【SLAM建图和导航仿真实例】(二)- 根据已知地图进行定位和导航
引言 在这个-SLAM建图和导航仿真实例-项目中,主要分为三个部分,分别是 (一)模型构建 (二)根据已知地图进行定位和导航 (三)使用RTAB-MAP进行建图和导航 该项目的slam_bot已经上传 ...
最新文章
- jquery easyui datagrid 排序列
- Python 3.x 引入了函数注释
- 版本管理工具 (git 或者 svn)
- linux系统in命令,Linux中的In命令
- flex 只有仅限于文件系统的 SWF 文件和可信的本地 SWF 文件可以访问本地资源
- Java String 类的方法
- 当有脚本错误时,停止运行。
- layout_gravity 属性和 gravity属性的区别
- 读书篇:《细说PHP》三、PHP的语言结构
- arm linux usb打印机,嵌入式Linux系统中打印机驱动的移植
- vue做混合式app_基于vue2.0开发混合app的思考
- 【云栖大会精华汇】2017杭州云栖大会主论坛、分论坛在内的100+视频分享
- 爬取Google Play中app的用户评论(1)
- 刺沙冰雪高爆版辅助多开方案
- ijkplayer源码---倍速
- 基因数据处理44之cloud-scale-bwamem安装
- 蓝牙外设配对时输入密码的三种状态说明
- 软件测试(基础知识)
- 手机控必备网站,让自己拥有更好的智能手机
- spring boot 集成socketIo 做消息推送