概述

ros中的规划层已经有加速度的限制

但是输出的还是不够友好导致机器人行走不流畅，那么就在速度接口这边就要执行一个平滑的过程。

turtlebot中给出了一个非常好的速度插值的包 yocs_velocity_smoother，但该包不支持y方向的线速度输出，导致无法使用在全向轮中(除非只希望发送x方向线速度)，本文介绍该包和一种扩展使得其能够支持全向的机器人

yocs_velocity_smoother

输入

改包订阅了3个topic, 显然如果我们的时间发出的不是这些名称，需要remap, 不过我们可以直接在这里找到一个模版
velocity_smoother.launch

• raw_cmd_vel 控制命令，通过键盘、遥控或者navigation layer发出的
• odometry 里程, 实际使用了其中的机器人的反馈速度
• robot_cmd_vel 机器人反馈速度

输出

输出较为简单，即我们想要得到的平滑的速度

参数

这里有线速度和角速度的速度限制值和加速度限制值

• decel_facotor 减速/加速度比，对于惯性较大可以提高该值
• frequency 输出速度的频率
• robot_feedback
  • COMMANDS(2) 使用raw_cmd_vel作为作为反馈
  • ODOMETRY(1) 使用odometry中的速度作为反馈速度
  • NONE(0) 忽略了任何机器人反馈
    一般建议使用为2，详细可以查看下官方的说明
    

配置

照着官方的配置还是比较方便

<!--
  YOCS velocity smoother launcher
  -->

<launch>
  <arg name="node_name"             default="velocity_smoother"/>
  <arg name="nodelet_manager_name"  default="nodelet_manager"/>
  <arg name="config_file"           default="$(find yocs_velocity_smoother)/param/standalone.yaml"/>
  <arg name="raw_cmd_vel_topic"     default="raw_cmd_vel"/>
  <arg name="smooth_cmd_vel_topic"  default="smooth_cmd_vel"/>
  <arg name="robot_cmd_vel_topic"   default="robot_cmd_vel"/>
  <arg name="odom_topic"            default="odom"/>

  <node pkg="nodelet" type="nodelet" name="$(arg node_name)"
        args="load yocs_velocity_smoother/VelocitySmootherNodelet $(arg nodelet_manager_name)">
        
    <!-- parameters -->
    <rosparam file="$(arg config_file)" command="load"/>

    <!-- velocity commands I/O -->
    <remap from="$(arg node_name)/raw_cmd_vel"    to="$(arg raw_cmd_vel_topic)"/>
    <remap from="$(arg node_name)/smooth_cmd_vel" to="$(arg smooth_cmd_vel_topic)"/>

    <!-- Robot velocity feedbacks -->
    <remap from="$(arg node_name)/robot_cmd_vel"  to="$(arg robot_cmd_vel_topic)"/>
    <remap from="$(arg node_name)/odometry"       to="$(arg odom_topic)"/>
  </node>
</launch>

这是我的配置

<launch>
    <arg name="node_name"             value="velocity_smoother"/>
    <arg name="nodelet_manager_name"  value="nodelet_manager"/>
    <arg name="config_file"           value="$(find alpha_bringup)/params/yocs_velocity_smoother.yaml"/>
    <arg name="raw_cmd_vel_topic"     value="cmd_vel"/>
    <arg name="smooth_cmd_vel_topic"  value="smoother_cmd_vel"/>
    <arg name="robot_cmd_vel_topic"   value="smoother_cmd_vel"/>
    <arg name="odom_topic"            value="odom"/> 
  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager"/> 
  <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>

全向扩展支持

概述

全向的需要一个y速度，但查看代码发现

根本没有y方向的输出,线速度只有x方向，怎么让他输出y, 修改源码，见到这一堆

      double MA = sqrt(    v_inc *     v_inc +     w_inc *     w_inc);
      double MB = sqrt(max_v_inc * max_v_inc + max_w_inc * max_w_inc);

      double Av = std::abs(v_inc) / MA;
      double Aw = std::abs(w_inc) / MA;
      double Bv = max_v_inc / MB;
      double Bw = max_w_inc / MB;
      double theta = atan2(Bw, Bv) - atan2(Aw, Av);

      if (theta < 0)
      {
        // overconstrain linear velocity
        max_v_inc = (max_w_inc*std::abs(v_inc))/std::abs(w_inc);
      }
      else
      {
        // overconstrain angular velocity
        max_w_inc = (max_v_inc*std::abs(w_inc))/std::abs(v_inc);
      }

      if (std::abs(v_inc) > max_v_inc)
      {
        // we must limit linear velocity
        cmd_vel->linear.x  = last_cmd_vel.linear.x  + sign(v_inc)*max_v_inc;
      }

      if (std::abs(w_inc) > max_w_inc)
      {
        // we must limit angular velocity
        cmd_vel->angular.z = last_cmd_vel.angular.z + sign(w_inc)*max_w_inc;
      }

果断从入门到放弃。

解决方案

总体思路

全向相对与差分的多个y方向的线速度，我们采用先合成在分解的方式，把vx，vy 合成为一个vxy作为yocs_velocity_smoother的输入，根据输入的比例分解输出的vxy分解为vx，vy即可