DIY on 阿波的博客

做了一个能听懂人话的巡检机器人

Sat, 06 Sep 2025 23:22:26 +0800

总体实现框架图：

找了个实际的变电站环境，改动了一下导入到了Gazebo中用作实验的仿真环境

目前有了一个初步的demo，等做完后开源并对算法进行详细的讲解

Autoware中topic_state_monitor模块调研

Sun, 09 Mar 2025 21:39:42 +0800

一、topic_state_monitor模块简介

此节点用于监控任意话题是否存在异常，例如超时和低频率。话题的诊断结果将通过ROS Diagnostics发布诊断信息。

输入：任意名称、任意类型的话题
输出：/diagnostics 诊断信息

二、topic_state_monitor模块启动流程

通过解析 topic_state_monitor 核心代码部分并逐级向上追溯，可得到 topic_state_monitor 模块完整的启动流程，如下图所示：上层的两个 .launch.xml 文件主要配置了两个参数。一个是 tier4_system_component.launch.xml 中配置的 topics.yaml 文件路径。topics.yaml 文件内容是配置所有需要监控的话题信息以及监控阈值信息。配置文件的内容示例如下:

- module: control
 mode: [online, logging_simulation, planning_simulation]
 type: autonomous
 args:
 topic_type: autoware_control_msgs/msg/Control
 best_effort: false
 transient_local: false
 warn_rate: 5.0
 error_rate: 1.0
 timeout: 1.0
- module: control
 mode: [online, logging_simulation, planning_simulation]
 type: autonomous
 args:
 node_name_suffix: control_command_control_cmd
 topic: /control/command/control_cmd
 topic_type: autoware_control_msgs/msg/Control
 best_effort: false
 transient_local: false
 warn_rate: 5.0
 error_rate: 1.0
 timeout: 1.0
- module: localization
 mode: [online, logging_simulation, planning_simulation]
 type: autonomous
 args:
 node_name_suffix: transform_map_to_base_link
 topic: /tf
 frame_id: map
 child_frame_id: base_link
 best_effort: false
 transient_local: false
 warn_rate: 5.0
 error_rate: 1.0
 timeout: 1.0

代码2-1 topics.yaml配置⽂件⽰例

配置文件的各个参数的解释如下:

名称	含义
`module`	话题所属的模块
`mode`	指定了配置适用的模式，通过设置 `mode`，可以控制配置文件在不同运行环境下是否生效
`type`	指定话题的类型，根据不同的 `type` 分类，`component_state_monitor` 模块会分析 `/diagnostics` 诊断信息给出 `/type/module` 模块的状态（判断是否正常运行，消息类型为自定义的 `ModeChangeAvailable`）
`node_name_suffix`	节点名称的后缀，用于动态生成节点名称
`topic`	指定要监控的目标话题的名称
`topic_type`	指定目标话题的数据类型
`best_effort`	指定是否使用 "Best Effort" QoS策略。如果设置为 `true`，意味着使用“尽力而为”策略，即消息传输尽力而为，可能会丢失消息。设置为 `false` 则表示采用 "Reliable" QoS策略，确保消息的可靠传输，但可能带来更高的延迟。
`transient_local`	指定是否使用 "Transient Local" QoS策略。如果设置为 `true`，即使订阅者在发布时不存在，消息也会暂时保存在中间件中，直到订阅者连接。此策略适用于需要历史消息的场景。如果设置为 `false`，则采用普通的QoS策略，只有当前连接的订阅者可以接收到发布的消息。
`warn_rate`	话题发布频率警告阈值，当话题的发布频率低于该值时，话题状态将被设置为 WarnRate，即警告状态
`error_rate`	话题发布频率错误阈值，当指定当话题的发布频率低于该值时，话题状态将被设置为 ErrorRate，即错误状态
`timeout`	这个参数指定如果话题订阅在超过指定的时间（单位为秒）内没有收到消息，话题状态将被设置为 Timeout
`frame_id`	坐标变换的父坐标系ID
`child_frame_id`	坐标变换的子框架ID
`window_size`	计算目标主题频率使用的窗口大小，例如，窗口大小为10表示计算过去10个发布周期的频率
`uprate_rate`	定时器回调频率，用于设置更新和检查主题状态的频率

