一种室内通用多移动机器人算法验证系统的制作方法

本申请涉及室内机器人控制的技术领域，具体而言，涉及一种室内通用多移动机器人算法验证系统。

背景技术：

近年来，多机器人系统的协同控制算法受到广泛关注，研究了多种分布式控制算法来解决从环境监测到集体材料处理等各种各样的任务。这些发展是由算法改进，硬件小型化和成本降低共同推动的，并且已经开发出许多引人注目的多机器人测试平台。

目前已有的多机器人测试平台，大多数都是基于gps开发，需要在室外开阔的场地中运行，无法在室内完成对算法的测试。部分在室内开发的基于视觉的多机器人算法测试平台，由于布置了运动捕获系统，导致平台价格昂贵，不利于推广拓展。同时，现有的多机器人测试平台无法同时进行多机器人体系结构、通信拓扑结构的研究，且机器人类型固定，不能进行异构机器人协同控制研究。综上所述，所以亟需一种可靠、灵活、可拓展、价格便宜方便其它研究者应用的多机器人平台。

技术实现要素：

本申请的目的在于：实现了无gps情况下室内多机器人算法验证系统能够稳定运行，提高了全向机器人测距的准确性和全向机器人移动的安全性。

本申请的技术方案是：提供了一种室内通用多移动机器人算法验证系统，包括测试平台、至少两个全向机器人和至少四个锚点，两个全向机器人被配置为编队后根据锚点、协同向目标点移动，测试平台，包括：无线定位模块，定位算法模块，避碰模块和运动模块；无线定位模块用于确定全向机器人与锚点之间的距离；定位算法模块被配置为根据距离和锚点的第一坐标，采用三边定位法，确定全向机器人的第二坐标；避碰模块被配置为根据第一坐标和第二坐标，计算全向机器人之间的排斥力以及、全向机器人与锚点之间的引力，避碰模块还被配置为当判定排斥力等于引力时，根据扰动函数调整排斥力；运动模块被配置为根据排斥力和引力，计算全向机器人的运动速度和运动方向，控制全向机器人向锚点移动。

上述任一项技术方案中，进一步地，避碰模块还被配置为：当判定排斥力大于引力时，根据全向机器人与锚点之间的距离，修订排斥力，其中，排斥力的修订函数为：

式中，f″b(qa)为修订函数，kb为预设排斥力系数，d(qa,qc)为全向机器人a和锚点c之间的距离，d(qa,qb)为全向机器人a和全向机器人b之间的距离，e(qb,qa)为全向机器人b指向全向机器人a的单位矢量，qa为全向机器人a的第二坐标，qb为全向机器人b的第二坐标，qc为锚点c的第一坐标，ρr为预设壁障半径，其中，设定全向机器人b位于锚点c的附近，全向机器人a向锚点c靠近。

上述任一项技术方案中，进一步地，测试平台，还包括：奇异值滤除模块；奇异值滤除模块被配置为根据最小协方差行列式，计算距离的马氏距离，奇异值滤除模块还被配置为，当判定马氏距离大于预设阈值时，将对应的距离标记为奇异值、且滤除奇异值，奇异值滤除模块还被配置为，将滤除奇异值后的距离发送至定位算法模块。

上述任一项技术方案中，进一步地，测试平台，还包括：通信模块；通信模块用于测试平台与锚点之间、测试平台与全向机器人之间以及、两个全向机器人之间的数据传输，其中，数据包括距离、第一坐标、第二坐标、排斥力和引力。

上述任一项技术方案中，进一步地，全向机器人上还设置有激光传感器，激光传感器用于检测全向机器人运动方向上是否存在障碍物以及、全向机器人与障碍物之间的障碍距离；运动模块还被配置为，当判定障碍距离小于或等于距离阈值时，调整运动速度和运动方向，控制全向机器人避开障碍物、且向锚点移动。

上述任一项技术方案中，进一步地，无线定位模块为超宽带模块。

本申请的有益效果是：本申请通过设置无线定位模块，并将无线定位模块配置为超宽带模块，不仅实现了在室内环境中gps失效情况下，全向机器人与锚点间的距离检测，还提高了距离检测的精确度，有利于提高对机器人移动算法验证的可靠性。通过设置避碰模块，有利于降低全向机器人因算法问题发生异常碰撞的可能性，通过设置奇异值滤除模块，降低了无线定位模块测距后数据出现异常的可能性。

附图说明

本申请的上述和/或附加方面的优点在结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1是根据本申请的一个实施例的室内通用多移动机器人算法验证系统的示意框图；

图2是根据本申请的一个实施例的排斥力和引力计算的示意图；

