[期末设计] 基于LEGO EV3的巡线及物料搬运机器人系统设计与实现

# [期末设计] 基于LEGO EV3的巡线及物料搬运机器人系统设计与实现

## 摘要

本项目设计并实现了一款基于LEGO MINDSTORMS EV3平台的智能寻迹与物料搬运机器人。针对复杂赛道中的多岔路口识别、动态寻迹以及特定物料的抓取需求，本系统在底层运动控制中引入了基于离散时间系统的改进型比例-微分（PD）控制算法，结合差速驱动运动学模型，实现了机器人平滑且自适应的轨迹跟踪。在宏观路径规划与任务执行中，构建了基于布尔逻辑与有限状态机（FSM）的确定性决策树模型，解决了"传感器致盲"、"路口误判"以及"假阳性终点"等典型工程难题。本文详细阐述了该系统的控制理论基础、算法数学推导、软硬件协同设计，并结合控制论历史与相关实证研究，对比了不同控制器在直线、椭圆以及8字形复杂轨迹下的性能表现。同时，针对嵌入式系统物理特性（如CPU饥饿、PWM控制响应）提出了底层算力优化方案$^{[1]}$。

## 1. 引言

在移动机器人领域，路径跟踪（Line Tracking）与自主导航是实现复杂任务的基础，其核心在于设计一种能够应对动态环境与硬件约束的可靠控制器。在早期的自动控制发展中，简单的开关控制（Bang-Bang或ON-OFF控制）常被采用，但这种"非黑即白"的控制逻辑往往导致机器人运行轨迹震荡严重、机械损耗大且极易在高速下脱轨。由于双轮差速机器人属于典型的非完整约束（Non-holonomic constraint）系统，其横向速度恒为零，只能通过偏航角速度来调整横向偏差，因此对转向指令的平滑度与精确度提出了极高要求$^{[1]}$。

为提升系统的鲁棒性与动态响应能力，本项目引入了工业界广泛应用的PID（Proportional-Integral-Derivative）闭环控制算法$^{[2]}$。PID控制理论的历史可以追溯到1890年代的调速器设计，并在1922年由俄裔美国工程师尼古拉斯·米诺尔斯基（Nicolas Minorsky）在为美国海军设计船舶自动转向系统时首次进行了严格的数学理论分析$^{[2]}$。米诺尔斯基通过观察舵手的动作，发现优秀的控制不仅需要依据当前的误差，还必须考虑过去的累计误差以及误差的变化趋势$^{[2]}$。在此基础上，本研究结合LEGO EV3的多光电传感器融合技术，对传统PID进行离散化与降维改良，实现了对引导线的自适应跟踪。相关实证研究表明，在LEGO EV3机器人上部署PID控制器，能够有效消除底盘的高频震荡，提升复杂赛道下的寻迹效率与稳定性$^{[1]}$。

LEGO MINDSTORMS EV3是LEGO公司推出的第三代机器人套件，其核心可编程砖搭载了ARM9架构的TI Sitara AM1808处理器，主频300 MHz，运行轻量级Linux操作系统$^{[3]}$。EV3支持多种传感器（颜色传感器、超声波传感器、陀螺仪等）和伺服电机，并通过LabVIEW图形化编程环境或MicroPython等高级语言进行编程$^{[3]}$。这些硬件特性为部署控制算法提供了基础。

## 2. 核心底层算法：差速运动学与改进型PD控制

寻迹控制的核心在于通过颜色传感器的光通量反馈，实时计算车体偏离引导线的误差，并将其转化为左右驱动轮的动态差速，从而产生纠偏力矩。本项目采用了双颜色传感器跨线行走的硬件布局，并构建了数字化的PD控制器以适应嵌入式微处理器的采样特性。

### 2.1 差速驱动机器人的运动学模型

在建立控制算法前，必须明确机器人的运动学底盘特性，以此作为控制量输出的物理基础。设左右驱动轮的线速度分别为 $V_L$ 和 $V_R$，两轮中心间距（轮距）为 $d$，驱动轮半径为 $r$。根据轮式移动机器人（Wheeled Mobile Robot）的非完整运动学模型，机器人质心的整体线速度 $V$ 和偏航角速度 $\omega$ 可由以下方程推导得出$^{[2]}$：

