做速度平滑处理的背景
ROS 导航模块move_base 输出的/cmd_vel topic指定了为机器人规划的线速度和角速度, 但是这个输出值还是不够友好导致机器人运动不够流畅,这就需要对这个输出速度值进行一个平滑的过程。ROS中的yocs_smoother_velocity是一个非常好的速度插值的包, 可以对速度、加速度进行限制,用来防止机器人的速度、转速变化过快或过慢, 是其运行平滑流畅。
yocs_smoother_velocity功能包解析
- 订阅的话题
(1)〜raw_cmd_vel(geometry_msgs / Twist)
要平滑的输入速度。
(2)〜odometry (nav_msgs /Odometry )
里程计数据,这个可以是直接的里程计,也可以是经过位姿估计调整后的里程计。
(3)〜robot_cmd_vel(geometry_msgs / Twist)
这个数据一般是base_controller实际发送给机器人电机的速度值,把这个值发布出来,yocs_smoother_velocity可以参考这个值避免大的数据波动。
- 发布的话题
(1)〜smooth_cmd_vel(geometry_msgs / Twist)
根据速度和加速度极限的平滑输出相应的速度。不使用yocs_smoother_velocity,base controller的输入是move_base的输出/cmd_vel;使用速度平滑处理,base controller的输入是/smooth_cmd_vel。
如何使用yocs_smoother_velocity
下载链接(melodic):https://gitee.com/zhankun3280/yujin_ocs/tree/release%2F0.8-melodic/
需要修改3个文件:
- yocs_velocity_smoother/param文件夹下的参数配置文件
standalone.yaml
- yocs_velocity_smoother/launch文件夹下的launch文件
standalone.launch
- 机器人控制节点base_controller的启动文件
- standalone.yaml参数配置文件
# Mandatory parameters
speed_lim_v: 0.5 # 线速度的最大值
speed_lim_w: 2.0 # 角速度的最大值
accel_lim_v: 0.55 # 线加速度的最大值
accel_lim_w: 2.2 # 角加速度的最大值
# Optional parameters
frequency: 30.0 # 输出速度的频率,不管输入的数据频率,velocity smoother会保持这个频率发布数据
decel_factor: 1.0 # 减速/加速度比,对于惯性较大可以提高该值
# Robot velocity feedback type:
# 0 - none
# 1 - odometry
# 2 - end robot commands
robot_feedback: 2 # 机器人对此发布数据的反馈
根据自己的机器人小车实际情况调整。上述是我的配置情况。
- standalone.launch平滑处理启动文件
<launch>
<arg name="node_name" value="velocity_smoother"/>
<arg name="nodelet_manager_name" value="nodelet_manager"/>
<arg name="config_file" value="$(find yocs_velocity_smoother)/param/standalone.yaml"/>
<arg name="raw_cmd_vel_topic" value="/lp_robot/cmd_vel"/>
<arg name="smooth_cmd_vel_topic" value="smooth_cmd_vel"/>
<arg name="robot_cmd_vel_topic" value="smooth_cmd_vel"/>
<arg name="odom_topic" value="odom"/>
<!-- nodelet manager -->
<node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager"/>
<!-- velocity smoother -->
<include file="$(find yocs_velocity_smoother)/launch/velocity_smoother.launch">
<arg name="node_name" value="$(arg node_name)"/>
<arg name="nodelet_manager_name" value="$(arg nodelet_manager_name)"/>
<arg name="config_file" value="$(arg config_file)"/>
<arg name="raw_cmd_vel_topic" value="$(arg raw_cmd_vel_topic)"/>
<arg name="smooth_cmd_vel_topic" value="$(arg smooth_cmd_vel_topic)"/>
<arg name="robot_cmd_vel_topic" value="$(arg robot_cmd_vel_topic)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
</launch>
需要修改3处
<arg name="raw_cmd_vel_topic" value="/lp_robot/cmd_vel"/>
<arg name="smooth_cmd_vel_topic" value="smooth_cmd_vel"/>
<arg name="robot_cmd_vel_topic" value="smooth_cmd_vel"/>
"raw_cmd_vel_topic"
是平滑处理前订阅的速度话题(也就是yocs_velocity_smoother包的输入);"smooth_cmd_vel_topic"
是经过平滑处理后要发布给底盘的速度话题(也就是yocs_velocity_smoother包的输出);"robot_cmd_vel_topic"
是机器人的实际速度话题,因为机器人当前的速度是不知道的,所以这里smooth_cmd_vel_topic和robot_cmd_vel_topic写成了同一个话题名,意思是机器人的当前速度就是平滑处理后的速度。
注意:话题名需要修改成自己的,同时还要和机器人控制节点话题对应。
- 机器人控制节点base_controller
我这里的速度话题流向是move_base规划路径,发布一个/cmd_vel话题,经过超声波避障会订阅这个话题,发布一个经过避障处理的/lp_robot/cmd_vel话题。
然后,平滑处理的速度就是将超声波避障发布的速度做处理,所以订阅的是/lp_robot/cmd_vel话题,发布的是/smooth_cmd_vel话题,最后要发布给机器人底盘,控制机器人运动。
所以在机器人控制节点的launch文件中(或者是总的launch文件中),要将发布给底盘的/cmd_vel话题remap成平滑处理后的smooth_cmd_vel话题。
<!--robot-->
<node pkg="base_controller" type="base_controller" name="base_controller" output="screen">
<!--若加超声波避障,需要将/cmd_vel remap成/lp_robot/cmd_vel-->
<!--remap from="/cmd_vel" to="/lp_robot/cmd_vel" /-->
<remap from="/cmd_vel" to="/smooth_cmd_vel" />
</node>
注意,你的机器人控制节点可能不是base_controller,根据自己的实际情况修改即可。我是在总的launch启动文件下remap的,dwa_loop_move_base_simulation_object_zhan_test.launch。
注意事项
- 平滑包参数,速度最大值和加速度的对应关系一般为1:1.5(是我这么认为),比如speed_lim_v是0.5,则accel_lim_v为0.75,这样可以较平稳的控制小车。
- 要注意yocs_velocity_smoother包中使用了动态参数配置的功能,在yocs_velocity_smoother/cfg文件夹下的params.cfg文件如下。注意standalone.yaml配置文件中的速度和加速度参数不要超过cfg文件中动态参数的最大值。
- 对x方向的vel、acc进行约束;对vel进行平滑,防止vel突变引起机器人抖动;vel平滑效果比ros自带的平滑效果更好。
参考链接:
https://blog.csdn.net/lclfans1983/article/details/105444059
https://blog.csdn.net/windxf/article/details/109671898
功能拓展
此外,如果有多个节点要想控制机器人的运动,多路复用器(cmd_vel_mux)允许平行使用所有这些命令,并且可以设置优先级,优先级高的会抢占优先级低的任务。
可以参考如博客:yocs_cmd_vel_mux和yocs_velocity_smoother
下载链接(melodic):https://gitee.com/zhankun3280/yujin_ocs/tree/release%2F0.8-melodic/yocs_cmd_vel_mux
launch文件里将yocs_cmd_vel_mux/mux_cmd_vel输给yocs_velocity_smoother/raw_cmd_cmd进行vel约束和平滑。