图3是根据本申请的一个实施例的三个全向机器人编队跟踪圆轨迹试验的移动轨迹坐标监测图；

图4是根据本申请的一个实施例的三个全向机器人编队障碍物躲避试验的移动轨迹坐标监测图。

具体实施方式

为了能够更清楚地理解本申请的上述目的、特征和优点，下面结合附图和具体实施方式对本申请进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互结合。

在下面的描述中，阐述了很多具体细节以便于充分理解本申请，但是，本申请还可以采用其他不同于在此描述的其他方式来实施，因此，本申请的保护范围并不受下面公开的具体实施例的限制。

以下结合图1至图4对本申请实施例进行说明。

如图1所示，本实施例提供了一种室内通用多移动机器人算法验证系统，包括测试平台10、至少两个全向机器人和至少四个锚点，两个全向机器人被配置为编队后根据锚点、协同向目标点移动，测试平台10，包括：无线定位模块11，定位算法模块12，避碰模块13和运动模块14；无线定位模块11用于确定全向机器人与锚点之间的距离；

优选地，无线定位模块11为超宽带模块。

具体地，将待验证的机器人移动算法植入本实施例中的算法验证系统，对全向机器人进行编队，并设定任一个全向机器人作为领队，带领编队后的全向机器人编队根据设置好的四个锚点进行三维定位，向目标点移动，以便于验证植入的机器人移动算法是否可靠。并在作为领队的全向机器人和设定好的锚点中设置无线定位模块11，由无线定位模块11对全向机器人编队和锚点之间的距离进行定位。通过将无线定位模块11设置为超宽带(ultrawideband，uwb)模块，利用其亚纳秒级超窄脉冲进行精确定位，以避免在无法使用gps的情况下，对锚点和全向机器人编队的定位，并且能够提高定位的准确性。

作为领队的全向机器人通过uwb模块向锚点发送定位请求信息，锚点接收到定位请求信息后，通过uwb模块向作为领队的全向机器人发送定位确认信息，进而由全向机器人中的uwb模块根据定位请求信息和定位确认信息中的时间戳计算时间差，再根据tof算法进行距离计算，该tof算法为常用技术手段，此处不再赘述，最后发送计算出的距离。

进一步地，测试平台10，还包括：奇异值滤除模块15；奇异值滤除模块15被配置为根据最小协方差行列式，计算距离的马氏距离，奇异值滤除模块15还被配置为，当判定马氏距离大于预设阈值时，将对应的距离标记为奇异值，且滤除奇异值，奇异值滤除模块15还被配置为，将滤除奇异值后的距离发送至定位算法模块12。

具体地，设定在定位周期内，对于任一个全向机器人，接收到的该全向机器人与第n个锚点之间的距离集合dn＝{xni|i＝1,2,…}，其中，n＝1、2、3、4，xni表示在预设周期内接收到的第n个锚点与全向机器人之间的第i个距离，计算距离集合dn中对应的均值、记作距离均值tn，以及对应的协方差矩阵、记作距离协方差sn，则根据最小协方差行列式的马氏距离d(dn)对应的计算公式为：

bn＝[xni-tn](i＝1,2,…)，

通过对马氏距离d(dn)的计算，可以得出一个近似服从自由度为p的卡方分布设置信度为α，则预设阈值设定为

奇异值滤除模块15对计算出的马氏距离d(dn)与预设阈值进行比较，当判定马氏距离时，则表明在当前定位周期内，测量出的结果为奇异值，需要将该距离滤除，以提高得到距离的准确性和系统的稳定性。

在本实施例中，定位算法模块12被配置为根据距离和锚点的第一坐标，采用三边定位法，确定全向机器人的第二坐标；

具体地，定位算法模块12接收到锚点和全向机器人之间的距离后，采用计算量小、容易实现的三边定位法，根据设定好的四个锚点的第一坐标和接收到的距离，对编队后的全向机器人进行定位，确定全向机器人的第二坐标。设定四个锚点的位置坐标分别为(x1,y1,z1)、(x2,y2,z2)、(x3,y3,z3)和(x4,y4,z4)，三边定位法对应的计算公式为：

式中，(x0,y0,z0)为该全向机器人的第二坐标。

在本实施例中，避碰模块13被配置为根据第一坐标和第二坐标，计算全向机器人之间的排斥力以及、全向机器人与锚点之间的引力，避碰模块13还被配置为当判定排斥力等于引力时，根据扰动函数调整排斥力；

