智能车平衡控制：从倒立摆建模到LQR控制器设计-平芜编程栈

1. 项目概述：从“会跑”到“站得住”的跨越

搞智能车竞赛的兄弟们都清楚，单车组（也叫独轮车或平衡车组别）是整个比赛里技术门槛最高、也最“秀”的一个方向。别的车四个轮子或者两个轮子着地，首要任务是跑得快、拐得稳；而单车组，你的车得先学会“站”起来。这个“站”，就是平衡控制。我当年带队做这个项目，第一周全组人都在跟陀螺仪和加速度计的数据较劲，车不是往前“啪”一下拍地上，就是往后猛仰过去，场面一度十分滑稽。所以，今天我们不聊具体代码和电路，就深挖一下这个让车“站起来”的核心——平衡控制的理论基础。这就像练武，招式（代码）可以千变万化，但内功心法（理论）不通，永远成不了高手。这篇文章，就是帮你把这份“内功心法”捋明白，让你知道每一个参数调整背后，到底在跟物理世界的哪些规律较劲。

2. 核心需求解析：平衡的本质是什么？

在深入数学公式之前，我们得先想明白，我们要解决的到底是个什么问题。一辆两轮自平衡车，你可以把它想象成一个倒立摆。一根杆子（车身）底下用一个轮子（或两个并排很近的轮子等效为一个支点）支撑着。这个系统天然是不稳定的，就像你用手指顶着一根棍子，稍微一偏，重力就会把它拉倒。我们的目标，就是通过控制底部的轮子前后移动，来对抗重力的影响，让这根“棍子”（车身）始终保持竖直，或者稳定在一个我们期望的倾斜角度上。

所以，平衡控制的核心需求可以分解为两个层次：

静态平衡：让车在静止状态下保持直立。这是最基本也是最难的一步，因为任何微小的扰动（如地面不平、电机微小的力矩波动）都会破坏平衡。
动态平衡：在静态平衡的基础上，让车能够按照我们的指令前进、后退、转弯。这时，平衡控制不仅要抵抗重力，还要处理因运动产生的惯性力、离心力等额外的干扰。

为了实现这些，我们需要三样东西：感知（知道车现在“歪”了多少）、决策（计算需要让轮子怎么动来纠正这个“歪”）、执行（驱动电机产生准确的力矩）。理论分析主要聚焦在“决策”环节，即如何根据“感知”到的姿态信息，计算出“执行”机构需要的控制量。

3. 动力学建模：把物理世界“翻译”成数学方程

理论分析的第一步是建模。我们需要用数学方程来描述这个倒立摆系统的运动规律。这是整个控制理论的基石，模型建得准，后面的控制器设计才有意义。常见的建模方法有牛顿-欧拉法和拉格朗日法。对于智能车这样的简单系统，牛顿-欧拉法更直观。

3.1 模型简化与假设

为了抓住主要矛盾，我们通常先建立一个简单的平面模型（只考虑车子在前进方向上的俯仰运动，忽略侧倾和横摆），并做以下合理假设：

车身和轮子都是刚体。
电机驱动轮子，且电机响应足够快，我们可以认为控制输入直接是轮子的转矩。
车体绕轮轴的转动惯量、质量分布等参数可以通过测量或估算得到。

3.2 建立运动方程

我们设：

θ：车体与竖直方向的夹角（俯仰角）。θ=0表示完全直立，前倾为正，后仰为负。
x：轮子中心点的水平位移。
τ：电机作用在轮子上的转矩（控制输入）。
M,m：分别为轮子质量和车体质量。
R,L：分别为轮子半径和车体质心到轮轴的距离。
I_w,I_b：分别为轮子绕其轴心的转动惯量和车体绕其质心的转动惯量。

通过对轮子和车体分别进行受力分析，并应用牛顿第二定律和转动定律，我们可以推导出描述θ和x的两个耦合的非线性微分方程。这个过程涉及一些矢量运算和受力分解，是理论分析中最“硬核”的部分。最终得到的方程形式大致如下：

(M + m) * ẍ + m*L*cosθ * θ̈ - m*L*sinθ * (θ̇)² = (τ / R) (m*L*cosθ) * ẍ + (I_b + m*L²) * θ̈ - m*g*L*sinθ = -τ

这里ẍ和θ̈分别是位移和角度的二阶导（即加速度和角加速度），θ̇是角速度的一阶导。g是重力加速度。这两个方程清晰地展示了系统的非线性（存在sinθ,cosθ,(θ̇)²项）和耦合性（ẍ和θ̈相互影响）。