表2-1 topics.yaml配置⽂件参数解释

system.launch.xml 文件中设置了配置所选择的模式，根据模式的选择来监听不同的话题组。话题所属的模式在上述 topics.yaml 文件中设置。

<arg name="run_mode" default="online" description="options: online, logging_simulation, planning_simulation

最终通过 component_state_monitor.launch.py 生成了一系列针对目标话题的监控节点。这些节点发布诊断数据到 /diagnostics 话题中。

三、topic_state_monitor模块运行效果

运行 Autoware 的规划demo样例后：

cd autoware
source ~/autoware/install/setup.bash
ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit

然后运行 ros2 node list 可以看到 topic_state_monitor 模块运行起来一些监控节点来对配置文件中的话题进行监控。监控节点名称为：“topic_state_monitor” + topic.yaml 中话题的 node_name_suffix 名称。

这些节点输出诊断信息到 ROS Diagnostics 诊断系统中，可以在 rqt_runtime_monitor 中看到诊断结果。

同时，component_state_monitor::StateMonitor 节点的 main.cpp 订阅 /diagnostics 话题并通过定时器周期性地检查不同模块整体的状态是否正常。输出的话题格式是 /type/module，例如，对于 type 为 launch 中的 map 模块，输出话题为 /launch/map。

话题输出的消息格式是自定义的 ModeChangeAvailable.msg 格式，用于表示某个模块是否可用。消息包含以下内容:

stamp：时间戳，表示消息生成的时间。
available：布尔值，表示该模块当前是否有效。true 表示模块正常，false 表示模块不可用。

四、topic状态监控部分实现原理

分析源码后总结下来，其核心是通过 component_state_monitor.launch.py 对 config.yaml 文件中要监控的 topic 一对一创建一个 node。

随后将 node 启动部分注册成一个插件，这样可以用同一个源码启动多个 node。在 node 源码中，根据监控 topic 是否为 tf 话题创建 tf 类订阅者和非 tf 类订阅者。这里针对非 tf 类 topic，使用了 rclcpp::SerializedMessage 一个通用的消息类型，表示经过序列化的消息。它不关心消息的具体内容，只是将消息传递为原始的字节流。

五、一些考量点

topic_state_monitor 模块可针对任意类型的 topic 生成一个 node 去监控 topic 的状态，通用性强。但如果需要监控的 topic 过多可能进程开销大，因此要对该模块对于性能的开销进行测试来判断是否可迁移到人形机器人上。

topic_state_monitor 模块采用了组合节点并将组合节点放入 container 中来启动一堆 node 去监控各个 topic。这样做的好处是将多个节点加载到同一个进程中，每个节点仍然保持独立的逻辑和功能，但它们可以更高效地共享数据和资源。这种方法不仅减少了通信延迟，而且降低了系统的整体资源消耗（例如，CPU时间内存使用）。

实际使用 Autoware 进行测试时发现，输入 ros2 component list 查看容器节点信息可知组合节点 /system/component_state_monitor/component 有被放入 /system/component_state_monitor/container 容器中。

但是查询系统进程资源占用情况时发现：启动的各个监控 node 并没有在同一个进程中进行，而是各自占用了一个进程，这样性能总开销过大。

猜测是启动文件中 node 的启动方式存在问题。经过分析发现问题出在 component_state_monitor.launch.py 启动文件对于 node 节点的启动是通过启动一个个 launch.xml 文件来生成监控 node 节点，并没有将 node 节点放入 container 容器中，因此占用了多个进程。

想要减少进程的占用，需要重构 component_state_monitor.launch.py 对于 node 节点的启动部分。修改后的代码如下:

from collections import defaultdict
from pathlib import Path

import launch
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
import yaml

def create_diagnostic_name(row):
 return f"{row['module']}_topic_status"

def create_topic_monitor_name(row):
 diag_name = create_diagnostic_name(row)
 node_name_suffix = row["args"]["node_name_suffix"]
 # 替换非法字符，如冒号和空格为下划线(防止报错Couldn't parse remap rule: '-r __node......）
 node_name_suffix = node_name_suffix.replace(":", "_").replace(" ", "_")
 diag_name = diag_name.replace(":", "_").replace(" ", "_")
 return f"topic_state_monitor_{node_name_suffix}_{diag_name}"

