CN115166686B - 卫星拒止环境下多无人机分布式协同定位与建图方法 - Google Patents

卫星拒止环境下多无人机分布式协同定位与建图方法 Download PDF

Info

Publication number
CN115166686B
CN115166686B CN202211081054.6A CN202211081054A CN115166686B CN 115166686 B CN115166686 B CN 115166686B CN 202211081054 A CN202211081054 A CN 202211081054A CN 115166686 B CN115166686 B CN 115166686B
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
feature
pose
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.)
Active
Application number
CN202211081054.6A
Other languages
English (en)
Other versions
CN115166686A (zh
Inventor
田栢苓
卢俊杰
谌宏鸣
左宗玉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tianjin University
Original Assignee
Tianjin University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Tianjin University filed Critical Tianjin University
Priority to CN202211081054.6A priority Critical patent/CN115166686B/zh
Publication of CN115166686A publication Critical patent/CN115166686A/zh
Application granted granted Critical
Publication of CN115166686B publication Critical patent/CN115166686B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30181—Earth observation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Abstract

本发明公开了卫星拒止环境下多无人机分布式协同定位与建图方法，包括：无人机自主定位，每架无人机利用激光雷达提供的原始点云数据提取自身的点云特征信息，然后估计自身的自主定位信息；多无人机协同定位，每架无人机利用其他无人机发布的点云特征信息在自身的特征地图上做匹配，从而计算其他无人机在自身参考坐标系中的相对位姿信息；多无人机协同建图，每架无人机在自身参考坐标系下，利用自主定位信息、相对位姿信息、点云特征信息维护一张全局特征点云地图。本发明采用上述定位与建图方法，拒止环境下，无需外部传感器或高性能地面处理器，即可使多无人机系统中的每架无人机均维护一张全局一致的特征地图。

Description

卫星拒止环境下多无人机分布式协同定位与建图方法
技术领域
本发明涉及多无人机导航技术领域，尤其是涉及卫星拒止环境下多无人机分布式协同定位与建图方法。
背景技术
近年来，无人机被广泛的应用于地下洞穴探测、森林监测、复杂工业园区重建等领域。上述应用均依赖稳定、实时的多无人机定位系统。基于全球导航卫星系统（globalnavigation satellite system, GNSS）、运动捕捉系统(motion capture system,Mocap)、超宽带测距（ultra-wideband, UWB）等外部传感器的无人机定位方法在过去的几十年中被广泛的应用。然而，外部传感器通常依赖离线部署与标定，无法在一些狭小的非结构化区域为无人机提供稳定的定位信息，这在很大程度上限制了多无人机的应用范围。因此，在卫星拒止环境下实现多无人机的协同定位与建图仍是一项具有挑战的工作。
卫星拒止环境下多无人机分布式协同定位与建图方法，按照传感器类型可以分为基于视觉的多无人机协同定位与建图方法、基于超宽带（Ultra-Wideband，UWB）的多无人机协同定位与建图方法、基于激光雷达的多无人机协同定位与建图方法。
基于视觉的多无人机协同定位与建图方法：利用无人机机载相机拍摄到的图片，在多无人机间做多视图几何匹配，实现多无人机的协同定位。但是，受限于机载相机的有限视场角，当多个无人机间的视场角不存在重叠区域时，会产生较大累计误差。
基于UWB的多无人机协同定位与建图方法：UWB是近年来一种新颖的电波测距技术，超宽带技术通过测量电波的到达时间、到达时间差或到达角计算出两个模块之间的距离。无人机利用自身定位信息配合UWB测距信息可以实现多无人机的协同定位功能。但是，UWB测距信息受其体积与功率的影响，存在测距范围小、易受障碍物遮挡的缺点，容易导致多无人机协同定位失效。
基于激光雷达的多无人机协同定位与建图方法：随着，近年来激光雷达体积、重量、成本的持续降低，使基于激光雷达的无人机自主导航技术成为了可能。得益于激光的主动深度测量特性，激光雷达可以提供不受光照影响的点云信息，从而实现多无人机的协同定位与建图。然而，激光雷达产生的大量点云信息导致基于激光雷达的多无人机协同定位与建图方法的计算复杂度较高，通常依赖集中式地面站实现协同定位与建图功能。
综上所述，目前针对卫星拒止环境下多无人机协同定位与建图方面的相关工作，多受限于相机视场角、UWB测距范围、地面集中式运算平台，难以适应计算资源受限的多无人机平台和对实时性要求高的任务。因此，研究卫星拒止环境下多无人机分布式协同定位与建图方法对于多无人机系统的实际落地应用至关重要。
发明内容
为克服现有技术的不足，本发明旨在提出一种适用于卫星拒止环境的多无人机分布式协同定位与建图方法，无人机通过确定自身定位信息以及其他无人机的相对定位信息，从而为多无人机系统提供全局一致的环境地图信息。
本发明的卫星拒止环境下多无人机分布式协同定位与建图方法，包括：无人机自主定位，每架无人机利用激光雷达提供的原始点云数据提取自身的点云特征信息，然后估计自身的自主定位信息；多无人机协同定位，每架无人机利用其他无人机发布的点云特征信息在自身的特征地图上做匹配，从而计算其他无人机在自身参考坐标系中的相对位姿信息；多无人机协同建图，每架无人机在自身参考坐标系下，利用自主定位信息、相对位姿信息、点云特征信息维护一张全局特征点云地图。
进一步的，无人机自主定位的具体步骤包括：
S11、从激光雷达点云中提取特征点集
Figure 923502DEST_PATH_IMAGE001
，包括位于平坦区域的面特征点集
Figure 475706DEST_PATH_IMAGE002
和位于边缘或尖锐区域的线特征点集
Figure 271624DEST_PATH_IMAGE003
；
S12、通过特征点集
Figure 24816DEST_PATH_IMAGE004
与特征地图
Figure 30818DEST_PATH_IMAGE005
的匹配关系构建雷达约束残差；
特征点在无人机自身参考坐标系下的位置：
Figure 793238DEST_PATH_IMAGE006
线特征点集的匹配残差：
Figure 568296DEST_PATH_IMAGE007
面特征点集的匹配残差：
Figure 757969DEST_PATH_IMAGE008
式中，
Figure 126633DEST_PATH_IMAGE009
为特征点云
Figure 286219DEST_PATH_IMAGE010
中的特征点，
Figure 322308DEST_PATH_IMAGE011
为无人机在自身参考坐标系下的位姿初值；
Figure 13708DEST_PATH_IMAGE012
为线特征点集中的特征点，
Figure 604090DEST_PATH_IMAGE013
与
Figure 567366DEST_PATH_IMAGE014
为无人机自身特征地图
Figure 192383DEST_PATH_IMAGE015
中距离
Figure 582913DEST_PATH_IMAGE016
最近且不在同一条扫描线上的任意两个点；
Figure 660590DEST_PATH_IMAGE017
为面特征点集中的特征点，
Figure 161979DEST_PATH_IMAGE018
、
Figure 907081DEST_PATH_IMAGE019
、
Figure 468512DEST_PATH_IMAGE020
为无人机自身特征地图
Figure 33486DEST_PATH_IMAGE021
中距离
Figure 338565DEST_PATH_IMAGE022
最近的任意三个点；
S13、根据特征点集的匹配残差，将基于雷达特征-地图匹配的无人机自主定位问题转化为非线性最小二乘问题；通过高斯-牛顿法对非线性最小二乘问题的迭代求解，即可实现多无人机系统中每架无人机对自身自主定位
Figure 672594DEST_PATH_IMAGE023
的估计；
Figure 545873DEST_PATH_IMAGE024
式中，
Figure 722776DEST_PATH_IMAGE025
为无人机在自身参考坐标系中的位姿，
Figure 706913DEST_PATH_IMAGE026
为通过无人机在自身参考坐标系中位姿的估计值，
Figure 20082DEST_PATH_IMAGE027
为线特征点集
Figure 64262DEST_PATH_IMAGE028
中特征点的数量，
Figure 869407DEST_PATH_IMAGE029
为面特征点集
Figure 507499DEST_PATH_IMAGE030
中特征点的数量。
进一步的，S11步骤中依据平滑度确定特征点集
Figure 81700DEST_PATH_IMAGE031
，具体地，
平滑度：
Figure 296781DEST_PATH_IMAGE032
式中，
Figure 713856DEST_PATH_IMAGE033
为激光雷达点云中的点，
Figure 774216DEST_PATH_IMAGE034
为
Figure 202923DEST_PATH_IMAGE035
的邻域点集，
Figure 713539DEST_PATH_IMAGE036
为
Figure 555593DEST_PATH_IMAGE037
的邻域点集中点的数量，
Figure 278698DEST_PATH_IMAGE038
为向量的范数，
Figure 296333DEST_PATH_IMAGE039
为
Figure 181112DEST_PATH_IMAGE040
的邻域点集中包含的点；
依据平滑度值对点云排序，最大的数十个点即为线特征点集
Figure 307200DEST_PATH_IMAGE041
，最小的数十个点即为面特征点集
Figure 974942DEST_PATH_IMAGE042
，且规定一个特征点周围10个邻近点不能作为特征点。
进一步的，多无人机协同定位的具体步骤包括：
S21、构建无人机的自主定位因子；
无人机自主定位观测：
Figure 378241DEST_PATH_IMAGE043
位姿增量：
Figure 965080DEST_PATH_IMAGE044
位姿观测增量：
Figure 453831DEST_PATH_IMAGE045
自主定位因子：
Figure 787247DEST_PATH_IMAGE046
式中，
Figure 779474DEST_PATH_IMAGE047
表示
Figure 943739DEST_PATH_IMAGE048
时刻的无人机自主定位观测矩阵，
Figure 44419DEST_PATH_IMAGE049
表示
Figure 788384DEST_PATH_IMAGE050
时刻的无人机自主定位观测矩阵，
Figure 166276DEST_PATH_IMAGE051
表示
Figure 94918DEST_PATH_IMAGE052
至
Figure 292681DEST_PATH_IMAGE053
间无人机的位姿增量，
Figure 230550DEST_PATH_IMAGE054
表示无人机自主定位观测协方差；
Figure 197369DEST_PATH_IMAGE055
表示
Figure 703437DEST_PATH_IMAGE056
时刻
Figure 513130DEST_PATH_IMAGE057
在
Figure 864477DEST_PATH_IMAGE058
参考坐标系下的位姿的逆，
Figure 951381DEST_PATH_IMAGE059
表示
Figure 487405DEST_PATH_IMAGE060
时刻
Figure 659760DEST_PATH_IMAGE061
在
Figure 673853DEST_PATH_IMAGE062
参考坐标系下的位姿；
Figure 615264DEST_PATH_IMAGE063
表示
Figure 56610DEST_PATH_IMAGE064
时刻
Figure 450682DEST_PATH_IMAGE065
自主定位观测矩阵的逆矩阵；
Figure 271395DEST_PATH_IMAGE066
表示
Figure 801733DEST_PATH_IMAGE067
时刻
Figure 679559DEST_PATH_IMAGE068
相对于
Figure 826507DEST_PATH_IMAGE069
参考坐标系位姿矩阵的估计矩阵对应的逆矩阵，
Figure 447981DEST_PATH_IMAGE070
表示
Figure 363985DEST_PATH_IMAGE071
时刻
Figure 22499DEST_PATH_IMAGE072
相对于
Figure 781376DEST_PATH_IMAGE073
参考坐标系位姿矩阵的估计矩阵；
S22、构建无人机的相对定位因子；
无人机
Figure 81908DEST_PATH_IMAGE074
的特征点在无人机
Figure 852418DEST_PATH_IMAGE075
的参考坐标系下的位置：
Figure 806467DEST_PATH_IMAGE076
无人机
Figure 662428DEST_PATH_IMAGE077
的线特征点集到无人机
Figure 422442DEST_PATH_IMAGE078
特征地图的匹配残差：
Figure 906513DEST_PATH_IMAGE079
无人机
Figure 437989DEST_PATH_IMAGE080
的面特征点集到无人机
Figure 515666DEST_PATH_IMAGE081
特征地图的匹配残差：
Figure 285563DEST_PATH_IMAGE082
无人机
Figure 30666DEST_PATH_IMAGE083
相对于无人机
Figure 592097DEST_PATH_IMAGE084
参考坐标系的相对定位因子：
Figure 157070DEST_PATH_IMAGE085
式中，
Figure 196571DEST_PATH_IMAGE086
为
Figure 530600DEST_PATH_IMAGE087
在
Figure 262933DEST_PATH_IMAGE088
的参考坐标系下的位姿初值，
Figure 580782DEST_PATH_IMAGE089
为
Figure 423973DEST_PATH_IMAGE090
的特征点；
Figure 143667DEST_PATH_IMAGE091
与
Figure 187846DEST_PATH_IMAGE092
为
Figure 852046DEST_PATH_IMAGE093
特征地图
Figure 374294DEST_PATH_IMAGE094
中距离
Figure 948495DEST_PATH_IMAGE095
的特征点的投影最近且不在同一条扫描线上的任意两个点；
Figure 22630DEST_PATH_IMAGE096
、
Figure 49492DEST_PATH_IMAGE097
、
Figure 500065DEST_PATH_IMAGE098
为
Figure 397614DEST_PATH_IMAGE099
特征地图中距离
Figure 657299DEST_PATH_IMAGE100
的特征点的投影最近的任意三个点；
Figure 764932DEST_PATH_IMAGE101
为
Figure 222458DEST_PATH_IMAGE102
线特征点集
Figure 505672DEST_PATH_IMAGE103
中特征点的数量，
Figure 187189DEST_PATH_IMAGE104
为
Figure 923064DEST_PATH_IMAGE105
面特征点集
Figure 715439DEST_PATH_IMAGE106
中特征点的数量；
S23、将自主定位因子和相对定位因子融合，然后通过高斯噪声假设将多无人机分布式相对位姿图优化问题转化为最小二乘问题；
Figure 853160DEST_PATH_IMAGE107
式中，
Figure 439999DEST_PATH_IMAGE108
为
Figure 663170DEST_PATH_IMAGE109
相对于
Figure 134602DEST_PATH_IMAGE110
参考坐标系的位姿，
Figure 517042DEST_PATH_IMAGE111
为
Figure 415728DEST_PATH_IMAGE112
相对于
Figure 126195DEST_PATH_IMAGE113
参考坐标系的位姿的估计值。
进一步的，多无人机协同建图中，全局特征点云地图
Figure 525952DEST_PATH_IMAGE114
包括线特征地图
Figure 903844DEST_PATH_IMAGE115
与面特征地图
Figure 707852DEST_PATH_IMAGE116
，其表示如下：
Figure 33179DEST_PATH_IMAGE117
式中，
Figure 846414DEST_PATH_IMAGE118
分别为
Figure 937867DEST_PATH_IMAGE119
时刻无人机
Figure 912776DEST_PATH_IMAGE120
在自身参考坐标系中的位姿，
Figure 863414DEST_PATH_IMAGE121
分别为无人机
Figure 604974DEST_PATH_IMAGE122
在
Figure 426300DEST_PATH_IMAGE123
时刻提取的线特征，
Figure 103269DEST_PATH_IMAGE124
分别为无人机
Figure 134679DEST_PATH_IMAGE125
在
Figure 289716DEST_PATH_IMAGE126
时刻提取的面特征，
Figure 496707DEST_PATH_IMAGE127
分别为
Figure 938053DEST_PATH_IMAGE128
时刻无人机
Figure 597704DEST_PATH_IMAGE129
在无人机
Figure 556433DEST_PATH_IMAGE130
参考坐标系中的位姿，
Figure 476984DEST_PATH_IMAGE131
分别为无人机
Figure 230177DEST_PATH_IMAGE132
在
Figure 501758DEST_PATH_IMAGE133
时刻提取的线特征，
Figure 998598DEST_PATH_IMAGE134
分别为无人机
Figure 649023DEST_PATH_IMAGE135
在
Figure 963329DEST_PATH_IMAGE136
时刻提取的面特征。
综上，本发明采用上述多无人机分布式协同定位与建图方法，具备如下优势：
1、本发明提出了无人机自主定位方法，该方法仅依赖机载激光雷达与无人机机载计算机，无需外部传感器或高性能地面处理器，可用于支撑无人机在卫星导航拒止环境下的自主飞行与相关应用。
2、本发明提出了基于特征-地图匹配的多无人机分布式协同定位方法，该方法利用激光雷达点云特征与地图的匹配关系实现了对无人机自身位姿与多无人机系统内部其他无人机的相对位姿的求解，可用于支撑多无人机在卫星导航拒止环境下的协同飞行。
3、本发明提出了多无人机协同建图方法，该方法将每架无人机自身提取的特征点云与其他无人机发布的特征点云投影到无人机自身的参考坐标系，使多无人机系统中的每架无人机均可以维护一张全局一致的特征地图。
附图说明
图1为本发明实施例中卫星拒止环境下多无人机协同定位方法总体结构图；
图2为本发明实施例中分布式相对位姿图优化方法示意图；
图3为本发明实施例中卫星拒止环境下无人机自主定位对比图；
图4为本发明实施例中室内运动捕捉系统环境下协同定位对比图；
图5为本发明实施例中丛林环境下三架无人机协同定位轨迹与建图效果；
图6为本发明实施例中楼宇环境下三架无人机协同建图效果。
具体实施方式
以下结合附图和实施例对本发明的技术方案作进一步说明。
本方案所选用的无人机均搭载有三维激光雷达和机载处理器。三维激光雷达用于采集无人机周围环境的环境点云信息，测量精度高，覆盖范围大，能够将环境信息以三维点云数据形式发布出来。机载处理器用于处理三维激光雷达点云数据与卫星拒止环境下多无人机协同定位方法运行，获得无人机自身位姿及系统内其他无人机位姿。
如图1所示，本方案所设计的卫星拒止环境下多无人机协同定位算法主要包含三部分，卫星拒止环境下无人机自主定位、卫星拒止环境下多无人机相对定位以及卫星拒止环境下多无人机协同建图。
一、卫星拒止环境下无人机自主定位。
基于雷达特征-地图匹配的无人机自主定位方法利用激光雷达提供的原始点云数据估计多无人机系统中的每架无人机自身的定位信息。
S11、点云特征提取。从激光雷达点云中提取两种特征点，包括位于平坦区域的面特征点和位于边缘或尖锐区域的线特征点。为提取这两种特征点，对点云中每个点
Figure 597573DEST_PATH_IMAGE137
定义平滑度
Figure 632525DEST_PATH_IMAGE138
，其计算公式如下：
Figure 530598DEST_PATH_IMAGE139
（1）
式中，
Figure 625593DEST_PATH_IMAGE140
为激光雷达点云中的点，
Figure 606188DEST_PATH_IMAGE141
代表
Figure 444831DEST_PATH_IMAGE142
的邻域点集，
Figure 69847DEST_PATH_IMAGE143
表示
Figure 725956DEST_PATH_IMAGE144
的邻域点集中点的数量，
Figure 803634DEST_PATH_IMAGE145
表示向量的范数，
Figure 711547DEST_PATH_IMAGE146
表示
Figure 581283DEST_PATH_IMAGE147
的邻域点集中包含的点。
根据平滑度
Figure 18080DEST_PATH_IMAGE148
的定义，如果点
Figure 848633DEST_PATH_IMAGE149
分布在平面上，
Figure 153713DEST_PATH_IMAGE150
的值接近0；若点
Figure 753321DEST_PATH_IMAGE151
分布在边缘上，
Figure 626599DEST_PATH_IMAGE152
的值很大。先计算每个点云的平滑度值，排序后取最大和最小的数十个点，即为特征点。在特征提取时为了避免特征点分布过于集中，规定一个特征点周围10个邻近点不能作为特征点，最终特征提取环节获得了线特征点集
Figure 803503DEST_PATH_IMAGE153
与面特征点集
Figure 522060DEST_PATH_IMAGE154
。因此，特征点集可以定义为
Figure 366388DEST_PATH_IMAGE155
，其中
Figure 410567DEST_PATH_IMAGE156
表示两个集合的并集。
S12、雷达特征-地图匹配残差函数构建。特征-地图匹配通过特征点云
Figure 950133DEST_PATH_IMAGE157
与特征地图
Figure 862594DEST_PATH_IMAGE158
的匹配关系构建雷达约束残差。
对于特征点云
Figure 436795DEST_PATH_IMAGE159
中的每个特征点
Figure 386297DEST_PATH_IMAGE160
，通过无人机位姿初值
Figure 275143DEST_PATH_IMAGE161
（可直接转化为无人机在自身坐标系中的位姿初值）将
Figure 866661DEST_PATH_IMAGE162
投影至无人机自身坐标系，其投影公式如下：
Figure 154423DEST_PATH_IMAGE163
（2）
式中，
Figure 540405DEST_PATH_IMAGE164
表示特征点
Figure 320142DEST_PATH_IMAGE165
在世界坐标系下的位置。
对于线特征点集
Figure 308827DEST_PATH_IMAGE166
中的每个点
Figure 592040DEST_PATH_IMAGE167
，找到特征地图
Figure 414503DEST_PATH_IMAGE168
中距离
Figure 275012DEST_PATH_IMAGE169
在参考坐标系下的投影
Figure 942753DEST_PATH_IMAGE170
最近且不在同一条扫描线上的
Figure 80474DEST_PATH_IMAGE171
个点（
Figure 198471DEST_PATH_IMAGE172
）。假设
Figure 156063DEST_PATH_IMAGE173
与
Figure 893075DEST_PATH_IMAGE174
为特征地图
Figure 9935DEST_PATH_IMAGE175
中距离
Figure 908621DEST_PATH_IMAGE176
最近且不在同一条扫描线上的任意2个点。
Figure 884667DEST_PATH_IMAGE177
与
Figure 18846DEST_PATH_IMAGE178
、
Figure 131158DEST_PATH_IMAGE179
连成的线构成线特征对应关系，定义线特征点集
Figure 325379DEST_PATH_IMAGE180
中的第
Figure 523142DEST_PATH_IMAGE181
个点
Figure 601957DEST_PATH_IMAGE182
到地图的匹配残差
Figure 684620DEST_PATH_IMAGE183
为：
Figure 659530DEST_PATH_IMAGE184
（3）
式中，分子为向量
Figure 610168DEST_PATH_IMAGE185
与向量
Figure 86149DEST_PATH_IMAGE186
叉乘的模，其数值等于以两向量为边的平行四边形的面积。分母是向量
Figure 173053DEST_PATH_IMAGE187
的模，其数值等于三角形底边的长。因此，
Figure 443498DEST_PATH_IMAGE188
数值上等于特征点
Figure 615853DEST_PATH_IMAGE189
到
Figure 36470DEST_PATH_IMAGE190
、
Figure 836936DEST_PATH_IMAGE191
连成的线的垂直距离。
对于平面特征点集
Figure 684806DEST_PATH_IMAGE192
中的每个点
Figure 78878DEST_PATH_IMAGE193
，找到特征地图
Figure 162241DEST_PATH_IMAGE194
中距离
Figure 958159DEST_PATH_IMAGE195
在参考坐标系下的投影
Figure 835985DEST_PATH_IMAGE196
最近的
Figure 451774DEST_PATH_IMAGE197
个点
Figure 73248DEST_PATH_IMAGE198
，以张成一个平面。假设
Figure 520410DEST_PATH_IMAGE199
、
Figure 306488DEST_PATH_IMAGE200
、
Figure 940732DEST_PATH_IMAGE201
为特征地图中距离
Figure 100317DEST_PATH_IMAGE202
最近的任意3个点。
Figure 870827DEST_PATH_IMAGE203
与
Figure 231402DEST_PATH_IMAGE204
、
Figure 946417DEST_PATH_IMAGE205
、
Figure 50639DEST_PATH_IMAGE206
张成的平面构成平面特征对应关系，定义平面特征点集
Figure 800289DEST_PATH_IMAGE207
中的第
Figure 66185DEST_PATH_IMAGE208
个点
Figure 409442DEST_PATH_IMAGE209
到地图的匹配残差
Figure 910830DEST_PATH_IMAGE210
为：
Figure 655933DEST_PATH_IMAGE211
（4）
式中，分子为以向量
Figure 217364DEST_PATH_IMAGE212
与向量
Figure 47917DEST_PATH_IMAGE213
为边的平行四边形沿向量
Figure 228362DEST_PATH_IMAGE214
拉伸所构成的三维物体体积。分母是以向量
Figure 218184DEST_PATH_IMAGE215
与向量
Figure 825883DEST_PATH_IMAGE216
为边的平行四边形的面积。因此，
Figure 737207DEST_PATH_IMAGE217
在数值上等于特征点
Figure 986923DEST_PATH_IMAGE218
到
Figure 441038DEST_PATH_IMAGE219
、
Figure 347201DEST_PATH_IMAGE220
、
Figure 886767DEST_PATH_IMAGE221
张成的平面的垂直距离。
S13、非线性优化求解：根据线特征点集
Figure 533649DEST_PATH_IMAGE222
中的第
Figure 107850DEST_PATH_IMAGE223
个点
Figure 57351DEST_PATH_IMAGE224
到地图的匹配残差
Figure 474426DEST_PATH_IMAGE225
与平面特征点集
Figure 534786DEST_PATH_IMAGE226
中的第
Figure 229073DEST_PATH_IMAGE227
个点
Figure 474109DEST_PATH_IMAGE228
到地图的匹配残差
Figure 722688DEST_PATH_IMAGE229
的定义，将基于雷达特征-地图匹配的无人机自主定位问题转化为非线性最小二乘问题。
Figure 976952DEST_PATH_IMAGE230
（5）
式中，
Figure 525745DEST_PATH_IMAGE231
表示无人机在自身参考坐标系中的位姿，
Figure 817049DEST_PATH_IMAGE232
表示通过无人机在自身参考坐标系中位姿的估计值，
Figure 677558DEST_PATH_IMAGE233
表示线特征点集
Figure 345299DEST_PATH_IMAGE234
中线特征的数量，
Figure 873233DEST_PATH_IMAGE235
表示面特征点集
Figure 335438DEST_PATH_IMAGE236
中面特征的数量。
通过高斯-牛顿法对公式（5）中定义的非线性最小二乘问题的迭代求解，即可实现多无人机系统中每架无人机对自身位姿
Figure 683243DEST_PATH_IMAGE237
的估计，如图3所示。
二、卫星拒止环境下多无人机相对定位。
多无人机相对定位方法利用多无人机系统中其他无人机发布的自主定位信息与点云特征信息构建因子图，从而计算其他无人机在每架无人机参考坐标系中的位姿。因子图中包括2种因子：1）无人机自主定位因子；2）无人机相对定位因子，如图2所示。
下面以2架无人机
Figure 420254DEST_PATH_IMAGE238
与
Figure 412481DEST_PATH_IMAGE239
组成的多无人机系统为例，介绍卫星拒止环境下
Figure 32206DEST_PATH_IMAGE240
在
Figure 867307DEST_PATH_IMAGE241
的参考坐标系中的位姿
Figure 876851DEST_PATH_IMAGE242
的解算方法。
S21、无人机自主定位因子构建。无人机自主定位因子用于描述因子图中两相邻节点间的位姿关系。对于一个多无人机系统，每架无人机可以估计自身的位姿并将其发布至其他无人机。定义无人机自主定位观测如下：
Figure 989164DEST_PATH_IMAGE243
（6）
式中，
Figure 448964DEST_PATH_IMAGE244
表示
Figure 381148DEST_PATH_IMAGE245
时刻的无人机自主定位观测矩阵，
Figure 319017DEST_PATH_IMAGE246
表示
Figure 20256DEST_PATH_IMAGE247
时刻的无人机自主定位观测矩阵，
Figure 526324DEST_PATH_IMAGE248
表示
Figure 336017DEST_PATH_IMAGE249
至
Figure 687364DEST_PATH_IMAGE250
间无人机的位姿增量，
Figure 633323DEST_PATH_IMAGE251
表示无人机自主定位观测协方差。
根据位姿增量
Figure 310292DEST_PATH_IMAGE252
的坐标系转换不变性，对于
Figure 217068DEST_PATH_IMAGE253
相对于
Figure 511389DEST_PATH_IMAGE254
的位姿估计问题，位姿增量
Figure 452800DEST_PATH_IMAGE255
可以写为如下形式：
Figure 300670DEST_PATH_IMAGE256
（7）
式中，
Figure 819376DEST_PATH_IMAGE257
表示
Figure 778105DEST_PATH_IMAGE258
时刻
Figure 698656DEST_PATH_IMAGE259
在
Figure 451849DEST_PATH_IMAGE260
参考坐标系下的位姿的逆，
Figure 333217DEST_PATH_IMAGE261
表示
Figure 954691DEST_PATH_IMAGE262
时刻
Figure 870695DEST_PATH_IMAGE263
在
Figure 919422DEST_PATH_IMAGE264
参考坐标系下的位姿。
将公式（7）代入公式（6）中，可得：
Figure 288086DEST_PATH_IMAGE265
（8）
式中，
Figure 323039DEST_PATH_IMAGE266
表示
Figure 483761DEST_PATH_IMAGE267
至
Figure 578756DEST_PATH_IMAGE268
间
Figure 293772DEST_PATH_IMAGE269
的位姿观测增量，
Figure 397994DEST_PATH_IMAGE270
表示
Figure 288589DEST_PATH_IMAGE271
时刻
Figure 679119DEST_PATH_IMAGE272
自主定位观测矩阵的逆矩阵。
因此，定义
Figure 756797DEST_PATH_IMAGE273
相对于
Figure 399131DEST_PATH_IMAGE274
参考坐标系的自主定位因子
Figure 6217DEST_PATH_IMAGE275
为：
Figure 708594DEST_PATH_IMAGE276
（9）
式中，
Figure 398201DEST_PATH_IMAGE277
表示
Figure 844226DEST_PATH_IMAGE278
时刻
Figure 178255DEST_PATH_IMAGE279
相对于
Figure 176167DEST_PATH_IMAGE280
参考坐标系位姿矩阵的估计矩阵对应的逆矩阵，
Figure 228437DEST_PATH_IMAGE281
表示
Figure 337207DEST_PATH_IMAGE282
时刻
Figure 791322DEST_PATH_IMAGE283
相对于
Figure 569923DEST_PATH_IMAGE284
参考坐标系位姿矩阵的估计矩阵。
S22、无人机相对定位因子构建。
Figure 499701DEST_PATH_IMAGE285
利用
Figure 287529DEST_PATH_IMAGE286
发布的激光雷达点云特征
Figure 596150DEST_PATH_IMAGE287
在自身特征地图
Figure 935865DEST_PATH_IMAGE288
中做匹配。对于
Figure 962727DEST_PATH_IMAGE289
发布的激光雷达点云特征
Figure 288666DEST_PATH_IMAGE290
中的每个特征点
Figure 576428DEST_PATH_IMAGE291
，通过无人机位姿初值将
Figure 962410DEST_PATH_IMAGE292
投影至
Figure 601201DEST_PATH_IMAGE293
的参考坐标系，其投影公式如下：
Figure 465252DEST_PATH_IMAGE294
（10）
式中，
Figure 14045DEST_PATH_IMAGE295
为为
Figure 432913DEST_PATH_IMAGE296
在
Figure 434367DEST_PATH_IMAGE297
的参考坐标系下的位姿初值，
Figure 226742DEST_PATH_IMAGE298
表示特征点
Figure 630042DEST_PATH_IMAGE299
在
Figure 92247DEST_PATH_IMAGE300
的参考坐标系下的位置。
对于
Figure 440052DEST_PATH_IMAGE301
发布的线特征点集
Figure 911484DEST_PATH_IMAGE302
中的每个点
Figure 28345DEST_PATH_IMAGE303
，找到
Figure 192610DEST_PATH_IMAGE304
特征地图
Figure 903077DEST_PATH_IMAGE305
中距离
Figure 37255DEST_PATH_IMAGE306
在
Figure 415147DEST_PATH_IMAGE307
参考坐标系下的投影
Figure 219155DEST_PATH_IMAGE308
最近且不在同一条扫描线上的
Figure 541552DEST_PATH_IMAGE309
个点（
Figure 354787DEST_PATH_IMAGE310
）。假设
Figure 446240DEST_PATH_IMAGE311
与
Figure 686728DEST_PATH_IMAGE312
为特征地图
Figure 371788DEST_PATH_IMAGE313
中距离
Figure 847768DEST_PATH_IMAGE314
最近且不在同一条扫描线上的任意2个点。
Figure 934673DEST_PATH_IMAGE315
与
Figure 208047DEST_PATH_IMAGE316
、
Figure 380402DEST_PATH_IMAGE317
连成的线构成线特征对应关系，定义
Figure 394495DEST_PATH_IMAGE318
发布的线特征点集
Figure 601485DEST_PATH_IMAGE319
的第
Figure 183776DEST_PATH_IMAGE320
个点
Figure 968062DEST_PATH_IMAGE321
到地图的匹配残差
Figure 926790DEST_PATH_IMAGE322
为：
Figure 457129DEST_PATH_IMAGE323
（11）
式中，分子为向量
Figure 600534DEST_PATH_IMAGE324
与向量
Figure 481902DEST_PATH_IMAGE325
叉乘的模，其数值等于以两向量为边的平行四边形的面积。分母是向量
Figure 244322DEST_PATH_IMAGE326
的模，其数值等于三角形底边的长。因此，
Figure 19380DEST_PATH_IMAGE327
数值上等于特征点
Figure 943474DEST_PATH_IMAGE328
到
Figure 577717DEST_PATH_IMAGE329
、
Figure 737303DEST_PATH_IMAGE330
连成的线的垂直距离。
对于
Figure 773392DEST_PATH_IMAGE331
发布的面特征点集
Figure 727442DEST_PATH_IMAGE332
中的每个点
Figure 848982DEST_PATH_IMAGE333
，找到
Figure 953204DEST_PATH_IMAGE334
特征地图
Figure 578220DEST_PATH_IMAGE335
中距离
Figure 968750DEST_PATH_IMAGE336
在
Figure 312007DEST_PATH_IMAGE337
参考坐标系下的投影
Figure 219920DEST_PATH_IMAGE338
最近的
Figure 815288DEST_PATH_IMAGE339
个点
Figure 517665DEST_PATH_IMAGE340
，以张成一个平面。假设
Figure 348217DEST_PATH_IMAGE341
、
Figure 653297DEST_PATH_IMAGE342
、
Figure 252905DEST_PATH_IMAGE343
为特征地图中距离
Figure 126183DEST_PATH_IMAGE344
最近的任意3个点。
Figure 303087DEST_PATH_IMAGE345
与
Figure 287223DEST_PATH_IMAGE346
、
Figure 865972DEST_PATH_IMAGE347
、
Figure 910152DEST_PATH_IMAGE348
张成的平面构成平面特征对应关系，定义平面特征点集
Figure 715297DEST_PATH_IMAGE349
中的第
Figure 503124DEST_PATH_IMAGE350
个点
Figure 936379DEST_PATH_IMAGE351
到地图的匹配残差
Figure 151460DEST_PATH_IMAGE352
为：
Figure 178322DEST_PATH_IMAGE353
（12）
式中，分子为以向量
Figure 628895DEST_PATH_IMAGE354
与向量
Figure 57602DEST_PATH_IMAGE355
为边的平行四边形沿向量
Figure 443584DEST_PATH_IMAGE356
拉伸所构成的三维物体体积。分母是以向量
Figure 82376DEST_PATH_IMAGE357
与向量
Figure 212006DEST_PATH_IMAGE358
为边的平行四边形的面积。因此，
Figure 495220DEST_PATH_IMAGE359
在数值上等于特征点
Figure 176737DEST_PATH_IMAGE360
在
Figure 178191DEST_PATH_IMAGE361
参考坐标系下的投影
Figure 111512DEST_PATH_IMAGE362
到
Figure 111216DEST_PATH_IMAGE363
、
Figure 104580DEST_PATH_IMAGE364
、
Figure 452385DEST_PATH_IMAGE365
张成的平面的垂直距离。
据
Figure 923817DEST_PATH_IMAGE366
发布的线特征点集
Figure 181623DEST_PATH_IMAGE367
中的第
Figure 345888DEST_PATH_IMAGE368
个点
Figure 915410DEST_PATH_IMAGE369
到
Figure 190534DEST_PATH_IMAGE370
特征地图的匹配残差
Figure 427480DEST_PATH_IMAGE371
与
Figure 497067DEST_PATH_IMAGE372
发布的平面特征点集
Figure 960409DEST_PATH_IMAGE373
中的第
Figure 632699DEST_PATH_IMAGE374
个点
Figure 865097DEST_PATH_IMAGE375
到
Figure 105586DEST_PATH_IMAGE120
特征地图的匹配残差
Figure 915279DEST_PATH_IMAGE376
的定义，将
Figure 532205DEST_PATH_IMAGE377
相对于
Figure 619110DEST_PATH_IMAGE378
参考坐标系的相对定位因子
Figure 155133DEST_PATH_IMAGE379
定义为：
Figure 327489DEST_PATH_IMAGE380
（13）
式中，
Figure 216947DEST_PATH_IMAGE381
表示
Figure 548572DEST_PATH_IMAGE283
发布的线特征点集
Figure 130863DEST_PATH_IMAGE382
中线特征的数量，
Figure 790514DEST_PATH_IMAGE383
表示
Figure 611227DEST_PATH_IMAGE384
发布的面特征点集
Figure 672724DEST_PATH_IMAGE385
中面特征的数量。
S23、分布式相对位姿图优化方法。分布式相对位姿图优化方法对无人机自主定位信息与多无人机相对定位信息进行融合，实现精确、鲁棒、实时的分布式多无人机协同定位。
本方案采用增量式位姿图优化求解器分布式求解多无人机位姿，以求解
Figure 691496DEST_PATH_IMAGE386
相对于
Figure 697498DEST_PATH_IMAGE387
参考坐标系的位姿为例，通过高斯噪声假设将多无人机分布式相对位姿图优化问题转化为最小二乘问题：
Figure 194338DEST_PATH_IMAGE388
（14）
通过开源求解器GTSAM (Georgia Tech Smoothing and Mapping)求解公式（14）中定义的相对定位位姿图优化问题即可实现对
Figure 110341DEST_PATH_IMAGE389
相对于
Figure 300014DEST_PATH_IMAGE390
参考坐标系的位姿
Figure 793313DEST_PATH_IMAGE391
的求解，如图4所示。
三、卫星拒止环境下多无人机协同建图方法。
多无人机系统中的每架无人机都需要维护在其自身参考坐标系下维护一张特征点云地图
Figure 93844DEST_PATH_IMAGE392
。特征点云地图构建就是利用激光雷达扫描的特征数据构建地图的过程，激光雷达特征与特征点云地图是通过位姿关系进行关联的。
以
Figure 864354DEST_PATH_IMAGE393
和
Figure 83983DEST_PATH_IMAGE394
两架无人机组成的多无人机系统为例，
Figure 205522DEST_PATH_IMAGE395
时刻
Figure 44165DEST_PATH_IMAGE396
维护的特征地图
Figure 59395DEST_PATH_IMAGE397
是
Figure 325291DEST_PATH_IMAGE398
个单帧特征点云在世界坐标系下投影的集合，其包含两个子集，
Figure 668548DEST_PATH_IMAGE399
时刻
Figure 435515DEST_PATH_IMAGE400
维护的线特征地图
Figure 180618DEST_PATH_IMAGE401
与面特征地图
Figure 742049DEST_PATH_IMAGE402
，公式如下：
Figure 572602DEST_PATH_IMAGE403
（15）
式中，
Figure 753047DEST_PATH_IMAGE404
分别为
Figure 618235DEST_PATH_IMAGE405
时刻无人机
Figure 350568DEST_PATH_IMAGE406
在自身参考坐标系中的位姿，
Figure 668417DEST_PATH_IMAGE407
分别为无人机
Figure 652553DEST_PATH_IMAGE408
在
Figure 234232DEST_PATH_IMAGE123
时刻提取的线特征，
Figure 278411DEST_PATH_IMAGE409
分别为无人机
Figure 83556DEST_PATH_IMAGE410
在
Figure 730438DEST_PATH_IMAGE411
时刻提取的面特征，
Figure 304639DEST_PATH_IMAGE412
分别为
Figure 519719DEST_PATH_IMAGE413
时刻无人机
Figure 405636DEST_PATH_IMAGE414
在无人机
Figure 997154DEST_PATH_IMAGE415
参考坐标系中的位姿，
Figure 284916DEST_PATH_IMAGE416
分别为无人机
Figure 670898DEST_PATH_IMAGE417
在
Figure 840848DEST_PATH_IMAGE418
时刻提取的线特征，
Figure 970478DEST_PATH_IMAGE419
分别为无人机
Figure 112747DEST_PATH_IMAGE420
在
Figure 935209DEST_PATH_IMAGE421
时刻提取的面特征。
根据公式(15)中特征点云地图的定义可以，本发明提出的卫星拒止环境下多无人机协同定位与建图方法可以在多无人机系统中包含的每架无人机上维护一张全局一致的特征地图，如图5、图6所示。
以上是本发明的具体实施方式，但本发明的保护范围不应局限于此。任何熟悉本领域的技术人员在本发明所揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内，因此本发明的保护范围应以权利要求书所限定的保护范围为准。

Claims (4)

1.卫星拒止环境下多无人机分布式协同定位与建图方法，其特征在于，包括：
无人机自主定位，每架无人机利用激光雷达提供的原始点云数据提取自身的点云特征信息，然后估计自身的自主定位信息；
多无人机协同定位，每架无人机利用其他无人机发布的点云特征信息在自身的特征地图上做匹配，从而计算其他无人机在自身参考坐标系中的相对位姿信息；
多无人机协同建图，每架无人机在自身参考坐标系下，利用自主定位信息、相对位姿信息、点云特征信息维护一张全局特征点云地图；
其中，无人机自主定位的具体步骤包括：
S11、从激光雷达点云中提取特征点集
Figure DEST_PATH_IMAGE001
，包括位于平坦区域的面特征点集
Figure 917693DEST_PATH_IMAGE002
和位于边缘或尖锐区域的线特征点集
Figure DEST_PATH_IMAGE003
；
S12、通过特征点集
Figure 974380DEST_PATH_IMAGE001
与特征地图
Figure 348860DEST_PATH_IMAGE004
的匹配关系构建雷达约束残差；
特征点在无人机自身参考坐标系下的位置：
Figure DEST_PATH_IMAGE005
线特征点集的匹配残差：
Figure 496857DEST_PATH_IMAGE006
面特征点集的匹配残差：
Figure DEST_PATH_IMAGE007
式中，
Figure 988887DEST_PATH_IMAGE008
为特征点云
Figure DEST_PATH_IMAGE009
中的特征点，
Figure 431239DEST_PATH_IMAGE010
为无人机在自身参考坐标系下的位姿初值；
Figure DEST_PATH_IMAGE011
为线特征点集中的特征点，
Figure 350522DEST_PATH_IMAGE012
与
Figure DEST_PATH_IMAGE013
为无人机自身特征地图
Figure 746999DEST_PATH_IMAGE014
中距离
Figure DEST_PATH_IMAGE015
最近且不在同一条扫描线上的任意两个点；
Figure 60298DEST_PATH_IMAGE016
为面特征点集中的特征点，
Figure DEST_PATH_IMAGE017
、
Figure 91577DEST_PATH_IMAGE018
、
Figure DEST_PATH_IMAGE019
为无人机自身特征地图
Figure 181762DEST_PATH_IMAGE020
中距离
Figure DEST_PATH_IMAGE021
最近的任意三个点；
S13、根据特征点集的匹配残差，将基于雷达特征-地图匹配的无人机自主定位问题转化为非线性最小二乘问题；通过高斯-牛顿法对非线性最小二乘问题的迭代求解，即可实现多无人机系统中每架无人机对自身位姿
Figure 298491DEST_PATH_IMAGE022
的估计；
Figure DEST_PATH_IMAGE023
式中，
Figure 899368DEST_PATH_IMAGE024
为无人机在自身参考坐标系中的位姿，
Figure DEST_PATH_IMAGE025
为通过无人机在自身参考坐标系中位姿的估计值，
Figure 56592DEST_PATH_IMAGE026
为线特征点集
Figure DEST_PATH_IMAGE027
中特征点的数量，
Figure 583257DEST_PATH_IMAGE028
为面特征点集
Figure DEST_PATH_IMAGE029
中特征点的数量。
2.根据权利要求1所述的卫星拒止环境下多无人机分布式协同定位与建图方法，其特征在于，S11步骤中依据平滑度确定特征点集
Figure 921703DEST_PATH_IMAGE030
，具体地，
平滑度：
Figure DEST_PATH_IMAGE031
式中，
Figure 90385DEST_PATH_IMAGE032
为激光雷达点云中的点，
Figure DEST_PATH_IMAGE033
为
Figure 66563DEST_PATH_IMAGE034
的邻域点集，
Figure DEST_PATH_IMAGE035
为
Figure 758270DEST_PATH_IMAGE036
的邻域点集中点的数量，
Figure DEST_PATH_IMAGE037
为向量的范数，
Figure 849592DEST_PATH_IMAGE038
为
Figure DEST_PATH_IMAGE039
的邻域点集中包含的点；
依据平滑度值对点云排序，最大的数十个点即为线特征点集
Figure 290806DEST_PATH_IMAGE040
，最小的数十个点即为面特征点集
Figure DEST_PATH_IMAGE041
，且规定一个特征点周围10个邻近点不能作为特征点。
3.根据权利要求2所述的卫星拒止环境下多无人机分布式协同定位与建图方法，其特征在于，多无人机协同定位的具体步骤包括：
S21、构建无人机的自主定位因子；
无人机自主定位观测：
Figure 885604DEST_PATH_IMAGE042
位姿增量：
Figure DEST_PATH_IMAGE043
位姿观测增量：
Figure 521116DEST_PATH_IMAGE044
自主定位因子：
Figure DEST_PATH_IMAGE045
式中，
Figure 840014DEST_PATH_IMAGE046
表示
Figure DEST_PATH_IMAGE047
时刻的无人机自主定位观测矩阵，
Figure 84919DEST_PATH_IMAGE048
表示
Figure DEST_PATH_IMAGE049
时刻的无人机自主定位观测矩阵，
Figure 799803DEST_PATH_IMAGE050
表示
Figure DEST_PATH_IMAGE051
至
Figure 370331DEST_PATH_IMAGE052
间无人机的位姿增量，
Figure DEST_PATH_IMAGE053
表示无人机自主定位观测协方差；
Figure 406551DEST_PATH_IMAGE054
表示
Figure DEST_PATH_IMAGE055
时刻
Figure 285226DEST_PATH_IMAGE056
在
Figure DEST_PATH_IMAGE057
参考坐标系下的位姿的逆，
Figure 120195DEST_PATH_IMAGE058
表示
Figure DEST_PATH_IMAGE059
时刻
Figure 64886DEST_PATH_IMAGE060
在
Figure DEST_PATH_IMAGE061
参考坐标系下的位姿；
Figure 86938DEST_PATH_IMAGE062
表示
Figure DEST_PATH_IMAGE063
时刻
Figure 679505DEST_PATH_IMAGE060
自主定位观测矩阵的逆矩阵；
Figure 463922DEST_PATH_IMAGE064
表示
Figure 205613DEST_PATH_IMAGE047
时刻
Figure 574015DEST_PATH_IMAGE060
相对于
Figure 59354DEST_PATH_IMAGE061
参考坐标系位姿矩阵的估计矩阵对应的逆矩阵，
Figure DEST_PATH_IMAGE065
表示
Figure 603337DEST_PATH_IMAGE066
时刻
Figure 515929DEST_PATH_IMAGE060
相对于
Figure 371627DEST_PATH_IMAGE061
参考坐标系位姿矩阵的估计矩阵；
S22、构建无人机的相对定位因子；
无人机
Figure 395078DEST_PATH_IMAGE060
的特征点在无人机
Figure 381183DEST_PATH_IMAGE061
的参考坐标系下的位置：
Figure DEST_PATH_IMAGE067
无人机
Figure 838578DEST_PATH_IMAGE060
的线特征点集到无人机
Figure 417458DEST_PATH_IMAGE061
特征地图的匹配残差：
Figure 244600DEST_PATH_IMAGE068
无人机
Figure 91071DEST_PATH_IMAGE060
的面特征点集到无人机
Figure 345466DEST_PATH_IMAGE061
特征地图的匹配残差：
Figure DEST_PATH_IMAGE069
无人机
Figure 316702DEST_PATH_IMAGE060
相对于无人机
Figure 947535DEST_PATH_IMAGE061
参考坐标系的相对定位因子：
Figure 185530DEST_PATH_IMAGE070
式中，
Figure DEST_PATH_IMAGE071
为
Figure 515886DEST_PATH_IMAGE060
在
Figure 803779DEST_PATH_IMAGE061
的参考坐标系下的位姿初值，
Figure 238302DEST_PATH_IMAGE072
为
Figure 324945DEST_PATH_IMAGE060
的特征点；
Figure DEST_PATH_IMAGE073
与
Figure 826202DEST_PATH_IMAGE074
为
Figure 132550DEST_PATH_IMAGE061
特征地图
Figure DEST_PATH_IMAGE075
中距离
Figure 762244DEST_PATH_IMAGE060
的特征点的投影最近且不在同一条扫描线上的任意两个点；
Figure 939278DEST_PATH_IMAGE076
、
Figure DEST_PATH_IMAGE077
、
Figure 611437DEST_PATH_IMAGE078
为
Figure 139501DEST_PATH_IMAGE061
特征地图中距离
Figure 148783DEST_PATH_IMAGE060
的特征点的投影最近的任意三个点；
Figure DEST_PATH_IMAGE079
为
Figure 616543DEST_PATH_IMAGE060
线特征点集
Figure 554543DEST_PATH_IMAGE080
中特征点的数量，
Figure DEST_PATH_IMAGE081
为
Figure 215243DEST_PATH_IMAGE060
面特征点集
Figure 529681DEST_PATH_IMAGE082
中特征点的数量；
S23、将自主定位因子和相对定位因子融合，然后通过高斯噪声假设将多无人机分布式相对位姿图优化问题转化为最小二乘问题；
Figure DEST_PATH_IMAGE083
式中，
Figure 586368DEST_PATH_IMAGE084
为
Figure 960848DEST_PATH_IMAGE060
相对于
Figure 696461DEST_PATH_IMAGE061
参考坐标系的位姿，
Figure DEST_PATH_IMAGE085
为
Figure 719650DEST_PATH_IMAGE060
相对于
Figure 991362DEST_PATH_IMAGE061
参考坐标系的位姿的估计值。
4.根据权利要求3所述的卫星拒止环境下多无人机分布式协同定位与建图方法，其特征在于，多无人机协同建图中，全局特征点云地图
Figure 536744DEST_PATH_IMAGE086
包括线特征地图
Figure DEST_PATH_IMAGE087
与面特征地图
Figure 894739DEST_PATH_IMAGE088
，其表示如下：
Figure DEST_PATH_IMAGE089
式中，
Figure 456039DEST_PATH_IMAGE090
分别为
Figure DEST_PATH_IMAGE091
时刻无人机
Figure 487318DEST_PATH_IMAGE061
在自身参考坐标系中的位姿，
Figure 938022DEST_PATH_IMAGE092
分别为无人机
Figure 648227DEST_PATH_IMAGE061
在
Figure DEST_PATH_IMAGE093
时刻提取的线特征，
Figure 753498DEST_PATH_IMAGE094
分别为无人机
Figure 999803DEST_PATH_IMAGE061
在
Figure DEST_PATH_IMAGE095
时刻提取的面特征，
Figure 526468DEST_PATH_IMAGE096
分别为
Figure DEST_PATH_IMAGE097
时刻无人机
Figure 897537DEST_PATH_IMAGE060
在无人机
Figure 128536DEST_PATH_IMAGE061
参考坐标系中的位姿，
Figure 494927DEST_PATH_IMAGE098
分别为无人机
Figure 989231DEST_PATH_IMAGE060
在
Figure DEST_PATH_IMAGE099
时刻提取的线特征，
Figure 770234DEST_PATH_IMAGE100
分别为无人机
Figure 306389DEST_PATH_IMAGE060
在
Figure DEST_PATH_IMAGE101
时刻提取的面特征。
CN202211081054.6A 2022-09-06 2022-09-06 卫星拒止环境下多无人机分布式协同定位与建图方法 Active CN115166686B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211081054.6A CN115166686B (zh) 2022-09-06 2022-09-06 卫星拒止环境下多无人机分布式协同定位与建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211081054.6A CN115166686B (zh) 2022-09-06 2022-09-06 卫星拒止环境下多无人机分布式协同定位与建图方法

Publications (2)

Publication Number Publication Date
CN115166686A CN115166686A (zh) 2022-10-11
CN115166686B true CN115166686B (zh) 2022-11-11

Family

ID=83481392

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211081054.6A Active CN115166686B (zh) 2022-09-06 2022-09-06 卫星拒止环境下多无人机分布式协同定位与建图方法

Country Status (1)

Country Link
CN (1) CN115166686B (zh)

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106595659A (zh) * 2016-11-03 2017-04-26 南京航空航天大学 城市复杂环境下多无人机视觉slam的地图融合方法
CN112000130A (zh) * 2020-09-07 2020-11-27 哈尔滨工业大学 一种无人机的多机协同高精度建图定位系统

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11048277B1 (en) * 2018-01-24 2021-06-29 Skydio, Inc. Objective-based control of an autonomous unmanned aerial vehicle
CN109945856B (zh) * 2019-02-18 2021-07-06 天津大学 基于惯性/雷达的无人机自主定位与建图方法
CN111812978B (zh) * 2020-06-12 2023-01-24 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 一种多无人机协作slam方法与系统
CN112347840B (zh) * 2020-08-25 2022-12-02 天津大学 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN113985429A (zh) * 2021-09-23 2022-01-28 天津大学 基于三维激光雷达的无人机环境扫描与重构方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106595659A (zh) * 2016-11-03 2017-04-26 南京航空航天大学 城市复杂环境下多无人机视觉slam的地图融合方法
CN112000130A (zh) * 2020-09-07 2020-11-27 哈尔滨工业大学 一种无人机的多机协同高精度建图定位系统

Also Published As

Publication number Publication date
CN115166686A (zh) 2022-10-11

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN110446159B (zh) 一种室内无人机精确定位与自主导航的系统及方法
Li et al. Multi-sensor fusion for navigation and mapping in autonomous vehicles: Accurate localization in urban environments
Hebel et al. Change detection in urban areas by object-based analysis and on-the-fly comparison of multi-view ALS data
He et al. Non-cooperative spacecraft pose tracking based on point cloud feature
Sanfourche et al. Perception for UAV: Vision-Based Navigation and Environment Modeling.
Eynard et al. Real time UAV altitude, attitude and motion estimation from hybrid stereovision
Kim et al. UAV-UGV cooperative 3D environmental mapping
CN108535789A (zh) 一种基于机场跑道的异物识别系统
Deng et al. Global optical flow-based estimation of velocity for multicopters using monocular vision in GPS-denied environments
Wang et al. Correlation flow: robust optical flow using kernel cross-correlators
Warren et al. Towards visual teach and repeat for GPS-denied flight of a fixed-wing UAV
Ivanovas et al. Block matching based obstacle avoidance for unmanned aerial vehicle
CN110187337B (zh) 一种基于ls和neu-ecef时空配准的高机动目标跟踪方法及系统
Lippiello et al. Closed-form solution for absolute scale velocity estimation using visual and inertial data with a sliding least-squares estimation
Ivancsits et al. Visual navigation system for small unmanned aerial vehicles
Cheng Real-time surface slope estimation by homography alignment for spacecraft safe landing
Li et al. Metric sensing and control of a quadrotor using a homography-based visual inertial fusion method
Mansur et al. Real time monocular visual odometry using optical flow: study on navigation of quadrotors UAV
Han et al. Multiple targets geolocation using SIFT and stereo vision on airborne video sequences
Wang et al. A resource-efficient online target detection system with autonomous drone-assisted IoT
CN115166686B (zh) 卫星拒止环境下多无人机分布式协同定位与建图方法
Contreras et al. Efficient decentralized collaborative mapping for outdoor environments
Zheng et al. Dual LIDAR online calibration and mapping and perception system
Zhang et al. Hybrid iteration and optimization-based three-dimensional reconstruction for space non-cooperative targets with monocular vision and sparse lidar fusion

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