概述
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即可
