基于多信号融合的竞技机器人控制研究

朱梅　赖日海　罗昭明　樊中奎

摘  要：基于机器人大赛制作竞技机器人的需要，提出一种机器人软/硬件控制方案。该方案以STM32F407IGX开发板作为主控，由M3508无刷电机构成底盘驱动模块、M6020无刷电机构成云台驱动模块。根据机器人各个通信模块的数据传输特点及造价，采用CAN、SPI、IIC及UART等多信号融合组成通信系统。底盘模块设计侧重于移动功能，通过矢量分解公式实现全向移动，通过位置式PID算法闭环控制底盘电机转速；云台模块设计侧重Yaw轴电机和Pitch轴电机的控制及其他模块和主控间的通信，结合IMU姿态解算、Kalman滤波及串级PID算法控制云台电机，通过搭载于云台的图传模块得到第一人称视角控制机器人。

关键词：嵌入式算法；STM32F407；通信协议；无刷电机

中图分类号：TP242  文献标识码：A  文章编号：2096-4706（2023）20-0022-06

Research on Control for Competitive Robots Based on Multi-signal Fusion

ZHU Mei1， LAI Rihai1， LUO Zhaoming2， FAN Zhongkui1

（1.School of Software Engineering， Jiangxi University of Science and Technology， Nanchang  330013， China;

2.School of Energy and Mechanical Engineering， Jiangxi University of Science and Technology， Nanchang  330013， China）

Abstract： Based on the need of making competitive robots in robot competition， a software/hardware control scheme of robot is proposed. In this scheme， STM32F407IGX development board is used as the main control， M3508 brushless motor constitutes the chassis drive module， and M6020 brushless motor constitutes the cradle head drive module. According to the data transmission characteristics and cost of each communication module of the robot， the Controller Area Network （CAN）， Serial Peripheral Interface （SPI）， Inter-Integrated Circuit （IIC）， and Universal Asynchronous Receiver/Transmitter （UART） and other signals fuse together to form a communication system. The chassis module design focuses on mobile function， the omnidirectional movement is realized by vector decomposition formula， and the speed of chassis motor is controlled by position-type PID （Proportion Integral Differential） algorithm in closed loop. The design of cradle head module focuses on the control of Yaw shaft motor and Pitch shaft motor and the communication between other modules and main control. The cradle head motor is controlled by combining IMU attitude calculation， Kalman filtering and cascade PID algorithm. The robot is controlled from the first person perspective through the picture transmission module attached to the cradle head.

Keywords： embedded algorithm; STM32F407; communication protocol; brushless motor

0  引  言

機器人竞赛是推动机器人技术研发、培养机器人技术工程师的重要方式[1]。近年来，随着各种机器人竞赛的兴起，各参赛团体为了能够在比赛中获得良好的名次展开了激烈的竞争，对于机器人各方面的研究在不断地深入，对于相关技术也在不断地升级突破。机器人控制技术一直是机器人领域的重点研究方向，好的机器人控制方案的使用，不仅是机器人可以正常运行的保障，更是团队取得优异成绩的关键[2]。然而在实际设计过程中，很难找到匹配的机器人控制方案，对于不同的任务需求机器人的制作设计常常有很大差异，存在运行性能和开发成本的矛盾。如何设计一种合适的控制系统方案，在既能满足竞赛任务需求的同时花费最小代价，既是机器人竞赛的主题，也是竞赛团体研究的关键。

控制系统包括硬件系统和软件系统，硬件部分是嵌入式系统的基础，对嵌入式系统的性能有决定性影响，也决定了平台的功能和拓展性[3]。主控制板是机器人的控制中枢，选用合适的主控一方面可以降低成本费用，另一方面可以最大程度地满足机器人对算力的需求；通信系统是机器人协调各个模块正常运动的关键，针对机器人内部不同的通信线路，选择合适的通信外设可以提高通信效率、匹配该模块的要求响应速度以及降低开发成本。机器人的软件设计方案质量的优劣也会直接影响到机器人应用层质量的优劣与模块稳定性，其中各重要模块电机的控制方案更是软件设计方案中的关键问题之一。为了保证电机控制的精确性和稳定性，即电机能够快速准确地响应输入，不会出现抖动、疯转或失控等异常现象，需选取和设计一套合适的电机控制方案，其控制逻辑能够很好地满足比赛需求、易于维护和拓展，其控制算法能够在运用中很好地发挥出作用。