def create_topic_monitor_node(row):
 diag_name = create_diagnostic_name(row)
 # 基础参数
 parameters = [{"diag_name": diag_name}]
 # 根据是否为 TF 类话题，添加不同的参数
 remappings = []
 if "topic_type" in row["args"]:
 # 非 TF 类话题
 parameters.append({"topic_type": row["args"]["topic_type"]})
 else:
 # TF 类话题
 parameters.append({"frame_id": row["args"]["frame_id"]})
 parameters.append({"child_frame_id": row["args"]["child_frame_id"]})
 # 其他参数
 for k, v in row["args"].items():
 if k not in ["topic_type", "frame_id", "child_frame_id"]:
 parameters.append({k: v})
 # 如果需要进行主题重映射，可以在这里添加 remappings
 # 假设 YAML 文件中不包含 remaps 字段，因此 remappings 为空
 # if "remaps" in row["args"]:
 # for remap in row["args"]["remaps"]:
 # remappings.append(remap)
 return ComposableNode(
 namespace="system",
 package="topic_state_monitor",
 plugin="topic_state_monitor::TopicStateMonitorNode",
 name=create_topic_monitor_name(row),
 parameters=parameters,
 remappings=remappings # 目前为空
 )

def launch_setup(context, *args, **kwargs):
 # 获取启动模式和配置文件
 mode = LaunchConfiguration("mode").perform(context)
 config_file = LaunchConfiguration("file").perform(context)
 rows = yaml.safe_load(Path(config_file).read_text())
 rows = [row for row in rows if mode in row["mode"]]
 # 创建所有监控节点
 topic_monitor_nodes = [create_topic_monitor_node(row) for row in rows]
 topic_monitor_names = [create_topic_monitor_name(row) for row in rows]
 # 组织参数
 topic_monitor_param = defaultdict(lambda: defaultdict(list))
 for row in rows:
 topic_monitor_param[row["type"]][row["module"]].append(create_topic_monitor_name(row))
 topic_monitor_param = {name: dict(module) for name, module in topic_monitor_param.items()}
 # 创建主组件节点（StateMonitor）
 component = ComposableNode(
 namespace="component_state_monitor",
 package="component_state_monitor",
 plugin="component_state_monitor::StateMonitor",
 name="component",
 parameters=[{"topic_monitor_names": topic_monitor_names}, topic_monitor_param],
 )
 # 创建容器，并将所有ComposableNode添加到容器中
 container = ComposableNodeContainer(
 namespace="component_state_monitor",
 name="container",
 package="rclcpp_components",
 executable="component_container",
 composable_node_descriptions=[component] + topic_monitor_nodes, # 将监控节点加入容器
 output="screen",
 )
 return [container]

def generate_launch_description():
 return launch.LaunchDescription(
 [
 DeclareLaunchArgument("file"),
 DeclareLaunchArgument("mode"),
 OpaqueFunction(function=launch_setup),
 ]
 )

代码修改后运行可发现监控 node 成功被放入 container 容器中在同一个进程中运行。

但是检查进程占用情况发现 component_state_monitor 进程总占用在 70% 左右，占用过高。修改参数列表中的 window_size 与 update_rate，将其从默认的 10 调小至 2 后，占用可下降至 15% 左右。经分析，TopicStateMonitor 的逻辑本身相对简单，只是维护一个时间戳队列、计算话题接收速率，并根据阈值进行状态判断。单独看这段代码，性能开销并不大，主要是一些基本的内存操作（push_back、pop_front）、时间计算和简单的条件判断。这些操作本身不是CPU占用的主要来源。

然而，当有 15个 这样的监控节点，每个节点都在高频率地接收消息并调用 update() 函数时，整体系统的CPU使用率就可能显著增加。由于 Autoware 系统繁杂臃肿，消息的频率不太好手动控制。为了控制变量研究消息频率与数量对监控模块占用系统资源的情况，于是手动创建一些节点，观察不同 topic 数量、频率与 QoS 策略下的系统资源占用情况。

测试编写脚本创建多个发布者发布 std_msgs/msg/String 格式的话题，然后启动 topic_state_monitor，其中 window_size 设为 2、update_rate 设为 1。之后对 topic_state_monitor 进程的 CPU 占用率进行监控 120秒，每秒测两次，将测试结果绘制成表格如下所示:

影响因素	Topic数量	Topic频率	best_effort	transient_local	平均占用率	最小占用率	最大占用率
	15	5	false	true	3.55%	2.0%	5.0%
	15	10	false	true	4.53%	3.0%	6.0%
	15	15	false	true	5.63%	3.0%	7.0%
	15	20	false	true	8.44%	4.0%	12.0%
	15	35	false	true	10.23%	4.0%	14.0%
	15	40	false	true	11.36%	8.0%	16.0%
	15	45	false	true	13.87%	10.0%	18.0%
	15	50	false	true	14.30%	10.0%	18.0%
QoS策略对占用率的影响	15	50	false	false	12.47%	8.0%	16.0%
	15	50	true	true	11.42%	8.0%	16.0%
	15	50	true	false	11.17%	6.0%	16.0%
Topic数量对占用率的影响	20	50	true	false	16.30%	10.0%	22.0%
	10	50	true	false	6.54%	4.0%	12.0%
	5	50	true	false	4.54%	2.0%	8.0%

将表格绘制成折线图如下所示： ![](/uploads/2025/03/topic频率对cpu占用影响-1024x718.png) ![](/uploads/2025/09/topic数量对cpu占用影响.png)

由表格数据可以得出结论：

Topic 数量与频率的增加会增加 topic_state_monitor 模块对系统的占用。如果以 15% 的占用率为界限定义为对 CPU 占用较少、为可用状态的话，那么 15个50Hz的Topic以下 或者 更多的更低Hz的Topic、更少的更高Hz的Topic 为此监控系统工作的最佳状态。
同时，QoS 策略也会对系统占用产生影响。设置 best_effort 为 true，即不使用 reliable 可靠性传输策略；设置 transient_local 为 false，即不使用 Volatile 持久性 QoS 策略。这样可以牺牲一点消息传输的可靠性来换取更低的资源占用。

小结

topic_state_monitor 模块可针对任意类型的 topic 生成一个 node 去监控 topic 的状态，通用性强。但如果需要监控过多高频的 Topic 消息可能会导致进程开销大。因此 Autoware 中对进行监控的 topic 进行了模块化分类处理，根据不同的应用场景选择性地监控关键模块的关键话题。例如，定位模块着重关注 map 到 base_link 的 tf 转换是否正常，控制模块着重关注发布的 control_cmd 控制 topic 是否正常。

编译雷达驱动PCL库缺失问题

Fri, 08 Sep 2023 22:03:02 +0800

咸鱼淘了个一百多块的镭神雷达，看参数性能还挺不错，而且是TOF测距雷达，原价四五百呢。

雷达参数

之后将驱动包拷贝到工作空间的src目录下后进行编译，可以看到代码包的名称与cmake里project的名称都是lslidar_driver，因此在工作空间目录下输入colcon build –packages-select lslidar_driver编译驱动包。

雷达驱动文件包

但是编译的时候报了错，没有找到pcl_conversions的配置文件。

编译报错

我求助了一下万能的Claude，他让我用apt安装libpcl-dev这个包。

万能的Claude

但是我执行命令后发现我已经安装过这个包了，那为什么还是找不到啊。

接下来他又让我进行设置环境变量，甚至源码编译库文件等等操作，都没有解决这个问题。我后来发现缺失的是pcl_conversions 这个库，但是他却一直让我安装的是libpcl-dev。我又问了Claude这两个有什么区别，看起来libpcl-dev包含了pcl_conversions 。

于是我决定找一下pcl_conversions 的安装目录。可以看到pcl_conversions 并没有被apt安装。

Claude提示寻找安装目录

pcl_conversions 包确实未找到

于是执行apt安装libpcl-conversions-dev后，成功编译驱动包了。（虽然还是不知道为什么之前装了但是找不到）

sudo apt update
sudo apt install libpcl-conversions-dev

之后执行ros2 launch lslidar_driver lsn10_launch.py可启动雷达。打开rviz2，FixedFrame中输入雷达坐标系的名称，这个名称实在雷达驱动文件中的yaml文件中设置的

rviz2

驱动代码中的配置文件