$$V(t) = \frac{V_R(t) + V_L(t)}{2}$$

$$\omega(t) = \frac{V_L(t) - V_R(t)}{d}$$

由此数学模型可知，通过对左右轮施加大小相等、方向相反的差速补偿量（即改变 $V_L$ 和 $V_R$ 的相对大小），即可线性地控制机器人的偏航角速度 $\omega$。这种差速补偿即为系统所需的转向力矩，是实现机器人姿态纠偏的动力学基础。

### 2.2 离散时间PID控制算法的数学推导

连续时间域内的经典PID控制方程旨在通过对误差的比例、积分和微分运算来计算控制输出，其标准公式表示为$^{[2]}$：

$$u(t) = K_p e(t) + K_i \int_{0}^{t} e(\tau) d\tau + K_d \frac{de(t)}{dt}$$

其中 $K_p, K_i, K_d$ 分别为比例增益、积分增益和微分增益，而 $e(t)$ 为系统设定值与反馈值的误差$^{[2]}$。

然而，EV3的主控处理器（ARM9）是一个基于离散时间采样的数字嵌入式系统，无法直接处理连续微积分运算。因此，我们需要利用有限差分法将上述连续方程进行离散化。假设微处理器的恒定采样周期为 $\Delta t$，第 $k$ 次采样的误差为 $E(k)$，根据后向有限差分原理，一阶微分可近似为差分方程 $\frac{E(k) - E(k-1)}{\Delta t}$，积分项可近似为离散累加和 $\sum_{j=0}^{k} E(j) \Delta t$。由此得到位置式数字PID方程$^{[2]}$：

$$U(k) = K_p E(k) + K_i \sum_{j=0}^{k} E(j) \Delta t + K_d \frac{E(k) - E(k-1)}{\Delta t}$$

在对称双光电传感器的寻迹场景中，引导线的曲率变化较为迅速。如果保留积分项 $I$，当设定值发生较大变动（如大曲率过弯）时，积分器会累积过大的误差。这可能导致控制输出量被上下限截断而发生"积分饱和（Integral Windup）"现象，表现为系统输出维持在饱和上限，即使误差量符号改变，输出仍无法及时响应，最终导致车体超调或冲出赛道。因此，本项目对传统模型进行降维优化，舍弃了积分项，并将恒定的采样周期 $\Delta t$ 隐式并入系数中，得到离散数字PD控制器$^{[2]}$：

$$U(k) = K_p \cdot E(k) + K_d \cdot [E(k) - E(k-1)]$$

### 2.3 转向补偿算法的系统级映射与参数解析

在项目的实际软件代码中，通过嵌套的高级数学模块实现了以下转向补偿（Turn）运算方程：

$$Turn = (a-b) \times 0.8 + ((a-b)-c) \times 0.6$$

该方程与上述推导的离散数字PD方程存在对应关系。首先，在误差信号的获取上，$a$ 代表左侧传感器反射光强度数字量，$b$ 代表右侧传感器。基于光学的漫反射原理，当前误差信号 $E(k) = a - b$。当车体居中时，左右传感器处于黑白交界处，光吸收率相近，使得 $a-b \approx 0$。若车体向右偏离，左侧传感器进入白色高反光区，右侧进入黑色低反光区，产生正向误差信号。

其次，方程中的 $(a-b) \times 0.8$ 对应比例控制项（Proportional Term）。比例控制仅考虑当前误差，产生与当前偏差成正比的恢复力矩。比例增益 $K_p = 0.8$ 决定了系统抵抗扰动的响应强度，偏差越大，输出纠偏力矩越强。

最后，方程中的 $((a-b)-c) \times 0.6$ 映射了微分控制项（Derivative Term），其中 $c$ 代表通过寄存器存储的上一周期历史误差 $E(k-1)$。微分控制项相当于系统的阻尼器，它计算误差的一阶导数以预测未来趋势。当车体在比例控制作用下向中心线靠拢时，虽然当前位置误差 $E(k)$ 仍大于0，但其变化率 $\Delta E(k)$ 已变为负数。此时微分项会提前产生一个反向的制动力矩，削弱总控制量，从而抑制欠阻尼系统的超调（Overshoot）与震荡现象。$K_d = 0.6$ 即为系统整定后的微分阻尼系数。需要注意的是，理想微分器对高频噪声较为敏感，但由于本系统采样频率较高且误差源仅为两路光通量差值，因此未额外加入低通滤波器。