针对竞技机器人的控制方案展开研究，主要将从硬件层面和软件层面进行介绍和分析，包括主控制板的选型、机器人通信系统的设计；以及机器人的整体控制逻辑，并对其中关键模块展开讨论，介绍使用的嵌入式算法，并结合实际情况探讨可行性。

1  硬件设计

1.1  硬件设计思路

如图1所示，硬件控制方案以云台作为控制中枢，搭载主控制板通过CAN信号控制云台和底盘的电机及通过UART信号其他模块，主控内部间的通信采用IIC、SPI通信，主控和外设间的通信采用UART通信，主控和电机间的通信采用CAN通信，底盘和云台需要分别设计转接控制板起扩展、转接信号和供电的作用。放置于底盘的电源通过该转接控制板将电流输送给主控，主控对底盘的控制信号经过一条CAN线路，通过转接控制板分散给四个M3508电机，云台转接板仅负责云台电机的接口扩展，在云台电机和主控之间传递信号和供电。

1.2  主控模块选型及分析

鉴于比赛中对于机器人的操作要求和难度越来越高，即对于控制及响应的要求越来越高，因此选用一款高性價比的开发板是满足机器人快速响应和精确控制的重要硬件支持之一。如图2所示，选用的主控开发板采用高性能的STM32F407芯片，支持宽电压输入，集成专用的扩展接口，通信接口以及高精度 IMU 传感器。其具有如下外设：用户自定义LED、5 V接口、BOOT配置接口、micro USB接口、SWD接口、按键、可配置I/O接口、UART接口、CAN总线接口、PWM接口、DBUS接口、数字摄像头FPC接口、蜂鸣器、电压检测ADC、六轴惯性测量单元和磁力计。

1.3  通信配置及分析

实际开发过程中有多种通信方式可供选择，如何在众多通信方式中选择一种最优解实现两个模块间的通信以达到更好的效果是通信设计的重要讨论问题之一。需要从机器人自身的机械结构与通信线路的匹配程度、外设接口数量、数据传输内容、软件实现难易以及价格等多方面综合考虑。

1.3.1  UART通信协议

UART通信协议是一种较古老的全双工异步通信协议，由于价格便宜、方便使用且可靠性高仍广泛应用于嵌入式开发，它支持一段字符的连续传输，因此特别适合用于一次性传输大量数据串的场景。UART数据传输主要通过中断或直接内存存取（direct memory access， DMA）的方式实现[4，5]，如果对于传输速度没有很高的要求，UART通信无疑是十分合适的选择。遥控器对机器人的控制是通过和UART通信极其相似的DBUS通信协议进行控制信号的传输，每次传输的一帧有18个字节的数据，由于其传输数据量大的需要，采用UART协议作为控制信号传输方案，并配置DMA直接存取极大节约了CPU的读取时间。

1.3.2  CAN通信协议

CAN通信是一种串行通信总线方式，其传输速度高达1 M/s，CAN总线因具有实时性强、传输距离较远、抗电磁干扰能力好、成本低、检错能力强等优点，在现场总线中应用广泛[6]。总线一帧携带的数据达8个字节，拥有较高数据承载能力。机器人的驱动需要多个电机的同时配合、快速响应和精确控制，并且能够承受机器人在运动过程中可能受到的各种干扰信号，电机反馈给主控的数据较多，根据CAN协议的主要特点，故而对于主控模块与电机的通信使用CAN协议。

1.3.3  SPI通信协议

SPI是一种高速全双工同步通信总线，由设备选择线（CS）、时钟线（SCK）、串行输出数据线（MOSI）、串行输入数据线（MISO）构成，主要是在SCK的控制下进行双向数据交换[7]。SPI通信总线一次能传输一个字节的数据，适用于对数据传输量小但速度要求高的通信电路。BMI088高性能惯性测量单元是本方案采纳的主控制板中内置的一款传感器，它需要实时将测量的角度数据发送给主控以便调整姿态，由于其传输量小但速度要求高的特点，故而采用SPI协议。

1.3.4  IIC通信协议

