ROS小车基于yocs_smoother_velocity做速度平滑处理

做速度平滑处理的背景

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的启动文件

1. 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    # 机器人对此发布数据的反馈

根据自己的机器人小车实际情况调整。上述是我的配置情况。

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写成了同一个话题名，意思是机器人的当前速度就是平滑处理后的速度。

注意：话题名需要修改成自己的，同时还要和机器人控制节点话题对应。

3. 机器人控制节点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：1.5（是我这么认为），比如speed_lim_v是0.5，则accel_lim_v为0.75，这样可以较平稳的控制小车。
2. 要注意yocs_velocity_smoother包中使用了动态参数配置的功能，在yocs_velocity_smoother/cfg文件夹下的params.cfg文件如下。注意standalone.yaml配置文件中的速度和加速度参数不要超过cfg文件中动态参数的最大值。
3. 对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约束和平滑。