基于FA-A*优化算法的实验样品配送机器人控制系统设计

钟 昊，丁仲熙

(1.长沙海关技术中心，长沙 410004；2.西北工业大学 明德学院，西安 710124)

0 引言

实验样品是指用于实验过程中的材料，一般存放于规定容器和条件下，在实验过程中需要将所需的实验样品运输至实验执行地点，然而由于实验中使用的样品数量较大，样品类型较多，使用人工运输的方式存在工作效率低、工作量大、样品在运输途中被损坏等问题，为此在实验室中引入配送机器人。配送机器人指的是用于物品配送的人工智能机器人，而配送就是根据用户需求，采用信息管理和调度的方式将所需物品传输给目标用户的方式。配送机器人是一种可以在有障碍的工作空间中进行自主操作，从而完成预定的工作任务的机器人系统。通过配送机器人的应用，有效提高实验室的样品获取速度和精准度，同时降低实验样品的搬运难度。

配送机器人的移动精度直接影响了实验室样品的配送效果，为保证配送机器人能够按照预期方案执行配送任务，设计并开发了配送机器人控制系统。现阶段发展较为成熟的机器人控制系统主要包括：文献[1]提出的基于数字化技术的机器人控制系统、文献[2]提出的基于ROS的机器人控制系统以及文献[3]提出的基于改进模糊PID的机器人控制系统，其中文献[1]提出系统在设备端、云平台端、工业网关等硬件设备的支持下，采用Neptune云平台实现设备的管理功能，同时采用GTS运动控制卡，接收云平台端的控制指令及控制电机。文献[2]提出系统在RVIZ可视化工具中，对机器人进行了建模，并将其输出到统一的机器人说明格式文件中。改进快速拓展随机树算法以综合考虑角度和距离的权重，将改进RRT算法作为机器人控制系统的运动避障规划器。另外文献[3]提出系统以机器人的状态空间模型为基础，将跟踪误差信息和电机电压作为输入、输出来设计控制器，同时设计了一种论域放大因子优化的模糊控制器，并通过试验测量，初步确定了控制参数的取值范围。引入逆向学习算法，对个体进行初始化，并设计加权系数，以控制个体与个体距离较近的个体所占的比重，避免个体出现早熟现象。然而上述传统的机器人控制系统主要针对的是工业搬运机器人，将其应用到实验样品配送机器人的控制工作中存在明显的控制效果不佳的问题，主要体现在控制误差大、应用性能差等方面，为此引入FA-A*优化算法[4-7]。

FA-A*优化算法也就是改进萤火虫算法(FA，firefly algorithm)，是一种启发式算法，通过模拟萤火虫个体发光机制实现信息共享，利用荧光强度较高的个体对荧光强度较低的个体的吸引作用，使得荧光强度较高的个体对荧光强度较低的个体的趋向性，从而达到群体的最优进化[8-9]。本文在经典FA算法基础上，提出FA-A*优化算法，强化算法的路径寻优性能，对实验样品配送机器人控制系统进行优化设计，以期能够提升系统的控制效果[10-12]。

1 实验样品配送机器人控制系统的硬件部分设计

优化设计的实验样品配送机器人控制系统中硬件系统用来支持软件功能的实现，主要由控制器、传感器、处理器以及驱动器等多个部分组成，其中控制器是控制功能的执行元件，传感器用来获取配送机器人的实时运行状态数据，处理器用来分析机器人状态，为控制指令的生成提供参考，驱动器为控制指令的作用对象，实现对配送机器人各个运动模块驱动控制。为了满足FA-A*优化算法和配送机器人控制功能的运行要求，需要对硬件系统中的部分元件进行优化设计[13-15]。实验样品机器人的控制系统的硬件部分以控制器为中心，还包括位姿传感器、数据处理器、电机驱动器，各模块的连接情况如图1所示。

图1 硬件部分的总体架构设计

1.1 配送机器人位姿传感器

为了实现对实验样品配送机器人实时运行状态的精准采集，对内设在机器人中的位姿传感器进行改装优化[16]。优化设计的位姿传感器由位置反馈元件、视觉元件、测距元件等部分组成，实现配送机器人多维度位姿信息的采集[17-18]。