IIC协议是一种半双工串行同步通信总线协议，具有两根信号线，一根是双向的数据传输线SDA，另一个是时钟线SCL，通过对SCL和SDA线高低电平时序的控制，产生IIC协议所需要的信号进行数据的传递[8]，具有接口少，控制方式简单，通信速率较高等优点[9]。机器人主控内置IST8310磁力计模块，需要发送给CPU磁偏角，并且主控模块内置温度传感器及ADC采集等监控机器人状态。此类传感器的数据传输量小、速度低，但数量较多，使用IIC协议既能够满足其正常的数据传输需求，又能减小其他性能高的外设的承载负担。

2  软件设计

2.1  软件整体设计

软件设计的主要任务是搭建框架、完成逻辑设计以及算法设计。框架的搭建借助软件快速生成实现底层以及中间层的工程目录，从而极大提高软件开发效率；基于此框架编写应用层代码，即完成逻辑设计，机器人需要根据控制信号完成各种操作，不仅仅是给电机固定不变的电流，对每个电机根据其需要实现的功能设计相应的一套控制逻辑；仅仅依靠逻辑程序很难做到如臂使指地控制电机，这势必会影响机器人速度的稳定性，或导致云台转动无力及高频振荡等影响机器人正常运行并损害电机的现象发生，故机器人的控制层需配合算法以保证机器人的正常稳定运行。软件总体结构如图3所示。

2.2  底盘控制方案

2.2.1  移动方案设计

机器人移动功能由四个M3508电机完成。遥控器给机器人发送一个向任意方向的某个速度的控制信号后，主控不会直接将该信号解析后发送给底盘电机让其以该数值转动，而是需要事先完成发送给机器人的原生信号到对单个电机控制信号的转换。由于四个全向轮的所处位置、安装方式的差异，每个车轮加上驱动电机都是一个独立的控制单元，每个单元需要对应的速度才能让机器人按指定速度移动。而电机的速度对应给电机的电流，因此可以把实际上的速度控制转变为给输出电流控制，如何从遥控器实时传回的数据中分别计算出四个电机的电流值是该模块软件设计重点，具体实现流程如图4所示。

所制作的机器人是在地面移动的，因此可以将地面看作为一个以机器人为中心，以机器人正前方向为y轴，正右方向为x轴的动态二维坐标系，机器人朝某一方向的速度可以看作一個起点在中心的速度矢量，前面提到，每个电机都是一个独立的控制单元，同样分别需要四个独立的坐标系及速度矢量。大坐标系的矢量速度，经过矢量分解得到四个小坐标系的速度矢量。图5是底盘分解公式，假定给机器人一个方向上的速度，以其为例进行分析，首先通过式（1）得到机器人整体速度在x轴和y轴上的分速度：

Vx = V cosα，Vy = V sinα                  （1）

其中，遥控器给机器人的速度为V，V与x轴的夹角为α，通过三角函数得到机器人在x轴分速度Vx和y轴分速度Vy。根据Vx和Vy分别求出四个电机速度：

Vleftup =  + Vx + Vy                                         （2）

Vleftup是左上位置电机速度，由图示电机安装位置可知电机需要顺时针转动，因此Vx和Vy都是正方向上的矢量相加。

Vrightup =  + Vx - Vy                                        （3）

Vrightup是右上位置电机速度，由图示电机安装位置可知电机需要逆时针转动，因此Vx是正方向，Vy是负方向。

Vleftdown =  - Vx + Vy                                       （4）

Vleftdown是左下位置电机速度，由图示电机安装位置可知电机需要顺时针转动，因此Vx是负方向，Vy是正方向。

Vrightdown =  - Vx - Vy                                     （5）

Vrightdown是右下位置电机速度，由图示电机安装位置可知电机需要逆时针转动，因此Vx和Vy都是负方向上的矢量相加。

2.2.2  底盘算法设计

得出各个电机的设定速度后，在实际测试中，如果仅仅依靠设定速度给电机一定的电流，通常达不到预期速度值，甚至速度会时快时满，这是因为没有考虑到地面阻力的影响。如何考虑上阻力因素的影响，让实际速度保持在一个较恒定的、极其接近设定速度的范围内，这就需要闭环控制，对此采取位置式PID算法，在给每个电机一个电流值前，需要先获取到电机当前实际速度，再比对设定速度得出二者间的误差，最后计算出应发送给电机的电流值，具体如何计算出电流值是PID算法的核心内容。