注意：很多同学看到这一堆符号和方程就头大。其实关键在于理解其物理意义：第一个方程本质是轮子水平方向的“力平衡”，控制转矩τ转化为了轮子前进的力。第二个方程是车体绕轮轴的“力矩平衡”，重力m*g*L*sinθ是导致倾倒的主要力矩，而电机转矩τ和轮子加速度带来的惯性力共同作用来平衡它。耦合项m*L*cosθ * θ̈意味着车体的转动会影响到轮子的平动，反之亦然。

3.3 线性化处理

上面的非线性方程直接用来设计控制器非常困难。好在对于平衡控制，我们主要关心的是θ在0度（直立点）附近的小范围运动。此时，我们可以应用小角度假设：当θ很小时，sinθ ≈ θ，cosθ ≈ 1，并且(θ̇)²作为高阶小量可以忽略。这样，复杂的非线性方程就被简化成了线性的状态空间方程，形式如下：

状态向量 X = [x, θ, ẋ, θ̇]ᵀ 状态方程：Ẋ = A * X + B * u 输出方程：Y = C * X + D * u

其中，u就是我们的控制量τ。矩阵A和B中的元素全部由系统的物理参数（M, m, L, I等）构成，变成了常数。这一步线性化是至关重要的，它使得我们可以运用成熟且强大的线性系统控制理论（如PID控制、状态反馈、LQR等）来系统性地设计控制器。

4. 控制器设计：PD控制的登场与局限

对于刚入门的队伍，最直接、最常用的控制器就是PD控制器（比例-微分控制）。它不依赖于精确的数学模型，属于“经典控制理论”的范畴，靠的是经验和调试。

4.1 PD控制原理

PD控制器针对的是单输入单输出系统。在我们的平衡控制中，输入是期望的直立角度（通常为0），输出是测量得到的实际角度θ。控制器根据误差e = θ_desired - θ（这里θ_desired=0）来生成控制量（电机PWM占空比或转矩指令）。其控制律为：

u = Kp * e + Kd * (de/dt)

比例项 (Kp)：与当前角度误差成正比。车体往哪边倒，就命令轮子往哪边加速，以产生一个恢复力矩。Kp越大，纠正的力度越强，反应越快。但过大容易引起超调和振荡。
微分项 (Kd)：与角度误差的变化率（即角速度θ̇）成正比。它起到“阻尼”的作用，能够预测车的倾倒趋势。当车体正在快速倾倒时，微分项会产生一个反向力矩来抑制这个速度，防止系统冲过头而振荡。这是平衡控制中不可或缺的一项，没有微分项，系统几乎不可能稳定。

在实际单片机编程中，de/dt通常由陀螺仪输出的角速度信号直接获得，这比用角度差分更准确、噪声更小。

4.2 PD参数整定经验

调参是个手艺活，但有一些基本准则：

先调Kd，后调Kp：这是平衡车调试的黄金法则。先将Kp设为0，逐渐增大Kd。你会发现车体受到扰动后，会缓慢地倒下，而不是“啪”地一下砸下去。这说明微分阻尼开始起作用了。
加入Kp：在有一个基础Kd的前提下，慢慢增加Kp。你会看到车开始尝试“抵抗”倾倒。当Kp增加到某个临界点，车可能会开始高频小幅振荡（“发抖”）。
精细调整：此时需要回调Kp或微增Kd来抑制振荡，寻找一个既能快速响应又能平稳站立的点。用手轻轻推一下车，它应该能迅速回正，且最多有1-2次衰减振荡。
注意饱和：电机的输出力矩是有限的（PWM有上下限）。如果Kp或Kd过大，计算出的控制量u很容易达到饱和限幅，此时系统会进入非线性区，性能急剧下降甚至失稳。调试时一定要在代码中加入输出限幅。

实操心得：不要指望一次就能调好。环境变化（如电池电压下降、地面摩擦系数不同）、传感器噪声都会影响最佳参数。我们当时的做法是，调出一组在实验室光滑地板上能稳定站立的参数后，会在这组参数周围设置几组微调值，通过拨码开关或蓝牙指令能在比赛现场快速切换，以应对不同的赛道材质（如沥青、环氧地坪）。

4.3 PD控制的局限性

PD控制器简单有效，但它存在几个固有缺陷：

无法消除静差：对于平衡控制，我们的期望角度是0，理论上不存在静差问题。但如果引入速度控制（想让车在平衡的同时以某个速度巡航），纯PD就会产生静差。
应对复杂动态能力有限：PD只关注“角度”和“角速度”两个状态。而我们的系统实际上有四个状态变量（位置x、角度θ、速度ẋ、角速度θ̇）。当车需要同时进行平衡和位置控制（比如停在指定点）时，PD控制器难以协调，容易顾此失彼。
抗干扰性依赖参数：对于大幅值或持续性的外部干扰（如强风、持续坡道），PD控制的恢复能力完全取决于Kp和Kd的取值，没有最优保证。

