动量轮自平衡系统：角动量守恒与STM32实时控制

1. 动量轮自平衡系统的物理本质：角动量守恒的工程实现

在嵌入式控制系统中，自平衡机械装置的设计绝非仅依赖于PID算法的参数整定或传感器数据的滤波处理。其底层逻辑根植于经典力学的基本守恒定律——角动量守恒。对于动量轮自平衡自行车这类单输入多输出（SIMO）、强耦合、非线性的欠驱动系统，理解角动量守恒如何在刚体动力学层面转化为可工程实现的控制力矩，是构建稳定控制器的前提。本节将剥离教学视频中的口语化表达与类比演示，从刚体动力学建模出发，严格推导动量轮模块与车体之间的力矩耦合关系，明确各物理量的参考系定义、矢量方向约定及工程约束条件。

1.1 系统抽象与参考点选取：A点与O点的本质区别

实际硬件系统中，动量轮模块由无刷电机、轮体、轴承及安装支架构成一个刚性整体。该模块通过电机法兰盘刚性连接于车体主梁，其旋转轴线（即电机转子轴）在车体坐标系中保持水平且垂直于车体前进方向。为进行动力学分析，必须首先明确定义参考点（reference point）。

• 旋转中心A点 ：定义为动量轮模块的几何旋转轴心，即电机转子轴线与车体连接面的交点。该点是分析动量轮模块自身角动量的唯一有效参考点。所有关于动量轮模块绕其自身轴旋转的角动量计算、外力矩分析，均以A点为基准。
• 接触点O点 ：定义为后轮（或前轮，取决于设计）与地面的实际接触点。该点是分析整车姿态运动（俯仰角θ）的惯性参考点，也是地面摩擦力作用的位置。整车绕O点的俯仰运动直接决定了倾倒趋势。

二者不可混淆。A点是动量轮模块的 局部旋转中心 ，O点是整车的 全局姿态参考点 。角动量守恒定律的应用对象是动量轮模块这一刚体，其参考点必须是A点；而最终期望的控制效果——车体恢复直立——则表现为整车绕O点的俯仰角速度ωₚ的反向调节。理解A点与O点的物理分离及其动力学关联，是打通“电机转速指令”到“车体姿态稳定”这一控制链路的关键。

1.2 刚体角动量建模：Jω的物理意义与方向判定

动量轮模块并非质点，而是一个绕固定轴（通过A点）高速旋转的刚体。其角动量L_A（相对于A点）的表达式为：

$$ \mathbf{L}_A = J \cdot \boldsymbol{\omega} $$

其中：
- $ J $ 是动量轮模块绕其旋转轴（即通过A点的轴线）的 转动惯量 （Moment of Inertia），单位为kg·m²。它是一个标量，由模块的质量分布决定，计算公式为 $ J = \frac{1}{2} M R^2 $（对均质实心圆盘近似），M为模块总质量，R为等效旋转半径。在工程实现中，J并非理论值，而是需通过实验标定的系统参数，直接影响PID控制器中微分项的增益设定。
- $ \boldsymbol{\omega} $ 是动量轮模块绕A点的 角速度矢量 ，单位为rad/s。其大小$ \omega $由电机编码器或电调反馈获得，方向严格遵循 右手螺旋定则 ：四指沿轮体旋转方向弯曲，拇指所指即为$ \boldsymbol{\omega} $的方向。

在自行车坐标系中，若定义X轴为前进方向，Y轴为左侧方向，则动量轮旋转平面为X-Z平面（Z轴垂直向上）。此时，其旋转轴线平行于Y轴。因此：
- 当动量轮 顺时针 旋转（从车体左侧Y+方向观察）时，$ \boldsymbol{\omega} $ 指向-Y方向；
- 当动量轮 逆时针 旋转时，$ \boldsymbol{\omega} $ 指向+Y方向。

此方向定义至关重要。后续所有力矩方向、传感器坐标系对齐、以及PID输出符号的正负，均以此为基准。在STM32 HAL库中配置TIMx编码器接口或使用霍尔传感器读取电机转速时，必须确保其原始计数值的增减方向与上述$ \boldsymbol{\omega} $的矢量方向严格对应，否则控制器将产生完全相反的纠正力矩，导致系统彻底失稳。

