path_tracking_pid

概述

Path Tracking PID 提供一个可调节的 PID 控制回路，将转向和前向速度分离开来。前向速度通过使用目标速度和加速度以开放回路的方式生成。

其中一个跟踪选项使用一个长度为 l 的虚拟点（carrot）在机器人前方，以当前全局点（GP）和控制点（CP）之间的横向误差确定转向动作：

如果提供的是平滑路径，控制器可以选择直接用 base_link 跟踪路径，而不是滞后于虚拟点。在这种情况下，还会计算一个投影全局点（PGP），由 CP 跟踪。在这种模式下，偏航误差也可以作为控制输入。

PID 包含两个闭环：横向和角度闭环，以及一个开放环：纵向闭环。对于非全向机器人，横向和角度闭环可以结合来计算转向动作。

模型预测控制

作为一个可选功能，我们使用模型预测控制（MPC）概念来调节前向速度。用户可以指定跟踪路径的最大期望误差范围。使用机器人的模型和控制器设置，计算出一个预测路径，如果误差超出范围，机器人的前向速度将减小，直到预测路径在可接受的范围内。

三轮车模型

三轮车模型也受支持。

假设前轮被操纵和驱动。转向轮链接（SL）可以相对于 base_link（BL）非对称地放置。转向和驱动的限制也会被考虑。

防碰撞

作为一个插件使用时，Path Tracking PID 使用 costmap（和来自 costmap 的机器人轮廓）进行防碰撞。不进行规避动作（坚持路径是关键）。但是当检测到障碍物靠近机器人或在机器人行进方向上时，前向速度会减小。这会在障碍物周围产生更安全的速度。如果无法避免障碍物或障碍物离机器人太近，将停止并取消导航。

机器人的最大速度将随 costmap 中机器人路径周围或路径中的值线性缩放。可以使用膨胀层（甚至社交层）在 costmap 中构建这些梯度。

关键词：跟踪、pid、本地规划器、轨迹、模型预测控制

许可证

待定

作者：Cesar Lopez, cesar.lopez@nobleo.nl

维护者：Cesar Lopez, cesar.lopez@nobleo.nl

隶属：Nobleo Projects

该 path_tracking_pid 包在 ROS Melodic 和 Ubuntu 18.04 下进行了测试。

安装

从源代码构建 依赖项

• 机器人操作系统（ROS）（机器人中间件）

构建 从该存储库克隆最新版本到您的 catkin 工作区并使用以下命令编译该包：

cd catkin_workspace/src 
git clone https://bitbucket.org/nobleo/path_tracking_pid.git
cd ../
catkin_make

单元测试

使用以下命令运行单元测试：

catkin run_tests path_tracking_pid

用法

path_tracking_pid 是 move_base_flex 的一个插件。

要使用 path_tracking_pid 插件运行 move_base_flex：

roslaunch path_tracking_pid path_tracking_pid_mbf.launch

静态参数

• base_link_frame（字符串，默认值：base_link） 基链接框架的名称。
• holonomic_robot（布尔值，默认值：false） 对于全向机器人为真。 --> 未维护，预期存在错误
• estimate_pose_angle（布尔值，默认值：false） 是否直接从路径获取姿态角或从连续的路径姿态估算它们。
• use_tricycle_model（布尔值，默认值：false） 使用三轮车模型而不是差速驱动时为真。
• steered_wheel_frame（字符串，默认值：steer） 转向轮框架的名称。
• use_mpc（布尔值，默认值：false） 使用 MPC 来调节 x 速度时为真。

在 RQT 中配置

tracking_pid 参数都可以通过（rqt_）dynamic_reconfigure 获取。主要参数有：

• l（双精度数，默认值：0.5） 从机器人的旋转点到轨迹的跟随距离。
• track_base_link（布尔值，默认值：false） 是否使用 base_link 而不是前方的控制点跟踪路径。需要平滑路径。
• init_vel_method（枚举，默认值：1） 选择新路径初始速度的方法。
• init_vel_max_diff（双精度数，默认值：0.5） 启动新路径时可接受的最大速度差异。如果内部状态和当前里程计的速度差异超过此值，路径将被中止。设置为 -1 忽略此检查。此检查仅在 'init_vel_method' == InternalSetpoint 时激活。
• 横向和角度闭环的比例、积分和微分动作。
  • Kp_lat（双精度数，默认值：1.0） 横向闭环的比例动作增益。
  • Ki_lat（双精度数，默认值：0.0） 横向闭环的积分动作增益。
  • Kd_lat（双精度数，默认值：0.3） 横向闭环的微分动作增益。
  • Kp_ang（双精度数，默认值：1.0） 角度闭环的比例动作增益。
  • Ki_ang（双精度数，默认值：0.0） 角度闭环的积分动作增益。
  • Kd_ang（双精度数，默认值：0.3） 角度闭环的微分动作增益。
