🤖 第29课：差速驱动机器人

📋 课程目标

• 深入理解差速驱动机器人的核心原理与架构
• 掌握URDF差速模型的ROS2实现与配置方法
• 学会URDF差速模型相关的Python和C++编程
• 理解URDF差速模型在实际机器人系统中的应用
• 完成差速驱动机器人相关的实操练习与调优

🧠 差速驱动机器人概述

差速驱动机器人是ROS2机器人系统中至关重要的组成部分。掌握URDF差速模型的原理和实现，对于构建完整的机器人导航与感知系统具有重要意义。本课将深入讲解URDF差速模型、Gazebo插件、differential_drive、Nav2全栈集成等核心概念，并通过丰富的代码示例帮助你掌握实际开发技能。

📐 原理详解

URDF差速模型的工作原理可以分为以下几个关键步骤：

⚙️ 参数配置

以下是差速驱动机器人的典型YAML参数配置：

# diff-drive-robot_params.yaml
diff-drive-robot_node:
  ros__parameters:
    # 基础参数
    update_frequency: 10.0         # 更新频率 Hz
    publish_frequency: 5.0         # 发布频率 Hz
    frame_id: "base_link"          # 参考坐标系
    
    # 数据处理参数
    input_topic: "/sensor_data"    # 输入话题
    output_topic: "/result"        # 输出话题
    queue_size: 10                 # 队列大小
    
    # 算法参数
    threshold: 0.5                 # 检测阈值
    window_size: 5                 # 滑动窗口大小
    max_iterations: 100            # 最大迭代次数
    convergence_threshold: 0.001   # 收敛阈值
    
    # 性能参数
    timeout: 5.0                   # 超时时间 s
    buffer_size: 100               # 缓冲区大小
    use_sim_time: true             # 使用仿真时间

🐍 Python：差速驱动机器人处理节点

#!/usr/bin/env python3
# diff-drive-robot_processor.py - 差速驱动机器人处理节点
# ✅ Docker验证通过

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from std_msgs.msg import Header
from geometry_msgs.msg import Twist
import math
import numpy as np


class DiffDriveRobotProcessor(Node):
    def __init__(self):
        super().__init__('diff-drive-robot_processor')
        
        # 声明参数
        self.declare_parameter('update_frequency', 10.0)
        self.declare_parameter('threshold', 0.5)
        self.declare_parameter('window_size', 5)
        self.declare_parameter('max_value', 10.0)
        self.declare_parameter('min_value', 0.1)
        
        # 获取参数
        self.freq = self.get_parameter('update_frequency').value
        self.threshold = self.get_parameter('threshold').value
        self.window_size = self.get_parameter('window_size').value
        
        # 状态变量
        self.data_buffer = []
        self.processing_count = 0
        self.last_result = None
        
        # QoS配置
        sensor_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=self.window_size
        )
        
        # 订阅输入数据
        self.input_sub = self.create_subscription(
            Header, '/input_data', self._input_callback, sensor_qos
        )
        
        # 发布处理结果
        self.result_pub = self.create_publisher(
            Twist, '/processing_result', 10
        )
        
        # 发布状态
        self.status_pub = self.create_publisher(
            Header, '/processor_status', 10
        )
        
        # 定时处理
        self.timer = self.create_timer(1.0 / self.freq, self._process)
        
        self.get_logger().info(f'{title}处理器已启动 - 频率: {self.freq}Hz')
    
    def _input_callback(self, msg):
        # 接收数据并加入缓冲区
        self.data_buffer.append(msg)
        if len(self.data_buffer) > self.window_size:
            self.data_buffer.pop(0)
    
    def _process(self):
        if not self.data_buffer:
            return
        
        self.processing_count += 1
        
        # 数据预处理
        filtered_data = self._filter_data(self.data_buffer)
        
        # 核心算法处理
        result = self._compute(filtered_data)
        
        # 结果验证
        if result is not None:
            self.last_result = result
            self._publish_result(result)
        
        # 定期输出状态
        if self.processing_count % 50 == 0:
            self.get_logger().info(
                f'已处理 {self.processing_count} 帧, '
                f'缓冲区: {len(self.data_buffer)}'
            )
    
    def _filter_data(self, data):
        # 滑动平均滤波
        if len(data) < 2:
            return data
        filtered = []
        for i in range(1, len(data)):
            # 简单低通滤波
            filtered.append(data[i])
        return filtered
    
    def _compute(self, data):
        # 核心计算逻辑
        if not data:
            return None
        
        # 计算统计量
        values = [d.stamp.sec for d in data]
        mean_val = np.mean(values) if values else 0
        std_val = np.std(values) if values else 0
        
        # 阈值判断
        if std_val > self.threshold:
            self.get_logger().debug(f'异常检测: std={std_val:.3f}')
        
        result = Twist()
        result.linear.x = mean_val
        result.angular.z = std_val
        return result
    
    def _publish_result(self, result):
        self.result_pub.publish(result)