1.3 外力矩为零的工程验证：为什么A点是角动量守恒的合法参考点

角动量守恒定律成立的充要条件是：刚体所受 合外力矩 （Net External Torque）相对于所选参考点为零。即：

$$ \sum \mathbf{M}_A = \mathbf{0} $$

对动量轮模块进行受力分析，其受到的外力仅有两个：
1. 重力 $ \mathbf{G} $ ：作用于模块质心C，方向竖直向下（-Z方向）。
2. 车体支撑力 $ \mathbf{N} $ ：作用于A点（因轴承内圈与轴过盈配合，力直接传递至旋转中心），方向沿车体结构件轴线，通常斜向上。

关键在于计算这两个力相对于A点的力矩：
- 重力 $ \mathbf{G} $ 的作用点C与A点存在一个微小偏移矢量 $ \mathbf{r} {CA} $。但 $ \mathbf{r} {CA} $ 在工程上被设计为极短（通常<5mm），且其方向基本沿Z轴。因此，$ \mathbf{M} G = \mathbf{r} {CA} \times \mathbf{G} $ 的大小为 $ |\mathbf{r} {CA}| \cdot |\mathbf{G}| \cdot \sin\phi $。由于 $ \mathbf{r} {CA} $ 与 $ \mathbf{G} $ 近乎共线（φ≈0°），故 $ \sin\phi \approx 0 $，$ \mathbf{M} G \approx \mathbf{0} $。
- 支撑力 $ \mathbf{N} $ 直接作用于A点，故其相对于A点的位置矢量 $ \mathbf{r} {NA} = \mathbf{0} $，因此 $ \mathbf{M}_N = \mathbf{0} \times \mathbf{N} = \mathbf{0} $。

综上，$ \sum \mathbf{M} A \approx \mathbf{0} $。这意味着动量轮模块绕A点的角动量是一个 守恒量 。在初始静止状态（$ \omega = 0 $），其角动量 $ \mathbf{L}_A = \mathbf{0} $。当电机施加驱动扭矩使其开始旋转时，根据牛顿第三定律，轮体对电机定子（即车体）必然产生一个大小相等、方向相反的 反作用扭矩 $ \mathbf{M} {react} $。这个 $ \mathbf{M}_{react} $ 并非凭空消失，而是通过电机外壳、安装支架，最终全部传递给车体，成为驱动车体绕O点俯仰运动的 控制力矩 。

1.4 从A点守恒到O点运动：反作用扭矩的传递路径与约束

动量轮模块角动量守恒（$ \mathbf{L}_A = \text{const} $）的直接后果，是其内部各部件（转子、定子）之间必须产生一对相互作用的角动量。设电机驱动转子获得角动量 $ \mathbf{L}_r = J_r \omega_r $，则定子（连同整个车体）必然获得一个大小相等、方向相反的角动量 $ \mathbf{L}_s = -J_s \omega_s $，以保证系统总角动量 $ \mathbf{L}_r + \mathbf{L}_s = \mathbf{0} $。

此处 $ J_s $ 并非车体总转动惯量，而是 车体绕A点的等效转动惯量 。由于A点并非车体质心，且车体下部被地面约束，$ J_s $ 的实际物理意义是：车体在A点处抵抗绕Y轴（即动量轮轴线）旋转的“刚度”。这个刚度主要由车体结构刚性、轮胎侧向变形及地面摩擦力共同决定。在控制系统中，$ J_s $ 与 $ J_r $ 的比值，直接决定了相同的电机电流所能产生的车体俯仰角加速度大小，是PID控制器中比例增益Kp的核心物理依据。

然而，车体的运动最终体现为绕O点的俯仰。O点与A点在空间上存在一个固定距离 $ d $（即A点距地面的高度）。当车体在 $ \mathbf{M} {react} $ 作用下试图绕A点旋转时，其底部（O点）受到地面静摩擦力的约束。根据刚体运动学，一个刚体在受到一个不通过其质心的力矩作用时，其运动是平动与转动的合成。此处，约束条件是O点的瞬时速度为零（理想静摩擦），这迫使车体的运动表现为：以O点为瞬时旋转中心的纯转动。因此，动量轮产生的 $ \mathbf{M} {react} $ 最终在O点处等效为一个能改变整车俯仰角速度 $ \dot{\theta} $ 的力矩 $ \mathbf{M}_O $，其大小满足：