PID控制器正如其名，有三个控制系数P：比例、I：积分、D：微分。相比于其他简单的控制运算，PID控制器可根据历史数据及差别的出现率来调整输入值，这样可使系统更加准确，更加稳定[10]。误差在PID控制器中指设定目标值和实际目标值的查。比例系数的作用是根据误差值大小，按一定比例调节控制量，使系统快速响应。但是比例作用不能消除稳态误差。积分系数的作用是根据误差的累积量调节控制量，使系统达到稳态，从而解决稳态误差问题，但积分会导致超调和震荡。微分作用是根据偏差的变化率，预测未来的趋势，调节控制量，使系统更加平稳，但是微分作用会放大噪声和干扰。PID控制算法的输出结果是三个控制系数处理结果之和，故算法要解决的是如何得出比例输出值、积分输出值及微分输出值。以下是算法核心式，首先通过式（6）求得误差：

error = Inset - Inrel                      （6）

error是设定量Inset和实际量Inrel的偏差量（误差），即通过相减得出，反映了目标设定量和实际测量值的差距。

Pout = P×error                         （7）

Pout是比例输出值，其和误差呈线性关系，其值是一个比例系数P与error的乘积，比例输出值体现了响应的快慢程度。

Iout + = I×error                        （8）

Iout是积分输出值，是积分输出值和本次积分系数I与误差间乘积的相加，程序中Iout的积分求和是离散的，每次执行到该条语句进行一次积分值相加。

Dout = D（errorn - errorn-1）                 （9）

设本次是第n次执行，Dout是微分输出值，即微分系数D和此次运算误差errorn与上一次误差errorn-1间差的乘积。

out = Pout + Iout + Dout                                （10）

out是PID算法的最终输出，是式（7）（8）和（9）分别求得的Pout、Iout和Dout的求和。

2.3  云台控制方案

2.3.1  转动策略

云台是机器人的控制中枢，并搭载视觉系统，通过客户端连接到云台图传模块后，用户能够以第一人称视角操纵机器人，云台可以在水平上转动，也可以抬头和低头，为展现给用户稳定的视角，云台驱动电机的角度控制相比于底盘的速度控制精度要求更高，设计中采用能反馈当前角度的M6020无刷电机作为机器人的云台驱动模块，具体控制流程如图6所示。

机器人云台可以在Yaw轴0-360度范围内转动，但碍于机械结构的限制，在Pitch轴只能在一个角度区间内转动。用户给机器人Yaw轴方向上的控制信号，云台会根据值的大小以相应速度持续转动；同样，当用户给机器人Pitch轴方向上的控制信号，云台也会持续转动直到达到限制边界。这其中涉及两个技术点：如何从360°转到0°造成的角度突变问题以及如何限制电机在一定范围内转动。

角度突变问题在通过PID算法计算角度突变时的计算结果会有极大偏差，容易造成云台失控快速转动，其主要原因是从0°到360°的巨大误差破坏了原本的连续性，故而将从本次的≤360°到下一次的＞0°或从本次的≥0°到下一次的＜360°的突变情况转换成连续情况：添加一个虚拟设定角度参与到PID计算，如果设定角度＞0°，在PID計算中是从实际角度的≤360°到虚拟设定角度的＞360°，在软件上云台转到了＞360°的位置，但实际上是转回到＞0°的某个位置，从另一方向上的突变情况同理。而边界限制问题是防止云台在Pitch轴上持续转而不动造成电机烧毁发生，故而需要设定俯仰区间保证电机只能在一定范围内转动，当持续施加给电机转动信号时，如果设定目标值已经超出边界值，则实际目标值等于边界值，两个边界值根据实际测试结果得出。

2.3.2  算法设计

云台电机的输出值融合三个控制算法：IMU姿态解算将获取到的主控内置BMI088传感器的角速度值和角加速度值，将BMI088传感器的多轴数据从四元数形式变换为欧拉角进行计算[11]，计算得到云台在Yaw轴、Pitch轴以及Roll轴的姿态信息。姿态角反映了云台当前的绝对角度，通过姿态角控制云台电机从而摆脱对底盘的依赖，即不再受地形起伏以及底盘转动的影响。串行PID算法是以嵌套的方式在速度环的基础上增加了角度环从而提高了电机的稳定性和响应速度，很适合对控制精度要求极高的云台电机。