图2为位姿传感器中位置反馈元件使用旋转变压器，该元件是基于互感原理工作的。从电动机的工作原理出发，可以把转子看作具有转动能力的变压器，其定子和转子可以看成原边和副边绕组。转子的角度变化时，原边绕组与副边绕组间的电磁场耦合强度也随之变化，因此，转子的角度变化将直接影响到转子绕组输出电压的幅值与相位[19-20]。传感器中的视觉元件用来完成配送机器人对电梯按钮的识别功能，要求装设的视觉元件具有较高稳定性和拍照灵敏度，优化设计系统中选用ac A1300-30 uc型号的CCD相机，并组装RICOH镜头。测距元件主要用来测量实验样品配送对象与机器人执行终端之间距离，选用测距元件利用激光来精确地测量目标距离的测距元件。此测距单元支持动态测量、可预设测量模式、随时待机等功能，并有一般小型物品自动储存和超大型自动储存两种测量模式，并可在周围环境温度从零下40～50 ℃之间使用。将上述元件在电源电路的支持下按照图1表示结构进行连接，完成配送机器人位姿传感器的优化设计。

图2 配送机器人位姿传感器组成结构图

1.2 配送机器人数据处理器

选择嵌入式S3C2410X微处理器作为实验样品配送机器人实时运行数据的分析元件，是16/32位精简指令集微处理器，采用了0.18 μm技术标准，在数据处理器中的存储器通过每行8字节长度的独立16 K指令缓存和数据缓存，实现了MMU功能、AMBA总线及Harvard缓存结构。

1.3 配送机器人电机驱动器

优化设计系统的机器人电机驱动器由光耦，晶体管的H桥，门电路等构成。其中，光耦合实现了控制与驱动之间的隔断，MOS管的H桥元件与门电路实现了对电动机运行状态的控制。驱动器内部结构如图3所示。

图3 配送机器人电机驱动器内部结构图

如果三极管K2、K3被接通，则由供电正极经K3的电流由右至左流过电机，再经K2返回供电负极，从而使电流向驱动电机的方向逆时针方向转动。通过对4个三极管开关的控制，实现了对配送机器人电机运行状态的调整。电机驱动器拥有双路电机接口，一块板子就能驱动两路电机，每路额定输出电流可达7 A，配送机器人采用的直流电机在全速运转时消耗的电流大约为1 A，该模块满足驱动要求。配送机器人电机连接到相应的驱动器，再通过CAN收发器连接到底盘控制板的CAN端口，通过发送数据帧的方式控制电机。

1.4 配送机器人控制器

配送机器人控制器通过各个终端与外部执行单元相连接，进行数据的交换和控制信号的发送，并在机器人控制器中的各个分支程序的基础上，适时地调用各个子系统单元来进行相应的操作。优化设计系统中使用的控制器增加一个DSP扩展功能，以2片CY7C1021VC33芯片为核心，对128 K静态 RAM进行了扩充，以提高 DSP的存储能力。为保证系统能够持续、稳定的为硬件元件提供电力支持，同时避免作用至配送机器人不同关节控制指令之间产生相互干扰，在系统中加设信号隔离电路，并对电源电路进行调整。优化设计实验样品配送机器人控制系统能够提供6 V和3.3 V两个等级的电压，满足驱动器、传感器、控制器等硬件设备正常工作条件。在稳压芯片的作用下，输出不同电压等级的信号，并将其发送至不同的执行终端。

2 实验样品配送机器人控制系统软件功能设计

在系统硬件设备的支持下，综合考虑实验样品的配送任务需求以及当前配送机器人的工作姿态生成相应的系统控制任务，并在机器人运动学理论的支持下，实现相应的控制功能。配送机器人的配送任务的执行原理为：配送机器人利用内置的视觉传感器设备，获取实验样品的位置，根据当前配送机器人末端执行器与实验样品之间的位置偏差，得出配送机器人位置控制指令的生成结果，另外，根据实验样品的需求速度生成机器人移动速度控制指令，最终通过硬件系统中控制器的执行，实现对配送机器人的控制功能。

