基于Arduino与BLDC电机的四足机器人步态协调控制实现

很多朋友一看到四足机器人下意识就觉得这是搞深度学习、搞SLAM、跑各种视觉算法的大工程。我自己最开始也是这个想法直到用一套Arduino加BLDC电机方案把四足步态协调控制算法从数学公式变成能走、能转弯、能抗干扰的实际机器人之后才意识到这个题目真正的难点根本不在传感器数量上而在于你怎么把什么时候抬哪条腿、抬多高、往前迈多少这件听上去很简单的事变成电机驱动器能精确执行的高频指令流。这个开发过程中我用了一套基于Arduino的无刷电机四足方案包含磁编码器、BLDC驱动器、自研的步态规划与腿部逆运动学模块。整个项目跑下来最有价值的收获不是机器人走得有多稳而是我终于把步态协调这四个字拆成了可以逐层实现、可以单独调试、可以反复优化的清晰模块。这篇文章就围绕这套方案完整记录我在选型、算法、编码、调试过程中踩过和填平的坑希望能给同样想用非舵机方案做四足的朋友一条可以直接上手的路线。1. 为什么四足机器人的腿绕来绕去最后还是选了BLDC1.1 舵机方案的够用和不够用市面上大量入门级四足机器人用的都是舵机原因很简单便宜、自带位置反馈、直接给脉宽就能转角度一个几百块的舵机加一块舵机驱动板就能让四条腿动起来。如果是做静态展示、慢速行走、外形demo舵机方案完全够用甚至比无刷方案省心得多。但一旦你想让机器人走得更快一点、步态频率更高一点、腿部动作更利落一点舵机的短板就会非常明显。首先是响应速度。绝大多数舵机内部就是直流电机加电位器加减速齿轮位置闭环的带宽通常只有十几赫兹。这意味着你给它一个快速变化的关节角度指令它跟不上表现出来就是腿部动作粘滞、抬腿轨迹明显失真。其次是齿轮箱。舵机为了放大扭矩几乎都带塑料或金属减速齿轮齿轮间隙在正反转切换时会造成往复空程步态里最容易出现的关节来回摆动工况恰恰会无限放大这个空程。再其次就是堵转发热四足落地冲击时关节经常处于近似堵转的状态舵机在这种状态下持续工作很容易过热损害寿命。我最初用某款大扭矩舵机做过一版腿部原型通电空载摆动没问题一旦把机器人放到地上真正迈步舵机升温速度非常快几分钟后扭矩肉眼可见地下降。这让我意识到如果想认真研究步态协调控制算法本身执行器的动态响应才是决定算法能不能兑现的基础。1.2 BLDC加磁编码器才是真正意义上的关节BLDC电机解决的问题恰恰是舵机的短板。无刷电机本身转动惯量低、扭矩密度高、响应速度快配合磁场定向控制FOC之后电流环响应可以做到几百赫兹甚至上千赫兹位置环做到几百赫兹也远比舵机从容。更重要的是BLDC没有减速齿轮箱你完全可以做成直驱结构从电机输出轴直接连接腿部连杆彻底消灭齿轮间隙。但换成BLDC并不是插上电就能用它带来一整套新的工程问题电机本身没有位置感知能力必须外接磁编码器电机是三相驱动的需要专门的驱动器更关键的是无刷电机默认只是输出扭矩你需要自己在外面搭位置闭环才能把它变成关节。这套复杂度换来的是完全不同的控制性能你可以精确控制每个关节的扭矩、速度、位置可以做阻抗控制可以承受落地冲击可以高频响应步态切换。我在这个项目里采用的是常见开源无刷FOC方案也就是SimpleFOC风格的架构。磁编码器选用14位分辨率的型号SPI接口输出绝对角度驱动器负责电流环和速度环主控通过串口或者PWM接口下发位置目标。主控里面跑位置环实际上就是用一个PID控制器把期望关节角换算成期望速度再交给驱动器里面的速度环去执行。整个链路层次清晰哪一环出了问题都很容易定位。提示如果你的BLDC项目和Arduino平台配合建议优先选择带电流环、速度环、位置环三层结构的驱动器方案。想从零开始自己写FOC也不是不行但步态协调控制算法的调试工作量本身已经很大执行器层面越稳定算法层排错越容易。1.3 电机参数和减速比的估算思路很多人在选BLDC电机时会纠结KV值、额定扭矩这些参数。我的建议是不要按电机手册的标称值来选而要先算你的腿到底需要多大的扭矩和速度。估算过程其实不复杂。先估一条腿的重量分布比如大腿加小腿加电机自身总共200克腿长大约15厘米期望的关节最大角速度是每秒300度左右。那么峰值角加速度如果按2倍角速度变化来估大约就是10.5弧度每秒方。把质量分布近似成一个集中在腿长中点的质点转动惯量大约是质量乘以长度平方的三分之一大致就是0.0015千克平方米。按这个数字算扭矩峰值峰值扭矩等于转动惯量乘角加速度加上重力项估算出来在零点二牛米到零点四牛米之间留出三倍以上的裕量目标峰值扭矩就落在1牛米附近。有了扭矩目标再决定直驱还是减速方案。直驱的好处是零间隙、可反驱、力控制方便但对电机本身的扭矩密度要求很高通常需要大直径的盘式电机。如果选择带减速箱谐波减速箱或者行星减速箱都可以减速比5比1到10比1是比较常见的区间这样电机侧扭矩要求可以降低很多但会引入少量间隙和额外的摩擦力。考虑到Arduino平台的算力有限也考虑到成本我最终给每条腿选了中小尺寸高KV的无刷电机加5比1行星减速箱的组合每条腿的关节峰值输出扭矩大约在1.5牛米以上同时保留了较好的反向驱动能力落地冲击不会被硬生生顶回来。2. Trot步态的核心是相位差不是每条腿单独抬2.1 步态的本质四条腿在时间上的编队步态协调控制算法听起来很高深但拆到最底层它做的事情就是决定四条腿各自在什么时间点进入抬腿动作、什么时间点落地支撑。所有复杂的步态walk、trot、pace、gallop本质区别只是每条腿的周期、相位和占空比不同。用一个核心概念来概括就是相位。定义步态周期为T每条腿在一个周期内有两个阶段支撑相脚触地、身体向前移动和摆动相脚抬起、向前摆动。摆动相开始的那个时间点相对于周期起点的偏移就是相位。如果四条腿的相位关系安排好了步态就有了。这就像一个四人接力跑的队伍每个人跑的速度都一样但接棒的时间点不同整体看起来就是一条连贯的流动线。在这个项目里我选择的是最常用的trot步态也就是对角步态左前腿和右后腿作为一个对角组同时摆动右前腿和左后腿作为另一组。这种步态的占空比接近0.5意味着任意时刻都有两条腿在支撑地面同时有两条腿在摆动机器人始终处于动态稳定状态。2.2 为什么对角腿必须同相位刚开始做步态规划时我直觉上觉得应该让四条腿按顺时针顺序依次迈步模仿马走路的样子。后来看了稳定性分析才明白四足机器人静态稳定和动态稳定的条件差别很大如果四条腿的支撑顺序安排不当重心很容易跑出支撑多边形之外。trot步态最大的优点就是支撑状态对称。左前腿和右后腿落地时另外两条腿抬起身体重心大致落在对角支撑线附近由于腿部对称布置重心投影仍然位于两条支撑腿连线与身体边缘围成的支撑多边形内。这样一来即使在步态切换的过渡瞬间机器人也不容易发生侧翻。从相位表角度看trot的相位关系就是两组腿相差半个周期。以周期T为360度来算左前腿相位为0度右后腿相位也为0度右前腿相位为180度左后腿相位为180度。这形成了一个非常简洁的步序表所有逻辑都可以用数组和取模运算来实现Arduino这种资源有限的芯片也能轻松处理。2.3 用一个数据驱替循环体来管理步态步态协调控制算法在代码上如果写成switch case分支每条腿分别写一套逻辑代码量会爆炸而且改一个步态参数要同步改四份代码。我用的是数据驱动的方式把步态周期、相位偏移、抬腿高度、步幅这些参数全部放进一个配置结构体然后用统一函数去驱动每一条腿。struct GaitConfig { float period; // 步态周期单位秒 float dutyFactor; // 支撑相占比trot约0.5 float stepHeight; // 抬腿高度单位米 float stepLength; // 步幅长度单位米 float phaseOffset[4]; // 四条腿的相位偏移单位弧度 }; GaitConfig gait { 0.6f, // 周期0.6秒约1.7Hz步频 0.5f, // trot占空比 0.04f, // 抬腿4厘米 0.08f, // 步幅8厘米 {0.0f, PI, 0.0f, PI} // LF0, RFPI, LHPI, RH0 对角线同相 };四条腿的相位偏移表里我用的是物理对称关系左前LF和右后RH同相右前RF和左后LH同相两组相位相差180度。这种写法最大的好处是如果你想改成pacing步态同侧腿同相或者bound步态前后腿分组只需要修改这个数组算法主体完全不用动。3. Arduino端步态控制器的分层实现3.1 控制周期怎么定低频状态机加高频关节控制Arduino Uno这类板子的主频和算力都不算强如果所有逻辑全部塞进一个高频中断里步态规划、逆运动学、PID计算、串口调试这些任务堆在一起很容易跑崩。所以我的架构是分层的低频的状态机负责步态相位更新和轨迹参数计算跑在50Hz左右高频的关节控制循环负责从当前期望轨迹中生成关节角度、转化成驱动器指令跑在500Hz到1kHz。低频和高频之间用一组共享变量做桥梁。低频循环算出当前时刻四条腿足端的期望位置更新到全局变量高频中断读取这些变量做逆运动学解算和PID控制。这种设计的好处是步态参数的调整不会影响关节控制的稳定性关节控制频率也不会被慢速的状态机拖累。Arduino Uno的算力其实可以跑到更高的频率但我在实践中发现对于精度要求不高的演示型四足1kHz的关节控制频率已经足够因为BLDC驱动器本身已经处理了电流环和速度环主控侧的位置环不需要太激进。3.2 足端位置生成器让轨迹从参数变成坐标步态轨迹生成是整个算法中我最喜欢写的部分。它接收一个时间相位输入输出该时刻这条腿足端在机器人坐标系下的目标位置。这一步可以用非常简洁的数学描述。我用一个辅助变量phase表示这条腿在周期内的进度范围从0到2π。当phase处于摆动相时足端从后往前做抛物线运动当phase处于支撑相时足端相对身体往后移动产生向前推进的动力。void computeFootTrajectory(float t, int legIndex, float x, float z) { float phase fmod(t / gait.period, 1.0f); float phi phase gait.phaseOffset[legIndex] / (2.0f * PI); phi fmod(phi, 1.0f); float x0 0.0f; // 足端初始位置 float z0 -0.12f; // 足端自然站立高度负值表示向下 if (phi 0.5f) { // 支撑相身体前移足端相对向后移动 float progress phi / 0.5f; x x0 - gait.stepLength * 0.5f gait.stepLength * progress; z z0; } else { // 摆动相抬腿前摆 float progress (phi - 0.5f) / 0.5f; x x0 - gait.stepLength * 0.5f gait.stepLength * progress; // 用正弦曲线抬腿最高点在摆动中点 z z0 gait.stepHeight * sin(progress * PI); } }正弦曲线抬腿的优点是速度曲线平滑、加速度连续落点和起点的速度都为零对关节的冲击最小。后期我改成摆线轨迹后冲击进一步减小这个后面细说。3.3 控制循环与指令输出高频中断里的代码反而不复杂。核心就是读取当前低频生成的四条腿足端目标依次做逆运动学解算得到四个关节的目标角度然后送给四个PID位置环去生成速度指令。void controlLoop() { // 低频变量保护读取前先禁用中断 noInterrupts(); for (int i 0; i 4; i) { footTargetX[i] footX[i]; footTargetZ[i] footZ[i]; } interrupts(); for (int i 0; i 4; i) { // 逆运动学 float q1, q2; inverseKinematics(footTargetX[i], footTargetZ[i], q1, q2); // 位置环PID输出到BLDC驱动器的速度指令 float cmd1 pidHip[i].update(q1, currentHipAngle[i]); float cmd2 pidKnee[i].update(q2, currentKneeAngle[i]); driveSetVelocity(i, 0, cmd1); // 髋关节电机 driveSetVelocity(i, 1, cmd2); // 膝关节电机 } }这里有一个工程上的重要细节位置环的输出是目标速度而不是目标角度。因为BLDC驱动器内部通常已经做好了速度环主控直接在速度环上做位置控制比直接向驱动器发角度指令更稳定响应也更快。这也是SimpleFOC方案在Arduino四足上表现的优秀架构。3.4 腿部逆运动学从足端坐标到髋膝转角逆运动学是连接足端轨迹和关节电机的桥梁这一步不写明白整个步态算法就悬在空中。每条腿简化为两个连杆大腿长度L1小腿长度L2髋关节在肩膀处膝关节在中间。已知足端在身体坐标系下的位置(x, z)求髋关节角q1和膝关节角q2。这个模型用余弦定理直接解就行。距离d sqrt(xx zz)如果d大于L1L2说明足端超出了机械臂的物理范围这个目标不可达需要做限位处理。小腿和身体沿线的夹角用余弦定理求髋关节的角度稍微整理一下就能算出来void inverseKinematics(float x, float z, float hipAngle, float kneeAngle) { const float L1 0.10f; // 大腿长度 const float L2 0.12f; // 小腿长度 float d sqrt(x*x z*z); if (d L1 L2) d L1 L2; // 距离限制 float a acos((L1*L1 L2*L2 - d*d) / (2 * L1 * L2)); float b atan2(-x, z); // 注意坐标轴方向 float c acos((L1*L1 d*d - L2*L2) / (2 * L1 * d)); hipAngle b c; kneeAngle a - PI; }注意到这里的坐标定义z轴向下为负值所以角度计算会有一点别扭。实际调试中很容易犯的错就是坐标轴符号搞反导致腿部反着转。建议在正式运行前先把每条腿的足端目标设成固定点单独测试逆运动学输出是否正确。4. 轨迹平滑与触地冲击抑制4.1 为什么正弦抬腿会被替换成摆线第一步用正弦曲线做抬腿轨迹跑起来之后机器人能走但每次落地都会听到明显的咯噔声机身也有微小的弹跳。原因是正弦曲线的抬腿轨迹虽然速度连续但加速度不连续特别是在落地的瞬间足端速度虽然接近零但加速度突变会产生一个冲击力。后来我把摆动相的轨迹换成了摆线cycloid轨迹。摆线有一个非常好的性质起点和终点的速度和加速度都为零这样可以极大减少对关节电机的冲击。它的参数方程很简洁// 摆线轨迹progress从0到1 float angle progress * 2.0f * PI; float boost (1.0f - cos(angle)) * 0.5f; // 0到1之间平滑变化 footX xStart (xEnd - xStart) * boost; footZ zStart - gait.stepHeight * (boost - progress);其中xStart和xEnd分别对应这条腿在这个步态周期内支撑相起点和终点的位置。摆线的本质是让x方向的运动和z方向的抬升共用同一个平滑曲线参数这样一来足端在空间中划过的轨迹是一条光滑的弧线落地前足端几乎以零速度贴近地面。4.2 抬腿高度和步幅的匹配逻辑很多初次做四足的朋友会认为抬腿越高越稳这个直觉不完全对。步幅越大抬腿越高是合理的但如果步幅很小、抬腿很高腿部在空中划过一个很大的弧形摆动相时间被拉长机器人在单支撑期的姿态波动反而更大。我的经验是步幅和抬腿高度的比例维持在2比1到3比1之间比较舒服。比如步幅8厘米抬腿高度3到4厘米就行了。实际情况中如果地面不平整可以适当增大抬腿高度但要配合降低步态频率给腿部留出足够的摆动时间。4.3 速度前馈是消除轨迹跟踪延迟的关键Arduino端的PID位置环是最容易调得看似稳定、实际滞后的环节。光有位置环的时候机器人低速行走问题不大但一旦把步态周期缩短到0.5秒以下会发现腿部动作明显跟不上期望轨迹看起来像慢半拍。根本原因是位置环的响应存在固有滞后。要想让实际关节角度紧紧贴住期望轨迹需要在PID输出上叠加速度前馈。也就是把期望角度的导数乘以一个前馈系数加进去让电机提前朝目标方向运动而不是等误差出现后再纠正。这个技巧在做高频步态时几乎是必须的它可以让轨迹跟踪误差缩小一个数量级。5. 实测记录三个绕不开的问题和对应解决路径5.1 编码器零点没有对齐导致机器人横着走第一版装好之后我只做了简单的通电转动测试就上电跑步态结果四条腿动作非常不协调明明相位表没问题机器人却像螃蟹一样横着往一侧漂移。排查了半天才发现问题不在算法而是四个髋关节电机的编码器零点都没有和连杆方向对齐。BLDC电机和编码器之间如果没有做机械零点标定那么控制器读到的零度并不是机械上我们定义的髋关节朝前方零度。每一条腿的偏移量不同算法给它发的0度指令实际关节角度就是不同的。这会导致步态协调完全失效。解决方法是做一次零点校准。在程序里初始化阶段让所有腿依次转动到机械限位位置然后把这个位置的编码器角度记录下来作为零点偏移。校准完成后所有关节角度都转换到以机身坐标系为基准的逻辑角度上。5.2 电池电压跌落导致后腿扭矩不足步态跑起来之后发现一个间歇性的现象在加速转弯的时候两条后腿偶尔会突然软一下机器人明显下沉。排查了整个控制链路没有发现问题最后用示波器监控电源电压才发现瞬时大扭矩输出时锂聚合物电池的压降接近1.5伏驱动器因为欠压保护限流电机输出扭矩瞬间下降。这个问题有两层解决思路。硬件上在靠近驱动器的地方并联稳压电容阵一组22微法陶瓷电容加一个470微法以上电解电容能明显抑制瞬时压降。软件上在算法里增加扭矩分配感知逻辑检测到电压跌落时降低步态频率和步幅避免四条腿同时输出峰值扭矩。5.3 机身姿态倾斜扰动步态相位把机器人放到稍微不平的地面上行走时发现步态越来越歪最终甚至原地打转。原因是我的步态协调算法假设机身始终水平四条腿的足端轨迹都是相对于机身坐标系的。当地面不平时某条腿提前触地会导致其他腿的支撑时间发生变化整个步态相位就慢慢漂移了。这个问题的根本解法是加入机身姿态反馈用一个IMU实时读取机身倾角把倾斜带来的足端偏移补偿到逆运动学目标里。在Arduino端可用简单的互补滤波或者四元数解算姿态然后把期望足端位置变换到世界坐标系这样即使机身有倾斜腿部也能自动调整。如果IMU还没接上临时办法是把步态频率调低、步幅调小让机器人慢慢走减少姿态扰动的影响。但这只是权宜之计真正解决还是要靠姿态闭环。6. 再往后可以扩展的方向这套步态协调控制算法跑通之后整个项目的架构已经给后续扩展留好了接口。相位表机制可以扩展到更多步态姿态反馈可以升级成完整的状态估计器位置环的PID可以替换成更高级的阻抗控制或者力位混合控制。我在实际调试中最大的体会是做四足步态不要被四条腿的数量吓住。算法上真正复杂的东西并不多最需要耐心的地方反而是执行器的校准、机械结构的刚性和调试工具的准备。把底层关节控制做好了步态协调控制算法的实现和调试会比想象中顺畅很多。如果你也准备用Arduino做四足建议先把单腿运动学调通再做双腿协调最后才上四腿步态。一次只管一条腿出问题时定位范围小很多整个项目推进起来也会稳很多。