《从零到一：基于定制 FOC 的双驱轮式机器人底盘全栈构建指南》

导言：告别黑盒，拥抱纯粹的底层控制

做机器人底盘，最怕的就是底层硬件不听话。通过采购通用双驱无刷 FOC 控制板和 36V 轮毂电机，结合底层驱动参考方案，我们直接跳过了痛苦的“硬件逆向”阶段。

这套方案的绝对优势：

• 即插即用： 硬件引脚在底层固件中已完美定义，无需跑线。
• 物理白盒： 真正的 FOC 矢量控制，提供纯粹的速度/力矩闭环响应，0 延迟。
• 超大杯动力（10寸直驱）： 相比普通的 6.5 寸电机，10 寸 350W 轮毂电机拥有更大的轮胎外径，这意味着在相同转速下，它的直线行驶极速更高、越野通过性更强；同时得益于直驱外转子设计，它依然保持着惊人的低速大扭矩，配合 36V 动力，载重百斤轻松自如，是目前具备极高性价比的强力 AGV 底盘方案。

---

第一阶段：硬件集结（钢铁之躯拼装）

手撕黑盒的第一步，是把这堆散件变成一辆真正的“车”。

1. 机械组装

• 动力轮安装： 将两个 350W 轮毂电机固定在准备好的车架钢板两侧。注意确保两个电机的轴心在同一条水平线上，否则车子走直线会跑偏。
• 万向轮就位： 在车架前后（或单前/单后）安装万向轮，形成稳定的三点或四点支撑结构。
• 主板固定： 将通用双驱无刷 FOC 控制板固定在钢板中央，注意主板底部要做好绝缘（垫上绝缘柱），防止引脚短路到钢板上。

2. 神经与血管接驳

• 相线连接： 将左右电机的粗线（黄、绿、蓝）分别接入主板两侧的电机接线柱。
• 霍尔线连接： 将电机的 5 芯霍尔线（红、黑、黄、绿、蓝）插入主板对应的白色端子中。
• 电源准备： 准备一块 36V 动力锂电池（满电 42V），接入主板的 XT60 接口（注意：此时先不要通电！）。

---

第二阶段：灵魂重铸（固件烧录与环境搭建）

通用控制板出厂通常带有其他应用固件，我们需要用 ST-Link 将其“洗脑”。

1. 武器库准备

• 硬件： ST-Link V2 下载器、杜邦线。
• 软件： 安装 STM32CubeProgrammer 和 Visual Studio Code（并安装 PlatformIO IDE 插件）。
• 源码获取： 准备好基于底层驱动参考方案定制的 FOC 固件源码。

2. 破壁解锁（擦除原厂固件）

1. 在主板上找到 4 针 SWD 调试接口（通常丝印为 GND, 3.3V, SWDIO, SWCLK）。
2. 将 ST-Link 对应连接到主板。（警告：全程断开 36V 电池，仅靠 ST-Link 的 3.3V 供电即可）
3. 打开 STM32CubeProgrammer，点击 Connect。
4. 如果提示芯片被锁（Read Out Protection），进入 Option Bytes 菜单，将 RDP 改为 Level 0 (AA)，点击 Apply。
5. 芯片将被瞬间清空，主板进入“白纸”状态。

---

第三阶段：FOC 引擎调校（参数配置）

打开 VS Code，进入 PlatformIO 工程，我们需要针对 36V 系统和机器人需求进行定制。

1. 锁定通信变体

在 platformio.ini 文件中，找到 [platformio] 项，修改为串口控制模式：

default_envs = VARIANT_USART

这告诉编译器，我们将通过主板侧边的通用排线口使用 UART 接收外部指令。

2. 核心参数标定 (Inc/config.h)

这是最关键的一步，必须与您的硬件物理特性匹配：

• 控制模式： 机器人底盘最稳妥的选择是速度闭环。

  #define CTRL_MOD_REQ SPD_MODE

• 电池电压适配（极其重要）： 您的电机是 36V 系统（10串锂电池），必须修改电压阈值，防止低压误报或过压炸管。

  #define BAT_CELLS 10 // 36V 电池通常是 10 串
  #define BAT_LVL1_ENABLE 1 // 开启低压报警
  #define BAT_LVL2_ENABLE 1 // 开启低压断电保护

• 电机参数匹配（10寸专属说明）： 底层固件默认的 PID 参数是基于 6.5 寸电机调校的。您的 10寸电机 内部极对数（Pole Pairs）大概率也是 15 对，可以直接跑起来。但因为轮胎变大、转动惯量变大，如果启动或低速时电机出现轻微抖动/异响，后续可能需要在 BLDC_controller_data.c 中稍微微调一下电流环或速度环的 PID 参数。

