设计并实现一款集视觉识别、机械臂抓取与自主运输于一体的物流小车。以Jetson为上位机进行视觉处理，通过UART与STM32通信，控制小车底盘运动及机械臂抓取，完成对指定颜色物块的识别、抓取与定点运输。

1.1 项目核心功能

• 二维码识别：Python 程序使用 pyzbar 解码二维码，并通过串口发送二维码内容。
• 颜色物块识别：Python 程序使用 HSV 阈值识别红、绿、蓝三类物块。
• 图像坐标提取：上位机计算物块中心像素坐标，并按左/中/右区域分类。
• 串口通信：上位机通过 /dev/ttyUSB0、115200 波特率把识别数据发给 STM32。
• 四电机底盘控制：STM32 使用 TIM8 四路 PWM 驱动四个电机，TIM2/3/4/5 读取四路编码器。
• 位置式 PID 控制：System/Control.c 中实现编码器位置闭环。
• 姿态辅助转向：HWT101 姿态传感器通过 USART2 输入 yaw 角，用于转向闭环。
• 机械臂控制：DS3218 舵机通过 TIM1、TIM9、TIM12 输出 PWM，完成多关节控制与夹爪控制。
• OLED 与串口屏显示：用于显示运行状态、调试数据或二维码/任务信息。

2. 系统总体架构

该项目采用分层架构。各层职责清晰：感知层负责识别环境目标，通信层负责数据传输，控制层负责运动控制，执行层负责电机和舵机动作。

2.1 分层结构

层级	组成	职责
感知层	摄像头、二维码、红绿蓝色块识别	获取任务信息和目标物块在图像中的位置。
上位机处理层	Jetson/Nano/Linux + Python + OpenCV + pyzbar	完成图像处理、坐标提取、分区判断和串口发送。
通信层	USART1、USART2、UART4	USART1 接上位机，USART2 接 HWT101，UART4 接串口屏。
控制层	STM32F407VETx + HAL + PID + 状态机	解析数据，执行底盘运动、转向控制和机械臂动作。
执行层	四电机底盘、DS3218 舵机机械臂、夹爪	完成移动、转向、抓取、放置。
显示/调试层	OLED、串口屏、OpenCV 窗口	显示状态、二维码内容、图像识别结果和调试信息。

2.2 控制闭环

底盘控制有两类闭环：第一类是编码器位置闭环，用于控制四个电机转过指定编码器脉冲；第二类是姿态角闭环，用 HWT101 输出的 yaw 角与目标角度比较，计算角度误差后执行左转或右转。

机械臂部分采用关节角控制。底层 DS3218.c 把数学关节角转换成舵机 PWM；上层 Servo.c 则对关节目标角做分步逼近，从而避免舵机突然跳变。

3. 硬件组成与接口分配

项目主控为 STM32F407VETx，封装 LQFP100。CubeMX 配置中系统时钟为 168 MHz，外部 HSE 为 8 MHz，使用 PLL 倍频到 168 MHz。

3.1 主要硬件清单

硬件	作用	代码/接口位置
STM32F407VETx	下位机主控，负责所有实时控制	Core、Hardware、System、MDK-ARM
Jetson/Nano/Linux 上位机	运行 Python 视觉识别程序	VT4_2.py
四个带编码器电机	小车底盘运动	Motor.c、Encoder.c、Control.c
电机驱动器	接收 PWM 与方向信号驱动电机	TIM8 + PE0/PE1/PE2/PE3/PE8/PE9/PE10/PE12
HWT101 姿态传感器	输出 yaw 偏航角辅助转向	USART2，HWT101.c
DS3218 舵机	机械臂关节与夹爪执行器	TIM1、TIM9、TIM12，DS3218.c
OLED 显示屏	显示状态和调试数据	OLED.c，PA11/PE13 软件 I2C 类接口
淘晶驰串口屏	显示二维码或任务信息	UART4，Uart_Screen.c
摄像头	采集二维码和物料画面	OpenCV VideoCapture(0/1)

3.2 STM32 外设分配