• 每个闭环可以单独启用/禁用。
  • feedback_lat（布尔值，默认值：true） 启用横向闭环反馈。
  • feedback_ang（布尔值，默认值：false） 启用角度闭环反馈。
• 此外，可以启用/禁用使用轨迹速度的前馈。
  • feedforward_lat（布尔值，默认值：true） 启用横向闭环的速度前馈。
  • feedforward_ang（布尔值，默认值：false） 启用角度闭环的速度前馈。
• 用于生成开放环前向速度的目标速度和加速度：
  • target_x_vel（双精度数，默认值：2.0） 标称目标前向速度。
  • target_end_x_vel（双精度数，默认值：0.0） 路径结束时的目标前向速度。
  • target_x_acc（双精度数，默认值：2.0） 路径开始时的期望加速度。
  • target_x_decc（双精度数，默认值：2.0） 路径结束时的期望减速度。
• 生成速度的约束：
  • abs_minimum_x_vel（双精度数，默认值：0.025） 车辆的最小速度，用于到达路径的终点。
  • max_error_x_vel（双精度数，默认值：1.0） 最大允许的 x 速度误差。
  • max_yaw_vel（双精度数，默认值：2.0） 最大允许的偏航速度。
  • max_yaw_acc（双精度数，默认值：2.0） 最大允许的偏航加速度。
  • min_turning_radius（双精度数，默认值：0.0） 车辆的最小转弯半径。
• 三轮车模型的转向轮约束：
  • max_steering_angle（双精度数，默认值：3.1416） 三轮车模型的最大转向角度。
  • max_steering_x_vel（双精度数，默认值：3.0） 三轮车模型的最大转向 x 速度。
  • max_steering_x_acc（双精度数，默认值：2.0） 三轮车模型的最大转向 x 加速度。
  • max_steering_yaw_vel（双精度数，默认值：0.5） 三轮车模型的最大转向偏航速度。
  • max_steering_yaw_acc（双精度数，默认值：0.5） 三轮车模型的最大转向偏航加速度。
• 防碰撞参数：
  • collision_look_ahead_length_offset（双精度数，默认值：1.0） 沿路径投影矩形碰撞的长度偏移量。
  • collision_look_ahead_resolution（双精度数，默认值：1.0） 沿路径投影矩形碰撞的空间分辨率。
• 调试主题启用：
  • controller_debug_enabled（布尔值，默认值：false） 启用调试主题。
• 配置 MPC 行为的参数：
  • mpc_max_error_lat（双精度数，默认值：0.5） MPC 最大允许横向误差。
  • mpc_min_x_vel（双精度数，默认值：0.5） MPC 最小绝对前向速度。
  • mpc_simulation_sample_time（双精度数，默认值：0.05） MPC 模拟采样时间。
  • mpc_max_fwd_iterations（整数，默认值：200） 预测迭代次数。总模拟时间为 mpc_max_fwd_iterations*mpc_simulation_sample_time。
  • mpc_max_vel_optimization_iterations（整数，默认值：5） MPC 最大允许速度优化迭代次数。

启动文件

• launch/path_tracking_pid_mbf.launch：使用 path_tracking_pid 插件运行 move_base_flex。
• test/test_path_tracking_pid.launch：如果一切正常，一个红色方块会沿着覆盖正方形的锯齿路径移动。base_link 框架应该跟踪该方块。

插件

• path_tracking_pid/TrackingPidLocalPlanner：用于 move_base_flex。查看 launch/path_tracking_pid_mbf.launch 以查看示例。

订阅主题

• path（[nav_msgs/Path]） 要跟踪的路径。
• odom（[nav_msgs/Odometry]） 机器人里程计。
• /tf（[tf2_msgs/TFMessage]） 机器人相对于接收地图的框架的位置。
• vel_max（[std_msgs/Float64]） 动态最大速度。例如当达到最大功率需求时使用。高级软件也可以使用重新配置界面来设置新的速度值，但在需要反馈循环时，推荐使用这个流主题。

发布主题

• cmd_vel（[geometry_msgs/Twist]） 机器人跟踪所需路径的扭矩。
• feedback（[path_tracking_pid/PidFeedback]） 估计剩余时间和到最终目标的进度。
• visualization_marker（[visualization_msgs/Marker]） 指示当前控制目标的标记。
• collision_footprint_marker（[visualization_msgs/Marker]） 沿路径用于前方碰撞检测的轮廓标记。
• debug（[path_tracking_pid/PidDebug]） PID 控制器的中间值。默认情况下主题是静默的，可以通过动态重新配置启用。

错误和功能请求

请使用问题跟踪器报告错误和请求功能。