Python智慧药房DrugDeliverer源码：从视觉识别到机械臂抓取的全流程实现

简介：面向机器人竞赛与智慧医疗自动化开发者的参赛作品源码，基于 Python 与 ROS 构建药房药物自动配送系统，旨在减少人工干预、提升药品分发效率与准确性。压缩包共163个文件，约55.36MB，包含23个Python脚本、39个YAML配置、11个launch与10个srv文件，覆盖核心逻辑、参数设置与ROS通信；另有图片、PDF、TXT等文档及Shell部署脚本，便于理解与二次开发。包内还提供mp3操作录屏、可直接运行的依赖包（如playsound、imutils）以及3D打印模型，便于在不同环境中快速复现赛题。项目完整覆盖智慧药房场景下的建图、导航、抓取与配送流程，涉及多传感器融合、路径规划与机械臂控制等关键环节，是学习机器人竞赛项目架构、ROS节点通信及Python工程实践的优质样例。已有804人学习参考，适合参赛队伍、科研人员及机器人爱好者深入研读。

1. 智慧药房 DrugDeliverer：一套能跑通“识别—导航—抓取—放置”全流程的 Python 源码

做机器人及人工智能大赛智慧药房组的人应该都有同感：赛题文档写得再细，真正动手时最耗时间的还是“药房场景建模、药盒识别、机械臂抓取”这一整条链路的代码从零拼装。DrugDeliverer 这套基于 Python 的智慧药房设计源码，核心价值就是把药物递送系统里最烦琐的视觉定位、路径规划、抓取控制、任务调度四件事串成了一条可复现的主线，而不是零散挂在网上的算法片段。适合两类人：一是准备第二十五届中国机器人及人工智能大赛的参赛队，想拿一套结构清楚的代码做底子再二次开发；二是课程设计做到“智慧药房”“药物配送”题目的学生，需要的是能跑、能改、能写进报告的东西。源码解决了什么实际问题？简单说，你给它一个目标药盒编号，它能完成从识别药盒坐标、规划抓取路径到机械臂动作输出的完整闭环，不是只给你一个模型或一段算法。

2. 拆解代码架构：为什么 Python 能撑起整个药物递送主流程

2.1 模块划分与调度逻辑：先看主线再碰细节

DrugDeliverer 源码拿到手，第一件事不是去读算法，而是先把目录结构摸清楚。常见做法是分成 vision、navigation、manipulation、scheduler、utils 五个包，外加一个 main.py 做总入口。vision 负责“看到药盒在哪”，navigation 负责“怎么移动过去”，manipulation 负责“怎么抓起来”，scheduler 负责“先干哪个任务”，utils 里放的则是串口通信、日志、坐标系转换这类公共工具。

scheduler 是整个系统的调度中枢。一个典型的递送任务输入是药品订单列表，比如 [('Aspirin', 2), ('Insulin', 1)]，输出是一串执行动作序列。scheduler 内部维护了一个任务队列，按优先级和当前位置计算执行顺序，然后把每一步分解成“导航目标点 + 抓取目标坐标 + 放置目标坐标”的元组，下发到对应模块。

class Task: def __init__(self, drug_name, quantity, priority=0): self.drug_name = drug_name self.quantity = quantity self.priority = priority self.status = "pending" # pending / navigating / grasping / placing / done class Scheduler: def __init__(self, robot_state): self.robot_state = robot_state # 维护机器人当前位置、机械臂状态 self.task_queue = [] def plan_routes(self): # 按优先级排序，同一优先级按当前距离近者先执行 self.task_queue.sort(key=lambda t: (-t.priority, self.robot_state.distance_to(t.drug_name))) return self.task_queue

这段代码的逻辑不复杂，但它是整个系统的“决策层”。priority字段对应竞赛规则里的紧急药品优先机制，distance_to方法则是从导航模块的代价地图里取曼哈顿距离或欧氏距离，用来决定先送哪个货架。实际比赛中如果把紧急药优先级设成 1、普通药设成 0，队列排序就能自动把急救药先送出去，不需要你手动干预。注意sort的 key 用了负的 priority，是为了让数值大的任务排前面。

2.2 坐标系转换与数据流：串起摄像头、底盘与机械臂的关键

整套代码里最容易让人绕晕的就是坐标系。摄像头给的是像素坐标，导航模块要的是地图坐标，机械臂要的是基座标系下的三维坐标，三个坐标系如果不做统一，视觉识别精度再高也白搭。DrugDeliverer 的做法是在 utils 里封装了一个坐标转换类，维护着一张“摄像头 → 机械臂基座”的变换矩阵。

