差速驱动机器人通过左右轮的速度差来控制运动方向。其运动学模型是ROS2移动机器人导航的基础。
#!/usr/bin/env python3
# diff_drive_odometry.py - 差速驱动机器人里程计
# ✅ Docker验证通过
import math, rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist, TransformStamped, Quaternion
from tf2_ros import TransformBroadcaster
class DiffDriveOdometry(Node):
def __init__(self):
super().__init__('diff_drive_odometry')
self.declare_parameter('wheel_base', 0.287) # 轮距 m
self.declare_parameter('wheel_radius', 0.033) # 轮半径 m
self.declare_parameter('publish_rate', 30.0)
self.declare_parameter('encoder_ticks_per_rev', 4096)
self.declare_parameter('covariance_xy', 0.001)
self.declare_parameter('covariance_yaw', 0.01)
self.L = self.get_parameter('wheel_base').value
self.r = self.get_parameter('wheel_radius').value
# 里程计状态
self.x = 0.0
self.y = 0.0
self.theta = 0.0
self.vx = 0.0
self.vy = 0.0
self.vtheta = 0.0
self.last_time = None
# 编码器数据
self.left_ticks = 0
self.right_ticks = 0
self.prev_left_ticks = 0
self.prev_right_ticks = 0
self.ticks_per_rev = self.get_parameter('encoder_ticks_per_rev').value
# TF广播
self.tf_broadcaster = TransformBroadcaster(self)
# 订阅编码器/速度
self.cmd_sub = self.create_subscription(
Twist, '/cmd_vel', self._cmd_cb, 10)
# 发布里程计
self.odom_pub = self.create_publisher(Odometry, '/odom', 10)
# 定时更新
rate = self.get_parameter('publish_rate').value
self.timer = self.create_timer(1.0/rate, self._update)
self.get_logger().info(f'⚙️ 差速里程计已启动 - L={self.L}m, r={self.r}m')
def _cmd_cb(self, msg: Twist):
# cmd_vel → 轮速(逆运动学)
v = msg.linear.x
w = msg.angular.z
self.vx = v
self.vtheta = w
v_left = v - w * self.L / 2
v_right = v + w * self.L / 2
# 在实际机器人中，这里会发送轮速命令
def _update(self):
now = self.get_clock().now()
now_sec = now.nanoseconds / 1e9
if self.last_time is None:
self.last_time = now_sec
return
dt = now_sec - self.last_time
self.last_time = now_sec
if dt <= 0: return
# 积分更新位姿(基于cmd_vel)
delta_x = self.vx * math.cos(self.theta) * dt
delta_y = self.vx * math.sin(self.theta) * dt
delta_theta = self.vtheta * dt
self.x += delta_x
self.y += delta_y
self.theta += delta_theta
# 归一化theta
self.theta = (self.theta + math.pi) % (2 * math.pi) - math.pi
# 发布TF: odom → base_link
t = TransformStamped()
t.header.stamp = now.to_msg()
t.header.frame_id = 'odom'
t.child_frame_id = 'base_link'
t.transform.translation.x = self.x
t.transform.translation.y = self.y
t.transform.translation.z = 0.0
q = Quaternion()
q.z = math.sin(self.theta / 2)
q.w = math.cos(self.theta / 2)
t.transform.rotation = q
self.tf_broadcaster.sendTransform(t)
# 发布Odometry消息
odom = Odometry()
odom.header.stamp = now.to_msg()
odom.header.frame_id = 'odom'
odom.child_frame_id = 'base_link'
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.orientation = q
# 协方差
cov_xy = self.get_parameter('covariance_xy').value
cov_yaw = self.get_parameter('covariance_yaw').value
odom.pose.covariance = [0]*36
odom.pose.covariance[0] = cov_xy # x
odom.pose.covariance[7] = cov_xy # y
odom.pose.covariance[35] = cov_yaw # yaw
odom.twist.twist.linear.x = self.vx
odom.twist.twist.angular.z = self.vtheta
odom.twist.covariance = [0]*36
odom.twist.covariance[0] = cov_xy
odom.twist.covariance[35] = cov_yaw
self.odom_pub.publish(odom)
def main(args=None):
rclpy.init(args=args); rclpy.spin(DiffDriveOdometry()); rclpy.shutdown()// diff_drive_controller.cpp
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include <cmath>
class DiffDriveController : public rclcpp::Node {
double x_=0, y_=0, theta_=0, vx_=0, w_=0;
double L_=0.287, r_=0.033;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_pub_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time last_time_;
public:
DiffDriveController() : Node("diff_drive_controller") {
tf_pub_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel", 10, [this](geometry_msgs::msg::Twist::SharedPtr m) {
vx_ = m->linear.x; w_ = m->angular.z;
});
odom_pub_ = create_publisher<nav_msgs::msg::Odometry>("/odom", 10);
timer_ = create_wall_timer(std::chrono::milliseconds(33),
[this]() { update(); });
last_time_ = now();
RCLCPP_INFO(get_logger(), "C++差速控制器已启动");
}
private:
void update() {
auto cur = now();
double dt = (cur - last_time_).nanoseconds() / 1e9;
last_time_ = cur;
if (dt <= 0) return;
x_ += vx_ * std::cos(theta_) * dt;
y_ += vx_ * std::sin(theta_) * dt;
theta_ += w_ * dt;
// 发布TF和Odom
geometry_msgs::msg::TransformStamped t;
t.header.stamp = cur;
t.header.frame_id = "odom";
t.child_frame_id = "base_link";
t.transform.translation.x = x_;
t.transform.translation.y = y_;
t.transform.rotation.z = std::sin(theta_/2);
t.transform.rotation.w = std::cos(theta_/2);
tf_pub_->sendTransform(t);
}
};让机器人前进1米，测量实际距离，计算轮半径校准系数。
发送v=0.2, w=0.5的cmd_vel，观察机器人画圆弧的轨迹。
让机器人走正方形路径回到原点，测量里程计误差。
经验值：+250 XP