具体地，如图2所示，考虑到测试平台10的主要功能是对全向机器人移动算法的验证，为了防止移动算法发生故障，因此，在测试平台中设置避碰模块13，以提高测试平台10的安全性。避碰模块13接收到编队后的全向机器人的第二坐标后，对每个全向机器人的行驶状态进行评估，判断全向机器人之间是否会发生碰撞。通过引入引力场和排斥力场，以编队后的全向机器人a、全向机器人b以及锚点c为例，其中，全向机器人a的坐标qa＝(xa,ya)、全向机器人b的坐标qb＝(xb,yb)以及锚点c的坐标qc＝(xc,yc)，且全向机器人b静止不动，全向机器人a向锚点c移动。

设定锚点c对全向机器人a的引力场势能函数ua(qa)为：

式中，ka为预设引力系数，d(qa,qc)为全向机器人a与锚点c之间的距离，e(qa,qc)为从全向机器人a指向锚点c的单位矢量。因此，可以计算出锚点c对全向机器人a的引力fc(qa)为：

fc(qa)＝kad(qa,qc)e(qa,qc)。

设定全向机器人b对全向机器人a的排斥力场势能函数ub(qa)为：

式中，kb为预设排斥力系数，d(qa,qb)为全向机器人a与全向机器人b之间的距离，e(qb,qa)为从全向机器人b指向全向机器人a的单位矢量，ρr为预设壁障半径。因此，可以计算出全向机器人b对全向机器人a的排斥力fb(qa)为：

避碰模块13得出全向机器人a的排斥力fb(qa)和引力fc(qa)后，判断排斥力fb(qa)是否与引力fc(qa)相等，若排斥力fb(qa)等于引力fc(qa)时，则全向机器人a会出现停止不前或者在当前坐标处附近来回振荡，通过扰动函数ζe(qa)，对排斥力fb(qa)进行调整，调整后的排斥力f′b(qa)对应的计算公式为：

式中，ζ为预设调整系数，ζ∈(0，1)，e(qa)为与全向机器人a朝向垂直的单位矢量。

因此，全向机器人a受到的排斥力f′b(qa)小于引力fc(qa)，使得全向机器人a能够向锚点c靠近。

进一步地，避碰模块13还被配置为：当判定排斥力大于引力时，根据全向机器人与锚点之间的距离，修订排斥力，其中，排斥力的修订函数为：

式中，kb为预设排斥力系数，d(qa,qc)为全向机器人a和锚点c之间的距离，d(qa,qb)为全向机器人a和全向机器人b之间的距离，e(qb,qa)为全向机器人b指向全向机器人a的单位矢量，qa为全向机器人a的第二坐标，qb为全向机器人b的第二坐标，qc为锚点c的第一坐标，ρr为预设壁障半径，其中，设定全向机器人b位于锚点c的附近，全向机器人a向锚点c靠近。

具体地，当编队后的一个全向机器人靠近锚点之后，编队中的其余全向机器人(待靠近全向机器人)靠近锚点时，会导致待靠近全向机器人收到的排斥力大于引力，无法正常移动，出现目标不可达问题。因此，当判定排斥力大于引力时，需要根据待靠近全向机器人与锚点之间的距离，修订待靠近全向机器人受到的排斥力，使得待靠近全向机器人能够靠近锚点。

设定全向机器人b位于锚点c的附近，全向机器人a为待靠近全向机器人，向锚点c移动，修订后的全向机器人b对全向机器人a的排斥力f″b(qa)对应的计算公式为：

式中，kb为预设排斥力系数，d(qa,qc)为全向机器人a和锚点c之间的距离，d(qa,qb)为全向机器人a和全向机器人b之间的距离，e(qb,qa)为全向机器人b指向全向机器人a的单位矢量，qa为全向机器人a的第二坐标，qb为全向机器人b的第二坐标，qc为锚点c的第一坐标，ρr为预设壁障半径。

通过对全向机器人a受到的排斥力f″b(qa)进行修订，排斥力f″b(qa)随着全向机器人a与锚点c之间距离的减小而减小，使得全向机器人a能够靠近锚点c。

在本实施例中，运动模块14被配置为根据排斥力和引力，计算全向机器人的运动速度和运动方向，控制全向机器人向锚点移动。

具体地，运动模块14根据壁障模块13计算出的排斥力和引力，采用动力学和动力学方程计算出全向机器人的运动速度和运动方向，控制全向机器人向目标点移动。运动学方程为：

f(qa)＝fc(qa)+fb(qa)′，