编译代码并点击 Upload 烧录至主板。

---

第四阶段：上位机接管（Teensy 4.1 点火测试）

现在，主板已经是一台等待串口指令的纯粹伺服驱动器了。我们将使用性能强悍的 Teensy 4.1 作为大脑来接管它。

1. 硬件通信连接

在主板边缘找到用于通讯的 4 芯扩展排线插座（引脚定义通常为 GND, 5V/15V, TX, RX）。

• 将您的 Teensy 4.1 与主板进行连接：Teensy 的 TX 接主板 RX，Teensy 的 RX 接主板 TX，GND 接 GND。

• 极其危险的避坑： 绝对不要盲猜电源脚！主板排线里通常有一根是 15V 高压供电。Teensy 4.1 的 IO 口最高只能承受 3.3V 逻辑电平（它不耐 5V 更不耐 15V）。请务必只接 TX、RX 和 GND 这三根线，Teensy 4.1 使用单独的 USB 或降压模块供电，千万别把主板的 15V 接进 Teensy！

2. 协议下发与解析

参考底层驱动提供的串口通信示例，这套通信协议的运作逻辑如下：

• 发送指令（控制）： 每次发送一个结构体数据包。注意，数据包内不是直接发左右轮速度，而是发送 Steer (转向幅度) 和 Speed (总线速度/油门)，固件底层会自动进行差速解算。此外还包含 0xABCD 帧头和 XOR 校验和（Checksum）以防止数据被干扰。

  • 测试逻辑： 例如发送 Send(0, 100);，底盘就会直线前进；发送 Send(50, 0);，底盘就会原地打转。

• 接收反馈（状态监测）： Teensy 4.1 必须在主循环中不断运行一个非阻塞的 Receive() 状态机函数。它会通过寻找 0xABCD 包头并核对校验和，精准剥离出主板传回的实时数据，包括：左右轮真实转速、电池实时电压、主板温度。这对于您构建机器人的里程计（Odom）和电量监测至关重要。

第五阶段：跨文件解耦的现代机器人软件架构设计

摒弃把所有代码揉在一坨的“意大利面条式”写法，将 底层电机运动学/控制逻辑 与 上层 ROS 2 (micro-ROS) 通信组件 完全解耦。这样一来，main.cpp 成了纯粹的底盘控制大脑，而外围的 DDS 通信只作为数据管道存在。

Teensy 4.1 拥有 600MHz 的强劲算力和 8 个全硬件串口，咱们彻底抛弃孱弱的 SoftwareSerial，直接起飞。

📂 项目目录结构规划

├── include/
│   ├── chassis_protocol.h   # 底层通信协议数据包定义
│   └── ros2_interface.h     # ROS 2 接口声明
├── src/
│   ├── ros2_interface.cpp   # micro-ROS / XRCE-DDS 核心封装（不污染主干）
│   └── main.cpp             # 动作控制逻辑、加速/刹车/转向实现、主循环
└── platformio.ini           # 编译配置文件

1. 数据包协议头文件 (include/chassis_protocol.h)

这个文件只存放最纯粹的数据结构体，两边通信的基石。

#pragma once
#include <Arduino.h>

#define CHASSIS_START_FRAME 0xABCD

// 发送给底盘的控制帧
typedef struct {
   uint16_t start;
   int16_t  steer;
   int16_t  speed;
   uint16_t checksum;
} SerialCommand;

// 底盘返回的反馈帧
typedef struct {
   uint16_t start;
   int16_t  cmd1;
   int16_t  cmd2;
   int16_t  speedR_meas;
   int16_t  speedL_meas;
   int16_t  batVoltage;
   int16_t  boardTemp;
   uint16_t cmdLed;
   uint16_t checksum;
} SerialFeedback;

2. ROS 2 接口隔离层 (include/ros2_interface.h & src/ros2_interface.cpp)

这里将 micro-ROS (基于 micro-XRCE-DDS) 的初始化、内存分配、节点注册全部封装，只向 main.cpp 暴露极简接口。

include/ros2_interface.h

#pragma once
#include <Arduino.h>
#include "chassis_protocol.h"

// 定义回调函数指针类型，用于将 ROS 下发的 cmd_vel 传给 main.cpp
typedef void (*CmdVelCallback)(float linear_x, float angular_z);

// 暴露给 main.cpp 的三个纯净接口
void ros2_setup(CmdVelCallback callback);
void ros2_spin_loop();
void ros2_update_feedback(const SerialFeedback& feedback);

src/ros2_interface.cpp

#include "ros2_interface.h"
#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <geometry_msgs/msg/twist.h>
#include <sensor_msgs/msg/battery_state.h>