在用PID算法计算出结果前，需要先对设定角度进行滤波，在此用到Kalman滤波器。卡尔曼滤波算法是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，它是一种最优化自回归数据算法，是描述全部计算更新数据的过程，提供一组有效的递归推算方程组来估计过程的状态量，使估计均方差误差最小化[12]。Kalman滤波器通过对动态系统建立一个模型，假设系统状态是该模型内的一个变量，通过实时获取的测量值更新状态变量的概率分布，从而在含有噪声干扰的测量值中得出动态系统的最优估计量。假设本次是第k-1次计算，根据式（11）获取第k次系统状态的预测值：

其中x表示系统的状态向量，F表示状态转移矩阵，B表示控制矩阵，u表示控制向量。之后通过式（12）估计预测误差：

P表示误差协方差矩阵，Q表示过程噪声协方差矩阵，T表示转置矩阵。通过式（13）计算卡尔曼增益：

其中K表示卡尔曼增益矩阵，H表示观测矩阵，R表示观测噪声协方差矩阵。之后计算式（14）更新系统的状态：

其中z表示观测向量。最后通过式（15）更新误差协方差矩阵：

其中I表示单位矩阵。至此求得第k次的误差协方差。

3  结  论

提出的机器人控制设计方案详细阐述了一种竞技机器人的硬件与软件设计过程和使用的策略，包括硬件重要模块的选型、电路设计以和分模块化设计控制逻辑、对嵌入式算法的分析及在机器人契合使用。机器人在场地测试和实际比赛过程中都能够流畅无误地接收命令并稳定地实现相应功能，充分体现了该机器人控制方案的可行性和可靠性，为机器人竞赛提供了一种较为实用的控制方案。

参考文献：

[1] 王旭.竞技机器人比赛裁判原则与技术 [J].机器人技术与应用，2022（2）：43-48.

[2] 宋宇航，娄照坤，任雨婷，等.Robomaster比赛工程机器人控制系统 [J].中国科技信息，2022（15）：44-47.

[3] 刘放，刘蕾.模块化机器人控制系统的设计与实现 [J].信息技术与信息化，2020（1）：34-36.

[4] 邵伟恒，顾昂，隋俊杰.基于STM32的便携式恒流源设计 [J].自动化与仪表，2015，30（12）：29-32.

[5] 邢丽华，高志鹏，袁德殿.基于散列DMA的高速串口驱动设计 [J].单片机与嵌入式系统应用，2010（4）：40-42.

[6] 邱小军，刘浏，吴超，等.水下机器人通信系统的CAN总线冗余设计 [J].自动化仪表，2012，33（11）：23-26.

[7] 盛杨博严.基于SPI总线的Arduino显示与控制模块设计 [J].单片机与嵌入式系统应用，2020，20（3）：74-76+80.

[8] 董雪.基于FPGA的SPI与ⅡC总线通信系统的设计与实现 [D].银川：宁夏大学，2017.

[9] 李斯祺，曾荣鑫.基于STM32的红外传感器的数据采集系统的设计 [J].工业控制计算机，2018，31（8）：30-31+33.

[10] 王露峰.基于PID控制的电动汽车直流驱动电机调速研究 [J].汽车实用技术，2020（10）：106-108.

[11] 刘晓宇，孟瑞锋，贾伟.四旋翼飞行器姿态解算研究及最优化设计 [J].南方农机，2018，49（15）：40-41.

[12] 余尔标，陈宏华.一种有缆式四轴水下机器人的设计与制作 [J].江西电力职业技术学院学报，2022，35（10）：142-144.

作者简介：朱梅（1980—），女，汉族，江苏江阴人，中级实验师，硕士研究生，研究方向：人工智能；赖日海（2001—），男，汉族，江西赣州人，本科在读，研究方向：機器人控制、软件开发；樊中奎（1980—），男，汉族，江西南昌人，讲师，硕士研究生，研究方向：人工智能。

收稿日期：2023-04-10

基金项目：江西理工大学大学生创新创业训练资助项目（202110407039）