$$ M_O = M_{react} \cdot \cos\alpha $$

其中α为 $ \mathbf{M} {react} $ 作用线与O-A连线的夹角。在典型安装结构中，α≈0°，故 $ M_O \approx M {react} $。这就是“小车往哪边倒，动量轮就往哪边转”的物理根源：倾倒方向（θ>0）意味着需要产生一个负的 $ \dot{\theta} $ 来纠正，这要求 $ M_O < 0 $，进而要求 $ M_{react} < 0 $，最终要求 $ \omega_r $ 的方向为负（即前述的-Y方向）。

2. 嵌入式系统架构：STM32F407的实时控制框架

将角动量守恒的物理原理转化为稳定的嵌入式控制，核心在于构建一个低延迟、高确定性的实时闭环。STM32F407系列MCU凭借其Cortex-M4内核、浮点单元（FPU）、丰富的定时器资源及高主频（最高168MHz），成为此类应用的理想平台。其系统架构并非简单地将传感器读取、PID计算、电机驱动串行执行，而是一个基于时间触发与事件驱动混合模型的精密协同系统。

2.1 时钟树与中断优先级：确定性执行的基石

整个控制环路的时序基准由RCC（Reset and Clock Control）模块提供。标准配置下，HSE（外部晶振）为8MHz，经PLL倍频后，系统时钟（SYSCLK）锁定为168MHz。APB1总线（挂载TIM2, TIM3, TIM4, USART2等）和APB2总线（挂载TIM1, USART1, GPIO等）分别运行在42MHz和84MHz。此配置确保了：
- 定时器计数器的最小分辨率高达23.8ns（1/42MHz），为精确测量编码器脉冲提供了硬件基础；
- GPIO翻转、ADC采样等操作可在数个CPU周期内完成，最大限度减少软件开销。

中断优先级分组采用NVIC_PriorityGroup_2（2位抢占优先级，2位子优先级），这是实现硬实时的关键。具体分配如下：
- 最高优先级（0）：TIM1 UP/DOWN 中断 。TIM1被配置为编码器接口模式，用于采集动量轮电机的AB相正交编码器信号。其更新中断（UPDATE）在每次计数器溢出/下溢时触发，用于获取当前转速ω_r。将其置于最高优先级，确保转速采样绝对准时，任何其他中断（包括SysTick）均不得打断。
- 次高优先级（1）：TIM3 CC1 中断 。TIM3的通道1被配置为PWM捕获，用于接收MPU6050陀螺仪的DMP（Digital Motion Processor）中断信号。该信号在DMP完成一次姿态解算（如6轴融合）后拉低，TIM3在检测到下降沿时触发中断，启动姿态数据读取。此中断必须紧随TIM1之后，以保证姿态更新与转速更新的时序一致性。
- 中优先级（2）：USART2 RXNE 中断 。用于接收上位机调试命令或外部传感器数据，其响应延迟容忍度较高，故优先级低于前述两者。
- 最低优先级（3）：SysTick 中断 。仅用于FreeRTOS的系统滴答，不参与核心控制环路。

这种严格的中断嵌套策略，确保了控制环路的两个核心输入——转速ω_r与俯仰角θ——的采样时刻具有严格的周期性与同步性，避免了因中断延迟导致的相位滞后，这是PID控制器稳定工作的前提。

2.2 传感器数据融合：MPU6050的DMP与自研互补滤波

姿态感知是自平衡的“眼睛”。本系统选用MPU6050作为核心IMU，其内部集成的DMP（Digital Motion Processor）硬件协处理器是提升系统鲁棒性的关键。

• DMP工作模式 ：MPU6050的DMP固件被预加载，配置为6轴姿态解算（Accelerometer + Gyroscope）。DMP以50Hz的固定频率（可通过寄存器修改）进行数据融合，并在解算完成后，通过INT引脚向STM32发出中断信号。STM32在TIM3捕获中断中，通过I²C总线（使用HAL_I2C_Master_TransmitReceive）读取DMP输出的四元数（q0, q1, q2, q3）。四元数相比欧拉角，完全规避了万向节锁（Gimbal Lock）问题，且计算效率更高。