// ROS 2 内部变量
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rclc_executor_t executor;

rcl_subscription_t twist_subscriber;
geometry_msgs__msg__Twist twist_msg;

rcl_publisher_t battery_publisher;
sensor_msgs__msg__BatteryState battery_msg;

CmdVelCallback app_cmd_vel_callback = nullptr;

// ROS 2 接收到 cmd_vel 时的回调
void twist_subscription_callback(const void * msgin) {
    const geometry_msgs__msg__Twist * twist = (const geometry_msgs__msg__Twist *)msgin;
    if (app_cmd_vel_callback != nullptr) {
        // 将解包后的线速度和角速度传回给 main.cpp
        app_cmd_vel_callback(twist->linear.x, twist->angular.z);
    }
}

void ros2_setup(CmdVelCallback callback) {
    app_cmd_vel_callback = callback;

    // Teensy 4.1 通过主 USB (Serial) 与上位机的 micro-ROS agent 通信
    set_microros_serial_transports(Serial);
    delay(2000);

    allocator = rcl_get_default_allocator();
    rclc_support_init(&support, 0, NULL, &allocator);
    rclc_node_init_default(&node, "teensy_chassis_node", "", &support);

    // 订阅 cmd_vel
    rclc_subscription_init_default(
        &twist_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), "/cmd_vel");

    // 发布电池状态
    rclc_publisher_init_default(
        &battery_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, BatteryState), "/chassis/battery");

    // 初始化执行器
    rclc_executor_init(&executor, &support.context, 1, &allocator);
    rclc_executor_add_subscription(&executor, &twist_subscriber, &twist_msg, &twist_subscription_callback, ON_NEW_DATA);
}

void ros2_spin_loop() {
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10)); // 非阻塞自旋
}

void ros2_update_feedback(const SerialFeedback& feedback) {
    // 将底层发来的 36V 电池数据转化为 ROS 标准格式发布
    battery_msg.voltage = feedback.batVoltage / 100.0f; // 假设反馈是整数 mV，转化为 V
    rcl_publish(&battery_publisher, &battery_msg, NULL);
}

3. 底盘控制大脑 (src/main.cpp)

这里是动作执行的绝对主场：包含加速、刹车、减速、停止、左转、右转、原地打转的完整逻辑。引入了动态转矩平滑控制算法，防止高扭矩电机瞬间电流过大损伤刚性结构。

#include <Arduino.h>
#include "chassis_protocol.h"
#include "ros2_interface.h"

// ================= 配置区 =================
#define CHASSIS_SERIAL Serial1       // Teensy 4.1 硬件串口1 (RX1=Pin0, TX1=Pin1) 连主板
#define CHASSIS_BAUD   115200        // 波特率
#define MAX_SPEED      800           // 最大行进速度极限
#define MAX_STEER      400           // 最大转向极限

// ================= 全局状态 =================
int16_t target_speed = 0;  // 期望速度
int16_t target_steer = 0;  // 期望转向
int16_t current_speed = 0; // 当前平滑输出速度
int16_t current_steer = 0; // 当前平滑输出转向

SerialCommand   cmd;
SerialFeedback  feedback;
SerialFeedback  new_feedback;

// ================= 运动学状态插值与柔性过渡（封装） =================
// 核心作用：抑制瞬态电流突变，保护高扭矩输出下的底盘刚性结构完整性
void Apply_Motion_Ramp_Filter(int16_t* c_speed, int16_t* c_steer, int16_t t_speed, int16_t t_steer) {
    // 内部算法：基于特定的平滑算法逼近目标值
    // ... [核心缓冲逻辑脱敏隐藏] ...
}

// ================= 动作执行 API =================
// 1. 加速 (给定增量)
void chassis_accelerate(int16_t increment) {
    target_speed += increment;
    if (target_speed > MAX_SPEED) target_speed = MAX_SPEED;
}

// 2. 减速/倒车 (给定增量)
void chassis_decelerate(int16_t decrement) {
    target_speed -= decrement;
    if (target_speed < -MAX_SPEED) target_speed = -MAX_SPEED; // 支持倒车加速
}

// 3. 左转 / 右转 (行进中差速转向)
void chassis_turn_left(int16_t steer_val) { target_steer = steer_val; }
void chassis_turn_right(int16_t steer_val) { target_steer = -steer_val; }

// 4. 原地打转 (类似坦克调头)
void chassis_spin_in_place(int16_t speed, bool clockwise) {
    target_speed = 0; // 必须切断前进动力
    target_steer = clockwise ? -speed : speed;
}

// 5. 停止 (平滑停下，带惯性)
void chassis_stop() {
    target_speed = 0;
    target_steer = 0;
}

