CN116608851A - 地下管道双机器人步进式协同导航方法、系统及服务器 - Google Patents
地下管道双机器人步进式协同导航方法、系统及服务器 Download PDFInfo
- Publication number
- CN116608851A CN116608851A CN202310883369.0A CN202310883369A CN116608851A CN 116608851 A CN116608851 A CN 116608851A CN 202310883369 A CN202310883369 A CN 202310883369A CN 116608851 A CN116608851 A CN 116608851A
- Authority
- CN
- China
- Prior art keywords
- robot
- navigation
- attitude angle
- target
- coordinate system
- 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 74
- 230000003068 static effect Effects 0.000 claims abstract description 33
- 230000008569 process Effects 0.000 claims description 20
- 238000004364 calculation method Methods 0.000 claims description 6
- 230000009977 dual effect Effects 0.000 claims description 5
- 238000004590 computer program Methods 0.000 claims description 4
- 230000008901 benefit Effects 0.000 abstract description 6
- 238000009825 accumulation Methods 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 8
- 239000011159 matrix material Substances 0.000 description 8
- 230000009466 transformation Effects 0.000 description 8
- 238000004422 calculation algorithm Methods 0.000 description 7
- 238000006243 chemical reaction Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 4
- 230000000007 visual effect Effects 0.000 description 4
- 238000004891 communication Methods 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000005286 illumination Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 239000004973 liquid crystal related substance Substances 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000000284 resting effect Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012795 verification 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
- G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
-
- 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
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0257—Hybrid positioning
- G01S5/0263—Hybrid positioning by combining or switching between positions derived from two or more separate positioning systems
- G01S5/0264—Hybrid positioning by combining or switching between positions derived from two or more separate positioning systems at least one of the systems being a non-radio wave positioning system
Abstract
本发明涉及机器人定位与导航技术领域，公开了地下管道双机器人步进式协同导航方法、系统及服务器。本发明由两台机器人交替进行定位，每台机器人内置IMU单元、UWB单元和服务器，通过IMU单元测量处于静止状态的机器人的绝对方位角，在另一机器人向目标点的方向前进至新节点后，利用UWB单元测量两台机器人间的相对方位角和距离，并由服务器基于IMU单元和UWB单元测得的数据计算另一机器人的位置，以计算下一节点的位置并生成相应控制指令来控制该机器人前进至下一节点，实现两台机器人步进式前进。本发明能够解决纯惯性导航误差快速累积和UWB定位所需提前布置基站的问题，具有低误差、低成本、低功耗和自由度高的优点。
Description
技术领域
本发明涉及机器人定位与导航技术领域，尤其涉及地下管道双机器人步进式协同导航方法、系统及服务器。
背景技术
城市地下管网是城市内极其重要的基础设施，维系着城市运行的生命线，同时，也是未来军情侦察、军事战斗的一个重要方向。伴随着城市地下管网存在缺少GNSS（全球导航卫星系统）信号、光照条件差、无线信号屏蔽以及地下管网结构日趋复杂等问题，如何让机器人从地下管网移动到城市的任何一个地方，是目前地下环境机器人自主精准导航亟待解决的关键问题。
针对地下导航情景，目前通常采用的方案有惯性导航、UWB（超宽带）定位以及视觉导航。惯性导航方案中利用加速度计和陀螺仪测量出载体的加速度和角速度，进行数学积分得到载体的速度和位姿，以此进行导航；UWB是一种相对距离测量技术，需提前布置好至少三个基站，给测量物体装配一个标签，利用标签与三个基站的距离以及提前知道的三个基站的坐标，计算出标签坐标来进行导航。视觉导航方案是使用相机等视觉设备采取环境信息，利用高性能计算机进行一系列算法处理，进行建图，进而实现导航。
然而，惯性导航方案中，由于其误差随时间累计，不能在长时间工作中提供精确导航；UWB定位方案需要提前布置基站，且通信距离受限，在地下管网中提前布置基站，成本高昂；视觉导航方案对光照条件要求苛刻，在地下环境中，光照条件并不能满足其要求，需额外增加光源设备，但光源设备会产生巨大的电源功耗。
因此，有必要提供一种低成本、低功耗及低误差的地下导航方案。
发明内容
本发明提供了地下管道双机器人步进式协同导航方法、系统及服务器，解决了如何实现低成本、低功耗及低误差的地下导航的技术问题。
本发明第一方面提供一种地下管道双机器人步进式协同导航方法，地下管道双机器人步进式协同导航场景中的两台机器人交替进行定位，每台所述机器人内置有IMU（惯性传感器）单元、UWB单元和服务器，所述方法应用于所述服务器，所述方法包括：
获取服务器所在的目标机器人在导航坐标系下的第一姿态角；所述第一姿态角由所述目标机器人内置的IMU单元在所述目标机器人处于静止状态时利用地磁进行测角得到，所述目标机器人停留在出发点或抵达新的节点后处于静止状态，所述导航坐标系以所述出发点为坐标原点；
获取地下管道双机器人步进式协同导航场景中的另一机器人相对于所述目标机器人的方位角和距离；所述方位角和距离由所述目标机器人内置的UWB单元测得，所述另一机器人在所述目标机器人处于静止状态时向目标点的方向前进且所抵达的位置位于或邻近所述另一机器人行至的上一节点与目标点的连线；
根据所述第一姿态角和所述方位角，计算所述另一机器人行至的上一节点与所述另一机器人的当前位置的连线在所述导航坐标系下的姿态角，得到第二姿态角；
确定所述目标机器人在所述导航坐标系下的当前位置；
根据确定的目标机器人在所述导航坐标系下的当前位置、所述第二姿态角和所述距离计算得到另一机器人的位置；
根据得到的另一机器人的位置计算出下一节点的位置并生成相应的控制指令，以控制所述目标机器人前进至下一节点。
根据本发明第一方面的一种能够实现的方式，所述根据所述第一姿态角和所述方位角，计算所述另一机器人行至的上一节点与所述另一机器人的当前位置的连线在所述导航坐标系下的姿态角，得到第二姿态角，包括：
按照下式计算得到第二姿态角：
；
式中，为由机器人/>内置的IMU单元在机器人/>处于静止状态时利用地磁进行测角得到，所述目标机器人停留在出发点或抵达新的节点后处于静止状态的姿态角，/>为机器人/>相对于机器人/>的方位角，/>为机器人/>的当前位置与机器人/>的当前位置的连线在所述导航坐标系下的姿态角。
根据本发明第一方面的一种能够实现的方式，所述根据确定的目标机器人在所述导航坐标系下的当前位置、所述第二姿态角和所述距离计算得到另一机器人的位置，包括：
按照下式计算得到另一机器人的位置：
；
式中，为计算得到的机器人/>在导航坐标系下的坐标，/>为确定的机器人/>在所述导航坐标系下的当前位置的坐标，/>为机器人/>相对于机器人/>的距离，/>为机器人/>的当前位置与机器人/>的当前位置的连线在所述导航坐标系下的姿态角。
本发明第二方面提供一种电子设备，包括：
存储器，用于存储指令；其中，所述指令用于实现如上任意一项能够实现的方式所述的地下管道双机器人步进式协同导航方法；
处理器，用于执行所述存储器中的指令。
本发明第三方面提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如上任意一项能够实现的方式所述的地下管道双机器人步进式协同导航方法。
本发明第四方面提供一种服务器，应用于地下管道双机器人步进式协同导航场景，地下管道双机器人步进式协同导航场景中的两台机器人交替进行定位，每台所述机器人内置有IMU单元、UWB单元和所述服务器，所述服务器包括：
第一获取模块，用于获取所在的目标机器人在导航坐标系下的第一姿态角；所述第一姿态角由所述目标机器人内置的IMU单元在所述目标机器人处于静止状态时利用地磁进行测角得到，所述目标机器人停留在出发点或抵达新的节点后处于静止状态，所述导航坐标系以所述出发点为坐标原点；
第二获取模块，用于获取地下管道双机器人步进式协同导航场景中的另一机器人相对于所述目标机器人的方位角和距离；所述方位角和距离由所述目标机器人内置的UWB单元测得，所述另一机器人在所述目标机器人处于静止状态时向目标点的方向前进且所抵达的位置位于或邻近所述另一机器人行至的上一节点与目标点的连线；
第一计算模块，用于根据所述第一姿态角和所述方位角，计算所述另一机器人行至的上一节点与所述另一机器人的当前位置的连线在所述导航坐标系下的姿态角，得到第二姿态角；
确定模块，用于确定所述目标机器人在所述导航坐标系下的当前位置；
第二计算模块，用于根据确定的目标机器人在所述导航坐标系下的当前位置、所述第二姿态角和所述距离计算得到另一机器人的位置；
控制模块，用于根据得到的另一机器人的位置计算出下一节点的位置并生成相应的控制指令，以控制所述目标机器人前进至下一节点。
根据本发明第四方面的一种能够实现的方式，所述第一计算模块具体用于：
按照下式计算得到第二姿态角：
；
式中，为由机器人/>内置的IMU单元在机器人/>处于静止状态时利用地磁进行测角得到，所述目标机器人停留在出发点或抵达新的节点后处于静止状态的姿态角，/>为机器人/>相对于机器人/>的方位角，/>为机器人/>的当前位置与机器人/>的当前位置的连线在所述导航坐标系下的姿态角。
根据本发明第四方面的一种能够实现的方式，所述第二计算模块具体用于：
按照下式计算得到另一机器人的位置：
；
式中，为计算得到的机器人/>在导航坐标系下的坐标，/>为确定的机器人/>在所述导航坐标系下的当前位置的坐标，/>为机器人/>相对于机器人/>的距离，/>为机器人/>的当前位置与机器人/>的当前位置的连线在所述导航坐标系下的姿态角。
本发明第五方面提供一种地下管道双机器人步进式协同导航坐标系统，包括：
应用于地下管道双机器人步进式协同导航场景中的两台机器人；
两台机器人交替进行定位，每台所述机器人内置有IMU单元、UWB单元以及如上任意一项能够实现的方式所述的服务器。
根据本发明第五方面的一种能够实现的方式，每台所述机器人内置的IMU单元和UWB单元共轴安装。
从以上技术方案可以看出，本发明具有以下优点：
本发明地下管道双机器人步进式协同导航场景中的两台机器人交替进行定位，每台机器人内置有IMU单元、UWB单元和服务器，由服务器执行：获取所在的目标机器人在导航坐标系下的第一姿态角；所述第一姿态角由目标机器人内置的IMU单元在目标机器人处于静止状态时利用地磁进行测角得到，所述目标机器人停留在出发点或抵达新的节点后处于静止状态，所述导航坐标系以所述出发点为坐标原点；获取地下管道双机器人步进式协同导航场景中的另一机器人相对于所述目标机器人的方位角和距离；所述方位角和距离由所述目标机器人内置的UWB单元测得，所述另一机器人在目标机器人处于静止状态时向目标点的方向前进且所抵达的位置位于或邻近所述另一机器人行至的上一节点与目标点的连线；根据第一姿态角和方位角，计算另一机器人行至的上一节点与另一机器人的当前位置的连线在所述导航坐标系下的姿态角，得到第二姿态角；确定目标机器人在所述导航坐标系下的当前位置；根据确定的目标机器人在所述导航坐标系下的当前位置、所述第二姿态角和所述距离计算得到另一机器人的位置；根据得到的另一机器人的位置计算出下一节点的位置并生成相应的控制指令，以控制所述目标机器人前进至下一节点；基于本发明的地下管道双机器人步进式协同导航，本发明中的两台机器人步进式前进，可完成GNSS拒止环境中的导航，解决了UWB定位所需提前布置基站的问题；两台机器人交替进行定位，使得机器人仅在处于某些节点时才进行定位，能够避免纯惯性导航的频繁积分操作，因而避免了纯惯性导航在地下、室内等GNSS拒止环境中快速累积误差的问题；与现有的导航方案相比，本发明拥有低误差、低成本、低功耗、自由度高的优点。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1为本发明一个可选实施例提供的一种地下管道双机器人步进式协同导航方法的流程图；
图2为导航坐标系和载体系的坐标转换关系示意图；
图3为本发明一个可选实施例提供的以导航坐标系为出发点的东北坐标系为例时图1所示方法的原理图；
图4为进行双机器人步进式协同导航时的运行轨迹示意图；
图5为双机器人步进式协同导航统计精度的示意图；
图6为使用战术级IMU（零偏稳定性）进行纯惯性导航时的运行轨迹示意图；
图7为本发明一个可选实施例提供的一种服务器的结构连接框图。
附图标记：
1-第一获取模块；2-第二获取模块；3-第一计算模块；4-确定模块；5-第二计算模块；6-控制模块。
具体实施方式
本发明实施例提供了地下管道双机器人步进式协同导航方法、系统及服务器，用于解决如何实现低成本、低功耗及低误差的地下导航的技术问题。
为使得本发明的发明目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，下面所描述的实施例仅仅是本发明一部分实施例，而非全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
本发明提供了一种地下管道双机器人步进式协同导航方法。其中，地下管道双机器人步进式协同导航场景中的两台机器人交替进行定位，每台所述机器人内置有IMU单元、UWB单元和服务器，本发明实施例中的方法应用于所述服务器。
请参阅图1，图1示出了本发明实施例提供的一种地下管道双机器人步进式协同导航方法的流程图。
本发明实施例提供的一种地下管道双机器人步进式协同导航方法，包括步骤S1-S6。
步骤S1，获取服务器所在的目标机器人在导航坐标系下的第一姿态角；所述第一姿态角由所述目标机器人内置的IMU单元在所述目标机器人处于静止状态时利用地磁进行测角得到，所述目标机器人停留在出发点或抵达新的节点后处于静止状态，所述导航坐标系以所述出发点为坐标原点。
本发明实施例中，由地下管道内的两台机器人交替进行定位。以两台机器人分别为机器人和机器人/>为例，若机器人/>停留在出发点或抵达新的节点后，此时机器人/>保持静止并利用地磁进行测角以确定自身的姿态角，机器人/>行至新节点并且位于上一节点与目标点的连线或连线附近。
为更好地阐述本申请的方法，下面对所述导航坐标系的定义进行详细说明。
机器人在地下管道中运动，一般可以假设其运动在二维平面上。为了避免杆壁效应，将载体系的原点与各传感器的几何中心对齐。因此，定义常用坐标系如下：
（1）地球地固坐标系（e系WGS84），其中，z轴指向北极，x轴在赤道面内指向本初子午线，y轴成右手法则；
（2）导航坐标系（n系）：东北天坐标系；
其中，原点在IMU单元的几何中心, />轴指向当地东向，/>轴指向当地北向，轴指向当地天向；
（3）载体系（b系）：；
原点在IMU单元的几何中心，/>轴指向载体右侧，/>轴指向载体前进方向，/>轴成右手法则；
一般情况下，导航算法中的IMU单元和UWB单元中的传感器可能会存在杆壁效应，本实施例中，为了避免杆壁效应，采用IMU单元和UWB单元共轴安装的方法，使其质心在水平面内对齐，但是在高度方向可不做要求；
（4）UWB基站体坐标系：
原点位于UWB基站几何中心，y轴朝向基站天线阵列前向，z轴与载体系z轴重合，x轴成右手法则；一般将基站天线阵列前向与载体前向对齐，因此UWB基站体坐标系的x、y、z轴方向与载体系平行，与载体系仅存在原点差异。
下面对坐标系转换关系进行阐述。
由n系到e系WGS84的坐标转换矩阵为：
；
式中，为由n系到e系WGS84的坐标转换矩阵，/>表示经度，/>表示纬度；
由e系WGS84到n系的坐标转换矩阵为其转置矩阵，即：
；
式中，为由e系WGS84到n系的坐标转换矩阵，上标/>表示转置。
如图2所示，只考虑水平面的运动和姿态，设导航坐标系旋转/>角后与载体系轴重合，/>角逆时针为正。则b系坐标到n系的坐标转换矩阵为：
；
即：
；
；
式中，表示n系的坐标，/>表示b系的坐标，/>为b系坐标到n系的坐标转换矩阵。
需要说明的是，如果载体运动范围较小（数千米以内），则导航坐标系的东向和北向可以认为是固定值，导航坐标系可以固定为出发点的东北坐标系。
执行本申请方法时，若给到的目标点位置坐标不是基于导航坐标系，则前进的机器人需要基于服务器将目标点的位置坐标转换为导航坐标系的坐标，以导航坐标系为出发点的东北坐标系为例，具体的转换过程如下。
机器人的出发点的绝对坐标已知，其纬经高坐标设为，目标点的纬经高坐标设为/>。以出发点为坐标原点，建立东北导航坐标系，并将目标点位置转换至该坐标系。转换过程为：先将出发点和目标点的纬经高坐标转换至地心地固坐标系，再将目标点与出发点坐标之差转换到出发点所在的当地东北天坐标系。具体转换公式如下：
第一步：由求解地心地固坐标系/>：
；
其中，地球横向曲率半径为：
；
地球长半轴为：
；
地球椭球偏心率为：
；
由上述公式可以得到和/>。
第二步：计算地心地固坐标系中目标点相对出发点的坐标：
；
第三步：计算目标点在出发点所在的当地东北天坐标系的坐标：
；
其中，为由e系WGS84到n系的坐标转换矩阵。
步骤S2，获取地下管道双机器人步进式协同导航场景中的另一机器人相对于所述目标机器人的方位角和距离；所述方位角和距离由所述目标机器人内置的UWB单元测得，所述另一机器人在所述目标机器人处于静止状态时向目标点的方向前进且所抵达的位置位于或邻近所述另一机器人行至的上一节点与目标点的连线。
所述另一机器人在所述目标机器人处于静止状态时向目标点的方向前进时，可以通过预置的路径选择算法选择最优通路，再调整自身姿态与选定的通道角度相同，然后沿该通道运动。
需要说明的是，该路径选择算法可以根据实际情况在现有技术中的相关算法中进行选择，本实施例中，对此不做限定。
另一机器人相对于所述目标机器人的方位角和距离主要由UWB的AOA（Angle ofArrival）模式实现。UWB单元中的UWB设备分为标签和基站两种类型，标签与基站之间的距离是通过测量两者之间无线电信号的精确飞行时间，再乘以光速后获得。通过基站内置的多天线阵列，测量标签到基站的无线电磁波相位差，求得标签与基站原点连线在基站坐标系的角度。
步骤S3，根据所述第一姿态角和所述方位角，计算所述另一机器人行至的上一节点与所述另一机器人的当前位置的连线在所述导航坐标系下的姿态角，得到第二姿态角。
在一种能够实现的方式中，所述根据所述第一姿态角和所述方位角，计算所述另一机器人行至的上一节点与所述另一机器人的当前位置的连线在所述导航坐标系下的姿态角，得到第二姿态角，包括：
按照下式计算得到第二姿态角：
；
式中，为由机器人/>内置的IMU单元在机器人/>处于静止状态时利用地磁进行测角得到，所述目标机器人停留在出发点或抵达新的节点后处于静止状态的姿态角，/>为机器人/>相对于机器人/>的方位角，/>为机器人/>的当前位置与机器人/>的当前位置的连线在所述导航坐标系下的姿态角。
步骤S4，确定所述目标机器人在所述导航坐标系下的当前位置。
所述目标机器人在所述导航坐标系下的当前位置可以通过现有导航算法进行确定，本实施例中，对此不做限定。
步骤S5，根据确定的目标机器人在所述导航坐标系下的当前位置、所述第二姿态角和所述距离计算得到另一机器人的位置。
在一种能够实现的方式中，所述根据确定的目标机器人在所述导航坐标系下的当前位置、所述第二姿态角和所述距离计算得到另一机器人的位置，包括：
按照下式计算得到另一机器人的位置：
；
式中，为计算得到的机器人/>在导航坐标系下的坐标，/>为确定的机器人/>在所述导航坐标系下的当前位置的坐标，/>为机器人/>相对于机器人/>的距离，/>为机器人/>的当前位置与机器人/>的当前位置的连线在所述导航坐标系下的姿态角。
本发明上述实施例中，两机器人交替式重复以上定位步骤，可完成地下管道自主精准导航。
步骤S6，根据得到的另一机器人的位置计算出下一节点的位置并生成相应的控制指令，以控制所述目标机器人前进至下一节点。以导航坐标系为出发点的东北坐标系为例，本申请方法的原理图如图3所示。图3中，为出发点的坐标，/>为节点1的坐标。
本发明上述实施例中，以双机器人分别为机器人和机器人/>为例，机器人/>静止在a点，a点坐标已知，机器人/>抵到新的节点b，利用UWB和机器人/>的IMU单元计算出b点的坐标，计算完成后，机器人/>不动，机器人/>会直接行驶到b点的下一个节点c，此时b点坐标已知，这时利用UWB和机器人/>的IMU单元计算出c点坐标，依次反复进行。本发明上述实施例至少具有以下意想不到的效果：
本发明中的两台机器人步进式前进，可完成GNSS拒止环境中的导航，解决了UWB定位所需提前布置基站的问题；两台机器人交替进行定位，使得机器人仅在处于某些节点时才进行定位，能够避免纯惯性导航的频繁积分操作，因而避免了纯惯性导航在地下、室内等GNSS拒止环境中快速累积误差的问题；与现有的导航方案相比，本发明拥有低误差、低成本、低功耗、自由度高的优点。
为验证导航技术方案的定位精度和比较优势，进行仿真验证实验。两台机器人（分为机器人和机器人/>）以（/>）为起点，步进式协同导航至目标点（/>）。导航设置离目标点1m以内停止前进，最终总路程为262.4km，行进速度设置为80km/h, 耗时6.5h抵达。两台机器人行进路线如图4所示，最终进入目标点1m范围内。相同场景仿真1000次，并统计导航结果与目标点的误差，得到北向的末导航精度/>为0.49m，东向的末导航精度/>为0.39m，如图5所示。相同场景使用战术级IMU（零偏稳定性/>）进行纯惯性导航，结果与目标点的误差较大，远远偏离目标点，如图6所示。
本发明还提供了一种服务器，该服务器可用于执行本发明上述任一项实施例所述的地下管道双机器人步进式协同导航方法。
请参阅图2，图2示出了本发明实施例提供的一种服务器的结构连接框图。
本发明实施例提供的一种服务器，包括：
第一获取模块1，用于获取所在的目标机器人在导航坐标系下的第一姿态角；所述第一姿态角由所述目标机器人内置的IMU单元在所述目标机器人处于静止状态时利用地磁进行测角得到，所述目标机器人停留在出发点或抵达新的节点后处于静止状态，所述导航坐标系以所述出发点为坐标原点；
第二获取模块2，用于获取地下管道双机器人步进式协同导航场景中的另一机器人相对于所述目标机器人的方位角和距离；所述方位角和距离由所述目标机器人内置的UWB单元测得，所述另一机器人在所述目标机器人处于静止状态时向目标点的方向前进且所抵达的位置位于或邻近所述另一机器人行至的上一节点与目标点的连线；
第一计算模块3，用于根据所述第一姿态角和所述方位角，计算所述另一机器人行至的上一节点与所述另一机器人的当前位置的连线在所述导航坐标系下的姿态角，得到第二姿态角；
确定模块4，用于确定所述目标机器人在所述导航坐标系下的当前位置；
第二计算模块5，用于根据确定的目标机器人在所述导航坐标系下的当前位置、所述第二姿态角和所述距离计算得到另一机器人的位置；
控制模块6，用于根据得到的另一机器人的位置计算出下一节点的位置并生成相应的控制指令，以控制所述目标机器人前进至下一节点。
在一种能够实现的方式中，所述第一计算模块3具体用于：
按照下式计算得到第二姿态角：
；
式中，为由机器人/>内置的IMU单元在机器人/>处于静止状态时利用地磁进行测角得到，所述目标机器人停留在出发点或抵达新的节点后处于静止状态的姿态角，/>为机器人/>相对于机器人/>的方位角，/>为机器人/>的当前位置与机器人/>的当前位置的连线在所述导航坐标系下的姿态角。
在一种能够实现的方式中，所述第二计算模块5具体用于：
按照下式计算得到另一机器人的位置：
；
式中，为计算得到的机器人/>在导航坐标系下的坐标，/>为确定的机器人/>在所述导航坐标系下的当前位置的坐标，/>为机器人/>相对于机器人/>的距离，/>为机器人/>的当前位置与机器人/>的当前位置的连线在所述导航坐标系下的姿态角。/>
本发明还提供一种地下管道双机器人步进式协同导航坐标系统，包括：
应用于地下管道双机器人步进式协同导航场景中的两台机器人；
两台机器人交替进行定位，每台所述机器人内置有IMU单元、UWB单元以及如上任意一项实施例所述的服务器。
作为一种能够实现的方式，每台所述机器人内置的IMU单元和UWB单元共轴安装。
本发明还提供一种电子设备，包括：
存储器，用于存储指令；其中，所述指令用于实现如上任意一项实施例所述的地下管道双机器人步进式协同导航方法；
处理器，用于执行所述存储器中的指令。
本发明还提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如上任意一项实施例所述的地下管道双机器人步进式协同导航方法。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、设备、服务器、模块和单元的具体工作过程，可以参考前述方法实施例中的对应过程，上述描述的系统、设备、服务器、模块和单元的具体有益效果，可以参考前述方法实施例中的对应有益效果，在此不再赘述。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统、设备、服务器和方法，可以通过其它的方式实现。例如，以上所描述的服务器实施例仅仅是示意性的，例如，所述模块的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个模块或组件可以结合或者可以集成到另一个装置，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或模块的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的模块可以是或者也可以不是物理上分开的，作为模块显示的部件可以是或者也可以不是物理模块，即可以位于一个地方，或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能模块可以集成在一个处理模块中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。
所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是个人计算机，服务器，或者网络设备等）执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-OnlyMemory）、随机存取存储器（RAM，Random Access Memory）、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (10)
1.一种地下管道双机器人步进式协同导航方法，其特征在于，地下管道双机器人步进式协同导航场景中的两台机器人交替进行定位，每台所述机器人内置有IMU单元、UWB单元和服务器，所述方法应用于所述服务器，所述方法包括：
获取服务器所在的目标机器人在导航坐标系下的第一姿态角；所述第一姿态角由所述目标机器人内置的IMU单元在所述目标机器人处于静止状态时利用地磁进行测角得到，所述目标机器人停留在出发点或抵达新的节点后处于静止状态，所述导航坐标系以所述出发点为坐标原点；
获取地下管道双机器人步进式协同导航场景中的另一机器人相对于所述目标机器人的方位角和距离；所述方位角和距离由所述目标机器人内置的UWB单元测得，所述另一机器人在所述目标机器人处于静止状态时向目标点的方向前进且所抵达的位置位于或邻近所述另一机器人行至的上一节点与目标点的连线；
根据所述第一姿态角和所述方位角，计算所述另一机器人行至的上一节点与所述另一机器人的当前位置的连线在所述导航坐标系下的姿态角，得到第二姿态角；
确定所述目标机器人在所述导航坐标系下的当前位置；
根据确定的目标机器人在所述导航坐标系下的当前位置、所述第二姿态角和所述距离计算得到另一机器人的位置；
根据得到的另一机器人的位置计算出下一节点的位置并生成相应的控制指令，以控制所述目标机器人前进至下一节点。
2.根据权利要求1所述的地下管道双机器人步进式协同导航方法，其特征在于，所述根据所述第一姿态角和所述方位角，计算所述另一机器人行至的上一节点与所述另一机器人的当前位置的连线在所述导航坐标系下的姿态角，得到第二姿态角，包括：
按照下式计算得到第二姿态角：
；
式中，为由机器人/>内置的IMU单元在机器人/>处于静止状态时利用地磁进行测角得到，所述目标机器人停留在出发点或抵达新的节点后处于静止状态的姿态角，/>为机器人/>相对于机器人/>的方位角，/>为机器人/>的当前位置与机器人/>的当前位置的连线在所述导航坐标系下的姿态角。
3.根据权利要求1所述的地下管道双机器人步进式协同导航方法，其特征在于，所述根据确定的目标机器人在所述导航坐标系下的当前位置、所述第二姿态角和所述距离计算得到另一机器人的位置，包括：
按照下式计算得到另一机器人的位置：
；
式中，为计算得到的机器人/>在导航坐标系下的坐标，/>为确定的机器人/>在所述导航坐标系下的当前位置的坐标，/>为机器人/>相对于机器人/>的距离，为机器人/>的当前位置与机器人/>的当前位置的连线在所述导航坐标系下的姿态角。
4.一种电子设备，其特征在于，包括：
存储器，用于存储指令；其中，所述指令用于实现如权利要求1-3任意一项所述的地下管道双机器人步进式协同导航方法；
处理器，用于执行所述存储器中的指令。
5.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1-3任意一项所述的地下管道双机器人步进式协同导航方法。
6.一种服务器，应用于地下管道双机器人步进式协同导航场景，其特征在于，地下管道双机器人步进式协同导航场景中的两台机器人交替进行定位，每台所述机器人内置有IMU单元、UWB单元和所述服务器，所述服务器包括：
第一获取模块，用于获取所在的目标机器人在导航坐标系下的第一姿态角；所述第一姿态角由所述目标机器人内置的IMU单元在所述目标机器人处于静止状态时利用地磁进行测角得到，所述目标机器人停留在出发点或抵达新的节点后处于静止状态，所述导航坐标系以所述出发点为坐标原点；
第二获取模块，用于获取地下管道双机器人步进式协同导航场景中的另一机器人相对于所述目标机器人的方位角和距离；所述方位角和距离由所述目标机器人内置的UWB单元测得，所述另一机器人在所述目标机器人处于静止状态时向目标点的方向前进且所抵达的位置位于或邻近所述另一机器人行至的上一节点与目标点的连线；
第一计算模块，用于根据所述第一姿态角和所述方位角，计算所述另一机器人行至的上一节点与所述另一机器人的当前位置的连线在所述导航坐标系下的姿态角，得到第二姿态角；
确定模块，用于确定所述目标机器人在所述导航坐标系下的当前位置；
第二计算模块，用于根据确定的目标机器人在所述导航坐标系下的当前位置、所述第二姿态角和所述距离计算得到另一机器人的位置；
控制模块，用于根据得到的另一机器人的位置计算出下一节点的位置并生成相应的控制指令，以控制所述目标机器人前进至下一节点。
7.根据权利要求6所述的服务器，其特征在于，所述第一计算模块具体用于：
按照下式计算得到第二姿态角：
；
式中，为由机器人/>内置的IMU单元在机器人/>处于静止状态时利用地磁进行测角得到，所述目标机器人停留在出发点或抵达新的节点后处于静止状态的姿态角，/>为机器人/>相对于机器人/>的方位角，/>为机器人/>的当前位置与机器人/>的当前位置的连线在所述导航坐标系下的姿态角。
8.根据权利要求6所述的服务器，其特征在于，所述第二计算模块具体用于：
按照下式计算得到另一机器人的位置：
；
式中，为计算得到的机器人/>在导航坐标系下的坐标，/>为确定的机器人/>在所述导航坐标系下的当前位置的坐标，/>为机器人/>相对于机器人/>的距离，为机器人/>的当前位置与机器人/>的当前位置的连线在所述导航坐标系下的姿态角。
9.一种地下管道双机器人步进式协同导航坐标系统，其特征在于，包括：
应用于地下管道双机器人步进式协同导航场景中的两台机器人；
两台机器人交替进行定位，每台所述机器人内置有IMU单元、UWB单元以及如权利要求6-8任意一项所述的服务器。
10.根据权利要求9所述的地下管道双机器人步进式协同导航坐标系统，其特征在于，每台所述机器人内置的IMU单元和UWB单元共轴安装。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310883369.0A CN116608851B (zh) | 2023-07-19 | 2023-07-19 | 地下管道双机器人步进式协同导航方法、系统及服务器 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310883369.0A CN116608851B (zh) | 2023-07-19 | 2023-07-19 | 地下管道双机器人步进式协同导航方法、系统及服务器 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116608851A true CN116608851A (zh) | 2023-08-18 |
| CN116608851B CN116608851B (zh) | 2023-09-26 |
Family
ID=87676802
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310883369.0A Active CN116608851B (zh) | 2023-07-19 | 2023-07-19 | 地下管道双机器人步进式协同导航方法、系统及服务器 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116608851B (zh) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110190964A1 (en) * | 2010-01-29 | 2011-08-04 | Eurocopter | Turning-stabilized estimation of the attitude angles of an aircraft |
| CN104848867A (zh) * | 2015-05-13 | 2015-08-19 | 北京工业大学 | 基于视觉筛选的无人驾驶汽车组合导航方法 |
| CN105698762A (zh) * | 2016-01-15 | 2016-06-22 | 中国人民解放军国防科学技术大学 | 一种单机航迹上基于不同时刻观测点的目标快速定位方法 |
| CN110967017A (zh) * | 2019-11-22 | 2020-04-07 | 河南科技大学 | 一种用于双移动机器人刚体协作搬运的协同定位方法 |
| US20200318971A1 (en) * | 2019-04-02 | 2020-10-08 | Kabushiki Kaisha Toyota Chuo Kenkyusho | Inertial navigation device and inertial navigation method |
| CN115129085A (zh) * | 2022-07-25 | 2022-09-30 | 中国安全生产科学研究院 | 一种多群组机器人协同执行任务的方法 |
-
2023
- 2023-07-19 CN CN202310883369.0A patent/CN116608851B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110190964A1 (en) * | 2010-01-29 | 2011-08-04 | Eurocopter | Turning-stabilized estimation of the attitude angles of an aircraft |
| CN104848867A (zh) * | 2015-05-13 | 2015-08-19 | 北京工业大学 | 基于视觉筛选的无人驾驶汽车组合导航方法 |
| CN105698762A (zh) * | 2016-01-15 | 2016-06-22 | 中国人民解放军国防科学技术大学 | 一种单机航迹上基于不同时刻观测点的目标快速定位方法 |
| US20200318971A1 (en) * | 2019-04-02 | 2020-10-08 | Kabushiki Kaisha Toyota Chuo Kenkyusho | Inertial navigation device and inertial navigation method |
| CN110967017A (zh) * | 2019-11-22 | 2020-04-07 | 河南科技大学 | 一种用于双移动机器人刚体协作搬运的协同定位方法 |
| CN115129085A (zh) * | 2022-07-25 | 2022-09-30 | 中国安全生产科学研究院 | 一种多群组机器人协同执行任务的方法 |
Non-Patent Citations (1)
| Title |
|---|
| 杨健;孙正杰;张晨光;余跃;: "基于协同再入飞行器的舰船目标定位方法研究", 计算机仿真, no. 03, pages 108 - 113 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116608851B (zh) | 2023-09-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN1995917B (zh) | 一种电子地图俯视角度显示控制方法、装置及设备 | |
| CN113311436B (zh) | 一种移动平台上激光测风雷达运动姿态测风订正方法 | |
| CN103196445B (zh) | 基于匹配技术的地磁辅助惯性的载体姿态测量方法 | |
| CN106197406A (zh) | 一种基于惯性导航和rssi无线定位的融合方法 | |
| Sun et al. | Accuracy improvement of SINS based on IMU rotational motion | |
| CN103697885A (zh) | 自动补偿磁偏角的远程定位方法 | |
| CN110763238A (zh) | 基于uwb、光流和惯性导航的高精度室内三维定位方法 | |
| CN105203103A (zh) | 地面航天器相对地理方位关系的实时测量方法 | |
| Shi et al. | Omnidirectional angle constraint based dynamic six-degree-of-freedom measurement for spacecraft rendezvous and docking simulation | |
| CN115326084A (zh) | 车辆定位方法、装置、计算机设备、存储介质 | |
| Taylor | An analysis of observability-constrained Kalman filtering for vision-aided navigation | |
| CN108106597B (zh) | 全捷联激光导引头在目标出线性视场情况下角度测量方法 | |
| CN102607563B (zh) | 利用背景天文信息对于航天器进行相对导航的系统 | |
| CN113900061A (zh) | 基于uwb无线定位与imu融合的导航定位系统及方法 | |
| CN116608851B (zh) | 地下管道双机器人步进式协同导航方法、系统及服务器 | |
| CN102967308B (zh) | 一种远距离任意目标的三维定位方法 | |
| Wang et al. | A revised monte carlo method for target location with uav | |
| Liu et al. | Multi-robot cooperative localization with range-only measurement by UWB | |
| CN102269594B (zh) | 基于路径积分的空间导航方法 | |
| ZHANG et al. | A Survey of Scene-Based Augmentation Systems for Comprehensive PNT | |
| CN114894180A (zh) | 基于相对导航信息的多源融合导航方法及系统 | |
| CN114608564B (zh) | 一种基于夜间月光偏振-星光信息融合的组合定位方法 | |
| Kaiser et al. | Position and orientation of an aerial vehicle through chained, vision-based pose reconstruction | |
| Li et al. | Variance optimization of UWB observation based on map matching for PPP/INS/UWB tightly coupled positioning | |
| Zhang et al. | A new method of single celestial-body sun positioning based on theory of mechanisms |
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 |