2.1 搭建配送机器人移动环境模型

搭建配送机器人移动环境模型的目的是确定配送机器人的移动与控制范围，并实现对实验样品以及配送机器人实时位置信息的量化表示。采用栅格法搭建配送机器人移动环境模型，栅格法是将机器人工作区域均匀地划分为若干网格，网格的数值代表了该区域内的障碍物分布情况。机器人可以在无障碍分布的网格间自由地穿梭，因为网格法是对每一个网格都进行了编码，所以路径规划问题就变成了寻找两个不同编码的网格间的最佳路径，这样就可以利用多种优化方法来寻找路径，满足FA-A*优化算法的运行要求。由于栅格法是将环境地图离散化处理的，会使栅格地图与实际区域之间存在较大差异，这种偏差主要受栅格尺寸影响。假设实验样品配送机器人的移动区域为方形区域，则栅格尺寸的具体设置情况如下：

κgrid≥2U

(1)

式(1)中，U为实验样品配送机器人中心点与其最远轮廓点之间的距离，根据栅格尺寸参数的设置结果，得出移动环境中每列和每行的栅格数量表示为：

(2)

式(2)中，H和L分别为配送机器人实际移动环境的长度和宽度，将公式(1)的计算结果代入式(2)中，得出移动环境模型中栅格数量的求解结果。在划分栅格的路径区域中，构建直角坐标系，从左上方的第一个栅格开始，以向右、向下递增的排列方式对栅格进行编号。并确定任意栅格的具体位置坐标为：

(3)

式(3)中，ymod()和yceil()分别为取余运算函数和向上取整运算函数，b为移动环境模型中任意栅格的编码值，同理可以得出模型中所有栅格的位置坐标，由此完成对配送机器人移动环境模型的搭建。

2.2 识别实验样品配送对象位置

在搭建的配送机器人移动环境下，利用视觉传感器设备获取移动环境中所有实验样品图像，通过特征提取与匹配，判断该图像中是否存在目标实验样品，以目标实验样品作为配送对象，确定其在移动环境模型中的位置坐标。配送机器人采集到的视觉图像可以表示为：

I(x，y)=(xenvϑ，yenvϑ)

(4)

式(4)中，xenv和yenv分别为移动环境模型中任意事物的实际坐标，ϑ为视觉传感器的工作系数。以公式(4)表示的视觉图像为研究对象，对其中的颜色与纹理特征进行提取，其中颜色特征的提取结果为：

(5)

式(5)中，n(ci)为视觉图像中第i个颜色分量的像素个数。另外，环境图像中纹理特征的提取结果为：

(6)

式(6)中，g(x，y)为环境图像中(x，y)位置上的像素值，gmax和gmin为像素值的最大值和最小值，Lg为图像梯度阈值。对式(5)和式(6)的特征提取结果进行融合，并将其与目标实验样品特征进行匹配，匹配结果如下：

(7)

式(7)中，τcon为环境图像的融合特征，τExperimental samples为目标实验样品对应图像的标准特征。若计算得出特征匹配值s(i)高于阈值s0，说明当前视觉图像中存在待配送的实验样品，否则认为图像中不存在配送对象。针对满足特征匹配条件的视觉图像，提取其像素点位置，并在配送机器人移动环境模型中确定实验样品配送对象的实际位置坐标。

(8)

式(8)中，κmapping为视觉图像与环境之间的映射关系，(xs，ys)表示的是满足特征匹配条件的像素点位置，也就是视觉图像中实验样品对应的像素位置。按照上述流程即可得出实验样品配送对象位置的识别结果。

2.3 利用FA-A*优化算法规划机器人配送路径

以当前配送机器人位置为起始点，识别得出的实验样品配送对象位置为终止点，利用FA-A*优化算法规划机器人的配送路径。FA-A*优化算法的基本原理如图4所示。

图4 FA-A*优化算法原理图

