一种多机器人协同定位与控制方法与流程

本发明属于机器人协同定位与控制领域，更具体地，涉及一种多机器人协同定位与控制方法。

背景技术：

目前机器人协同定位与控制主要解决的问题是在非gps环境下，机器人网络中单个机器人难以获得其准确位置，从而难以实现机器人按照给定队形前进的问题。该机器人网络中仅有若干个(3个以上)机器人知道其初始坐标，且每个机器人都没有精确的运动设备，无法实现精确运动。当前主流的算法为利用已知三个初始坐标，通过梯度下降算法估算其他机器人坐标；之后通过维持机器人网络的最小刚性和无穷小刚性，实现机器人网络的编队控制。然而，由于该方案在维持无穷小刚性的过程中需要实时计算整个机器人网络的刚性矩阵，对于大型机器人网络来说，其刚性矩阵计算量太大，难以在机器人的嵌入式处理器上计算。同时，由于该方案初始定位使用的是梯度下降算法，当网络规模扩大，网络连通性较低的情况下，容易陷入局部最优，难以获得准确的定位，从而存在定位精度的问题。

技术实现要素：

为了克服目前机器人协同定位与控制主要解决的问题是在非gps环境下，机器人网络中单个机器人难以获得其准确位置，从而难以实现机器人按照给定队形前进的问题。该机器人网络中仅有若干个(3个以上)机器人知道其初始坐标，且每个机器人都没有精确的运动设备，无法实现精确运动的问题，本发明提出一种多机器人协同定位与控制方法，本发明采用的技术方案是：

一种多机器人协同定位与控制方法，包括以下步骤：

s10.获取若干个机器人中的世界坐标系坐标，机器人通过传感器获得与之邻近灯塔机器人的距离；

s20.获得距离之后通过dv-distance算法估算机器人的位置，

s30.使用随机梯度下降算法对步骤s20估算的位置进行优化；

s40.通过三角扩展理论，对经历过优化的机器人的位置构建全局刚性图，并使用guass-newton算法计算出精确解；实现对机器人的精确定位；

s50.在运动阶段，移动所有灯塔机器人b，设其理论速度为v，运动时间为t，则可用v和t计算出灯塔机器人的目标位置p，灯塔机器人使用已经定位的机器人通过梯度下降定位出其运动后的实际坐标p′；

s60.固定灯塔机器人，移动其他机器人，运用s10-s50的方案定位其余机器人。

优选的，所述步骤s30的具体步骤如下：

假设pi为第i个机器人的估算位置坐标；bi为第i个机器人的邻居机器人的集合；dij表示机器人i和机器人j的传感器测量距离,d′ij表示两个机器人估算的距离，

则可定义损失函数如下，最小化损失函数则可以进一步获得准确解；

通过三角定位求出得到机器人pi优化后的坐标px′,py′。

优选的，s40计算出精确解的步骤具体为：

假设b1，b2为两个已知位置点，其儿子节点为pi，儿子节点pi到b1，b2直接测量距离为d1，d2，则有：

将步骤s40中得到的机器人p的估计坐标为px′,py′,将其作为pi的初始值带入guass-newton中，即可求解出机器人pi的最终坐标。

优选的，在s50中，根据机器人实际坐标pi和目的坐标pi′之间的差值与阈值δh作比较，当δh＜||pi′-pi||，机器人调整位置。

与现有技术相比，本发明技术方案的有益效果是：

目前机器人定位大量依赖gps定位系统，然而在复杂条件如隧道、地底，gps无法使用，同时，对于大规模机器人系统，各个机器人均安装gps成本太高。本发明使用的协同定位方式，只需要开始有3个以上机器人已知其坐标，通过定位算法能够迅速计算出全部机器人的位置，并在运动过程中实时计算机器人位置。同时本发明提出的编队控制策略在机器人运动设备误差很大的情况下也能够获得比较理想的编队运动效果。

附图说明

图1为本发明提供的多机器人协同定位与控制方法的流程图。

图2为本发明提供的多机器人协同定位与控制方法的三角扩展构建全局刚性示意图；

图3为实施例2中的100个机器人的随机位置图；

图4为实施例2中的三角扩展的扩展图。

图5为实施例2中机器人的原始位置和定位出来的位置对比图。

图6为实施例2中本发明算法的定位准确率和传统的定位算法比较结果

图7为实施例2中运动阶段11个机器人符合三角扩展的位置示意图。

图8为实施例2中机器人运动100m之后的位置与期望位置及其运动轨迹示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，仅用于示例性说明，不能理解为对本专利的限制。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

下面结合附图和实施例对本发明的技术方案做进一步的说明。

实施例1

图1所示，一种多机器人协同定位与控制方法，包括以下步骤：

s10.获取若干个机器人中的世界坐标系坐标，机器人通过传感器获得与之邻近灯塔机器人的距离；

s20.获得距离之后通过dv-distance算法估算机器人的位置，

s30.使用随机梯度下降算法对步骤s20估算的位置进行优化；

s40.通过三角扩展理论，对经历过优化的机器人的位置构建全局刚性图，并使用guass-newton算法计算出精确解；实现对机器人的精确定位；

s50.在运动阶段，移动所有灯塔机器人b，设其理论速度为v，运动时间为t，则可用v和t计算出灯塔机器人的目标位置p，灯塔机器人使用已经定位的机器人通过梯度下降定位出其运动后的实际坐标p′；

s60.固定灯塔机器人，移动其他机器人，运用s10-s50的方案定位其余机器人。

优选的，所述步骤s30的具体步骤如下：

假设pi为第i个机器人的估算位置坐标；bi为第i个机器人的邻居机器人的集合；dij表示机器人i和机器人j的传感器测量距离,d′ij表示两个机器人估算的距离，

则可定义损失函数如下，最小化损失函数则可以进一步获得准确解；

通过三角定位求出得到机器人pi优化后的坐标px′,py′。

优选的，s40计算出精确解的步骤具体为：

假设b1，b2为两个已知位置点，其儿子节点为pi，儿子节点pi到b1，b2直接测量距离为d1，d2，则有：

将步骤s40中得到的机器人p的估计坐标为px′,py′,将其作为pi的初始值带入guass-newton中，即可求解出机器人pi的最终坐标。

优选的，在s50中，根据机器人实际坐标pi和目的坐标pi′之间的差值与阈值δh作比较，当δh＜||pi′-pi||，机器人调整位置。

实施例2

在本实施例中，通过全局刚性图的定位算法在100*100的空间内放置100个机器人，机器人通信距离为25。则有图2，图3，图4。图5表示机器人的随机位置，图6为三角扩展的扩展图。图7为原始位置和定位出来的位置，平均定位误差小于10-2。图5为本发明算法的定位准确率和传统的定位算法比较结果。

实施例3

在本实施例中，有11个机器人如图6摆放并进行运动。机器人速度大小误差为20％，角度误差为30°，通信距离为30m。其中0，1，10机器人初始位置已知，其余8个机器人初始位置未知。机器人向方向运动100m之后的位置与期望位置及其运动轨迹如图7所示。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。