之后点击Rviz 左下角的Add 按键，在弹出的窗口中点击By topic 选中/scan话题下的LaserScan 并点击OK。

rviz中添加LaserScan

成功添加LaserScan 后我们便可以在Rviz 中看到这样的雷达点云图像。

给旭日X3派适配千兆网卡

Wed, 09 Aug 2023 22:44:58 +0800

由于旭日X3派自带的网卡实在是太拉胯，我家的WIFI都扫不出来，只好外加一USB网卡，购买的是如下款式。

然后就是驱动的配置，如果想自己编译的话可以参考这个博客：体验极速——在旭日X3派上使用双频1300M USB无线网卡_1300m无线网卡实际速度_小玺玺的博客-CSDN博客，这里给出编译好的驱动下载连接：https://pan.baidu.com/s/1qhFyLxFiLhlTaKJfRXK4Ow 提取码：kuq1。

先将驱动文件在Mobaxterm里ssh登录后拷贝至旭日派中，这里我拷贝至了home目录下，然后再执行命令拷贝到系统驱动目录下。

sudo cp 88x2bu.ko /lib/modules/4.14.87/

之后使用cd /lib/modules/4.14.87/进入文件夹后输入以下指令对ko文件签名：

sudo hobot-sign-file 88x2bu.ko

然后输入以下两条指令加载驱动，这个时候可以看到网卡驱动的灯开始闪烁。

sudo /sbin/depmod -a 4.14.87

sudo insmod 88x2bu.ko

输入ifconfig后产看网卡名称。可以看到图中的“wlx200db0c78392”为我们新增的网卡名称。

如果想永久启用，输入sudo vim /etc/modules，在里面添加88x2bu即可。（一定要切忌在网卡驱动未正常启动的情况下永久启用）

然后输入以下指令扫描出最新的WIFI名称。

 sudo nmcli device wifi rescan # 扫描wifi网络

sudo nmcli device wifi list # 列出找到的wifi网络

最后输入 sudo nmcli dev wifi connect “wifi名称” password “密码” ifname 网卡名称即可连接指定wifi，我的是。

sudo nmcli dev wifi connect "RM2100_F7BB" password "18*********" ifname wlx200db0c78392

巡检防疫机器人

Mon, 18 Apr 2022 20:24:00 +0800

去年大半年的时间借着参加各种工科竞赛的功夫做了一辆巡检机器人，通过搭建这款巡检机器人，边学边做有了一定的Python和C语言的编程基础，可以绘制一些初级的PCB电路板，从前端到后端、Linux系统的使用、服务器的搭建都有了一定的了解。接下来就主要介绍这辆巡检机器人的主要功能及实现过程。

巡检防疫机器人外观

功能

做这款机器人的起始时间正值新冠疫情刚开始，因此将这辆巡检机器人的应用背景定为防疫相关。智能防疫机器人可通过摄像头识别人脸是否正确佩戴口罩。机器人还搭载有消毒模块，可开启消毒模式实现定点喷杀消毒，同时搭的温湿度传感器、可燃气体传感器、空气质量传感器等，可将传感器数据上传至云端并在Web界面实时显示分析，若检测到异常数值可在钉钉群里通过钉钉机器人推送告警信息。

Web可视化界面1

Web可视化界面2

实现原理

系统硬件设计

本文设计的智能防疫机器人的控制计算单元使用了‘’大脑‘’+‘’小脑‘’的设计方案，底层的STM32控制板为低算力、低时延、运行实时操作系统的‘’小脑‘’，用于控制底层电机，采集传感器数据等；上层的树莓派为高算力、高时延、运行Liunx操作系统的大脑，用于感知、思考和决策。其硬件设计框图如图所示。

系统硬件框图

系统软件设计

软件设计分为四部分，一是树莓派端程序设计，二是底层STM32单片机程序设计，三是手机APP程序设计，四是云端服务器程序设计。

树莓派端程序设计

树莓派端程序主要包括巡检过程中的口罩识别代码、QT编写的UI界面部分、与服务器交互的部分。

巡检过程中的口罩识别由于要兼顾巡检、人脸识别与语音播报，程序运行负载较大，因此为了提升整体程序运行的流畅度尽量用上所有CPU的资源，因此采用多进程来实现这一功能，在不降低精度的情况下大大提升了运行速度，达到了实时性的要求，部分实现代码如下：

