ros中的速度平滑处理和yocs_smoother_velocity介绍及关于全向扩展包实现

概述

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，详细可以查看下官方的说明
    
    

配置

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

这是我的配置

全向扩展支持

概述

全向的需要一个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即可