本发明涉及多无人船自主探索自主定位和地图构建,具体而言,尤其涉及一种多无人船未知区域协同探索的自干扰持续消除方法。
背景技术:
1、随着科技水平和认知水平的提高,人类的研究早已不局限于较为简单的地表,而是把目光投向了海洋深处、地下及外太空等复杂的区域拓展。这些区域往往环境较为恶劣且复杂多变,这对人类的探索工作提出了巨大的挑战。
2、无人船的出现可以代替人类完成一部分危险任务,但无人船的工作需要高精度的电子地图作为支撑,于是诸多研究者对无人船的自主探索算法开展了大量研究。搭载slam模块的无人船配合快速拓展随机树或图像边缘检测的边界探索算法的自主探索架构已经较为成熟,具有很高的实用价值。但单无人船的探索效率是有上限的,于是研究者们把目光转向了多无人船协同探索。
3、与单无人船自主探索相比,多无人船自主探索在大幅度提高探索效率的同时也带来了明显的问题。在探索过程中,无人船很难保证不出现在彼此的激光雷达感知范围内,于是有可能成为彼此的动态障碍物。这将导致无人船的地图构建中出现由动态障碍物干扰产生杂点,在特殊情况下甚至会凭空出现障碍物,产生自干扰问题。
4、目前现有的方法,对于动态障碍物的识别需要对多帧概率地图进行变化计算,这将带来巨大的计算需求。然而,由于无人船的算力资源和带载能力有限,庞大的计算量将显著降低系统的实时性,进而对无人船的安全性和地图的准确性产生不利影响。
5、因此,本发明创新性的提出了一种轻量化且低功耗的多无人船协同探索方法,既能持续消除无人船探索中的自干扰问题,同时确保系统高效性和稳定性。
技术实现思路
1、根据上述提出多无人船自主探索中的自干扰的技术问题,本发明提出了一种轻量化且低功耗的多无人船协同探索方法,既能持续消除无人船探索中的自干扰问题,同时确保系统高效性和稳定性;本发明方法通过持续剔除动态障碍物所导致的激光雷达点云数据,达到提高探索任务成功率和构建高精度、高质量地图的目的。
2、本发明采用的技术手段如下:
3、一种多无人船未知区域协同探索的自干扰持续消除方法,包括:
4、s1、从系统的tf树中获得无人船之间的坐标转换关系,计算得到无人船之间的旋转矩阵集合{ri}和平移矩阵集合{ti};
5、s2、通过平移矩阵计算本无人船之间的间距{di},如果存在{di}小于范围ε,即di≤ε,发出自干扰报警;
6、s3、通过角度和激光雷达深度信息与di进行比较,若误差在允许范围内,视作存在自干扰问题,使相关无人船进入解决自干扰问题的流程;否则误报警,直接跳转至步骤s6发布雷达数据;
7、s4、将激光雷达数据转化在笛卡尔坐标系后,使用dbscan对激光雷达数据进行聚类处理;
8、s5、使用icp算法得到这一帧激光雷达数据与前一帧激光雷达数据的匹配关系,并分析两帧激光雷达数据的动态障碍物位置关系,处理动态障碍物的激光雷达点云;
9、s6、发布处理前和处理后激光雷达数据;
10、s7、若未完成探索任务,回到步骤s1继续探索,否则结束探索任务。
11、进一步地,所述步骤s1,具体包括:
12、s11、采用slam模块和多地图融合模块建立无人船坐标转换关系,其中slam模块计算出无人船与无人船子地图的坐标关系;多地图融合模块计算出多无人船子地图间的坐标关系;
13、s12、计算无人船之间的旋转矩阵r,计算公式如下:
14、
15、其中,rz表示绕z轴旋转ψ角度的旋转矩阵;
16、s13、计算无人船之间的平移矩阵t,计算公式如下:
17、t=[δx δy 0]t
18、其中,δx和δy分别表示在x轴和y轴上的平移距离。
19、进一步地,在所述步骤s2中,所述范围ε大于激光雷达扫描有效范围。
20、进一步地,所述步骤s5,具体包括:
21、s51、使用icp计算动态障碍物在两帧激光雷达数据的匹配簇,计算簇中所有元素的中心点坐标计算公式如下:
22、
23、
24、s52、得到前一帧动态障碍物中心点坐标本帧动态障碍物中心点坐标为
25、s53、分析两帧激光雷达数据的动态障碍物位置关系,处理动态障碍物的激光雷达点云,若dist(p,q)≤λ*vmax,则动态障碍物匹配成功;其中,dist(p,q)是p和q的欧氏距离,vmax是无人船的最大速度,λ应略大于1以避免微小误差。
26、进一步地,在所述步骤s6中:
27、发布处理前的激光雷达数据,用于为无人船动态路径规划提供数据,保证无人船运动的安全性;
28、发布处理后的激光雷达数据,用于消除由自干扰导致的激光雷达杂点,提高构建地图的质量。
29、较现有技术相比,本发明具有以下优点:
30、1、本发明提供的多无人船未知区域协同探索的自干扰持续消除方法,通过持续剔除动态障碍物所导致的激光雷达点云数据,达到提高探索任务成功率和构建高精度、高质量地图的目的。
31、2、本发明提供的多无人船未知区域协同探索的自干扰持续消除方法,既能持续消除无人船探索中的自干扰问题,同时确保系统高效性和稳定性。
32、基于上述理由本发明可在无人船自主探索自主定位和地图构建等领域广泛推广。
1.一种多无人船未知区域协同探索的自干扰持续消除方法,其特征在于,包括:
2.根据权利要求1所述的多无人船未知区域协同探索的自干扰持续消除方法,其特征在于,所述步骤s1,具体包括:
3.根据权利要求1所述的多无人船未知区域协同探索的自干扰持续消除方法,其特征在于,在所述步骤s2中,所述范围ε大于激光雷达扫描有效范围。
4.根据权利要求1所述的多无人船未知区域协同探索的自干扰持续消除方法,其特征在于,所述步骤s5,具体包括:
5.根据权利要求1所述的多无人船未知区域协同探索的自干扰持续消除方法,其特征在于,在所述步骤s6中: