基于STM32与FreeRTOS的六自由度机械臂：总线舵机与PS2手柄控制实战

最近在做一个基于STM32的六自由度机械臂项目，从选型、硬件搭建到软件调试，整个过程踩了不少坑，也积累了不少实战经验。这个项目最大的特点是将FreeRTOS实时操作系统、总线舵机（如UART TTL总线舵机）和PS2手柄控制整合在一起，实现了一个响应灵敏、控制灵活的小型机械臂系统。无论是用于学习嵌入式实时系统、机器人运动学，还是作为毕业设计或科创项目，都具有很高的参考价值。

本文将完整分享该项目的实现过程，从硬件选型、FreeRTOS任务设计、总线舵机驱动、PS2手柄解码到运动控制算法，提供全部核心代码和配置。即使你是嵌入式新手，跟着步骤也能一步步搭建起来；对于有经验的开发者，文中的多任务架构设计、通信协议处理和运动解算思路也值得借鉴。

1. 项目背景与核心概念

1.1 什么是六自由度机械臂

六自由度（6-DOF）机械臂，指的是具有六个独立旋转关节的机械臂。这六个关节通常模仿人类手臂的结构：基座旋转、肩部俯仰、肘部俯仰、腕部旋转、腕部俯仰和末端夹持器旋转。六个自由度意味着其末端执行器可以在三维空间内到达任意位置和姿态（位置XYZ和姿态RPY），具备很强的灵活性，广泛应用于工业分拣、教育科研、桌面级机器人等领域。

在嵌入式平台上实现这样一个机械臂，核心挑战在于如何同时、协调地控制六个关节电机，并响应外部的实时控制指令。如果使用传统的裸机编程（前后台系统），随着逻辑复杂化，程序会变得难以维护，实时性也难以保证。因此，引入实时操作系统（RTOS）是更优的选择。

1.2 为什么选择FreeRTOS + 总线舵机 + PS2手柄

• FreeRTOS：一个开源、轻量级、可裁剪的实时操作系统内核。它提供了任务（线程）管理、消息队列、信号量、软件定时器等核心机制。在本项目中，我们可以为舵机控制、手柄数据解析、运动规划等不同功能创建独立的任务，让它们并行运行，并通过任务间通信（IPC）协调工作。这大大提高了代码的模块化程度和系统的实时响应能力。
• 总线舵机：与传统PWM舵机不同，总线舵机（如基于UART TTL的舵机）通过串行总线（如单线半双工）进行通信。所有舵机并联在同一总线上，每个舵机有唯一ID。控制器通过发送数据包（包含ID、指令）来同时控制多个舵机。其优点非常明显：
  • 节省IO口：控制多个舵机仅需1个UART TX引脚。
  • 可读取状态：可以回读舵机的角度、温度、电压等信息。
  • 位置、速度、扭矩可控：指令更丰富，控制更精准。
• PS2手柄：作为输入设备，PS2手柄价格低廉、易于获取，且提供了双摇杆、多个按键和扳机键，非常适合用来直观地控制机械臂各个关节的运动。我们需要通过SPI或模拟时序的方式读取手柄数据，并将其转换为对机械臂的控制指令。

技术栈总结：STM32作为主控大脑，运行FreeRTOS来调度任务；通过一个UART控制所有总线舵机协同运动；通过SPI接口读取PS2手柄的操控数据。三者结合，构建了一个软硬件解耦、实时性强、交互直观的机械臂控制系统。

2. 硬件准备与开发环境搭建

2.1 硬件清单

2.2 开发环境与软件准备

1. IDE：Keil uVision 5 (MDK-ARM)。也可以使用STM32CubeIDE或VSCode+EIDE插件，本文以Keil为例。
2. STM32固件库：HAL库或标准外设库。推荐使用STM32CubeMX配合HAL库，可以快速生成FreeRTOS工程框架，极大简化移植。
3. STM32CubeMX：用于图形化配置STM32引脚、时钟、外设以及集成FreeRTOS。
4. 串口调试助手：如XCOM、SSCOM，用于调试舵机通信。
5. 逻辑分析仪或示波器（可选）：用于分析PS2手柄通信时序，调试阶段非常有用。

2.3 工程创建与基础配置（使用STM32CubeMX）

这是最关键的一步，正确配置可以避免大量底层驱动问题。

1. 新建工程：打开CubeMX，选择你的MCU型号（如STM32F103C8Tx）。
2. 系统核心（SYS）配置：
   • Debug: 根据你的调试器选择，例如Serial Wire（对应ST-Link）。
3. 时钟（RCC）配置：
   • High Speed Clock (HSE): 选择Crystal/Ceramic Resonator（如果你的核心板有外部晶振）。
   • 在Clock Configuration标签页，将系统时钟（SYSCLK）配置到MCU的最高稳定频率（如STM32F103C8T6为72MHz）。
4. FreeRTOS配置：
   • 在Middleware分类下，找到FREERTOS，选择Interface为CMSIS_V2（更通用）。
   • 在Tasks and Queues标签页，我们可以先创建两个任务：
     • Task_PS2: 优先级设为High，用于处理手柄输入。
     • Task_ArmCtrl: 优先级设为Normal，用于机械臂运动控制。
   • 在Config parameters标签页，可以调整堆栈大小、时钟频率等。默认配置通常够用。
5. 外设引脚配置：
   • 舵机总线UART：假设使用USART1。找到USART1，模式选择Asynchronous（异步）。将PA9配置为USART1_TX，PA10配置为USART1_RX。波特率根据舵机手册设置，常见为115200。
   • PS2手柄SPI：假设使用SPI1。找到SPI1，模式选择Full-Duplex Master。引脚通常为PA5(SCK),PA6(MISO),PA7(MOSI)。另外需要一个GPIO作为片选CS，例如PA4，配置为GPIO_Output。
   • 调试串口UART：建议再配置一个USART（如USART2）连接电脑，用于打印调试信息，波特率115200。
6. 生成工程：
   • 在Project Manager标签页，设置项目名称、路径，选择MDK-ARM作为Toolchain。
   • 点击GENERATE CODE，生成Keil工程。

3. 核心模块驱动与原理

3.1 总线舵机通信协议解析

以常见的LX-16A舵机为例，其通信协议如下：

• 帧结构：[0x55 0x55] [ID] [Length] [Cmd] [Data...] [Checksum]

  • 0x55 0x55: 帧头。
  • ID: 舵机ID（1-253），广播ID为0xFE。
  • Length: 数据长度（从Cmd到Checksum的字节数）。
  • Cmd: 指令，如0x01（位置控制），0x02（读取位置）。
  • Data: 指令参数。
  • Checksum: 校验和，计算方式为~(ID + Length + Cmd + Data1 + ... + DataN)。

• 核心指令：

  • 位置控制：Cmd=0x01，Data为2字节的目标位置（0-1000对应0-240度）。
  • 位置读取：Cmd=0x02，舵机会返回其当前位置。

我们需要编写一个舵机驱动层，封装数据包的构建和发送。

// servo_driver.h #ifndef __SERVO_DRIVER_H #define __SERVO_DRIVER_H #include "main.h" #include "usart.h" #define SERVO_HEADER_0 0x55 #define SERVO_HEADER_1 0x55 #define CMD_SERVO_MOVE 0x01 #define CMD_SERVO_READ 0x02 #define BROADCAST_ID 0xFE typedef struct { UART_HandleTypeDef *huart; uint8_t tx_buffer[16]; uint8_t rx_buffer[16]; } ServoBus_t; void ServoBus_Init(ServoBus_t *bus, UART_HandleTypeDef *huart); HAL_StatusTypeDef Servo_SetPosition(ServoBus_t *bus, uint8_t id, uint16_t position, uint16_t time); HAL_StatusTypeDef Servo_ReadPosition(ServoBus_t *bus, uint8_t id, uint16_t *position); #endif

// servo_driver.c #include "servo_driver.h" ServoBus_t g_servo_bus; void ServoBus_Init(ServoBus_t *bus, UART_HandleTypeDef *huart) { bus->huart = huart; // 可以初始化DMA或IT接收，这里用简单的轮询发送 } static uint8_t _calc_checksum(uint8_t *buf, uint8_t len) { uint8_t sum = 0; for(uint8_t i=0; i<len; i++) { sum += buf[i]; } return ~sum; } HAL_StatusTypeDef Servo_SetPosition(ServoBus_t *bus, uint8_t id, uint16_t position, uint16_t time) { uint8_t *tx = bus->tx_buffer; tx[0] = SERVO_HEADER_0; tx[1] = SERVO_HEADER_1; tx[2] = id; tx[3] = 7; // Length: Cmd(1) + Data(4) + Checksum(1) = 6? 注意：Length字段包含自身之后的所有字节数，这里应该是7。 tx[4] = CMD_SERVO_MOVE; tx[5] = (uint8_t)(position & 0xFF); // 位置低字节 tx[6] = (uint8_t)(position >> 8); // 位置高字节 tx[7] = (uint8_t)(time & 0xFF); // 时间低字节 tx[8] = (uint8_t)(time >> 8); // 时间高字节 // 校验和计算：ID + Length + Cmd + Data[0-3] tx[9] = _calc_checksum(&tx[2], 7); // 从ID到时间高字节共7个字节 return HAL_UART_Transmit(bus->huart, tx, 10, 100); // 发送10字节 } // 读取位置函数类似，需要处理接收

3.2 PS2手柄通信与解码

PS2手柄采用SPI-like协议。主机（STM32）需要先拉低CS片选，然后发送一个命令字节0x01，随后主机在发送0x42（请求数据）的同时，会接收到手柄返回的数据。数据包通常为9字节，包含了所有按键和摇杆的状态。

// ps2_controller.h #ifndef __PS2_CONTROLLER_H #define __PS2_CONTROLLER_H #include "main.h" #include "spi.h" typedef struct { SPI_HandleTypeDef *hspi; GPIO_TypeDef *cs_port; uint16_t cs_pin; uint8_t rx_data[9]; uint8_t tx_data[9]; } PS2_Handle_t; typedef struct { uint16_t buttons; // 用位域表示按键状态 uint8_t joy_rx; // 右摇杆X uint8_t joy_ry; // 右摇杆Y uint8_t joy_lx; // 左摇杆X uint8_t joy_ly; // 左摇杆Y } PS2_Data_t; void PS2_Init(PS2_Handle_t *hps2, SPI_HandleTypeDef *hspi, GPIO_TypeDef *cs_port, uint16_t cs_pin); uint8_t PS2_Read(PS2_Handle_t *hps2, PS2_Data_t *data); #endif

// ps2_controller.c #include "ps2_controller.h" void PS2_Init(PS2_Handle_t *hps2, SPI_HandleTypeDef *hspi, GPIO_TypeDef *cs_port, uint16_t cs_pin) { hps2->hspi = hspi; hps2->cs_port = cs_port; hps2->cs_pin = cs_pin; HAL_GPIO_WritePin(cs_port, cs_pin, GPIO_PIN_SET); // CS高电平，不选中 } uint8_t PS2_Read(PS2_Handle_t *hps2, PS2_Data_t *data) { HAL_GPIO_WritePin(hps2->cs_port, hps2->cs_pin, GPIO_PIN_RESET); HAL_Delay(1); // 短暂延时 // 发送0x01启动通信，同时接收第一个字节（通常是0x73或0x79，表示手柄模式） hps2->tx_data[0] = 0x01; HAL_SPI_TransmitReceive(hps2->hspi, hps2->tx_data, hps2->rx_data, 9, 100); HAL_GPIO_WritePin(hps2->cs_port, hps2->cs_pin, GPIO_PIN_SET); // 解码rx_data[1]~rx_data[8]到PS2_Data_t结构体 // 这里简化处理，实际需要根据协议解析 >// freertos.c 中 MX_FREERTOS_Init 函数内 /* 定义控制指令结构体 */ typedef struct { uint8_t joint_id; // 关节ID: 0~5 int8_t direction; // 方向: +1 增加角度, -1 减小角度, 0 停止 uint8_t button_cmd; // 按钮命令，如夹爪开合 } Arm_Ctrl_Cmd_t; /* 创建消息队列 */ QueueHandle_t xArmCtrlQueue; xArmCtrlQueue = xQueueCreate(10, sizeof(Arm_Ctrl_Cmd_t));

4. 完整项目实战：代码整合与运动控制

4.1 项目文件结构

在Keil工程中，组织文件如下：

/Project /Core /Src main.c stm32f1xx_it.c ... /Inc main.h ... /Drivers /Middlewares/FreeRTOS /UserApp /Src servo_driver.c ps2_controller.c arm_kinematics.c (可选，运动学解算) arm_ctrl_task.c ps2_task.c /Inc servo_driver.h ps2_controller.h arm_kinematics.h arm_ctrl_task.h ps2_task.h /MDK-ARM

4.2 任务函数实现

PS2任务 (ps2_task.c)：

#include "ps2_task.h" #include "ps2_controller.h" #include "FreeRTOS.h" #include "task.h" #include "queue.h" extern QueueHandle_t xArmCtrlQueue; PS2_Handle_t myPS2; PS2_Data_t ps2_data; void Task_PS2(void *argument) { // 初始化PS2手柄 PS2_Init(&myPS2, &hspi1, GPIOA, GPIO_PIN_4); Arm_Ctrl_Cmd_t cmd = {0}; TickType_t xLastWakeTime = xTaskGetTickCount(); const TickType_t xFrequency = pdMS_TO_TICKS(10); // 10ms周期 for(;;) { if(PS2_Read(&myPS2, &ps2_data)) { // 解析左摇杆上下控制关节1（基座） if(ps2_data.joy_ly < 100) { // 摇杆向上 cmd.joint_id = 0; cmd.direction = 1; xQueueSend(xArmCtrlQueue, &cmd, 0); } else if(ps2_data.joy_ly > 150) { // 摇杆向下 cmd.joint_id = 0; cmd.direction = -1; xQueueSend(xArmCtrlQueue, &cmd, 0); } // 解析右摇杆左右控制关节2（大臂） // ... 类似逻辑 // 解析按键控制夹爪 if(ps2_data.buttons & (1 << 0)) { // 假设按键0是夹爪闭合 cmd.joint_id = 5; // 夹爪关节ID cmd.direction = 1; xQueueSend(xArmCtrlQueue, &cmd, 0); } if(ps2_data.buttons & (1 << 1)) { // 按键1是夹爪打开 cmd.joint_id = 5; cmd.direction = -1; xQueueSend(xArmCtrlQueue, &cmd, 0); } } vTaskDelayUntil(&xLastWakeTime, xFrequency); // 精确周期延迟 } }

机械臂控制任务 (arm_ctrl_task.c)：

#include "arm_ctrl_task.h" #include "servo_driver.h" #include "FreeRTOS.h" #include "task.h" #include "queue.h" extern QueueHandle_t xArmCtrlQueue; ServoBus_t g_servo_bus; // 每个关节的当前目标位置 (0-1000) uint16_t joint_target_pos[6] = {500, 500, 500, 500, 500, 500}; // 关节运动范围限制 const uint16_t joint_min[6] = {0, 100, 100, 0, 0, 0}; const uint16_t joint_max[6] = {1000, 900, 900, 1000, 1000, 1000}; void Task_ArmCtrl(void *argument) { ServoBus_Init(&g_servo_bus, &huart1); Arm_Ctrl_Cmd_t cmd; const uint16_t step = 5; // 单次步进值 for(;;) { // 从队列接收控制命令，非阻塞方式 if(xQueueReceive(xArmCtrlQueue, &cmd, pdMS_TO_TICKS(20)) == pdTRUE) { if(cmd.joint_id < 6) { int32_t new_pos = (int32_t)joint_target_pos[cmd.joint_id] + cmd.direction * step; // 位置限幅 if(new_pos < joint_min[cmd.joint_id]) new_pos = joint_min[cmd.joint_id]; if(new_pos > joint_max[cmd.joint_id]) new_pos = joint_max[cmd.joint_id]; joint_target_pos[cmd.joint_id] = (uint16_t)new_pos; } } // 周期性地将目标位置发送给所有舵机（可以加入同步移动指令） for(uint8_t i=0; i<6; i++) { Servo_SetPosition(&g_servo_bus, i+1, joint_target_pos[i], 20); // ID从1开始，时间20ms } vTaskDelay(pdMS_TO_TICKS(50)); // 控制周期50ms } }

4.3 主函数初始化

在main.c中，确保外设初始化和FreeRTOS启动。

int main(void) { HAL_Init(); SystemClock_Config(); MX_GPIO_Init(); MX_USART1_UART_Init(); // 舵机UART MX_USART2_UART_Init(); // 调试串口 MX_SPI1_Init(); // PS2 SPI MX_FREERTOS_Init(); // FreeRTOS初始化（创建任务和队列） printf("System Start!\r\n"); // 重定向printf到USART2 osKernelStart(); // 启动FreeRTOS调度器 while (1) { // 不会执行到这里 } }

4.4 编译、下载与调试

1. 在Keil中编译工程，确保无错误。
2. 使用ST-Link连接核心板，下载程序。
3. 硬件连接：
   • STM32 USART1_TX -> 舵机总线信号线（可能需要电平转换）。
   • STM32 SPI1引脚 -> PS2接收器对应引脚（注意电源5V）。
   • 舵机和PS2接收器共地，并连接独立5V/6A电源。
4. 上电后，打开串口调试助手，观察启动信息。
5. 操作PS2手柄，观察机械臂运动。

5. 常见问题与排查思路

6. 进阶优化与最佳实践

6.1 运动平滑与轨迹规划

上面的示例是直接给舵机发送目标位置，舵机会以最快速度运动，导致抖动。可以通过线性插值实现平滑运动。

// 在arm_ctrl_task.c中 uint16_t joint_current_pos[6] = {500, 500, 500, 500, 500, 500}; void Task_ArmCtrl(void *argument) { // ... 初始化 for(;;) { // ... 接收队列命令，更新joint_target_pos // 平滑移动：每次将current_pos向target_pos靠近一步 for(uint8_t i=0; i<6; i++) { if(joint_current_pos[i] < joint_target_pos[i]) { joint_current_pos[i] += 1; } else if(joint_current_pos[i] > joint_target_pos[i]) { joint_current_pos[i] -= 1; } Servo_SetPosition(&g_servo_bus, i+1, joint_current_pos[i], 0); // 时间为0，使用舵机内部速度控制 } vTaskDelay(pdMS_TO_TICKS(20)); // 更短的控制周期，如20ms } }

6.2 增加运动学解算（逆运动学）

要实现末端夹爪移动到空间某一点(X,Y,Z)，需要计算每个关节的角度，即逆运动学。对于6自由度机械臂，计算较为复杂，但可以通过几何法（针对特定构型）或数值法（如雅可比矩阵迭代）实现。这将使控制从“关节空间”提升到“笛卡尔空间”，实现更直观的控制（例如，用手柄摇杆直接控制夹爪在空间中的移动）。

6.3 使用DMA进行UART通信

频繁使用HAL_UART_Transmit会阻塞任务。可以改为UART DMA传输，解放CPU。在CubeMX中为USART1的TX启用DMA，并修改舵机驱动函数使用HAL_UART_Transmit_DMA。

6.4 增加安全保护

• 软件限位：如上文代码所示，必须为每个关节设置软件角度限位，防止机械结构损坏。
• 异常检测：在任务中定期读取舵机温度、电压，超过阈值则进入安全状态（停止发送运动指令）。
• 看门狗：启用STM32的独立看门狗(IWDG)，防止程序跑飞。

6.5 代码模块化与可配置性

• 将舵机参数（ID、限位、偏移）定义为结构体数组，存放在单独的配置头文件中。
• 将PS2手柄的按键映射（哪个按键对应哪个功能）做成可配置的。
• 使用状态机来管理机械臂的工作模式（如手动模式、自动演示模式、回零模式）。

这个项目融合了嵌入式系统、实时操作系统、通信协议和机器人控制等多个知识点。从调试第一个舵机转动，到用手柄流畅操控整个机械臂完成抓取动作，整个过程充满挑战也极具成就感。希望这份详细的教程能为你提供一个坚实的起点。你可以在此基础上，尝试添加更多传感器（如摄像头进行视觉抓取）、实现更复杂的轨迹规划、或者移植到性能更强的STM32平台。