外设	用途	关键配置/引脚
TIM8 CH1-CH4	四个底盘电机 PWM	PC6/PC7/PC8/PC9；Period=1000-1，Prescaler=840-1
TIM2	编码器 A	PA5 / PB3
TIM3	编码器 B	PB4 / PB5
TIM4	编码器 C	PB6 / PB7
TIM5	编码器 D	PA0 / PA1
TIM6	运动控制周期中断	Period=1000-1，Prescaler=1680-1；源码注释称周期约 20 ms
TIM1 CH1/CH2	机械臂舵机 PWM	PA8 / PE11；Period=10000-1，Prescaler=168-1
TIM9 CH1/CH2	机械臂舵机 PWM	PE5 / PE6；Period=10000-1，Prescaler=168-1
TIM12 CH1/CH2	机械臂舵机 PWM	PB14 / PB15；Period=10000-1，Prescaler=84-1
USART1	上位机视觉数据接收	PA9 TX / PA10 RX，115200 8N1
USART2	HWT101 姿态传感器	PA2 TX / PA3 RX，115200 8N1
UART4	串口屏通信	PC10 TX / PC11 RX，115200 8N1
GPIO	电机方向、按键、OLED	KEY1 PC1，KEY2 PC2；OLED_SDA PA11，OLED_SCK PE13

3.3 电机与机械臂接口

对象	PWM 通道	方向/控制	说明
电机 A	TIM8_CH1 / PC6	PE0 AIN1、PE1 AIN2	左前轮或 A 轮，编码器接 TIM2。
电机 B	TIM8_CH2 / PC7	PE2 BIN1、PE3 BIN2	左后轮或 B 轮，编码器接 TIM3。
电机 C	TIM8_CH3 / PC8	PE8 CIN1、PE9 CIN2	右前轮或 C 轮，编码器接 TIM4。
电机 D	TIM8_CH4 / PC9	PE10 DIN1、PE12 DIN2	右后轮或 D 轮，编码器接 TIM5。
关节 1	TIM1_CH1 / PA8	PWM 舵机控制	底座旋转关节。
关节 2	TIM9_CH1 / PE5 + TIM1_CH2 / PE11	双舵机联动	源码中关节 2 使用两个舵机反向补偿。
关节 3	TIM9_CH2 / PE6	PWM 舵机控制	机械臂中段关节。
关节 4	TIM12_CH1 / PB14	PWM 舵机控制	末端姿态关节。
夹爪	TIM12_CH2 / PB15	PWM 舵机控制	Release=松开，Catch=夹取。

4. 软件工程结构

4.1 主要目录结构

目录/文件	作用	是否核心
README.md	项目简介、硬件/软件说明、快速开始	是
Core/	CubeMX 生成的主程序、外设初始化和中断入口	是
Drivers/	STM32 HAL、CMSIS、DSP 等库文件	必要库
Hardware/	硬件驱动封装：电机、编码器、舵机、OLED、串口、姿态传感器等	是
System/	控制逻辑：PID、转向控制、机械臂运动、总状态机等	是
MDK-ARM/	Keil 工程文件与启动文件	是
Cubemx-car6.ioc	CubeMX 外设配置文件	是
VT4_2.py	上位机视觉识别与串口发送程序	是
image_to_world(success).ipynb	相机像素坐标到世界坐标转换实验	待集成
robot_arm6(逆运动).ipynb	四轴机械臂逆运动学求解与可视化	待集成
.git/	Git 仓库历史和对象	非运行必需

5. STM32 下位机功能模块

5.1 main.c 主程序

main.c 是下位机主入口。它完成 HAL 初始化、系统时钟配置、GPIO/TIM/UART 外设初始化，然后依次初始化 Motor、Encoder、OLED、HWT101、Nano_uart、DS3218。

主循环中持续读取按键、实时读取 HWT101 yaw 角，并刷新 OLED。当 KEY1 被按下时，代码启动 TIM6 中断，用于运动控制或机械臂测试。

5.2 电机驱动 Motor.c

Motor.c 主要负责启动 TIM8 的四路 PWM，并提供 Left_Motor_SetSpeed() 与 Right_Motor_SetSpeed() 两个简单速度设置函数。底盘电机方向通过 PE 口方向引脚控制，速度通过 TIM8 的 CCR 寄存器控制。

代码注释中我把四个电机标记为 A、B、C、D，并给出相对位置：A/C 在前，B/D 在后。Motor.c 更偏底层直接控制；真正的闭环运动控制主要在 Control.c 中完成。

5.3 编码器 Encoder.c

Encoder.c 启动 TIM2、TIM3、TIM4、TIM5 的编码器模式，并通过 Read_Encoder() 读取计数器值。代码中用 short 强制转换计数值，目的是处理 16 位计数器反向计数时出现的 65535 等溢出表示。

Num_Encoder_Cnt(num, ppr, ratio) 用于把轮子转数转换为编码器脉冲数，公式为 num × ratio × ppr × 4，其中 4 表示四倍频。

