1. 项目概述“简易双轮足平衡机器人”并非传统意义上的轮式移动平台或四足仿生机构而是一种融合双轮自平衡原理与多自由度腿部结构的混合构型实验平台。其核心设计意图明确指向控制算法研究与嵌入式系统工程实践——以物理载体为依托构建一个可扩展、可验证、可复现的实时控制试验床。项目采用经典的“大小脑”分层控制架构下位机承担高实时性任务姿态感知、底层电机闭环、运动学解算上位机RK3566泰山派负责复杂逻辑调度、路径规划、强化学习策略部署及人机交互。这种软硬件解耦的设计思路既规避了单一MCU资源瓶颈对高级算法的制约又保留了底层控制回路的确定性与时效性。项目定位清晰非竞赛导向如RoboMaster、RoboCon、非商用产品原型而是面向嵌入式控制工程师与算法研究者的教学-研究一体化平台。其价值不在于最终形态的完成度而在于各子系统间接口定义的规范性、硬件资源分配的合理性、以及控制链路中关键瓶颈的暴露与解决过程。正如项目说明中所强调“小车只是一个载体上面可以有很多技术实现”本项目将双轮平衡作为基础动态约束条件将六自由度腿部结构作为执行末端本质上是在构建一个具备强动态耦合特性的机电系统为后续LQR最优控制、EKF状态估计、乃至基于Mujoco的强化学习训练提供真实物理边界与数据接口。2. 系统架构与硬件设计2.1 整体架构分层系统采用三级物理层级堆叠设计底层执行层由两套三自由度腿部模块构成每条腿包含髋关节俯仰偏航、膝关节屈伸共三个主动自由度驱动单元为宇树GO-M8010、伺泰威GIM-6010、达妙DMH6215等不同厂商伺服电机。腿部通过法兰轴承初始选用5×13×4mm规格与连杆机构刚性连接整体结构借鉴本末科技“刑天”机器人的多连杆拓扑兼顾运动包络与结构刚度。中层控制层即下位机主控板基于STM32H743XIH6微控制器承担全部实时控制任务。该层通过CAN总线与各伺服电机通信通过SPI/I2C采集IMU与GNSS数据通过USB与上位机交换指令与状态信息。上层决策层RK3566泰山派开发板运行Linux系统作为算法容器与人机交互终端。其通过USB虚拟串口接收下位机上传的传感器原始数据流并下发高层运动指令如目标倾角、步态相位、关节轨迹点。三层之间通过明确定义的通信协议进行数据交换物理隔离确保了实时性与安全性的边界划分。2.2 下位机主控板设计主控板设计以“高带宽、低延迟、强扩展”为原则围绕STM32H743XIH6的全功能外设展开重点解决多源异步数据融合与高速电机闭环控制两大挑战。2.2.1 电源管理子系统电源输入采用XT60接口支持8V–27V宽压范围满足锂电池组3S–6S及适配器供电需求。输入级集成三重保护防反接采用P沟道MOSFET如SI2301构成无损反向阻断电路过压钳位TVS二极管如SMAJ24A配合瞬态抑制网络缓启动NTC热敏电阻串联限流配合MOSFET栅极RC延时电路抑制上电浪涌电流。主电源通路采用矽力杰SY8368QNC同步降压控制器输入8V–28V输出5V/8A。该芯片内置上下桥MOSFET支持强制PWM模式以保证全负载范围内恒定开关频率提升EMI可控性并集成过流、过温、欠压锁定保护。PCB布局时功率环路VIN→SW→GND→CIN采用最短路径、大铜厚走线并在背面预留散热片安装区域输出滤波电容固态电解陶瓷阵列紧邻芯片放置降低高频纹波。5V电源经两级LDO如AMS1117-3.3与TLV70233分别生成3.3V数字逻辑与3.3V模拟传感电源域二者通过磁珠隔离避免数字噪声耦合至IMU、ADC等敏感模拟电路。2.2.2 核心处理器与存储扩展STM32H743XIH6作为Cortex-M7内核MCU主频480MHz具备双精度浮点单元FPU与DSP指令集满足EKF状态估计算法的实时运算需求。其外部存储接口配置如下SDRAMW9825G6KH-6I32MB工作于133MHz/CL2模式用于存放实时采集的IMU原始数据缓冲区、电机位置/速度历史序列、以及FreeRTOS任务堆栈。SDRAM控制器FMC配置需严格匹配器件时序参数特别是tRCDRAS to CAS Delay、tRPRow Precharge Time等关键参数。NOR FLASHW25Q256JVEIQ32MB用于存储Bootloader、固件镜像、校准参数表及日志文件。其SPI Quad I/O模式提供高达40MB/s读取带宽支持XIPeXecute In Place直接执行代码缩短启动时间。NAND FLASHMT29F4G08ABADAWP512MB作为大容量数据记录介质用于长时间运行时的传感器数据录波如IMU 1kHz采样持续1小时。其坏块管理与ECC校验由HAL库底层驱动实现。系统时钟采用8MHz有源晶振经H7内部PLL倍频至480MHz复位电路采用HX811TDB4RG专用复位IC提供精确的240ms复位脉冲宽度与±1.5%电压检测精度确保系统冷启动可靠性。2.2.3 传感器与通信接口IMU子系统BMI088惯性测量单元集成16-bit陀螺仪2000dps量程与16-bit加速度计24g量程。为抑制机械振动引入的高频噪声PCB布局时将BMI088置于独立刚性子板并采用硅胶垫片进行物理隔振同时在电源引脚增加π型LC滤波网络10μH电感 10μF陶瓷电容降低电源噪声对MEMS传感器的影响。I2C接口上拉电阻选用2.2kΩ匹配400kHz标准速率。GNSS定位中科微ATGM336H-5N31模块支持GPS/BD双模定位。RF前端严格遵循50Ω阻抗匹配设计天线馈点处放置0402尺寸的匹配电容/电感PCB顶层铺地完整避免射频信号辐射干扰其他模拟电路。模块通过UART与MCU通信波特率设为115200bps。视觉接口OV5640图像传感器通过DVP并行接口接入使用0.5mm间距26Pin FPC连接器。其供电由VRD1528低压差稳压器1.2V1.5A单独提供该LDO具备超低噪声15μVrms与高PSRR特性保障图像传感器模拟供电纯净度。工业总线CAN总线TJA1051TK/3/1J收发器支持ISO 11898-2标准速率可达1Mbps。总线端接采用120Ω贴片电阻靠近收发器CANH/CANL引脚放置。RS485SIT3088ETK半双工收发器驱动能力达±60V共模电压适用于长距离抗干扰通信。DE/RE使能信号由MCU GPIO控制确保总线空闲时自动进入接收态。USB与调试USB2.0 Device接口采用Micro-B连接器D/D-线串联22Ω端接电阻并靠近MCU放置VBUS引脚经MT9700单向电源开关芯片接入防止上位机USB口被反向灌电。调试接口为SWD标准兼容ST-Link V2/V3。其余外设LCD、蜂鸣器、LED、风扇、DBUS、蓝牙模块均通过GPIO或USART扩展具体电气特性与驱动方式详见原理图。2.3 电源分配板设计电源板作为系统的能源中枢承担着高压输入分配、大电流供给与热管理三大职能。其设计核心是解决“8A持续电流下的低损耗与高散热”问题。电机供电通路采用XT3022大电流端子支持单路40A峰值电流。PCB上对应走线宽度达8mm2oz铜厚并贯穿整板开窗裸露铜皮表面镀锡增强导电性与焊接强度。该通路直接为腿部伺服电机供电不经过任何DC-DC转换最大限度降低能量损耗与电压跌落。主控供电通路SY8368QNC输出的5V/8A电源通过FPC排线0.5mm间距12Pin接入主控板。FPC选型要求载流能力≥10A/线且具备良好弯折寿命。接口处设置TVS阵列如SP1003防护ESD冲击。扩展供电接口配备两路Type-C母座输出5V/3A为未来可能增加的外设如激光雷达、额外传感器提供标准化供电接口。Type-C CC引脚经10kΩ电阻下拉强制设备端识别为UFPUpstream Facing Port。热设计SY8368QNC散热焊盘通过8×8个0.3mm过孔连接至PCB背面整块覆铜层该覆铜层面积≥10cm²并预留M2.5螺丝孔位用于安装铝制散热片。实测满载工况下芯片结温稳定在75°C以内满足工业级应用要求。3. 软件系统设计3.1 实时操作系统与任务划分下位机软件基于FreeRTOS v10.4.6构建充分利用STM32H743的双核特性CM7主核运行控制任务CM4协核可选作数据预处理。系统创建5个优先级递增的任务任务名称优先级周期主要职责default_task1启动后常驻系统初始化、看门狗喂狗、低功耗管理imu_task31msBMI088数据采集、温度补偿、原始数据打包parse_task42ms解析上位机USB指令ASCII协议、生成控制参数left_leg_task52ms左腿运动学逆解、关节PID闭环、CAN报文发送right_leg_task52ms右腿运动学逆解、关节PID闭环、CAN报文发送所有任务间通过消息队列xQueueCreate与信号量xSemaphoreCreateBinary进行同步避免全局变量竞争。例如imu_task将最新IMU数据包含时间戳、加速度、角速度写入imu_queueparse_task与leg_task从中读取确保数据新鲜度与时序一致性。3.2 姿态感知与状态估计姿态解算采用扩展卡尔曼滤波EKF融合BMI088的六轴IMU数据与ATGM336H的GNSS位置/速度信息。EKF状态向量定义为$$ \mathbf{x} [\phi\ \theta\ \psi\ \dot{\phi}\ \dot{\theta}\ \dot{\psi}\ \mathbf{p}_x\ \mathbf{p}_y\ \mathbf{p}_z\ \dot{\mathbf{p}}_x\ \dot{\mathbf{p}}_y\ \dot{\mathbf{p}}_z]^T $$其中$\phi,\theta,\psi$为滚转、俯仰、偏航角$\mathbf{p}$为三维位置。系统模型基于刚体运动学方程观测模型则分别建立IMU测量方程陀螺积分加速度计观测与GNSS观测方程伪距/多普勒。EKF实现参考达妙科技开源框架关键优化点包括陀螺零偏在线估计将陀螺零偏$\mathbf{b}_g$纳入状态向量通过观测残差驱动其收敛自适应噪声协方差根据IMU数据方差动态调整过程噪声$Q$提升滤波鲁棒性GNSS失效降级当GNSS信号丢失时自动切换至纯IMU导航模式仅保留姿态与角速度估计。3.3 平衡控制算法演进初期采用串级PID控制外环为倾角环目标俯仰角$\theta_{ref}$内环为角速度环目标角速度$\dot{\theta}_{ref}$。其结构简单但存在明显缺陷在不平整地面运行时轮组接触点高度突变导致IMU俯仰角测量值剧烈跳变引发控制器饱和震荡髋关节未固定导致腿部质心动态变化破坏了倒立摆模型的刚性假设使PID参数难以全域整定。因此项目转向LQR线性二次型调节器控制。在Simulink中建立简化动力学模型将双轮足系统抽象为“轮组躯干腿部”的三质量块模型输入为左右轮扭矩$\tau_L,\tau_R$输出为躯干俯仰角$\theta$与水平位移$x$状态权重矩阵$Q$侧重惩罚$\theta$与$\dot{\theta}$控制权重矩阵$R$侧重约束$\tau$幅值。LQR求解得到最优反馈增益矩阵$K$其离散化形式直接嵌入FreeRTOS任务中// LQR反馈控制律伪代码 float theta_err theta_ref - theta_est; float theta_dot_err theta_dot_ref - theta_dot_est; float u -K[0][0] * theta_err - K[0][1] * theta_dot_err; // u映射为左右轮PWM占空比当前瓶颈在于髋关节自由度引入的非线性后续计划在Mujoco中构建高保真模型通过ILQR迭代LQR生成时变增益矩阵实现非线性系统的局部最优控制。4. 结构设计与机械实现4.1 多连杆腿部构型单腿采用三自由度串联构型髋关节俯仰偏航→ 膝关节屈伸。此设计突破传统双轮平衡机器人仅靠轮组旋转维持平衡的局限赋予系统主动调整质心位置的能力。结构件全部采用Fusion360参数化建模关键特征包括连杆轻量化在应力集中区域如电机安装座、轴承孔周边保留足够壁厚≥3mm其余区域采用网格加强筋与减重孔设计单腿铝合金CNC件重量控制在850g以内关节密封性伺服电机输出轴与连杆连接处采用O型圈迷宫槽双重密封防止润滑油渗出及粉尘侵入轴承选型验证初始选用5×13×4mm法兰轴承在15kg静态负载下出现轻微蠕动。经ANSYS Static Structural仿真将轴承升级为8×16×5mm规格后最大接触应力从1.2GPa降至0.7GPa满足ISO 281疲劳寿命要求。4.2 装配工艺与公差控制CNC加工主体结构件采用7075-T6航空铝三轴加工中心完成关键配合面如轴承孔、电机法兰面表面粗糙度Ra≤0.8μm位置度公差±0.05mm3D打印部件外壳与装饰件使用PA12碳纤维增强尼龙HP Multi Jet Fusion工艺层厚80μm确保尺寸稳定性与抗冲击性装配基准以腿部基座平面为统一装配基准所有电机安装孔位通过一次装夹完成钻孔攻丝消除累积误差动态平衡校准整机组装后在静止状态下采集各关节编码器零点偏移并存入NOR FLASH运行时通过卡尔曼滤波实时补偿机械间隙引起的测量偏差。5. BOM关键器件选型依据器件类别型号选型依据替代建议主控MCUSTM32H743XIH6480MHz主频FPUSDRAM/NOR/NAND控制器满足EKF与多轴运动解算NXP i.MX RT1176需重构外设驱动IMUBMI08816-bit分辨率2000dps量程低噪声密度0.004dps/√Hz工业级温漂ICM-20602成本更低性能略逊GNSSATGM336H-5N31国产化替代方案BD/GPS双模-165dBm跟踪灵敏度u-blox NEO-M8N国际主流成本高30%电机驱动GO-M8010/GIM-6010/DMH6215支持CAN总线控制、内置绝对编码器、峰值扭矩25N·m满足腿部动态响应若预算受限可降级为ROBOTIS XM430-W350需重写CAN协议栈电源管理SY8368QNC8A输出能力内置MOS宽输入范围国产高性价比方案TPS546B24TI方案成本高2倍6. 开发工具链与调试方法IDE环境ClionCMake项目管理 STM32CubeIDE底层驱动生成与烧录通过OpenOCD实现SWD在线调试通信协议USB虚拟串口采用自定义ASCII协议帧格式为$CMD,PARAM1,PARAM2,...*CSCRLF校验和为逗号分隔字段ASCII码异或值数据可视化上位机PythonPyQtGraph实时绘制IMU三轴数据、关节角度曲线、轮组速度波形支持数据导出为CSV硬件调试使用Saleae Logic Pro 16逻辑分析仪捕获CAN总线报文ID:0x101为左腿位置指令0x102为右腿位置指令验证控制指令下发时序与周期稳定性。项目当前处于算法验证阶段所有硬件设计、PCB布局、结构件加工均已完成实物验证。下一步工作聚焦于Mujoco模型构建与强化学习训练目标是生成可迁移至真实机器人的步态策略最终形成从仿真到物理世界的闭环验证能力。
双轮足平衡机器人：嵌入式实时控制与LQR算法实践
发布时间：2026/6/11 18:27:14
1. 项目概述“简易双轮足平衡机器人”并非传统意义上的轮式移动平台或四足仿生机构而是一种融合双轮自平衡原理与多自由度腿部结构的混合构型实验平台。其核心设计意图明确指向控制算法研究与嵌入式系统工程实践——以物理载体为依托构建一个可扩展、可验证、可复现的实时控制试验床。项目采用经典的“大小脑”分层控制架构下位机承担高实时性任务姿态感知、底层电机闭环、运动学解算上位机RK3566泰山派负责复杂逻辑调度、路径规划、强化学习策略部署及人机交互。这种软硬件解耦的设计思路既规避了单一MCU资源瓶颈对高级算法的制约又保留了底层控制回路的确定性与时效性。项目定位清晰非竞赛导向如RoboMaster、RoboCon、非商用产品原型而是面向嵌入式控制工程师与算法研究者的教学-研究一体化平台。其价值不在于最终形态的完成度而在于各子系统间接口定义的规范性、硬件资源分配的合理性、以及控制链路中关键瓶颈的暴露与解决过程。正如项目说明中所强调“小车只是一个载体上面可以有很多技术实现”本项目将双轮平衡作为基础动态约束条件将六自由度腿部结构作为执行末端本质上是在构建一个具备强动态耦合特性的机电系统为后续LQR最优控制、EKF状态估计、乃至基于Mujoco的强化学习训练提供真实物理边界与数据接口。2. 系统架构与硬件设计2.1 整体架构分层系统采用三级物理层级堆叠设计底层执行层由两套三自由度腿部模块构成每条腿包含髋关节俯仰偏航、膝关节屈伸共三个主动自由度驱动单元为宇树GO-M8010、伺泰威GIM-6010、达妙DMH6215等不同厂商伺服电机。腿部通过法兰轴承初始选用5×13×4mm规格与连杆机构刚性连接整体结构借鉴本末科技“刑天”机器人的多连杆拓扑兼顾运动包络与结构刚度。中层控制层即下位机主控板基于STM32H743XIH6微控制器承担全部实时控制任务。该层通过CAN总线与各伺服电机通信通过SPI/I2C采集IMU与GNSS数据通过USB与上位机交换指令与状态信息。上层决策层RK3566泰山派开发板运行Linux系统作为算法容器与人机交互终端。其通过USB虚拟串口接收下位机上传的传感器原始数据流并下发高层运动指令如目标倾角、步态相位、关节轨迹点。三层之间通过明确定义的通信协议进行数据交换物理隔离确保了实时性与安全性的边界划分。2.2 下位机主控板设计主控板设计以“高带宽、低延迟、强扩展”为原则围绕STM32H743XIH6的全功能外设展开重点解决多源异步数据融合与高速电机闭环控制两大挑战。2.2.1 电源管理子系统电源输入采用XT60接口支持8V–27V宽压范围满足锂电池组3S–6S及适配器供电需求。输入级集成三重保护防反接采用P沟道MOSFET如SI2301构成无损反向阻断电路过压钳位TVS二极管如SMAJ24A配合瞬态抑制网络缓启动NTC热敏电阻串联限流配合MOSFET栅极RC延时电路抑制上电浪涌电流。主电源通路采用矽力杰SY8368QNC同步降压控制器输入8V–28V输出5V/8A。该芯片内置上下桥MOSFET支持强制PWM模式以保证全负载范围内恒定开关频率提升EMI可控性并集成过流、过温、欠压锁定保护。PCB布局时功率环路VIN→SW→GND→CIN采用最短路径、大铜厚走线并在背面预留散热片安装区域输出滤波电容固态电解陶瓷阵列紧邻芯片放置降低高频纹波。5V电源经两级LDO如AMS1117-3.3与TLV70233分别生成3.3V数字逻辑与3.3V模拟传感电源域二者通过磁珠隔离避免数字噪声耦合至IMU、ADC等敏感模拟电路。2.2.2 核心处理器与存储扩展STM32H743XIH6作为Cortex-M7内核MCU主频480MHz具备双精度浮点单元FPU与DSP指令集满足EKF状态估计算法的实时运算需求。其外部存储接口配置如下SDRAMW9825G6KH-6I32MB工作于133MHz/CL2模式用于存放实时采集的IMU原始数据缓冲区、电机位置/速度历史序列、以及FreeRTOS任务堆栈。SDRAM控制器FMC配置需严格匹配器件时序参数特别是tRCDRAS to CAS Delay、tRPRow Precharge Time等关键参数。NOR FLASHW25Q256JVEIQ32MB用于存储Bootloader、固件镜像、校准参数表及日志文件。其SPI Quad I/O模式提供高达40MB/s读取带宽支持XIPeXecute In Place直接执行代码缩短启动时间。NAND FLASHMT29F4G08ABADAWP512MB作为大容量数据记录介质用于长时间运行时的传感器数据录波如IMU 1kHz采样持续1小时。其坏块管理与ECC校验由HAL库底层驱动实现。系统时钟采用8MHz有源晶振经H7内部PLL倍频至480MHz复位电路采用HX811TDB4RG专用复位IC提供精确的240ms复位脉冲宽度与±1.5%电压检测精度确保系统冷启动可靠性。2.2.3 传感器与通信接口IMU子系统BMI088惯性测量单元集成16-bit陀螺仪2000dps量程与16-bit加速度计24g量程。为抑制机械振动引入的高频噪声PCB布局时将BMI088置于独立刚性子板并采用硅胶垫片进行物理隔振同时在电源引脚增加π型LC滤波网络10μH电感 10μF陶瓷电容降低电源噪声对MEMS传感器的影响。I2C接口上拉电阻选用2.2kΩ匹配400kHz标准速率。GNSS定位中科微ATGM336H-5N31模块支持GPS/BD双模定位。RF前端严格遵循50Ω阻抗匹配设计天线馈点处放置0402尺寸的匹配电容/电感PCB顶层铺地完整避免射频信号辐射干扰其他模拟电路。模块通过UART与MCU通信波特率设为115200bps。视觉接口OV5640图像传感器通过DVP并行接口接入使用0.5mm间距26Pin FPC连接器。其供电由VRD1528低压差稳压器1.2V1.5A单独提供该LDO具备超低噪声15μVrms与高PSRR特性保障图像传感器模拟供电纯净度。工业总线CAN总线TJA1051TK/3/1J收发器支持ISO 11898-2标准速率可达1Mbps。总线端接采用120Ω贴片电阻靠近收发器CANH/CANL引脚放置。RS485SIT3088ETK半双工收发器驱动能力达±60V共模电压适用于长距离抗干扰通信。DE/RE使能信号由MCU GPIO控制确保总线空闲时自动进入接收态。USB与调试USB2.0 Device接口采用Micro-B连接器D/D-线串联22Ω端接电阻并靠近MCU放置VBUS引脚经MT9700单向电源开关芯片接入防止上位机USB口被反向灌电。调试接口为SWD标准兼容ST-Link V2/V3。其余外设LCD、蜂鸣器、LED、风扇、DBUS、蓝牙模块均通过GPIO或USART扩展具体电气特性与驱动方式详见原理图。2.3 电源分配板设计电源板作为系统的能源中枢承担着高压输入分配、大电流供给与热管理三大职能。其设计核心是解决“8A持续电流下的低损耗与高散热”问题。电机供电通路采用XT3022大电流端子支持单路40A峰值电流。PCB上对应走线宽度达8mm2oz铜厚并贯穿整板开窗裸露铜皮表面镀锡增强导电性与焊接强度。该通路直接为腿部伺服电机供电不经过任何DC-DC转换最大限度降低能量损耗与电压跌落。主控供电通路SY8368QNC输出的5V/8A电源通过FPC排线0.5mm间距12Pin接入主控板。FPC选型要求载流能力≥10A/线且具备良好弯折寿命。接口处设置TVS阵列如SP1003防护ESD冲击。扩展供电接口配备两路Type-C母座输出5V/3A为未来可能增加的外设如激光雷达、额外传感器提供标准化供电接口。Type-C CC引脚经10kΩ电阻下拉强制设备端识别为UFPUpstream Facing Port。热设计SY8368QNC散热焊盘通过8×8个0.3mm过孔连接至PCB背面整块覆铜层该覆铜层面积≥10cm²并预留M2.5螺丝孔位用于安装铝制散热片。实测满载工况下芯片结温稳定在75°C以内满足工业级应用要求。3. 软件系统设计3.1 实时操作系统与任务划分下位机软件基于FreeRTOS v10.4.6构建充分利用STM32H743的双核特性CM7主核运行控制任务CM4协核可选作数据预处理。系统创建5个优先级递增的任务任务名称优先级周期主要职责default_task1启动后常驻系统初始化、看门狗喂狗、低功耗管理imu_task31msBMI088数据采集、温度补偿、原始数据打包parse_task42ms解析上位机USB指令ASCII协议、生成控制参数left_leg_task52ms左腿运动学逆解、关节PID闭环、CAN报文发送right_leg_task52ms右腿运动学逆解、关节PID闭环、CAN报文发送所有任务间通过消息队列xQueueCreate与信号量xSemaphoreCreateBinary进行同步避免全局变量竞争。例如imu_task将最新IMU数据包含时间戳、加速度、角速度写入imu_queueparse_task与leg_task从中读取确保数据新鲜度与时序一致性。3.2 姿态感知与状态估计姿态解算采用扩展卡尔曼滤波EKF融合BMI088的六轴IMU数据与ATGM336H的GNSS位置/速度信息。EKF状态向量定义为$$ \mathbf{x} [\phi\ \theta\ \psi\ \dot{\phi}\ \dot{\theta}\ \dot{\psi}\ \mathbf{p}_x\ \mathbf{p}_y\ \mathbf{p}_z\ \dot{\mathbf{p}}_x\ \dot{\mathbf{p}}_y\ \dot{\mathbf{p}}_z]^T $$其中$\phi,\theta,\psi$为滚转、俯仰、偏航角$\mathbf{p}$为三维位置。系统模型基于刚体运动学方程观测模型则分别建立IMU测量方程陀螺积分加速度计观测与GNSS观测方程伪距/多普勒。EKF实现参考达妙科技开源框架关键优化点包括陀螺零偏在线估计将陀螺零偏$\mathbf{b}_g$纳入状态向量通过观测残差驱动其收敛自适应噪声协方差根据IMU数据方差动态调整过程噪声$Q$提升滤波鲁棒性GNSS失效降级当GNSS信号丢失时自动切换至纯IMU导航模式仅保留姿态与角速度估计。3.3 平衡控制算法演进初期采用串级PID控制外环为倾角环目标俯仰角$\theta_{ref}$内环为角速度环目标角速度$\dot{\theta}_{ref}$。其结构简单但存在明显缺陷在不平整地面运行时轮组接触点高度突变导致IMU俯仰角测量值剧烈跳变引发控制器饱和震荡髋关节未固定导致腿部质心动态变化破坏了倒立摆模型的刚性假设使PID参数难以全域整定。因此项目转向LQR线性二次型调节器控制。在Simulink中建立简化动力学模型将双轮足系统抽象为“轮组躯干腿部”的三质量块模型输入为左右轮扭矩$\tau_L,\tau_R$输出为躯干俯仰角$\theta$与水平位移$x$状态权重矩阵$Q$侧重惩罚$\theta$与$\dot{\theta}$控制权重矩阵$R$侧重约束$\tau$幅值。LQR求解得到最优反馈增益矩阵$K$其离散化形式直接嵌入FreeRTOS任务中// LQR反馈控制律伪代码 float theta_err theta_ref - theta_est; float theta_dot_err theta_dot_ref - theta_dot_est; float u -K[0][0] * theta_err - K[0][1] * theta_dot_err; // u映射为左右轮PWM占空比当前瓶颈在于髋关节自由度引入的非线性后续计划在Mujoco中构建高保真模型通过ILQR迭代LQR生成时变增益矩阵实现非线性系统的局部最优控制。4. 结构设计与机械实现4.1 多连杆腿部构型单腿采用三自由度串联构型髋关节俯仰偏航→ 膝关节屈伸。此设计突破传统双轮平衡机器人仅靠轮组旋转维持平衡的局限赋予系统主动调整质心位置的能力。结构件全部采用Fusion360参数化建模关键特征包括连杆轻量化在应力集中区域如电机安装座、轴承孔周边保留足够壁厚≥3mm其余区域采用网格加强筋与减重孔设计单腿铝合金CNC件重量控制在850g以内关节密封性伺服电机输出轴与连杆连接处采用O型圈迷宫槽双重密封防止润滑油渗出及粉尘侵入轴承选型验证初始选用5×13×4mm法兰轴承在15kg静态负载下出现轻微蠕动。经ANSYS Static Structural仿真将轴承升级为8×16×5mm规格后最大接触应力从1.2GPa降至0.7GPa满足ISO 281疲劳寿命要求。4.2 装配工艺与公差控制CNC加工主体结构件采用7075-T6航空铝三轴加工中心完成关键配合面如轴承孔、电机法兰面表面粗糙度Ra≤0.8μm位置度公差±0.05mm3D打印部件外壳与装饰件使用PA12碳纤维增强尼龙HP Multi Jet Fusion工艺层厚80μm确保尺寸稳定性与抗冲击性装配基准以腿部基座平面为统一装配基准所有电机安装孔位通过一次装夹完成钻孔攻丝消除累积误差动态平衡校准整机组装后在静止状态下采集各关节编码器零点偏移并存入NOR FLASH运行时通过卡尔曼滤波实时补偿机械间隙引起的测量偏差。5. BOM关键器件选型依据器件类别型号选型依据替代建议主控MCUSTM32H743XIH6480MHz主频FPUSDRAM/NOR/NAND控制器满足EKF与多轴运动解算NXP i.MX RT1176需重构外设驱动IMUBMI08816-bit分辨率2000dps量程低噪声密度0.004dps/√Hz工业级温漂ICM-20602成本更低性能略逊GNSSATGM336H-5N31国产化替代方案BD/GPS双模-165dBm跟踪灵敏度u-blox NEO-M8N国际主流成本高30%电机驱动GO-M8010/GIM-6010/DMH6215支持CAN总线控制、内置绝对编码器、峰值扭矩25N·m满足腿部动态响应若预算受限可降级为ROBOTIS XM430-W350需重写CAN协议栈电源管理SY8368QNC8A输出能力内置MOS宽输入范围国产高性价比方案TPS546B24TI方案成本高2倍6. 开发工具链与调试方法IDE环境ClionCMake项目管理 STM32CubeIDE底层驱动生成与烧录通过OpenOCD实现SWD在线调试通信协议USB虚拟串口采用自定义ASCII协议帧格式为$CMD,PARAM1,PARAM2,...*CSCRLF校验和为逗号分隔字段ASCII码异或值数据可视化上位机PythonPyQtGraph实时绘制IMU三轴数据、关节角度曲线、轮组速度波形支持数据导出为CSV硬件调试使用Saleae Logic Pro 16逻辑分析仪捕获CAN总线报文ID:0x101为左腿位置指令0x102为右腿位置指令验证控制指令下发时序与周期稳定性。项目当前处于算法验证阶段所有硬件设计、PCB布局、结构件加工均已完成实物验证。下一步工作聚焦于Mujoco模型构建与强化学习训练目标是生成可迁移至真实机器人的步态策略最终形成从仿真到物理世界的闭环验证能力。
