别再死记公式了！用Python+ROS从零推导差速机器人运动模型（附代码）

从零构建差速机器人运动模型：Python与ROS实战指南

差速转向机器人是移动机器人领域最基础也最经典的模型之一。无论是实验室的履带车、扫地机器人还是仓储AGV，差速驱动都是最常见的运动方式。但很多初学者在学习时往往陷入两个极端：要么死记硬背公式却不知其所以然，要么被复杂的数学推导吓退而无法动手实践。本文将带你用Python和ROS从第一性原理出发，通过编写代码来直观理解差速机器人的运动规律。

1. 差速机器人基础原理

差速转向的核心思想非常简单：通过控制两个驱动轮的速度差来实现转向。当左右轮速度相同时，机器人直线前进；当速度不同时，机器人会沿着某个圆弧轨迹运动；当两轮速度大小相等方向相反时，机器人原地旋转。

1.1 运动学基本假设

在开始推导前，我们需要明确几个基本假设：

• 机器人只在二维平面运动（不考虑跳跃或飞行）
• 轮子与地面之间是纯滚动无滑动
• 机器人底盘是刚体，不会变形
• 忽略轮子的厚度和质量分布

这些假设虽然简化了现实情况，但对于大多数室内移动机器人应用已经足够准确。

1.2 关键参数定义

让我们先定义几个关键参数：

线速度与角速度的关系很简单：v = ω × r。因此左右轮的线速度可以表示为：

v_l = omega_l * r v_r = omega_r * r

2. 运动模型推导

2.1 瞬时运动分析

差速机器人在任意时刻的运动都可以分解为两种基本运动的叠加：

1. 直线运动：由两轮速度的平均值决定
2. 旋转运动：由两轮速度差决定

具体来说，机器人质心的线速度和角速度可以表示为：

v = (v_r + v_l) / 2 omega = (v_r - v_l) / L

这个简单的公式是差速机器人运动模型的核心。让我们用Python代码来验证一下：

import numpy as np def calculate_robot_velocity(v_l, v_r, L): """计算机器人质心的线速度和角速度""" v = (v_r + v_l) / 2 omega = (v_r - v_l) / L return v, omega # 测试不同速度组合 print(calculate_robot_velocity(1.0, 1.0, 0.5)) # 直线前进：(1.0, 0.0) print(calculate_robot_velocity(1.0, 0.5, 0.5)) # 弧线运动：(0.75, -1.0) print(calculate_robot_velocity(1.0, -1.0, 0.5)) # 原地旋转：(0.0, -4.0)

2.2 转弯半径计算

当机器人做弧线运动时，其转弯半径R可以通过几何关系推导出来。考虑机器人绕瞬时转动中心(ICC)旋转，可以得到：

R = L/2 * (v_r + v_l) / (v_r - v_l)

有趣的是，当v_r = v_l时，转弯半径趋近于无穷大，对应直线运动；当v_r = -v_l时，转弯半径为0，对应原地旋转。

2.3 位姿更新模型

为了在仿真中模拟机器人的运动轨迹，我们需要离散化模型。假设采样时间为Δt，机器人的位姿(x,y,θ)更新公式为：

def update_pose(x, y, theta, v, omega, dt): """更新机器人位姿""" if abs(omega) < 1e-6: # 直线运动 x += v * np.cos(theta) * dt y += v * np.sin(theta) * dt else: # 弧线运动 R = v / omega dtheta = omega * dt x += R * (np.sin(theta + dtheta) - np.sin(theta)) y -= R * (np.cos(theta + dtheta) - np.cos(theta)) theta += dtheta return x, y, theta

3. ROS中的实现

3.1 ROS控制接口

在ROS中，差速机器人通常通过/cmd_vel话题接收控制命令，消息类型为geometry_msgs/Twist。这个消息包含线速度(x,y,z)和角速度(x,y,z)。对于平面移动机器人，我们主要关心：

• 线速度x分量（前进速度）
• 角速度z分量（转向速度）

我们需要将Twist消息转换为左右轮速度。转换公式为：

def twist_to_wheel_speeds(twist_msg, L): """将Twist消息转换为左右轮速度""" v = twist_msg.linear.x omega = twist_msg.angular.z v_l = v - omega * L / 2 v_r = v + omega * L / 2 return v_l, v_r

3.2 完整的ROS节点示例

下面是一个简单的ROS节点实现，它订阅/cmd_vel话题并模拟机器人运动：

#!/usr/bin/env python3 import rospy import numpy as np from geometry_msgs.msg import Twist, PoseStamped from nav_msgs.msg import Path class DifferentialDriveSimulator: def __init__(self): rospy.init_node('diff_drive_simulator') # 机器人参数 self.L = rospy.get_param('~wheel_separation', 0.5) self.r = rospy.get_param('~wheel_radius', 0.1) # 初始状态 self.x = 0.0 self.y = 0.0 self.theta = 0.0 # 路径记录 self.path = Path() self.path.header.frame_id = "odom" # ROS接口 self.cmd_vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.cmd_vel_callback) self.path_pub = rospy.Publisher('/path', Path, queue_size=10) # 定时器更新位姿 self.last_time = rospy.Time.now() self.timer = rospy.Timer(rospy.Duration(0.1), self.update_pose) def cmd_vel_callback(self, msg): # 将Twist转换为轮速 v_l, v_r = twist_to_wheel_speeds(msg, self.L) # 计算机器人速度 v = (v_r + v_l) / 2 omega = (v_r - v_l) / self.L # 更新机器人状态 self.v = v self.omega = omega def update_pose(self, event): # 计算时间间隔 current_time = rospy.Time.now() dt = (current_time - self.last_time).to_sec() self.last_time = current_time # 更新位姿 self.x, self.y, self.theta = update_pose( self.x, self.y, self.theta, self.v, self.omega, dt) # 发布路径 pose = PoseStamped() pose.header.stamp = current_time pose.header.frame_id = "odom" pose.pose.position.x = self.x pose.pose.position.y = self.y # 省略姿态四元数设置... self.path.poses.append(pose) self.path_pub.publish(self.path) if __name__ == '__main__': try: simulator = DifferentialDriveSimulator() rospy.spin() except rospy.ROSInterruptException: pass