5.4 位置闭环 Control.c

Control.c 实现四电机位置闭环。Set_ALL_Target() 把每个轮子的目标位移量转换为目标编码器计数；Control() 周期性读取四路编码器并调用 Position_PID() 输出 PWM。

代码中还加入了角度修正 Cor_A/B/C/D：当 HWT101 读取到的角度与目标角度有误差时，会给部分电机额外修正量，帮助小车在直线运动时保持方向。

变量/函数	作用
Position_KP / KI / KD	位置式 PID 参数，当前 KP=0.9，KI=0，KD=0。
Small_Angle_K	小角度纠偏系数，当前为 30。
Target_Position_A/B/C/D	四个电机的目标编码器位置。
Encoder_cnt_R_A/B/C/D	处理后的实际编码器计数，其中 C/D 做了取反。
Set_Pwm_A/B/C/D()	根据 PID 输出设置方向引脚和 PWM 占空比。
Xianfu_Pwm()	PWM 限幅，防止输出过大。
Position_PID()	位置式 PID 算法。

5.5 姿态转向 Dir_Control.c

Dir_Control.c 以 HWT101 的 yaw 角为反馈，完成角度闭环转向。calculate_angle_error() 会把目标角与当前角的误差限制在 -180 到 +180 度范围内，避免跨越 0/360 度时方向判断错误。

Dir_Control() 中设置了 1 度的误差容忍范围。如果误差在容忍范围内，调用 Stop_Turn() 并置 Turn_OK=1；否则根据误差正负执行 Turn_Left() 或 Turn_Right()。

Target_Angle：目标车体角度

Re_Angle：HWT101 实时 yaw 角

angle_error = calculate_angle_error(Re_Angle, Target_Angle)

误差 > 0：逆时针方向修正

误差 < 0：顺时针方向修正

abs(误差) <= 1：认为转向完成

5.6 HWT101 姿态传感器 HWT101.c

HWT101.c 通过 USART2 接收传感器数据。接收状态机寻找帧头 0x55 和角度帧标识 0x53，然后缓存后续 9 字节数据。HWT101_Getyaw() 使用接收数组中的 yaw 字节换算成角度值。

HWT101_Init() 中启动 USART2 中断接收，并向传感器发送指令，用于调整波特率和获取偏航角。

5.7 上位机串口解析 Nano_uart.c

Nano_uart.c 通过 USART1 接收上位机发来的视觉识别数据。它的状态机主要解析颜色物块协议，即 ac + 颜色 + x坐标 + x + y坐标 + 区域 + a。

解析完成后，数据会分别写入 Red、Blue、Green 三个 Color_block 结构体，字段包括 center_x、center_y 和 region。

例：acr320x240ba

a    帧头

c    色块数据标识

r    red，红色物块

320  图像中心 x 坐标

x    x/y 坐标分隔符

240  图像中心 y 坐标

b    区域标识

a    下一帧帧头或结束标志

5.8 机械臂控制 DS3218.c 与 Servo.c

DS3218.c 是舵机 PWM 和关节角映射层。它启动 TIM1、TIM9、TIM12 共 6 路 PWM，并提供关节 1、2、3、4 与夹爪的角度设置函数。源码中关节 2 使用两个舵机通道联动。

Servo.c 是运动平滑层。它维护当前关节角 Joint*_now 和目标关节角 Joint*_target，每次中断只按 Step1/2/3/4 递增或递减，避免机械臂突然快速摆动。Servo_all_control() 按关节 1 → 关节 2 → 关节 3 → 关节 4 的顺序逐步到位。

对象	范围/说明	PWM 通道
Joint1	[-180, 180]，底座旋转，数学角会加 146 做舵机角映射	TIM1_CH1
Joint2	[32, 137]，映射时减2	TIM9_CH1 + TIM1_CH2
Joint3	[-143, 82]，映射时加 143	TIM9_CH2
Joint4	[-127, 28]，映射时加 212	TIM12_CH1
Gripper	Release=0 度，Catch=210 度	TIM12_CH2

5.9 OLED 与串口屏

OLED.c 用于软件方式驱动 OLED 显示，主循环中调用 OLED_Show() 更新信息。Uart_Screen.c 通过 UART4 发送字节、字符串和数字，并重定向 fputc，使 printf 可以输出到串口屏或调试串口。

5.10 总任务控制 ALL_Control.c

ALL_Control.c 是总任务状态机。

6. Python 上位机视觉程序