### 2.4 动态自适应基础速度（Base Speed）非线性模型

传统寻迹机器人多采用恒定基础速度，但在包含直道与弯道的复合赛道中，基础速度需要根据路径曲率动态调整。本项目引入了基于误差绝对值的自适应速度反馈调节模型：

$$BaseSpeed = 100 - |a-b| \times 0.6$$

该方程建立了一个非线性负反馈映射函数 $f(|e|)$。常数 $100$ 为底盘电机在当前供电电压下允许的最高PWM占空比。当处于直道稳态时，车体居中导致位置误差较小（$|a-b| \approx 0$），基础速度接近上限 $100$，实现快速直线行进。当遭遇弯道时，误差绝对值增大，系统通过负反馈自动降低基础速度，减小了转弯所需的向心力，降低了侧滑风险。

值得注意的是，该公式中的常数项（100）与早期版本中的常数（80）有所不同。经过多轮实地测试与参数整定，将基础速度上限提升至100能够在不牺牲弯道稳定性的前提下，提升直道段的平均行进速度。误差衰减系数 $0.6$ 经过调校，平衡了速度降幅与转向需求。

### 2.5 完整控制律的数学统一表达与Python仿真验证

将上述两个核心公式联立，即可得到本项目所使用的完整离散时间PD控制律：

$$\begin{aligned}
BaseSpeed(k) &= 100 - |E(k)| \times 0.6 \\
Turn(k) &= E(k) \times 0.8 + [E(k) - E(k-1)] \times 0.6 \\
LeftPWM(k) &= BaseSpeed(k) - Turn(k) \\
RightPWM(k) &= BaseSpeed(k) + Turn(k)
\end{aligned}$$

其中 $E(k) = a(k) - b(k)$ 为当前采样周期的误差信号，$E(k-1)$ 为上一周期的历史误差。左右轮PWM指令通过基础速度与转向补偿的线性组合得到，当 $Turn > 0$ 时左轮减速、右轮加速，实现向左转向；反之则向右转向。

为验证该控制律的收敛特性，我们基于Python构建了一套离散时间仿真系统。该仿真系统建模了以下物理要素：

1. **圆形传感器探测模型**：每个颜色传感器的探测区域被建模为直径1.5 cm的圆形区域，通过数值积分计算该圆形区域与黑色引导线（宽度2.0 cm）的重叠面积，从而精确计算反射光强度读数$^{[1]}$。
2. **差速驱动运动学**：基于EV3标准车轮几何参数（轮距12.0 cm，车轮半径2.8 cm），将PWM指令映射为前向速度与偏航角速度。
3. **拖尾式传感器布局**：传感器轴位于驱动轮轴前方10.0 cm处，这种"拖尾"构型引入了杠杆臂效应，是产生震荡动力学行为的原因之一。
4. **高斯测量噪声**：在传感器读数上叠加标准差为0.3的高斯白噪声，以模拟真实环境中的光学干扰。

仿真初始条件设定为：车体初始偏航角 $+10^\circ$（向右偏斜），左侧传感器读数 $a=10$（完全覆盖在黑线上方），右侧传感器读数 $b=50$（半覆盖在黑线边缘）。仿真时间步长 $\Delta t = 0.1$ s，初始偏航角 $+10^\circ$，初始传感器读数 $a=10, b=50$。

**图1：EV3寻迹机器人离散时间PD控制仿真结果**

图1展示了四组关键物理量的时域演化曲线：

**面板(a) — 传感器反射率与误差信号**：左传感器 $a(t)$（红色）与右传感器 $b(t)$（蓝色）的读数随时间变化。初始时刻 $a=10$（全黑）、$b=50$（半黑半白），误差 $e(t)=a-b=-40$。随着PD控制器介入，误差信号在约5秒内快速收敛至零附近，随后在零值附近做小幅波动。这种收敛行为验证了PD控制律的稳态跟踪能力。

