CN113608556B - 一种基于多传感器融合的多机器人相对定位方法 - Google Patents
一种基于多传感器融合的多机器人相对定位方法 Download PDFInfo
- Publication number
- CN113608556B CN113608556B CN202110814224.6A CN202110814224A CN113608556B CN 113608556 B CN113608556 B CN 113608556B CN 202110814224 A CN202110814224 A CN 202110814224A CN 113608556 B CN113608556 B CN 113608556B
- Authority
- CN
- China
- Prior art keywords
- image
- visual
- robot
- information
- pose
- 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.)
- Active
Links
- 230000004927 fusion Effects 0.000 title claims abstract description 25
- 238000000034 method Methods 0.000 title claims abstract description 24
- 230000000007 visual effect Effects 0.000 claims abstract description 31
- 239000011159 matrix material Substances 0.000 claims abstract description 17
- 238000004891 communication Methods 0.000 claims description 10
- 238000001914 filtration Methods 0.000 claims description 9
- 238000002372 labelling Methods 0.000 claims description 3
- 239000003086 colorant Substances 0.000 claims description 2
- 238000012423 maintenance Methods 0.000 abstract description 5
- 238000004364 calculation method Methods 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 4
- 238000013461 design Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000007639 printing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- 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/12—Target-seeking control
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明涉及一种基于多传感器融合的多机器人相对定位方法,旨在实现多无人机协同运输时的构型感知功能,为控制模块进行阵型保持提供位姿信息。所采用的技术方案包括以下步骤:单机对视觉可见范围内其他无人机的视觉位姿估计;IMU信息与视觉估计和UWB测距信息融合;多机间定位结果融合。由于采用了确定尺寸的合作标识,使得视觉定位的鲁棒性、精度和计算速度均得以提高,加快观测更新频率;采用了IMU预测+UWB测距+视觉图像观测的多传感器组合解算,使得多机器人间的定位精度得以进一步提高,并且可以在不依靠GPS信息的情况下保持系统相对构型。采用了环形网络拓扑进行阵型估计,使得系统在部分观测失效的情况下仍能保持运作,具有一定的冗余性。
Description
技术领域
本发明属于无人机感知与定位研究领域,涉及一种基于多传感器融合的多机器人相对定位方法,具体为一种基于多传感器融合的多无人机协同运输系统相互定位方法。
背景技术
多机器人协同系统具有方便组合、灵活、冗余等特点,在近年来取得了长足的发展。在空中搬运作业方面,相较于单架大飞机调运,多无人机系统更是具有单机成本低、维护方便的优点。根据载荷重量的不同,通过增加单元数量,即可增加系统的负载能力。多无人机协同运输系统由无人机单元、柔性系绳、载荷连接装置和地面站组成。每架无人机单元上均搭载有相机、惯导传感器、超宽带(UWB)信标、计算单元和控制系统,相互间能进行实时通信。在工作时,无人机单元间相距10~20m。地面站主要用于监视无人机状态,在出现安全隐患时及时切换到人工控制。
多无人机协同运输系统相互定位方式有多种,目前主流的方法是使用全球定位系统(GPS)/北斗卫星导航系统(BDS)等卫星定位系统定位绝对位置,再进行相对位置解算,目前已经在行业展开应用。GPS等卫星定位系统民用精度通常在米级,只能满足单机作业的定位要求。GPS-RTK(GPS-Real-time-kinematic)是一种采用载波相位动态实时差分的定位方法,该方法的定位精度可以达到厘米级,但由于设备价格较为昂贵且需要地面基站等原因,因此无法得到广泛应用。此外,多无人机协同运输相对无人机编队而言,是一个强耦合的系统,系绳上的张力分配直接影响整个系统的续航时间,因此对相对构型、高度差更为敏感,这就对相对位姿定位提出了更高的要求。为了解决上述问题,我们提出了基于惯导、超宽带(UWB)技术和视觉的多传感器融合定位方案,通过冗余求解使得相对定位误差最小。此外,由于上述传感器都不依赖外部基站,故系统能在GPS受干扰的情况下正常工作。
在机器人定位领域,多传感器融合的定位方案已被行业广泛采用。例如,申请号为CN202011388909.0的中国专利提出了一种基于IMU数据、轮速里程计数据、图像数据、激光数据和UWB数据的机器人定位方法,通过对IMU数据预积分、与图像观测对齐等,与激光雷达等传感器数据融合,得到鲁棒的位姿数据。申请号为CN202011071053.4的中国专利提出了一种基于图像、深度图、IMU数据和2D激光雷达的融合定位方法。可见,单机的高精度、高鲁棒性定位方案已经相对成熟,但是多机高精度相对定位方面还存在较大的缺口。无人机在三维欧氏空间内自由运动,且相距较远(大于一般激光雷达的工作范围),又对传感器的选型和融合方案提出了更多的限制。
发明内容
要解决的技术问题
为了避免现有技术的不足之处,本发明提出一种基于多传感器融合的多机器人相对定位方法,在于设计一种基于多传感器融合的相互定位方法。旨在实现多无人机协同运输时的构型感知功能,为控制模块进行阵型保持提供位姿信息。
技术方案
一种基于多传感器融合的多机器人相对定位方法,其特征在于步骤如下:
步骤1、单元机器人对视觉可见范围内其他机器人的视觉位姿估计:
以AprilTag作为合作标识对每一个机器人进行标识;
系统内任一单元机器人只对特定的对象进行检测和跟踪;
任一单元机器人从机载相机读入图像,使用OpenCV将像素信息转化为cv::MAT矩阵形式,并将图像转为灰度图;
对后续输入图像进行裁剪:根据上一时刻合作标识在图像中的位置裁剪下一时刻输入图像,仅保留合作标识附近的图像区域;对图像区域寻找合作标识,计算合作标识在图像平面的位置/>和齐次矩阵/>即视觉解算结果,广播在ROS通信网络中;
如果在图像区域中未找到合作标识时,在下一帧图像中扩大剪裁区域直至能够找到合作标识;
步骤2、IMU信息与视觉估计和UWB测距信息融合:
解算任一单元机器人IMU中的信息,获得位姿增量ΔRij,Δvij,Δpij和协方差矩阵δΦij,δvij,δpij,作为预测单元机器人在下一时刻的位置和协方差即惯导预测结果;使用UWB标签进行DS-TWR测距,获取各机间距离信息;
各单元机器人将解算出本机的惯导数据和UWB测距信息广播在ROS通信网络中;
各单元将步骤1中视觉解算结果和UWB测距信息结果作为观测,与惯导预测结果进行融合滤波,将融合结果广播在ROS通信网络中;如果出现长时间视觉观测失效或结果协方差大于阈值的情况,为滤波结果贴上Exception标签,标记为不可信的数据;
步骤3、多机器人间定位结果融合:
领航的单元机器人根据融合后的相对位姿数据计算当前系统相对构型,环形网络拓扑最多容忍一个成员定位失效;
如果存在两个或以上成员定位失效,或通信错误时,系统暂停执行当前任务,保持悬停状态并在地面站上报错。
无论何时,地面站具有最高优先级,对当前任务状态进行超控。
所述合作标识采用黑白两色,外边框大小为0.5m。
所述读入图像的分辨率大于1920*1080。
有益效果
本发明提出的一种基于多传感器融合的多机器人相对定位方法,旨在实现多无人机协同运输时的构型感知功能,为控制模块进行阵型保持提供位姿信息。所采用的技术方案包括以下步骤:单机对视觉可见范围内其他无人机的视觉位姿估计;IMU信息与视觉估计和UWB测距信息融合;多机间定位结果融合。
与现有技术相比,本发明具有以下益处:
(1)由于采用了确定尺寸的合作标识,使得视觉定位的鲁棒性、精度和计算速度均得以提高,加快观测更新频率;
(2)由于采用了IMU预测+UWB测距+视觉图像观测的多传感器组合解算,使得多机器人间的定位精度得以进一步提高,并且可以在不依靠GPS信息的情况下保持系统相对构型。
(3)由于采用了环形网络拓扑进行阵型估计,使得系统在部分观测失效的情况下仍能保持运作,具有一定的冗余性。
附图说明
图1:合作标识加速计算流程图
图2:多传感器融合位姿估计示意图
图3:系统拓扑结构示意图
图4:系统流程图
具体实施方式
现结合实施例、附图对本发明作进一步描述:
本发明的目的在于设计一种基于多传感器融合的相互定位方法。旨在实现多无人机协同运输时的构型感知功能,为控制模块进行阵型保持提供位姿信息。
为了实现上述目的,本发明所采用的技术方案包括以下步骤:
1)单机对视觉可见范围内其他无人机的视觉位姿估计;
2)IMU信息与视觉估计和UWB测距信息融合;
3)多机间定位结果融合。
所述的步骤1)单机对其他无人机的视觉位姿估计:
1.1)合作标识的设置:使用AprilTag作为合作标识,黑白打印,外边框大小0.5m。
1.2)确定系统拓扑:受限于机载计算单元的性能,单元无人机只对特定的对象进行检测和跟踪:设集群数量为n,标号为i的飞机仅由标号为i+1的飞机进行追踪,标号为n-1的飞机由标号为0的飞机进行追踪,组成环形网络。
1.3)图像预处理:从机载相机读入较高分辨率的图像,使用OpenCV将像素信息转化为cv::MAT矩阵形式,并将图像转为灰度图,记灰度图尺寸为gray=(cols,rows)T。
1.4)位姿初始化:使用AprilTag算法进行全图搜索,找到目标合作标识在图像平面的位置使用PnP方法求解从相机到合作标识的齐次矩阵/>更新裁剪偏移量裁剪区域大小size=kw×gray,其中kw<1为人为给定的缩放系数。
porigin.uv=pdetect.uv+bias-size/2
若图像区域内未检测到目标合作标识,且当前距离上一次检测到该标识的时间差小于阈值,即tnow-tlastframe<tthreshold,更新size=2×size;若超时,重置窗口大小size=gray,并返回1.3)进行重新初始化。
所述的步骤2)IMU信息与视觉估计和UWB测距信息融合:
2.1)解算IMU信息,获得位姿增量ΔRij,Δvij,Δpij和协方差矩阵δΦij,δvij,δpij;
2.2)使用UWB标签进行DS-TWR测距,获取各机间距离信息。
2.3)各单元将解算出的惯导数据和UWB测距信息广播在ROS通信网络中。
2.4)各单元根据自身惯导数据和跟踪目标的惯导数据做积分预测。
2.5)如图2所示,各单元将1)中视觉解算结果和UWB测距结果作为观测,与惯导预测结果进行融合滤波。其中,视觉解算结果包含了目标方位角和距离信息,作为主要的观测值,视为主节点;UWB信标仅提供了距离信息,作为从属节点附加在主节点上,施加“弹簧”约束,作为测量距离变化的边界;对IMU数据依照观测进行修正,求取相对位姿使误差最小。
如果出现长时间视觉观测失效或结果协方差大于阈值的情况,为滤波结果贴上Exception标签,标记为不可信的数据。
所述的步骤3)多机间定位结果融合:
3.1)领航者根据融合后的相对位姿数据估计当前系统相对构型。如图3所示,相对位姿矩阵通过环形网络传递,网络最多可以容忍一个成员定位失效,此时依旧可以保证构型估计的完整性。
3.2)如果存在两个或以上成员定位失效,或通信错误时,系统暂停执行当前任务,保持悬停状态并在地面站上报错。
3.3)无论何时,地面站具有最高优先级,可以对当前任务状态进行超控。系统总体流程图如图4所示。
Claims (4)
1.一种基于多传感器融合的多机器人相对定位方法,其特征在于步骤如下:
步骤1、单元机器人对视觉可见范围内其他机器人的视觉位姿估计:
以AprilTag作为合作标识对每一个机器人进行标识;
系统内任一单元机器人只对特定的对象进行检测和跟踪;
任一单元机器人从机载相机读入图像,使用OpenCV将像素信息转化为cv::MAT矩阵形式,并将图像转为灰度图;
对后续输入图像进行裁剪:根据上一时刻合作标识在图像中的位置裁剪下一时刻输入图像,仅保留合作标识附近的图像区域;对图像区域寻找合作标识,计算合作标识在图像平面的位置/>和齐次矩阵/>即视觉解算结果,广播在ROS通信网络中;
如果在图像区域中未找到合作标识时,在下一帧图像中扩大剪裁区域直至能够找到合作标识;
步骤2、IMU信息与视觉估计和UWB测距信息融合:
解算任一单元机器人IMU中的信息,获得位姿增量ΔRij,Δvij,Δpij和协方差矩阵δΦij,δvij,δpij,作为预测单元机器人在下一时刻的位置和协方差即惯导预测结果;使用UWB标签进行DS-TWR测距,获取各机间距离信息;
各单元机器人将解算出本机的惯导数据和UWB测距信息广播在ROS通信网络中;
各单元将步骤1中视觉解算结果和UWB测距信息结果作为观测,与惯导预测结果进行融合滤波,将融合结果广播在ROS通信网络中;如果出现长时间视觉观测失效或结果协方差大于阈值的情况,为滤波结果贴上Exception标签,标记为不可信的数据;
步骤3、多机器人间定位结果融合:
领航的单元机器人根据融合后的相对位姿数据计算当前系统相对构型,环形网络拓扑最多容忍一个成员定位失效;
如果存在两个或以上成员定位失效,或通信错误时,系统暂停执行当前任务,保持悬停状态并在地面站上报错。
2.根据权利要求1所述基于多传感器融合的多机器人相对定位方法,其特征在于:无论何时,地面站具有最高优先级,对当前任务状态进行超控。
3.根据权利要求1或2所述基于多传感器融合的多机器人相对定位方法,其特征在于:所述合作标识采用黑白两色,外边框大小为0.5m。
4.根据权利要求1或2所述基于多传感器融合的多机器人相对定位方法,其特征在于:所述读入图像的分辨率大于1920*1080。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110814224.6A CN113608556B (zh) | 2021-07-19 | 2021-07-19 | 一种基于多传感器融合的多机器人相对定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110814224.6A CN113608556B (zh) | 2021-07-19 | 2021-07-19 | 一种基于多传感器融合的多机器人相对定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113608556A CN113608556A (zh) | 2021-11-05 |
CN113608556B true CN113608556B (zh) | 2023-06-30 |
Family
ID=78337909
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110814224.6A Active CN113608556B (zh) | 2021-07-19 | 2021-07-19 | 一种基于多传感器融合的多机器人相对定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113608556B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114136306B (zh) * | 2021-12-01 | 2024-05-07 | 浙江大学湖州研究院 | 一种可拓展的基于uwb和摄像头的相对定位的设备和方法 |
CN114353800B (zh) * | 2021-12-31 | 2023-10-24 | 哈尔滨工业大学 | 一种基于谱图方法的多机器人互定位可观性判别方法及系统 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110865650A (zh) * | 2019-11-19 | 2020-03-06 | 武汉工程大学 | 基于主动视觉的无人机位姿自适应估计方法 |
WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
-
2021
- 2021-07-19 CN CN202110814224.6A patent/CN113608556B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
CN110865650A (zh) * | 2019-11-19 | 2020-03-06 | 武汉工程大学 | 基于主动视觉的无人机位姿自适应估计方法 |
Non-Patent Citations (1)
Title |
---|
UWB和IMU技术融合的室内定位算法研究;王嘉欣;李桂林;曹海东;;单片机与嵌入式系统应用(第08期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN113608556A (zh) | 2021-11-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106767853B (zh) | 一种基于多信息融合的无人驾驶车辆高精度定位方法 | |
US10788830B2 (en) | Systems and methods for determining a vehicle position | |
US9921065B2 (en) | Unit and method for improving positioning accuracy | |
US11227168B2 (en) | Robust lane association by projecting 2-D image into 3-D world using map information | |
CN113608556B (zh) | 一种基于多传感器融合的多机器人相对定位方法 | |
CN108362294B (zh) | 一种应用于自动驾驶的多车协同建图方法 | |
US9122278B2 (en) | Vehicle navigation | |
CN106226780A (zh) | 基于激光扫描雷达的多旋翼室内定位系统及实现方法 | |
Bischoff et al. | Fusing vision and odometry for accurate indoor robot localization | |
CN111025366B (zh) | 基于ins及gnss的网格slam的导航系统及方法 | |
CN111426320A (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
Rady et al. | A hybrid localization approach for UAV in GPS denied areas | |
CN114877883B (zh) | 车路协同环境下考虑通信延迟的车辆定位方法及系统 | |
CN114485619A (zh) | 基于空地协同的多机器人定位和导航方法及装置 | |
Sabatini et al. | Low-cost navigation and guidance systems for Unmanned Aerial Vehicles. Part 1: Vision-based and integrated sensors | |
Andert et al. | Improving monocular SLAM with altimeter hints for fixed-wing aircraft navigation and emergency landing | |
CN113093759A (zh) | 基于多传感器信息融合的机器人编队构造方法及系统 | |
Andert et al. | Radar-aided optical navigation for long and large-scale flights over unknown and non-flat terrain | |
CN113670318A (zh) | 协同定位方法及定位系统 | |
US20240200953A1 (en) | Vision based cooperative vehicle localization system and method for gps-denied environments | |
Roger-Verdeguer et al. | Visual odometry with failure detection for the aegis UAV | |
Ishii et al. | Autonomous UAV flight using the Total Station Navigation System in Non-GNSS Environments | |
Wang et al. | Raillomer: Rail vehicle localization and mapping with LiDAR-IMU-odometer-GNSS data fusion | |
US20230115602A1 (en) | Methods and Systems for Determining a Position and an Acceleration of a Vehicle | |
JP7302966B2 (ja) | 移動体 |
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 |