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

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
  • 或者修改配置文件

纠错,疑问,交流: 请进入讨论区点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: teb轨迹规划算法教程