**面板(b) — PD控制信号**：基础速度 $BaseSpeed(t)$（绿色）与转向补偿 $Turn(t)$（红色）的时域曲线。初始大误差导致基础速度降至 $100 - 40 \times 0.6 = 76$，同时转向补偿产生较大负值（$Turn \approx -40 \times 0.8 + (-40-0) \times 0.6 = -56$），驱动右轮加速、左轮减速，产生向左纠偏力矩。随着误差收敛，基础速度回升至接近100，转向补偿趋于零，系统进入稳态直行模式。

**面板(c) — 机器人位姿与震荡收敛**：偏航角 $\theta(t)$（橙色）、横向偏移 $y(t)$（紫色）和偏航角速度 $\omega(t)$（蓝色）的时域曲线。这是验证PD控制器阻尼特性的核心面板。初始 $+10^\circ$ 的偏航角在PD控制下呈现典型的欠阻尼二阶系统响应特征——经过约2-3次衰减震荡后收敛至零。横向偏移 $y(t)$ 同样呈现震荡收敛趋势，最终稳定在引导线中心附近。偏航角速度 $\omega(t)$ 的幅值随时间递减，表明系统动能被微分阻尼项有效耗散。

**面板(d) — 偏航角震荡放大视图（前10秒）**：该面板以散点图形式标注了每个离散采样时刻的偏航角，清晰展示了PD控制器在每个时间步长内的逐步纠偏过程。这种"步进式"的修正轨迹正是离散时间控制系统的典型特征——控制器在每个采样周期内根据当前误差计算新的PWM指令，驱动车体姿态逐步逼近目标。

**面板(e) — 俯视轨迹**：车轮轴心在二维平面上的运动轨迹。黑色半透明条带代表宽度2.0 cm的引导线。轨迹曲线显示机器人从起点（绿色方块）出发后，经过一段S形路径迅速收敛至引导线，随后沿直线稳定前进至终点（红色方块）。轨迹的平滑度直观反映了PD控制器的跟踪精度。

该仿真结果从理论上验证了本项目的PD控制律能够在约5秒内将初始 $+10^\circ$ 的偏航角误差收敛至零，且稳态误差控制在 $\pm 0.5^\circ$ 以内。这一收敛速度与精度为后续的实地测试提供了理论依据。

## 3. 宏观路径规划：有限状态机与决策树算法

在机器学习与决策分析理论中，决策树（Decision Tree）是一种特殊的树型结构预测模型，它代表对象属性与对象值之间的映射关系，通过计算条件概率或设定判定法则来创建到达目标的最佳规划$^{[4]}$。在底层PD算法保证基本轨迹连续性的基础上，机器人必须具备处理复杂迷宫拓扑及特定交互任务的高级逻辑能力。本项目利用嵌套分支（Nested Switches）构建了基于状态转移的确定性决策树，确保逻辑的严密性与鲁棒性。

为了明确实验中与物料处理相关的策略，程序在初始化阶段引入了简单的随机化与计数变量：共放置两个桶，桶的位置在四个候选位点中编号为 1、2、3、4；初始化时分别从集合 {1,2} 与 {3,4} 中各随机选择一个，从而确定两个要抓取的桶的位置（避免两个桶位于同一侧的固定模式）。程序运行时使用布尔变量 `HasBarrel` 表示当前是否持有桶；使用整型变量 `BarrelNum`（初始为 0）记录抓取的桶数量，每抓取一个桶则 `BarrelNum += 1`；使用整型变量 `CheckNum`（初始为 0）记录已通过的红色打卡点数量，每通过一处打卡点则 `CheckNum += 1` 并在屏幕上对应行输出该打卡点的耗时。

### 3.1 基于布尔逻辑的有限状态机与传感器致盲防护

在自动物料搬运任务中，机器人依赖超声波传感器检测目标桶并执行抓取。抓取动作完成后，物料通常会停留在传感器前方，可能使超声波测距读数持续满足触发条件，从而引发重复抓取的"传感器致盲"问题。为解决此问题，并满足实验要求的两桶放置策略，本项目在有限状态机中引入以下变量：布尔变量 `HasBarrel`（表示当前是否持有桶）、整型计数 `BarrelNum`（记录已抓取桶的数量，初始为 0）与整型计数 `CheckNum`（记录已通过的红色打卡点数量，初始为 0）。

