RoboMaster电控组工作总结与基本思路
绪言
经过2020到2022三个赛季在创梦之翼战队电控组的工作，我从正式队员退役成了顾问。本文将通过总结我自己的生涯经历，并结合一些具体的例子，阐述我对包括但不仅限于RoboMaster的机器人竞赛电控组基本思路的理解与认识，希望能对现役队员的工作起到一点启发作用。
个人经历
2020赛季
2019年9月，我在大一开学后不久就成了战队实验室的常客，基本每天都会去实验室的角落学C语言和单片机。在2020赛季规则发布后的战队会议中，队长把英雄机器人的电控工作交给我，我的学习内容就从单片机的使用转移到了的如何调车上。自己比着上交的开源代码和官方的例程还有扬神发在cnblog的RoboMaster电控入门系列一点点学。
以我当时的知识储备和理论水平，我的调车工作可以简单概括为“代码实现与参数整定”。说白了就是写程序和调参，写嵌入式软件代码和调底盘与云台电机的PID参数，甚至连姿态解算算法也是用的学长给的现成代码。利用现成代码以及从网上各种乱七八糟博客中学的PID，我在寒假之前使机器人完成了中期形态视频中的大部分基本功能。
此外，在与几位学长的交流中我逐渐认识到系统思维与理论工具的重要性，这使我在大一的第一个学期形成了自己的第一套基本思路或者说方法论：工程实践促进理论学习、理论工具指导工程实践。
2020年1月，疫情爆发。由于疫情无法返校，在家无车可调，这给了我大量的时间与精力去静下心来学习理论知识与仿真方法，并在学习的过程中尝试通过仿真加深自己对理论内容的理解与认识。在了解卡尔曼滤波基本思路后，我利用CMSIS DSP库中的矩阵运算工具开发了一套可用于STM32平台的卡尔曼滤波C语言实现，这为后续的自瞄系统与姿态解算奠定了物质基础。
除了对理论基础的学习，我还对云台控制与机器人自瞄的基本思路做了一点浅浅地思考，并以仿真的形式做了一些简单的验证。唯一做了实验的内容，是用MPU6500实现了姿态解算。
再后来线下赛取消了，比赛变成了线上评审，我便把我对云台控制的思考与仿真加入到了评审内容中的云台控制系统部分。最后在答辩过程中评委工程师对此的评价则是，这里仿真好用的方法，到实际机器人上不一定好用，其中的XXX大概率不好用。就这样，20赛季草草结束，疫情也得到控制，大二开学便能回到学校了。
2021赛季
2020年9月，时隔半年回到实验室。第一件事是在机器人底盘3508电机速度控制问题中，尝试了一下前馈控制，对速度跟随的实时性提升效果显著，这算是我第一次通过理论方法提升机器人实际表现。之后在思考如何提高电机的抗扰性能时，了解到了扰动观测器的概念，并通过参考文献，在机器人云台串级控制当中的速度环，根据系统 nominal model G_n(s) 实现了图示结构的线性扰动观测器。
在机器人云台上具体实现之前，我已经在仿真中验证了我对该扰动观测器的理解，仿真效果很好。实际系统中的表现显然没有仿真理想，但也能基本准确的补偿掉理想的常值扰动，且补偿速度显著敏捷于速度环控制器中的积分环节。但由于我对其系统设计思路理解不够准确，导致其并不能保证在绝大多数工况下高效且稳定的工作，因此该尝试告一段落，并没有实现实机部署。
我继续负责英雄机器人的电控工作，并在学期末中期形态视频截止提交前用一种极简陋的方法实现了装甲板自动跟踪瞄准。最后把五花大绑的机器人快递回家继续研究自瞄。
2021年1月，正式开始自瞄系统的开发。第一件事是读懂视觉的祖传代码，此外还和当时已经退役的大四视觉学长远程协作尝试了一下miniPC与STM32的NTP对时。在搞懂代码框架以及通道分离二值化膨胀腐蚀PnP等方法基本原理以后，我开始针对装甲板识别的鲁棒性和识别率以及我最关注的识别速度进行优化，在视觉的代码里留下了属于自己的屎山。经过优化，装甲板识别的鲁棒性和识别率获得了一定提升（一点点 但不多），而识别速度有显著提升（其实只是把不必要的memcpy换成了指针传递）。
经过以上工作的铺垫，我开始思考自瞄系统的具体框架与实现，其中关键问题包括：运动预测方案与上下位机分工。通过查阅一些资料以及自己的胡思乱想，我最终确定运动预测的基本方案为：对PnP解算得到的目标相机系坐标进行坐标变换得到其云台系坐标，根据姿态四元数将云台系坐标变换到惯性系得到其惯性系坐标，在惯性系下估计目标相对机器人云台的位置与速度，根据估计得到的位置与速度结合弹道模型确定预测步长与抬枪补偿。上下位机分工方案为：miniPC进行PnP解算得到坐标后通过串口发送给STM32，剩余工作均由单片机完成。
确定基本方案后开始着手实现，过程中发现在相机良好标定的前提下，机器人云台的转动会使解算出的静止装甲板的惯性系坐标发生变化。经过思考我意识到，是因为PnP解算得到的坐标与用于坐标变换的姿态四元数没有进行时间同步，即需要采用该坐标对应的照片被拍摄时刻的姿态四元数进行坐标变换。解决上述问题后就可以利用卡尔曼滤波便可以稳定估计目标的位置与速度，再解决掉各种乱七八糟的细节问题便可以实现带预测的自瞄。但由于我是在英雄机器人上测试的自瞄，没法在家里发弹，所以并不能测试命中率到底如何。
返校后测试自瞄命中率，对于匀速运动目标，由于弹道模型参数以及速度估计值的不准确性，基本可以做到百发不中。我开始陷入深深的自我怀疑之中，随着我持续的自我怀疑，黑龙江赛区联盟 赛开始了。由于我代码框架中某个Bug导致的稳定性问题，导致我们的机器人在赛场上频繁失控，任人宰割。再加上我的自瞄在场上完全不能用，在一次步兵1v1对抗中亲自操作机器人被某新队伍碾压后，我彻底破防了。赛后我坐在备场区抱头痛哭（一点点 但不多 介于抱头痛哭和掩面而泣之间），在队长唐滨的耐心劝导下我才慢慢平复了心情（其实是哭累了）。经过无数次注定的失败和一两次侥幸的成功，噩梦一般的联盟赛结束了。
联盟赛后痛定思痛，在对机器人性能追求以及自瞄优化工作之外，我开始认真对待机器人稳定性，认真思考和斟酌每一个有可能影响机器人稳定性的细节。尽管这样，我仍然在北部区域赛中犯下了一个险些让战队万劫不复的错误。
北部区域赛16进8淘汰赛，胜者可以直接拿到晋级国赛的门票，败者则需要在淘汰后连胜两场才能晋级国赛，否则只能参与复活赛或彻底淘汰，而晋级国赛对当时的我们来说几乎算是一个所有人都为之拼命的执念。
就在16进8对阵北京理工大学的第三局决胜局中，所有人都在紧张的观察着这场生死局的发展，我负责自瞄与射击部分的哨兵机器人突然开始对着空气射击，很快枪口热量打满最后超热量死亡。看到这场面，我浑身上下瞬间冷汗就冒出来了，我意识到，完了，我要成为战队的千古罪人了。这个突发状况直接导致我们输掉了比赛，错失了晋级国赛的最好机会。事发之后我整个人精神是恍惚的，感觉天旋地转，最终有了这张世界名画《北理工队员在庆祝胜利》。
画面右上方，北理工的队员喜极而泣欢呼着拥抱在一起；画面左下角，是一个心如死灰的可怜虫在试图不让自己昏死过去。
当我恢复思考之后，我很快意识到枪口热量限制失效的原因：代码中没有区分双枪管ID，导致过程中程序读取的热量始终是0。回到备场区后我打开代码发现果然，果然是我的粗心导致的这一切。至于为什么机器人会对着空气射击，我只能回忆起在临近出发前发生过这种情况，但我并没有检查出原因也没有给出完善的应对方案。也就是说，这是我的两次错误叠加作用的结果。
万幸的是，我们在后面的两场比赛均获得了胜利，最终拿到了八强之外仅有的两个国赛名额。事后客观分析这件事，最好的办法应当是多进行对抗测试。因为粗心不可能完全避免，要允许粗心犯错，与此同时需要多测试进行纠错，如果在备赛过程中我做过足够多的测试，就能避免这个悲剧的发生。
除了稳定性问题，区域赛还暴露出了机器人自瞄容易失效的问题，尤其是经过碰撞后。分区赛后我花了相当多的时间和精力试图提高自瞄的稳定性。通过改进自启脚本和异常检测逻辑，确实一定程度提高了自瞄的稳定性，至少在备赛测试过程中不那么容易出现失效的情况了。但仍然存在我没有考虑到的因素，结果就是在国赛赛场上机器人迫切需要自瞄功能的时候，自瞄是失效的。这导致我们在小组赛中被淘汰，止步32强。
赛后参加青年工程师大会，有幸可以上台分享我们的自瞄技术，惭愧的是并没有吸引到大家来听。国赛旅程草草结束，留下的只有几张照片和几段视频。
2022赛季
2021年9月，我开始编写自瞄系统设计文档。编写文档的同时也在不断优化自瞄的性能，硬件上，把21赛季使用的USB相机彻底淘汰，换成了工业相机，在分辨率和成像效果上有显著进步。算法上，主要在预测策略和滤波器量测模型两方面做出了一些改进，一定程度提升了自瞄的性能以及适用范围。
除了自瞄系统的迭代，我还和视觉组同学合作正式开始着手开发击打能量机关的功能。我主要负责系统框架与流程的总体设计以及正弦角速度参数估计算法的仿真验证。其中正弦角速度参数估计算法的迭代与优化历程值得一提，起初我们采用扩展卡尔曼滤波 (EKF) 估计参数 \phi,a,\Omega，在上海交通大学交龙战队于2021赛季青工会分享的相位估计算法基础上，在状态中加入参数 \phi ,\Omega。通过MATLAB进行仿真验证，实际角度与测量角度及其估计值与正弦信号幅值 a 与频率 \Omega 估计情况如下图所示。
通过仿真发现，实际角速度估计值在滤波初始阶段具有较大噪声，在 t = 3s 时刻才得到噪声较小的估计值，与此同时幅值 a 估计结果准确收敛，但频率 \Omega 在仿真 8s 全过程中自始至终未收敛至真实值。经过多次仿真试验，发现该方法对参数的估计效果与收敛性很大程度受状态初值的影响，且鲁棒性较差。
为提高参数估计的鲁棒性，我们抛弃了同步迭代的方法，转而采取 梯度下降方法进行异步优化求解。通过MATLAB仿真验证发现，该方法不仅在收敛时间还是鲁棒性上均优于 EKF，之后我把仿真代码交给视觉组学弟由他完成C++实现与实际代码部署。但该方法的实现并非一帆风顺，在实际击打能量机关的测试过程中我们发现，我们采用的最朴素实现方法可以在程序运行初期稳定收敛，但随着估计过程的进行算法会突然发散。
考虑到该参数估计问题实质上不存在与环境的交互，对于同一组数据在机器人计算平台中在线求解和导出到仿真环境中离线求解等价。为了避免对着terminal大眼瞪小眼绞尽脑汁思考原因的尴尬局面，我们将完整数据导出，并试图通过MATLAB离线求解寻找发散原因。根据离线求解过程的全部迭代信息，我们很快找到了发散原因并成功验证了一套解决方案的可行性，最后部署到机器人上得到了与仿真效果相同的收敛情况。至此，在我和视觉组同学们的共同努力下，创梦之翼战队有史以来第一次具备了击打能量机关的能力，实现了从无到有的突破。
对于平衡步兵项目，我们团队在赛季初会议中确定机器人将采用轮腿构型，因为相比于传统轮式倒立摆模型，腿的加入使机器人机构获得了更多的自由度、为机器人的平衡与运动提供了新的思路，可以极大程度提升机器人的运动表现。但更多的自由度意味着更复杂的模型和控制系统，平衡控制问题中系统建模对腿部五连杆机构的处理是一件一直让我头疼的问题。在参考了本末科技Diablo机器人后，我选择忽略机器人腿部五连杆的动态以及腿长变化，将五连杆简化为一根硬杆，得到图示简化模型。
参考Control Tutorials for MATLAB and Simulink - Inverted Pendulum: System Modeling (umich.edu)利用牛顿力学列写模型中各刚体运动微分方程并线性化，通过MATLAB符号运算求解得到系统线性模型状态空间表达。通过 Simscape Multibody 搭建图示简易模型验证模型准确性。
仿真结果显示，根据状态空间模型计算的反馈增益可使该建议模型保持稳定并跟随期望信号，且动态过程与根据线性模型进行的仿真接近。因此我有理由相信我推导出了不完全错误的线性模型（虽然后面回顾的时候发现推导过程有地方求导求措了，但线性化后就消失了所以不影响结果）。
接下来便需要继续利用仿真验证VMC方法对于该问题是否具有可行性，即建模环节中对腿部五连杆动态的简化是否对于该控制问题是可以接受的。此外，为考虑机器人不同腿长的工况，我在腿长区间内每10mm对系统模型进行一次线性化，并求解其反馈增益矩阵 \boldsymbol K。对矩阵每一个元素 K_{ij} 随腿长的变化拟合多项式方程得到。仍然利用 Simscape Multibody 搭建图示模型进行仿真验证，结果表明，该简化模型与VMC结合可以满足机器人的控制要求。
平衡与纵向运动是平衡步兵综合运动中最重要的部分，除此之外还需对机器人高度与横滚姿态等状态进行控制。在系统平衡点附近，近似假设机器人的其他运动相互解耦，因此我根据工程经验对其分别设计控制器，最终得到如图所示完整的机器人控制系统。
以上系统设计过程均采用基于模型的设计思路进行，这导致模型的不确定性与环境的随机性会影响整个系统的稳定性，特别体现在机器人双轮离地的情况下该系统无法保持机器人姿态稳定。因此在驱动轮离地后需采用其他控制策略以避免空中姿态发散。为保证控制系统能够快速且准确的进行切换，可靠的机器人离地检测算法至关重要。
为解决此问题，我对机器人的简化模型进行基础的力学分析后发现，通过加速度计与关节电机力矩反馈解算地面对机器人驱动轮竖直向上的支持力判断机器人是否离地可兼顾快速性和准确性。得益于该方法，机器人可以在飞坡过程中以良好姿态落地，并借助腿部五连杆机构极长的缓冲行程实现真正的“平稳通过飞坡”。
经过以上努力，机器人已经具备了优雅的运动表现，但距离能在赛场上稳定发挥还差很远。为保证赛场上绝对的稳定性，还需克服诸多工程问题。其中最棘手的，便是机器人在高速运动中踩到子弹或盲道等障碍物后摔倒的问题。这个问题从一开始就一直困扰着我，始终是我的一个心结，让我连续两个月睡不了安稳觉。
为解决该问题，我尝试了大大小小各种各样的方法，调参什么乱七八糟的都试过，还是无济于事。后来通过反复观看机器人摔倒的慢动作录像我意识到，在机器人踩到子弹的瞬间速度突然减小，此时控制器会希望机器人加速。而该模型加速需要驱动轮移动到机器人整体质心的后方，因此会给驱动轮电机一个反转的力矩。这个力矩会让电机在滞空阶段疾速反转，最终导致整个系统彻底发散。
对于这个速度突变与打滑共同作用导致的问题，只要合理处理其中一个造成的影响即可避免问题。结构上的思考便是，更换半径更大摩擦力更大质地更软的轮胎，但受制于尺寸限制，这个方法在经过多次尝试之后最终宣告失败。因此只能在算法层面解决该问题，首先速度突变是物理世界的客观事实，难以改变，因此我需要找到一种方法减小驱动轮打滑的影响。
对于打滑问题，很容易想到通过融合电机编码器测得的相对速度与惯导得到的绝对加速度以获得机器人相对惯性空间的运动速度，相对惯性空间则完全不存在打滑的概念。通过设计简单的状态观测器实现传感器数据融合，机器人频繁摔倒的问题随之解决。解决了这个困扰我许久的心头大患，我当即跑出实验室买了好几瓶酒，当晚在寝室痛饮一场一醉方休。至此，算是得到了平衡步兵机器人完整形态的最后一块拼图，真正具备了上场对抗的能力。
最终，凭借着平衡步兵机器人的独特结构和优秀性能，以及机器人自瞄系统的出色表现，再加上相比上赛季有了质的飞跃的机器人稳定性，在创梦之翼战队全体队员的努力下，我们最终以15场全胜无败绩的战绩，夺得了2022赛季东部区域赛的冠军。
思路总结
除了技术层面，我对RoboMaster比赛电控组基本思路的认识与实践同样贯穿我作为电控组负责人的工作。那就是我在大一认识到的道理：工程实践促进理论学习、理论工具指导工程实践。我总对学弟学妹们讲：要善于用理论工具分析和解决工程问题，能用推公式跑仿真解决的问题，就不要把时间花费在工程调参上！
对于用理论工具分析和解决工程问题这种工作思路，我的具体理解和实践经验可以将其分为五个步骤：问题定位与调研、理论分析与设计、仿真验证、试验验证、泛化。
问题定位与调研
问题定位与调研，即通过对问题的思考结合自己现有的知识体系，对问题在学科领域、技术方向层面进行定性。首先明确自己面对是哪个领域的问题，涉及哪些技术方向，并通过调研参考前人的工作，尝试从前人的工作中获得启发和引导。经过对问题准确的定位和广泛的调研，得到一个或多个正确且具体的前进方向，便获得了成功的先决条件。
在云台抗扰性能的尝试中的体现则是对抗扰控制的调研，通过调研我了解到了扰动观测器，并通过参考相关文献了解到其基本原理。
在自瞄系统设计中的体现之一是运动预测方案的拟定，如果我没有根据自己的思考和调研确定惯性系估计目标运动状态并预测的基本思路，那只有奇迹才能使这个自瞄系统最终具有有如此性能。
在平衡步兵机器人控制系统设计中的体现之一是系统建模的思路，具体就是对腿部五连杆机构动态的处理，如果不做这样的简化，那以我的力学水平根本无法推导出系统的模型。
理论分析与设计
理论分析与设计，要求在对问题有明确认识，并具有解决问题的基本方向后，通过理论知识得到问题的数学描述，即将工程问题通过理论分析转化为数学问题。并用理论方法得到这个数学问题的解，这个解或许是解析的也或许是数值的，也可能在严格意义上不算是解而是具有指导意义的结论，这都为我们进一步的工作提供了极大的帮助。
在自瞄系统设计中的体现之一之一是目标运动状态估计的实现，采用卡尔曼滤波器可以更灵活的应对 PnP 解算结果的噪声，并且通过异步量测的改动可以完美应对上下位机分工带来的异步问题。
在平衡步兵机器人控制系统设计中的体现之一是机器人的离地检测，通过对机器人进行力学分析得到的检测方案易实现效果好，极大提高了机器人在复杂环境下的运动表现。
仿真验证
仿真验证，主要目的是对理论分析与设计得到的结果进行初步验证，仿真环境相比实际环境具有灵活度高成本低等有点，非常适合用于理论结果的验证。
在云台抗扰性能的尝试中的体现则是对扰动观测器的仿真实验，仿真的过程除 了验证了我对扰动观测器的理解外，也加深了我对其思想的认识。
在平衡步兵机器人控制系统设计中的体现便是通过 Simscape Multibody 进行的两次仿真验证，帮我明确了具体设计方案的可行性。
在能量机关正弦信号参数估计中，仿真工作不仅起到了方案验证的作用，也起到了优化设计的作用。由于该参数估计问题不与实际物理环境进行交互，仅从物理环境中获取数据，这使得通过仿真进行设计优化是简单易实现的。
试验验证
试验验证，是在仿真验证后将相同的方法和设计部署到实际环境中进行验证。由于传感器测量噪声、模型误差、以及环境的不确定性与随机性，其相比仿真验证具有更高的难度，但也具有更大的价值和指导意义。
我的许多尝试在这一步以失败告终，比如我的在线参数辨识在仿真中勉强能收敛，但在实际实验平台上则完全无法收敛。
泛化
泛化，我也称为工程化，指将经过试验验证的方法和设计扩展到更广泛的平台与更复杂的环境中，比如在当前机器人上能良好运行的控制系统能否在新的同构性的机器人上具有同样的性能和鲁棒性，或在试验验证的场合中可行的方案是否能经得起复杂且严酷的赛场考验。这些问题使得泛化成为这整个流程中最困难的环节，也是最终的环节。
我的诸多项目均在这一步宣告失败，只有最终经过泛化的功能或项目，才能在赛场上真正留下自 己的足迹，告诉每一届参赛队员，我们曾经来过。