基于STM32的仿生六足机器人舵机控制系统设计与实现

简介：这份基于STM32的仿生六足机器人控制系统设计文档，面向嵌入式系统学习者、机器人竞赛参赛者及电子相关专业学生，旨在帮助读者掌握足式机器人控制系统的完整设计思路。文档以STM32F103VCT6为主控芯片，系统讲解了六足机器人行进步态原理、定时器输出PWM驱动舵机的方法，并详细阐述蓝牙串口通讯实现手控行走、超声波模块实现自动避障、OV7670摄像头实现拍照等核心功能的软硬件实现方案。压缩包内为1个docx文件，大小约1.61MB，内容涵盖绪论、主要原理、硬件选型、控制系统设计、总结与展望等完整章节，附有电路原理图、程序框图和关键代码，便于对照学习。已有259人学习下载，可作为课程设计、毕业设计或机器人入门项目的系统性参考资料，帮助读者从硬件选型到代码实现快速搭建并调试自己的六足机器人控制系统。

1. 仿生六足机器人控制系统的设计边界与整体思路

六足机器人和四轮小车最大的区别在于：运动不依赖连续旋转的轮子，而依赖多关节的离散协同。每条腿至少 3 个自由度，6 条腿就是 18 路舵机或电机，这直接决定了控制系统的核心矛盾不是算力，而是时基同步与 PWM 资源分配。STM32 在这个场景里之所以常用，是因为它具备多个高级定时器、DMA 以及可抢占的中断优先级，能在 5 到 10 毫秒的控制周期内完成整机 18 通道的角度刷新。控制系统设计的本质工作是三件事：把足端目标坐标转化为关节角度，把角度按时序写入伺服执行器，再把机体姿态和步态参数通过有限状态机组织成稳定的行走动作。本文按几何建模、控制分层、步态调度、调试验收的顺序展开，涉及的都是 STM32 项目里最常见的 GPIO、定时器、串口和 I²C 资源，适合正在做毕业设计或打算把机械腿真正跑起来的开发者参考。

2. 基于舵机驱动的腿结构建模与关节角度解析

2.1 三自由度腿的坐标系与 D-H 参数简化

绝大多数仿生六足机器人采用每条腿 3 个旋转关节的构型，从机体向外依次是髋关节横滚、髋关节俯仰和膝关节俯仰。前一个关节负责抬腿时向侧面打开角度，后两个关节在同一个平面内控制大腿和小腿的伸展。为减少正解反解的计算量，通常把每条腿放在一个不可变的局部坐标系里，坐标原点设在髋关节横滚轴线上，x 轴指向机器人前进方向，z 轴垂直向上，y 轴沿机体横向。此时足端位置可以用三元组 (x, y, z) 表示。

建模时建议忽略舵机安装倾角差异，假设膝关节、髋关节俯仰轴与 z 轴在同一平面内。经过这样简化，髋关节横滚角度只由足端横向位移和前后位移决定，大腿与小腿的角度则完全落在经过髋关节的纵向平面内。设计阶段只需要测量大腿长度 L1、小腿长度 L2 和髋关节到膝关节的水平偏距 L0 三个结构参数，并存入一个结构体里备用。