抓取逻辑的触发条件仍以超声波距离为主：

$$Trigger = (d_{ultrasonic} < 15) \land \neg HasBarrel$$ 当抓取动作执行并确认抓取成功时，执行 `HasBarrel = True` 并 `BarrelNum += 1`。按照组内约定的放置策略：第一个抓到的桶（`BarrelNum == 1`）应被放置到随后遇到的第一个红色打卡点；第二个抓到的桶（`BarrelNum == 2`）应该一直携带到终点再放下。为此，在到达打卡点时，放桶动作的条件为 `HasBarrel == True && BarrelNum == 1`；如果 `HasBarrel == True` 但 `BarrelNum != 1`，则只执行掉头或相应的路口动作而不放桶。完成放桶后需将 `HasBarrel` 复位为 False。 ### 3.2 基于有限状态机的路口决策算法 面对分支赛道（如T型或十字路口），系统采用基于有限状态机的确定性决策树进行路径选择。该决策树通过嵌套条件分支实现，其内部逻辑可被精确测试与验证。具体执行流程如下： 当左右两侧传感器检测到反射光强度低于阈值（< 15）时，系统判定可能进入路口区域。系统首先执行探针校验：对于右侧传感器触发的情况，电机以功率30向前行进0.55秒进行位置调整，随后以30/-30功率执行右转0.3秒，并持续保持该差速直到中间传感器检测到黑线（反射光强度 < 15），最后停止电机并等待0.02秒稳定。对于左侧传感器触发的情况，电机先以功率30向前行进0.5秒，随后以-30/30功率执行左转0.3秒，并持续保持该差速直到中间传感器检测到黑线。若左右传感器均未触发但中间传感器检测到黑线，则进入默认PID直行逻辑。若中间传感器检测到红色标记，则执行180度掉头操作。 ### 3.2.1 本组最终版路口决策与物料放置逻辑（实现说明） 本节说明本组最终程序的具体实现细节，以避免文中较通用的决策树描述与最终代码产生歧义。关键要点如下（以代码实现为准）： 1. 初始化阶段在四个候选位置（编号 1、2、3、4）中随机选取两个放桶位置：分别从集合 {1,2} 与 {3,4} 中各随机选择一个，确定要抓取的两个桶的位置。 2. 使用变量：布尔 `HasBarrel`（是否持桶），整型 `BarrelNum`（抓取桶计数，初始 0），整型 `CheckNum`（打卡点计数，初始 0）。抓取成功时执行 `HasBarrel = True; BarrelNum += 1`。 3. 放置策略：第一个抓到的桶（`BarrelNum == 1`）在遇到的第一个红色打卡点放下；第二个抓到的桶（`BarrelNum == 2`）一直携带至终点再放下。因此，到达打卡点时触发放桶的条件为 `HasBarrel == True && BarrelNum == 1`。放下后将 `HasBarrel` 复位为 `False`。 4. 迷宫行走策略采用右手定则：能右转就右转，右转不行就直走，直走不行就左转。由于传感器并列放置仅能感知左右与脚下颜色/反光度，流程在程序中按优先级判断： - 若中间传感器检测到绿色：判定为终点，退出主循环并执行终点程序。 - 若中间传感器检测到红色：判定为打卡点，执行 `CheckNum += 1` 并在屏幕上对应行输出该打卡点耗时；若满足 `HasBarrel == True && BarrelNum == 1` 则放桶并掉头；否则仅掉头。 - 否则若右侧传感器反光度小于15（检测到黑线），直接执行右转逻辑。 - 否则若左侧传感器检测到黑线，则先向前小探一步并重新读取中间传感器：若探前后中间传感器检测到黑线则直行，若未检测到则左转。 - 以上均未触发时，进入保底 PD 巡线逻辑。 5. PD 巡线备底参数在代码中如下实现： $$BaseSpeed = 100 - |a-b| * 0.6$$ $$Turn = (a-b) * 0.8 + ((a-b) - c) * 0.6$$ 其中 `a`、`b` 为左右传感器读数，`c` 为上周期误差寄存器。 该子节为最终版程序的实现说明，文中其它决策树与探针时序描述为通用参考，实际行为以本节所述逻辑为准。 ### 3.3 终点识别：异源传感器解耦与软件消抖 在早期测试版本中，终点判定逻辑（检测终点黑色区域）与路口判定逻辑（检测黑线）使用了相同的反射率灰度特征，可能因环境光导致的阈值漂移引发误判。为解决此问题，系统将终点标志物设定为绿色贴纸，由居中的第三号传感器使用色彩模式（Color Mode）进行识别，实现了导航与终点判定在传感器层面的分离。同时，为防止起跑区的绿色标记引发误判，系统加入了计时器约束： $$Condition_{End} = (\text{Color}_{Mid} == 3) \land (\text{Timer}_{Total} > 10s)$$

