LQR控制在差速驱动机器人轨迹跟踪中的应用

1. 项目概述

在移动机器人控制领域，轨迹跟踪是一个经典而重要的课题。想象一下，当你需要让一台差速驱动的移动机器人（比如常见的扫地机器人或仓储AGV）精确地沿着预设的路径移动时，会遇到哪些挑战？机器人的运动存在惯性，电机响应有延迟，地面可能有打滑，这些因素都会导致实际轨迹偏离预期。LQR（线性二次型调节器）最优控制正是解决这类问题的有力工具。

我最近在一个仓储物流机器人项目中实践了基于Simulink的LQR控制器设计，效果令人满意。与传统的PID控制相比，LQR能够系统性地考虑状态误差和控制能耗之间的平衡，特别是在处理非线性系统的小范围线性化控制时表现出色。本文将详细分享从建模到实现的完整过程，包括几个关键的技术细节和实际调试中获得的经验。

2. 移动机器人建模与线性化

2.1 差速驱动机器人运动学

差速驱动机器人的运动学模型是控制器设计的基础。这类机器人通常有两个驱动轮和一个或多个万向轮，通过调节左右轮速差来实现转向。在全局坐标系下，机器人的位姿可以用向量q=[x, y, θ]ᵀ表示，其中(x,y)是位置坐标，θ是朝向角度。

从轮速到位姿变化的推导值得仔细理解：

• 线速度v是左右轮速的平均值：v = (v_L + v_R)·r/2
• 角速度ω与轮速差成正比：ω = (v_R - v_L)·r/L
  其中r是轮半径，L是轮距（两驱动轮中心之间的距离）。

有了v和ω，机器人的运动学方程就可以表示为：
ẋ = v·cosθ
ẏ = v·sinθ
θ̇ = ω

这个模型看起来简单，但已经包含了移动机器人运动的核心特性——非完整性约束（机器人不能直接横向移动）。

2.2 轨迹跟踪误差模型

为了设计跟踪控制器，我们需要定义机器人当前状态与参考轨迹之间的误差。这里有个技巧：将误差定义在机器人本体坐标系下比在全局坐标系下更有利于控制器设计。

误差向量e=[e_x, e_y, e_θ]ᵀ通过旋转矩阵从全局误差转换而来：
e_x = (x_r - x)cosθ + (y_r - y)sinθ
e_y = -(x_r - x)sinθ + (y_r - y)cosθ
e_θ = θ_r - θ

对误差求导后得到的非线性动态方程反映了误差随时间变化的规律：
ė_x = -v + v_r·cos(e_θ) + ω_r·e_y
ė_y