基于编队共用地图的多个体三维协同惯性SLAM方法及装置

基于编队共用地图的多个体三维协同惯性slam方法及装置
技术领域
1.本发明属于惯性和协同导航系统技术领域，具体涉及一种基于编队共用地图的多个体三维协同惯性slam方法及装置。


背景技术：

2.近年来在不同行人个体的定位与导航技术发展迅速，mems惯性测量器由于其携带方便、损耗较小的优点受到了广泛关注，但由于imu噪声的影响，其定位误差会随着时间的增加而变大；与此同时，在多个体协同定位导航中，多个体之间需要获得除自己以外其余多个体的位置信息，因此为了获得多个体之间的初始相对姿态，仍需在原有设备基础之上布置辅助定位的设施，这对解决协同导航的问题造成了一定的困难。
3.目前应用的惯性定位与沟通(slam)方法仍处于二维构图阶段，无法在垂直方向上为多个体提供精确导航，虽然惯性传感器拥有低成本的显著优势，但互联网时代对地图构建的需求已经逐步由二维向三维发展，而三维构图不仅计算量大而且难以实现于低成本的实时导航定位系统；且在众多领域中，多个体间的协同导航可使工作效率大大提高，通过不同多个体间的信息的相互交换，可以有效提高单个体导航定位性能。因此使用惯导信息、多个体轨迹等简单信息达到精度高、协同效果好的算法框架具有突出的使用价值。
4.三维协同惯性slam方法属于分布式协同导航的范围，不存在中心节点，鲁棒性更高且运算效率高，多个体的位姿和栅格地图在分布式协同导航中相互独立；三维协同惯性slam方法可以有效抑制惯导的累计误差，即使不知道多个体间的初始相对位姿，也可通过不同多个体所构建地图之间的旋转、平移关系修正多个体的定位误差，实时对地图进行估计，因此可实现三维环境下长时间稳定可靠的多个体定位与导航，提高了三维环境下的多个体协同惯性定位系统的实时性、普遍性、抗干扰性，对未来的工程应用有重要研究意义。


技术实现要素：