#定义传递图像队列和传递图像处理结果队列
q_frame = Queue()
q_respond = Queue()
q_car= Queue()
#采集摄像头进程：
get_camera_frame=Process(target=camera_frame_func, args=("获取摄像头图像", q_frame, mydict,q_car))
#处理图片进程：
proc_frame=Process(target=proc_frame_func, args=("处理图像", q_frame, q_respond, mydict))
#播报语音信息：
read_rst=Process(target=read_rst_func, args=("播报语音信息", q_respond,q_car))
机器人巡检进程：
carrun=Process(target=carrun_fun,args=("小车巡检", q_car))
# 启动任务
get_camera_frame.start()
proc_frame.start()
read_rst.start()
carrun.start()

第一个进程获取摄像头图像，并利用OpenCV中的训练好的人脸Haar特征分类级联器判断画面中是否存在人脸，若存在人脸则将人脸照片传至消息队列中。第二个进程通过消息队列取出人脸照片并上传至百度智能云平台，百度智能云平台可根据云端数据库中的信息匹配出人脸身份信息，并判断人脸是否正确佩戴口罩将判断结果返回，若没有正确佩戴口罩则进程三启动播出提示语音，并停止巡检进程，向底层运动控制系统发出停止指令。正确佩戴口罩后恢复巡检进程。

巡检系统流程图

机器人上层有一五寸可触摸电容屏，界面如图所示。该多媒体屏幕可进行一定的人机交互与多媒体宣传功能。该界面采用QT编写（代码已申请软著），天气查询通过调用API接口实现，词条查询通过爬取百度百科实现。当按下开启消毒模式时，树莓派通过串口向底层单片机发送开启指令。

QT界面

控制模块总按钮位于上位机软件主界面的左下方的“手动控制”，点击后即可进入手动控制界面

手动控制界面

当点击“播放音乐”按钮后，会弹出音乐播放器界面，可点击曲库中的音乐进行播放，左下角可切换“单曲循环”、“顺序播放”、“随机播放”模式。若想要播放曲库中没有的音乐，可对防疫机器人说出像要播放的曲名，机器人会联网下载歌曲后进行播放。

音乐播放器

树莓派通过串口接收到底层单片机采集到的各种数据通过MQTT协议将其传至阿里云服务器，也可接收服务器下发的指令来远程控制机器人。部分代码如下：

def Alink(params):
 AlinkJson = {}
 AlinkJson["id"] = random.randint(0,999999)
 AlinkJson["version"] = "1.0"
 AlinkJson["params"] = params
AlinkJson["method"]="thing.event.property.post"
return json.dumps(AlinkJson)
# 消息回调（云端下发消息的回调函数）
def on_message(client, userdata, msg):
 print(msg.payload)
 if(SET==msg.topic):
 Msg = json.loads(msg.payload)
 switch = Msg['params']['PowerLed']
 rpi.powerLed(switch)
 print(msg.payload) # 开关值

#连接回调（与阿里云建立链接后的回调函数）
def on_connect(client, userdata, flags, rc):
 pass
# 构建与云端模型一致的消息结构
updateMsn = {
'natural_gas':natural_gas,
'fired_gas':fired_gas,
'Harmful_gas':harmful_gas
}
JsonUpdataMsn = aliLink.Alink(updateMsn)
mqtt.push(POST,JsonUpdataMsn) # 定时向阿里云IOT推送我们构建好的Alink协议数据

底层单片机程序设计

底层STM32单片机程序分为主初始化程序、主程序、通讯中断子程序、功能模块子程序、PID电机控制子程序等。通讯中断子程序主要利用串口与树莓派进行通信，功能模块子程序为消毒喷洒装置控制函数、测温函数、LCD屏幕显示函数等。电机子程序内容为根据编码电机上的编码器返回的电机速度利用PID算法实时通过输出PWM波来控制电机的速度，从而保证机器人行进的稳定。

手机APP程序设计

手机APP程序采用Android studio软件开发和编译，分为主控界面、数据显示界面、模式选择界面等。进入控制界面后，可在此界面控制机器人的前进、后退、左转、右转、旋转以及摄像头云台的俯仰与旋转。还可控制防疫机器人进入巡检模式、消毒模式、以及选择人员进行精确测温等。同时可在此界面查看机器人传回的实时画面。数据显示界面可查看机器人检测到的各类传感器数据的数值、拍摄到的未正确佩戴口罩人脸的照片、体温检测异常的人的身份等。

