简介基于STM32的六足机器人完整工程资料面向嵌入式开发者、机器人爱好者及高校相关课程设计人群解决从微控制器选型、电机驱动到步态控制与系统联调的全流程实现问题适合有一定单片机基础并希望深入机器人运动控制的读者。压缩包共856个文件包含C/H源码、Keil工程文件、PCB与原理图设计文件、Hex固件、APK遥控端及PDF说明等覆盖软硬件设计全链路整体约46.48MB目前已有1103人学习下载。内容包含STM32F4系列主控的硬件原理图与PCB工程、PWM电机驱动与传感器接口设计固件采用模块化结构集成FreeRTOS任务调度与PID控制算法并提供直行、转弯、爬坡等多步态代码可快速移植到同类多足平台。同时附带无线通信、调试配置与线上APK等说明便于理解整机搭建与排错是一份可直接参考的六足机器人实战资料。1. 六足机器人为何要锁定 STM32 而不是树莓派六足机器人在嵌入式项目里是一个分水岭它比小车多出“步态”这个维度又比机械臂多出“多腿支撑与摆动交替”的时序问题。18 个舵机要在一个控制周期内全部收到新目标角度三角步态下每条腿落地的瞬间还要完成坐标解算这时候如果主控系统还在被操作系统调度打断机身就会出现肉眼可见的抽动。选择 STM32 的核心理由不是算力而是定时器外设的确定性——PWM 波形由硬件生成CPU 只负责在每个步态周期内计算角度任务边界非常清晰。本文从 F103/F407 这类常见芯片出发把六足机器人的硬件分配、运动学逆解、HAL 固件框架和调参顺序串成一套可以照着落地的方案适合已经具备基本嵌入式开发经验、想第一次把六足真正立起来走路的工程师。2. 六足机器人硬件骨架18 自由度布局、舵机选型与电源预算2.1 每条腿三个关节六条腿怎么分配自由度标准六足布局是每条腿三个转动关节分别对应髋关节、股关节和胫关节。髋关节轴线垂直机身平面负责整条腿在水平面上的内外摆动股关节和胫关节轴线平行共同决定足端在竖直平面内的位置。每条腿 3 个自由度六条腿一共 18 个自由度对应 18 路 PWM 输出。这个自由度分配也是大多数开源六足项目的默认配置好处是逆解计算可以拆成“平面投影 余弦定理”两步在 STM32 上做浮点运算也不会太吃力。腿部尺寸比例会影响步态的极限范围。髋关节到股关节之间的固定段通常取 4~6 cm这段距离决定了足端能否收到身体正下方距离过短时足端会明显外撇转向步态会变得笨拙。股节和胫节长度比较常见的比例是 1:1.2 到 1:1.5比如股节 7 cm、胫节 9 cm。比例太接近膝关节屈伸角度会很极端做三角步态时相邻两条腿容易互相干涉比例拉太长整机重心被抬高侧向行走时翻倒力矩会变大。装机前最好先在纸上画出腿的极限位置确认整个运动范围内股节与胫节不会碰到机身底板。2.2 舵机选型与扭矩估算舵机选型主要看扭矩和响应速度。机身重量假设为 1.5 kg三角步态下任意时刻只有三条腿着地单条腿至少要承担 0.5 kg 的静载荷考虑加速冲击和舵机传动损耗股关节的峰值扭矩要按 2~3 倍安全系数估算。入门方案常用 MG996R 或同类数字舵机标称堵转扭矩在 9.4 kg·cm 左右对一台 1.5 kg 以下的小型六足足够用再往上走可以用液压不更贵的舵机如 GDW DS5160 或 Invioid 系列——应避免特定折扣。用一两张参考表舵机类型标称扭矩工作电压控制信号适用机身MG90S1.8 kg·cm4.8V20ms 周期500-2500us 脉冲桌面级迷你六足MG996R9.4 kg·cm4.8-6.0V20ms 周期500-2500us 脉冲1.5 kg 以内入门六足30kg 级别金属舵机30 kg·cm6.0-7.4V数字信号支持更高频率3 kg 以上载重实验平台注意市面上舵机参数虚标很常见国产 MG996R 实际扭矩往往到不了标称值。协议层面建议在步态周期里留出 20% 的扭矩余量否则电池电压一掉腿部会在摆动相末端软掉出现拖地。2.3 电源预算、舵机供电与 MCU 供电必须分离18 个舵机同时动作时瞬时电流会非常难看。以 MG996R 为例单舵机堵转电流约 1.2A即使正常摆动只按 0.3A 平均估算瞬时尖峰也可能到 3A 以上。如果把 STM32 和舵机共用同一路电源PWM 引脚上的干扰会直接传导到复位电路表现为机身一动主控就重启。常见做法是双电源方案舵机用 2S 锂电池供电经过 BEC 或降压模块稳定到 6VSTM32 板单独用另一路 5V 电源地线在电池负极单点汇合。若手头只有一个电池则至少要加一枚容量大于 1000uF 的低 ESR 电解电容跨接在舵机电源输入端并用 DC-DC 隔离模拟区电容。3. 运动学逆解从足端坐标到 18 路角度推导与 C 实现3.1 先算髋关节角水平面上的投影问题每条腿的逆解可以分两步走。第一步看水平面把足端目标点投影到机身平面上投影点与髋关节轴心的连线相对机身中轴线形成的夹角就是髋关节角度。用atan2f直接求解即可float coxa_angle atan2f(y, x);其中x是足端在机体坐标系中沿机身轴方向的距离y是沿腿部安装轴向外方向的距离。这里atan2f比atanf更安全因为它不需要调用方处理 x0 的四象限问题返回范围正好覆盖舵机从 -90 度到 90 度的常见行程。算出髋关节角后把目标点变换到“髋关节旋转后的平面”中原先的三维问题就降维成了二维新的水平距离为sqrt(x*x y*y)再减去髋关节物理长度剩下的就是股关节和胫关节要在竖直平面内覆盖的距离。这个小技巧让后面的三角函数推导只发生在一个平面内不容易出错。3.2 股关节与胫关节余弦定理求解平面三角形降维之后的问题可以归纳为已知股节长度femur、胫节长度tibia以及从股关节轴心到足端的距离在竖直平面内的投影R和垂直高度z求解两个关节角。从股关节到足端的空间直线距离D为D sqrt(R*R z*z)在股节、胫节和虚拟连线D构成的三角形中三条边长度都已知直接用余弦定理求膝关节角tibia_angle PI - acos((femur^2 tibia^2 - D^2) / (2*femur*tibia))股关节角则由两个角度叠加从竖直方向到虚拟连线的夹角以及虚拟连线和股节之间的夹角。前一个用atan2f(R, -z)后一个再用一次余弦定理。这里是完整的 C 函数#include math.h typedef struct { float coxa_len; /* 髋关节固定段长度, 单位 m */ float femur_len; /* 股节长度 */ float tibia_len; /* 胫节长度 */ } LegGeometry; typedef struct { float coxa; /* 髋关节角度, 单位 rad */ float femur; /* 股关节角度 */ float tibia; /* 胫关节角度 */ } LegAngle; /* 足端坐标: x 向前, y 向外, z 向下为负 */ int leg_ik(const LegGeometry *geo, float x, float y, float z, LegAngle *angle) { float L0 sqrtf(x*x y*y); float R L0 - geo-coxa_len; if (R 0.01f) return -1; /* 目标太靠近身体, 无法触达 */ float D sqrtf(R*R z*z); if (D geo-femur_len geo-tibia_len - 0.002f) return -2; /* 目标超出机械臂展 */ if (D fabsf(geo-femur_len - geo-tibia_len) 0.002f) return -3; /* 目标过近, 机构无法折叠到该点 */ float phi atan2f(R, -z); float beta acosf((R*R z*z geo-femur_len*geo-femur_len - geo-tibia_len*geo-tibia_len) / (2.0f * D * geo-femur_len)); float gamma acosf((geo-femur_len*geo-femur_len geo-tibia_len*geo-tibia_len - D*D) / (2.0f * geo-femur_len * geo-tibia_len)); angle-coxa atan2f(y, x); angle-femur phi - beta; angle-tibia 3.14159265f - gamma; return 0; }函数返回非 0 值表示目标点超出关节可及范围调用方应丢弃该目标。特别注意z的方向约定我把正方向定义为向上六足站立时足端在机身下方所以z传入负值。atan2f(R, -z)里的负号不能省否则股关节角度符号会翻转走路时腿会朝反方向蹬。3.3 机体坐标系转换步幅和转向怎么叠加每条腿的足端目标不只在腿部坐标系中计算还要加上机身位姿变化。实际项目中我习惯定义两个坐标系机体坐标系固定于机身中心世界坐标系固定于地面机身可平移和绕 Y 轴旋转转向。步态算法生成的是世界坐标系下的足端位置装入腿之前要先变换回机体坐标系。一个常见的简化处理是忽略机身横滚与俯仰只对偏航角做二维旋转同时把机身平移量直接叠加到足端坐标上。对第 i 条腿足端在机体坐标系下的位置可以写成world_x body_forward leg_local_x[i] * cos(yaw) - leg_local_y[i] * sin(yaw); world_y body_side leg_local_x[i] * sin(yaw) leg_local_y[i] * cos(yaw);其中body_forward是步态周期内机身的位移量body_side用于侧移步态yaw是转向角。这样腿部逆解函数本身只关心足端相对髋关节的位置步态的平移、旋转全部在调用前完成代码边界清晰后续加 IMU 姿态反馈也只需要改动装配体坐标变换这一层。4. STM32 HAL 库固件PWM 输出、步态状态机与姿态补偿4.1 用定时器分配 18 路 PWM 的最小配置18 路舵机信号在 STM32 上需要两个或三个定时器协同工作。F103 系列的 TIM1、TIM2、TIM3、TIM4 都带有多个捕获比较通道以 TIM2 和 TIM3 为例各出 4 路 PWM再加上 TIM4 的两路就凑齐 18 路。如果使用 F407还可以启用 TIM1 的互补通道但驱动舵机其实不需要互补输出常规通道就够。HAL 库配置时注意几个边界条件预分频系数和自动重装载值必须保证 PWM 周期为 20ms。以 72 MHz 主频的 STM32F103 为例预分频设为72-1使计数器时钟为 1MHz自动重装载值设为20000-1输出频率就是 1MHz / 20000 50Hz这正是模拟舵机最常见的刷新频率。下面是初始化一路通道的最小代码TIM_HandleTypeDef htim2; TIM_OC_InitTypeDef sConfigOC {0}; __HAL_RCC_TIM2_CLK_ENABLE(); htim2.Instance TIM2; htim2.Init.Prescaler 72 - 1; /* 72MHz / 72 1MHz */ htim2.Init.Period 20000 - 1; /* 1MHz / 20000 50Hz */ htim2.Init.CounterMode TIM_COUNTERMODE_UP; htim2.Init.ClockDivision TIM_CLOCKDIVISION_DIV1; HAL_TIM_PWM_Init(htim2); sConfigOC.OCMode TIM_OCMODE_PWM1; sConfigOC.Pulse 1500; /* 初始脉冲宽度 1500us */ sConfigOC.OCPolarity TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode TIM_OCFAST_DISABLE; HAL_TIM_PWM_ConfigChannel(htim2, sConfigOC, TIM_CHANNEL_1); HAL_TIM_PWM_Start(htim2, TIM_CHANNEL_1);修改舵机角度时只需要调用__HAL_TIM_SET_COMPARE(htim2, TIM_CHANNEL_1, pulse)其中pulse的单位是微秒。如果后续换成 333Hz 数字舵机把Period改为3000-1即可Pulse 上限也要跟着缩放到 500~2500us 区间内这个映射表建议放在单独的函数里统一管理不要散落在步态代码中。4.2 步态状态机三角步态与波纹步态的时序参数步态的本质是给六条腿分配“支撑相”和“摆动相”。支撑相足端着地相对地面后移推动机身前进摆动相足端抬起向前抬到新落地点。三角步态的优势是任意时刻只有三条腿在摆动另外三条腿形成稳定三角形支撑对机身静稳定性要求最低。相位分配通常是腿部奇数组0、2、4与偶数组1、3、5各占半个周期如下图文字表示中的leg_phase[i]数组可以用均匀错开法得到。我用一个简单状态机管理步态相位每条腿维护自己的phase计时器#define LEG_COUNT 6 typedef struct { float cycle_time; /* 一个完整周期, 单位 s */ float swing_ratio; /* 摆动相占整个周期的比例 */ float step_length; /* 单步步幅, 单位 m */ float step_height; /* 摆动相抬腿高度 */ } GaitParam; float leg_phase[LEG_COUNT]; /* 每条腿当前所处时间点 */ void gait_init(float cycle, float swing_ratio, GaitParam *gp) { gp-cycle_time cycle; gp-swing_ratio swing_ratio; for (int i 0; i LEG_COUNT; i) { leg_phase[i] (float)i / LEG_COUNT * cycle; } } int leg_in_swing(int idx, const GaitParam *gp) { return leg_phase[idx] gp-cycle_time * gp-swing_ratio; } void gait_tick(float dt, GaitParam *gp) { for (int i 0; i LEG_COUNT; i) { leg_phase[i] dt; while (leg_phase[i] gp-cycle_time) { leg_phase[i] - gp-cycle_time; } } }这里摆动相时长由cycle_time * swing_ratio决定。三角步态的相位偏移恰好是两只腿组之间差半个周期gait_init里均匀分配相位正好形成“三只抬、三只落”的三角步态。若要实现波纹步态则把相位差改成cycle_time / 6每次只抬起一条腿稳定余量最大但速度最慢。实际调速度时优先缩短cycle_time而不是增大step_length因为大步幅会加剧机身重心起伏。摆动相的足端轨迹还需要一个竖直方向的正弦或梯形插值否则会出现瞬间加速抖动。我通常对抬腿高度做半正弦曲线float swing_z_offset(float progress) { return sinf(progress * 3.14159265f) * gp.step_height; }progress是摆动相内部的归一化时间从 0 到 1。这个函数让足端先抬后落起落速度在两端都趋于零机身明显更稳。三种步态的本质区别只是相位差和摆动相占比状态机的骨架可以共用。4.3 用 MPU6050 做姿态补偿修正机身倾斜地面稍有不平六足机身就会歪向一侧步态调得再好也会出现“长短腿”。常见做法是在机身中心装一个 MPU6050读取加速度计数据估算横滚角与俯仰角再把这部分角度换算成腿部坐标补偿量。加速度计读取函数用 I2C 直接读取原始值即可uint8_t mpu60_read_angle(float *roll, float *pitch) { uint8_t buf[6]; if (HAL_I2C_Mem_Read(hi2c1, 0x68 1, 0x3B, 1, buf, 6, 100) ! HAL_OK) { return 1; } int16_t ax (int16_t)((buf[0] 8) | buf[1]); int16_t ay (int16_t)((buf[2] 8) | buf[3]); int16_t az (int16_t)((buf[4] 8) | buf[5]); *roll atan2f((float)ay, (float)az); *pitch atan2f(-(float)ax, sqrtf((float)ay * ay (float)az * az)); return 0; }注意 MPU6050 需要先配置电源管理寄存器把器件从睡眠模式唤醒并设置加速度计量程。姿态解算在六足上不必用完整的四元数卡尔曼滤波atan2近似就够用——因为机身运动速度远低于碰撞冲击频率加速度计的低频噪声可以通过简单低通处理。拿到横滚角后把它乘以腿部到机身中心的 Y 向距离叠加到2.3节的机体坐标变换里就能让足端在倾斜地面上自动调整落点实现姿态保持。采样频率可放到 100Hz 左右与控制周期错开避免 I2C 阻塞步态时序。5. 调参顺序与三处最容易被忽略的坑5.1 先标定舵机零位机械装配与代码零点对齐六足机器人最常见的第一个故障是单腿抬起时其余腿乱抽根源多半是零位没对齐。舵机出厂时零位通常在中位脉冲 1500us但机械臂装配角度各不相同同一块舵机安装在不同腿上时其“水平伸展”位置对应的脉冲宽度可能相差几十微秒。正式调步态前先逐个通道发送 1500us 脉冲用目测或量角器把每条腿的三个关节都掰到机械参考位置然后记录每个舵机的零位偏差写进一个数组float pwm_offset[LEG_COUNT][3] { { 1480, 1520, 1495 }, { 1505, 1490, 1510 }, /* 每行对应一条腿的髋、股、胫零位偏移 */ };这个数组在输出 PWM 前与逆解角度叠加。零位偏差与步幅、步态参数无关优先调好它后续所有角度换算才有意义。5.2 用串口打印与逻辑分析仪验证 18 路 PWM 的实时性代码跑起来后靠眼睛判断步态是否正常很困难尤其是机身只有轻微抖动时。我通常分两步做验证先从工程里导出实际发送的 18 路脉冲宽度通过串口以 115200 bps 周期打印观察相邻控制周期内角度跳变是否超过预设阈值再用逻辑分析仪同时抓两路 PWM确认周期稳定在 20.00ms 而不是 19.5ms 这种漂移值。如果发现周期不稳原因基本集中在三处PWM 定时器的预分频和自动重装载没按 1MHz/50Hz 组合配置中断服务函数里做了耗时的浮点运算导致主循环卡顿舵机刷新频率与步态控制周期没有间隔对齐。将步态计算放在定时器中断里、PWM 脉宽更新放在主循环中是最稳妥的分工——前者保证相位推进的确定性后者避免在中断里调用HAL_TIM_PWM_Start这类重函数。5.3 给运动学加限幅防止意外指令损坏舵机与结构逆解函数返回负数时只是在说“算不出来”实际舵机并不会因此停在原处。真正危险的场景是目标点刚好在可达域边缘逆解得出的角度接近 180 度而 PWM 脉宽映射到舵机硬件极限外此时舵机堵转发热齿轮组会直接崩坏。最后的保护措施是角度限幅void clamp_angle(float *angle, float min, float max) { if (*angle min) *angle min; if (*angle max) *angle max; }把每个关节的允许角度范围做成数组在leg_ik返回成功后立即调用。限幅值取机械行程的 80%留下余量给运动中的惯性过冲。验证限幅是否生效的做法是让步行步幅参数从 10mm 逐步增大到 50mm同时串口打印每次被限制到的关节索引和限制次数如果限制次数随步幅线性增长说明机身结构或足端轨迹设计需要调整而不是简单调低限幅范围。本文还有配套的精品资源点击获取
STM32六足机器人实战：运动学逆解与步态控制实现
简介基于STM32的六足机器人完整工程资料面向嵌入式开发者、机器人爱好者及高校相关课程设计人群解决从微控制器选型、电机驱动到步态控制与系统联调的全流程实现问题适合有一定单片机基础并希望深入机器人运动控制的读者。压缩包共856个文件包含C/H源码、Keil工程文件、PCB与原理图设计文件、Hex固件、APK遥控端及PDF说明等覆盖软硬件设计全链路整体约46.48MB目前已有1103人学习下载。内容包含STM32F4系列主控的硬件原理图与PCB工程、PWM电机驱动与传感器接口设计固件采用模块化结构集成FreeRTOS任务调度与PID控制算法并提供直行、转弯、爬坡等多步态代码可快速移植到同类多足平台。同时附带无线通信、调试配置与线上APK等说明便于理解整机搭建与排错是一份可直接参考的六足机器人实战资料。1. 六足机器人为何要锁定 STM32 而不是树莓派六足机器人在嵌入式项目里是一个分水岭它比小车多出“步态”这个维度又比机械臂多出“多腿支撑与摆动交替”的时序问题。18 个舵机要在一个控制周期内全部收到新目标角度三角步态下每条腿落地的瞬间还要完成坐标解算这时候如果主控系统还在被操作系统调度打断机身就会出现肉眼可见的抽动。选择 STM32 的核心理由不是算力而是定时器外设的确定性——PWM 波形由硬件生成CPU 只负责在每个步态周期内计算角度任务边界非常清晰。本文从 F103/F407 这类常见芯片出发把六足机器人的硬件分配、运动学逆解、HAL 固件框架和调参顺序串成一套可以照着落地的方案适合已经具备基本嵌入式开发经验、想第一次把六足真正立起来走路的工程师。2. 六足机器人硬件骨架18 自由度布局、舵机选型与电源预算2.1 每条腿三个关节六条腿怎么分配自由度标准六足布局是每条腿三个转动关节分别对应髋关节、股关节和胫关节。髋关节轴线垂直机身平面负责整条腿在水平面上的内外摆动股关节和胫关节轴线平行共同决定足端在竖直平面内的位置。每条腿 3 个自由度六条腿一共 18 个自由度对应 18 路 PWM 输出。这个自由度分配也是大多数开源六足项目的默认配置好处是逆解计算可以拆成“平面投影 余弦定理”两步在 STM32 上做浮点运算也不会太吃力。腿部尺寸比例会影响步态的极限范围。髋关节到股关节之间的固定段通常取 4~6 cm这段距离决定了足端能否收到身体正下方距离过短时足端会明显外撇转向步态会变得笨拙。股节和胫节长度比较常见的比例是 1:1.2 到 1:1.5比如股节 7 cm、胫节 9 cm。比例太接近膝关节屈伸角度会很极端做三角步态时相邻两条腿容易互相干涉比例拉太长整机重心被抬高侧向行走时翻倒力矩会变大。装机前最好先在纸上画出腿的极限位置确认整个运动范围内股节与胫节不会碰到机身底板。2.2 舵机选型与扭矩估算舵机选型主要看扭矩和响应速度。机身重量假设为 1.5 kg三角步态下任意时刻只有三条腿着地单条腿至少要承担 0.5 kg 的静载荷考虑加速冲击和舵机传动损耗股关节的峰值扭矩要按 2~3 倍安全系数估算。入门方案常用 MG996R 或同类数字舵机标称堵转扭矩在 9.4 kg·cm 左右对一台 1.5 kg 以下的小型六足足够用再往上走可以用液压不更贵的舵机如 GDW DS5160 或 Invioid 系列——应避免特定折扣。用一两张参考表舵机类型标称扭矩工作电压控制信号适用机身MG90S1.8 kg·cm4.8V20ms 周期500-2500us 脉冲桌面级迷你六足MG996R9.4 kg·cm4.8-6.0V20ms 周期500-2500us 脉冲1.5 kg 以内入门六足30kg 级别金属舵机30 kg·cm6.0-7.4V数字信号支持更高频率3 kg 以上载重实验平台注意市面上舵机参数虚标很常见国产 MG996R 实际扭矩往往到不了标称值。协议层面建议在步态周期里留出 20% 的扭矩余量否则电池电压一掉腿部会在摆动相末端软掉出现拖地。2.3 电源预算、舵机供电与 MCU 供电必须分离18 个舵机同时动作时瞬时电流会非常难看。以 MG996R 为例单舵机堵转电流约 1.2A即使正常摆动只按 0.3A 平均估算瞬时尖峰也可能到 3A 以上。如果把 STM32 和舵机共用同一路电源PWM 引脚上的干扰会直接传导到复位电路表现为机身一动主控就重启。常见做法是双电源方案舵机用 2S 锂电池供电经过 BEC 或降压模块稳定到 6VSTM32 板单独用另一路 5V 电源地线在电池负极单点汇合。若手头只有一个电池则至少要加一枚容量大于 1000uF 的低 ESR 电解电容跨接在舵机电源输入端并用 DC-DC 隔离模拟区电容。3. 运动学逆解从足端坐标到 18 路角度推导与 C 实现3.1 先算髋关节角水平面上的投影问题每条腿的逆解可以分两步走。第一步看水平面把足端目标点投影到机身平面上投影点与髋关节轴心的连线相对机身中轴线形成的夹角就是髋关节角度。用atan2f直接求解即可float coxa_angle atan2f(y, x);其中x是足端在机体坐标系中沿机身轴方向的距离y是沿腿部安装轴向外方向的距离。这里atan2f比atanf更安全因为它不需要调用方处理 x0 的四象限问题返回范围正好覆盖舵机从 -90 度到 90 度的常见行程。算出髋关节角后把目标点变换到“髋关节旋转后的平面”中原先的三维问题就降维成了二维新的水平距离为sqrt(x*x y*y)再减去髋关节物理长度剩下的就是股关节和胫关节要在竖直平面内覆盖的距离。这个小技巧让后面的三角函数推导只发生在一个平面内不容易出错。3.2 股关节与胫关节余弦定理求解平面三角形降维之后的问题可以归纳为已知股节长度femur、胫节长度tibia以及从股关节轴心到足端的距离在竖直平面内的投影R和垂直高度z求解两个关节角。从股关节到足端的空间直线距离D为D sqrt(R*R z*z)在股节、胫节和虚拟连线D构成的三角形中三条边长度都已知直接用余弦定理求膝关节角tibia_angle PI - acos((femur^2 tibia^2 - D^2) / (2*femur*tibia))股关节角则由两个角度叠加从竖直方向到虚拟连线的夹角以及虚拟连线和股节之间的夹角。前一个用atan2f(R, -z)后一个再用一次余弦定理。这里是完整的 C 函数#include math.h typedef struct { float coxa_len; /* 髋关节固定段长度, 单位 m */ float femur_len; /* 股节长度 */ float tibia_len; /* 胫节长度 */ } LegGeometry; typedef struct { float coxa; /* 髋关节角度, 单位 rad */ float femur; /* 股关节角度 */ float tibia; /* 胫关节角度 */ } LegAngle; /* 足端坐标: x 向前, y 向外, z 向下为负 */ int leg_ik(const LegGeometry *geo, float x, float y, float z, LegAngle *angle) { float L0 sqrtf(x*x y*y); float R L0 - geo-coxa_len; if (R 0.01f) return -1; /* 目标太靠近身体, 无法触达 */ float D sqrtf(R*R z*z); if (D geo-femur_len geo-tibia_len - 0.002f) return -2; /* 目标超出机械臂展 */ if (D fabsf(geo-femur_len - geo-tibia_len) 0.002f) return -3; /* 目标过近, 机构无法折叠到该点 */ float phi atan2f(R, -z); float beta acosf((R*R z*z geo-femur_len*geo-femur_len - geo-tibia_len*geo-tibia_len) / (2.0f * D * geo-femur_len)); float gamma acosf((geo-femur_len*geo-femur_len geo-tibia_len*geo-tibia_len - D*D) / (2.0f * geo-femur_len * geo-tibia_len)); angle-coxa atan2f(y, x); angle-femur phi - beta; angle-tibia 3.14159265f - gamma; return 0; }函数返回非 0 值表示目标点超出关节可及范围调用方应丢弃该目标。特别注意z的方向约定我把正方向定义为向上六足站立时足端在机身下方所以z传入负值。atan2f(R, -z)里的负号不能省否则股关节角度符号会翻转走路时腿会朝反方向蹬。3.3 机体坐标系转换步幅和转向怎么叠加每条腿的足端目标不只在腿部坐标系中计算还要加上机身位姿变化。实际项目中我习惯定义两个坐标系机体坐标系固定于机身中心世界坐标系固定于地面机身可平移和绕 Y 轴旋转转向。步态算法生成的是世界坐标系下的足端位置装入腿之前要先变换回机体坐标系。一个常见的简化处理是忽略机身横滚与俯仰只对偏航角做二维旋转同时把机身平移量直接叠加到足端坐标上。对第 i 条腿足端在机体坐标系下的位置可以写成world_x body_forward leg_local_x[i] * cos(yaw) - leg_local_y[i] * sin(yaw); world_y body_side leg_local_x[i] * sin(yaw) leg_local_y[i] * cos(yaw);其中body_forward是步态周期内机身的位移量body_side用于侧移步态yaw是转向角。这样腿部逆解函数本身只关心足端相对髋关节的位置步态的平移、旋转全部在调用前完成代码边界清晰后续加 IMU 姿态反馈也只需要改动装配体坐标变换这一层。4. STM32 HAL 库固件PWM 输出、步态状态机与姿态补偿4.1 用定时器分配 18 路 PWM 的最小配置18 路舵机信号在 STM32 上需要两个或三个定时器协同工作。F103 系列的 TIM1、TIM2、TIM3、TIM4 都带有多个捕获比较通道以 TIM2 和 TIM3 为例各出 4 路 PWM再加上 TIM4 的两路就凑齐 18 路。如果使用 F407还可以启用 TIM1 的互补通道但驱动舵机其实不需要互补输出常规通道就够。HAL 库配置时注意几个边界条件预分频系数和自动重装载值必须保证 PWM 周期为 20ms。以 72 MHz 主频的 STM32F103 为例预分频设为72-1使计数器时钟为 1MHz自动重装载值设为20000-1输出频率就是 1MHz / 20000 50Hz这正是模拟舵机最常见的刷新频率。下面是初始化一路通道的最小代码TIM_HandleTypeDef htim2; TIM_OC_InitTypeDef sConfigOC {0}; __HAL_RCC_TIM2_CLK_ENABLE(); htim2.Instance TIM2; htim2.Init.Prescaler 72 - 1; /* 72MHz / 72 1MHz */ htim2.Init.Period 20000 - 1; /* 1MHz / 20000 50Hz */ htim2.Init.CounterMode TIM_COUNTERMODE_UP; htim2.Init.ClockDivision TIM_CLOCKDIVISION_DIV1; HAL_TIM_PWM_Init(htim2); sConfigOC.OCMode TIM_OCMODE_PWM1; sConfigOC.Pulse 1500; /* 初始脉冲宽度 1500us */ sConfigOC.OCPolarity TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode TIM_OCFAST_DISABLE; HAL_TIM_PWM_ConfigChannel(htim2, sConfigOC, TIM_CHANNEL_1); HAL_TIM_PWM_Start(htim2, TIM_CHANNEL_1);修改舵机角度时只需要调用__HAL_TIM_SET_COMPARE(htim2, TIM_CHANNEL_1, pulse)其中pulse的单位是微秒。如果后续换成 333Hz 数字舵机把Period改为3000-1即可Pulse 上限也要跟着缩放到 500~2500us 区间内这个映射表建议放在单独的函数里统一管理不要散落在步态代码中。4.2 步态状态机三角步态与波纹步态的时序参数步态的本质是给六条腿分配“支撑相”和“摆动相”。支撑相足端着地相对地面后移推动机身前进摆动相足端抬起向前抬到新落地点。三角步态的优势是任意时刻只有三条腿在摆动另外三条腿形成稳定三角形支撑对机身静稳定性要求最低。相位分配通常是腿部奇数组0、2、4与偶数组1、3、5各占半个周期如下图文字表示中的leg_phase[i]数组可以用均匀错开法得到。我用一个简单状态机管理步态相位每条腿维护自己的phase计时器#define LEG_COUNT 6 typedef struct { float cycle_time; /* 一个完整周期, 单位 s */ float swing_ratio; /* 摆动相占整个周期的比例 */ float step_length; /* 单步步幅, 单位 m */ float step_height; /* 摆动相抬腿高度 */ } GaitParam; float leg_phase[LEG_COUNT]; /* 每条腿当前所处时间点 */ void gait_init(float cycle, float swing_ratio, GaitParam *gp) { gp-cycle_time cycle; gp-swing_ratio swing_ratio; for (int i 0; i LEG_COUNT; i) { leg_phase[i] (float)i / LEG_COUNT * cycle; } } int leg_in_swing(int idx, const GaitParam *gp) { return leg_phase[idx] gp-cycle_time * gp-swing_ratio; } void gait_tick(float dt, GaitParam *gp) { for (int i 0; i LEG_COUNT; i) { leg_phase[i] dt; while (leg_phase[i] gp-cycle_time) { leg_phase[i] - gp-cycle_time; } } }这里摆动相时长由cycle_time * swing_ratio决定。三角步态的相位偏移恰好是两只腿组之间差半个周期gait_init里均匀分配相位正好形成“三只抬、三只落”的三角步态。若要实现波纹步态则把相位差改成cycle_time / 6每次只抬起一条腿稳定余量最大但速度最慢。实际调速度时优先缩短cycle_time而不是增大step_length因为大步幅会加剧机身重心起伏。摆动相的足端轨迹还需要一个竖直方向的正弦或梯形插值否则会出现瞬间加速抖动。我通常对抬腿高度做半正弦曲线float swing_z_offset(float progress) { return sinf(progress * 3.14159265f) * gp.step_height; }progress是摆动相内部的归一化时间从 0 到 1。这个函数让足端先抬后落起落速度在两端都趋于零机身明显更稳。三种步态的本质区别只是相位差和摆动相占比状态机的骨架可以共用。4.3 用 MPU6050 做姿态补偿修正机身倾斜地面稍有不平六足机身就会歪向一侧步态调得再好也会出现“长短腿”。常见做法是在机身中心装一个 MPU6050读取加速度计数据估算横滚角与俯仰角再把这部分角度换算成腿部坐标补偿量。加速度计读取函数用 I2C 直接读取原始值即可uint8_t mpu60_read_angle(float *roll, float *pitch) { uint8_t buf[6]; if (HAL_I2C_Mem_Read(hi2c1, 0x68 1, 0x3B, 1, buf, 6, 100) ! HAL_OK) { return 1; } int16_t ax (int16_t)((buf[0] 8) | buf[1]); int16_t ay (int16_t)((buf[2] 8) | buf[3]); int16_t az (int16_t)((buf[4] 8) | buf[5]); *roll atan2f((float)ay, (float)az); *pitch atan2f(-(float)ax, sqrtf((float)ay * ay (float)az * az)); return 0; }注意 MPU6050 需要先配置电源管理寄存器把器件从睡眠模式唤醒并设置加速度计量程。姿态解算在六足上不必用完整的四元数卡尔曼滤波atan2近似就够用——因为机身运动速度远低于碰撞冲击频率加速度计的低频噪声可以通过简单低通处理。拿到横滚角后把它乘以腿部到机身中心的 Y 向距离叠加到2.3节的机体坐标变换里就能让足端在倾斜地面上自动调整落点实现姿态保持。采样频率可放到 100Hz 左右与控制周期错开避免 I2C 阻塞步态时序。5. 调参顺序与三处最容易被忽略的坑5.1 先标定舵机零位机械装配与代码零点对齐六足机器人最常见的第一个故障是单腿抬起时其余腿乱抽根源多半是零位没对齐。舵机出厂时零位通常在中位脉冲 1500us但机械臂装配角度各不相同同一块舵机安装在不同腿上时其“水平伸展”位置对应的脉冲宽度可能相差几十微秒。正式调步态前先逐个通道发送 1500us 脉冲用目测或量角器把每条腿的三个关节都掰到机械参考位置然后记录每个舵机的零位偏差写进一个数组float pwm_offset[LEG_COUNT][3] { { 1480, 1520, 1495 }, { 1505, 1490, 1510 }, /* 每行对应一条腿的髋、股、胫零位偏移 */ };这个数组在输出 PWM 前与逆解角度叠加。零位偏差与步幅、步态参数无关优先调好它后续所有角度换算才有意义。5.2 用串口打印与逻辑分析仪验证 18 路 PWM 的实时性代码跑起来后靠眼睛判断步态是否正常很困难尤其是机身只有轻微抖动时。我通常分两步做验证先从工程里导出实际发送的 18 路脉冲宽度通过串口以 115200 bps 周期打印观察相邻控制周期内角度跳变是否超过预设阈值再用逻辑分析仪同时抓两路 PWM确认周期稳定在 20.00ms 而不是 19.5ms 这种漂移值。如果发现周期不稳原因基本集中在三处PWM 定时器的预分频和自动重装载没按 1MHz/50Hz 组合配置中断服务函数里做了耗时的浮点运算导致主循环卡顿舵机刷新频率与步态控制周期没有间隔对齐。将步态计算放在定时器中断里、PWM 脉宽更新放在主循环中是最稳妥的分工——前者保证相位推进的确定性后者避免在中断里调用HAL_TIM_PWM_Start这类重函数。5.3 给运动学加限幅防止意外指令损坏舵机与结构逆解函数返回负数时只是在说“算不出来”实际舵机并不会因此停在原处。真正危险的场景是目标点刚好在可达域边缘逆解得出的角度接近 180 度而 PWM 脉宽映射到舵机硬件极限外此时舵机堵转发热齿轮组会直接崩坏。最后的保护措施是角度限幅void clamp_angle(float *angle, float min, float max) { if (*angle min) *angle min; if (*angle max) *angle max; }把每个关节的允许角度范围做成数组在leg_ik返回成功后立即调用。限幅值取机械行程的 80%留下余量给运动中的惯性过冲。验证限幅是否生效的做法是让步行步幅参数从 10mm 逐步增大到 50mm同时串口打印每次被限制到的关节索引和限制次数如果限制次数随步幅线性增长说明机身结构或足端轨迹设计需要调整而不是简单调低限幅范围。本文还有配套的精品资源点击获取