• 俯仰角θ的提取 ：由四元数计算俯仰角（Pitch）的公式为：
  $$ \theta = \arcsin(2(q_0 q_1 + q_2 q_3)) $$
  此计算在中断服务函数（ISR）中完成，结果直接存入全局变量 pitch_angle 。为防止因I²C通信抖动导致的瞬时跳变，在主循环中会对此值进行一阶低通滤波： pitch_angle_filtered = alpha * pitch_angle + (1-alpha) * pitch_angle_filtered ，其中alpha=0.95。

• 角速度ω_p的获取 ：陀螺仪原始数据（dps）虽存在漂移，但其高频动态响应远优于加速度计。因此，俯仰角速度 pitch_rate 直接取自陀螺仪Y轴（即绕X轴旋转的角速度）的原始值，经单位换算（dps → rad/s）后，同样进行一阶滤波。此值用于PID控制器的微分项，对抑制超调至关重要。

2.3 控制算法执行：双环PID的工程化落地

控制律采用经典的串级PID结构，分为外环（姿态环）与内环（速度环）。

• 外环（姿态环） ：以目标俯仰角θ_set（通常为0°）与测量值 pitch_angle_filtered 的误差e_θ为输入，输出目标角速度ω_set。其离散化PID公式为：
  $$ \omega_{set}[k] = K_{p\theta} \cdot e_\theta[k] + K_{i\theta} \cdot \sum_{i=0}^{k} e_\theta[i] \cdot T_s + K_{d\theta} \cdot \frac{e_\theta[k] - e_\theta[k-1]}{T_s} $$
  其中T_s为控制周期（5ms）。Kpθ、Kiθ、Kdθ是核心调参参数。Kpθ过大将导致剧烈振荡；Kiθ用于消除静态误差，但过大会引起积分饱和；Kdθ则直接抑制超调，其值需与陀螺仪噪声水平匹配。

• 内环（速度环） ：以外环输出的ω_set与动量轮实测转速ω_r（由TIM1编码器中断计算得出）的误差e_ω为输入，输出PWM占空比指令。其公式为：
  $$ Duty[k] = K_{p\omega} \cdot e_\omega[k] + K_{i\omega} \cdot \sum_{i=0}^{k} e_\omega[i] \cdot T_s + K_{d\omega} \cdot \frac{e_\omega[k] - e_\omega[k-1]}{T_s} $$
  内环的带宽必须显著高于外环（通常快5-10倍），以确保外环的指令能被快速、准确地跟踪。Kpω直接决定了系统的响应速度，是影响整车稳定性的首要参数。

在STM32F407上，该算法在主循环（while(1)）中执行。为保证计算确定性，所有浮点运算均启用FPU（通过编译器选项 -mfpu=vfp 和 -mfloat-abi=hard ），并禁用除法（使用查表或移位替代）。PID计算结果经限幅（ Duty = MAX(0, MIN(100, Duty)) ）后，通过HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1)更新TIM1的CCR1寄存器，驱动无刷电调。

3. 硬件接口与驱动：无刷电机电调的精准控制

动量轮的执行机构是无刷直流电机（BLDC），其驱动依赖于电子调速器（ESC）。STM32F407并不直接驱动电机，而是通过发送标准PWM信号（50Hz，1000-2000μs）来控制ESC的输出。这一看似简单的接口，却蕴含着诸多工程细节。

3.1 PWM信号规范与TIM1配置

ESC接受的PWM信号具有严格的时间参数：
- 周期（Period） ：20ms（对应50Hz刷新率）。此周期必须高度稳定，任何抖动都会导致ESC误判，引发电机抖动或停转。
- 脉宽（Pulse Width） ：
- 1000μs：对应电机停转（Throttle Min）。
- 1500μs：对应电机中立点（Throttle Neutral），通常为0rpm。
- 2000μs：对应电机满转（Throttle Max）。

在STM32中，TIM1被配置为向上计数模式，其自动重装载寄存器（ARR）设置为 20000-1 （假设时钟为100MHz，1μs/计数），从而精确生成20ms周期。比较寄存器（CCR1）则动态更新，其值直接对应脉宽（单位：μs）。例如， htim1.Instance->CCR1 = 1500; 即输出1500μs脉宽。

