原文发表于 CSDN（作者：hjz0810），发布日期 2025-11-16。本文为转载备份，已征得作者同意。

华北五省的服务机器人赛道...
上篇
1.机器人的搭建及用料及机器人逻辑
雏形构思
导航逻辑处理
串口发送逻辑
此机器人应以小车为地盘，具备基本的前进后退和转向的能力，高度不能低于0.8米，因为从桌子上取东西，最少要比桌子高，那么可以采用角钢或铝型材进行加高处理。
铝型材买欧标2020的40CM的最佳，铝型材一定要一次买够因为每次的运费都相当贵，配套的脚码和螺丝要多要几套，以下为反例
由于买两次多花了30运费
2020的细长，重量较轻，可以减少轮上负担，40CM是因为货架的梯度是30，60，90，120CM，40CM的铝型材加上小车底盘刚好能够到60CM的货架一档，但是我们不能忽略薯片，饮料自身是有一定高度的，因此，选用电动推杆，其自身长40余CM，可选配其他长度的伸缩长，放在小车底部刚好抵住顶部亚克力板，
比如现在桌子高度0.8m，货架选60CM和90CM，那么就选用30CM的推杆，加上机械臂自身云台的高度刚好到90CM，
电动推杆的工作原理也很简单，在12伏工作电压下，通电即伸长，交换正负极就会降回去，买的时候一定要选带自锁的，因为我们机械臂本身以及要夹的东西是有自身重量的，约三公斤左右，不带自锁会受重力作用降会原长，
如图所示，推杆可选推力和速度，成反比关系，即选配的速度越快，推力越小，推力的单位为N，
根据一些高中知识，去掉个0就是千克
比如7mm速度对应的1000N就是能推动100千克的东西，当然我们不需要这么多，可以选用48-60速度的，价格不变
使用时通过速度计算需要通电的时长，比如速度60mm需要上升30厘米，通电时长为5秒，记得转换电位，单片机一般是毫秒，如果下降30厘米就把正负极交换，同样通电5秒
按照赛制，实际比赛过程中我们不能触碰机器人，也就不能手动去更换正负极，在此处分享我们的经验:1.电动推杆本身有一红一黑(推杆不分正负，用红黑好理解)，每根线各延出两根，现在就变成了两红两黑，分别记为红1，红2，黑1，黑2，电池同理，变为两正两负，分别记为正1，正2，负1，负2，按照以下方法连线，
正1，继电器，红1，推杆，黑1，负1
正2，继电器，黑2，推杆，红2，负2
这样，只需要一个电源，一个控制器就能实现简易的正负极变换
通过继电器开关时间计算上升距离，就能实现推动，拉回，急停。
电动推杆由于其质量大，耗电高，因此不推荐买四根放在四个角，而买一根放在中间推有会因为重心分散而摇晃甚至倾覆，解决这个问题的方法可以参照以下经验:
因为我们购买的是2020的铝型材，仔细观察发现铝型材中间是有一个5毫米的小孔的，只要定制和其直径一样的钢管，插入铝型材中，就可以使重心通过钢管传递到底盘，从而得到平衡。
亚克力板根据铝型材长度进行调整，推荐大一号，比如4545，因为如果你是新手拼接铝型材极有可能拼出一个长方体而不是正方体，而拼出来的长方体长不是42就是44，因此保底起见推荐买4545的亚克力板，最少需要4块，厚度不能低于4mm，否则承重时形变很大
可以选黑色或透明，但一定要有透明板，方便在安装推杆时找中心，平衡位置
该赛道没有对芯片的限制，几乎可以用到全部的单片机，如树莓派，泰山派，甚至是mini主机
这里我对各硬件间的配合和逻辑进行简单阐述
底盘部分
底盘采用立创·天空星(STM32F407)来控制，采取简单的串口控制策略，
即接收到0123分别代表前进，后退，左右转，
通过TB6612驱动四个轮子，每次前进量为10厘米，每次转向角为30度，
接收7，8时启动对电动推杆的上下控制，这里的控制可以直接用TB6612,如果没有则可选用我在上方提到的方法。
核心代码:比赛未开始，暂不向外公布（比赛已结束，代码见尾页）
机械臂部分
机械臂价格一般比较昂贵，可以去咸鱼找一些二手的，也可以买舵机和材料自己拼装，这里我们用的是亚博机械臂，采用Arduino控制，可连接蓝牙控制，
但注意，大部分便宜的机械臂标配舵机力量都较小，我们这个20千克舵机实测不能举起一瓶矿泉水，因为根据高中知识，抓取时形成的的是费力杠杆效应，所以推荐买更高级的机械臂
如果手中依然拮据，我们还有第二解决方案，即从问题的角度解决问题，既然形成的是费力杠杆，那修改杠杆长度，就可以提高效率。
因此，可以将六轴机械臂拆除中间部分，只留下云台转向轴，上下抬起轴和抓取轴，这样就能轻松举起矿泉水这种“重物”。
如果能正对目标物，则机械臂可以无需调整直接夹取
当Arduino通过串口收到25，机械臂夹取，收到90，机械臂张开。
由于比赛官方临时决定增加难度(说实话我就不该问)
原本是通过K230识别物品的商标，比如乐事的商标这种不会太大变形的目标，这样能判断出距离，驱动底盘抵近夹取，
然而朝向随机后，商标的位置就随机了，k230不仅识别能力会大大降低，判断距离的能力也直接失去了，
因此，我们临时给机械臂加上了超声波测距，用k230驱动底盘使物品位于画面中心，通过串口2发送25激活超声波，此时物品必然正对超声波，当面前物品达到设定阈值距离，机械臂闭合夹取物体，并通过串口发回52给K230，k230收到后通过串口1转发25给底盘，底盘继续运动，当到达B点后机械臂收到90松爪，继续下一步动作。
核心代码:比赛未开始，暂不向外公布（比赛已结束，代码见尾页）
语音模块
机器人与外接交互主要通过语音，因为比赛开始人就不能触碰机器人了。
1.启动跟随:串口发送start，建图和导航模块确认当前位置为起点，开始接受串口其他信息
2.这里是桌子:串口发送desk，建图和导航模块以当前位置标记为A点。
3.这里是货架:串口发送shelf，建图和导航模块以当前位置标记为B点。
4.请帮我收纳XXX到第x层:串口发送对应指令，如FD1，代表收纳芬达到第一层，LS2代表收纳芬达到第二层。
5.延迟10秒后串口发送“Keep”，导航和建图模块以当前位置为C点，开始执行后续命令
核心代码:比赛未开始，暂不向外公布（比赛已结束，代码见尾页）
视觉模块
采用立创·庐山派(堪智K230)➕3.3寸立创屏幕，
用Make Sense对各类物品的照片进行目标标注，标注完用堪智AIcube进行Ai训练，
然后部署在K230上，同时加入领航员的标注信息，如特定衣服上的特定标识，使机器人只跟随一个人，不会被场地中其他复杂环境干扰，通过判断识别框的面积占整个画面的大小判断距离，对待每个目标，识别框有大有小，需要单独调试每个目标的判断逻辑。
雷达与建图
采用激光雷达进行建图，需要Ubuntu+Ros系统，
激光雷达采用串口连接到Ubuntu系统上，而Ubuntu运行在小主机上，
这里以思岚A1为例，具体步骤是先安装Ubuntu，再配置镜像源安装ros2，然后再安装官方依赖，最后运行雷达建图，但是此操作复杂而繁琐，因此我直接封装好了一个镜像，供大家参考使用，具体教程见下篇
在雷达建图过程中，如果雷达方向发生改变，如果没有给雷达输入坐标变换，会导致TF变换错误，建的图会直接重叠或乱序，而能实现这一功能要么安装官方教程购买旭日派x3小车底盘(约2000元)，
或者购买官方的手持建图工具(当然比赛肯定用不到)
这两种都需要较高花费，无论是比赛亦或是学习都有较高的经济压力，因此，我从底层开始编写了一款软件，可以直接运行在安装好的系统中，可以不用输入繁琐的代码直接打开雷达进行建图
并且根据比赛设定，我又写了一款可以建图后自主导航，将语音，视觉，底盘，方向，机械臂，雷达数据总和到一起，进行底盘的操控导航，视觉跟随，路线记录，定点夹取，共万余行代码，附在下篇中
下篇
视觉部分
首先通过以下链接下载图片素材和训练工具，自己拍照也可以。
数据标注
1.视觉模块训练文件（以及完成数据标注）：下载链接
如果是自己拍图片，打开make sense进行数据标注，点左下角“Get Started”，将需要标注的图片全部拖入，选左边的“Object Detection（目标检测）”，进入后先添加标签，点左上加号添加如
点“start project”，标出目标位置，右上角选择标签，一直到标完，点击“action”，选择“Export Annotations”
随后可以选vnc还是yolo，推荐yolo
解压标注文件，将原来的图片文件夹命名为“Annotations”，再新建一个“JPEGImages”文件夹，将解压后的标注文件复制至此。再新建txt文件名为“labels.txt“，将之前的标签自己写进去，比如可乐（换行）奥利奥（换行）芬达（换行）....
模型训练
2.视觉模块训练方式1：AIcube
下载后解压到任意文件夹，点aicube.exe即可启动，点"新建项目'—“目标检测”——数据文件选刚才的整个问价夹，
进入后检查标签是否正确，点拆分，按8：1：1拆分就行（训练集：测试及：验证集），
随后进入训练界面，直接点开始训练即可（训练完成会自动结束）这时候去评估页面导出即可。
进入到导出问价夹，进入到\deployment_source\mp_deployment_source，将里面的所有内容复制到K230的/scard文件夹，然后打开det_video.py（用canMVide），点工具，再点第三个选项（save open ...as main.py）。随后给K230通电，即可完成视觉识别
视觉模块训练方式2：在线AI模型训练 - 勘智开发者社区，适用于电脑性能有限或没有独立显卡
点“数据集”，点创建数据集，创建完成点设置小符号，导入需要标注的图片，右上角添加标签。
标注完成点击训练：
训练完成在训练记录中下载。
.视觉模块训练完成模型：链接
完成代码
这段代码中是人物识别+颜色识别（可替换Ai模型），还包含了部分串口发送逻辑，比如，当识别到的人物大小下雨画面的85%，串口2发送0，同样，左右出现便宜对应发送02，3，当底盘到达指定位置，通过串口三收到指令会停止人物识别，转而进行目标检测，调整目标在屏幕中心位置，便于后续的夹取，语音模块会将接收的夹取物品转成串口发送至K230，K230通过AI模型+lab色值双重检测方法确保检测准确。
以下是华北五省在常规光照下的机器视觉lab阈值：
1.奥利奥
(33, 89, -17, 3, -5, 19)
2.饼干
(58, 93, -12, 1, -2, 10)
3.芬达
(28, 96, -21, 3, -9, 21)
4.薯片
(39, 100, -9, 6, -3, 14)
5.可乐
(30, 90, -14, 8, -13, 16)
6.乐事
(19, 93, -12, 17, -4, 18)
7.矿泉水
(32, 100, -14, 7, -8, 25)
8.雪碧
(31, 89, -14, 5, -7, 15)
9.洗手液
(57, 97, -4, 21, -8, 29)
cs
成品代码：
import gc
import os
import aicube
import image
import nncase_runtime as nn
import ujson
import ulab.numpy as np
from libs.PipeLine import ScopedTiming
from libs.Utils import *
from media.display import *
from media.media import *
from media.sensor import *
display_mode = "hdmi"
if display_mode == "lcd":
DISPLAY_WIDTH = ALIGN_UP(800, 16)
DISPLAY_HEIGHT = 480
else:
DISPLAY_WIDTH = ALIGN_UP(1920, 16)
DISPLAY_HEIGHT = 1080
OUT_RGB888P_WIDTH = ALIGN_UP(640, 16)
OUT_RGB888P_HEIGH = 360
root_path = "/sdcard/mp_deployment_source/"
config_path = root_path + "deploy_config.json"
deploy_conf = {}
debug_mode = 1
def two_side_pad_param(input_size, output_size):
ratio_w = output_size[0] / input_size[0] # 宽度缩放比例
ratio_h = output_size[1] / input_size[1] # 高度缩放比例
ratio = min(ratio_w, ratio_h) # 取较小的缩放比例
new_w = int(ratio * input_size[0]) # 新宽度
new_h = int(ratio * input_size[1]) # 新高度
dw = (output_size[0] - new_w) / 2 # 宽度差
dh = (output_size[1] - new_h) / 2 # 高度差
top = int(round(dh - 0.1))
bottom = int(round(dh + 0.1))
left = int(round(dw - 0.1))
right = int(round(dw - 0.1))
return top, bottom, left, right, ratio
def read_deploy_config(config_path):