式中，f(qa)为全向机器人受到的合力，fb(qa)′为全向机器人行进过程中的排斥力的合力，θ为机体坐标系与全局坐标系的夹角，即坐标系变换过程中的夹角，l为全向机器人等效质心到轮子的距离，vω(ω＝1,2,3)为三轮全向机器人第ω个轮子的线速度，m为全向机器人的质量，i为全向机器人绕自身中心旋转的转动惯量，r为轮子的半径，uω(ω＝1，2，3全向机器人第ω个轮子对应的电机电压，ra为电机的电枢电阻，kt为电机转矩常数。

进一步地，测试平台10，还包括：通信模块16；通信模块16用于测试平台10与锚点之间、测试平台10与全向机器人之间以及、两个全向机器人之间的数据传输，其中，数据包括距离、第一坐标、第二坐标、排斥力和引力。

具体地，为了便于通信，在测试平台10中设置通信模块16，采用多节点(master)的结构构建通信模块16的框架，并在锚点和全向机器人上设置相应的模块，不仅进行测试平台10中各个模块的通信，还进行测试平台10、锚点以及全向机器人之间的通信，以便于传输距离、第一坐标、第二坐标、排斥力和引力的信息。同时，将通信模块16设置为多master结构，有利于进行异构全向机器人的协同控制，提高了算法验证系统的可扩展性和灵活性。

进一步地，全向机器人上还设置有激光传感器，激光传感器用于检测全向机器人运动方向上是否存在障碍物以及、全向机器人与障碍物之间的障碍距离；运动模块14还被配置为，当判定障碍距离小于或等于距离阈值时，调整运动速度和运动方向，控制全向机器人避开障碍物、且向锚点移动。

具体地，在全向机器人上搭载激光传感器，以便于全向机器人对移动路径上是否存在障碍物进行检测，当检测到存在障碍物时，根据激光测距原理，检测全向机器人与障碍物之间的障碍距离，并将检测到的障碍距离通过通信模块16传递至运动模块14，使得运动模块14调整全向机器人的运动方向和运动速度，以避开障碍物，保证全向机器人的安全移动。

在本实施例中，测试平台10主要通过dwm1000模块和stm32模块进行实现，对基于虚拟结构法的编队控制算法进行了验证。虚拟结构法的基本思路是将整个全向机器人编队看成一个刚体，各全向机器人是该刚体上的节点，在编队移动过程中，全向机器人之间保持相对位置不变，从而维持队形。设计了多机器人编队跟踪圆轨迹实验，全向器人采用集中式控制，由中央处理机统一进行数据处理，并解算出每个全向机器人所需要的编队控制律，采用的通信拓扑结构为环状拓扑结构，全向机器人1、全向机器人2和全向机器人3的期望移动轨迹如图3(a)所示，通过获取uwb模块的定位数据，全向机器人1、全向机器人2和全向机器人3的实际移动轨迹如图3(b)所示，因此，可以验证本实施例中多移动机器人算法验证系统的可靠性。

进一步地，为了验证本实施例中多移动机器人算法验证系统的验证能力及其灵活性、可拓展性，设计了多全向机器人编队避障实验，全向机器人1、全向机器人2和全向机器人3的移动轨迹如图4所示。采用领队/跟随(leader-follower)的多全向机器人编队控制算法，为领队的全向机器人搭载rplidar型激光传感器，当领队的全向机器人附近出现障碍物且与其距离小于距离阈值时，保持既定任务不改变，但在其上加一个反向于障碍物的控制力，与障碍物距离越近，反向控制越强烈，从而完成避障，体现了本实施例中多移动机器人算法验证系统的验证能力及其灵活性、可拓展性。

以上结合附图详细说明了本申请的技术方案，本申请提出了一种室内通用多移动机器人算法验证系统，该系统，包括：无线定位模块，定位算法模块，避碰模块和运动模块；无线定位模块用于确定全向机器人与锚点之间的距离；定位算法模块被配置为根据距离和锚点的第一坐标，采用三边定位法，确定全向机器人的第二坐标；避碰模块被配置为根据第一坐标和第二坐标，计算全向机器人之间的排斥力以及、全向机器人与锚点之间的引力，避碰模块还被配置为当判定排斥力等于引力时，根据扰动函数调整排斥力；运动模块被配置为根据排斥力和引力，计算全向机器人的运动速度和运动方向，控制全向机器人向锚点移动。通过本申请中的技术方案，提高了室内多移动机器人算法验证系统安全运行的可靠性和稳定性。

本申请中的步骤可根据实际需求进行顺序调整、合并和删减。

本申请装置中的单元可根据实际需求进行合并、划分和删减。

尽管参考附图详地公开了本申请，但应理解的是，这些描述仅仅是示例性的，并非用来限制本申请的应用。本申请的保护范围由附加权利要求限定，并可包括在不脱离本申请保护范围和精神的情况下针对发明所作的各种变型、改型及等效方案。