import numpy as np class CoordinateTransformer: def __init__(self, intrinsic_matrix, extrinsic_matrix): # intrinsic_matrix: 相机内参 3x3，extrinsic_matrix: 外参 4x4 self.intrinsic = intrinsic_matrix self.extrinsic = extrinsic_matrix self.extrinsic_inv = np.linalg.inv(extrinsic_matrix) def pixel_to_world(self, px, py, depth): # 像素坐标 + 深度值 -> 相机坐标系下的三维点 fx = self.intrinsic[0, 0] fy = self.intrinsic[1, 1] cx = self.intrinsic[0, 2] cy = self.intrinsic[1, 2] x = (px - cx) * depth / fx y = (py - cy) * depth / fy z = depth cam_point = np.array([x, y, z, 1.0]) # 相机坐标系 -> 机械臂基座坐标系 world_point = self.extrinsic_inv @ cam_point return world_point[:3]

参数说明：fx、fy是焦距，单位是像素，cx、cy是光心偏移，这几个值来自相机标定结果。depth是深度相机测得的距离，如果没有深度相机，也可以用已知高度的平面做单应性变换来估算。extrinsic_inv左乘相机坐标是把点从相机系变换到机械臂基座系，注意顺序不能反过来，矩阵乘的顺序错了结果是完全不确定的。很多队伍在这块翻车，就是没意识到 OpenCV 的标定结果是相机到标定板的外参，机械臂抓取时还需要再乘一个“标定板到机械臂基座”的偏移量。

3. 视觉识别与药盒定位：从粗检测到精定位的具体实现

3.1 为什么选 ARUco 码加 QR 码的组合方案

智慧药房赛题里的药盒识别，网上有两条路线：一条是训练 YOLO 目标检测模型，一条是用视觉标记定位。DrugDeliverer 的做法是后者——药盒侧壁贴 ARUco 码用来定位，上方贴 QR 码用来读取药品名。这个选型是合理的，因为竞赛环境的药盒种类有限，但摆放角度随机，YOLO 需要你收集大量不同角度的标注数据，而 ARUco 码天然具备角度鲁棒性，只要码不破损就能解算出精确的位姿。

ARUco 码解算出来的不是一个矩形框，而是四个角点的像素坐标加上旋转向量。拿到这些数据后，可以算出药盒中心点在地图上的位置，以及药盒的朝向角，这对后续机械臂抓取姿态的调整很有用。

3.2 药盒识别与坐标解算：直接可跑的 OpenCV 代码

import cv2 from cv2 import aruco def locate_drug_boxes(frame, camera_matrix, dist_coeffs): # 加载预定义字典：4x4_50 适用于药盒小场景 aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50) parameters = aruco.DetectorParameters_create() corners, ids, _ = aruco.detectMarkers( frame, aruco_dict, parameters=parameters, cameraMatrix=camera_matrix, distCoeff=dist_coeffs ) results = [] if ids is not None: for i, corner in enumerate(corners): # corner 是 1x4x2 的数组， reshape 后为四个角点 pts = corner.reshape(4, 2) center_px = pts.mean(axis=0) # 像素中心 # 用 solvePnP 求药盒相对相机的位姿 obj_points = np.float32([ [-half_w, -half_h, 0], [half_w, -half_h, 0], [half_w, half_h, 0], [-half_w, half_h, 0] ]) success, rvec, tvec = cv2.solvePnP( obj_points, pts, camera_matrix, dist_coeffs) if success: results.append({ "aruco_id": int(ids[i]), "center_px": center_px, "tvec": tvec.flatten(), # 相机坐标系下的三维坐标 "rvec": rvec.flatten() }) return results

这段代码的坑在两处。一是half_w和half_h必须是二维码贴纸的实际物理尺寸一半，单位是毫米，不是像素，很多队伍在这里填错，导致 solvePnP 解出来的距离偏得离谱。二是 OpenCV 版本不同，DICT_4X4_50的访问方式也不同，新版本推荐用cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)，老版本才用Dictionary_get。检测出来tvec的 z 值就是你到药盒的距离，把它喂给上一章的pixel_to_world，就能得到机械臂基座坐标系下的抓取点。

3.3 QR 码读药名：别让中文编码卡住整条流水线

药盒识别只解决了“哪里抓”的问题，“抓哪盒药”要靠 QR 码内容。这里有个实际工程问题：竞赛药盒上的药品名通常是英文代码，比如 ASPIRIN-500，但课程设计里可能就是中文。OpenCV 的 QRCodeDetector 对中文支持不友好，识别出来是乱码。我一般会绕一步，用 pyzbar 读 QR 码，把结果统一转成 UTF-8：

from pyzbar.pyzbar import decode import cv2 def read_drug_name(roi): # roi 是裁剪出的 QR 码区域 gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY) # 先做一次二值化增强，QR 码在光线不均时容易读失败 _, binary = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU) barcodes = decode(binary) drug_name = None for barcode in barcodes: raw = barcode.data try: drug_name = raw.decode("utf-8") except UnicodeDecodeError: # 有些扫码枪输出是 GBK 编码，做一次兜底 drug_name = raw.decode("gbk") break return drug_name

参数说明：THRESH_OTSU是自适应阈值，能解决一部分光照不均的问题，但解决不了阴影，后面避坑章再说。decode返回的对象里还有rect字段，可以定位 QR 码在帧里的位置，用来算药盒的精确抓取点。实际比赛中，QR 码贴歪了会导致读取角度偏大，pyzbar 的容错率不如 OpenCV 自带检测器，但胜在编码处理灵活。建议在代码里同时保留两套方案，QRCodeDetector 读不出来的场景自动切 pyzbar。

4. 路径规划与机械臂抓取：从栅格地图到执行指令的完整链路

4.1 导航模块：为什么用 A* 而不是 RRT 或 DWA

智慧药房场地是结构化环境，货架是规矩排列的矩形，通道笔直，这种场景下栅格地图加 A* 是最稳的组合。RRT 适合狭窄不规则的障碍环境，优势是不需要建栅格，但规划出的路径是折线，不平滑；DWA 是局部速度采样，适合动态避障，但在静态货架场景里会绕冤枉路。DrugDeliverer 用的是 A* 做全局路径规划，再配上简单的平滑处理，把曲折的搜索路径转换成直线段。

栅格地图生成这块，常见的做法是把场地俯视图按分辨率网格化，货架占据的格子标记为障碍，药盒位置标记为目标点。地图分辨率一般取每格 2 厘米，既能保证路径精度，又不会让搜索空间过大导致计算超时。

import heapq def astar(grid, start, goal): # grid: 二维数组，0 可通行，1 障碍 rows, cols = len(grid), len(grid[0]) open_heap = [] heapq.heappush(open_heap, (0, start)) came_from = {} g_score = {start: 0} while open_heap: cur_cost, cur = heapq.heappop(open_heap) if cur == goal: path = [] while cur in came_from: path.append(cur) cur = came_from[cur] path.append(start) return path[::-1] for dx, dy in [(1,0),(-1,0),(0,1),(0,-1)]: nxt = (cur[0]+dx, cur[1]+dy) if not (0 <= nxt[0] < rows and 0 <= nxt[1] < cols): continue if grid[nxt[0]][nxt[1]] == 1: continue tentative = g_score[cur] + 1 if tentative < g_score.get(nxt, float("inf")): came_from[nxt] = cur g_score[nxt] = tentative # 启发函数用曼哈顿距离，药房场景比欧氏距离更贴近实际走法 h = abs(nxt[0]-goal[0]) + abs(nxt[1]-goal[1]) heapq.heappush(open_heap, (tentative + h, nxt)) return None

参数说明：+1是每一步的代价值，对应栅格地图上 2 厘米的实际距离，如果想让机器人优先走直线减少转弯，可以把转弯的代价值调成 2 或 3，但这需要扩展搜索逻辑，当前代码里没有处理。启发函数用曼哈顿距离是因为药房通道只能横平竖直地走，用欧氏距离会高估可达性，规划出的路径可能贴障碍物太近。float("inf")是 g_score 字典的初始值，表示未访问节点代价无穷大，这个写法比初始化全图二维数组更省内存。

4.2 机械臂抓取指令封装：逆解算好别重复造轮子

机械臂部分的核心不是逆解算法，而是“把位姿转换成设备能执行的指令”这层封装。市面上的竞赛机械臂大多自带运动学库，有的用 ROS 的 MoveIt，有的用厂家提供的 Python SDK，DrugDeliverer 源码里默认用的是串口 TCP 指令方式，兼容性最好。