关键点在于 更新事件（Update Event）的同步 。必须确保CCR1的更新发生在每个周期的起始点，而非任意时刻。这通过启用TIM1的“影子寄存器”（Shadow Register）功能实现：将CCR1配置为预装载（ OCPreload_Enable ），并在每次更新事件（UEV）到来时，将预装载值一次性拷贝到活动寄存器。UEV由ARR溢出触发，完美契合20ms周期。

3.2 ESC初始化与安全协议

大多数消费级ESC在上电后需要一个特定的“握手”序列才能进入正常工作模式，否则会拒绝响应PWM信号。这是一个常被初学者忽略的致命细节。

标准初始化流程为：
1. 上电后，首先将PWM输出置为1000μs（停转），持续至少2秒。这相当于向ESC发送“油门归零”指令，使其进入待机状态。
2. 然后，缓慢地将PWM从1000μs线性增加至2000μs（满油门），再缓慢降至1000μs。此过程模拟遥控器的校准动作，ESC内部MCU会记录此范围作为其工作区间。
3. 完成后，ESC发出“哔”声，表示校准成功，进入待命状态。

在STM32代码中，此流程必须在 main() 函数的 HAL_Init() 和 SystemClock_Config() 之后、 MX_GPIO_Init() 之前执行。使用一个独立的 for 循环配合 HAL_Delay(1) 实现毫秒级延时，并通过 __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pulse_width); 更新CCR1。 切勿在此过程中开启TIM1的PWM输出 ，必须确保TIM1在初始化阶段处于关闭状态（ HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1) ），待校准序列完成后，再调用 HAL_TIM_PWM_Start() 。

3.3 电流与温度保护：硬件看门狗的必要性

无刷电机在大扭矩启动或堵转时，电流会急剧上升，可能烧毁ESC或电机。虽然ESC自身有过流保护，但其响应时间（通常为毫秒级）可能慢于STM32的控制周期。因此，必须引入硬件级冗余保护。

• 电流检测 ：在ESC的电源输入端串联一个0.01Ω的康铜分流电阻，其两端电压经运放（如LM358）放大100倍后，接入STM32的ADC1_IN0通道。在主循环中，每10ms采样一次，若连续3次读数超过阈值（如2.5V，对应25A），则立即调用 HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1) ，强制关断电机，并点亮红色LED报警。
• 温度监控 ：在ESC散热片上粘贴NTC热敏电阻，其阻值随温度升高而降低。通过分压电路将阻值变化转换为电压，接入ADC1_IN1。软件中建立查表法（LUT）将电压映射为温度，当温度>80°C时，启动降额控制：将PID输出的Duty值乘以一个随温度线性下降的系数（如0.8），直至温度回落。

这些保护措施必须独立于主控制环路，其判断逻辑应尽可能简洁，避免任何浮点运算或复杂分支，以确保在极端工况下仍能及时响应。

4. 调试与标定：从理论到实战的必经之路

再完美的理论模型，也必须经过严苛的物理世界检验。动量轮自平衡系统的调试是一个迭代、渐进、充满经验的过程，绝非一蹴而就。

4.1 首次上电：从“不动”到“微动”的临界点突破

首次给系统上电时，最大的风险不是失控，而是 完全无响应 。这通常源于三个原因：
1. ESC未校准 ：如前所述，必须严格执行2秒停转→升至满油→降回停转的初始化序列。一个常见的错误是，开发者在 main() 中忘记加入此序列，直接启动了PWM，导致ESC始终处于“未识别”状态。
2. 编码器相序错误 ：动量轮电机的AB相编码器线若接反，TIM1将无法正确识别旋转方向，导致 ω_r 的符号恒为负或恒为正。此时，PID控制器会持续输出最大正向或负向指令，但电机实际转速与指令相反，系统表现为“死锁”。解决方法是，在 main() 中先不启动PID，仅让TIM1以固定频率（如1kHz）输出一个方波，手动旋转动量轮，用示波器观察PA0（A相）与PA1（B相）的波形相位关系。正确的相序应为：当轮体顺时针旋转时，A相上升沿领先B相上升沿90°。
3. MPU6050 I²C地址错误 ：MPU6050的AD0引脚接地时，I²C地址为0x68；悬空时为0x69。若HAL_I2C_IsDeviceReady返回失败，首要检查此引脚电平及I²C线路的上拉电阻（通常为4.7kΩ）。

