一种协同定位和建图方法和装置、存储介质

1.本发明属于地面无人系统技术领域，尤其涉及一种基于多传感器和多节点的协同定位和地图构建方法和装置、存储介质。


背景技术：

2.多传感器定位和地图构建技术已经广泛应用于数字城市应用、工程建筑测量、灾害监测与环境监测、油气勘探等领域，而随着生产力的不断发展，社会生产生活的方式的高效化，单个无人节点在探索较大区域时存在的传感器作用范围小、观测能力有限、计算复杂性高、存储性差的缺点。


技术实现要素：

3.本发明要解决的技术问题是，提供一种基于多传感器和多节点的协同定位和地图构建方法和装置、存储介质，以解决针对单个无人节点在探索较大区域时存在的传感器作用范围小、观测能力有限、计算复杂性高、存储性差问题。
4.为实现上述目的，本发明采用如下的技术方案：
5.一种协同定位和地图构建方法，包括以下步骤：
6.步骤s1、在初始时刻建立多个节点之间的无线通信网络，其中，每个节点位于初始位置；
7.步骤s2、多节点对初始位置的同一场景进行激光建图，确定全局坐标系；
8.步骤s3、多节点根据初始位置的点云图，向不同方向分散运动，每个节点根据全局坐标系独立地进行同时定位和建图，直到完成规定区域的定位建图任务；
9.步骤s4、在每个节点独立地进行同时定位和建图过程中通过所述无线网络进行通信时，交换各自节点历史的点云编码和优化后的坐标，以检测是否经过同一区域，并加入到位姿图中进行全局优化。
10.作为优选，步骤s2中，所述节点对初始位置的同一场景进行激光建图，得到初始位置的点云图；通过配准多节点的点云图，得到多节点间的初始位姿约束；根据初始位姿约束，确定全局坐标系。
11.作为优选，步骤s2中，交换各自节点得到的点云图，并通过配准的方式获得两两节点之间的相对位姿变换，即初始位姿约束；根据多节点之间的初始位姿约束，将多节点的坐标变换到第一个节点的坐标系下，第一个节点的坐标系即为全局坐标系。
12.作为优选，步骤s2中，对于非重复扫描式激光雷达，在静止状态下得到初始位置的点云图。
13.作为优选，步骤s2中，对于重复扫描式激光雷达，则在初始场景处进行平移或旋转式的往复运动，得到初始位置的点云图。
14.作为优选，所述节点包含激光雷达、组合导航和超宽带无线通信装置。
15.本发明还提供一种协同定位和地图构建装置，包括：
16.初始化模块，用于在初始时刻建立多个节点之间的无线通信网络，其中，每个节点位于初始位置；
17.全局坐标确定模块，用于通过多节点对初始位置的同一场景进行激光建图，确定全局坐标系；
18.单节点建图模块，用于通过每个节点根据全局坐标系独立地进行同时定位和建图，直到完成规定区域的定位建图任务；
19.协同模块，用于在每个节点独立地进行同时定位和建图过程中通过所述无线网络进行通信时，交换各自节点历史的点云编码和优化后的坐标，以检测是否经过同一区域，并加入到位姿图中进行全局优化
20.作为优选，所述全局坐标确定模块，包括：
21.激光建图单元，用于通过所述节点对初始位置的同一场景进行激光建图，得到初始位置的点云图；
22.配准单元，用于通过配准多节点的点云图，得到多节点间的初始位姿约束；
23.确定单元，用于根据初始位姿约束，确定全局坐标系。
24.作为优选，所述节点包含激光雷达、组合导航和超宽带无线通信装置。
25.本发明还提供一种存储介质，所述存储介质存储有机器可执行指令，所述机器可执行指令在被处理器调用和执行时，所述机器可执行指令促使所述处理器实现协同定位和地图构建方法。
26.本发明利用多传感器之间的信息互补和多节点之间的信息交互，实现多传感器和多节点的协同定位和地图构建。
附图说明
27.图1为本发明协同定位和地图构建方法的流程图；
28.图2为本发明协同定位和地图构建装置的结构示意图。
具体实施方式
29.为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。
30.实施例1：
31.如图1所示，本发明提供一种基于多传感器和多节点的协同定位和地图构建方法，包括以下步骤：
32.步骤s1、在初始时刻将多个节点放置在初始位置，建立多节点之间的无线通信网络，其中，节点包含激光雷达、组合导航、超宽带无线通信装置，节点可以为无人机、无人车无人等移动平台；
33.步骤s2、多节点对初始位置的同一场景进行激光建图，获得初始位置的点云图；通过配准多节点的点云图，获得多节点间的初始位姿约束；根据初始位姿约束，确定全局坐标系；
34.步骤s3、多节点根据初始位置的点云图，向不同方向分散运动，每个节点根据全局坐标系独立地进行同时定位和建图，直到完成规定区域的定位建图任务；
35.步骤s4、在每个节点独立地进行同时定位和建图过程中，当节点之间可以通过无线网络进行通信时，交换各自节点历史的点云编码和优化后的坐标，以检测是否经过同一区域，并加入到位姿图中进行全局优化。
36.作为本实施例的一种实施方式，步骤s2包括：
37.步骤2.1、对于非重复扫描式激光雷达，可以在静止状态下建立初始位置的点云图，如果视场角中包含其他节点的移动平台，可以使用立方体滤波将其从初始场景中删除；
38.步骤2.2、对于重复扫描式激光雷达，则需要在初始场景处进行平移或旋转式的往复运动，来获得较好的初始位置的点云图，同样，如果视场角中包含其他节点的移动平台，可以使用立方体滤波将其从初始场景中删除；
39.步骤2.3、当每个节点都完成初始位置点云图的建立后，交换各自建立的点云图，并通过配准的方式获得两两节点之间的相对位姿变换，即初始位姿约束；
40.步骤2.4、根据多节点之间的初始位姿约束，将多节点的坐标变换到第一个节点的坐标系下，第一个节点的坐标系即为全局坐标系。
41.作为本实施例的一种实施方式，步骤s3包括：
42.步骤3.1、各节点在初始位置点云图的基础上，向不同方向开始独立地进行同时定位和建图，并扩展点云图的范围；
43.步骤3.2、获取各传感器的数据，包括激光雷达的点云数据、组合导航的运动信息(三轴线加速度和三轴角速度)和位置信息(经纬度和高度)、超宽带无线通信设备的距离信息；
44.步骤3.3、降低获得的激光雷达点云数据的数量。将每帧点云划分为边长为n的立方体，n的取值范围通常在0.1米至1米之间，每个立方体内最多保留一个激光点；
45.步骤3.4、使用组合导航的三轴线加速度和三轴角速度数据对步骤3.3处理后的点云进行运动畸变矫正。首先对三轴线加速度和三轴角速度数据进行积分，获得每个测量数据对应的坐标pj、速度vj和旋转qj(四元数形式)，
[0046][0047][0048][0049]
其中δt表示相邻两个测量数据之间的时间间隔，gw表示世界坐标系下的重力向量，ak和ωk表示三轴线加速度和三轴角速度数据，rk为旋转qk的矩阵形式，b
gk
和b
ak
表示组合导航中陀螺仪和加速度计的偏置，δqk表示旋转的变化量。
[0050]
每个激光点的运动矫正矩阵由平移t和旋转qr两部分构成，其中平移部分t直接通过上述公式中的相对坐标p线性插值得到，
[0051][0052]
其中pj和p
j 1
为激光点相邻的两个相对坐标，tj和t
j 1
为相应的时间戳，ti为该激光点的时间戳。
[0053]
旋转部分qr通过对四元数进行插值得到，再转换为旋转矩阵，
[0054]
qr＝squad(qi，q
i 1
，si，s
i 1
，α)
[0055]
＝slerp(slerp(q，q
i 1
，α)，slerp(si，s
i 1
，α)，2α(1-α))
[0056]
其中，
[0057][0058][0059][0060]
θ＝acos(qi，q
i 1
)
[0061]
式中，qi和q
i 1
为激光点相邻的两个旋转四元数，和为相应的时间戳，ti为该激光点的时间戳。
[0062]
将qr转换为旋转矩阵r，
[0063][0064]
其中，
[0065]
qr＝w xi yj zk
[0066]
i2＝j2＝k2＝-1
[0067]
i0＝j0＝k0＝1
[0068]
矫正后的每个激光点可以表示为x
correct
，
[0069]
x
correct
＝rx
origin
t
[0070]
步骤3.5、对步骤3.4运动畸变矫正后的点云进行特征提取。对点云中的每个点搜索该点一定范围内的其他点构成一个集合，使用主成分分析计算这个集合的特征值和特征向量，其中最大的特征向量记为主方向，最小的特征向量记为法线方向，这个集合的局部线性度σ
1d
和局部平面度σ
2d
可以通过特征值λ1，λ2，λ3获得，
[0071]
[0072][0073]
当局部线性度σ
1d
和主方向同时满足阈值条件时，将该点记为线性点；当局部平面度σ
2d
和法线方向同时满足阈值条件时，将该点记为平面点。
[0074]
步骤3.6、使用优化的方式将步骤3.5处理后的特征点云与地图进行匹配，在初始时刻使用初始位置的点云图作为待匹配的地图。在地图中搜索特征点一定范围内的其他点构成一个集合，当该特征点为线性点时，计算集合的主方向，误差函数为线性点到该主方向v的距离d
linear
；当该特征点为平面点时，计算集合的法线方向n，误差函数为平面点到平面的距离d
planar
，
[0075]dlinear
＝||r
×
v||2[0076]dplanar
＝r
·n[0077]
其中，
[0078]
r＝x
feature-x
′
map
[0079]
＝x
feature-(rx
map
t)
[0080]
r和t为待优化的旋转和平移矩阵。
[0081]
最终的目标函数为，
[0082][0083]
其中ωi和ωj为两种特征点的权重函数，ωi与该线性点的距离呈负相关，ωj与该平面点的距离呈正相关，一种典型的表达形式为，
[0084][0085][0086]
其中d为原点到该点的距离。
[0087]
步骤3.7、优化结束后，使用步骤3.6优化后的坐标将步骤3.5处理后的特征点云变换到全局坐标系下，并加入到地图中。
[0088]
作为本实施例的一种实施方式，步骤s4包括：
[0089]
步骤4.1、在每个节点独立地进行同时定位和建图过程中，将步骤3.2获得的激光雷达点云进行编码。以每帧点云中心为圆心等距离地画多个同心圆，形成nr个等间距的圆环，再将这些圆环等角度地划分为ns个扇区，以该扇区内点的最大高度作为该扇区的编码，
[0090][0091]
因此，激光雷达点云编码的矩阵形式可以表示为，
[0092]
[0093]
步骤4.2、当两个节点在超宽带无线通信的信号范围内，并且超宽带无线通信设备输出的距离小于5米时，建立无线连接，交换步骤4.1得到的激光雷达点云编码和步骤3.6优化后的坐标；
[0094]
步骤4.3、根据激光雷达点云编码和优化后的坐标，使用成对一致的测量集最大化来实现回环外点的排除，保留满足下式的点云编码对，
[0095][0096]
其中，表示两个关键帧i和j之间的相对位姿，表示节点α和节点β之间的相对位姿，||
·
||
∑
表示马氏距离，成对一致的测量集最大化的阈值γ可以通过给定概率水平的卡方分布的四分位数来确定。
[0097]
步骤4.4、根据步骤4.3确定的有效点云编码对进一步确定回环。将需要查询的点云记为iq，将候选点云记为ic，两个点云通过列方向上的比较来计算相似度，即将对应同一列的距离求和得到总体距离偏差，
[0098][0099]
将记为原始点云ic偏移n个单位后的点云，则最佳偏移量d(iq，ic)和距离偏差n
*
为，
[0100][0101][0102]
对每个需要查询的点云，遍历候选点云集，当满足下式时，则确认回环，
[0103][0104]
其中是候选点云集，τ是被接受的阈值，c
*
为检测到的回环。
[0105]
如果没有检测到有效的回环，则在位姿图中加入组合导航的位置信息节点。
[0106]
步骤4.5、根据步骤4.4确定的回环或组合导航的位置信息进行多节点全局位姿图优化。将定义为所有单节点定位和建图的任务，n
α
为第α个节点中估计位姿的数量，将的数量，将定义为节点α中的第i个位姿，所有的节点位姿记为给定节点β中的第j帧和节点α中的第i帧，位姿图边的残差可以写为，
[0107][0108]
其中，
[0109][0110][0111]
其中表示第α个节点中估计的第i个位置和姿态，表示第β个节点中估计的第j个位置和姿态，表示节点β中的第j帧和节点α中的第i帧之间的相对位置和姿态，[q]
xyz
为四元数q中的向量部分。
[0112]
类似的，给定同一节点α中的相邻两帧第i帧和第i 1帧，位姿图边的残差可以写为，
[0113][0114]
将定义为所有的回环边，则位姿图优化可以被定义为，
[0115][0116]
其中，l表示步骤4.4检测到的回环，from(l)和to(l)表示回环中两个关键帧的位姿，
[0117][0118]
针对单个无人节点在探索较大区域时存在的传感器作用范围小、观测能力有限、计算复杂性高、存储性差等缺点，本发明可以高效地实现对有限大区域空间探测的同时，准确定位场景中的多个节点。每个节点按照一定规则降低传感器的数据来加快处理速度，组合导航输出的高频运动信息有助于矫正激光雷达点云，对激光点云特征提取，进一步降低点云数量的同时提高了点云的质量。在多个节点搜索回环时，使用编码后的点云加快了数据传输的速度和回环的搜索效率。
[0119]
实施例2：
[0120]
如图2所示，本发明提供一种基于多传感器和多节点的协同定位和地图构建装置，包括：
[0121]
初始化模块，用于在初始时刻建立多个节点之间的无线通信网络，其中，每个节点位于初始位置；所述节点包含激光雷达、组合导航和超宽带无线通信装置，节点可以为无人机、无人车无人等移动平台；
[0122]
全局坐标确定模块，用于通过多节点对初始位置的同一场景进行激光建图，确定全局坐标系；
[0123]
单节点建图模块，用于通过每个节点根据全局坐标系独立地进行同时定位和建图，直到完成规定区域的定位建图任务；
[0124]
协同模块，用于在每个节点独立地进行同时定位和建图过程中通过所述无线网络进行通信时，交换各自节点历史的点云编码和优化后的坐标，以检测是否经过同一区域，并
加入到位姿图中进行全局优化
[0125]
作为本实施例的一种实施方式，所述全局坐标确定模块，包括：
[0126]
激光建图单元，用于通过所述节点对初始位置的同一场景进行激光建图，得到初始位置的点云图；
[0127]
配准单元，用于通过配准多节点的点云图，得到多节点间的初始位姿约束；
[0128]
确定单元，用于根据初始位姿约束，确定全局坐标系。
[0129]
实施例3：
[0130]
本发明还一种存储介质，所述存储介质存储有机器可执行指令，所述机器可执行指令在被处理器调用和执行时，所述机器可执行指令促使所述处理器实现所述的基于多传感器和多节点的协同定位和地图构建方法。
[0131]
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。