在色彩判定通过后，程序还加入了0.1秒的软件消抖（Software Debounce）延时，确保获取到稳定的绿色信号后才触发终点停机。

到达终点时，屏幕上依次显示每个打卡点的耗时、总耗时以及访问的打卡点个数；代码实现上使用 `CheckNum` 来确定打印所在行，并在通过打卡点时即时写入对应耗时。

图2展示了在LEGO EV3图形化编程环境中实现的完整程序代码截图，该程序实现了上述有限状态机与决策树逻辑。程序源文件为 main.ev3。

**图2：EV3图形化程序源码。** 程序实现了基于有限状态机的路径规划与物料搬运控制逻辑，包含初始化、主循环（路口决策、抓取/放置、终点判定）及收尾三个阶段。

## 4. 嵌入式系统底层特性与算力优化

LEGO EV3的主控固件底层运行于轻量级的Linux操作系统环境中，其图形化界面本质是对底层硬件抽象层（HAL）的封装调用$^{[3]}$。在要求毫秒级实时响应的闭环控制系统中，不当的代码结构可能引发系统级资源问题。本程序对此进行了软件工程级别的优化。

### 4.1 CPU饥饿预防（CPU Starvation Avoidance）

在主循环（While Loop）中，若不加干预，高频率的指令执行可能占用ARM处理器的大部分时钟周期，导致负责I/O通信的底层轮询守护进程（Daemon）被挂起，即产生"CPU饥饿"现象。其直接后果是传感器数据寄存器无法及时更新，PID控制器读取陈旧数据，导致电机控制异常。为此，在主循环末尾插入了0.01秒的等待（Yield/Delay）模块，主动让出处理器的调度时间片（Time Slice），使底层I/O进程能够正常刷新传感器寄存器。

### 4.2 PWM输出模式优化（Coast vs Brake）

若将电机驱动模块置于"开启指定时间"模式，底层硬件驱动器会在时间阈值到达时强制触发电气制动（短接电机内部绕组），频繁制动会导致车体出现高频抖动。需要注意的是，在本组的实现中并未采用持续常开（On）模式，因为常开模式会导致主控在电机驱动期间无法及时返回主循环去处理其他逻辑（如传感器读数、状态更新与决策），存在阻塞导致系统“卡死”的风险。为平衡连续输出与可调度性，我们采用了短时步进驱动策略：PID/PD 巡线以 0.1 秒为周期，每次只驱动电机 0.1 秒（即小步前进），然后返回主循环进行传感器读取与逻辑判断。该方式既避免了频繁制动带来的抖动问题，又保证了系统可以及时响应其他事件与状态变化。

### 4.3 进程管理与停机流程（Process Management）

在频繁调用音频文件反馈状态的程序架构中，非正常的物理强制关机可能导致底层音频守护进程异常。系统在终点判定分支后引入了规范的停机流程，通过调用停止模块确保传感器与电机端口安全断电、后台线程规范销毁，以保障系统在多轮次连续运行环境下的稳定性。

## 5. 实验结果与系统性能分析

为验证所设计的"改进型离散PD控制器+有限状态机决策树"混合控制架构在LEGO Mindstorms EV3底盘上的实际综合效能，我们在测试场地上进行了多轮次性能实测，并与传统ON-OFF开关控制器进行了对比分析。

### 5.1 实验环境与赛道特征