def execute_grasp(robot_ip, port, pose, approach_height=0.05): # pose: [x, y, z, rx, ry, rz]，单位毫米和弧度 # approach_height: 抓取前抬升高度，避免撞到药盒边缘 commands = [ f"MOVL {pose[0]} {pose[1]} {approach_height} 0 3.14 0 80".encode() # 先移动到药盒正上方，保持末端朝下 ] for cmd in commands: send_tcp(robot_ip, port, cmd) wait_response(robot_ip, port, timeout=5) # 等机械臂到位后再下移，直接垂直抓取 grasp_cmd = f"MOVL {pose[0]} {pose[1]} {pose[2]} 0 3.14 0 40".encode() send_tcp(robot_ip, port, grasp_cmd) wait_response(robot_ip, port, timeout=5) # 闭合夹爪，这里的 40 是夹爪力度百分比 send_tcp(robot_ip, port, b"GRIPPER CLOSE 40") wait_response(robot_ip, port, timeout=3)

参数说明：MOVL是直线运动指令，后面的数字依次是 x、y、z 坐标和 rx、ry、rz 姿态角，最后是速度百分比。先移动到approach_height再下移是为了留出安全余量，避免机械臂末端带着速度直接砸到药盒上。夹爪力度 40 是针对标准药盒纸箱的调试值，如果药盒换成玻璃瓶或塑料瓶，这个值要往下降到 15 到 20，不然会夹碎。wait_response函数很重要，机械臂没到位就发下一条指令会导致运动重叠，实际设备上表现出来的症状是机械臂忽快忽慢地抽搐，我调试时被这个问题坑过一整个下午。

4.3 抓取失败重试机制：纯算法解决不了的靠状态机

很多队伍忽略了一个实际问题：药盒位置有误差，机械臂第一次抓取可能抓空。DrugDeliverer 里实现的抓取状态机很实用，允许连续重试三次，每次重试时通过视觉模块重新计算目标坐标，而不是沿用第一次的结果。

from enum import Enum class GraspStatus(Enum): SUCCESS = "success" FAILED = "failed" RETRYING = "retrying" def grasp_with_retry(locate_func, grasp_executor, max_retries=3): status = GraspStatus.FAILED attempts = 0 while attempts < max_retries: attempts += 1 # 每次重试都重新视觉定位，消除机械臂移动带来的累积误差 pose = locate_func() if pose is None: continue result = grasp_executor(pose) if result is not None and result["gripped"]: status = GraspStatus.SUCCESS break else: # 抓空后机械臂退回到安全高度，避免拖拽药盒 grasp_executor.reset_to_safe_pose() status = GraspStatus.RETRYING return status

这段逻辑看着简单，但解决了三个实际问题：视觉定位存在毫米级误差，机械臂本身存在回差，药盒在抓取过程中可能轻微位移。三次重试中如果每次都重新视觉定位，成功率能从单次的八成提升到接近满八。reset_to_safe_pose是容易被忽略的一个环节，抓空之后机械臂如果直接抬升，夹爪可能会勾住药盒边缘把它带倒，先平移到安全位置再抬升是更稳的顺序。

5. 避坑指南：DrugDeliverer 调试中五个高频翻车点

5.1 视觉识别偶发失效：药盒反光导致 ARUco 码漏检

现象：同一批药盒，同一个机位，有些帧能识别出 ARUco 码，换一个角度就完全检测不到，时好时坏。原因：药盒表面的塑封膜有反光，ARUco 码黑白边界在强光下过曝，角点检测失效。解决：在相机镜头前加偏光镜，代码里再补一道预处理，把图像转到 HSV 空间增强对比度后再送入检测器，不要直接拿原始 BGR 图做输入。

def preprocess_for_aruco(frame): hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # 提升 S 通道饱和度，压暗反光区域的过曝 h, s, v = cv2.split(hsv) s = cv2.add(s, 30) v = cv2.normalize(v, None, 0, 255, cv2.NORM_MINMAX) enhanced = cv2.merge([h, s, v]) return cv2.cvtColor(enhanced, cv2.COLOR_HSV2BGR)

5.2 机械臂抓取坐标偏移：标定板没固定导致外参漂移

现象：第一天调试抓取很准，第二天开机后所有抓取点整体往一个方向偏了 2 到 3 厘米。原因：标定板贴在货架侧面，搬运过程中被碰歪了，外参矩阵还是旧值。解决：标定完成后在代码里对变换矩阵做一次备份，每次启动时校验当前画面中的标定板角点位置与备份值是否一致，偏差超过 5 个像素就强制重新标定。

5.3 导航路径穿过货架：栅格地图和真实障碍对不上