VT4_2.py 是项目的上位机视觉程序。它运行在 Linux/Jetson/Nano 上，使用 OpenCV 读取摄像头，pyzbar 识别二维码，HSV 阈值识别红、绿、蓝色块，然后通过串口发送给 STM32。

6.1 程序依赖

依赖	作用
opencv-python / cv2	读取摄像头、图像处理、绘制检测结果。
numpy	数组运算、颜色阈值、坐标计算。
pyserial	打开 /dev/ttyUSB0，并以 115200 波特率发送数据。
pyzbar	二维码解码。
threading / multiprocessing.Value / Lock	串口监听与模式变量同步。

6.2 工作模式

程序设计了二维码扫描模式和物料检测模式。初始状态是二维码扫描模式，识别到二维码后，会把二维码数据发给 STM32，并切换到物料检测模式。也可以通过键盘或串口命令手动切换模式。

模式	触发条件	行为
二维码扫描模式	程序启动默认进入，或按键/串口命令切换	从 QR 摄像头读取画面，使用 pyzbar 解码二维码。
物料检测模式	识别到二维码后自动进入，或手动切换	从颜色摄像头读取画面，检测红/绿/蓝色块，发送中心坐标。
二维码检测开关	键盘 r 或串口命令	开启/关闭二维码识别功能。

6.3 颜色识别逻辑

颜色识别使用 HSV 阈值。程序对图像做 GaussianBlur，然后转换为 HSV，再分别生成红、绿、蓝三种 mask。红色由于 HSV 色相跨 0 度，使用了两段阈值合并。

轮廓检测后，程序通过 cv2.moments() 计算物块中心点 cX、cY，并根据横向位置判断 left、middle、right 区域。最后把区域映射为 d、b、c 等单字符后发送给 STM32。

识别结果示例：

颜色：r / g / b

中心坐标：cX, cY

区域：left / middle / right

发送格式：

final_data = f"ac{color_char}{cX}x{cY}{suffix}a"

6.4 摄像头配置

VT4_2.py 默认使用两个摄像头：VideoCapture(0) 作为二维码摄像头，VideoCapture(1) 作为颜色识别摄像头。如果第二个摄像头无法打开，程序会尝试复用第一个摄像头。分辨率设置为 640×480，帧率设置为 30 FPS。

7. 串口通信协议

7.1 串口通道分工

串口	连接对象	波特率	代码文件
USART1	Jetson/Nano/Linux 上位机	115200	Nano_uart.c / Serial_interrupt.c
USART2	HWT101 姿态传感器	115200	HWT101.c / Serial_interrupt.c
UART4	淘晶驰串口屏或调试显示	115200	Uart_Screen.c

8. 机械臂运动学与相机坐标转换

项目中包含两个 notebook，已经对机械臂运动学和相机坐标转换做过验证，但这些内容还没有完全变成 STM32 或 Python 主程序的一部分。

8.1 机械臂逆运动学 notebook

robot_arm6(逆运动).ipynb 使用 roboticstoolbox、spatialmath、scipy.optimize 等库建立四轴机械臂模型，并求解目标末端位置对应的关节角。源码和输出显示单位为 cm。

项目	说明
自由度	4 轴机械臂
Crgic DH 参数 a{i-1}	[0, 1.025, 22.2, 15.6] cm
Crgic DH 参数α{i-1}	[0, 90, 0, 0] 度
Crgic DH 参数 d_i	[0, 0, 0, 0] cm
Crgic DH 参数θ_i	关节角
基座偏移	(0, 0, 14.9) cm
工具偏移	(11.5, 1.6, 0) cm

8.2 相机像素坐标到世界坐标 notebook

image_to_world(success).ipynb 根据相机内参 fx、fy、cx、cy 和外参矩阵，把图像像素点换算到指定世界平面高度上的世界坐标。

参数	数值/说明
fx	722.98
fy	722.15
cx	311.94
cy	282.14
world_z	15 cm
外参矩阵	包含相机到世界的旋转和位移，位移项约为 x=12.5752、z=45.1130 cm 相机坐标系相对末端坐标系绕 Y 轴 +90°，(-2,0,0)(cm)

9. 项目总结

项目目前已完成 底盘、视觉、姿态和机械臂几个关键子系统。

项目最大的不足在于：完整自动物流任务链路尚未完全打通，尤其是二维码任务解析、视觉坐标到机械臂抓取位姿、机械臂动作序列以及 ALL_Control 总状态机仍需要继续开发。