1. 项目概述从理论到实践的平衡辅助革命在机器人辅助康复与老年助行领域如何让设备既足够“聪明”地提供支撑又足够“轻盈”地融入日常生活一直是个核心挑战。传统的助行器或智能轮椅虽然提供了支撑但往往牺牲了使用者的自主性与灵活性设备本身也较为笨重。我们团队历时数年最终将目光投向了最日常的助行工具——手杖并赋予它全新的“智慧”。今天要分享的就是我们基于倒立摆模型与李代数方法研发的一款能够主动维持使用者平衡的机器人手杖。这款手杖的核心目标非常明确它不仅仅是一个被动的支撑物更要成为一个主动的平衡伙伴。当使用者站立时它能像不倒翁一样自我稳定当使用者行走或身体发生倾斜时它能通过底部的全向轮迅速移动提供反向支撑力帮助使用者恢复重心从而有效实现防跌倒功能。这一切的背后是一套复杂的控制系统在实时运作我们以倒立摆来建模手杖与使用者的耦合动力学用李代数方法对这个强非线性系统进行精确的线性化再通过非线性扰动观测器来实时“感知”并估计使用者施加的意图和力。最终这套系统被集成在一个重量约7公斤、内置高性能嵌入式控制器的手杖硬件中。这篇文章我将抛开复杂的数学公式外壳重点拆解我们是如何将这套理论落地为一个可靠产品的。我会从系统设计的整体思路讲起深入硬件选型与集成的细节剖析控制算法的实现要点并分享在调试与实验中踩过的那些“坑”以及对应的解决方案。无论你是对机器人控制感兴趣的工程师还是关注康复机器人产品化的同行希望这篇超过五千字的实录能给你带来一些切实的参考。2. 核心思路与方案选型为什么是“倒立摆全向轮李代数”在项目启动之初我们面临几个关键决策用什么模型来描述手杖与人的交互用什么执行机构来实现快速、灵活的支撑用什么控制方法来处理系统的强非线性经过多轮论证与仿真我们确定了“倒立摆模型 全向轮 李代数线性化”这一技术路线。下面我详细解释一下每个选择背后的考量。2.1 倒立摆模型将复杂人机系统简化为可控对象选择倒立摆作为核心模型是基于其物理直观性与控制理论的成熟度。当使用者握持手杖站立时手杖-人体系统可以抽象为一个在垂直面内矢状面和侧面内冠状面的两个倒立摆。手杖的杆身相当于摆杆底部轮子的移动则相当于控制倒立摆小车在平面上的运动。这个模型的优势在于抓住了主要矛盾它忽略了人体关节运动的细节但精准刻画了整体重心与支撑点之间的动态关系这正是平衡控制的核心。模型统一无论是手杖自身的自平衡还是辅助使用者站立、行走都可以用同一套动力学方程来描述只是“扰动源”不同前者是内部误差后者是使用者施加的力。理论支撑丰富倒立摆的控制研究汗牛充栋从经典的PID、LQR到各种非线性控制方法为我们提供了坚实的理论起点。然而经典倒立摆控制如LQR通常只在平衡点竖直状态附近的小角度范围内有效。对于助行应用使用者的推、拉动作会导致手杖产生较大的倾角这就要求我们的控制器必须在大范围偏离平衡点时依然稳定。这正是我们引入李代数方法的主要原因。2.2 全向轮实现平面内任意方向的快速响应执行机构的选择直接决定了系统的响应能力和灵活性。我们放弃了传统的两轮差速或四轮结构选择了单全向轮方案。这个“全向轮”并非普通的麦克纳姆轮而是一个我们特别设计的、集成在齿轮箱内的智能轮组。它由一个主轮负责前后俯仰运动和两个侧向小轮负责左右横滚运动通过一套锥齿轮和谐波齿轮组合而成由两个无刷电机驱动。为什么是这种设计全向移动能力通过控制两个电机的转速和转向组合可以实现手杖在平面内任意方向的瞬时移动没有转向半径的限制。这对于快速补偿使用者突然的失衡至关重要。结构紧凑相比早期研究中使用三个全向轮的方案单轮设计极大地缩小了底盘尺寸和整体重量使手杖更接近传统手杖的形态提高了使用接受度。高扭矩输出谐波减速器提供了高减速比和紧凑体积内的巨大输出扭矩确保手杖即使在承受较大侧向力时也能迅速启动和制动。注意齿轮箱的设计是硬件成败的关键。我们最初版本使用了普通的行星齿轮箱但在频繁正反转和冲击负载下出现了回差大、噪音高的问题。后来切换到谐波齿轮型号CSD-20-50-2A-GR-SP其零背隙特性显著提升了控制精度和响应速度虽然成本上升但可靠性是医疗辅助设备的第一要务。2.3 李代数方法征服非线性拓宽稳定域如前所述LQR等基于局部线性化的方法在倾角较大时会失效。我们需要一种能在整个工作空间倾角约±0.5弧度内对系统进行精确线性化的方法。李代数方法Lie Algebra Method, LAM正是解决这一问题的利器。简单来说LAM的核心思想是通过状态变量的非线性变换将一个非线性系统在几何上“拉直”成一个线性系统。在我们的案例中我们构造了一个新的输出变量y它是杆身角度φ和轮子角度θ的复杂函数具体见论文公式(38)。通过对这个新变量y求导直到显式地出现控制输入u我们就得到了一个精确的输入-输出线性化关系。这么做的巨大好处是全局线性化只要变换可逆线性化关系在整个定义域内都成立不受“小角度假设”的限制。解耦控制将复杂的多变量非线性控制问题转化为对线性化后系统的标准控制问题如极点配置设计变得直观。与扰动观测器天然结合线性化后的系统模型更干净便于我们设计非线性扰动观测器来估计使用者施加的力d并将此估计值作为前馈补偿或参考轨迹生成的依据。方案选型对比表考虑维度可选方案A (差速两轮)可选方案B (三全向轮)我们选择的方案 (智能单全向轮)移动灵活性好但转向需时间极好全向移动极好全向移动结构复杂度低高三个电机与轮子中单轮组但齿轮箱复杂尺寸与重量较小较大紧凑最接近传统手杖控制复杂度低高需协调三个轮子中两个电机解耦控制抗扰动能力一般侧向支撑弱强强通过齿轮箱分配扭矩成本低高中高主要因谐波齿轮通过这张表可以清晰看出我们的方案在灵活性、紧凑性和性能之间取得了最佳平衡。当然其代价是较高的机械设计精度要求和控制算法复杂度。3. 硬件系统深度解析从图纸到实物的工程实现理论再优美最终都要落在实实在在的硬件上。我们的机器人手杖硬件系统是一个典型的机电一体化产品涉及机械结、传感器、执行器、控制器和电源管理。下面我分模块拆解其中的设计要点和实操经验。3.1 机械结构轻量化、高强度与功能集成手杖的机械结构主体分为三部分上部的握柄与杆身、中部的控制器舱体、下部的全向轮驱动总成。杆身与握柄采用碳纤维复合材料管在保证足够刚度的前提下最大限度减轻重量。握柄内部集成了触摸传感器和急停开关。这个触摸传感器至关重要它不仅是模式切换开关握住即进入辅助模式其信号还用于抑制手杖在自平衡模式下的微小高频振动后文会详述。控制器舱体3D打印的尼龙壳体内部安装了所有核心电子设备。布局上遵循“重在下、轻在上”的原则将电池等重物尽量放置在靠近轮轴的位置以降低整体重心有利于自平衡。全向轮驱动总成这是机械设计的核心。如图2所示它包含一个谐波减速器连接电机与主轴和一套锥齿轮组。两个无刷电机HK-4525-520KV的输出经过这套齿轮箱被巧妙地分配和合成两电机同向旋转驱动主轮俯仰方向前进或后退。两电机反向旋转驱动两侧小轮横滚方向左移或右移。电机差速旋转实现俯仰和横滚的复合运动。实操心得齿轮箱的装配与调试齿轮箱的装配精度直接决定了传动效率、噪音和寿命。我们使用了高精度的角接触轴承来支撑各轴并在装配后进行了严格的“跑合”测试——在空载和负载下以不同速度正反转运行数小时。同时在齿轮啮合处涂抹特定的长效润滑脂既能减少磨损也能抑制高频噪音这对提升用户体验很重要。3.2 传感与执行单元系统的“神经”与“肌肉”状态感知神经惯性测量单元我们选用了一款高性能的X-IMU模块它集成了三轴陀螺仪、三轴加速度计。它通过USB接口与主控制器通信以500Hz的频率提供杆身的姿态角俯仰角φ和横滚角、角速度。这是控制器的核心反馈信号。电机编码器两个无刷电机均配备了高精度磁性编码器用于测量电机转速和计算轮子实际位置构成速度闭环。力传感器用于验证在研发阶段我们在杆身特定位置加装了CFS080CS102A轴扭矩传感器用于直接测量使用者施加的力/力矩以此作为“金标准”来验证我们非线性扰动观测器的估计精度见图8对比结果。在产品化版本中此传感器可被移除以降低成本因为观测器已能可靠估计出力。动作执行肌肉无刷电机与驱动器电机选用的是HK-4525-520KV其高扭矩密度适合本应用。驱动器选用的是Elmo的G-DCWHI20/100EE数字伺服驱动器。选择它的原因不仅是性能更是其丰富的控制模式位置、速度、转矩模式和高速通信接口CANopen方便我们实现复杂的控制律。我们将驱动器设置为“转矩模式”由主控制器直接发送转矩指令响应更快。电池系统内置电池组7.0kg总重包含它可支持约30分钟高强度运行。为延长使用时间我们设计了一个外挂电池盒2.3kg25.2V, 13.8Ah可提供超过2小时的续航。电池管理单元负责监控电压、电流和温度并通过LED显示确保安全。3.3 控制器硬件高速计算与实时性保障主控制器是树莓派 3 Model B。很多人可能会疑问树莓派作为Linux系统实时性是否足够答案是经过精心优化完全可以。实时性改造我们使用了PREEMPT_RT补丁的内核将Linux内核转换为硬实时系统。同时将控制线程的优先级设为最高并绑定到独立的CPU核心上确保控制循环的周期抖动在微秒级。外设接口SPI连接高速DAC用于向两个电机驱动器发送模拟转矩指令-10V~10V。I2C连接ADC读取一些模拟量状态如备用电池电压。USB连接X-IMU读取姿态数据。GPIO读取编码器信号通过FPGA进行四倍频和解码减轻CPU负担、触摸传感器和急停开关状态。软件架构控制程序用C编写主要分为三个实时线程高速控制线程1kHz执行核心控制算法LAM扰动观测器计算并输出转矩指令。中速状态估计线程500Hz处理IMU数据进行传感器融合互补滤波得到更平滑的姿态角。低速管理线程100Hz处理用户交互模式切换、电池管理、数据记录和网络通信用于调试。这种架构确保了在最坏情况下控制算法的执行周期也能稳定在1ms满足了系统动态响应的要求。4. 控制算法实现与参数整定让理论“跑”起来有了硬件平台接下来就是让算法在上面运行起来。这一部分我将避开繁复的公式推导重点讲工程实现中的关键步骤、参数整定的经验以及如何将李代数方法LAM和非线性扰动观测器NDOB落地。4.1 系统动力学模型的离散化与编程论文中的连续时间方程(15)-(21)需要在数字控制器中离散化。我们采用零阶保持器方法控制周期为T 1ms。关键步骤状态定义我们定义状态向量x [φ, φ_dot, θ, θ_dot]^T即杆身角度、角速度、轮子角度、轮子角速度。计算惯性矩阵与力向量在每个控制周期根据当前测量的φ和φ_dot实时计算矩阵H11, H12, H22和向量b1, b2。这里需要注意cosφ和sinφ的计算比较耗时我们采用了查表法结合线性插值来加速。李代数变换的实现这是最核心的部分。我们需要实时计算输出y及其各阶导数直到三阶。公式(39)-(42)涉及积分和复杂函数的偏导数。积分处理公式(39)中的积分∫(H11/H12)dφ无法解析求出。我们的做法是在系统初始化时在φ的工作范围-0.5rad 到 0.5rad内预先计算一个密集的查找表。在线运行时根据当前φ值进行查表和线性插值速度极快。偏导数计算公式(41)(42)中的∂/∂φ (H11/H12)和∂/∂φ (b1/H12)同样通过预先计算偏导数的查找表来实现。计算控制律根据公式(43)和(46)计算线性化后的虚拟控制量v然后反解出实际电机转矩u。公式(46)中的λ_i是控制器增益我们通过极点配置法将其配置在s -20 ± 15i, -25, -30左右以保证足够的稳定裕度和响应速度。4.2 非线性扰动观测器的工程化扰动观测器用于实时估计使用者施加的力d1影响杆身和d2影响轮子。其离散化实现如下// 伪代码示例 // 当前周期 L computeLagrangian(phi, phi_dot, theta, theta_dot); // 计算拉格朗日量 tau_all computeTotalTorque(u); // 计算总控制转矩 // 观测器状态更新 (离散化公式(48)) xi_dot -K * xi K*K * partial_L_partial_qdot K * (partial_L_partial_q tau_all); xi xi xi_dot * T; // 欧拉积分 // 扰动估计 (公式(49)) d_hat xi - K * partial_L_partial_qdot; // 估计使用者施加的力 (公式(52)(53)) fx_hat d_hat_2 / r; if (abs(sin(phi)) 0.01) { // 避免除零 fz_hat - ( (lc*cos(phi)r)*d_hat_2 r*d_hat_1 ) / (lc * r * sin(phi)); } else { fz_hat 0; // 接近竖直时垂直力估计不可靠置零 }观测器增益K的整定K决定了观测器的带宽和噪声抑制能力。K越大估计响应越快但对传感器噪声越敏感。我们通过实验整定先给手杖施加一个已知的阶跃力用砝码和滑轮然后调整K使估计值d_hat能快速跟踪真实力同时稳态波动较小。最终我们选取的K对应观测器带宽约为10Hz在响应速度和抗噪性之间取得了平衡。4.3 参考轨迹生成从“力”到“动作”的翻译扰动观测器估计出使用者施加的力(fx_hat, fz_hat)后控制器需要将其翻译成手杖应该执行的动作。这是人机交互意图理解的关键。站立辅助当使用者在静止状态下施加一个水平力fx_hat时我们并不希望手杖无限地加速移动而是希望它移动到一个新的平衡位置来提供支撑。我们根据公式(54)(55)将估计的力矩d1_hat转换为一个期望的杆身角度φ_ref再进一步转换为期望的轮子位置θ_ref。这样使用者在静止状态下轻推手杖手杖会平滑地移动一小段距离后停住提供稳固的支撑。行走辅助当检测到使用者意图行走表现为持续的水平力fx_hat时我们切换到速度辅助模式。根据公式(56)将d2_hat积分得到期望的轮子速度θ_dot_ref。积分增益KI需要仔细调节太大则手杖“过于主动”容易让使用者有被拉扯感太小则辅助力度不足。我们通过让不同体重的测试者反复行走来调节找到一个普适性较好的值。防跌倒触发这是安全功能的核心。我们持续监控杆身角度φ和角速度φ_dot。当φ超过一个较大的阈值如0.4 rad且φ_dot也较大时系统判断使用者正在快速失衡。此时控制器会暂时覆盖其他参考指令生成一个使手杖快速向失衡方向移动的强控制信号同时触发棘轮框架支撑机构图24锁定形成一个稳固的三角支撑防止使用者完全摔倒。4.4 控制器参数整定实战经验参数整定是让系统从“能跑”到“跑得好”的关键。我们遵循“先内环后外环”、“先仿真后实机”、“先自平衡后交互”的顺序。整定流程自平衡模式整定无人持握第一步稳定轮子。先只控制轮子位置让手杖在平面上保持不动。调节轮子位置环的PID参数使其能抵抗轻微的推力。第二步启用LAM平衡。加入LAM平衡控制器但先将扰动观测器输出置零。调节LAM的极点配置参数λ_i目标是让杆身能从一定倾角如0.2 rad快速、无超调地回到竖直位置。这里有个坑仿真中表现良好的参数在实机上可能因为电机摩擦、齿轮间隙而振荡。需要适当降低增益增加微分项以抑制振荡。第三步整定扰动观测器。在平衡状态下用手施加一个已知的、缓慢变化的力观察d_hat的跟踪情况。调节观测器增益K直到估计力能平滑、准确地跟随真实力。辅助模式整定有人持握第一步站立辅助。让测试者轻轻推/拉手杖。调节从d1_hat到φ_ref的映射关系公式(54)中的等效杠杆长度lc使手杖的移动距离符合直觉——轻推移动小段重推移动大段但要有最大距离限制我们设为0.1m。第二步行走辅助。调节速度环的积分增益KI。目标是让使用者在平地上行走时感觉手杖提供了一个适中的“牵引力”既省力又不觉得被拖拽。这个参数与使用者体重和步态有关我们最终取了一个折中值并允许通过手机APP进行小幅度的个性化调节。重要提示所有参数的调节都必须在安全测试环境下进行测试者需佩戴护具且手杖应急开关必须处于可立即触发的位置。尤其是防跌倒功能的触发阈值需要在物理治疗师的指导下通过模拟各种失衡场景来谨慎设定。5. 系统测试、问题排查与性能优化实录一套系统从实验室走向稳定可靠离不开反复的测试和问题排查。这部分分享我们在仿真和实物测试中遇到的主要问题、分析思路和最终的解决方案。5.1 仿真与实机差异从理想模型到物理现实我们在MATLAB/Simulink和C三维动力学环境中都建立了高保真仿真模型。仿真结果如图10,11所示非常完美LAM控制器能在0.3秒内将杆身从0.17 rad稳定回零且超调极小。然而第一个实机原型的表现却令人失望杆身会在平衡点附近持续低频振荡约0.5Hz并且伴有高频噪音。问题排查检查传感器IMU数据是否噪声过大我们记录了原始数据并做频谱分析发现噪声在可接受范围。对陀螺仪和加速度计数据进行互补滤波后角度信号很平滑。检查执行器电机响应是否有延迟我们用示波器对比了控制器输出的转矩指令和驱动器实际输出的电流发现指令跟踪良好延迟小于0.5ms。检查机械结构最终发现问题根源——齿轮间隙和结构柔性。虽然谐波齿轮无背隙但电机轴到谐波发生器、以及锥齿轮啮合处存在微小的弹性变形和间隙。这在仿真模型中被忽略了。解决方案在控制器中引入状态观测器我们设计了一个龙伯格观测器除了估计角度和速度还额外估计一个“等效干扰”状态这个状态实际上包含了摩擦、间隙等未建模动态。然后将这个估计的干扰前馈补偿掉。增加微分项和低通滤波在LAM控制器的输出后增加了一个针对角度误差的微分项并经过一个低通滤波器。这有效抑制了由间隙引起的极限环振荡。机械改进在第二版硬件中我们提高了所有轴承座的配合精度并在锥齿轮啮合处增加了预紧弹簧进一步减小了回差。5.2 高频振动与触摸传感器抑振解决了低频振荡后在自平衡模式下手杖杆身出现了一种细微但令人烦躁的高频振动约50-100Hz肉眼可见手握时能感觉到“嗡嗡”声。分析这种高频振动并非来自控制不稳定而是源于高增益控制器与机械谐振频率的耦合。手杖的杆身并非绝对刚体有其一阶弯曲固有频率。我们的控制器带宽很高无意中激励了这个模态。解决方案——触摸传感器抑振 这是一个非常巧妙且低成本的解决方案。我们在握柄内部集成了电容式触摸传感器。逻辑当传感器检测到无人握持自平衡模式时在控制器的输出通道上额外加入一个陷波滤波器其中心频率就设置在机械谐振频率处。这个滤波器能大幅衰减对该频率信号的增益从而抑制振动。效果一旦使用者握住手柄电容值变化系统立即切换为“辅助模式”并** bypass **这个陷波滤波器。因为当人手握持时手-杖系统的阻尼大大增加谐振峰自然消失不再需要滤波器。同时人的手对高频振动也不敏感。 这个设计使得手杖在待机时安静稳定在使用时控制响应又不受影响。图21清晰地展示了触摸传感器触发前后杆身角度高频噪声的显著降低。5.3 不同地面与负载的适应性测试实验室光滑地板上表现良好不代表在实际环境中可靠。我们进行了多场景测试粗糙地面/短毛地毯轮子与面的摩擦系数变化导致有时会出现打滑。打滑时编码器反馈的轮子位置/速度信息将严重失真。对策我们引入了基于IMU的轮子速度估计作为冗余。通过IMU测量的杆身角度动态和动力学模型可以反推出一个轮子速度的估计值。当编码器速度与IMU估计速度差异超过阈值时系统会短暂降低控制增益并更多地依赖状态观测器进行预测直到速度信息重新收敛。斜坡测试在斜坡上重力分量会成为一个持续的“扰动”。我们的非线性扰动观测器成功地将这个恒定分量估计了出来并进行了补偿因此手杖在坡度小于5°的斜坡上依然能提供有效的平衡辅助。不同使用者负载使用者的体重和握持高度会影响系统的惯量和重心。对策系统参数如质量m、转动惯量Jφ是在标准负载下标定的。对于差异较大的使用者我们提供了一个简单的“校准模式”让使用者自然站立握持手杖系统会持续测量几秒钟内杆身的平均倾角和所需的平衡转矩并据此微调模型中的m和lc参数实现一定程度的自适应。5.4 安全机制与故障处理对于助行设备安全必须放在首位。我们设计了多层安全机制软件看门狗主控制循环如果1ms内未能完成看门狗会触发硬件复位。硬件急停开关握柄上的大按钮一旦按下直接切断电机驱动器的使能信号。状态监控与降级控制器持续监控IMU数据是否超限、电机温度、电流是否过载。任何一项异常系统会进入“安全缓行模式”控制输出被限制在很低的值并尝试缓慢移动手杖至完全竖直并锁死轮子。棘轮框架机械锁图24, 25这是最后的物理安全保障。当检测到严重跌倒风险或急停触发时一个电磁阀会释放刹车叉棘轮机构在螺旋弹簧的作用下迅速展开形成一个稳固的三脚架将手杖牢牢固定在地面上防止其随使用者一起倾倒。5.5 性能对比LAM vs. LQR我们与传统的LQR方法进行了对比实验图12, 13结果充分证明了LAM的优势LQR在平衡点附近±0.05 rad表现尚可但一旦初始角度稍大或受到持续扰动就会产生明显的稳态误差或低频振荡图12顶部25秒后失稳。其位置也会持续漂移图13顶部。LAM在整个测试范围内±0.3 rad都能将杆身角度稳定在零点附近波动±0.005 rad位置也基本保持不动图12,13底部。这验证了LAM全局线性化的有效性。5.6 用户体验与生物力学验证最终我们邀请了7位不同年龄的受试者进行测试图22并使用高精度陀螺仪传感器佩戴在他们腰部测量行走时的身体振动表VIII图23。结果令人鼓舞与无辅助行走相比使用机器人手杖使身体侧向振动降低了约3.12倍。与传统手杖相比传统手杖在离地瞬间无法提供支撑振动降低了约2.48倍。所有受试者主观反馈都表示使用机器人手杖行走更省力、更平稳尤其是在转身和上下坡时信心更足。这从生物力学角度证实了我们的机器人手杖在平衡辅助和步行辅助方面的有效性。它不仅仅是一个“不会倒的手杖”更是一个能主动适应使用者意图、提供动态支撑的智能伙伴。回顾整个项目从基于倒立摆和李代数的控制理论出发到全向轮机械设计、嵌入式系统实现再到最后的实验验证与问题打磨每一步都充满了挑战。最深的体会是做机器人产品尤其是与人紧密交互的辅助设备理论上的优美必须让位于实际中的可靠与安全。那个用于抑制高频振动的小小触摸传感器其价值不亚于复杂的LAM算法那套棘轮安全机构其重要性超过了所有控制循环。
