# 工程机器人算法调研2025

**Repository Path**: ucas-sas-robot-team/engineer-algorithm-research

## Basic Information

- **Project Name**: 工程机器人算法调研2025
- **Description**: 工程机器人相关调研。接下来要做什么事情，怎么做。
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2025-03-17
- **Last Updated**: 2025-03-30

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 工程机器人算法调研
工程机器人的开发，让我们实践移动抓取算法。乍一看可以一做。
## introduction

### 工程挑战赛任务

[RoboMaster 2025 机甲大师高校联盟赛比赛规则手册 V1.1.1（20250314）.pdf](https://terra-1-g.djicdn.com/b2a076471c6c4b72b574a977334d3e05/RoboMaster 2025 机甲大师高校联盟赛比赛规则手册 V1.1.1（20250314）.pdf) 提到工程挑战赛是将正方体放入指定兑换框的比赛。它是一个极其受限的任务，在实际生活中应用价值较低。从技术提升的角度，机械臂位姿解算和自定义控制器是唯二有迁移价值的工作。前者是所有机械臂控制的经典理论，涉及机械臂建模和逆运动学求解；后者是人机交互上的工作，在如今“模仿学习”潮流下有一定的意义。

> 虽然场景理解、姿态估计在计算机视觉中是重要的，利用计算机视觉可能可以实现全自动任务执行，我能想到两条路，但这两条路对于2025赛季的比赛都不可行：
>
> 1. 手动标注YOLO-Pose数据，训练YOLO模型，识别姿态关键点。然而，这件事很牛马，回报率低。且实际工作中很容易存在遮挡，效果可能并不好。网上连工程挑战赛的视频都搜不到，数据集也找不到。
> 2. 用一个非常大的3D视觉相关的模型，实现理解和解算。然而这件事并不容易，且算力要求高。队伍里并没有积累。实际上，如果能实现这样的功能且做得非常稳定，也算是通用机器人的脑袋了。
>
> 综上，我不会投入时间做视觉自动采矿。当然，上述第二条路是非常重要的research question。

>  但是模仿学习并不一定是正确的道路，数据昂贵，稀少，bias较多，难以泛化。目前，我不看好模仿学习的落地。因此，在本工作中，我倾向于通过某种方法向机器人快速输入末端位姿控制，而由机械臂解算算法自动计算所有的位置。

其他队伍的工程挑战赛视频：

[工程宝宝第一次参加工程挑战赛～_哔哩哔哩_bilibili](https://www.bilibili.com/video/BV1JiokYBEXH/?spm_id_from=333.1007.tianma.1-1-1.click&vd_source=8f9827becf0aab75469a2745528a3377)

[两个月大的工程宝宝第一次参加工程挑战赛_哔哩哔哩_bilibili](https://www.bilibili.com/video/BV15zo4YnEUs/?spm_id_from=333.337.search-card.all.click&vd_source=8f9827becf0aab75469a2745528a3377)

[【洪鹰×CubeMars】工程挑战赛夺冠！_哔哩哔哩_bilibili](https://www.bilibili.com/video/BV1o7otYWE2y/?spm_id_from=333.337.search-card.all.click&vd_source=8f9827becf0aab75469a2745528a3377)

### 机械臂控制算法基础知识

[【机械臂运动学教程】机械臂+旋转矩阵+变换矩阵+DH+逆解+轨迹规划+机器人+教程_哔哩哔哩_bilibili](https://www.bilibili.com/video/BV1oa4y1v7TY/?vd_source=8f9827becf0aab75469a2745528a3377)

## 前人工程机器人设计及其算法分析

#### 网络搜索结果

[【RM2023-工程视觉自动兑换+自动取矿开源】华南理工大学-RoboMaster 社区](https://bbs.robomaster.com/article/9427) 介绍了自动兑换算法，主要提供了两个技术路线。但开源的视频已经缺失，代码开源不充分，技术路线分析如下：

1. 金矿石姿态估计。提供了三维重建然后生成合成数据的方法用于训练姿态估计神经网络。我并不认为这能够把模型训练得很好，因为a) 金矿石本身有几面是完全一样的，某些视角上是不可能估计姿态的 b) 合成数据有自己的bias，例如没有背景，和实际场景有差别，可能导致模型错误估计背景的影响因素。
2. 兑换矿姿态解算。提供了基于手工规则的关键点识别算法，这件事除了在RM的某些年的比赛场景用用，没有任何其他价值。方法论能够提供的insight也极其有限。

[shelly-iris/DetectionforRM2022: 全国大学生机器人大赛 RoboMaster 机甲大师赛 工程机器人视觉程序开发](https://github.com/shelly-iris/DetectionforRM2022) 介绍了YOLO识别矿石表面，OpenCV传统规则算法识别兑换矿姿态的方法，然而代码很混乱，且没有模型权重。矿石表面的识别在我们的工程挑战赛中并无太大用处。

#### 官方开源汇总

##### 2023赛季

阅读[RoboMaster 培训体系（240130）](https://docs.qq.com/sheet/DUFlaU0FHZk1QS0l1?tab=bb08j2) ，并在下面介绍目前列举2023年发表的、名字里带“工程”的开源。这是因为开源数目众多，最新的更新都是2023赛季的。

[【RM2023-工程机器人开源】华中科技大学狼牙战队-RoboMaster 社区](https://bbs.robomaster.com/article/9458) 对比了夹爪式和吸盘式机器人，总结了吸盘式末端的灵活性。此外华中科大提到了用编码器组成的机械臂作为自定义控制器的方法。

[【RM2023-全兵种开源】中国石油大学（北京）-SPR战队-RoboMaster 社区](https://bbs.robomaster.com/article/9443) 的工程机器人部分，开源代码和图纸很完整，但是技术报告过于简略，看不到insight。且没有提到自定义控制器。

[【RM2023-工程开源】哈尔滨工业大学（深圳）-南工骁鹰-RoboMaster 社区 ](https://bbs.robomaster.com/article/9442) 也是使用t265作为自定义控制器的位姿来源，但是实际测试有事会因为识别效果不好而失败。

[【RM2023-工程电控相关开源】上海交通大学-云汉交龙战队-RoboMaster 社区](https://bbs.robomaster.com/article/9431) 编写了STM32上基于ARM_math 库的机器人正逆运动学计算库。star有103个。作者非计算机系出身。上交的学生质量确实高，能力确实强。[【RM2023-工程开源】上海交通大学-云汉交龙战队-RoboMaster 社区](https://bbs.robomaster.com/article/9432) 开源的内容较为简略，简述了一些设计思考。其中提到一种自定义控制器：使用3个IMU解算手臂姿态。使用的参数包括三个imu的姿态、imu1-imu2距离、imu2-imu3距离。进一步查阅公开资料，2018年的公开视频 [Real-time motion-tracking in Unity using IMU sensors](https://www.youtube.com/watch?v=BTmddX7imu4&ab_channel=JohnRichardAbella) 展示了使用手臂上3个IMU解算手臂姿态的视频。

[【RM2023-工程机器人机械结构开源】南方科技大学-ARTINX战队-RoboMaster 社区](https://bbs.robomaster.com/article/9433) 内含的技术报告在https://docs.qq.com/pdf/DV21kVW9PSFZyT1RJ?nlc=1 南科大的报告有如下insight:

1. 末端执行器尺寸比较小，使得在兑换矿石的时候精细操作对机器人整体的移动影响比较小。这也反映在了USTC的工程机器人的末端小电机群。
2. 用MATLAB帮助仿真计算，从而设计机械臂的各个尺寸参数，保证伸展空间覆盖工作空间
3. 自定义控制器使用现成的VSLAM算法（t265自带VPU，跑视觉SLAM算法），用realsense相机+ROS方案。这样不用电控攒太多硬件。（但是这么大一堆算法的设备很难带，电池、变压器、NUC、相机，且成本比较高啊）（不过话说回来Intel做的这个产品确实好，大家都在用。做IT的就是做服务的）

[【RM2023-工程机器人开源】沈阳航空航天大学-TUP战队-RoboMaster 社区](https://bbs.robomaster.com/article/9402) 设计了2平动3转动的机械臂。平动的缺点是让机器人变得非常臃肿。此外，其在技术报告末尾提到视觉采矿的性价比不如自定义控制器。视觉采矿对开发人员的精力消耗非常大。

#### 网上搜2024赛季

> 我记得中科大还有个开源啊，我一开始怎么完整错过了所有 2024赛季的资料



[中国科学技术大学RoboWalker战队 2024赛季工程机器人代码、技术报告开源-RoboMaster 社区](https://bbs.robomaster.com/article/18706) 提到了系列开发工具。Mujoco 仿真调试机械臂自定义控制器。

[【RM2024-工程机器人机械电控开源汇总】华北理工大学-Horizon-RoboMaster 社区](https://bbs.robomaster.com/article/9676) 提到了上海交通大学2023年的开源方案：3个IMU求解手臂位姿。文档提到上交的自定义控制器能够计算出三平动三转动共6个自由度。

[【RM2024-技术开源汇总】香港科技大学ENTERPRIZE战队（持续更新）-RoboMaster 社区](https://rmbbs.robomaster.com/article/54254) 港科大开源的东西好多啊，不愧DJI老板的母校[【RM2024-工程机器人（龙门架构型，5000经济）技术报告+机械结构开源+自定义控制器开源】香港科技大学ENTERPRIZE战队-RoboMaster 社区](https://rmbbs.robomaster.com/article/48014) 提到箱式工程比较稳定，操作比较简单。（这是比不过上交的研发能力吧）复活赛[第1场 香港科技大学 ENTERPRIZE战队 vs 德州农工大学 Texas Aimbots战队 RoboMaster 2024 超级对抗赛·港澳台及海外赛区_哔哩哔哩_bilibili](https://www.bilibili.com/video/BV17f421B74B/?spm_id_from=333.337.search-card.all.click&vd_source=8f9827becf0aab75469a2745528a3377) 中提到港科大的自定义控制器是“同构”的。开源图纸中展示的自定义控制器有三个平动自由度，末端有2个转动自由度。推测缺少的一个转动自由度由底盘提供。同构的好处是不需要复杂的解算，但由于他们的机器人自带P构型部件，自定义控制器的尺寸也比较大。他们使用了去掉减速箱的3508电机作为转动编码器。

[【RM2024-工程机器人机械结构开源】上海交通大学-云汉交龙战队-RoboMaster 社区](https://bbs.robomaster.com/article/54080) 组委会解说员在上交的冠军争夺赛时提到了模块化设计。上交的自定义控制器是一个可穿戴设备，手臂上有一系列电机编码器。上交的设备实现了很高的兑换效率。机器人设计要考虑走线等因素，避免线材外漏。

[【RM2024-工程机器人机械结构开源】华南理工大学 华南虎战队-RoboMaster 社区](https://rmbbs.robomaster.com/article/54106) 给出了详细的设计思路，技术报告从需求分析到方案对比再到实现详细且完整。自定义控制器是一个机械臂，构型中没有P，只有R。实际上平动自由度的空间占用很大，不如R型便携。在技术方法上也有insight

> 在方案调研和选择阶段需要根据多方的对比分析才能够下决定， 不能因为喜欢才去做，而是因为合适才去做。......一台机器人的上限不是测试出来的而是设计之初就决定好的，测试只是为了让 我们知道它目前能够达到的程度。

> 选择大于努力，结果从文献调研之初就已经有了大致走向。而队伍成员的行为方式，从老队员的选人和教诲开始就已经逐步定型了。

> ~~感觉今年比赛打完了得给这几个队长上上强度~~ 感觉得用正确的方法论示范一些做事逻辑。

[【技术报告-南京理工大学Alliance战队-RM2024工程开源】南京理工大学技术报告-RoboMaster 社区](https://bbs.robomaster.com/article/43607)的技术报告调研了各队伍的自定义控制器构型，将其分为并联型和示教器型，其中示教器型是提到进入16强的自定义控制器。我们猜测：示教器解算上较为简单：在摆放示教器的过程中直接提供了所有电机的目标姿态，能够避免电机驱动延迟、逆运动学多解性造成的影响。对于本科生来说，较为简单的解算更容易做出来，而困难的解算可能不太能做得稳定。

[【RM2024赛季-自定义控制器开源】厦门理工学院PFA战队-RoboMaster 社区](https://bbs.robomaster.com/article/437773?source=1) 是步兵自定义控制器。缺少z方向平动自由度。

## 阶段性分析——技术方案选择

综合上述调研结果，在自定义控制器上有如下insight

1. 基于视觉的方法普遍不稳定。即便是 VSLAM。视觉方法硬件开销巨大。
2. 机械+电控方案较为稳定。含有平动机械的控制器一般较大，不容易携带。
3. 简单的设计适合本科生开发调试——例如与机器人同构的结构。

自定义控制器问题，本质是：如何方便地输入位置（position）和姿态（pose）。单一9轴IMU只能提供姿态，而无法提供位移：这是因为无法提供误差反馈，IMU会飘掉，这也是纯IMU无法作为里程计的原因。我曾一度想使用纯IMU作为转动姿态传感器，使用三个电机+编码器，根据电机值求解末端平动。但现在看来，这个方案也有些复杂：单独的编码器需要较大开发工作量，使用M2006电机需要电调和24V供电。

最让我感兴趣的是上交2023年提到的纯IMU手臂姿态解算——3个IMU足矣解算手臂姿态。虽然其开源中提供的信息非常少，但网上有非常多的公开资料：使用多个IMU解算手臂位姿。上交是2023赛季和2024赛季全国总冠军，配套全R构型工程，使用了此技术方案。

接下来，我将对此技术方案进行调研：

## 进一步分析——多纯IMU方案原理

[Real-time motion-tracking in Unity using IMU sensors](https://www.youtube.com/watch?v=BTmddX7imu4&ab_channel=JohnRichardAbella) 2018年的视频展示了手臂解算的实时视频。视频中使用了3个IMU，分别在大臂外侧、小臂外侧、手背。系统可以检测手臂末端前后移动，手掌与手臂的相对弯折，较为鲁棒。（虚拟人的手臂偶尔有跳变）

[OpenSim Unity Plugin: Real-time Joint Space Motion Reconstruction using IMU sensors](https://www.youtube.com/watch?v=r17tR-loBps&ab_channel=JohnRichardAbella) 2019年的视频。提到了用于分析[Demircan_2014_Springer.pdf](https://home.csulb.edu/~edemirca/Demircan_2014_Springer.pdf)

[IMU-based Motion Capture System](https://www.youtube.com/watch?v=I017_Ufs-Fg&ab_channel=ZhengyuFU) 2021年的视频，使用2个IMU求解大臂、小臂姿态，能够得到手腕处的空间位置。其代码在 [Huoleit/Unity-Motion-capture: An IMU-based motion capture system and a real-time motion display in Unity.](https://github.com/Huoleit/Unity-Motion-capture) 

[LabNote: MPU6050 Based IMU Arm Motion Capture + Blender Realtime Sync](https://www.youtube.com/watch?v=F1IdRtIDdIs&ab_channel=-T.K.-) 2022年的视频，使用2个IMU求解大臂和小臂的运动。

一番搜索之后，我们发现网络上并没有明显的公开代码。唯一找到的有代码的是上述Unity相关代码，使用了2个IMU。我们需要自己推测一些问题。

研究自己的手臂，绘制solidworks 三维模型，试图建模手的运动。我们保存在了[文件夹](./arm_draft)中。

查看官方比赛视频可知，上交的这个方案在2023年和2024年是相似的。不同之处在于：2023年3个IMU直接绑在手上，之间没有刚性部件连接。研究自己的手臂可知，小臂可以有曲肘的旋转和转篮球的旋转，后者是靠小臂的两根骨头为轴的——靠近手腕处旋转强烈，靠近手肘处旋转不强烈，2023年上交工程操作手将大臂和小臂的IMU绑在了靠近手肘处。然而2024年，上交新增了一个亚克力转轴，使得大臂和小臂IMU的相对运动只有绕亚克力轴心的旋转，避免了小臂旋转自由度的影响。

我们从简单的结构开始。直觉上，两个分别在大臂和小臂上、均靠近手肘的IMU用于计算手腕的空间位置。再增加一个IMU用于提供末端姿态。

于是我们可以理解上交2023开源的铅笔草图中的含义

![image-20250326003249704](image/image-20250326003249704.png)

上述位姿映射中计算P依赖于前提假设： imu x轴与相应连杆共线

实际中很容易想到：P的计算中，R0 和 R1 是容易通过IMU精确获得的，l0 l1等于操作手的大臂和小臂长度。但是，人手上肌肉的凹凸、联盟赛春寒料峭3月份时所穿衣服的褶皱，容易影响共线条件。

因此，需要增加机械结构确保手臂与IMU轴稳定共线。上交2024年的比赛视频中已经透露了解决方案：使用一个类似小学文具折叠尺的东西，保证连杆与imu x轴共线。

这也太容易算了吧。

我们需要如此解释计算结果：以肩膀转轴为原点，建立某坐标系，在此坐标系下的P与R用上述公式计算。然而这个以肩膀转轴为原点的坐标系正方向是暂未明确定义的。因此，要提供初始化坐标系的功能。这可以是操作手把大臂竖直并在身体侧面，抬起小臂指向前方，按下初始化坐标系按钮实现初始化。

## 工程机器人算法设计

### 初始化肩膀坐标系



### 位姿计算



### 绝对运动模式与相对运动模式



## 实现

使用3块C板（大臂、小臂、手背）+3个按钮（初始化、绝对运动模式、相对运动模式）+一把折叠尺+2根4pin can线+1根4pin串口线。选择4pin是为了供电，同时减少冗余线材。

使用Standard Robot例程中的caliberate任务校准IMU，然后烧录新电控代码。电控代码分为三套：大臂、小臂、手掌，通过can总线传递四元数表示自身姿态。大臂上的4pin 串口线负责给全系统供给5V电源和与图传链路通信。三套代码均在FreeRTOS上执行。

| 信息          | ID                                                           |
| ------------- | ------------------------------------------------------------ |
| 手掌四元数x y | 0x11                                                         |
| 手掌四元数z w | 0x12                                                         |
| 小臂四元数x y | 0x13                                                         |
| 小臂四元数z w | 0x14                                                         |
| 大臂四元数x y | 0x15（其实大臂不用发送CAN数据，只用接受并发串口数据给图传链路） |
| 大臂四元数z w | 0x16（其实大臂不用发送CAN数据，只用接受并发串口数据给图传链路） |

## 使用方法

初始化之后，操作手不要转身子。不然，重新初始化。