从图5中可以看出，分别将三只萤火虫分别设定为a、b、c；a的搜寻半径大于b，并且b在a的搜寻距离之内，若a的光度大于b，b就会朝a的方向移动，反之，a的光度大于b的方向，b就会朝a方向移动。但c不同，a和c的亮度不在a的搜索范围之内，所以a和c的亮度不会互相移动。在实际的路径规划过程中，可将FA-A*优化算法的分为4个阶段，其中部署阶段是在待求的目标函数的可行区域内随机部署萤火虫，并在部署初期假定萤火虫的荧光强度和感应半径均为相等。假设萤火虫群的规模为Nfirefly，在搭建的机器人移动环境模型中，第i只萤火虫的位置可以表示为一个多维矢量。在FA-A*优化算法中，每个萤火虫的发光强度反映了它们在当前状态下的最优解。在每只萤火虫都被移走一次之后，下一个循环就会开始。在萤光点的更新阶段，每个萤光点都会被添加到其先前的萤光点上，并与其对应的目标函数的数值成正比关系。荧光素值更新公式如下：

图5 动态和静态障碍物场景设置

w(t+1)=(1-κattenuation)w(t)+γδ

(9)

式(9)中，w(t)和w(t+1)分别为t和t+1时刻的亮度值，κattenuation为亮度衰减常数，γ和δ分别对应的是比例常数和适应值。萤火虫位置更新结果即为配送机器人路径点位置生成结果，在此阶段，所有的萤火虫将以决定区域的半径及荧光强度为基础，生成一组邻近的萤火虫，并在邻近的一组中寻找比自己更明亮的萤火虫，最终以几率选取一组萤火虫为运动对象，并向该对象移动。任意时刻萤火虫的移动位置更新结果可以表示为：

(10)

式(10)，vi(t)和vj(t)分别为当前节点i与其邻居节点j在t时刻的位置值，β为步长。在该算法的实施中，在更新了位置后，萤火虫将根据其当前位置的邻域密度来更新该区域的判定半径。在不改变密度的情况下，决策区的半径也不会改变。通过FA-A*优化算法的运行，得出机器人配送路径的生成结果为：

(11)

将式(10)的输出结果代入式(11)中，即可得出实验样品机器人在目标移动环境中的配送路径生成结果，也就是机器人位置的控制目标。

2.4 跟踪配送机器人实时位姿

利用硬件系统中的传感器设备对配送机器人的实时位置与姿态进行跟踪测定，配送机器人执行终端位置信息的跟踪结果为：

(12)

式(12)中，(xDelivery(t-1)，yDelivery(t-1))为前一时刻配送机器人的位置坐标，υ为机器人的移动速度，Δt为前后时刻之间的时间差，θx和θy分别为移动方向角在水平和竖直方向上的分量。同理可以得出配送机器人所有关节与执行设备的实时运行数据，即完成对机器人位姿的跟踪。

2.5 计算实验样品配送机器人控制量

实验样品配送机器人的控制量就是预期目标与机器人实际位置之间的偏差，其中位置控制量的计算公式为：

(13)

式(13)中，xroute(t)和yroute(t)为规划配送路径中t时刻对应的机器人位置坐标。按照上述方式将实时位姿的跟踪结果和配送路径的规划结果代入到公式(13)中，按照上述计算原理得出位置与姿态角控制量的计算结果。另外通过机器人实际移动速度和目标移动速度的差值计算，即可得出机器人速度控制量的计算结果。

2.6 实现实验样品配送机器人控制功能

2.6.1 位置/速度控制

将计算得出的实验样品配送机器人控制量输入到硬件系统的控制器中，由控制器生成相应的控制指令，将其作用在配送机器人的各个执行元件上。在实际机器人的位置与速度控制过程中，需要考虑控制量的正负情况，若控制量取值为正，则向当前机器人的移动方向进行控制与调整，否则需要向当前机器人工作方向的反方向进行控制。

2.6.2 平衡控制

在配送机器人移动过程中，由于实验样品负载的施加，导致机器人出现平衡不稳的情况，因此需要结合配送实验样品的质量对机器人的重力分布进行调整，从而实现对配送机器人移动平衡的控制。在实验样品的作用下，配送机器人的平衡偏角可以表示为：

(14)