APP端对机器人的控制可选择不需要联网、时延低、控制距离有限的蓝牙控制，也可选择需要联网、时延相对较高、控制距离理论上无限的远程控制。当选择蓝牙控制模式时，手机蓝牙与防疫机器人身上的蓝牙相连来下发控制指令。当选择远程控制模式时，安卓APP通过MQTT协议与服务器建立连接，通过安卓APP将控制指令先发送给云端服务器，然后服务器再将数据发送给树莓派，从而达到对机器人远程控制的目的。

APP控制系统流程图

实现效果

卧龙凤雏的电磁炮

Thu, 17 Mar 2022 23:41:00 +0800

团队介绍

首先贴出卧龙凤雏团队与电磁炮的合影

最中间是总策划、酷爱军事、从小就造飞机的“曹工”，主要负责坦克车体外观及机械结构及PCB电路板的设计；左边的是我，负责机器人顶层控制逻辑中的视觉识别与追踪；右边是负责车体底层控制的“陈工”。

制作过程

外形设计

底盘设计

机器人底盘长788.45mm，宽308.00mm，高153.51mm，悬挂采用的是克里斯蒂悬挂系统，即一种拥有大直径负重轮，使用螺旋弹簧的独立式悬挂装置。这种悬挂均是由前后两个互相连接的圆柱形螺旋弹簧构成。位于前方的为可调式水平螺旋弹簧，后方的则是垂直螺旋弹簧，这种设计有更长的避震行程，可强化越野性能。

机器人底盘侧视图

“曹工”组装车体

组装完成

磁加速系统设计

磁加速系统实际模型

炮台底座也使用木制材料，中间开有41mm*174mm的矩形孔为炮位提供移动的空间，整个炮塔通过螺栓固定在履带底盘上部的旋转滑台上。磁加速系统的加速段整体用木板封装起来以此来达到绝缘的效果；为了防止热量堆积在炮管支架的木板上每一级加速线圈周围都开有方形空用来散热；

磁加速系统炮管

电磁加速系统为磁阻式电磁加速系统，采用五级线圈加速，光电门进行击发，电解电容储能，发射电压为400V。第一级电容容量为2000μF，第二级电容容量为1880μF，第三级电容容量为1880μF，第四级电容容量为1440μF，第五级电容容量为1440μF，这样的电容容量设计是为了减少炮弹在加速的过程中所受到的反拉

磁加速系统PCB

电容储能系统实物

该磁加速系统配备有自动装弹机，装弹机安装在炮管的尾部并随炮管运动可以实现任意角度装填，装填机构通过丝杆推动炮弹并将其送入炮膛，丝杆由四线两相步进电机提供动力，在装弹机滑台的两侧安装有两个限位开关来限制推弹杆的移动空间。

自动装弹机示意图

经过建立数学模型进行计算并通过测试，该磁加速系统可以将我们所设计的炮弹加速至45m/s左右。

炮弹实物

对炮弹的空气动力模拟计算图示

电路设计

遥控器原理图

遥控器PCB

底层控制板原理图

底层控制板PCB

上路测试

开着出去，抬着回来

软件部分

由于后来想要拿此机器人参加一些比赛，所以要为电磁炮想出应用一些民用价值。考虑到现在楼层建筑越来越高，若发生火灾消防部队扑救的难度很大，如果电磁炮可以对火源发出灭火弹则可迅速阻止火势扩大，因此就以“基于磁加速系统的高层建筑灭火机器人”为背景继续完善机器人。

初步方案是用树莓派进行火焰识别，并用舵机云台进行火焰追踪，将角度数据回传给底层STM32控制端后进行炮塔对火焰的追踪，最后再根据利用炮口的激光测距仪测出的炮口与火焰的距离进行炮弹射速的调整。

实现原理图

初步实现效果演示

优化改进

