一种结合单机积分式里程计与多机互相测距的分布式无人机集群定位方法技术

本发明专利技术公开了一种结合单机积分式里程计与多机互相测距的分布式无人机集群定位方法，包括以下步骤：S1，基于这一距离和收到的其他个体的定位数据通过互相测距设备进行测距定位解算，解算出一个位置p1；S2，个体进行累积式定位，得到一个无修正的定位输出p0；S3，p1减去p0得到一个实时定位误差；S4，通过一个低通滤波器过滤掉定位误差的定位噪声，使得定位误差估计更为平滑；S5，将滤波后的定位误差与累积式定位的无修正定位输出p0叠加，得到修正后的定位数据；S6，然后将修正后的定位数据广播至其它个体，供其他个体校准定位，从而避免碰撞，分布式架构无惧部分个体地离线或丢失，系统鲁棒性高。棒性高。棒性高。

【技术实现步骤摘要】
一种结合单机积分式里程计与多机互相测距的分布式无人机集群定位方法


[0001]本专利技术实施涉及智能系统集群自主导航领域，尤其涉及任意类型空中、地面、水中智能系统集群在各种环境下实现高精度的相对定位以及绝对定位的


技术介绍

[0002]大规模城市级或中等规模乡镇级环境地图中进行空中物品运输安全飞行路线的调度与规划需求，需要准确的无人机集群间相对定位数据以实现互相避碰和绝对世界坐标系下的定位数据以将包裹准确送达目标位置，已知相关的产业产品包括Amazon Prime Air空中物流无人机及其配套规划系统、美团配送无人机及其上层调度系统以及FedEx的快件混合翼运输系统等；空中编队表演，当队形较为密集时需要额外的测量的数据维持飞行器间的相互定位数据从而避免相互碰撞；已知国内有较大影响力的为深圳高巨创新的无人机编队表演；地面密集集群配送系统的避障需求，如自动仓库、码头的集群物流车等等；如海康威视、极智嘉、快仓等推出的智能仓储AGV车解决方案，图森未来的港口自动驾驶物流解决方案等；水面或水下高密度、类鱼群的智能体集群，业内相关平台有如北京曼宝科技开发的水下机器人系统，但该系统为遥控操作，无自主性。
[0003]目前主流的定位方式从大方向上分主要分为两类，(1)智能体个体独立运行的、不依赖已知外部特征位置信息的累积式定位，通过不断观察周围环境配合固连在智能体本体上感知运动的惯性传感器准确感知每一段时间内的相对定位变化量，通过在上一次的基础上不断累加变化量从而得到未来某一时刻的定位数据，该定位方式的优点是易于部署、对未知环境适应性强；(2)已知外部某些特征的位置信息的定位方式，如GPS定位、建立有固定基站的UWB定位、固定粘贴特征二维码的视觉定位等等，这一定位方式中部分可识别特征在世界坐标系下的位置是已知的，从而智能体个体根据对每一个特征的观测获得个体相对于特征的未知关系，从而结合特征的已知位置获得全局一致的定位测量，但其缺点则是依赖预先的环境布置，如固定基站的布置，或者依赖GPS信号质量，或者提前建立部署场景下的完整特征地图，或者使用室内动作捕捉系统等等，从而对部署时间成本、人力成本、经济成本较高，在未知环境下的适用性差，其中GPS在不依赖地面固定基站(RTK)校准的情况下，民用级精度约为10米。
[0004]在实际应用中，如大疆公司的无人机、各自动驾驶公司的车辆，往往是采用以上两者的结合，在室外环境有GPS信号的全局定位辅助，在室内环境下便仅依靠自身传感器对周围环境的感知运行累积式定位了。
[0005]传统的局部与全局相结合的定位方式在各个系统单独运行的时候(单机情境下)没有问题，但是一旦涉及到集群，有一个显著的问题无法得到有效、经济、稳定的解决，即互相避碰问题。由于累积式的定位方式是通过不断累积定位变化量来实现定位的，从而其天然的存在累积误差，即漂移问题，该误差会随着运动距离的增加而不断加大。在集群(或更一般的讲法：有互相避碰需求的)环境下，由于累积式定位是每一个体独立运行的，因此大
家漂移不一致，从而可能会导致两个体发生相撞，尽管各自的定位数据显示此时两者保持了一定的安全距离。融入GPS可以在一定程度上修正累积误差，但是GPS精度(民用级10米)通常较低，从而修正质量在集群密度较大的时候难以得到满足。除此以外，室内环境或地下环境中，GPS信号无法穿透，因此会完全失效，而采用其它的人为构建环境已知特征位置信息的方式，如UWB基站、提前建图、室内动作捕捉系统等等，各方面成本是巨大的，且在未知环境适用性都不佳，无法做到自主无人设备的开箱即用，在自动驾驶领域，为了避免多车辆的互相避碰，通常采用价格及其昂贵的激光雷达结合数十个高清摄像头、配合超高算力的车载计算系统识别周围环境车辆，实现互相避碰。该领域领导者之一的特斯拉公司舍弃了激光雷达，但是因识别错误导致的车祸事件屡见不鲜。可见视觉识别的方式是难以确保绝对的安全性的，且配合高算力计算机，高成本依旧是其推广的一大阻碍，其成本往往高达几十万元。另一实施路径属于纯粹的依赖外部已知信息，如车路协同计划，通过特殊的道路基础设施建设，为车辆提供实时的精确测距和通信，但是该方式如前文所说，一方面道路基础设施全面建设成本高昂，另一方面完全限制了自动驾驶车辆的可通行范围。除此以外，受制于载重限制，以上方式是难以在小型无人机平台部署的，综上分析，目前还没有一种可以成熟且廉价的实现集群系统互相精确定位从而确保不碰撞的解决方案。