式(14)中，MDelivery robot和MExperimental samples分别为配送机器人质量和实验样品质量。配送机器人的角度补偿量与式(14)的计算结果一致，但方向相反，由此实现对配送机器人移动平衡的控制。

2.6.3 自主搭乘电梯

针对多层实验室环境，需要跨楼层配送实验样品，此时需要配送机器人具有自主搭乘电梯的能力。首先利用配送机器人内置的传感器设备对电梯及其按钮进行识别与定位，将提取出的视觉图像特征与电梯及电梯按钮标准特征进行匹配，匹配过程与式(7)一致，通过匹配度系数与阈值条件之间的比对，确定电梯的位置信息，电梯位置的定位结果为：

(15)

式(15)中，dIx和dIy分别为图像中电梯像素点与成像终端之间距离在水平和竖直方向上的分量，(xI，yI)为图像中电梯像素的位置坐标；再确定配送机器人的角度φI：

(16)

式(16)中，MI Delivery robot和MI Experimental samples分别为对应配送机器人质量和实验样品质量。确定机器人的角度和位置信息，利用FA-A*优化算法规划配送机器人当前位置到达电梯位置的路径，并给目标楼层对应按钮施加一个压力，当电梯到达目标楼层且离开电梯后，完成配送机器人自主搭乘电梯操作。当配送机器人将指定实验样品配送至目标位置后，退出控制系统的软件运行程序。

3 系统测试

为了测试优化设计的基于FA-A优化算法的实验样品配送机器人控制系统的控制功能，分别在静态障碍物和动态障碍物环境下进行系统测试，其中静态障碍物环境就是在实验环境中安装多个大小、形状不同的障碍物，且在实验过程中障碍物不存在明显位移，而动态障碍物环境则是按照一定的移动速度，改变障碍物的实时位置。在上述两种实验环境下，得出不同系统控制下配送机器人的移动位置信息，从而得出反映系统控制功能的测试结果，静态、动态障碍物情景设置及障碍物移动方向，如图5所示。

3.1 选择配送机器人控制样机

根据实验室实际配送机器人的使用要求，选择六轮移动配送机器人作为系统的控制目标，该配送机器人由车体、摆臂和转动轮等多个部分组成，机器人的整个控制系统都安装在车体内，摆臂将车体与车轮连接起来，每一个摆臂都能绕车体进行360°摆动，极大地提高了机器人的灵活性，通过对各摆臂的摆角的控制，能够使机器人在空间中得到一定的姿态，从而能够保证机器人车体在非平坦路面上的水平。在每一台摆臂的驱动电机的后方，均装有一台电磁失电制动装置，可以随时对摆臂进行锁止，在突然停电时，制动装置将自动锁止，提高了对车身的支撑可靠性。6个轮子各自独立运行，使其在各种地形条件下的机动性能得到极大的提升，轮子采用泡沫轮，具有较好的环境适应能力。选择配送机器人样机的最大负载能力为1 000 kg，在空载和满载状态下移动速度最大可以达到5 m/h和3.6 km/h。

3.2 生成实验样品配送任务

选择某三层生化实验室作为实验环境，每层之间的间距为6 m，选择该实验室中所有的生化试剂作为待配送的实验样品，具体包括血清样本、病毒学拭子样本、污水样本、岩心样品等，以上述实验样本为配送对象，根据实验需求生成系统测试实验中的配送任务，部分配送任务的生成情况如表1所示。

表1 部分实验样品配送任务生成表

表1表示位置所在环境模型的坐标系，单位为m，配送目标位置均为位于实验室三层的操作室，同理可以得出实验中所有样品配送任务的生成结果，并生成具体的配送路线，得出任意时刻机器人预期位置的确定结果。

3.3 安装与调试控制系统硬件设备

根据实验样品配送机器人控制硬件系统的设计结果，将相关的硬件设备安装在配送机器人样机及其移动环境中，将所有的硬件设备与供电电源连接，实现硬件设备的连接。在此基础上，对配送机器人样机以及安装的所有硬件设备进行调试，设备调试工具选择型号为DSO-X2002A示波器，用于系统电路、控制器端口和时序的测试，判断输出波形是否与预期波形一致。另外，还需要对软件程序和硬件设备进行联合调试，保证软硬件之间的适配度。

