TEB轨迹规划算法教程-阿克曼车型路径规划

TEB轨迹规划算法教程-阿克曼车型路径规划

说明：

• 介绍如何为类似汽车的机器人设置规划器（实验）

原理：

• 导航包规划和导航不适用于类似汽车的机器人。

• 但是teb_local_planner试图通过提供适用于ackermann驱动器的本地规划来克服此限制。

• 这是通过将非完整约束延伸到转弯半径resp上的最小界限来实现的。

• 通过满足v/omega > r_min。

• teb_local_planner必须确实遵守导航包的规范，通过提供包含平移和角速度v和omega的geometry_msgs/Twist消息

• 分别用于命令机器人而不是提供ackermann_msgs/AckermannDriveStamped消息。

• 有关详细信息，请参阅命令接口部分。

• 最小转弯半径由ros参数〜/min_turning_radius设置（参见Node-Api）。

• 您可以将值设置得稍大，因为在优化（惩罚）中使用软约束执行边界。

• 请注意，必须关闭或替换导航包提供的backup/escape行为，因为它们不支持类似汽车的机器人。

• 类似汽车的机器人设置需要向后驱动的能力。

• 因此，如果最小转弯半径不为零，则忽略参数〜/weight_kinematics_forward_drive。

• 支持类似汽车的机器人/阿克曼驱动器仍处于试验阶段。

检查优化的轨迹

• 在设置用于导航的机器人之前，您可能想要尝试查看规划器如何优化类似汽车的机器人的轨迹。
• 重复优化教程设置和测试优化，并使用rqt_reconfigure调整参数〜/min_turning_radius。
• 看看发生了什么。

命令接口

• 本教程区分了与机器人硬件节点通信的不同类型的接口。

平移和角速度

• 这种硬件驱动程序接受机器人的平移和角速度。
• 在前轮驱动的情况下，通常在后轴的中心处限定速度。
• 对于此类型，可以直接使用已发布的geometry_msgs/Twist消息。

平移速度和转向角

• 包括平移速度v和转向角φ的相关变量如下图所示。
• ICR表示即时旋转中心。

• 对于消失的角速度ω= 0，转弯半径r趋于无穷大，这又导致零转向角phi = 0。
• 否则转弯半径r可能由v/omega计算。
• 然后，转向角由phi = atan（轴距/ r）导出（其确实包括第一种情况）。
• 注意，使用上面介绍的等式，没有为v = 0定义转向角。
• 可以使用最后已知的有效转向角。
• 然而，在下文中，我们假设对于消失的平移速度，将（期望的）转向角设定为零。

Twist消息

• 某些命令接口（例如类似汽车模式的stage模拟器）需要geometry_msgs/Twist，但语义已更改。
• 角速度（z轴）被解释为转向角。
• 注意，通常不优选更改消息的语义，如果可能，最好切换到ackermann_msgs接口。

Ackermann消息

• 如果机器人硬件节点接受ackermann_msgs包提供的ackermann_msgs/AckermannDriveStamped消息，则必须转换速度命令。
• 一种可能的方法是添加一个小的转换器脚本/节点，将geometry_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

• 在例程里，可以执行

rosrun teb_local_planner_tutorials cmd_vel_to_ackermann_drive.py

机器人足迹模型

• 类似汽车的机器人通常构成非圆形形状/轮廓。
• 因此，您可能需要仔细查看教程避障和机器人足迹模型。

完整例子：

• 安装例程，查看第一篇
• 检查参数和配置
• 启动脚本：

roslaunch teb_local_planner_tutorials robot_carlike_in_stage.launch

• 修改参数：

rosrun rqt_reconfigure rqt_reconfigure

• 或者修改配置文件