本项目的测试赛道由长直道路段与纯直角弯（Right-angle Turns）、T型交叉口及十字迷宫路口拼接而成。直角弯道在轨迹方程上对应导数不存在的非连续阶跃信号。针对此类包含阶跃信号的场地，单纯依靠线性PD控制器无法独立完成全赛道的覆盖，需要结合底层数字PD控制器与基于有限状态机的决策树进行协同处理。

图3展示了根据赛道布局绘制的栅格地图，其中黑色区域为引导路径，白色区域为空白区域，起点位于右下角区域，终点位于左上角区域。赛道包含长直道、直角弯、T型交叉口及十字路口等多种路况。

**图3：测试赛道栅格地图。**

### 5.2 直线段动态响应与稳态误差分析

在直线轨迹追踪测试中，采用ON-OFF开关控制器的机器人表现出明显的周期性振荡现象。开关控制器仅根据传感器二值化结果向左或向右调整方向，导致车体在引导线两侧反复摆动，平均推进速度较低。

采用离散PD控制器后，车体的振荡幅度显著降低。微分补偿项（Derivative Term）根据误差变化率提前产生反向阻尼力矩，抑制了超调现象。根据式（2.5）所示的控制律，基础速度根据误差绝对值自适应调节：直道稳态时 $|a-b| \approx 0$，基础速度接近上限100；当出现偏差时，基础速度自动降低以减小转弯所需的向心力。参考相关文献在相似硬件条件下的对比测定，引入PD控制机制后，直线段上的平均推进速度提升约36%$^{[1]}$。

### 5.3 直角弯道的决策树控制与执行效能

在 $90^\circ$ 直角弯测试中，ON-OFF控制器或纯PID控制器因无法处理反射率的阶跃变化而出现跟踪滞后或冲出赛道的情况。本系统采用有限状态机决策树处理此类路口，具体执行流程如下：

当左右两侧传感器检测到反射光强度低于阈值（< 15）时，系统判定可能进入路口区域。系统首先执行探针校验：对于右侧传感器触发的情况，电机以功率30向前行进0.55秒，随后以30/-30功率执行右转0.3秒，并持续保持该差速直到中间传感器检测到黑线（反射光强度 < 15），最后停止电机并等待0.02秒稳定。对于左侧传感器触发的情况，电机先以功率30向前行进0.5秒，随后以-30/30功率执行左转0.3秒，并持续保持该差速直到中间传感器检测到黑线。若左右传感器均未触发但中间传感器检测到黑线，则进入默认PID直行逻辑。 完成转向后，控制权交还给PD控制器继续执行直线追踪。该流程通过有限状态机的状态转移实现了对直角弯道的确定性控制。 ### 5.4 综合完赛率与系统稳健性 在叠加物料搬运任务的综合测试中，系统表现出以下特性： 1. **CPU饥饿预防**：主循环末尾插入0.01秒的等待模块，主动让出处理器调度时间片，确保底层I/O进程能够正常刷新传感器寄存器，避免了因CPU资源独占导致的传感器数据冻结问题。 2. **传感器致盲防护**：通过全局布尔状态变量 `HasBarrel` 构建有限状态机，超声波传感器的抓取触发条件为 `(distance < 15cm) AND (NOT HasBarrel)`。抓取完成后状态翻转，后续即使物料遮挡超声波传感器，系统也不会重复执行抓取序列，而是进入常规寻迹逻辑。 3. **终点识别**：采用中间颜色传感器以色彩模式识别绿色贴纸作为终点标志，配合0.1秒软件消抖和计时器约束（Timer > 10s），实现了对起点绿色标记的区分和终点判定。

4. **电机控制优化**：为在连续寻迹与事件响应之间取得平衡，本组采用短时步进驱动策略：PD/PD 巡线以 0.1 秒为基本周期，每次仅向电机下发 0.1 秒的驱动命令（小步前进），随后返回主循环进行传感器读取与逻辑判断。该方案避免了将电机置于持续常开（On）模式可能导致的主循环阻塞（从而无法及时处理传感器与状态切换）的风险，同时也减少了由频繁定时制动引发的机械抖动。