正因为这些局限性，在追求更高性能（如更快的响应、更强的抗扰、同时实现平衡与循迹）时，我们就需要更高级的控制策略。

5. 进阶之路：状态空间与LQR控制

当PD控制无法满足你的性能要求，或者你想从原理上更深刻地掌控你的车时，状态空间方法和线性二次型调节器（LQR）是一个优雅而强大的工具。它直接基于我们第3节中建立的线性化状态空间模型。

5.1 状态反馈控制

核心思想是：既然我们的系统状态X = [x, θ, ẋ, θ̇]ᵀ包含了所有描述系统动态的信息，那么最直接的控制方式就是根据所有这些状态变量的组合来决定控制输入。这被称为状态反馈，控制律为：

u = -K * X

其中，K = [k1, k2, k3, k4]是一个反馈增益矩阵。k1对应位置x的权重，k2对应角度θ的权重，k3对应速度ẋ的权重，k4对应角速度θ̇的权重。这个负号表示负反馈。

你会发现，PD控制器其实是状态反馈的一个特例：它只用了θ（对应k2）和θ̇（对应k4）这两个状态，而忽略了位置x和速度ẋ（即k1=0， k3=0）。

5.2 LQR：如何自动找到最优的K矩阵？

现在问题来了：K矩阵有四个增益参数，手动调试的复杂度呈指数上升。如何科学地确定一组“最优”的增益？LQR就是为了解决这个问题。

LQR的思想是定义一个衡量系统性能“好坏”的二次型代价函数：

J = ∫(Xᵀ * Q * X + uᵀ * R * u) dt

这个积分从0到无穷大时间。其中：

Xᵀ * Q * X惩罚状态偏离平衡点（0点）的程度。Q是一个半正定矩阵，对角线元素q11, q22, q33, q44分别代表我们对位置、角度、速度、角速度的重视程度。值越大，说明我们越不希望该状态量偏离。
uᵀ * R * u惩罚控制量的大小。R是一个正数（单输入系统），代表我们对控制能耗的重视程度。R越大，说明我们越希望使用小的控制量，避免电机饱和。

LQR理论证明，存在一个唯一的反馈增益矩阵K，能够最小化这个代价函数J。而这个K可以通过求解一个叫做Riccati的代数方程得到。幸运的是，在MATLAB、Python（Control库）甚至一些在线工具中，都有现成的函数（如lqr(A, B, Q, R)）可以一键求解。

5.3 LQR在智能车上的应用实践

确定模型(A, B)矩阵：这是最关键也最容易出错的一步。你需要相对准确地测量或估算出小车的质量、长度、转动惯量等物理参数，代入第3节线性化后的公式，计算出A和B矩阵。参数不准，得到的K效果会大打折扣。
设计Q和R矩阵：这是体现你控制“偏好”的地方。例如：
- 如果你只关心平衡，不关心车的位置漂移，可以把q11（位置权重）和q33（速度权重）设得很小，甚至为0，同时给q22（角度权重）和q44（角速度权重）设较大的值。
- 如果你希望车在平衡的同时还能稳稳地停在某一点（位置伺服），就需要给q11和q33赋予较大的权重。
- R的取值需要权衡。R太小，计算出的K增益会很大，可能导致控制量饱和；R太大，控制器过于“温柔”，响应慢。通常从1开始尝试，根据仿真或实际效果调整。
仿真验证：在将K矩阵烧录进小车前，务必在Simulink、Python或任何仿真环境中搭建模型进行测试。观察系统的阶跃响应、抗干扰能力，检查控制量是否饱和。
实物调试与微调：由于模型误差和现实噪声，理论计算出的最优K在实车上不一定是最佳的。但它提供了一个极好的起点。你可以以此为基础，对K中的各个增益进行小幅度的整体缩放或微调，效率远高于盲调PD参数。

注意事项：LQR控制要求能获取全部四个状态量。x和θ可以通过编码器和IMU（惯性测量单元）直接或间接测得。ẋ和θ̇可以通过对x和θ微分得到，但微分会放大噪声。更好的方法是使用状态观测器（如卡尔曼滤波器），它能够利用模型信息和传感器数据，最优地估计出无法直接测量或测量噪声大的状态（如速度），这是更高阶的玩法。

6. 实操中的核心环节与问题排查

理论再完美，落地到一块单片机和一堆传感器上时，总会遇到各种妖魔鬼怪。下面分享几个最关键环节的实操要点和常见问题。

6.1 传感器数据融合：姿态解算的基石

平衡控制的前提是获得准确、实时、低延迟的车体倾角θ和角速度θ̇。这主要依赖IMU（通常包含三轴加速度计和三轴陀螺仪）。