打开JSON文件以进行读取deploy_config

with open(config_path, "r") as json_file:
try:

从文件中加载JSON数据

config = ujson.load(json_file)
except ValueError as e:
print("JSON 解析错误:", e)
return config
def detection():
print("det_infer start")

使用json读取内容初始化部署变量

deploy_conf = read_deploy_config(config_path)
kmodel_name = deploy_conf["kmodel_path"]
labels = deploy_conf["categories"]
confidence_threshold = deploy_conf["confidence_threshold"]
nms_threshold = deploy_conf["nms_threshold"]
img_size = deploy_conf["img_size"]
num_classes = deploy_conf["num_classes"]
color_four = get_colors(num_classes)
nms_option = deploy_conf["nms_option"]
model_type = deploy_conf["model_type"]
if model_type == "AnchorBaseDet":
anchors = deploy_conf["anchors"][0] + deploy_conf["anchors"][1] + deploy_conf["anchors"][2]
kmodel_frame_size = img_size
frame_size = [OUT_RGB888P_WIDTH, OUT_RGB888P_HEIGH]
strides = [8, 16, 32]

计算padding值

top, bottom, left, right, ratio = two_side_pad_param(frame_size, kmodel_frame_size)

初始化kpu

kpu = nn.kpu()
kpu.load_kmodel(root_path + kmodel_name)