3.4 设定FA-A*优化算法运行参数

为保证优化设计实验样品配送机器人控制系统对FA-A*优化算法的成功调用，需要对萤火虫的初始相对荧光亮度、萤火虫间的吸引度等参数的具体取值进行设置，其中萤火虫数量参数设置为100，初始亮度和吸引度参数均设定为1，最大迭代次数为50，光强吸收系数为0.1，将上述FA-A*优化算法运行参数设定结果以数据形式导入到系统软件运行程序中。

3.5 系统测试过程

在实验开始之前对配送机器人的移动环境进行设置，将其划分成静态场景和动态场景两个部分，将准备的实验样本放置在指定的初始位置上。系统以WindowsXP为软件开发的平台，ABB的RobotStudioOnline是配送机器人控制程序开发的工具，完成系统软件程序的开发。将表1中生成的配送任务逐一输入到系统运行程序中，得出相应的控制结果。实验样品配送机器人控制系统的运行界面如图6所示。

图6 实验样品配送机器人控制系统运行界面

同理可以得出所有配送任务的控制执行结果。为体现出优化设计系统在控制功能方面的优势，设置传统的基于数字化技术的机器人控制系统和基于ROS的机器人控制系统作为实验的两个对比系统，在相同的实验环境下得出对比系统对配送机器人的控制结果数据。

3.6 设置系统测试指标

此次实验分别从配送机器人的位置和速度控制误差两个方面进行测试，其中位置控制误差的数值结果为：

ε=|xcontrol-xtarget|+|ycontrol-ytarget|+|zcontrol-ztarget|

(16)

式(16)中，变量(xcontrol，ycontrol，zcontrol)为控制系统作用下配送机器人对实验样品的实际送达位置坐标，(xtarget，ytarget，ztarget)为实验样品的预期送达位置，该变量的具体取值由表1生成的配送任务决定。另外速度控制误差的测试结果如下：

ευ=|υcontrol-υtarget|

(17)

其中：υcontrol和υtarget分别为配送机器人移动速度的控制值和目标值。最终计算得出位置控制误差和速度控制误差的值越小，证明对应系统的控制功能越优。

3.7 系统测试结果与分析

3.7.1 静态障碍物场景下的机器人控制功能

在静态障碍物场景下，通过相关数据的统计以及式(16)的计算，得出配送机器人位置控制误差指标的测试结果，如图7所示。

图7 静态场景下机器人控制系统位置控制误差测试结果

从图7可以看出，与两种对比系统相比，在优化设计系统的控制作用下，能够有效降低配送机器人的位置控制误差，即配送机器人将实验样品的实际送达位置与目标位置之间的偏差更小。在该场景下，配送机器人速度控制误差的测试结果，如表2所示。

表2 静态场景下配送机器人速度控制误差测试数据表

将表2中的数据代入式(17)中，计算得出3种控制系统下配送机器人的速度控制误差分别为0.55、0.29和0.05 m/s。

3.7.2 动态障碍物场景下的机器人控制功能

静态和动态障碍场景下配送机器人的配送任务一致，在动态障碍场景下，统计配送机器人的实际运行数据，通过式(16)和式(17)的计算，得出反映动态障碍物场景下机器人控制功能的测试对比结果，如图8所示。

图8 动态场景下机器人控制系统控制功能测试结果

从图8中可以直观地看出，与两种传统系统相比，优化设计基于FA-A优化算法的实验样品配送机器人控制系统的位置控制误差和速度控制误差均得到明显下降。

4 结束语

配送机器人是实验室中重要的组成设备，能够降低实验样品丢失、调换、泄漏等风险，在一定程度上提高了实验的安全性和精准度。在此次研究中，利用FA-A优化算法实现对实验样品配送机器人控制系统的优化设计，为配送机器人的工作提供约束条件。然而在系统测试中，针对的是空载状态下的配送机器人，未考虑配送机器人对其他物品的负载情况，针对这一问题还需要在今后的研究工作中进一步补充。