一种多机器人协同定位与控制方法技术

本发明专利技术涉及一种多机器人协同定位与控制方法。本发明专利技术使用的协同定位方式，只需要开始有3个以上机器人已知其坐标，通过定位算法能够迅速计算出全部机器人的位置，并在运动过程中实时计算机器人位置。同时本发明专利技术提出的编队控制策略在机器人运动设备误差很大的情况下也能够获得比较理想的编队运动效果。本发明专利技术通过将全局刚性结合到传统的定位算法中，获得了比现有的定位算法更高的定位精度，同时在编队控制过程中，仅仅需要维持全局刚性即可实现误差较大的机器人保持编队运动，比现有的编队控制算法省去了刚性矩阵运算。

A Cooperative Location and Control Method for Multiple Robots

The invention relates to a multi-robot cooperative positioning and control method. The cooperative positioning method used in the invention only needs to start with more than three robots whose coordinates are known. The position of all robots can be quickly calculated by the positioning algorithm, and the position of robots can be computed in real time in the course of motion. At the same time, the formation control strategy proposed by the invention can also achieve ideal formation motion effect under the condition of large error of the robot motion equipment. The method combines global rigidity with traditional positioning algorithm, and achieves higher positioning accuracy than existing positioning algorithm. At the same time, in the formation control process, only maintaining global rigidity is needed to achieve the formation motion of robots with larger errors, and the rigidity matrix operation is omitted compared with the existing formation control algorithm.

【技术实现步骤摘要】
一种多机器人协同定位与控制方法
本专利技术属于机器人协同定位与控制领域，更具体地，涉及一种多机器人协同定位与控制方法。
技术介绍
目前机器人协同定位与控制主要解决的问题是在非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所示。显然，本专利技术的上述实施例仅仅是为清楚地说明本专利技术所作的举例，而并非是对本专利技术的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式本文档来自技高网...

【技术保护点】
1.一种多机器人协同定位与控制方法，其特征在于，包括以下步骤：S10.获取若干个机器人中的世界坐标系坐标，机器人通过传感器获得与之邻近灯塔机器人的距离；S20.获得距离之后通过dv‑distance算法估算机器人的位置；S30.使用随机梯度下降算法对步骤S20估算的位置进行优化；S40.通过三角扩展理论，对经历过优化的机器人的位置构建全局刚性图，并使用guass‑Newton算法计算出精确解；实现对机器人的精确定位；S50.在运动阶段，移动所有灯塔机器人B，设其理论速度为v，运动时间为t，则可用v和t计算出灯塔机器人的目标位置P，灯塔机器人使用已经定位的机器人通过梯度下降定位出其运动后的实际坐标P′；S60.固定灯塔机器人，移动其他机器人，运用S10‑S50的方案定位其余机器人。

【技术特征摘要】
1.一种多机器人协同定位与控制方法，其特征在于，包括以下步骤：S10.获取若干个机器人中的世界坐标系坐标，机器人通过传感器获得与之邻近灯塔机器人的距离；S20.获得距离之后通过dv-distance算法估算机器人的位置；S30.使用随机梯度下降算法对步骤S20估算的位置进行优化；S40.通过三角扩展理论，对经历过优化的机器人的位置构建全局刚性图，并使用guass-Newton算法计算出精确解；实现对机器人的精确定位；S50.在运动阶段，移动所有灯塔机器人B，设其理论速度为v，运动时间为t，则可用v和t计算出灯塔机器人的目标位置P，灯塔机器人使用已经定位的机器人通过梯度下降定位出其运动后的实际坐标P′；S60.固定灯塔机器人，移动其他机器人，运用S10-S50的方案定位其余机器人。2.根据权利要求1所述的多机器人协同定位与控制方法，其特征在于，所述步骤S30的具体步骤如下：假设Pi为...

【专利技术属性】
技术研发人员：丁志敏，吴贺俊，
申请(专利权)人：中山大学，
类型：发明
国别省市：广东,44