初始化ai2d

ai2d = nn.ai2d()
ai2d.set_dtype(nn.ai2d_format.NCHW_FMT, nn.ai2d_format.NCHW_FMT, np.uint8, np.uint8)
ai2d.set_pad_param(True, [0, 0, 0, 0, top, bottom, left, right], 0, [114, 114, 114])
ai2d.set_resize_param(True, nn.interp_method.tf_bilinear, nn.interp_mode.half_pixel)
ai2d_builder = ai2d.build(
[1, 3, OUT_RGB888P_HEIGH, OUT_RGB888P_WIDTH], [1, 3, kmodel_frame_size[1], kmodel_frame_size[0]]
)

初始化并配置sensor

sensor = Sensor()
sensor.reset()

设置镜像

sensor.set_hmirror(False)

设置翻转

sensor.set_vflip(False)

通道0直接给到显示VO，格式为YUV420

sensor.set_framesize(width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT)
sensor.set_pixformat(PIXEL_FORMAT_YUV_SEMIPLANAR_420)

通道2给到AI做算法处理，格式为RGB888

sensor.set_framesize(width=OUT_RGB888P_WIDTH, height=OUT_RGB888P_HEIGH, chn=CAM_CHN_ID_2)
sensor.set_pixformat(PIXEL_FORMAT_RGB_888_PLANAR, chn=CAM_CHN_ID_2)

