CN109282817A - 一种多机器人协同定位与控制方法 - Google Patents
一种多机器人协同定位与控制方法 Download PDFInfo
- Publication number
- CN109282817A CN109282817A CN201811205306.5A CN201811205306A CN109282817A CN 109282817 A CN109282817 A CN 109282817A CN 201811205306 A CN201811205306 A CN 201811205306A CN 109282817 A CN109282817 A CN 109282817A
- Authority
- CN
- China
- Prior art keywords
- robot
- coordinate
- multirobot
- distance
- control method
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 14
- 230000033001 locomotion Effects 0.000 claims abstract description 17
- 238000005457 optimization Methods 0.000 claims description 6
- 238000005259 measurement Methods 0.000 claims description 4
- 230000007423 decrease Effects 0.000 claims 1
- 230000015572 biosynthetic process Effects 0.000 abstract description 9
- 239000011159 matrix material Substances 0.000 abstract description 3
- 238000011217 control strategy Methods 0.000 abstract description 2
- 230000000694 effects Effects 0.000 abstract description 2
- 238000004891 communication Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
- G05D1/0291—Fleet control
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
本发明涉及一种多机器人协同定位与控制方法。本发明使用的协同定位方式，只需要开始有3个以上机器人已知其坐标，通过定位算法能够迅速计算出全部机器人的位置，并在运动过程中实时计算机器人位置。同时本发明提出的编队控制策略在机器人运动设备误差很大的情况下也能够获得比较理想的编队运动效果。本发明通过将全局刚性结合到传统的定位算法中，获得了比现有的定位算法更高的定位精度，同时在编队控制过程中，仅仅需要维持全局刚性即可实现误差较大的机器人保持编队运动，比现有的编队控制算法省去了刚性矩阵运算。
Description
技术领域
本发明属于机器人协同定位与控制领域，更具体地，涉及一种多机器人协同定位与控制方法。
背景技术
目前机器人协同定位与控制主要解决的问题是在非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所示。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。
Claims (4)
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为第i个机器人的估算位置坐标；Bi为第i个机器人的邻居机器人的集合；dij表示机器人i和机器人j的传感器测量距离,d′ij表示两个机器人估算的距离，
则可定义损失函数如下，最小化损失函数则可以进一步获得准确解；
通过三角定位求出得到机器人Pi优化后的坐标Px′,Py′。
3.根据权利要求1所述的多机器人协同定位与控制方法，其特征在于，S40计算出精确解的步骤具体为：
假设B1，B2为两个已知位置点，其儿子节点为Pi，儿子节点Pi到B1，B2直接测量距离为d1，d2，则有：
将步骤S40中得到的机器人P的估计坐标为Px′,Py′,将其作为Pi的初始值带入guass-newton中，即可求解出机器人Pi的最终坐标。
4.根据权利要求1所述的多机器人协同定位与控制方法，其特征在于，在S50中，根据机器人实际坐标Pi和目的坐标Pi′之间的差值与阈值ΔH作比较，当ΔH＜||Pi′-Pi||，机器人调整位置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811205306.5A CN109282817B (zh) | 2018-10-16 | 2018-10-16 | 一种多机器人协同定位与控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811205306.5A CN109282817B (zh) | 2018-10-16 | 2018-10-16 | 一种多机器人协同定位与控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109282817A true CN109282817A (zh) | 2019-01-29 |
| CN109282817B CN109282817B (zh) | 2022-04-12 |
Family
ID=65177257
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811205306.5A Active CN109282817B (zh) | 2018-10-16 | 2018-10-16 | 一种多机器人协同定位与控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109282817B (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110147101A (zh) * | 2019-05-13 | 2019-08-20 | 中山大学 | 一种基于深度强化学习的端到端分布式多机器人编队导航方法 |
| CN110967017A (zh) * | 2019-11-22 | 2020-04-07 | 河南科技大学 | 一种用于双移动机器人刚体协作搬运的协同定位方法 |
| CN111198567A (zh) * | 2020-01-17 | 2020-05-26 | 北京大学 | 一种多agv协同动态追踪方法及装置 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101076282A (zh) * | 2004-09-30 | 2007-11-21 | 安科锐公司 | 移动目标的动态追踪 |
| CN101808398A (zh) * | 2010-02-05 | 2010-08-18 | 中国地质大学（武汉） | 一种无线传感器网络的二次加权混合定位方法 |
| CN104883733A (zh) * | 2015-04-14 | 2015-09-02 | 北京科技大学 | 一种外罚函数法与Powell算法结合的协同定位方法 |
| US20160209849A1 (en) * | 2015-01-15 | 2016-07-21 | William Dale Arbogast | System and method for decentralized, multi-agent unmanned vehicle navigation and formation control |
| CN105898865A (zh) * | 2016-06-17 | 2016-08-24 | 杭州电子科技大学 | 非线性非高斯条件下基于ekf和pf的协同定位方法 |
| CN107992035A (zh) * | 2017-11-15 | 2018-05-04 | 西北工业大学 | 一种基于室内全局定位的多移动机器人编队控制方法 |
| CN108616836A (zh) * | 2018-04-13 | 2018-10-02 | 重庆邮电大学 | 一种基于信号统计分布的wlan定位组网方法 |
-
2018
- 2018-10-16 CN CN201811205306.5A patent/CN109282817B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101076282A (zh) * | 2004-09-30 | 2007-11-21 | 安科锐公司 | 移动目标的动态追踪 |
| CN101808398A (zh) * | 2010-02-05 | 2010-08-18 | 中国地质大学（武汉） | 一种无线传感器网络的二次加权混合定位方法 |
| US20160209849A1 (en) * | 2015-01-15 | 2016-07-21 | William Dale Arbogast | System and method for decentralized, multi-agent unmanned vehicle navigation and formation control |
| CN104883733A (zh) * | 2015-04-14 | 2015-09-02 | 北京科技大学 | 一种外罚函数法与Powell算法结合的协同定位方法 |
| CN105898865A (zh) * | 2016-06-17 | 2016-08-24 | 杭州电子科技大学 | 非线性非高斯条件下基于ekf和pf的协同定位方法 |
| CN107992035A (zh) * | 2017-11-15 | 2018-05-04 | 西北工业大学 | 一种基于室内全局定位的多移动机器人编队控制方法 |
| CN108616836A (zh) * | 2018-04-13 | 2018-10-02 | 重庆邮电大学 | 一种基于信号统计分布的wlan定位组网方法 |
Non-Patent Citations (2)
| Title |
|---|
| WU, HEJUN等: "Triangle Extension: Efficient Localizability Detection in Wireless Sensor Networks", 《IEEE TRANSACTIONS ON WIRELESS COMMUNICATIONS》 * |
| 冉启可: "基于 DV-Distance 的无线传感器网络协作定 位算法研究", 《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》 * |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110147101A (zh) * | 2019-05-13 | 2019-08-20 | 中山大学 | 一种基于深度强化学习的端到端分布式多机器人编队导航方法 |
| CN110147101B (zh) * | 2019-05-13 | 2020-05-22 | 中山大学 | 一种基于深度强化学习的端到端分布式多机器人编队导航方法 |
| CN110967017A (zh) * | 2019-11-22 | 2020-04-07 | 河南科技大学 | 一种用于双移动机器人刚体协作搬运的协同定位方法 |
| CN110967017B (zh) * | 2019-11-22 | 2023-03-31 | 河南科技大学 | 一种用于双移动机器人刚体协作搬运的协同定位方法 |
| CN111198567A (zh) * | 2020-01-17 | 2020-05-26 | 北京大学 | 一种多agv协同动态追踪方法及装置 |
| CN111198567B (zh) * | 2020-01-17 | 2021-06-01 | 北京大学 | 一种多agv协同动态追踪方法及装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109282817B (zh) | 2022-04-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111060135B (zh) | 一种基于局部地图的地图修正方法及系统 | |
| CN109282817A (zh) | 一种多机器人协同定位与控制方法 | |
| TWI489126B (zh) | 一種動態無線訊號強度修正系統與方法 | |
| CN103114732B (zh) | 铸钢相贯管节点空间定位方法 | |
| CN105091878B (zh) | 一种基于步态的定位方法和装置 | |
| CN109548141A (zh) | 基于卡尔曼滤波算法的室内环境基站坐标位置标定方法 | |
| CN108316363B (zh) | 基坑水平位移自动监测系统及方法 | |
| CN104197935B (zh) | 一种基于移动智能终端的室内定位方法 | |
| CN103090845B (zh) | 一种基于多影像的远程测距方法 | |
| CN104776844A (zh) | 森林防火火点定位方法 | |
| CN112200863B (zh) | 基于同步定位与建图的无人机监测电线杆倾斜度系统 | |
| CN109470238A (zh) | 一种定位方法、装置和移动终端 | |
| CN107861509A (zh) | 一种锚点坐标校正方法及用于提高机器人定位精度的方法 | |
| CN112729301A (zh) | 一种基于多源数据融合的室内定位方法 | |
| CN103526783A (zh) | 一种测量建筑基坑水平位移的方法 | |
| CN109029455A (zh) | 一种室内导航方法 | |
| CN107677267A (zh) | 基于mems‑imu的室内行人导航航向反馈修正方法 | |
| WO2015184741A1 (zh) | 导航信息的获取方法及装置、移动终端 | |
| CN115767412A (zh) | 融合超宽带和惯性测量单元的室内定位方法 | |
| CN108612075B (zh) | 一种监测深基坑水平位移的方法 | |
| CN111783492A (zh) | 基于二维码的室内定位方法、装置、移动终端和存储介质 | |
| CN104196247A (zh) | 一种异型空间网络结构节点控制安装方法 | |
| CN102958153A (zh) | 一种基于无线传感器网络的定位方法 | |
| CN106441269A (zh) | 一种用于景区电子语音导游的区间定位方法 | |
| CN113447014A (zh) | 室内移动机器人、建图方法、定位方法以及建图定位装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |