第九章、模型替换以及TEB调参技巧

1、前言

上节内容让小车在gazebo仿真环境中实现导航，本节内容重点介绍一下gazebo仿真中的模型替换方法以及TEB调参的技巧。

2、模型替换

阿克曼小车项目从第四章开始完全使用的是自己写的模型，当然，用方块与圆柱构建的小车看上去自然不会太过于美观，那是否可以用其他精致的模型来替代方块圆柱呢？答案是肯定的，URDF支持加载STL与dae格式的模型文件。可以用第一章从SW中导出的tianracer模型来做一个替换。将第一章中的tianracer_description/meshes文件夹拷贝至racebot_description路径下，以替换base_link模型为例修改xacro文件中base_link的visual标签下的geometry标签，添加模型地址，以及在origin标签修改一下位置，代码如下：

<link name="base_link">
        <visual>
            <geometry>
                <!-- <box size="0.28 0.1 0.03"/> -->
                <mesh filename="package://racebot_description/meshes/base_link.STL" />
            </geometry>
            <origin xyz="-0.13 0 0" rpy="0 0 0" />            
            <material name="yellow">
                <color rgba="0.8 0.3 0.1 0.5" />
            </material>
        </visual>
        <collision>
            <geometry>
                <box size="0.28 0.1 0.03" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>
    </link>

不要去动collision标签，如果改动则会影响之前计算的惯性参数，复杂模型的惯性参数并不好计算，因此只在visual标签上做改动做视觉上的模型替换。用同样的方法对其它模型做替换，打开rviz查看模型：

发现模型已经替换完成。如果位置有些许对不上，可以通过visual标签中的origin子标签中进行调整。打开gazebo查看模型：

至此模型替换完成，是否比之前的方块圆柱更为美观呢？

3、TEB调参技巧

博主之前参加过大学生智能车竞赛ROS组别，当时用的TEB本地规划器让小车跑图，因此对于TEB调参有些许心得，当时也参考了其它大佬的博客，下面就来介绍一下TEB调参的一些技巧。

inflation_layer:
    enabled:              true
    cost_scaling_factor:  10.0  # exponential rate at which the obstacle cost drops off (default: 10)
    inflation_radius:     0.85  # max. distance from an obstacle at which costs are incurred for planning paths.

先来说说膨胀系数，如上代码所示，本来膨胀系数是放在costmap_common_params中的，但是在调参的过程中，需要修改local_costmap的膨胀系数，因此需要将上述代码提出并且分别放入全局和局部代价地图文件中。全局代价地图的膨胀系数可以在保持默认设置以便于全局的路径是一条较为平稳的路径，可以将局部代价地图的膨胀系数调小，车子在转弯的过程中路径会更短，其真实的路径会更加贴着弯道走。

# Robot
  max_vel_x: 2.5
  max_vel_x_backwards: 1
  max_vel_theta: 3.5
  acc_lim_x: 2.5
  acc_lim_theta: 3.5
  #仅适用于全向轮
  # max_vel_y (double, default: 0.0)  
  # acc_lim_y (double, default: 0.5)
  # ********************** Carlike robot parameters ********************
  min_turning_radius: 0.45     # 最小转弯半径 注意车辆运动学中心是后轮中点
  wheelbase: 0.26                 # 即前后轮距离
#设置为true时，ROS话题（rostopic） cmd_vel/angular/z 内的数据是舵机角度，
  cmd_angle_instead_rotvel: True 
# GoalTolerance
  footprint_model:
    type: "polygon"
    vertices: [[0.15, 0.15], [0.15, -0.15], [-0.15, -0.15], [-0.15, 0.15]]    
  xy_goal_tolerance: 0.1
  yaw_goal_tolerance: 1.0

上述代码是关于机器人控制的参数，这些会影响车的行驶效果。max_vel_x和acc_lim_x是速度与加速度的约束，整个车模运行过程中的加速过程和减速过程都会受这两个参数的制约。下面的角速度和角加速度约束也是同理，制约舵机转向的速度和加速度，左右转动的过程也都受这个限制。但是不代表你限制高了，转向的速度就快了，还有高速的转向也可能会带来振荡！max_vel_x_backwards是最大倒车速度，这个参数是不能设置为0或者负数的，否则会导致错误，TEB是不能够完全禁止倒车的，只能把倒车的惩罚提高，也就是提高前进的权重，但是不管权重多高，当你参数不合适，无法规划出前进的决策时，还是会出现停滞或者倒车的结果的。min_turning_radius就是最小转向半径了，这个参数还是比较重要的！可以大致估算，不过转向半径可以按需设置，转向半径如果大了，U形弯就会外道入弯，能比较轻松过弯，不过小弯道的时候也会贴外侧，这样会比较浪费时间，但是如果小了，小弯会贴内，节省时间，但是U形弯就会内侧入弯，那就有可能过不了弯了。所以需要自己调节一下。wheelbase是驱动轴和转向轴之间的距离，根据车模实际情况设置，可以在URDF中进行查看计算。cmd_angle_instead_rotvel是是否将收到的角速度消息转换为操作上的角度变化。设置成True时，话题cmd_vel.angular.z内的数据是舵机角度。Yaw_goal_tolerance代表了终点位姿容忍度适当调大可以让车子到达终点以后不用再花时间调整位姿，可缩短总体用时。

