别再只调PID了！从零手把手推导两轮差速机器人里程计（附Python仿真代码）

两轮差速驱动机器人的里程计是移动机器人定位的基础模块，但很多工程师在实际调试中往往只关注PID参数调整，忽略了里程计本身的原理与实现细节。本文将带你从运动学方程出发，一步步推导出代码中的速度更新公式，并通过Python仿真验证里程计的实际表现。

1. 两轮差速机器人运动学基础

差速驱动机器人是最常见的移动机器人平台之一，其运动学模型相对简单但非常实用。理解这个模型是后续里程计推导的基础。

1.1 机器人坐标系定义

首先我们需要明确几个关键坐标系和参数：

• 世界坐标系(W)：固定的全局参考系，通常以机器人初始位置为原点
• 机器人坐标系(R)：固定在机器人上的局部坐标系，原点通常在两轮轴心
• 关键参数：
  • 轮半径：r
  • 轮间距：L
  • 左轮速度：vₗ
  • 右轮速度：vᵣ

1.2 基本运动学方程

差速机器人的运动可以用以下微分方程描述：

code复制dx/dt = v * cos(θ)
dy/dt = v * sin(θ)
dθ/dt = ω

其中：

• (x,y)是机器人在世界坐标系中的位置
• θ是机器人朝向角
• v是机器人线速度
• ω是机器人角速度

2. 从编码器数据到速度计算

实际机器人通过编码器获取的是轮子的旋转信息，我们需要将这些原始数据转换为有用的速度信息。

2.1 编码器数据处理

假设编码器测量的是轮子的转速（圈/秒），我们可以通过以下步骤转换为线速度：

1. 获取左右轮编码器计数：aₗ, aᵣ（圈/秒）
2. 计算轮子周长：S = 2πr
3. 计算左右轮线速度：
   • vₗ = aₗ * S
   • vᵣ = aᵣ * S

注意：实际应用中需要考虑编码器分辨率，将脉冲数转换为旋转角度

2.2 机器人速度计算

基于左右轮速度，我们可以计算机器人的线速度和角速度：

• 线速度：v = (vᵣ + vₗ)/2
• 角速度：ω = (vᵣ - vₗ)/L

这两个公式是差速驱动机器人里程计的核心，它们将轮子运动与机器人整体运动联系起来。

3. 位置估计的数值实现

有了速度信息后，我们需要通过积分来估计机器