局部路径规划器teb_local_planner详解5：关于 robots models

前言

局部路径规划器teb_local_planner详解4：关于代价地图中，我们介绍了如何使用costmap_converter

本章，我们来详细看下，对于不同的robot model ，teb是如何进行规划的。

car-like robots

一、轨迹优化项

添加non-holomomic约束：限制最大转向角

重要参数：

• ~<name>/min_turning_radius : 最小转弯半径，最好稍微大点，因为最后会转换为软约束
  相关说明：

• <name>/weight_kinematics_forward_drive: 前向运动学权重，在car-like robot中失效

二、控制命令接口

1. geometry_msgs/Twist message

数据格式是机器人的线速度和角速度。

在前轮驱动的情况下，速度通常定义在后桥的中心。

这种情况，twist直接用。

2. ackemann

数据格式是线速度v和方向盘转角phi 如下图所示：

重要参数

• ~<name>/cmd_angle_instead_rotvel 设置为true的话，我们使用的是方向盘转角而非旋转角速度。
• ~<name>/wheelbase轮间距同样要设定好，配合angle使用。单位为m

如果机器人底盘接收的是ackermann_msgs而非twist， 我们需要写个简单脚本来实现转换：

#!/usr/bin/env python

import rospy, math
from geometry_msgs.msg import Twist
from ackermann_msgs.msg import AckermannDriveStamped

def convert_trans_rot_vel_to_steering_angle(v, omega, wheelbase):
  if omega == 0 or v == 0:
    return 0

  radius = v / omega
  return math.atan(wheelbase / radius)


def cmd_callback(data):
  global wheelbase
  global ackermann_cmd_topic
  global frame_id
  global pub
  
  v = data.linear.x
  steering = convert_trans_rot_vel_to_steering_angle(v, data.angular.z, wheelbase)
  
  msg = AckermannDriveStamped()
  msg.header.stamp = rospy.Time.now()
  msg.header.frame_id = frame_id
  msg.drive.steering_angle = steering
  msg.drive.speed = v
  
  pub.publish(msg)
  

if __name__ == '__main__': 
  try:
    
    rospy.init_node('cmd_vel_to_ackermann_drive')
        
    twist_cmd_topic = rospy.get_param('~twist_cmd_topic', '/cmd_vel') 
    ackermann_cmd_topic = rospy.get_param('~ackermann_cmd_topic', '/ackermann_cmd')
    wheelbase = rospy.get_param('~wheelbase', 1.0)
    frame_id = rospy.get_param('~frame_id', 'odom')
    
    rospy.Subscriber(twist_cmd_topic, Twist, cmd_callback, queue_size=1)
    pub = rospy.Publisher(ackermann_cmd_topic, AckermannDriveStamped, queue_size=1)
    
    rospy.loginfo("Node 'cmd_vel_to_ackermann_drive' started.\nListening to %s, publishing to %s. Frame id: %s, wheelbase: %f", "/cmd_vel", ackermann_cmd_topic, frame_id, wheelbase)
    
    rospy.spin()
    
  except rospy.ROSInterruptException:
    pass

holonomic robot

重要参数：

• max_vel_y: 对于non-holonomic robots而言，这个数值必须设置为0. 现在可以设个正数
• acc_lim_y: 同上，y轴加速度
• weight_kinematics_nh: 面向non-holonomic robots的运动学约束的权重。现在可以降低该权重，1就可以