4. 可视化与调试

4.1 使用Matplotlib实时可视化

在开发过程中，实时可视化机器人的运动轨迹非常有帮助。我们可以修改上面的代码，添加Matplotlib实时绘图功能：

import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation class VisualizedSimulator(DifferentialDriveSimulator): def __init__(self): super().__init__() # 初始化图形 self.fig, self.ax = plt.subplots() self.line, = self.ax.plot([], [], 'b-') self.robot_marker, = self.ax.plot([], [], 'ro') # 设置图形范围 self.ax.set_xlim(-5, 5) self.ax.set_ylim(-5, 5) self.ax.grid(True) # 启动动画 self.ani = FuncAnimation(self.fig, self.update_plot, interval=100) plt.show() def update_plot(self, frame): # 更新路径数据 x_data = [pose.pose.position.x for pose in self.path.poses] y_data = [pose.pose.position.y for pose in self.path.poses] self.line.set_data(x_data, y_data) # 更新机器人位置标记 self.robot_marker.set_data([self.x], [self.y]) return self.line, self.robot_marker

4.2 RViz中的可视化

对于更复杂的场景，我们可以将机器人的运动轨迹发布到RViz中显示。需要发布以下话题：

1. /path：nav_msgs/Path类型，显示历史轨迹
2. /odom：nav_msgs/Odometry类型，提供里程计信息
3. /tf：发布底盘到里程计的坐标系变换

# 在DifferentialDriveSimulator类中添加 self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10) self.tf_broadcaster = tf2_ros.TransformBroadcaster() def publish_odometry(self): odom = Odometry() odom.header.stamp = rospy.Time.now() 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.twist.twist.linear.x = self.v * np.cos(self.theta) odom.twist.twist.linear.y = self.v * np.sin(self.theta) odom.twist.twist.angular.z = self.omega self.odom_pub.publish(odom) # 发布TF变换 t = TransformStamped() t.header.stamp = odom.header.stamp t.header.frame_id = "odom" t.child_frame_id = "base_link" t.transform.translation.x = self.x t.transform.translation.y = self.y # 设置旋转... self.tf_broadcaster.sendTransform(t)

5. 实际应用中的注意事项

5.1 电机控制问题

在实际硬件实现中，我们需要考虑电机的一些特性：

• 电机响应延迟
• 速度饱和限制
• 加速度限制
• 左右轮速度匹配误差

一个更健壮的速度转换函数应该考虑这些因素：

def safe_twist_to_wheel_speeds(twist_msg, L, max_speed): v = twist_msg.linear.x omega = twist_msg.angular.z # 计算理论轮速 v_l = v - omega * L / 2 v_r = v + omega * L / 2 # 处理速度饱和 max_v = max(abs(v_l), abs(v_r)) if max_v > max_speed: scale = max_speed / max_v v_l *= scale v_r *= scale return v_l, v_r

5.2 里程计误差累积

纯里程计定位会因为以下原因产生误差累积：

• 轮子打滑
• 地面不平
• 轮子直径误差
• 轮距测量误差

因此在实际应用中，需要结合IMU、视觉或激光雷达进行传感器融合。一个简单的做法是使用robot_pose_ekf包融合里程计和IMU数据。

5.3 参数标定

差速机器人的两个关键参数L和r需要准确标定。可以通过以下方法标定：

1. 轮距L标定：

   • 让机器人原地旋转N圈，记录左右轮总转数
   • L ≈ (总转数之和 × r) / (π × N)

2. 轮半径r标定：

   • 让机器人直线行驶已知距离D
   • 记录轮子总转数N
   • r ≈ D / (2πN)

def calibrate_wheel_separation(left_rotations, right_rotations, rotations_count, wheel_radius): """标定两轮间距""" total_distance = (left_rotations + right_rotations) * wheel_radius return total_distance / (np.pi * rotations_count) def calibrate_wheel_radius(distance, rotations): """标定轮半径""" return distance / (2 * np.pi * rotations)

6. 扩展应用：轨迹跟踪控制

理解了差速模型后，我们可以实现更高级的功能，比如轨迹跟踪。一个简单的PD控制器可以这样实现：

class TrajectoryTracker: def __init__(self, L, kp=1.0, kd=0.1): self.L = L self.kp = kp self.kd = kd self.last_error = 0.0 def compute_control(self, current_pose, target_pose, target_velocity): # 计算位置误差 dx = target_pose[0] - current_pose[0] dy = target_pose[1] - current_pose[1] # 转换到机器人坐标系 robot_dx = dx * np.cos(current_pose[2]) + dy * np.sin(current_pose[2]) robot_dy = -dx * np.sin(current_pose[2]) + dy * np.cos(current_pose[2]) # 角度误差 angle_error = np.arctan2(robot_dy, robot_dx) # PD控制 angular_velocity = self.kp * angle_error + self.kd * (angle_error - self.last_error) self.last_error = angle_error # 创建Twist消息 twist = Twist() twist.linear.x = target_velocity twist.angular.z = angular_velocity return twist

这个控制器可以配合前面的差速模型使用，实现自动轨迹跟踪功能。在实际项目中，你可能需要更复杂的控制算法，如模型预测控制(MPC)或自适应控制，以处理更复杂的场景。