绑定通道0的输出到vo

sensor_bind_info = sensor.bind_info(x=0, y=0, chn=CAM_CHN_ID_0)
Display.bind_layer(**sensor_bind_info, layer=Display.LAYER_VIDEO1)
if display_mode == "lcd":

设置为ST7701显示，默认800x480

Display.init(Display.ST7701, to_ide=True)
else:

设置为LT9611显示，默认1920x1080

Display.init(Display.LT9611, to_ide=True)

创建OSD图像

osd_img = image.Image(DISPLAY_WIDTH, DISPLAY_HEIGHT, image.ARGB8888)

media初始化

MediaManager.init()

启动sensor

sensor.run()
rgb888p_img = None
ai2d_input_tensor = None
data = np.ones((1, 3, kmodel_frame_size[1], kmodel_frame_size[0]), dtype=np.uint8)
ai2d_output_tensor = nn.from_numpy(data)
while True:
with ScopedTiming("total", debug_mode > 0):
rgb888p_img = sensor.snapshot(chn=CAM_CHN_ID_2)
if rgb888p_img.format() == image.RGBP888:
ai2d_input = rgb888p_img.to_numpy_ref()
ai2d_input_tensor = nn.from_numpy(ai2d_input)

使用ai2d进行预处理

ai2d_builder.run(ai2d_input_tensor, ai2d_output_tensor)

