ROS-已知地图的无人机路径规划算法仿真
现在,基于这张建立好的地图进行路径规划算法仿真。
一、创建功能包
在工作空间下创建功能包:
-
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
-
-
-
update_frequency: 10.0
-
-
publish_frequency: 10.0
-
-
transform_tolerance: 0.5
-
-
-
static_map: true
本地代价地图配置文件:local_costmap_params.yaml
-
local_costmap:
-
global_frame: map
-
robot_base_frame: base_link
-
-
update_frequency: 10.0
-
publish_frequency: 10.0
-
transform_tolerance: 0.5
-
-
static_map: false
-
# 设置在机器人移动过程中是否需要滚动窗口,以保持机器人处于中心位置
-
rolling_window: true
-
# width、height和resolution用于设置代价地图的长(米)、高(米)和分辨率(米/格)
-
width: 3
-
height: 3
-
resolution: 0.05
move_base配置文件:move_base_params.yaml
-
-
shutdown_costmaps: false
-
-
controller_frequency: 2.0
-
-
controller_patience: 15.0
-
-
-
planner_frequency: 5.0
-
-
planner_patience: 5.0
-
-
-
conservative_reset_dist: 3.0
-
-
-
oscillation_timeout: 10.0
-
-
oscillation_distance: 0.2
-
-
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 Parameters
-
max_vel_x: 0.2 # x方向最大线速度绝对值,单位:m/s
-
min_vel_x: -0.2 # x方向最小线速度绝对值,负数代表可后退,单位:m/s
-
-
max_vel_y: 0.2 # y方向最大线速度绝对值,单位:m/s
-
min_vel_y: -0.2 # y方向最小线速度绝对值,负数代表可后退,单位:m/s
-
-
max_vel_trans: 0.3 # 机器人最大平移速度的绝对值,单位为:m/s
-
min_vel_trans: 0.01 # 机器人最小平移速度的绝对值,单位为:m/s(不可为零)
-
-
max_vel_theta: 0.1 # 机器人的最大旋转角速度的绝对值,单位为:rad/s
-
min_vel_theta: 0.01 # 器人的最小旋转角速度的绝对值,单位为:rad/s
-
-
acc_lim_x: 1 # 机器人在x方向的极限加速度,单位为:m/s^2
-
acc_lim_y: 1 # 机器人在y方向的极限加速度,单位为:m/s^2
-
acc_lim_theta: 1 # 机器人的极限旋转加速度,单位为:rad/s^2
-
-
# Goal Tolerance Parametes
-
xy_goal_tolerance: 0.05 # 到到目标点时,控制器在x和y方向上的容差(m)
-
yaw_goal_tolerance: 3.14 # 到达目标点时,控制器在偏航/旋转时的弧度容差(rad)
-
latch_xy_goal_tolerance: false # 设置为true时表示:如果到达容错距离内,机器人就会原地旋转
-
-
# Forward Simulation Parameters
-
sim_time: 2.0 # 前向模拟轨迹的时间,单位为s
-
vx_samples: 20 # x方向速度空间的采样点数
-
vy_samples: 20 # y方向速度空间的采样点数
-
vth_samples: 40 # 旋转方向的速度空间采样点数
-
controller_frequency: 2.0
-
-
# Trajectory Scoring Parameters
-
path_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/s
-
max_scaling_factor: 0.2 # 最大缩放因子
-
-
# Oscillation Prevention Parameters
-
oscillation_reset_dist: 0.05 # 机器人必须运动多少米远后才能复位震荡标记
-
# Debugging
-
publish_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”,然后在地图中任意位置点击并拖动鼠标确定方向,然后就可以看到地图上会自动显示一条绿色的轨迹,这是全局规划产生的路径,无人机坐标系附近还会有一条红色的轨迹,这是局部规划产生的路径。
至此,基于已知地图的无人机路径规划算法仿真就基本实现了。接下来还会进一步实现多个目标点的自动路径规划与导航实现。
这篇好文章是转载于:学新通技术网
- 版权申明: 本站部分内容来自互联网,仅供学习及演示用,请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系,请提供相关证据及您的身份证明,我们将在收到邮件后48小时内删除。
- 本站站名: 学新通技术网
- 本文地址: /boutique/detail/tanhgackia
-
photoshop保存的图片太大微信发不了怎么办
PHP中文网 06-15 -
Android 11 保存文件到外部存储,并分享文件
Luke 10-12 -
word里面弄一个表格后上面的标题会跑到下面怎么办
PHP中文网 06-20 -
《学习通》视频自动暂停处理方法
HelloWorld317 07-05 -
photoshop扩展功能面板显示灰色怎么办
PHP中文网 06-14 -
微信公众号没有声音提示怎么办
PHP中文网 03-31 -
怎样阻止微信小程序自动打开
PHP中文网 06-13 -
excel下划线不显示怎么办
PHP中文网 06-23 -
excel打印预览压线压字怎么办
PHP中文网 06-22 -
photoshop蒙版画笔没反应怎么办
PHP中文网 06-24