2. 机器人运动学基础
移动机器人要完成自主取放与搬运，第一步必须回答一个看似简单却贯穿全系统的问题：“我此刻在哪里，我要去的目标点相对于我是什么方向？”这个问题在数学上由运动学回答。运动学不关心力与质量，只研究位置、姿态、速度之间的几何关系。对工程实现而言，它决定了里程计如何积分、路径如何跟踪、机械臂抓取点如何从相机坐标系换算到末端执行器坐标系。
一、坐标系变换：把不同“视角”统一到同一张地图
机器人系统中同时存在多个坐标系：世界/地图坐标系 map、里程计坐标系 odom、车体坐标系 base_link、激光雷达坐标系 laser、相机坐标系 camera、机械臂基座与末端坐标系等。每个坐标系都有自己的原点和轴向，任何传感器数据只有经过变换，才能被其他模块正确理解。
二维平面上的刚体变换由旋转矩阵与平移向量组成。若点 P 在子坐标系中的坐标为 p_sub，则它在父坐标系中的坐标为：
p_parent = R(θ) · p_sub + t
R(θ) = [ cosθ -sinθ ]
[ sinθ cosθ ]
t = [ tx, ty ]ᵀ工程中更常用齐次变换矩阵 T，把旋转和平移合并为一次矩阵乘法，便于多级坐标串联：
T = [ R t ]
[ 0 1 ]
p_parent_h = T_parent_sub · p_sub_h例如相机识别到货箱中心在 camera 坐标系下为 (0.35, -0.12, 0.80)，要得到它在 base_link 下的位置，只需左乘 T_base_camera。若再要在地图上规划，则继续左乘 T_map_base。这种“链式相乘”是机器人软件中最基础的运算模式。
T_父_子，表示把子坐标系中的点变换到父坐标系，避免方向写反导致抓取偏移。二、正运动学与逆运动学：从轮速到位姿，再从位姿到轮速
对移动底盘而言，正运动学解决“给定左右轮速度，车体会怎样运动”；逆运动学解决“希望车体以某线速度、角速度运动，左右轮应给多少速度”。以两轮差速底盘为例，设轮半径 r，轮距 L，左轮角速度 ω_L、右轮角速度 ω_R，则：
v_L = ω_L · r
v_R = ω_R · r
v = (v_R + v_L) / 2
ω = (v_R - v_L) / L反过来，给定期望的线速度 v 与角速度 ω：
v_L = v - ω·L/2
v_R = v + ω·L/2
ω_L = v_L / r
ω_R = v_R / r位姿积分则常用离散形式。设当前位姿为 (x, y, θ)，控制周期为 dt：
x += v · cosθ · dt
y += v · sinθ · dt
θ += ω · dt这就是里程计递推的核心。它简单高效，但误差会随时间累积，因此实际系统通常用激光、视觉或UWB做重定位修正。
三、差速与全向底盘模型：运动能力决定规划方式
差速底盘结构简单、成本低、承载强，是搬运机器人最常见的形式。它的瞬时运动受非完整约束：车体只能沿自身前进方向产生速度，不能直接横向平移。因此它无法像全向底盘那样“横着走”，转弯必须依靠左右轮速度差。
全向底盘常见有麦克纳姆轮和全向轮两种。以四麦克纳姆轮为例，通过四个轮速的线性组合，可以同时解耦出 vx、vy、ω 三个自由度：
ω_1 = ( vx - vy - (Lx+Ly)·ω ) / r
ω_2 = ( vx + vy + (Lx+Ly)·ω ) / r
ω_3 = ( vx + vy - (Lx+Ly)·ω ) / r
ω_4 = ( vx - vy + (Lx+Ly)·ω ) / r其中 Lx、Ly 为轮组相对车体中心的纵向、横向距离。全向底盘能在狭窄货架间侧移、原地旋转，适合高密度仓储场景，但轮子磨损、地面平整度、负载分布对其精度影响明显。
四、运动约束：规划器必须尊重的“物理底线”
运动约束分为完整约束与非完整约束。完整约束可以直接写成位姿变量之间的代数方程，例如全向底盘在平面内可任意移动；非完整约束则涉及速度不可积，例如差速底盘不能横向平移。规划器若忽略这一点，就会生成“看起来很短、实际走不出来”的路径。
工程上常见的约束清单如下：
| 约束类型 | 典型对象 | 数学表现 | 工程影响 |
|---|---|---|---|
| 非完整约束 | 差速底盘 | ẋ·sinθ − ẏ·cosθ = 0 | 不能横移，需圆弧或S形路径 |
| 完整约束 | 麦克纳姆轮底盘 | vx、vy、ω 独立可控 | 可侧移，但轮面打滑影响精度 |
| 速度饱和 | 所有底盘 | |v| ≤ v_max，|ω| ≤ ω_max | 限制加速度与转弯半径 |
| 最小转弯半径 | 阿克曼底盘 | R_min = L / tan(δ_max) | 窄通道通过性受限 |
| 机械臂关节限位 | 取放机械臂 | q_min ≤ q ≤ q_max | 逆解需筛选可行解 |
五、案例与复盘：一次取放偏移的排查
某搬运机器人在货架前抓取失败，末端始终偏左约 4 cm。排查发现：相机外参标定矩阵 T_base_camera 的旋转角符号写反，导致目标点在 base_link 下横向偏移。修正后抓取成功率从 72% 提升到 96%。这说明坐标系变换的方向约定比公式本身更容易出错。
另一个常见误区是：把差速底盘当作全向底盘做横向路径跟踪。控制器输出 vy 后，底层无法执行，只能近似分解为前进加转向，结果轨迹抖动、耗时增加。正确做法是在规划层就使用差速运动模型，生成满足非完整约束的路径。
掌握坐标系变换、正逆运动学、差速与全向模型以及运动约束，是后续路径规划、轨迹跟踪、视觉伺服和机械臂协同取放的基础。只有让数学模型的边界与物理底盘的边界一致，机器人才能在真实货架间稳定完成搬运任务。