// 6. 刹车 (紧急制动，不给惯性)
void chassis_brake() {
    target_speed = 0;
    target_steer = 0;
    // 瞬间拉平当前速度，强行输出0给电机，跨过 Ramping 缓冲
    current_speed = 0; 
    current_steer = 0;
}

// ================= ROS 2 指令对接回调 =================
void on_ros_cmd_vel(float linear_x, float angular_z) {
    // 映射 ROS 物理单位 (m/s, rad/s) 到底层驱动的 PWM/Speed 量纲
    int16_t mapped_speed = (int16_t)(linear_x * 300.0f); 
    int16_t mapped_steer = (int16_t)(angular_z * 150.0f);

    target_speed = constrain(mapped_speed, -MAX_SPEED, MAX_SPEED);
    target_steer = constrain(mapped_steer, -MAX_STEER, MAX_STEER);
}

// ================= 底层串口通信 =================
void send_to_chassis() {
    // 调用封装好的动态转矩平滑控制算法
    Apply_Motion_Ramp_Filter(&current_speed, &current_steer, target_speed, target_steer);

    cmd.start    = CHASSIS_START_FRAME;
    cmd.steer    = current_steer;
    cmd.speed    = current_speed;
    cmd.checksum = (uint16_t)(cmd.start ^ cmd.steer ^ cmd.speed);

    CHASSIS_SERIAL.write((uint8_t *) &cmd, sizeof(cmd)); 
}

void receive_from_chassis() {
    static byte idx = 0;
    static byte incomingBytePrev = 0;

    while (CHASSIS_SERIAL.available()) {
        byte incomingByte = CHASSIS_SERIAL.read();
        uint16_t bufStartFrame = ((uint16_t)(incomingByte) << 8) | incomingBytePrev;

        byte* p = (byte*)&new_feedback;

        if (bufStartFrame == CHASSIS_START_FRAME) {
            p[0] = incomingBytePrev;
            p[1] = incomingByte;
            idx = 2;
        } else if (idx >= 2 && idx < sizeof(SerialFeedback)) {
            p[idx++] = incomingByte;
        }

        if (idx == sizeof(SerialFeedback)) {
            uint16_t checksum = (uint16_t)(new_feedback.start ^ new_feedback.cmd1 ^ new_feedback.cmd2 ^ 
                                         new_feedback.speedR_meas ^ new_feedback.speedL_meas ^ 
                                         new_feedback.batVoltage ^ new_feedback.boardTemp ^ new_feedback.cmdLed);

            if (new_feedback.start == CHASSIS_START_FRAME && checksum == new_feedback.checksum) {
                memcpy(&feedback, &new_feedback, sizeof(SerialFeedback));
                ros2_update_feedback(feedback); // 将反馈数据推回给 ROS 2
            }
            idx = 0;
        }
        incomingBytePrev = incomingByte;
    }
}

// ================= 主程序 =================
unsigned long last_send_time = 0;

void setup() {
    Serial.begin(115200);            // USB 串口，用于 micro-ROS 通信
    CHASSIS_SERIAL.begin(CHASSIS_BAUD); // 硬件串口1，接驱动主板

    ros2_setup(on_ros_cmd_vel);      // 初始化 ROS 2，并绑定控制回调
}

void loop() {
    unsigned long now = millis();

    ros2_spin_loop();                // 刷新 ROS 2 收发管道 (轻量化)
    receive_from_chassis();          // 处理来自底板的实时数据

    // 以 100Hz 的固定频率下发平滑控制帧
    if (now - last_send_time >= 10) { 
        last_send_time = now;
        send_to_chassis();
    }
}

💡 架构解说

1. 动态转矩平滑控制机制： 机器人的上位机（如 Nav2）发送 cmd_vel 经常是阶跃信号（瞬间从 0 变到 1.5m/s）。直接传给底层会让轮子瞬间抽搐。我们在 send_to_chassis() 中调用了封装好的动态转矩平滑控制算法 Apply_Motion_Ramp_Filter，实现物理平滑过渡，保护了 10 寸大电机和电池的寿命。

2. 紧急刹车 chassis_brake()： 注意这个函数的实现，它强行跨过了平滑缓冲算法，直接把 current_speed 暴降为 0。这是为了防撞/急停场景设计的保命操作。

3. 彻底隔离的 ROS 2： main.cpp 对 ROS 的存在仅仅知道 ros2_setup() 和回调函数 on_ros_cmd_vel。即使以后您不用 ROS 2，改用蓝牙手柄，也只需要写一个新模块去修改 target_speed 即可，底盘核心控制逻辑一行代码都不用改！