技术实现思路

[0006]本专利技术的目的，是为了解决
技术介绍
中的问题，提供一种结合单机积分式里程计与多机互相测距的分布式无人机集群定位方法。
[0007]本专利技术的上述技术目的是通过以下技术方案得以实现的：
[0008]一种结合单机积分式里程计与多机互相测距的分布式无人机集群定位方法，包括以下步骤：
[0009]S1，集群中各个个体之间互相测距和通信，集群各个体通过广播各自修正后的实时位置，从而对于每一个个体，它总是知道周围个体实时的理论绝对位置(可能存在偏差或漂移)和自身到周边个体的测距距离，从而基于这一距离和收到的其他个体的定位数据进行测距定位解算，解算出一个位置p1；
[0010]S2，个体进行累积式定位，得到一个无修正的定位输出p0；
[0011]S3，p1减去p0得到一个实时的定位误差；
[0012]S4，通过一个低通滤波器过滤掉定位误差的定位噪声，使得定位误差估计更为平滑；
[0013]S5，将滤波后的定位误差与累积式定位的无修正定位输出p0叠加，得到修正后的定位数据；
[0014]S6，然后将修正后的定位数据广播至其它个体，供其他个体校准定位。
[0015]“集群”在本专利技术中是指任何数量大于等于2的多智能体系统。
[0016]本专利技术采用了任何使用相对测距辅助定位的集群系统中，同时引入基于距离测量残差优化的位置解算方式。
[0017]本专利技术采用了集群中，任何显式或隐式存在独立的漂移估计环节的定位解算方法。
[0018]优选地，在S2步骤中加入EKF模块对累积式位进行修正得到所述定位输出p0，EKF
模块指的是拓展卡尔曼滤波。
[0019]优选地，以上的算法实现了集群个体间定位的一致性，但其定位数据尚未与世界坐标系(经纬度、航向角)对齐，为了提高精度，引入低精度的GPS定位用作修正，使定位系统得到准确的经纬度信息，引入磁罗盘，得到准确的航向角信息，同时也能纠正各个体自以为的世界坐标系的航向角不对齐问题，注意6自由度定位信息(3自由度位置、3自由度方向)方向信息中的俯仰与横滚已在累积式定位模块中与重力天然对齐，从而不再需要额外的修正。
[0020]优选地，所述互相测距设备常用的有UWB测距、毫米波雷达测距、超声波测距、激光测距、信号强度RSSI等诸多方式，本专利技术不对具体传感器的使用作要求，并将其统一数学建模为带噪声的距离测量r
i，j
＝R
i，j
+ω
d
，其中，R
i，j<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种结合单机积分式里程计与多机互相测距的分布式无人机集群定位方法，其特征在于，包括以下步骤：S1，集群中各个个体之间互相测距和通信，集群各个体通过广播各自修正后的实时位置，从而对于每一个个体，它总是知道周围个体实时的理论绝对位置(可能存在偏差或漂移)和自身到周边个体的测距距离，从而基于这一距离和收到的其他个体的定位数据通过互相测距设备进行测距定位解算，解算出一个位置p1；S2，个体进行累积式定位，得到一个无修正的定位输出p0；S3，p1减去p0得到一个实时的定位误差；S4，通过一个低通滤波器过滤掉定位误差的定位噪声，使得定位误差估计更为平滑；S5，将滤波后的定位误差与累积式定位的无修正定位输出p0叠加，得到修正后的定位数据；S6，然后将修正后的定位数据广播至其它个体，供其他个体校准定位。2.根据权利要求1所述的一种结合单机积分式里程计与多机互相测距的分布式无人机集群定位方法，其特征在于，在S2步骤中加入EKF模块对累积式位进行修正得到所述定位输出p0。3.根据权利要求1所述的一种结合单机积分式里程计与多机互相测距的分布式无人机集群定位方法，其特征在于，在S1步骤中的测距定位解算的方式为单步式，公式如下：令本个体编号为i，其它个体编号为j，其中j∈[1,N],j≠i,N为集群总数；对于每一有效帧，即包含了测距值数据和其它个体定位信息的的数据帧，构造优化问题问题其中，是本个体的定位值，也是优化问题的初值，右上角上标表示该数字变量当前处在流程的步骤，表示当前帧原始的未修正定位数据，表示未修正定位数据加上上一次定位校正所得的初步修正结果，表示考虑当前测距数据后得到的修正位置；优化项是指令优化的结果尽可能靠近上一轮定位数据得到的定位结果，使得减少优化问题的鞍点或局部极小值点，提高收敛率；后面的项与实际测距不一致的程度，各个平方项是为了尽可能保证优化问题的二阶导连续或跳变尽可能小，从而有利于优化算法对最优解的搜索。4.根据权利要求3所述的一种结合单机积分式里程计与多机互相测距的分布式无人机集群定位方法，其特征在于，经过以上优化问题得到基于当前次测距数据所得的定位位置后，减去原始未修正定位输出数据得到基于当前次测距数据的定位漂移量一般情况下，e
now
应当是较为接近e
last
的，因为原始的、基于累积式原理的定位数据的漂移应当是持续且变化
缓慢的；于是，这里采用加权滑动窗口的方式进行滤波，窗口内记录最新的K个漂移量原始数据，按照的方式获得当前帧的最佳漂移量估计，其中e
k
|
k＝K
即为e
now
，但e
k
|
k＝K
‑1≠e
last
，因为e
k
记录的是原始数据，而e
last
是上一帧的滤波后数据；右上角的*代表最佳估计；随后将当前帧的原始未校正定位数据加上基于当前帧测距...

【专利技术属性】
技术研发人员：高飞，周鑫，雷鸣，
申请(专利权)人：湖州快飞智能科技有限公司，
类型：发明
国别省市：