由于树莓派识别火焰的速率实在不敢恭维，于是家中有矿的”曹工“斥一顿早饭钱买了一块英伟达Jetson Nano边缘计算模块，Jetson nano与树莓派相比，NVIDIA Jetson Nano包含性能更高，功能更强大的GPU——NVIDIA Jetson Nano中具有128个CUDA核心的NVIDIA Maxwell GPU。NVIDIA Jetson Nano中更强大的GPU可以为图形处理，甚至人工智能（AI）和机器学习（ML）提供更强大的功能。

Jetson Nano

火焰识别算法采用的是NanoDet目标检测模型，NanoDet 是一个速度超快和轻量级的移动端目标检测模型，非常适合嵌入式部署。将在PC端训练好的模型移植到Jetson Nano边缘计算平台上即可进行火焰识别。识别到火焰后，根据火焰在视角中的位置，摄像头云台利用PID算法进行追踪。Jetson Nano通过PCA9655 舵机驱动板驱动摄像头云台舵机。

PID算法代码如下

# *****************************************************************#
# 增量式PID系统 #
# *****************************************************************#
class IncrementalPID:
 def __init__(self, P, I, D):
 self.Kp = P
 self.Ki = I
 self.Kd = D

 self.PIDOutput = 0.0 # PID控制器输出
 self.SystemOutput = 0.0 # 系统输出值
 self.LastSystemOutput = 0.0 # 上次系统输出值

 self.Error = 0.0 # 输出值与输入值的偏差
 self.LastError = 0.0
 self.LastLastError = 0.0

 # 设置PID控制器参数
 def SetStepSignal(self, StepSignal):
 self.Error = StepSignal - self.SystemOutput
 IncrementValue = self.Kp * (self.Error - self.LastError) + \
 self.Ki * self.Error + \
 self.Kd * (self.Error - 2 * self.LastError + self.LastLastError)

 self.PIDOutput += IncrementValue
 self.LastLastError = self.LastError
 self.LastError = self.Error

 # 设置一阶惯性环节系统 其中InertiaTime为惯性时间常数
 def SetInertiaTime(self, InertiaTime, SampleTime):
 self.SystemOutput = (InertiaTime * self.LastSystemOutput + \
 SampleTime * self.PIDOutput) / (SampleTime + InertiaTime)

 self.LastSystemOutput = self.SystemOutput

火焰追踪主函数代码如下

if __name__ == '__main__':
 pwm = PCA9685(0x40, debug=True)
 pwm.setPWMFreq(50)
 pwm.setServoPulse(0, 1500)
 pwm.setServoPulse(1, 1500)
 torch.backends.cudnn.enabled = True
 torch.backends.cudnn.benchmark = True
 load_config(cfg, config_path)
 logger = Logger(-1, use_tensorboard=False)
 predictor = Predictor(cfg, model_path, logger, device='cuda:0')
 logger.log('Press "Esc" to exit.')
 camera = USBCamera(width=320, height=240, capture_fps=30)
 camera.running = True
 while True:
 frame = camera.value
 # ret_val, frame = cap.read()
 # if ret_val == False:
 # continue #skip if capture fail
 meta, res = predictor.inference(frame)
 predictor.visualize(res, meta, cfg.class_names, 0.35)
 print('x,y', x, y)
 if (x != 0 and y != 0):
 # 输入X轴方向参数PID控制输入
 xservo_pid.SystemOutput = x
 xservo_pid.SetStepSignal(160)
 xservo_pid.SetInertiaTime(0.01, 0.1)
 target_valuex = int(1500 + xservo_pid.SystemOutput)
 # 输入Y轴方向参数PID控制输入
 yservo_pid.SystemOutput = y
 yservo_pid.SetStepSignal(120)
 yservo_pid.SetInertiaTime(0.01, 0.1)
 print('yservo_pid.SystemOutput', yservo_pid.SystemOutput)
 target_valuey = int(1500 - yservo_pid.SystemOutput)
 if target_valuey < 820:
 target_valuey = 820
 if target_valuey > 2500:
 target_valuey = 2500
 if target_valuey > 2500:
 target_valuex = 2500
 if target_valuex < 10:
 target_valuex = 10
 pwm.setServoPulse(0, target_valuex)
 pwm.setServoPulse(1, target_valuey)
 print('valuex,valuey', target_valuex, target_valuey)

火焰追踪效果如下

Jetson Nano火焰追踪效果