5.发明目的：本发明的目的是提供了一种基于编队共用地图的多个体三维协同惯性slam方法及装置，能够在三维环境下，实现分布式多个体协同惯性导航，有效提高了三维惯性多个体协同导航系统的定位精度。
6.技术方案：本发明提供了一种基于编队共用地图的多个体三维协同惯性slam方法，包括以下步骤：
7.(1)利用足绑式惯性传感器获取并解算步长、航向与高度变化，得到多个体基于惯性导航技术的三维先验位姿估计；位姿估计的集合形成三维运动轨迹，利用多个体的运动轨迹，通过静态二值贝叶斯滤波方法，构建编队共用地图，并估计地图的后验概率，即多个体可通行区域的概率；
8.(2)在步骤(1)中基于惯性导航技术所构建的编队共用地图的基础上，利用蓝牙检测多个体之间的相遇事件，通过蓝牙信息与惯导信息的组合算法获取相遇时多个体的每一位个体的位姿估计；采用基于svd分解的最小二乘刚性变换方法求取多个体间的刚性变换
最优估计值，也就是多个体间相对位姿的关系；
9.(3)将步骤(2)获得的刚性变换最优估计值用于提高多个体相对位姿的精度，在修正步骤(2)初始相对位姿估计误差的同时，校正编队共用地图，以实现多个体协同构图与定位的目标。
10.进一步地，所述步骤(1)包括以下步骤：
11.令多个体在t时刻的位姿变化量为控制量u
t
，包含坐标系中x、y、z轴上的距离变化量和个体运动时航向角的变化量，对其进行蒙特卡洛采样，表示第m个粒子在t时刻的控制量，表示第m个粒子在t时刻的多个体位姿采样；
12.多个体在t-1时刻的位姿分布为s
t-1
，对其进行蒙特卡洛采样，初值置为0，其位姿分布s
t-1
具体如下式所示：
[0013][0014][0015]
其中，u
t-1
为t-1时刻控制量，z
t-1
为t-1时刻观测量，m表示第m个粒子，为t-1时刻第m个粒子位姿的采样结果，为第m个粒子的权重，n为粒子总数；
[0016]
多个体通过自身传感器获取自身的位姿数据并依靠此位姿数据形成下一步的位姿控制量，位姿推算模型如下：
[0017][0018]
其中，φ
t
为t时刻的航向角，表示在t时刻多个体在三维坐标系下在*轴方向上的坐标，表示在t-1时刻多个体在三维坐标系下在*轴方向上的坐标，l
t
、
△
φ
t
、为多个体t时刻的距离、航向变化与高度变化，θ
t-1
表示在t-1时刻运动轨迹变化方向与x轴正轴方向的夹角；
[0019]
将三维地图坐标系下的环境分割为众多六面体组成的空间区域，每一块六面体所占的立方体可表示为i表示第i个立方体，θ表示该六面体在三维坐标系下的空间区域，上标t为向量的转置；记立方体的长、宽、高为l、w、h，则三维地图坐标系下的六面体所在位置的表示为：
[0020][0021]
其中，《*》表示对每个元素四舍五入运算；
[0022]
多个体位姿估计六面体空间区域的后验概率为p(θi|z
1:t
,s
1:t
)，其中s1:t为多个体1到t时刻的位姿，z
1:t
为1到t时刻的观测量，θi表示地图中的第i个六面体空间区域；多个体的位姿和三维地图估计的联合后验概率为：
[0023]
p(s
1:t
|θ
1:t
,u
1:t
)
[0024]
其中，u
1:t
为多个体惯性里程计输出1到t时刻的控制变化量，θ为六面体空间区域
的集合θ＝{θ1,θ2,
…
,θm}；
[0025]
采用粒子滤波实现对地图和位姿联合分布估计，可得到编队共用地图，每个粒子可以表示为：
[0026]st
＝[s
1:t
,θ
1:t
]
[0027]
其中，其中，分别表示为1到t时刻的多个体x、y、z方向上的距离变化，φ
1:t
表示为航向变化；
[0028]
利用静态二值贝叶斯滤波方法，上述过程中取样的第m个粒子在t时刻所在六面体空间区域的可通行概率结果如下：
[0029][0030]
进一步地，所述步骤(2)实现过程如下：
[0031]
定义个体间初始相对姿态所表示轨迹的刚性变换具体如下式所示：
[0032][0033]
其中，x2、y2、z2表示个体2在自己构建的三维地图中的位置，x
2,1
、y
2,1
、z
2,1
表示个体2在个体1所构建地图中的对应位置，表示旋转矩阵；定义个体1所构建的编队共用地图为基准地图，和v
2,1
分别为两个体轨迹之间的旋转关系和位移向量；
[0034]
个体间的初始相对旋转最优估计和初始相对位移最优估计v
2,1*
定义为：
[0035][0036]
其中，个体1和个体2在相遇时在各自构建编队地图中的位置标示为pi、qi，其中i＝1,2,3,...,n；
[0037]
初始相对旋转最优估计和初始相对位移最优估计为：
[0038][0039][0040]
其中，pi、qi所构成的两组相遇点点集可表示为p、q；为点集p、q的质心坐标，其内积为s，将s进行奇异值分解，即
[0041]
进一步地，所述步骤(3)实现过程如下：
[0042]
多个体轨迹与地图精对准的实现步骤具体由优化联合后验概率分布所示：
[0043][0044]
其中，t表示以个体1为基准坐标时个体2的相对位姿转换，k时刻是获取刚性变换估计值并完成多个体轨迹与地图的粗对准的时间点；
[0045]
将个体2的状态量观测量以及控制量视为其相对于个体1的局部观测以校正基准概率地图；多个体的位姿相互独立，因式分解如下：
[0046][0047]
将上式中多个体状态分布后验展开为：
[0048][0049]
根据贝叶斯滤波和一阶马尔科夫性质将上式中分解为相对位姿的先验估计和t时刻的观测值：
[0050][0051]
个体1的状态估计先验和个体2的相对位姿先验通过状态预测阶段因式分解得到，具体公式如下：
[0052][0053]
多个体状态分布的后验与预测的比值即为粒子权重：
[0054][0055]
由此，第m个粒子的权重即为：
[0056][0057]
其中，η为保证粒子权重和为1的归一化系数，上标{*,[m]}表示第*个多个体的第
[m]个粒子。
[0058]
基于相同的发明构思，本发明还提出一种基于编队共用地图的多个体三维协同惯性slam装置，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述计算机程序被加载至处理器时实现上述的基于编队共用地图的多个体三维协同惯性slam方法。
[0059]
有益效果：与现有技术相比，本发明的有益效果：本发明解决了三维室内环境下多个体协同定位问题，有效降低了仅依赖惯性里程计定位导航时的累计误差；本发明能在初始位姿未知的情况下得到三维编队共用地图，多个体在陌生封闭环境中时可获取各区域的可通行概率，此技术能在消防抢险等特种领域中发挥提高工作效率的作用；本发明能够解算出不同个体之间相对位姿的关系，是实现协同构图与定位的前提；本发明能实现多个体间协同一致构图与定位，并修正误差，定位精度可达到在2000米范围内只有5米的误差，具有运算量小、复杂度低的显著优势，适用于单兵定位、工厂巡检等未来大规模普及的工程应用。
附图说明
[0060]
图1是基于编队共用地图的多个体三维协同惯性slam方法的架构图；
[0061]
图2是三维环境下室内实验设计路线图；
[0062]
图3是多个体运动的三维六面体占用概率栅格地图；
[0063]
图4为个体轨迹估计图；其中，(a)是采用本发明的轨迹估计图；(b)采用是本发明轨迹俯视图；(c)是采用本发明轨迹侧视图；
[0064]
图5是本发明与传统的惯性多个体导航方法、uwb测距-粒子滤波法的定位误差累计分布图。
具体实施方式
[0065]
下面结合附图对本发明做进一步详细说明。
[0066]
如图1所示，本发明提出一种基于编队共用地图的多个体三维协同惯性slam方法，具体步骤如下：
[0067]
步骤1：利用足绑式惯性传感器获取并解算步长、航向与高度变化，得到多个体基于惯性导航技术的三维先验位姿估计；位姿估计的集合形成三维运动轨迹，利用多个体的运动轨迹，通过静态二值贝叶斯滤波方法，构建编队共用地图，并估计地图的后验概率，即多个体可通行区域的概率。
[0068]
令多个体在t时刻的位姿变化量为控制量u
t
，包含坐标系中x、y、z轴上的距离变化量和个体运动时航向角的变化量，对其进行蒙特卡洛采样，表示第m个粒子在t时刻的控制量，表示第m个粒子在t时刻的多个体位姿采样。
[0069]
多个体在t-1时刻的位姿分布为s
t-1
，对其进行蒙特卡洛采样，初值置为0，其位姿分布s
t-1
具体如下式所示：
[0070]
[0071][0072]
其中，u
t-1
为t-1时刻控制量，z
t-1
为t-1时刻观测量，m表示第m个粒子，为t-1时刻第m个粒子位姿的采样结果，为第m个粒子的权重，n为粒子总数。
[0073]
多个体通过自身传感器获取自身的位姿数据并依靠此位姿数据形成下一步的位姿控制量，位姿推算模型如下：
[0074][0075]
其中，φ
t
为t时刻的航向角，表示在t时刻多个体在三维坐标系下在*轴方向上的坐标，表示在t-1时刻多个体在三维坐标系下在*轴方向上的坐标，l
t
、
△
φ
t
、为多个体t时刻的距离、航向变化与高度变化，θ
t-1
表示在t-1时刻运动轨迹变化方向与x轴正轴方向的夹角。
[0076]
将三维地图坐标系下的环境分割为众多六面体组成的空间区域，每一块六面体所占的立方体可表示为i表示第i个立方体，θ表示该六面体在三维坐标系下的空间区域，上标t为向量的转置；记立方体的长、宽、高为l、w、h，则三维地图坐标系下的六面体所在位置的表示为：
[0077][0078]
其中，《*》表示对每个元素四舍五入运算。
[0079]
多个体位姿估计六面体空间区域的后验概率为p(θi|z
1:t
,s
1:t
)，其中s
1:t
为多个体1到t时刻的位姿，z
1:t
为1到t时刻的观测量，θi表示地图中的第i个六面体空间区域。多个体的位姿和三维地图估计的联合后验概率为：
[0080]
p(s
1:t
|θ
1:t
,u
1:t
)
[0081]
其中，u
1:t
为多个体惯性里程计输出1到t时刻的控制变化量，θ为六面体空间区域的集合θ＝{θ1,θ2,
…
,θm}。
[0082]
采用粒子滤波实现对地图和位姿联合分布估计，可得到编队共用地图，每个粒子可以表示为：
[0083]st
＝[s
1:t
,θ
1:t
]
[0084]
其中，其中，分别表示为1到t时刻的多个体x、y、z方向上的距离变化，φ
1:t
表示为航向变化。
[0085]
利用静态二值贝叶斯滤波方法，上述过程中取样的第m个粒子在t时刻所在六面体空间区域的可通行概率结果如下：
[0086][0087]
步骤2：在步骤1中基于惯性导航技术所构建的编队共用地图的基础上，利用蓝牙
检测多个体之间的相遇事件，通过蓝牙信息与惯导信息的组合算法获取相遇时多个体的每一位个体的位姿估计；采用基于svd分解的最小二乘刚性变换方法求取多个体间的刚性变换最优估计值，也就是多个体间相对位姿的关系。
[0088]
根据步骤1所表述的个体的位姿估计，定义个体间初始相对姿态所表示轨迹的刚性变换具体如下式所示：
[0089][0090]
其中，x2、y2、z2表示个体2在自己构建的三维地图中的位置，x
2,1
、y
2,1
、z
2,1
表示个体2在个体1所构建地图中的对应位置，表示旋转矩阵；定义个体1所构建的编队共用地图为基准地图，和v
2,1
分别为两个体轨迹之间的旋转关系和位移向量。
[0091]
个体间的初始相对旋转最优估计和初始相对位移最优估计v
2,1*
定义为：
[0092][0093]
其中，个体1和个体2在相遇时在各自构建编队地图中的位置标示为pi、qi，i＝1,2,3,...,n。
[0094]
初始相对旋转最优估计和初始相对位移最优估计为：
[0095][0096][0097]
其中，pi、qi所构成的两组相遇点点集可表示为p、q。为点集p、q的质心坐标，其内积为s，将s进行奇异值分解，即
[0098]
步骤3：将步骤2获得的刚性变换最优估计值用于提高多个体相对位姿的精度，在修正步骤2初始相对位姿估计误差的同时，校正编队共用地图，以实现多个体协同构图与定位的目标。
[0099]
多个体轨迹与地图精对准的实现步骤具体由优化联合后验概率分布如下所示：
[0100][0101]
其中，t表示以个体1为基准坐标时个体2的相对位姿转换，k时刻是获取刚性变换估计值并完成多个体轨迹与地图的粗对准的时间点。
[0102]
将个体2的状态量观测量以及控制量视为其相对于个体1的局部观测以校正基准概率地图；多个体的位姿相互独立，因式分解如下：
[0103][0104]
将上式中多个体状态分布后验展开为：
[0105][0106]
根据贝叶斯滤波和一阶马尔科夫性质将上式中分解为相对位姿的先验估计和t时刻的观测值：
[0107][0108]
个体1的状态估计先验和个体2的相对位姿先验通过状态预测阶段因式分解得到，具体公式如下：
[0109][0110]
多个体状态分布的后验与预测的比值即为粒子权重：
[0111][0112]
由此，第m个粒子的权重即为：
[0113][0114]
其中，η为保证粒子权重和为1的归一化系数，上标{*,[m]}表示第*个多个体的第[m]个粒子。
[0115]
基于相同的发明构思，本发明还提出一种基于编队共用地图的多个体三维协同惯性slam装置，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述计算机程序被加载至处理器时实现上述的基于编队共用地图的多个体三维协同惯性slam方法。
[0116]
为了验证发明所提出的基于概率栅格地图的室内多个体三维协调惯性slam方法的正确性和有效性，对本发明所述的基于概率栅格地图的室内多个体三维协调惯性slam方法进行验证。如图2所示，实验场景为实验楼内的走廊和房间，实验楼占地面积约为2000平方米，每层层高约3米，单层的楼道周长约为200米。真值为图中圆点，误差值为多个体通过标定真值点时的位置估计值与真值位置的欧氏距离。图3是多个体运动的三维六面体编队共用地图，表示多个体运动的可通行区域。
[0117]
图4为个体轨迹估计图，其中图4(a)为本发明的轨迹估计，图4(b)是本发明轨迹俯视图；图4(c)是本发明轨迹侧视图。由轨迹图可以看出，本发明在记录多个体真实运动轨迹时具有较高的准确性。在已经构建了编队共用地图的基础上，惯导系统的误差处于收敛状态不会随时间增加而发散，且误差值较小，即使在高度估计上也较精准。
[0118]
图5比较了本发明与传统的惯性导航方法、uwb测距-粒子滤波方法定位误差累积分布曲线。传统的惯性导航方法在90％置信度下误差为16.01米；基于uwb测距-粒子滤波的方法在90％置信度下误差为8.67米；本发明算法的定位误差最小，90％置信度下误差为2.47米。因此，可以看出本发明算法的定位精度优于其他的定位方法，适用于三维协同多个体惯性导航的实际工程应用。
[0119]
以上仅是本发明的技术思想与概述，不能以上述案例局限本发明的保护范围，凡属于本发明提及的技术范围内的方案均属于本发明的保护范围。应当指出，对于本技术领域的基础改动或是其他润色，在基于本发明原理的前提上，皆属于本发明的保护范围。