typedef struct { float L0; // 髋关节横滚轴到膝关节的水平偏距 float L1; // 大腿长度 float L2; // 小腿长度 float x_offset; // 足端目标在局部坐标系的x分量 float y_offset; // y分量 float z_offset; // z分量 } LegGeometry;

三个长度参数是逆运动学计算的输入前提。若 L0 为 0，即髋关节的横滚轴和俯仰轴承重合，公式可进一步简化，但机械上往往做不到完全同轴，保留 L0 会让计算结果更贴近实际舵机安装位置。在配置不同关节舵机时，注意大腿舵机与小腿舵机承受的扭矩有显著差异，L1 与 L2 的比例会影响静态力矩分布，通常取 L1 与 L2 接近可降低膝关节峰值负载。

2.2 足端坐标到关节角的几何反解

逆运动学反解的目标是根据足端位置求出三个关节角。常见做法是先把它拆成两个子问题。第一个子问题求解髋关节横滚角，第二个子问题在由大腿和小腿构成的平面内用余弦定理求解两个俯仰角。

设足端期望坐标为 (x, y, z)，髋转角的表达式为：

theta_hip = atan2f(y, x);

这里使用的是带有象限判断的反正切。atan2f 比 atan 在第二、第三象限时更安全，STM32 的 FPU 在单精度浮点下执行这个函数耗时在微秒级，完全跑得动。得到髋关节角后，把足端坐标转换到髋关节旋转之后形成的平面内，该平面上髋关节到足端的水平距离为 sqrt(xx + yy)，垂直距离仍为 z。此时该平面内的几何关系变成一段双连杆臂，直接用余弦定理求膝关节角。

float r = sqrtf(x*x + y*y); float d = sqrtf(r*r + z*z); if (d > (L1 + L2) || d < fabsf(L1 - L2)) { // 超出工作空间，不做舵机输出，保留上一帧角度 return; } float cos_knee = (L1*L1 + L2*L2 - d*d) / (2.0f * L1 * L2); float theta_knee = PI - acosf(cos_knee);

d 与 L1、L2 构成的三角形决定了膝关节角。这里限制 d 必须落在可达空间内，否则 acosf 的参数会超出 [-1, 1] 域，产生 NaN，进而污染后续 PWM 输出。大腿俯仰角的计算需要引入髋关节到足端的连线与水平面夹角，再用该夹角减去大腿边与连线构成的夹角：

float alpha = atan2f(z, r); float beta = acosf((L1*L1 + d*d - L2*L2) / (2.0f * L1 * d)); float theta_hip_pitch = alpha - beta;

beta 是连线与大腿之间的夹角，alpha 是连线与水平面的夹角，二者相减即得到大腿相对水平面的绝对角度。值得注意的是，不同设计会把髋关节零位定义为水平或垂直，实现时需要在代码中增加角度零偏修正。我的做法是把所有关节的角度统一转换成相对机械零位的度数，再叠加舵机中位偏移，最后写入 PWM 比较寄存器。

2.3 单腿逆解验证方法

在整机装配前，建议先在桌面环境下用 STM32 的串口终端输入一组足端坐标，观察三个关节角输出是否连续。常用做法是在主循环中读取 USART 接收到的三个浮点数值，调用逆解函数，并把结果通过串口打印出来。这样可以避免舵机未接线时用逻辑分析仪逐个通道检查。

验证数据可以参考以下表格：

坐标值单位是毫米，舵机角度单位是度。这里只看趋势是否正确，具体数值需要依据 L0、L1、L2 实测值重新计算。逆解结果连续即认为几何建模无误，接下来才能进入 PWM 与运动控制的联调。

3. 基于 STM32 定时器与 PCA9685 的伺服输出通道设计

3.1 18 路舵机 PWM 输出的资源分配方案

STM32 的定时器多，但单个定时器的比较通道是有限的，比如 TIM1 和 TIM8 各有 4 路捕获比较通道，TIM2 到 TIM5 各有 4 路。要让 18 路舵机同时工作，有三种常见做法。第一种是用多个定时器组合，例如让 TIM1、TIM2、TIM3、TIM4、TIM5 各自输出 4 路，再复用一个通道凑数，这种方案 GPIO 占用多，且中断同步逻辑复杂。第二种是用定时器中断配合 DMA，按预定时刻翻转电平，适合对脉冲时序要求较低的场合。第三种是外接 I²C 接口的 PCA9685 芯片，一颗芯片输出 16 路 PWM，用两颗就能覆盖 18 路，且频率由芯片内部振荡器控制，与 MCU 主频无关。

实际项目里更稳妥的是第三种方案的变体：STM32 负责逆解计算和步态规划，PCA9685 专任脉搏发生器，两者之间只用 SCL、SDA 两根线。把 18 路舵机的脉冲刷新率固定为 50 Hz，对应周期 20 ms，脉宽区间通常是 500 µs 到 2500 µs，对应舵机 0° 到 180°。这种分工让控制周期和舵机刷新周期解耦，MCU 即使在解算中出现微小抖动，也不会直接造成舵机抖动。

3.2 用 I²C 配置 PCA9685 输出通道

PCA9685 的寄存器寻址方式非常规则，写入 MODE1 寄存器开启自动增量，再设置 PRE_SCALE 寄存器决定 PWM 频率。PRE_SCALE 的公式为 round(25 MHz / (4096 × 频率)) - 1，代入 50 Hz 得到约 103。在 STM32 侧只需要通过 HAL 库的 HAL_I2C_Mem_Write 逐一写入寄存器即可。

void pca9685_init(void) { // 复位芯片 uint8_t mode1 = 0x00; HAL_I2C_Mem_Write(&hi2c1, 0x80, 0x00, I2C_MEMADD_SIZE_8BIT, &mode1, 1, 10); // 设置50Hz，写入PRE_SCALE寄存器 uint8_t prescale = 121; // 实际约50Hz，预留校准值 HAL_I2C_Mem_Write(&hi2c1, 0x80, 0xFE, I2C_MEMADD_SIZE_8BIT, &prescale, 1, 10); // 开启自动增量，降低连续写多通道的I2C开销 mode1 = 0x20; HAL_I2C_Mem_Write(&hi2c1, 0x80, 0x00, I2C_MEMADD_SIZE_8BIT, &mode1, 1, 10); } void pca9685_set_pwm(uint8_t channel, uint16_t on, uint16_t off) { uint8_t reg = 0x06 + channel * 4; HAL_I2C_Mem_Write(&hi2c1, 0x80, reg, I2C_MEMADD_SIZE_8BIT, &on, 1, 10); HAL_I2C_Mem_Write(&hi2c1, 0x80, reg + 1, I2C_MEMADD_SIZE_8BIT, &on >> 8, 1, 10); HAL_I2C_Mem_Write(&hi2c1, 0x80, reg + 2, I2C_MEMADD_SIZE_8BIT, &off, 1, 10); HAL_I2C_Mem_Write(&hi2c1, 0x80, reg + 3, I2C_MEMADD_SIZE_8BIT, &off >> 8, 1, 10); }

PRE_SCALE 值写入 0xFE 寄存器，但必须在芯片处于 sleep 模式下进行。如果直接调用初始化后立刻去设置脉宽，偶尔会出现频率未生效的问题，因此初始化函数里还需要写入 MODE1 的 RESTART 位。此处给出的 I2C 设备地址 0x80 是 7 位地址左移一位后的写法，实际地址取决于芯片 A0 到 A5 引脚的电平状态，当全部接地时为 0x40，左移一位后就是 0x80。

3.3 舵机脉宽到计数值的线性映射

舵机角度与脉宽之间近似线性关系。写入 PCA9685 的 off 计数值直接决定高电平持续时间，一个周期 4096 分，50 Hz 周期 20 ms，所以 1 µs 对应约 0.2048 个计数。实际工程中建议先做舵机脉宽标定，不直接用理论值。常见做法是用串口命令让某一路舵机依次输出 500 µs、750 µs、1000 µs 等离散脉宽，用角度尺或量角器记录真实角度，再拟合成偏移表。

typedef struct { int16_t zero_offset; // 中位偏移 float pulse_per_angle; // 该舵机每度对应的脉宽变化量 } ServoCalib; void servo_write_angle(uint8_t ch, float angle_deg, ServoCalib *calib) { float pulse_us = 1500.0f + (angle_deg + calib->zero_offset) * calib->pulse_per_angle; if (pulse_us < 600.0f) pulse_us = 600.0f; if (pulse_us > 2400.0f) pulse_us = 2400.0f; uint16_t off = (uint16_t)(pulse_us * 2.048f); pca9685_set_pwm(ch, 0, off); }

角度超过有效行程时，脉宽信号会被截断到 600 µs 到 2400 µs，这既保护舵机也避免机械限位碰撞。注意每条腿的 3 个舵机应规划在 PCA9685 的连续通道上，并且为 L0、L1、L2 方向不一致的左右腿分别维护正负号映射，这个映射关系一旦搞反，机器人会呈现出交叉步态，越调越乱。

4. 基于状态机的步态规划与 STM32 定时中断刷新

4.1 三角步态与波纹步态的时序逻辑

六足机器人的静态稳定范围大，6 条腿中有 3 着地即可形成稳定支撑三角形。三角步态是最常用的步态，把 6 条腿分成两组，一组摆动，一组支撑，半周期后交换角色。翻滚式步态则模仿动物的腿序，逐条抬起，稳定裕度更高，但速度慢。在实现时两种步态可以用同一个有限状态机，只改变腿组划分方式。

typedef enum { SWING_LEGS, // 摆动相 STANCE_LEGS, // 支撑相 } GaitPhase; typedef struct { GaitPhase phase; uint8_t swing_group; uint32_t phase_start_ms; float phase_duration_ms; float duty_cycle; // 摆动占空比 } GaitState;

在三角步态中，swing_group 为 0 时代表腿 0, 2, 4 摆动，1 时代表腿 1, 3, 5 摆动。状态机每执行一次，根据当前时间与 phase_start_ms 的差值，判断是否将 phase 翻转。支撑相与摆动相的比例通常可以调整，奔跑时摆动相缩短，慢走时摆动相相对较长。

4.2 撑地相与摆动相的足端轨迹生成

支撑相的足端轨迹相对简单：机体平移量叠加到足端目标坐标中即可。摆动相的足端需要从当前位置抬起一定高度，再按椭圆或抛物线路径落回前方。经典做法是用周期参数 t 归一化到 0 到 1，用半周期余弦函数生成抬腿高度，避免起点和终点处的速度突变。

void swing_traj(float t, float start_pos[3], float target_pos[3], float lift_height, float out[3]) { float phase = t * PI; float lift = sinf(phase); // 0 -> 1 -> 0 for (int i = 0; i < 3; i++) { out[i] = start_pos[i] + (target_pos[i] - start_pos[i]) * t; } out[2] += lift_height * lift; }

t 从 0 到 1的过程中，水平方向是线性插值，垂直方向是先抬高再回落的正弦变化。这会让足端在抬到最高点时速度水平分量最大，符合常见六足机器人的步态特点。每次步态切换时，需要从当前足端实际位置而不是理论位置出发，否则第一条腿切换时会有明显的跳变，表现为机器人突然抖一下。实际实现中需要存储当前足端坐标在结构体里，并在每个控制周期更新。

4.3 用定时器中断固定 200 Hz 控制频率

逆运动学计算与足端轨迹插值如果放在主循环里执行，主循环时间的波动会直接影响舵机刷新稳定性。因此建议在控制系统中使用一个 5 ms 的定时器中断，把运动逻辑放进中断回调，主循环只处理串口指令与状态显示。

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { if (htim == &htim3) { update_gait_state(); // 更新步态相位 for (int leg = 0; leg < 6; leg++) { compute_leg_target(leg); // 计算该腿足端目标 compute_ik_and_output(leg); // 逆解并写PWM } } }

这里把整机 6 条腿的计算都放进中断，总耗时要控制在 4 ms 以内。STM32F103 在 72 MHz 下执行 18 组 atan2f 和 acosf 大约需要 1 到 2 ms，留有余量。如果使用标准库而不是 HAL 库，回调函数的注册方式不同，但中断向量和处理逻辑相同。值得注意的是，PCA9685 的 I²C 写入本身较为耗时，若每条腿写 3 个寄存器串行执行，则整轮 18 通道写入可能达到数千微秒。缓解方案是开启芯片自动增量后，把 18 通道的 off 值连续写入同一块寄存器区，一次 I²C 事务完成所有通道刷新。

5. 串口指令协议与上下位机联调方法

5.1 自定义三个调试指令的帧格式

在整机联调阶段，必须能对单腿或单舵机进行独立控制。建议在串口协议中实现三个最基本指令：设置目标速度、设置抬腿高度、直接设置某条腿的足端坐标。帧结构采用起始字节加指令码加参数的方案，方便用串口助手或上位机脚本调用。

参数用二进制方式传输，每个浮点数解析成四个字节，避免使用文本解析带来的格式错误。接收端在串口中断里把字节流存入环形缓冲区，主循环再按状态机解析。中断回调里不能阻塞式等待下一个字节，这个在 STM32 项目中屡屡踩坑。

void parse_command_frame(uint8_t *buf, uint8_t len) { switch (buf[0]) { case 0x01: { uint16_t period = buf[1] | (buf[2] << 8); uint8_t lift = buf[3]; gaits.period_ms = period; gaits.lift_height = lift; break; } case 0x02: { uint8_t leg = buf[1]; float *coord = (float *)(&buf[2]); set_leg_target(leg, coord[0], coord[1], coord[2]); break; } default: break; } }

使用二进制格式时要注意字节序问题。Cortex-M 是小端模式，而上位机如果采用大端模式，需要先交换字节再解析为浮点数。在参数入口处统一封装修改函数，可以避免在多个调用点反复处理字节序。

5.2 万用表与逻辑分析仪判断舵机接线异常

舵机是执行环节最容易出问题的地方。接线异常时通常表现为机器人单腿无力、轻轻一碰就垂下，或者舵机抖动并伴有啸叫。先用万用表测量舵机电源电压，确保在舵机标称电压范围内，且高负载时压降不超过 0.5 V。再用逻辑分析仪截取 PCA9685 输出脚的电平波形，确认脉冲宽度是否稳定在设定值附近。

若波形正常但舵机不动作，检查信号线与舵机的连接顺序，部分舵机信号线在中间，部分在边上，三线的位置各品牌并不统一。控制板出厂时 PCA9685 的通道输出往往带短路保护电阻，舵机堵转时可能触发芯片过流保护，表现为整体无输出，复位后恢复。这类问题多是因为装配时机械连杆卡死或运动学计算越界，观察机器人双腿交叠位置一般能定位。

5.3 用上位机脚本做步态参数扫描

在 STM32 端实现步态参数接收后，可以写一个简单的桌面脚本按序发送不同参数组合，自动收集哪组参数产生横向偏移最小或行进直线度最好。相比手动每次改编译，这种方式能大幅提高调参效率。上位机和开发板之间通过 USB 转串口连接，通信波特率建议固定为 115200，在该速率下一个 20 字节以内的命令帧传输时间不足 2 ms，不会影响 200 Hz 控制周期。

import serial, struct, time ser = serial.Serial('COM30', 115200, timeout=0.1) for period_ms in [400, 600, 800]: for lift in [15, 25, 35]: frame = bytes([0x01]) + struct.pack('<H', period_ms) + bytes([lift]) ser.write(frame) time.sleep(3) # 让机器人走完该参数下的行程

从机器人运动来看，如果周期过短，机身抖动明显；抬腿高度过大则重心的上下起伏增加电机功耗。将采集到的位移和姿态数据记录到日志里，与参数做对照分析，可以更快锁定一组最优值。如果机器人行走方向跑偏，可通过调整左右腿的步幅比例或修改逆解中的 y 坐标补偿来修正，这类问题通常在机械装配阶段引入误差，脚本扫描只适合发现现象，不适合替代机械校准。

6. 样机落地前的 3 个验收技巧

整机装好、代码烧录完成后，建议按以下顺序做验收，而不是一上来就放地上跑。第一项是单腿阶梯测试，用串口指令把第 0 条腿的足端坐标从初始位置抬高 30 毫米再向前 40 毫米，观察髋关节横滚角与俯仰角是否按预期先后运动。第二项是静态承重测试，让机器人处于站立姿态，手掌缓慢按压机壳顶部，观察是否有某条腿的舵机出现抖动，如果抖动明显，多半是该腿的支撑腿相位与机体重心不匹配，需调整站立时的足端 z 坐标。第三项是慢速行走测试，把步态周期拉长到 1000 毫秒，先在原地走 5 个步态周期，确认腿序正确，再逐步缩短周期。

关于舵机中位偏置还有一个容易被忽略的技巧：不要依赖代码里统一角度偏移，误装了舵机臂后，角度偏移会整体偏转一个固定值。最简单的方法是在舵机臂尚未固定时，让该通道输出 90° 对应脉宽，手动把舵机臂垂直安装，再固定螺丝，以此消除大部分机械安装误差。剩余的小角度偏差再通过程序中的 per-channel 偏移表修正，这样可以把标定周期压缩到最少。

趁机器人未落地时，用塑料扎带固定好 PCA9685 与 STM32 控制板的排线，避免运动过程中排线被转动关节拉扯导致接触不良。一旦出现舵机偶发抖动，排查顺序是供电电压、接线端子、信号线长短，不要盲目修改控制代码。

本文还有配套的精品资源，点击获取