加速度计：测量的是比力，静态时可反映重力方向，从而计算出倾角。但对振动和运动加速度极其敏感，车一动，数据就“飘”了。
陀螺仪：测量角速度，积分可得角度。动态响应好，无运动加速度干扰，但存在零漂和积分累积误差，时间一长角度就“飞”了。

因此，必须进行数据融合。最经典的方法是互补滤波，其思想简洁而有效：

角度估计 = α * (陀螺仪积分角度) + (1-α) * (加速度计计算角度)

其中，α是一个介于0和1之间的滤波系数，通常取0.98左右。它的物理意义是：在高速动态时，相信陀螺仪；在低速静态时，相信加速度计。在单片机中实现一个一阶互补滤波器，代码量小，效果显著，是初学者的首选。

对于追求更高精度和稳定性的队伍，会采用卡尔曼滤波。它将系统模型和传感器噪声统计特性结合起来，提供最优估计。虽然数学模型复杂，但现在有很多开源库（如SimpleKalmanFilter）可以简化实现。

6.2 控制周期与实时性

平衡控制是一个对实时性要求极高的闭环系统。从读取传感器、解算姿态、计算控制量到输出PWM，必须在极短的时间内完成。

控制周期：建议至少达到5ms（200Hz），最好能达到2ms（500Hz）。周期越短，系统对扰动的响应越快，控制越“跟手”。
定时器中断：务必使用定时器中断来严格触发控制循环，保证周期恒定。避免使用delay()或非阻塞循环，它们会引入不确定的延迟。
传感器读取：优先选择支持SPI或高速I2C的IMU芯片，并确保在主循环中能以高于控制频率的速度读取数据。对于MPU6050这类常用芯片，开启DMP（数字运动处理器）单元可以减轻主控的运算负担。

6.3 常见问题排查速查表

现象	可能原因	排查思路与解决方法
车体高频抖动（“发抖”）	微分增益`Kd`过大；传感器噪声大；控制周期不稳定。	1. 适当减小`Kd`。 2. 检查并优化姿态解算滤波算法，增加滤波强度。 3. 确保使用定时器中断，并检查中断内代码执行时间是否超时。
车体缓慢单向倾倒	比例增益`Kp`过小；加速度计零点漂移未校准；机械重心不在轮轴正上方。	1. 适当增大`Kp`。 2. 上电静止时，采集加速度计数据求平均值作为零点偏移量，在程序中减去。 3. 检查机械结构，尽量降低重心并保证左右、前后对称。
响应迟钝，推一下恢复慢	`Kp`和`Kd`都偏小；电机驱动响应慢（死区大、频率低）。	1. 同时增大`Kp`和`Kd`（注意保持比例，避免振荡）。 2. 检查电机驱动桥的响应，提高PWM频率（通常8kHz-20kHz），减小死区设置。
启动瞬间“弹跳”或倒下	初始角度读取错误；控制器输出初始值不为零。	1. 系统上电后，保持车身直立静止1-2秒，让滤波器收敛，再启动控制循环。 2. 确保控制量输出初始化值为0，且从0开始平滑使能。
直线行走时严重跑偏	左右轮子直径、摩擦力或电机响应不一致；陀螺仪Z轴零漂。	1. 精细校准左右编码器脉冲当量。 2. 增加一个小的转向环PID，根据航向角偏差微调左右轮速差。 3. 定期（如每N个控制周期）更新陀螺仪零偏值。

6.4 从平衡到运动：速度环与转向环的引入

当车能稳定站立后，下一步就是让它动起来。这需要引入外环控制：

速度环：给定一个目标速度（可能是0，即定车；也可能是前进速度），将当前速度（由编码器测得）与目标速度比较，其误差经过一个PI控制器（注意，这里需要积分项I来消除静差），输出一个目标角度。这个目标角度再输入给内层的平衡环（PD或LQR）。也就是说，你想让车加速前进，实际上是让车“故意”前倾一个角度，平衡控制器为了维持这个前倾角度，就会驱动轮子向前加速，从而产生速度。这是一个非常巧妙且符合人体直觉的设计（人想骑车前进，也是先让身体前倾）。
转向环：通过陀螺仪的Z轴角速度或磁力计的航向角来获取当前转向率或航向。给定目标转向指令（如来自摄像头识别的路径偏差），通过一个PD控制器计算出左右轮的速度差，从而实现转向。

至此，一个完整的、具备平衡、速度、转向控制能力的智能车单车组控制系统框架就搭建起来了。理论分析为你提供了设计这个框架的蓝图和工具，而不断的调试、观察和思考，则是将蓝图变为现实的关键。