1. 项目概述从“会跑”到“站得住”的跨越搞智能车竞赛的兄弟们都清楚单车组也叫独轮车或平衡车组别是整个比赛里技术门槛最高、也最“秀”的一个方向。别的车四个轮子或者两个轮子着地首要任务是跑得快、拐得稳而单车组你的车得先学会“站”起来。这个“站”就是平衡控制。我当年带队做这个项目第一周全组人都在跟陀螺仪和加速度计的数据较劲车不是往前“啪”一下拍地上就是往后猛仰过去场面一度十分滑稽。所以今天我们不聊具体代码和电路就深挖一下这个让车“站起来”的核心——平衡控制的理论基础。这就像练武招式代码可以千变万化但内功心法理论不通永远成不了高手。这篇文章就是帮你把这份“内功心法”捋明白让你知道每一个参数调整背后到底在跟物理世界的哪些规律较劲。2. 核心需求解析平衡的本质是什么在深入数学公式之前我们得先想明白我们要解决的到底是个什么问题。一辆两轮自平衡车你可以把它想象成一个倒立摆。一根杆子车身底下用一个轮子或两个并排很近的轮子等效为一个支点支撑着。这个系统天然是不稳定的就像你用手指顶着一根棍子稍微一偏重力就会把它拉倒。我们的目标就是通过控制底部的轮子前后移动来对抗重力的影响让这根“棍子”车身始终保持竖直或者稳定在一个我们期望的倾斜角度上。所以平衡控制的核心需求可以分解为两个层次静态平衡让车在静止状态下保持直立。这是最基本也是最难的一步因为任何微小的扰动如地面不平、电机微小的力矩波动都会破坏平衡。动态平衡在静态平衡的基础上让车能够按照我们的指令前进、后退、转弯。这时平衡控制不仅要抵抗重力还要处理因运动产生的惯性力、离心力等额外的干扰。为了实现这些我们需要三样东西感知知道车现在“歪”了多少、决策计算需要让轮子怎么动来纠正这个“歪”、执行驱动电机产生准确的力矩。理论分析主要聚焦在“决策”环节即如何根据“感知”到的姿态信息计算出“执行”机构需要的控制量。3. 动力学建模把物理世界“翻译”成数学方程理论分析的第一步是建模。我们需要用数学方程来描述这个倒立摆系统的运动规律。这是整个控制理论的基石模型建得准后面的控制器设计才有意义。常见的建模方法有牛顿-欧拉法和拉格朗日法。对于智能车这样的简单系统牛顿-欧拉法更直观。3.1 模型简化与假设为了抓住主要矛盾我们通常先建立一个简单的平面模型只考虑车子在前进方向上的俯仰运动忽略侧倾和横摆并做以下合理假设车身和轮子都是刚体。电机驱动轮子且电机响应足够快我们可以认为控制输入直接是轮子的转矩。车体绕轮轴的转动惯量、质量分布等参数可以通过测量或估算得到。3.2 建立运动方程我们设θ车体与竖直方向的夹角俯仰角。θ0表示完全直立前倾为正后仰为负。x轮子中心点的水平位移。τ电机作用在轮子上的转矩控制输入。M,m分别为轮子质量和车体质量。R,L分别为轮子半径和车体质心到轮轴的距离。I_w,I_b分别为轮子绕其轴心的转动惯量和车体绕其质心的转动惯量。通过对轮子和车体分别进行受力分析并应用牛顿第二定律和转动定律我们可以推导出描述θ和x的两个耦合的非线性微分方程。这个过程涉及一些矢量运算和受力分解是理论分析中最“硬核”的部分。最终得到的方程形式大致如下(M m) * ẍ m*L*cosθ * θ̈ - m*L*sinθ * (θ̇)² (τ / R) (m*L*cosθ) * ẍ (I_b m*L²) * θ̈ - m*g*L*sinθ -τ这里ẍ和θ̈分别是位移和角度的二阶导即加速度和角加速度θ̇是角速度的一阶导。g是重力加速度。这两个方程清晰地展示了系统的非线性存在sinθ,cosθ,(θ̇)²项和耦合性ẍ和θ̈相互影响。注意很多同学看到这一堆符号和方程就头大。其实关键在于理解其物理意义第一个方程本质是轮子水平方向的“力平衡”控制转矩τ转化为了轮子前进的力。第二个方程是车体绕轮轴的“力矩平衡”重力m*g*L*sinθ是导致倾倒的主要力矩而电机转矩τ和轮子加速度带来的惯性力共同作用来平衡它。耦合项m*L*cosθ * θ̈意味着车体的转动会影响到轮子的平动反之亦然。3.3 线性化处理上面的非线性方程直接用来设计控制器非常困难。好在对于平衡控制我们主要关心的是θ在0度直立点附近的小范围运动。此时我们可以应用小角度假设当θ很小时sinθ ≈ θcosθ ≈ 1并且(θ̇)²作为高阶小量可以忽略。这样复杂的非线性方程就被简化成了线性的状态空间方程形式如下状态向量 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 - θ这里θ_desired0来生成控制量电机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、角度θ、速度ẋ、角速度θ̇。当车需要同时进行平衡和位置控制比如停在指定点时PD控制器难以协调容易顾此失彼。抗干扰性依赖参数对于大幅值或持续性的外部干扰如强风、持续坡道PD控制的恢复能力完全取决于Kp和Kd的取值没有最优保证。正因为这些局限性在追求更高性能如更快的响应、更强的抗扰、同时实现平衡与循迹时我们就需要更高级的控制策略。5. 进阶之路状态空间与LQR控制当PD控制无法满足你的性能要求或者你想从原理上更深刻地掌控你的车时状态空间方法和线性二次型调节器LQR是一个优雅而强大的工具。它直接基于我们第3节中建立的线性化状态空间模型。5.1 状态反馈控制核心思想是既然我们的系统状态X [x, θ, ẋ, θ̇]ᵀ包含了所有描述系统动态的信息那么最直接的控制方式就是根据所有这些状态变量的组合来决定控制输入。这被称为状态反馈控制律为u -K * X其中K [k1, k2, k3, k4]是一个反馈增益矩阵。k1对应位置x的权重k2对应角度θ的权重k3对应速度ẋ的权重k4对应角速度θ̇的权重。这个负号表示负反馈。你会发现PD控制器其实是状态反馈的一个特例它只用了θ对应k2和θ̇对应k4这两个状态而忽略了位置x和速度ẋ即k10 k30。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、PythonControl库甚至一些在线工具中都有现成的函数如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和θ微分得到但微分会放大噪声。更好的方法是使用状态观测器如卡尔曼滤波器它能够利用模型信息和传感器数据最优地估计出无法直接测量或测量噪声大的状态如速度这是更高阶的玩法。6. 实操中的核心环节与问题排查理论再完美落地到一块单片机和一堆传感器上时总会遇到各种妖魔鬼怪。下面分享几个最关键环节的实操要点和常见问题。6.1 传感器数据融合姿态解算的基石平衡控制的前提是获得准确、实时、低延迟的车体倾角θ和角速度θ̇。这主要依赖IMU通常包含三轴加速度计和三轴陀螺仪。加速度计测量的是比力静态时可反映重力方向从而计算出倾角。但对振动和运动加速度极其敏感车一动数据就“飘”了。陀螺仪测量角速度积分可得角度。动态响应好无运动加速度干扰但存在零漂和积分累积误差时间一长角度就“飞”了。因此必须进行数据融合。最经典的方法是互补滤波其思想简洁而有效角度估计 α * (陀螺仪积分角度) (1-α) * (加速度计计算角度)其中α是一个介于0和1之间的滤波系数通常取0.98左右。它的物理意义是在高速动态时相信陀螺仪在低速静态时相信加速度计。在单片机中实现一个一阶互补滤波器代码量小效果显著是初学者的首选。对于追求更高精度和稳定性的队伍会采用卡尔曼滤波。它将系统模型和传感器噪声统计特性结合起来提供最优估计。虽然数学模型复杂但现在有很多开源库如SimpleKalmanFilter可以简化实现。6.2 控制周期与实时性平衡控制是一个对实时性要求极高的闭环系统。从读取传感器、解算姿态、计算控制量到输出PWM必须在极短的时间内完成。控制周期建议至少达到5ms200Hz最好能达到2ms500Hz。周期越短系统对扰动的响应越快控制越“跟手”。定时器中断务必使用定时器中断来严格触发控制循环保证周期恒定。避免使用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控制器计算出左右轮的速度差从而实现转向。至此一个完整的、具备平衡、速度、转向控制能力的智能车单车组控制系统框架就搭建起来了。理论分析为你提供了设计这个框架的蓝图和工具而不断的调试、观察和思考则是将蓝图变为现实的关键。
1. 项目概述从“会跑”到“站得住”的跨越搞智能车竞赛的兄弟们都清楚单车组也叫独轮车或平衡车组别是整个比赛里技术门槛最高、也最“秀”的一个方向。别的车四个轮子或者两个轮子着地首要任务是跑得快、拐得稳而单车组你的车得先学会“站”起来。这个“站”就是平衡控制。我当年带队做这个项目第一周全组人都在跟陀螺仪和加速度计的数据较劲车不是往前“啪”一下拍地上就是往后猛仰过去场面一度十分滑稽。所以今天我们不聊具体代码和电路就深挖一下这个让车“站起来”的核心——平衡控制的理论基础。这就像练武招式代码可以千变万化但内功心法理论不通永远成不了高手。这篇文章就是帮你把这份“内功心法”捋明白让你知道每一个参数调整背后到底在跟物理世界的哪些规律较劲。2. 核心需求解析平衡的本质是什么在深入数学公式之前我们得先想明白我们要解决的到底是个什么问题。一辆两轮自平衡车你可以把它想象成一个倒立摆。一根杆子车身底下用一个轮子或两个并排很近的轮子等效为一个支点支撑着。这个系统天然是不稳定的就像你用手指顶着一根棍子稍微一偏重力就会把它拉倒。我们的目标就是通过控制底部的轮子前后移动来对抗重力的影响让这根“棍子”车身始终保持竖直或者稳定在一个我们期望的倾斜角度上。所以平衡控制的核心需求可以分解为两个层次静态平衡让车在静止状态下保持直立。这是最基本也是最难的一步因为任何微小的扰动如地面不平、电机微小的力矩波动都会破坏平衡。动态平衡在静态平衡的基础上让车能够按照我们的指令前进、后退、转弯。这时平衡控制不仅要抵抗重力还要处理因运动产生的惯性力、离心力等额外的干扰。为了实现这些我们需要三样东西感知知道车现在“歪”了多少、决策计算需要让轮子怎么动来纠正这个“歪”、执行驱动电机产生准确的力矩。理论分析主要聚焦在“决策”环节即如何根据“感知”到的姿态信息计算出“执行”机构需要的控制量。3. 动力学建模把物理世界“翻译”成数学方程理论分析的第一步是建模。我们需要用数学方程来描述这个倒立摆系统的运动规律。这是整个控制理论的基石模型建得准后面的控制器设计才有意义。常见的建模方法有牛顿-欧拉法和拉格朗日法。对于智能车这样的简单系统牛顿-欧拉法更直观。3.1 模型简化与假设为了抓住主要矛盾我们通常先建立一个简单的平面模型只考虑车子在前进方向上的俯仰运动忽略侧倾和横摆并做以下合理假设车身和轮子都是刚体。电机驱动轮子且电机响应足够快我们可以认为控制输入直接是轮子的转矩。车体绕轮轴的转动惯量、质量分布等参数可以通过测量或估算得到。3.2 建立运动方程我们设θ车体与竖直方向的夹角俯仰角。θ0表示完全直立前倾为正后仰为负。x轮子中心点的水平位移。τ电机作用在轮子上的转矩控制输入。M,m分别为轮子质量和车体质量。R,L分别为轮子半径和车体质心到轮轴的距离。I_w,I_b分别为轮子绕其轴心的转动惯量和车体绕其质心的转动惯量。通过对轮子和车体分别进行受力分析并应用牛顿第二定律和转动定律我们可以推导出描述θ和x的两个耦合的非线性微分方程。这个过程涉及一些矢量运算和受力分解是理论分析中最“硬核”的部分。最终得到的方程形式大致如下(M m) * ẍ m*L*cosθ * θ̈ - m*L*sinθ * (θ̇)² (τ / R) (m*L*cosθ) * ẍ (I_b m*L²) * θ̈ - m*g*L*sinθ -τ这里ẍ和θ̈分别是位移和角度的二阶导即加速度和角加速度θ̇是角速度的一阶导。g是重力加速度。这两个方程清晰地展示了系统的非线性存在sinθ,cosθ,(θ̇)²项和耦合性ẍ和θ̈相互影响。注意很多同学看到这一堆符号和方程就头大。其实关键在于理解其物理意义第一个方程本质是轮子水平方向的“力平衡”控制转矩τ转化为了轮子前进的力。第二个方程是车体绕轮轴的“力矩平衡”重力m*g*L*sinθ是导致倾倒的主要力矩而电机转矩τ和轮子加速度带来的惯性力共同作用来平衡它。耦合项m*L*cosθ * θ̈意味着车体的转动会影响到轮子的平动反之亦然。3.3 线性化处理上面的非线性方程直接用来设计控制器非常困难。好在对于平衡控制我们主要关心的是θ在0度直立点附近的小范围运动。此时我们可以应用小角度假设当θ很小时sinθ ≈ θcosθ ≈ 1并且(θ̇)²作为高阶小量可以忽略。这样复杂的非线性方程就被简化成了线性的状态空间方程形式如下状态向量 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 - θ这里θ_desired0来生成控制量电机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、角度θ、速度ẋ、角速度θ̇。当车需要同时进行平衡和位置控制比如停在指定点时PD控制器难以协调容易顾此失彼。抗干扰性依赖参数对于大幅值或持续性的外部干扰如强风、持续坡道PD控制的恢复能力完全取决于Kp和Kd的取值没有最优保证。正因为这些局限性在追求更高性能如更快的响应、更强的抗扰、同时实现平衡与循迹时我们就需要更高级的控制策略。5. 进阶之路状态空间与LQR控制当PD控制无法满足你的性能要求或者你想从原理上更深刻地掌控你的车时状态空间方法和线性二次型调节器LQR是一个优雅而强大的工具。它直接基于我们第3节中建立的线性化状态空间模型。5.1 状态反馈控制核心思想是既然我们的系统状态X [x, θ, ẋ, θ̇]ᵀ包含了所有描述系统动态的信息那么最直接的控制方式就是根据所有这些状态变量的组合来决定控制输入。这被称为状态反馈控制律为u -K * X其中K [k1, k2, k3, k4]是一个反馈增益矩阵。k1对应位置x的权重k2对应角度θ的权重k3对应速度ẋ的权重k4对应角速度θ̇的权重。这个负号表示负反馈。你会发现PD控制器其实是状态反馈的一个特例它只用了θ对应k2和θ̇对应k4这两个状态而忽略了位置x和速度ẋ即k10 k30。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、PythonControl库甚至一些在线工具中都有现成的函数如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和θ微分得到但微分会放大噪声。更好的方法是使用状态观测器如卡尔曼滤波器它能够利用模型信息和传感器数据最优地估计出无法直接测量或测量噪声大的状态如速度这是更高阶的玩法。6. 实操中的核心环节与问题排查理论再完美落地到一块单片机和一堆传感器上时总会遇到各种妖魔鬼怪。下面分享几个最关键环节的实操要点和常见问题。6.1 传感器数据融合姿态解算的基石平衡控制的前提是获得准确、实时、低延迟的车体倾角θ和角速度θ̇。这主要依赖IMU通常包含三轴加速度计和三轴陀螺仪。加速度计测量的是比力静态时可反映重力方向从而计算出倾角。但对振动和运动加速度极其敏感车一动数据就“飘”了。陀螺仪测量角速度积分可得角度。动态响应好无运动加速度干扰但存在零漂和积分累积误差时间一长角度就“飞”了。因此必须进行数据融合。最经典的方法是互补滤波其思想简洁而有效角度估计 α * (陀螺仪积分角度) (1-α) * (加速度计计算角度)其中α是一个介于0和1之间的滤波系数通常取0.98左右。它的物理意义是在高速动态时相信陀螺仪在低速静态时相信加速度计。在单片机中实现一个一阶互补滤波器代码量小效果显著是初学者的首选。对于追求更高精度和稳定性的队伍会采用卡尔曼滤波。它将系统模型和传感器噪声统计特性结合起来提供最优估计。虽然数学模型复杂但现在有很多开源库如SimpleKalmanFilter可以简化实现。6.2 控制周期与实时性平衡控制是一个对实时性要求极高的闭环系统。从读取传感器、解算姿态、计算控制量到输出PWM必须在极短的时间内完成。控制周期建议至少达到5ms200Hz最好能达到2ms500Hz。周期越短系统对扰动的响应越快控制越“跟手”。定时器中断务必使用定时器中断来严格触发控制循环保证周期恒定。避免使用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控制器计算出左右轮的速度差从而实现转向。至此一个完整的、具备平衡、速度、转向控制能力的智能车单车组控制系统框架就搭建起来了。理论分析为你提供了设计这个框架的蓝图和工具而不断的调试、观察和思考则是将蓝图变为现实的关键。