min_obstacle_dist: 0.2 # 与障碍的最小期望距离,
  include_costmap_obstacles: True #应否考虑到局部costmap的障碍设置为True后才能规避实时探测到的、建图时不存在的障碍物。
  costmap_obstacles_behind_robot_dist: 2.0 #考虑后面n米内的障碍物2.0
  obstacle_poses_affected: 30 #为了保持距离，每个障碍物位置都与轨道上最近的位置相连。
  costmap_converter_spin_thread: True
  costmap_converter_rate: 5

上面这部分是关于障碍物的参数。min_obstacle_dist是与障碍物的最小期望距离，米为单位。按需设置即可，如果你定位够精准，0.1m也是不会碰撞的，不过由于控制过程中速度的变化，以及里程计一定范围内的漂移，所以稍微设置大一些是必要的。costmap_obstacles_behind_robot_dist是考虑后方n米范围内的障碍物，设置的越大，考虑范围越广，不过相对而言计算量就越大，不过不能超过局部规划的区域，所以按需设置。

# Optimization     
  no_inner_iterations: 5
  no_outer_iterations: 4
  optimization_activate: True
  optimization_verbose: False
  penalty_epsilon: 0.1
  weight_max_vel_x: 2
  weight_max_vel_theta: 1
  weight_acc_lim_x: 1
  weight_acc_lim_theta: 2 #1
  weight_kinematics_nh: 1000
  weight_kinematics_forward_drive: 0.1 #1
  weight_kinematics_turning_radius: 1 #1
  weight_optimaltime: 1
  weight_obstacle: 10 #50
  weight_dynamic_obstacle: 10 # not in use yet
  alternative_time_cost: False # not in use yet
  selection_alternative_time_cost: False

上面这些参数是优化器的参数，都是对一些决策的权重设置。从penalty_epsilon开始，这个参数会为速度的约束提供一个缓冲的效果，就是在到达速度限制前会产生一定的惩罚让其提前减速达到缓冲的效果。weight_max_vel_x这个是最大速度的权重，后面还有最大角速度权重，以及加速度，角加速度权重，在整个运动过程中以主要以高速还是低速运行，则是看这些权重的分配。weight_kinematics_forward_drive这个参数就是倒车相关了，不过这个参数的意思是迫使机器人只选择前进的方向，也就是权重越大，倒车惩罚越大。这个权重的范围是0-1000，按需设置，不过即便是设置成1000也没有办法完全禁止停滞，倒车的效果。因为是否能前进，不仅仅只是看这个参数。还需要看转向半径，当前速度，等等一系列参数的决策。weight_kinematics_turning_radius是机器人最小转向半径的权重，越大则越容易达到最小转向半径的情况。范围同样是0-1000，按需设置。weight_optimaltime这这个参数是最优时间权重，如果大了，那么车会在直道上快速加速，并且路径规划的也会切内道，若是DWA算法只能靠膨胀缩小来实现的。这个参数越小，整个过程的车速会越稳定。我试过了0.5和100这两个值，差距还是蛮明显的，各位可以自己试试看体会一下，范围同样是0~1000。剩下的权重参数我就都没怎么调了，按默认参数即可，不过也可以尝试了解之后再去调试。weight_obstacle和weight_dynamic_obstacle是关于前方遇到静态障碍物与动态障碍物的权重，若参数越小，则前方障碍物对小车的前进影响越小，反之影响越大，可按需设置，范围都是0-1000。

local_costmap:
  # global_frame: odom
  global_frame: map
  robot_base_frame: base_footprint
  transform_tolerance: 0.5
  update_frequency: 15.0
  publish_frequency: 10.0
  rolling_window: true
  width: 15
  height: 15
  resolution: 0.05

接下来介绍一个有趣的参数，在local_costmap中，witdth和height代表了小车的探测范围，我如果将这两个数值调小，小车会基本沿着全局规划路径走，而且走得比较稳，速度也相对更慢，如果将数值调大，则由于前边设置权重的原因，小车会变得畏手畏脚，停滞不前，选择合理参数利于缩短用时。

4、使用rqt_reconfigure实现动态调参

使用命令：

rosrun rqt_reconfigure rqt_reconfigure

打开动态调参窗口，对teb参数进行动态调节，当调节完后，可以将参数记下自行修改。

5、小结

至此阿克曼小车gazebo仿真第一阶段收尾，下一章将该项目开源。

参考资料

1.古月老师的《ROS机器人开发实践》

2.智能车竞赛——TEB轨迹规划算法的参数调试：https://www.guyuehome.com/9811