机器人 / 系统课时
按固定周期执行控制并处理延迟
按固定周期执行控制并处理延迟
纠偏公式算对了,底盘仍可能因控制回调不均匀、指令到达过晚或上游断流而摆动甚至继续行驶。本课以 ROS 2 Jazzy 的 20 Hz 航向控制器为例,建立时间预算、隔离输入与输出节奏,并验证双层超时停车。
学习目标
- 换算采样周期、指令延迟与多走的距离或多转的角度。
- 使用
rclpy定时器固定输出节奏,检测回调抖动和输入超时。 - 区分应用层零速命令与底盘驱动层命令看门狗,并完成断流验证。
用时序预算选择周期
设控制周期为 ,频率为 。周期决定控制器多久有机会更新一次命令;端到端延迟则由采样等待、计算、通信、驱动处理共同构成,不能用高发布频率抵消已经产生的延迟。
| 控制周期 | 频率 | 选用时的关注点 |
|---|---|---|
| 20 ms | 50 Hz | 纠偏更及时,回调与通信负担更高 |
| 50 ms | 20 Hz | 本例采用;一般轻量航向纠偏可从此试起 |
| 100 ms | 10 Hz | 可能使快速运动的修正滞后 |
以角速度 0.5 rad/s、底盘速度 0.2 m/s 为例,假设一条命令比计划晚生效 150 ms,则这段时间机器人约多转 rad(约 4.3°),或多行驶 m。150 ms 已相当于三个 50 ms 周期;提高比例增益可能加剧过冲,不能代替排查延迟。
用定时器隔离采样与输出
订阅回调只保存最新航向误差,50 ms 定时器负责计算并下发 TwistStamped。这样输入以 10 Hz 更新时,输出仍可维持约 20 Hz;不要在订阅回调里立即下发每一条命令。示例约定 /heading_error 是以弧度表示的有符号目标航向误差,由上游节点提供;Jazzy 版 diff_drive_controller 订阅 ~/cmd_vel 的 TwistStamped。
在已安装并加载 ROS 2 Jazzy 的环境中,将以下完整程序保存为 fixed_period_controller.py:
import math
import time
import rclpy
from geometry_msgs.msg import TwistStamped
from rclpy.node import Node
from std_msgs.msg import Float64
PERIOD = 0.05 # 20 Hz 控制周期
INPUT_TIMEOUT = 0.20 # 仅检测输入到达中断
class HeadingController(Node):
def __init__(self):
super().__init__('heading_controller')
self.pub = self.create_publisher(
TwistStamped, '/diff_drive_controller/cmd_vel', 1)
self.sub = self.create_subscription(
Float64, '/heading_error', self.on_error, 1)
self.timer = self.create_timer(PERIOD, self.tick)
self.error = 0.0
self.last_input = None
self.last_tick = None
self.was_stale = True
def on_error(self, msg):
# 无效输入立即触发安全零速,而非沿用旧误差。
if not math.isfinite(msg.data):
self.last_input = None
return
self.error = msg.data
self.last_input = time.monotonic()
def tick(self):
now = time.monotonic()
if self.last_tick is not None and now - self.last_tick > 0.08:
self.get_logger().warning('控制回调间隔超过 80 ms')
self.last_tick = now
stale = (self.last_input is None or
now - self.last_input > INPUT_TIMEOUT)
cmd = TwistStamped()
cmd.header.stamp = self.get_clock().now().to_msg()
cmd.header.frame_id = 'base_link'
if not stale and abs(self.error) > 0.02:
# 比例纠偏:Kp=2.0,角速度限制为 ±0.6 rad/s。
cmd.twist.angular.z = max(-0.6, min(0.6, 2.0 * self.error))
self.pub.publish(cmd)
if stale and not self.was_stale:
self.get_logger().warning('输入超时,持续发布零速度')
self.was_stale = stale
def main():
rclpy.init()
node = HeadingController()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()预期结果(消息行为,不是程序打印的固定日志):
尚无 /heading_error:约 20 Hz 持续发送 twist.angular.z = 0.0
持续收到 data = 0.2:发送 twist.angular.z = 0.4 rad/s
输入中断约 0.20~0.25 s 后:再次发送 twist.angular.z = 0.0深度为 1 的队列减少积压旧数据的机会;time.monotonic() 用于测量本进程收到输入后的经过时间,header.stamp 则采用 ROS 时钟供下游识别命令时间。定时器不是硬实时调度:单线程执行器中若有阻塞回调,50 ms 仍可能被错过,本例把超过 80 ms 的回调间隔记为警告。
将输入断流与命令断流分别保护

本例有两道不同的保护:上游 200 ms 没有新误差,应用节点仍按 20 Hz 发布零速;若应用进程自身崩溃,连零速也不会继续发送,此时需由 diff_drive_controller 的 cmd_vel_timeout 负责停车。Jazzy 该参数默认 0.5 s,下面取 0.25 s 作为仿真起始测试值。
片段示例(并入现有底盘控制器的完整参数 YAML;不能单独作为可启动配置):
diff_drive_controller:
ros__parameters:
cmd_vel_timeout: 0.25参数在控制器配置阶段生效。0.25 s 对应五个 50 ms 周期;设得太短会因偶发调度抖动误停,太长则增大失联后的滑行距离。按 0.2 m/s 粗估,250 ms 就可能再前进 0.05 m,实际还要叠加制动时间与机械惯性。
运行与断流验收
先在终端 A 启动程序。若使用仿真时间,输入节点、控制节点与底盘控制器必须一致配置 use_sim_time 和 /clock。
source /opt/ros/jazzy/setup.bash
python3 fixed_period_controller.py预期结果:
节点持续发布零速 TwistStamped,终端通常不会逐周期打印日志。终端 B 用 10 Hz 连续输入 0.2 rad 的误差,以确保有新输入到达:
source /opt/ros/jazzy/setup.bash
ros2 topic pub -r 10 /heading_error std_msgs/msg/Float64 '{data: 0.2}'预期输出(节选,后续会持续发布):
publisher: beginning loop
publishing #1: std_msgs.msg.Float64(data=0.2)终端 C 检查指令频率和单条消息;ros2 topic hz 的具体统计值可能浮动,header.stamp 随运行变化。
source /opt/ros/jazzy/setup.bash
ros2 topic echo --once /diff_drive_controller/cmd_vel
ros2 topic hz /diff_drive_controller/cmd_vel预期结果:
消息中的 twist.linear.x = 0.0
消息中的 twist.angular.z = 0.4
average rate: 约 20.0然后在终端 B 按 Ctrl+C 停止输入,等待约 0.3 s,重新执行 ros2 topic echo --once /diff_drive_controller/cmd_vel,应看到角速度变为 0.0;终端 A 同时只记录一次输入超时警告。再停止终端 A,确认已激活的底盘控制器在最后一条命令之后,约 0.25 s 加上控制周期与通信抖动的时间内停车。
能够稳定达到接近 20 Hz、输入断流转零速、节点停发触发底盘看门狗,才算完成控制时序的基本验收;Python 定时器适合教学与上层控制,严格实时的电机内环应交给具有确定性调度的控制器。