构建桌面机器人

什么是桌上机器人？

桌子上有一个桌子机器人。比赛分为三个“阶段”：

• 第一阶段：建造一个从桌子的一端到另一端并返回的机器人。

• 第二阶段：让机器人将一块块从桌子的壁架上推下来。

• 第三阶段：让机器人将积木推入安装在桌子末端的鞋盒中。

还有一个非官方的第四阶段——那就是从桌面上掉下来并生存下来。我没有尝试这个阶段。

大多数桌面机器人都非常简单——几个声纳或红外传感器，它们在桌面上徘徊，希望完成不同的阶段。我的桌面机器人完全不同——当机器人在 2023 年 RoboGames 上获得金牌时，它得到了回报。

机器人构建

整个机器人是由3D打印部件和我手头的随机东西制成的。

我有一个99美元的LD-06激光雷达闲置了一段时间，并决定这是一个很棒的项目。我使用 Dynamixel AX-12 伺服器倾斜激光，以便找到桌子、立方体或目标。

所有代码都在STM32上运行，在我的定制Etherbotix板上，这是几年前为我的Maxwell机器人设计的。该机器人使用带有一些30：1 12V齿轮电机的差速驱动，这些电机于2008年从Lynxmotion购买，多年来用于各种消防机器人。

一组小型数字夏普红外传感器用作悬崖传感器。这些可以向上或向下移动，以使用一对调节螺钉针对不同的工作台表面进行校准。虽然传感器非常精确并阻止机器人，但它们在全速行驶时看不到足够远的前方，因此我也使用激光来检测桌子边缘何时接近。

阶段 1 软件

第 1 阶段非常简单——主要基于航位推算测程法：

• 激光向下倾斜以寻找桌子。这是通过将扫描投影到3D点，并过滤掉机器人前方大致工作台高度的任何物体来完成的。当桌子消失时（点数下降太低），我们将最大速度降低到悬崖传感器可以安全检测到的速度。

• 当激光传感器寻找工作台的末端时，机器人向前驱动，一个简单的反馈回路使用测程法使机器人保持在工作台的中心。

• 当悬崖传感器最终触发时，机器人会停下来，后退15厘米，然后旋转180度——所有这些都使用航位推算测程法。

• 然后重置最大速度，我们以相同的行为起飞到桌子的另一端。

阶段 2 软件

第 2 阶段的运动与第 1 阶段基本相同——我们向前行驶，以测程法为中心。速度比相位 1 低一点，因为激光器也在寻找块：

• 激光扫描投影到3D，我们根据高度过滤掉表格中的任何点。然后将对这些剩余点进行聚类，并分析聚类的大小。

• 如果一个集群是该块的良好候选者，则机器人转向该块（您猜对了，使用里程计的航位推算）。

• 然后，机器人使用简单的控制回路向块行驶以保持航向。

• 到达方块后，机器人直线行驶，直到悬崖传感器跳闸。

• 此时，机器人将轮子停在跳闸悬崖传感器的侧面，并非常缓慢地向前驱动另一个轮子，以便我们将机器人的前部与桌子的边缘对齐——确保块被推离桌子。

阶段 3 软件

最后阶段是最复杂的，但不是很多。与前面的阶段一样，机器人沿着桌子向下移动，找到块：

• 与第 2 阶段不同，机器人实际上接近块后面的姿势。

• 一旦达到这个姿势，机器人就会将激光倾斜回水平并找到目标。

• 然后机器人转向目标，就像它第一次转向块一样。

• 然后，机器人使用相同的简单控制回路接近目标，并在此过程中最终将块推向目标。

我的 Tablebot 的所有软件都可以在 GitHub 上找到（https://github.com/mikeferguson/stm32/tree/main/projects/tablebot）。

可视化

为了使开发更容易，我还编写了一个Python GUI，用于渲染表格，机器人测程轨迹，激光数据以及检测到的目标和立方体。

数学的乐趣

一路走来，我实际上在 ARM CMSIS DSP 库中遇到了一个错误。我使用该函数来计算我的测程法：arm_sin_cos_f32()

arm_sin_cos_f32(system_state.pose_th * 57.2958f, &sin_th, &cos_th);
system_state.pose_x += cos_th * d;
system_state.pose_y += sin_th * d;
system_state.pose_th = angle_wrap(system_state.pose_th + dth);

此函数获取角度（以度为单位！），并使用查找表和一些有趣的插值返回角度的正弦和余弦。通过机器人路径的可视化，我注意到机器人测程法偶尔会向侧面和向后跳跃——这毫无意义。

进一步的研究表明，对于非常小的负角度，返回了巨大的值。我深入研究了代码，发现有几个不同的版本：arm_sin_cos_f32

• 我的旧STM32库中的版本在非常小的负数下存在此特定问题。同样的错误仍然存在于arm帐户的官方CMSIS-DSP中。

• 当前STM32库中的版本对此进行了修复 - 但该修复随后破坏了整个象限的功能！

问题很简单：

• 该代码使用 512 元素查找表。

• 对于给定的角度，它必须在表中的上一个条目和下一个条目之间进行插值。

• 如果您的角度落在第 511 个条目和下一个条目之间（由于环绕，这将是第 0 个条目），那么您在下一个内存插槽中使用了一个随机值来插值（并计算插值）。在某一时刻，这导致 sin（-1/512） 返回了像 30 这样的离谱值。

修复该错误后，测程法之后完美无缺。事实证明，我在工作中的一些无刷电机控制代码中存在相同的功能/错误。