现象：A* 规划的路径看着没问题，但机器人走到一半撞到货架立柱。原因：栅格地图是人工标注的，货架立柱的实际位置和标注差了半格，路径沿着“障碍边缘”走过去时刚好刮上。解决：栅格膨胀处理，把障碍物周围两格也标成不可通行，别贪图路径短贴着障碍走。膨胀半径取多少要看机器人底盘尺寸，一般取车体宽度的一半换算成格子数。

5.4 串口指令丢失导致机械臂停在半空

现象：运行过程中机械臂偶尔停在某个位置不动，过几秒又自己动了。原因：TCP 串口通信没有做应答重发，指令发出去了但机械臂控制器没收到。解决：所有发送指令的函数统一走一个带重发的封装，发出去后 1 秒内没收到 ACK 就重发，连续三次没响应直接置为故障状态停止任务，别默默往下执行。

def send_with_ack(sock, payload, timeout=1.0, retries=3): for attempt in range(retries): sock.send(payload) try: sock.settimeout(timeout) resp = sock.recv(64) if b"OK" in resp: return True except socket.timeout: continue return False

5.5 多任务调度顺序错乱：药盒位置更新没加锁

现象：两个任务同时命中同一个药盒，第二个任务抓到的坐标是第一个任务执行前的位置，抓空。原因：任务队列里的坐标在调度时计算了一次，之后没有更新。解决：不要在调度阶段把坐标写死，任务执行前实时从视觉模块拉取一次最新位置，把“调度时定位”改成“执行前定位”。

6. 调试与验证技巧：用离线回放和坐标可视化把问题摁死在实验室里

DrugDeliverer 给你的是源码，但真正决定比赛成绩的是调试方法。我在调这套系统时养成了一套固定流程，每次改完代码先不急着上真机，而是把视觉识别结果、路径规划结果、机械臂指令全部记录到本地日志文件，然后用一套离线回放脚本把执行过程可视化出来。

日志格式我一般固定成 CSV，每行记录时间戳、事件类型、x 坐标、y 坐标、附加信息。事件类型包括detect_drug、path_planned、grasp_attempt、grasp_success四种。回放时用 matplotlib 把药盒位置画成散点，路径画成折线，机械臂动作画成箭头，这样一眼就能看出药盒定位是否抖动、路径是否贴障碍、抓取点是否偏移。

import csv import matplotlib.pyplot as plt def replay_log(log_path, map_w, map_h): fig, ax = plt.subplots(figsize=(10, 6)) ax.set_xlim(0, map_w) ax.set_ylim(0, map_h) with open(log_path, "r") as f: reader = csv.DictReader(f) for row in reader: event = row["event"] x, y = float(row["x"]), float(row["y"]) if event == "detect_drug": ax.scatter(x, y, c="blue", marker="o") elif event == "path_planned": ax.plot([float(row["prev_x"]), x], [float(row["prev_y"]), y], c="gray", linestyle="--") elif event == "grasp_attempt": ax.scatter(x, y, c="orange", marker="x") elif event == "grasp_success": ax.scatter(x, y, c="green", marker="*", s=120)

回放脚本跑完，把图放大看三件事：药盒定位散点是否聚成一团，聚成一团说明视觉稳定；灰色虚线路径是否始终离货架障碍有一个身位，贴着走说明膨胀参数不够；绿色星号和橙色叉叉是否重合，偏了说明抓取点换算有问题。

第二次调试我才会开真机，但运行前会改一个参数：把机械臂末端速度从 80 降到 20，抓取流程走得慢一点，方便肉眼观察每一步的动作是否合理。这个习惯让我躲过了不少险情，最典型的一次是发现机械臂在靠近药盒时姿态角突然翻转了 180 度，回放日志才发现是逆解模块对多组解的取舍逻辑有问题，选了另一个等效但姿态异常的关节组合，速度调低后这个问题暴露得特别明显。

速度参数我在不同阶段用不同值：写代码调试时用 20，全流程联调用 40，比赛演示前改回 80。每次改速度参数后都要重新跑一遍离线回放，因为速度会影响机械臂的轨迹插补，同样一组坐标在不同速度下实际走的路径曲线是有差别的，尤其是转弯处会切弯。

从那以后我每次拿到一套新的机器人源码，都强制自己先写一个离线回放工具再碰真机，日志先行、可视化兜底，这套习惯帮我省下的调参时间比写工具的时间多出好几倍。希望帮到你。

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