在多轮测试中，系统在多数情况下能够完成全赛道的寻迹与物料搬运任务，但运行过程中仍会出现传感器致盲、颜色传感器误识别等问题。总体上这些问题多由传感器硬件特性或环境（如光照）变化引起；软件逻辑经过多轮验证后被认为是稳健且可恢复的，能在多数测试中顺利完成既定任务。

### 5.5 全赛道实测结果

为验证系统在实际赛道上的完整运行效能，我们在图3所示的赛道上进行了正走与反走两个方向的实测。赛道共设有6个红色贴纸打卡点（触发180度掉头及放桶操作），每个方向各经过其中3个打卡点。

#### 5.5.1 正走方向测试结果

机器人从起点出发，沿引导路径正向行进，依次经过3个红色打卡点并完成对应的掉头与放桶操作，最终抵达终点。

全程运行时间为 **54.92秒**，经过**3个**打卡点，经过每个打卡点的用时分别为：**18.43秒、29.13秒、41.81秒**。

#### 5.5.2 反走方向测试结果

机器人从终点区域出发，沿引导路径反向行进，依次经过另外3个红色打卡点并完成对应的掉头与放桶操作，最终抵达起点区域。

全程运行时间为 **51.34秒**，经过**3个**打卡点，经过每个打卡点的用时分别为：**3.066秒、24.54秒、39.42秒**。

#### 5.5.3 实验视频

以下视频记录了正走与反走两个方向的完整运行过程。

**正走方向运行视频：**

https://www.youtube.com/watch?v=aTVxu1YXmjs
**视频1：正走方向全赛道运行记录**

**反走方向运行视频：**

https://www.youtube.com/watch?v=Uk0Ih7O7AaE
**视频2：反走方向全赛道运行记录**

#### 5.5.4 机器人硬件设计

本系统所使用的LEGO EV3机器人硬件设计如图4所示。机器人采用双轮差速驱动底盘，前端搭载两个反射光传感器用于寻迹，中间搭载颜色传感器用于终点识别与红色标记检测，前方搭载超声波传感器用于物料测距。

**图4：LEGO EV3寻迹与物料搬运机器人硬件平台**

## 6. 结语

本项目将离散PD控制算法、布尔逻辑与有限状态机相结合，构建了一套基于LEGO EV3平台的自主寻迹与物料搬运系统。在底层运动控制中，通过离散化PD控制器实现了对引导线的自适应轨迹跟踪；在宏观路径规划中，通过有限状态机实现了对多岔路口、物料抓取与放置、终点识别等任务的确定性控制。同时，针对嵌入式系统的CPU饥饿、电机制动抖动等问题进行了底层优化。实验结果表明，该系统能够稳定完成包含直角弯道、T型路口及物料搬运任务的赛道，为LEGO EV3平台的进阶开发提供了参考。

## 参考文献

[1] A. Stephan, N. Karthick Pugalum Perumal, M. Shivram Kumar, C. Akash, L. Varatharajan and M. Sivapalanirajan, "Performance Analysis of LEGO Mindstorm EV3 Robot with PID Controller for Trajectory Tracking Application," 2023 7th International Conference on Intelligent Computing and Control Systems (ICICCS), Madurai, India, 2023, pp. 1782-1787, doi: 10.1109/ICICCS56967.2023.10142757.

[2] Wikipedia contributors. (2026, May 25). PID controller. In Wikipedia, The Free Encyclopedia. Retrieved 05:44, May 27, 2026, from https://en.wikipedia.org/w/index.php?title=PID_controller&oldid=1356013976

[3] Wikipedia contributors. (2026, May 18). Lego Mindstorms EV3. In Wikipedia, The Free Encyclopedia. Retrieved 05:45, May 27, 2026, from https://en.wikipedia.org/w/index.php?title=Lego_Mindstorms_EV3&oldid=1354810351

[4] Wikipedia contributors. (2026, April 14). Decision tree. In Wikipedia, The Free Encyclopedia. Retrieved 05:49, May 27, 2026, from https://en.wikipedia.org/w/index.php?title=Decision_tree&oldid=1348906127

[5] Wikipedia contributors. (2026, May 24). Finite-state machine. In Wikipedia, The Free Encyclopedia. Retrieved 05:50, May 27, 2026, from https://en.wikipedia.org/w/index.php?title=Finite-state_machine&oldid=1355883596