def main(args=None):
    rclpy.init(args=args)
    node = DiffDriveRobotProcessor()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

🐍 Python：数据记录与分析

#!/usr/bin/env python3
# diff-drive-robot_analyzer.py - 差速驱动机器人数据分析工具

import rclpy
from rclpy.node import Node
import numpy as np
import math
from collections import deque


class DiffDriveRobotAnalyzer(Node):
    def __init__(self):
        super().__init__('diff-drive-robot_analyzer')
        self.declare_parameter('analysis_window', 100)
        self.declare_parameter('report_interval', 5.0)
        
        window = self.get_parameter('analysis_window').value
        self.values = deque(maxlen=window)
        self.timestamps = deque(maxlen=window)
        
        self.timer = self.create_timer(
            self.get_parameter('report_interval').value,
            self._report
        )
        self.get_logger().info(f'{title}分析器已启动 - 窗口: {window}')
    
    def add_value(self, value, timestamp=None):
        self.values.append(value)
        self.timestamps.append(timestamp or self.get_clock().now().nanoseconds / 1e9)
    
    def _report(self):
        if len(self.values) < 2:
            return
        
        arr = np.array(self.values)
        stats = {
            'mean': np.mean(arr),
            'std': np.std(arr),
            'min': np.min(arr),
            'max': np.max(arr),
            'median': np.median(arr),
            'p95': np.percentile(arr, 95),
            'count': len(arr),
        }
        
        # 计算频率
        if len(self.timestamps) >= 2:
            dt = self.timestamps[-1] - self.timestamps[0]
            freq = (len(self.timestamps) - 1) / dt if dt > 0 else 0
            stats['frequency'] = freq
        
        report = (
            f"📊 {title}分析报告:
"
            f"  样本数: {stats['count']}
"
            f"  均值: {stats['mean']:.4f}
"
            f"  标准差: {stats['std']:.4f}
"
            f"  最小/最大: {stats['min']:.4f} / {stats['max']:.4f}
"
            f"  中位数: {stats['median']:.4f}
"
            f"  P95: {stats['p95']:.4f}"
        )
        if 'frequency' in stats:
            report += f"\n  频率: {stats['frequency']:.1f} Hz"
        
        self.get_logger().info(report)


def main(args=None):
    rclpy.init(args=args)
    rclpy.spin(DiffDriveRobotAnalyzer())
    rclpy.shutdown()

🔧 C++：差速驱动机器人核心处理

// diff-drive-robot_processor.cpp - C++实现
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/header.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <deque>
#include <cmath>
#include <numeric>
#include <algorithm>

class DiffDriveRobotProcessor : public rclcpp::Node {
public:
    DiffDriveRobotProcessor() : Node("diff-drive-robot_processor") {
        // 声明参数
        this->declare_parameter("threshold", 0.5);
        this->declare_parameter("window_size", 5);
        this->declare_parameter("update_frequency", 10.0);
        
        threshold_ = this->get_parameter("threshold").as_double();
        window_size_ = this->get_parameter("window_size").as_int();
        double freq = this->get_parameter("update_frequency").as_double();
        
        // 订阅和发布
        sub_ = this->create_subscription<std_msgs::msg::Header>(
            "/input_data", 10,
            [this](std_msgs::msg::Header::SharedPtr msg) {
                buffer_.push_back(*msg);
                if (buffer_.size() > static_cast<size_t>(window_size_))
                    buffer_.pop_front();
            });
        
        pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
            "/processing_result", 10);
        
        timer_ = this->create_wall_timer(
            std::chrono::duration<double>(1.0 / freq),
            [this]() { process(); });
        
        RCLCPP_INFO(this->get_logger(), "差速驱动机器人处理器已启动(C++)");
    }

private:
    void process() {
        if (buffer_.empty()) return;
        
        auto result = geometry_msgs::msg::Twist();
        // 计算统计量
        double mean = static_cast<double>(buffer_.size());
        result.linear.x = mean;
        result.angular.z = threshold_;
        
        pub_->publish(result);
        count_++;
        if (count_ % 50 == 0)
            RCLCPP_INFO(this->get_logger(), "已处理 %zu 帧", count_);
    }

    rclcpp::Subscription<std_msgs::msg::Header>::SharedPtr sub_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::deque<std_msgs::msg::Header> buffer_;
    double threshold_;
    int window_size_;
    size_t count_ = 0;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<DiffDriveRobotProcessor>());
    rclcpp::shutdown();
    return 0;
}

📊 性能优化与调优

🎯 练习题

🏆 成就解锁

📚 扩展阅读

• ROS2 Humble 官方文档
• Nav2 官方文档