设置模型输入

kpu.set_input_tensor(0, ai2d_output_tensor)

模型推理

kpu.run()

获取模型输出

results = []
for i in range(kpu.outputs_size()):
out_data = kpu.get_output_tensor(i)
result = out_data.to_numpy()
result = result.reshape((result.shape[0] * result.shape[1] * result.shape[2] * result.shape[3]))
del out_data
results.append(result)

使用aicube模块封装的接口进行后处理

det_boxes = aicube.anchorbasedet_post_process(
results[0],
results[1],
results[2],
kmodel_frame_size,
frame_size,
strides,
num_classes,
confidence_threshold,
nms_threshold,
anchors,
nms_option,
)

绘制结果

osd_img.clear()
if det_boxes:
for det_boxe in det_boxes:
x1, y1, x2, y2 = det_boxe[2], det_boxe[3], det_boxe[4], det_boxe[5]
x = int(x1 * DISPLAY_WIDTH // OUT_RGB888P_WIDTH)
y = int(y1 * DISPLAY_HEIGHT // OUT_RGB888P_HEIGH)
w = int((x2 - x1) * DISPLAY_WIDTH // OUT_RGB888P_WIDTH)
h = int((y2 - y1) * DISPLAY_HEIGHT // OUT_RGB888P_HEIGH)
osd_img.draw_rectangle(x, y, w, h, color=color_four[det_boxe[0]][1:])
text = labels[det_boxe[0]] + " " + str(round(det_boxe[1], 2))
osd_img.draw_string_advanced(x, y - 40, 32, text, color=color_four[det_boxe[0]][1:])
Display.show_image(osd_img, 0, 0, Display.LAYER_OSD3)
gc.collect()
rgb888p_img = None
del ai2d_input_tensor
del ai2d_output_tensor

停止摄像头输出

sensor.stop()

去初始化显示设备

Display.deinit()

释放媒体缓冲区

MediaManager.deinit()
gc.collect()
time.sleep(1)
nn.shrink_memory_pool()
print("det_infer end")
return 0
if name == "main":
detection()
python
适配底盘的行走逻辑
加入了串口输入输出控制的集成视觉代码：
from libs.PipeLine import PipeLine
from libs.AIBase import AIBase
from libs.AI2D import Ai2d
import os
import ujson
from media.media import *
from time import *
import nncase_runtime as nn
import ulab.numpy as np
import time
import utime
import image
import random
import gc
import sys
import aicube
from media.display import *
from machine import UART, FPIOA, Timer

颜色检测类

class ColorDetection:
def init(self, display_size):
self.display_size = display_size

目标颜色的LAB阈值

self.target_colors = {
"ala1": (33, 89, -17, 3, -5, 19),
"ala2": (33, 89, -17, 3, -5, 19),
"bg1": (58, 93, -12, 1, -2, 10),
"bg2": (58, 93, -12, 1, -2, 10),
"fd1": (28, 96, -21, 3, -9, 21),
"fd2": (28, 96, -21, 3, -9, 21),
"sp1": (39, 100, -9, 6, -3, 14),
"sp2": (39, 100, -9, 6, -3, 14),
"kl1": (30, 90, -14, 8, -13, 16),
"kl2": (30, 90, -14, 8, -13, 16),
"ls1": (19, 93, -12, 17, -4, 18),
"ls2": (19, 93, -12, 17, -4, 18),
"s1": (32, 100, -14, 7, -8, 25),
"s2": (32, 100, -14, 7, -8, 25),
"xb1": (31, 89, -14, 5, -7, 15),
"xb2": (31, 89, -14, 5, -7, 15),
"xsy1": (57, 97, -4, 21, -8, 29),
"xsy2": (57, 97, -4, 21, -8, 29)
}
self.current_color_target = None
self.center_threshold = 0.1
self.min_blob_area = 500
def set_color_target(self, command):
if command in self.target_colors:
self.current_color_target = command
return True
return False
def clear_color_target(self):
return False

text

def clear_color_target(self):
    self.current_color_target = None