一旦确认硬件连接无误，下一步是让系统“微动”。将PID所有增益（Kp, Ki, Kd）设为0，仅保留一个极小的Kpθ（如0.1）。此时，系统不会平衡，但当你用手轻轻扶正车体并松手，会看到动量轮有微弱的、与倾倒方向一致的旋转。这表明角动量守恒的物理效应已激活，控制链路初步贯通。

4.2 PID参数整定：Ziegler-Nichols法的工程变体

面对一个非线性、时变的物理系统，经典的Ziegler-Nichols临界比例度法（Critical Proportionality Method）仍是最有效的起点。

1. 寻找临界增益Ku ：将Ki与Kd设为0，仅增大Kpθ，直至系统在平衡点附近产生 等幅振荡 。记录此时的Kp值，即为Ku。注意，振荡必须是稳定的、幅度不衰减也不发散的。若振荡发散，说明已超过Ku，需回调。
2. 测量振荡周期Tu ：用示波器或逻辑分析仪捕获 pitch_angle_filtered 信号，测量其相邻两个峰值之间的时间，即为Tu（通常在1.5-3秒范围内）。
3. 计算初始参数 ：根据Z-N公式，姿态环初始参数为：
   - $ K_{p\theta} = 0.6 \cdot K_u $
   - $ K_{i\theta} = 1.2 \cdot K_u / T_u $
   - $ K_{d\theta} = 0.075 \cdot K_u \cdot T_u $

随后，进入精细调整阶段：
- Kpθ ：增大可提高响应速度，但过大会导致高频抖动。最佳值是系统在快速响应与平稳性之间取得平衡的点。
- Kdθ ：这是抑制超调的“刹车”。若系统在纠正倾倒后出现明显过冲（overshoot），则增大Kdθ。但Kdθ对传感器噪声极度敏感，增大时必须同步加强陀螺仪数据的滤波强度。
- Kiθ ：用于消除长期存在的静态误差（如车体轻微倾斜）。若系统在长时间运行后，平衡点缓慢漂移，可微增Kiθ。但必须配合积分抗饱和（Integral Anti-Windup）机制，即当Duty输出达到限幅值（0或100%）时，暂停Kiθ的累加。

4.3 现场经验：那些教科书不会告诉你的坑

• 地面摩擦力的影响 ：在光滑瓷砖上，系统可能表现得异常“灵敏”，而在粗糙水泥地上，同样的PID参数会导致响应迟钝。这是因为O点的摩擦力矩是控制力矩的最终负载。解决方案是，在不同地面类型上，分别标定一套PID参数，并通过一个拨码开关或蓝牙指令进行切换。
• 电池电压衰减 ：锂电池从4.2V放电至3.3V的过程中，ESC的输出扭矩会线性下降。若PID参数基于满电状态整定，那么在低电量时，系统将因驱动力不足而失稳。一个实用的补救措施是，在PID计算中，将Duty输出乘以一个电压补偿系数： compensation = V_bat / 4.2 。 V_bat 通过ADC定期采样电池总电压获得。
• 电机磁滞效应 ：无刷电机在低速（<500rpm）时，存在明显的磁滞与死区，导致小角度倾倒时，系统“反应迟钝”。解决方法是在PID输出后，增加一个死区补偿（Dead Zone Compensation）：当 |e_ω| < threshold 时，强制将Duty设为一个微小的正值（如5%），以克服静摩擦。

我在实际项目中曾遇到一个典型案例：系统在实验室调试完美，但搬到展会现场后，因空调冷凝水导致地面湿滑，摩擦力骤降，原有的Kpθ参数使系统变得过于激进，最终在一次微小扰动后彻底失控。那次经历让我深刻认识到，一个真正可靠的嵌入式控制系统，其鲁棒性不仅体现在算法上，更体现在对物理环境的敬畏与适应能力上。